《ros turtlebot_follower ：让机器人跟随我们移动》由会员分享，可在线阅读，更多相关《ros turtlebot_follower ：让机器人跟随我们移动（12页珍藏版）》请在金锄头文库上搜索。
1、ROS turtlebot_follower ：让机器人跟随我们移动ROS turtlebot_follower 学习 首先在 catkin_ws/src 目录下载源码了解代码见注释（其中有些地方我也不是很明白） follower.cpp#include #include #include #include #include #include #include #include dynamic_reconfigure/server.h#include turtlebot_follower/FollowerConfig.h#include namespace turtlebot_follower
2、/* The turtlebot follower nodelet./* The turtlebot follower nodelet. Subscribes to point clouds* from the 3dsensor, processes them, and publishes command vel* messages.*/class TurtlebotFollower : public nodelet:Nodeletpublic:/*!* brief The constructor for the follower.* Constructor for the follower.
3、*/TurtlebotFollower() : min_y_(0.1), max_y_(0.5),min_x_(-0.2), max_x_(0.2),max_z_(0.8), goal_z_(0.6),z_scale_(1.0), x_scale_(5.0) TurtlebotFollower()delete config_srv_;private:double min_y_; /* config_srv_;/*!* brief OnInit method from node handle.* OnInit method from node handle. Sets up the parame
4、ters* and topics.* 初始化 handle，参数，和话题*/virtual void onInit()ros:NodeHandleros:NodeHandle/从参数服务器获取设置的参数（launch 文件中设置数值）private_nh.getParam(min_y, min_y_);private_nh.getParam(max_y, max_y_); private_nh.getParam(min_x, min_x_);private_nh.getParam(max_x, max_x_);private_nh.getParam(max_z, max_z_);private
5、_nh.getParam(goal_z, goal_z_);private_nh.getParam(z_scale, z_scale_);private_nh.getParam(x_scale, x_scale_);private_nh.getParam(enabled, enabled_);/设置机器人移动的话题（用于机器人移动）：/mobile_base/mobile_base_controller/cmd_vel（换成你的机器人的移动 topic）cmdpub_ = private_nh.advertise (/mobile_base/mobile_base_controller/cmd
6、_vel, 1);markerpub_ = private_nh.advertise(marker,1);bboxpub_ = private_nh.advertise(bbox,1);sub_= nh.subscribe(depth/image_rect, 1, switch_srv_ = private_nh.advertiseService(change_state, config_srv_ = new dynamic_reconfigure:Server(private_nh);dynamic_reconfigure:Server:CallbackType f =boost:bind(
7、config_srv_-setCallback(f);/设置默认值，详见 catkin_ws/devel/include/turtlrbot_follower/FollowerConfig.hvoid reconfigure(turtlebot_follower:FollowerConfig &config, uint32_t level)min_y_ = config.min_y;max_y_ = config.max_y;min_x_ = config.min_x;max_x_ = config.max_x;max_z_ = config.max_z;goal_z_ = config.go
8、al_z;z_scale_ = config.z_scale;x_scale_ = config.x_scale;/*!* brief Callback for point clouds.* Callback for depth images. It finds the centroid* of the points in a box in the center of the image. * 它找到图像中心框中的点的质心* Publishes cmd_vel messages with the goal from the image.* 发布图像中目标的 cmd_vel 消息* param
9、cloud The point cloud message.* 参数：点云的消息*/void imagecb(const sensor_msgs:ImageConstPtr& depth_msg)/ Precompute the sin function for each row and column wangchao 预计算每行每列的正弦函数uint32_t image_width = depth_msg-width;ROS_INFO_THROTTLE(1, image_width=%d, image_width);float x_radians_per_pixel = 60.0/57.0/
10、image_width;/每个像素的弧度float sin_pixel_ximage_width;for (int x = 0; x height;float y_radians_per_pixel = 45.0/57.0/image_width;float sin_pixel_yimage_height;for (int y = 0; y (int row_step = depth_msg-step / sizeof(float);for (int v = 0; v height; +v, depth_row += row_step)for (int u = 0; u width; +u)f
11、loat depth = depth_image_proc:DepthTraits:toMeters(depth_rowu); if (!depth_image_proc:DepthTraits:valid(depth) | depth max_z_) continue;/不是有效的深度值或者深度超出 max_z_float y_val = sin_pixel_yv * depth;float x_val = sin_pixel_xu * depth;if ( y_val min_y_ & y_val min_x_ & x_val 4000)x /= n;y /= n;if(z max_z_)
12、ROS_INFO_THROTTLE(1, Centroid too far away %f, stopping the robotn, z);if (enabled_)cmdpub_.publish(geometry_msgs:TwistPtr(new geometry_msgs:Twist();return;ROS_INFO_THROTTLE(1, Centroid at %f %f %f with %d points, x, y, z, n);publishMarker(x, y, z);if (enabled_)geometry_msgs:TwistPtr cmd(new geometr
13、y_msgs:Twist();cmd-linear.x = (z - goal_z_) * z_scale_;cmd-angular.z = -x * x_scale_;cmdpub_.publish(cmd);else ROS_INFO_THROTTLE(1, Not enough points(%d) detected, stopping the robot, n);publishMarker(x, y, z);if (enabled_)cmdpub_.publish(geometry_msgs:TwistPtr(new geometry_msgs:Twist();publishBbox(
14、);bool changeModeSrvCb(turtlebot_msgs:SetFollowState:Request& request,turtlebot_msgs:SetFollowState:Response& response)if (enabled_ = true) & (request.state = request.STOPPED)ROS_INFO(Change mode service request: following stopped);cmdpub_.publish(geometry_msgs:TwistPtr(new geometry_msgs:Twist();ena
15、bled_ = false;else if (enabled_ = false) & (request.state = request.FOLLOW)ROS_INFO(Change mode service request: following (re)started);enabled_ = true;response.result = response.OK;return true;/画一个圆点，这个圆点代表质心void publishMarker(double x,double y,double z)visualization_msgs:Marker marker;marker.header.frame_id = /camera_rgb_optical_frame;marker.header.stamp = ros:Time();marker.ns = my_mespace;marker.id = 0;marker.type = visualization_msgs:Marker:SPHERE;marker.action = visualization_msgs:Marker:ADD;marker.pose.position.x = x; marker.pose.position.y = y;
