ROS turtlebot_follower :让机器人跟随我们移动
来源:互联网 发布:java object equals 编辑:程序博客网 时间:2024/06/07 03:23
ROS turtlebot_follower 学习
首先在catkin_ws/src目录下载源码,地址:https://github.com/turtlebot/turtlebot_apps.git
了解代码见注释(其中有些地方我也不是很明白)
follower.cpp
#include <ros/ros.h>#include <pluginlib/class_list_macros.h>#include <nodelet/nodelet.h>#include <geometry_msgs/Twist.h>#include <sensor_msgs/Image.h>#include <visualization_msgs/Marker.h>#include <turtlebot_msgs/SetFollowState.h>#include "dynamic_reconfigure/server.h"#include "turtlebot_follower/FollowerConfig.h"#include <depth_image_proc/depth_traits.h>namespace turtlebot_follower{//* 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::Nodelet{public: /*! * @brief The constructor for the follower. * Constructor for the follower. */ 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_; /**< The minimum y position of the points in the box. */ double max_y_; /**< The maximum y position of the points in the box. */ double min_x_; /**< The minimum x position of the points in the box. */ double max_x_; /**< The maximum x position of the points in the box. */ double max_z_; /**< The maximum z position of the points in the box. 框中 点的最大z位置,以上四个字段用来设置框的大小*/ double goal_z_; /**< The distance away from the robot to hold the centroid 离机器人的距离,以保持质心*/ double z_scale_; /**< The scaling factor for translational robot speed 移动机器人速度的缩放系数*/ double x_scale_; /**< The scaling factor for rotational robot speed 旋转机器人速度的缩放系数*/ bool enabled_; /**< Enable/disable following; just prevents motor commands 启用/禁用追踪; 只是阻止电机命令,置为false后,机器人不会移动,/mobile_base/mobile_base_controller/cmd_vel topic 为空*/ // Service for start/stop following ros::ServiceServer switch_srv_; // Dynamic reconfigure server 动态配置服务 dynamic_reconfigure::Server<turtlebot_follower::FollowerConfig>* config_srv_; /*! * @brief OnInit method from node handle. * OnInit method from node handle. Sets up the parameters * and topics. * 初始化handle,参数,和话题 */ virtual void onInit() { ros::NodeHandle& nh = getNodeHandle(); ros::NodeHandle& private_nh = getPrivateNodeHandle(); //从参数服务器获取设置的参数(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_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<geometry_msgs::Twist> ("/mobile_base/mobile_base_controller/cmd_vel", 1); markerpub_ = private_nh.advertise<visualization_msgs::Marker>("marker",1); bboxpub_ = private_nh.advertise<visualization_msgs::Marker>("bbox",1); sub_= nh.subscribe<sensor_msgs::Image>("depth/image_rect", 1, &TurtlebotFollower::imagecb, this); switch_srv_ = private_nh.advertiseService("change_state", &TurtlebotFollower::changeModeSrvCb, this); config_srv_ = new dynamic_reconfigure::Server<turtlebot_follower::FollowerConfig>(private_nh); dynamic_reconfigure::Server<turtlebot_follower::FollowerConfig>::CallbackType f = boost::bind(&TurtlebotFollower::reconfigure, this, _1, _2); config_srv_->setCallback(f); }//设置默认值,详见catkin_ws/devel/include/turtlrbot_follower/FollowerConfig.h void 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.goal_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 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/image_width;//每个像素的弧度 float sin_pixel_x[image_width]; for (int x = 0; x < image_width; ++x) { //求出正弦值 sin_pixel_x[x] = sin((x - image_width/ 2.0) * x_radians_per_pixel); } uint32_t image_height = depth_msg->height; float y_radians_per_pixel = 45.0/57.0/image_width; float sin_pixel_y[image_height]; for (int y = 0; y < image_height; ++y) { // Sign opposite x for y up values sin_pixel_y[y] = sin((image_height/ 2.0 - y) * y_radians_per_pixel); } //X,Y,Z of the centroid 质心的xyz float x = 0.0; float y = 0.0; float z = 1e6; //Number of points observed 观察的点数 unsigned int n = 0; //Iterate through all the points in the region and find the average of the position 迭代通过该区域的所有点,找到位置的平均值 const float* depth_row = reinterpret_cast<const float*>(&depth_msg->data[0]); int row_step = depth_msg->step / sizeof(float); for (int v = 0; v < (int)depth_msg->height; ++v, depth_row += row_step) { for (int u = 0; u < (int)depth_msg->width; ++u) { float depth = depth_image_proc::DepthTraits<float>::toMeters(depth_row[u]); if (!depth_image_proc::DepthTraits<float>::valid(depth) || depth > max_z_) continue;//不是有效的深度值或者深度超出max_z_ float y_val = sin_pixel_y[v] * depth; float x_val = sin_pixel_x[u] * depth; if ( y_val > min_y_ && y_val < max_y_ && x_val > min_x_ && x_val < max_x_) { x += x_val; y += y_val; z = std::min(z, depth); //approximate depth as forward. n++; } } } //If there are points, find the centroid and calculate the command goal. //If there are no points, simply publish a stop goal. //如果有点,找到质心并计算命令目标。如果没有点,只需发布停止消息。 ROS_INFO_THROTTLE(1, " n ==%d",n); if (n>4000) { x /= n; y /= n; if(z > max_z_){ ROS_INFO_THROTTLE(1, "Centroid too far away %f, stopping the robot\n", 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 geometry_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(); } 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())); enabled_ = 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_namespace"; 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; marker.pose.position.z = z; marker.pose.orientation.x = 0.0; marker.pose.orientation.y = 0.0; marker.pose.orientation.z = 0.0; marker.pose.orientation.w = 1.0; marker.scale.x = 0.1; marker.scale.y = 0.1; marker.scale.z = 0.1; marker.color.a = 1.0; marker.color.r = 1.0; marker.color.g = 0.0; marker.color.b = 0.0; //only if using a MESH_RESOURCE marker type: markerpub_.publish( marker ); } //画一个立方体,这个立方体代表感兴趣区域(RIO) void publishBbox() { double x = (min_x_ + max_x_)/2; double y = (min_y_ + max_y_)/2; double z = (0 + max_z_)/2; double scale_x = (max_x_ - x)*2; double scale_y = (max_y_ - y)*2; double scale_z = (max_z_ - z)*2; visualization_msgs::Marker marker; marker.header.frame_id = "/camera_rgb_optical_frame"; marker.header.stamp = ros::Time(); marker.ns = "my_namespace"; marker.id = 1; marker.type = visualization_msgs::Marker::CUBE; marker.action = visualization_msgs::Marker::ADD; //设置标记物姿态 marker.pose.position.x = x; marker.pose.position.y = -y; marker.pose.position.z = z; marker.pose.orientation.x = 0.0; marker.pose.orientation.y = 0.0; marker.pose.orientation.z = 0.0; marker.pose.orientation.w = 1.0; //设置标记物的尺寸大小 marker.scale.x = scale_x; marker.scale.y = scale_y; marker.scale.z = scale_z; marker.color.a = 0.5; marker.color.r = 0.0; marker.color.g = 1.0; marker.color.b = 0.0; //only if using a MESH_RESOURCE marker type: bboxpub_.publish( marker ); } ros::Subscriber sub_; ros::Publisher cmdpub_; ros::Publisher markerpub_; ros::Publisher bboxpub_;};PLUGINLIB_DECLARE_CLASS(turtlebot_follower, TurtlebotFollower, turtlebot_follower::TurtlebotFollower, nodelet::Nodelet);}
接下来看launch文件follower.launch
建议在修改前,将原先的代码注释掉,不要删掉。
<!-- The turtlebot people (or whatever) follower nodelet. --><launch> <arg name="simulation" default="false"/> <group unless="$(arg simulation)"> <!-- Real robot --> <include file="$(find turtlebot_follower)/launch/includes/velocity_smoother.launch.xml"> <arg name="nodelet_manager" value="/mobile_base_nodelet_manager"/> <arg name="navigation_topic" value="/cmd_vel_mux/input/navi"/> </include> <!--modify by 2016.11.07 启动我的机器人和摄像头,这里更换成你的机器人的启动文件和摄像头启动文件--> <include file="$(find handsfree_hw)/launch/handsfree_hw.launch"> </include> <include file="$(find handsfree_bringup)/launch/xtion_fake_laser_openni2.launch"> </include> <!-- 将原先的注释掉<include file="$(find turtlebot_bringup)/launch/3dsensor.launch"> <arg name="rgb_processing" value="true"/> <arg name="depth_processing" value="true"/> <arg name="depth_registered_processing" value="false"/> <arg name="depth_registration" value="false"/> <arg name="disparity_processing" value="false"/> <arg name="disparity_registered_processing" value="false"/> <arg name="scan_processing" value="false"/> </include>--> <!--modify end --> </group> <group if="$(arg simulation)"> <!-- Load nodelet manager for compatibility --> <node pkg="nodelet" type="nodelet" ns="camera" name="camera_nodelet_manager" args="manager"/> <include file="$(find turtlebot_follower)/launch/includes/velocity_smoother.launch.xml"> <arg name="nodelet_manager" value="camera/camera_nodelet_manager"/> <arg name="navigation_topic" value="cmd_vel_mux/input/navi"/> </include> </group> <param name="camera/rgb/image_color/compressed/jpeg_quality" value="22"/> <!-- Make a slower camera feed available; only required if we use android client --> <node pkg="topic_tools" type="throttle" name="camera_throttle" args="messages camera/rgb/image_color/compressed 5"/> <include file="$(find turtlebot_follower)/launch/includes/safety_controller.launch.xml"/> <!-- Real robot: Load turtlebot follower into the 3d sensors nodelet manager to avoid pointcloud serializing --> <!-- Simulation: Load turtlebot follower into nodelet manager for compatibility --> <node pkg="nodelet" type="nodelet" name="turtlebot_follower" args="load turtlebot_follower/TurtlebotFollower camera/camera_nodelet_manager"> <!--更换成你的机器人的移动topic,我的是/mobile_base/mobile_base_controller/cmd_vel--> <remap from="turtlebot_follower/cmd_vel" to="/mobile_base/mobile_base_controller/cmd_vel"/> <remap from="depth/points" to="camera/depth/points"/> <param name="enabled" value="true" /> <!--<param name="x_scale" value="7.0" />--> <!--<param name="z_scale" value="2.0" /> <param name="min_x" value="-0.35" /> <param name="max_x" value="0.35" /> <param name="min_y" value="0.1" /> <param name="max_y" value="0.6" /> <param name="max_z" value="1.2" /> <param name="goal_z" value="0.6" />--> <!-- test 可以在此处调节参数--> <param name="x_scale" value="1.5"/> <param name="z_scale" value="1.0" /> <param name="min_x" value="-0.35" /> <param name="max_x" value="0.35" /> <param name="min_y" value="0.1" /> <param name="max_y" value="0.5" /> <param name="max_z" value="1.5" /> <param name="goal_z" value="0.6" /> </node> <!-- Launch the script which will toggle turtlebot following on and off based on a joystick button. default: on --> <node name="switch" pkg="turtlebot_follower" type="switch.py"/> <!--modify 2016.11.07 在turtlebot_follower下新建follow.rviz文件,加载rviz,此时rviz内容为空--> <node name="rviz" pkg="rviz" type="rviz" args="-d $(find turtlebot_follower)/follow.rviz"/><!--modify end --></launch>
编译,运行follow.launch 会将机器人和摄像头,rviz都启动起来,只需要运行这一个launch就可以了。
rviz中添加两个marker,pointcloud,camera。如图:
topic与frame名称与代码中要保持一致。
添加完之后,rviz显示如图:
红点代表质心,绿框代表感兴趣区域
当红点在我们身上时,机器人会跟随我们运动,注意:走动时,我们的速度要慢一点,机器人的移动速度也要调慢一点。
当感兴趣区域没有红点时,机器人停止跟随,直到出现红点。
4 0
- ROS turtlebot_follower :让机器人跟随我们移动
- Turtlebot_follower
- 让div跟随鼠标移动
- ROS学习笔记(四) 先锋机器人跟随
- 【ROS】移动机器人运动控制的层次
- 基于ROS平台的移动机器人-目录
- ROS移动机器人之SLAM(1)
- 如何让一个div跟随鼠标移动
- 用ROS制作我们的机器人小车: URDF简介
- ROS想说爱你不容易 英特尔开源ROS移动机器人平台丨机器人大家说
- 基于ROS平台的移动机器人-4-通过ROS利用键盘控制小车移动
- 如何设计一款基于ROS的移动机器人?
- ROS学习(四)机器人的移动与矫正
- 基于ROS平台的移动机器人-2-小车底盘控制
- 基于ROS平台的移动机器人-7-使用Kinect2建图
- 基于ROS平台的移动机器人-8-使用Kinect2导航
- ROS Robotics Projects(7)自主移动机器人
- 让ros机器人行走、建图、路径规划、定位和导航
- UVA 1455 Kingdom(线段树+并查集)
- 新华BBC笔记二
- 显示隐藏
- Android Studio 的快捷键之提取全局变量
- 进程地址空间分布
- ROS turtlebot_follower :让机器人跟随我们移动
- 理解 OpenStack 高可用(HA) (4): Pacemaker 和 OpenStack Resource Agent (RA)
- 新华BBC笔记三
- c# 深度解析方法参数的关键字ref
- applet没反应
- 自动生成Juint测试类
- 彻底解决APP渠道统计和邀请码之痛点问题
- Azure 媒体服务可将优质内容传输至 Apple TV
- Java 正则校验(手机,邮箱,固话........)