13.模拟一个白球
在继续编写process_image客户机节点之前，必须对一个白球建模，并将其放置在Gazebo World场景中。
建模后的白球，将控制它在Gazebo中的位置，放置在机器人的相机前不同的位置。process_image客户端节点将负责分析机器人的图像，并向服务器drive_bot节点请求服务以将机器人驶向白球。
现在，让我们继续使用Gazebo中的 Model Editor（模型编辑器）工具对白球建模!
模型编辑器
下面是如何打开模型编辑器的提示:
$ gazebo # then Edit-> Model Editor插入球体
在模型编辑器工具的简单形状菜单下，单击一个球体并将其插入场景中的任何位置。
编辑大小
双击球体，并在Visual（视觉）和Collision（碰撞）中改变其半径为0.1。
改变颜色
要将球的颜色更改为白色，将其Visual（视觉）环境、漫反射、镜面和发射RGBA值设置为1。
保存/插入球体
将白球模型保存为my_ball，在/home/workspace目录下。然后退出模型编辑器工具，回到Gazebo主界面，点击“Insert（插入）”，将白球放到场景中的任何地方。
重新启动节点
现在已经建模了白球，重新启动world.launch中的节点。然后验证可以myworld中的任何地方插入my_ball。
保存
把白球放在建筑物外面的任何地方，这样机器人就看不到它了。然后，将这个new world的副本保存在/home/workspace/catkin_ws/src/my_robot/worlds下，替换旧的my.world文件。当启动这个新保存的world文件时，能够看到建筑环境和白球。
总结：遵循以下步骤，建模一个白球，并将其纳入world场景:
14.ROS节点:process_image
在这个项目中编写的第二个节点是process_image节点。这个客户端节点将订阅机器人的摄像头图像并分析它们以确定白球的位置。一旦确定了球的位置，客户端节点将从drive_bot服务器节点请求一个服务来驱动机器人向球的方向移动。机器人可以向左、向右或向前行驶，这取决于机器人在图像中的位置。
写完这个节点后，将白球放在机器人的摄像头前面。如果一切正常，节点应该分析图像，检测球的位置，然后请求一个ball_chaser/command_robot服务来驱动机器人朝向白球行进!
必须订阅/camera/rgb/image_raw主题才能从机器人的相机获得瞬时图像。在回调函数内部，通过读取图像数据消息检索图像。图像消息包含许多字段，如图像高度、数据等。查看完整的ROS sensor_msgs/Image文档（http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/Image.html）。有了图像消息之后，就必须循环遍历图像数据。对于每个像素，将其与表示明亮白色像素的值255进行比较，如果找到该像素，则尝试识别它位于图像的哪个部分，是左、中还是右。然后，请求服务，驱动机器人驶向那个方向。
写process_image.cpp
现在是时候编写process_image.cpp客户机节点了。该节点将分析图像并请求服务来驱动机器人。在ball_chaser包的src目录中创建源代码文件，下面附上了一段完整的代码，其中有多个提示可以帮助完成实现：
#include"ros/ros.h"
#include"ball_chaser/DriveToTarget.h"
#include<sensor_msgs/Image.h>
// Define a global client that can request services
ros::ServiceClient client;
// This function calls the command_robot service to drive the robot in the specified direction
void drive_robot(float lin_x, float ang_z)
{
// TODO: Request a service and pass the velocities to it to drive the robot
}
// This callback function continuously executes and reads the image datavoidprocess_image_callback(const sensor_msgs::Image img)
{
int white_pixel = 255;
// TODO: Loop through each pixel in the image and check if there's a bright white one
// Then, identify if this pixel falls in the left, mid, or right side of the image
// Depending on the white ball position, call the drive_bot function and pass velocities to it
// Request a stop when there's no white ball seen by the camera
}
int main(int argc, char** argv)
{
// Initialize the process_image node and create a handle to it
ros::init(argc, argv, "process_image");
ros::NodeHandle n;
// Define a client service capable of requesting services from command_robot
client = n.serviceClient<ball_chaser::DriveToTarget>("/ball_chaser/command_robot");
// Subscribe to /camera/rgb/image_raw topic to read the image data inside the process_image_callback function
ros::Subscriber sub1 = n.subscribe("/camera/rgb/image_raw", 10, process_image_callback);
// Handle ROS communication events
ros::spin();
return0;
}将此代码复制到process_image.cpp，并进行必要的更改。
编辑CMakeLists.txt
除了之前为drive_bot.cpp添加的所有依赖项，下面是应该为process_image.cpp添加的依赖项:
添加add_executable
添加target_link_libraries
添加add_dependencies
构建包
现在已经在CMakeLists.txt中包含了process_image.cpp代码的特定指令，使用以下命令编译:
$ cd /home/workspace/catkin_ws/
$ catkin_make启动文件
编辑保存在/home/workspace/catkin_ws/src/ball_chaser/ Launch下的 ball_chaser.launch文件，并将process_image节点添加到其中。现在，启动这个文件应该运行drive_bot和process_image!
测试process_image
要测试刚才编写的代码是否按预期工作，首先在环境中启动机器人，然后运行drive_bot和process_image节点。
1）在myworld中启动机器人
通过启动world.launch文件实现:
$ cd /home/workspace/catkin_ws/
$ source devel/setup.bash
$ roslaunch my_robot world.launch2）运行drive_bot和process_image
通过执行ball_chase .launch实现:
$ cd /home/workspace/catkin_ws/
$ source devel/setup.bash
$ roslaunch ball_chaser ball_chaser.launch3 ）可视化
为了可视化机器人的相机图像，可以从RViz订阅相机RGB图像主题。或者运行rqt_image_view节点:
$ cd /home/workspace/catkin_ws/
$ source devel/setup.bash
$ rosrun rqt_image_view rqt_image_view 现在把白球放在机器人前面不同的位置，看看机器人是否有能力追球!
总结：按照以下步骤创建process_image节点:
注：process_image参考代码如下---
#include"ros/ros.h"
#include"ball_chaser/DriveToTarget.h"
#include<sensor_msgs/Image.h>
using namespace std;
// Define a global client that can request services
ros::ServiceClient client;
// This function calls the command_robot service to drive the robot in the specified direction
void drive_robot(float lin_x, float ang_z)
{
ROS_INFO_STREAM("Driving the robot");
// TODO: Request a service and pass the velocities to it to drive the robot
ball_chaser::DriveToTarget srv;
srv.request.linear_x = lin_x;
srv.request.angular_z = ang_z;
// Call the command_robot service and pass the requested motor commands
if (!client.call(srv))
{
ROS_ERROR("Failed to call service command_robot");
}
}
// This callback function continuously executes and reads the image data
void process_image_callback(const sensor_msgs::Image img)
{
int white_pixel = 255;
// TODO: Loop through each pixel in the image and check if there's a bright white one
// Then, identify if this pixel falls in the left, mid, or right side of the image
// Depending on the white ball position, call the drive_bot function and pass velocities to it
// Request a stop when there's no white ball seen by the camera
int i;
int ball_position;
int ball_position_center;
int ball_position_sum = 0;
int white_pixel_num = 0;
for (i = 0; i + 2 < img.data.size(); i = i + 3)
{
if ((img.data[i] == 255) && (img.data[i+1] == 255) && (img.data[i+2] == 255))
{
ball_position = (i % (img.width * 3)) / 3;
ball_position_sum += ball_position;
white_pixel_num++;
}
}
if (white_pixel_num == 0)
{
drive_robot(0, 0);
}
else
{
ball_position_center = ball_position_sum / white_pixel_num;
if(ball_position_center < img.width / 3)
{
drive_robot(0.1, 0.5);
}
else if(ball_position_center > img.width * 2 / 3)
{
drive_robot(0.1, -0.5);
}
else
{
drive_robot(0.1, 0);
}
}
}
int main(int argc, char** argv)
{
// Initialize the process_image node and create a handle to it
ros::init(argc, argv, "process_image");
ros::NodeHandle n;
// Define a client service capable of requesting services from command_robot
client = n.serviceClient<ball_chaser::DriveToTarget>("/ball_chaser/command_robot");
// Subscribe to /camera/rgb/image_raw topic to read the image data inside the process_image_callback function
ros::Subscriber sub1 = n.subscribe("/camera/rgb/image_raw", 10, process_image_callback);
// Handle ROS communication events
ros::spin();
return0;
}