ROSturtlebotfollower让机器人跟随我们移动.docx
《ROSturtlebotfollower让机器人跟随我们移动.docx》由会员分享,可在线阅读,更多相关《ROSturtlebotfollower让机器人跟随我们移动.docx(17页珍藏版)》请在冰豆网上搜索。
ROSturtlebotfollower让机器人跟随我们移动
ROS-turtlebot-follower-:
让机器人跟随我们移动
max_x_(0.2),
max_z_(0.8),goal_z_(0.6),
z_scale_(1.0),x_scale_(5.0)
{
}
~TurtlebotFollower()
deleteconfig_srv_;
private:
doublemin_y_;/**doublemax_y_;/**doublemin_x_;/**doublemax_x_;/**doublemax_z_;/**doublegoal_z_;/**doublez_scale_;/**doublex_scale_;/**boolenabled_;/**//Serviceforstart/stopfollowingros::ServiceServerswitch_srv_;//Dynamicreconfigureserver动态配置服务dynamic_reconfigure::Server:FollowerConfig>*config_srv_;/*!*@briefOnInitmethodfromnodehandle.*OnInitmethodfromnodehandle.Setsuptheparameters*andtopics.*初始化handle,参数,和话题*/virtualvoidonInit(){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:Twist>("/mobile_base/mobile_base_controller/cmd_vel",1);markerpub_=private_nh.advertise:Marker>("marker",1);bboxpub_=private_nh.advertise:Marker>("bbox",1);sub_=nh.subscribe:Image>("depth/image_rect",1,&TurtlebotFollower::imagecb,this);switch_srv_=private_nh.advertiseService("change_state",&TurtlebotFollower::changeModeSrvCb,this);config_srv_=newdynamic_reconfigure::Server:FollowerConfig>(private_nh);dynamic_reconfigure::Server:FollowerConfig>::CallbackTypef=boost::bind(&TurtlebotFollower::reconfigure,this,_1,_2);config_srv_->setCallback(f);}//设置默认值,详见catkin_ws/devel/include/turtlrbot_follower/FollowerConfig.hvoidreconfigure(turtlebot_follower::FollowerConfig&config,uint32_tlevel){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;}/*!*@briefCallbackforpointclouds.*Callbackfordepthimages.Itfindsthecentroid*ofthepointsinaboxinthecenteroftheimage.*它找到图像中心框中的点的质心*Publishescmd_velmessageswiththegoalfromtheimage.*发布图像中目标的cmd_vel消息*@paramcloudThepointcloudmessage.*参数:点云的消息*/voidimagecb(constsensor_msgs::ImageConstPtr&depth_msg){//Precomputethesinfunctionforeachrowandcolumnwangchao预计算每行每列的正弦函数uint32_timage_width=depth_msg->width;ROS_INFO_THROTTLE(1,"image_width=%d",image_width);floatx_radians_per_pixel=60.0/57.0/image_width;//每个像素的弧度floatsin_pixel_x[image_width];for(intx=0;x//求出正弦值sin_pixel_x[x]=sin((x-image_width/2.0)*x_radians_per_pixel);}uint32_timage_height=depth_msg->height;floaty_radians_per_pixel=45.0/57.0/image_width;floatsin_pixel_y[image_height];for(inty=0;y//Signoppositexforyupvaluessin_pixel_y[y]=sin((image_height/2.0-y)*y_radians_per_pixel);}//X,Y,Zofthecentroid质心的xyzfloatx=0.0;floaty=0.0;floatz=1e6;//Numberofpointsobserved观察的点数unsignedintn=0;//Iteratethroughallthepointsintheregionandfindtheaverageoftheposition迭代通过该区域的所有点,找到位置的平均值constfloat*depth_row=reinterpret_cast(&depth_msg->data[0]);introw_step=depth_msg->step/sizeof(float);for(intv=0;v<(int)depth_msg->height;++v,depth_row+=row_step){for(intu=0;u<(int)depth_msg->width;++u){floatdepth=depth_image_proc::DepthTraits::toMeters(depth_row[u]);if(!depth_image_proc::DepthTraits::valid(depth)||depth>max_z_)continue;//不是有效的深度值或者深度超出max_z_floaty_val=sin_pixel_y[v]*depth;floatx_val=sin_pixel_x[u]*depth;if(y_val>min_y_&&y_valx_val>min_x_&&x_val{x+=x_val;y+=y_val;z=std::min(z,depth);//approximatedepthasforward.n++;}}}//Iftherearepoints,findthecentroidandcalculatethecommandgoal.//Iftherearenopoints,simplypublishastopgoal.//如果有点,找到质心并计算命令目标。如果没有点,只需发布停止消息。ROS_INFO_THROTTLE(1,"n==%d",n);if(n>4000){x/=n;y/=n;if(z>max_z_){ROS_INFO_THROTTLE(1,"Centroidtoofaraway%f,stoppingtherobot\n",z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}return;}ROS_INFO_THROTTLE(1,"Centroidat%f%f%fwith%dpoints",x,y,z,n);publishMarker(x,y,z);if(enabled_){geometry_msgs::TwistPtrcmd(newgeometry_msgs::Twist());cmd->linear.x=(z-goal_z_)*z_scale_;cmd->angular.z=-x*x_scale_;cmdpub_.publish(cmd);}}else{ROS_INFO_THROTTLE(1,"Notenoughpoints(%d)detected,stoppingtherobot",n);publishMarker(x,y,z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}}publishBbox();}boolchangeModeSrvCb(turtlebot_msgs::SetFollowState::Request&request,turtlebot_msgs::SetFollowState::Response&response){if((enabled_==true)&&(request.state==request.STOPPED)){ROS_INFO("Changemodeservicerequest:followingstopped");cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));enabled_=false;}elseif((enabled_==false)&&(request.state==request.FOLLOW)){ROS_INFO("Changemodeservicerequest:following(re)started");enabled_=true;}response.result=response.OK;returntrue;}//画一个圆点,这个圆点代表质心voidpublishMarker(doublex,doubley,doublez){visualization_msgs::Markermarker;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;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;//onlyifusingaMESH_RESOURCEmarkertype:markerpub_.publish(marker);}//画一个立方体,这个立方体代表感兴趣区域(RIO)voidpublishBbox(){doublex=(min_x_+max_x_)/2;doubley=(min_y_+max_y_)/2;doublez=(0+max_z_)/2;doublescale_x=(max_x_-x)*2;doublescale_y=(max_y_-y)*2;doublescale_z=(max_z_-z)*2;visualization_msgs::Markermarker;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;//onlyifusingaMESH_RESOURCEmarkertype:bboxpub_.publish(marker);}ros::Subscribersub_;ros::Publishercmdpub_;ros::Publishermarkerpub_;ros::Publisherbboxpub_;};PLUGINLIB_DECLARE_CLASS(turtlebot_follower,TurtlebotFollower,turtlebot_follower::TurtlebotFollower,nodelet::Nodelet);}接下来看launch文件follower.launch建议在修改前,将原先的代码注释掉,不要删掉。--Theturtlebotpeople(orwhatever)followernodelet.-->--Realrobot-->--modifyby2016.11.07启动我的机器人和摄像头,这里更换成你的机器人的启动文件和摄像头启动文件--> --将原先的注释掉--><
doublemax_y_;/**doublemin_x_;/**doublemax_x_;/**doublemax_z_;/**doublegoal_z_;/**doublez_scale_;/**doublex_scale_;/**boolenabled_;/**//Serviceforstart/stopfollowingros::ServiceServerswitch_srv_;//Dynamicreconfigureserver动态配置服务dynamic_reconfigure::Server:FollowerConfig>*config_srv_;/*!*@briefOnInitmethodfromnodehandle.*OnInitmethodfromnodehandle.Setsuptheparameters*andtopics.*初始化handle,参数,和话题*/virtualvoidonInit(){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:Twist>("/mobile_base/mobile_base_controller/cmd_vel",1);markerpub_=private_nh.advertise:Marker>("marker",1);bboxpub_=private_nh.advertise:Marker>("bbox",1);sub_=nh.subscribe:Image>("depth/image_rect",1,&TurtlebotFollower::imagecb,this);switch_srv_=private_nh.advertiseService("change_state",&TurtlebotFollower::changeModeSrvCb,this);config_srv_=newdynamic_reconfigure::Server:FollowerConfig>(private_nh);dynamic_reconfigure::Server:FollowerConfig>::CallbackTypef=boost::bind(&TurtlebotFollower::reconfigure,this,_1,_2);config_srv_->setCallback(f);}//设置默认值,详见catkin_ws/devel/include/turtlrbot_follower/FollowerConfig.hvoidreconfigure(turtlebot_follower::FollowerConfig&config,uint32_tlevel){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;}/*!*@briefCallbackforpointclouds.*Callbackfordepthimages.Itfindsthecentroid*ofthepointsinaboxinthecenteroftheimage.*它找到图像中心框中的点的质心*Publishescmd_velmessageswiththegoalfromtheimage.*发布图像中目标的cmd_vel消息*@paramcloudThepointcloudmessage.*参数:点云的消息*/voidimagecb(constsensor_msgs::ImageConstPtr&depth_msg){//Precomputethesinfunctionforeachrowandcolumnwangchao预计算每行每列的正弦函数uint32_timage_width=depth_msg->width;ROS_INFO_THROTTLE(1,"image_width=%d",image_width);floatx_radians_per_pixel=60.0/57.0/image_width;//每个像素的弧度floatsin_pixel_x[image_width];for(intx=0;x//求出正弦值sin_pixel_x[x]=sin((x-image_width/2.0)*x_radians_per_pixel);}uint32_timage_height=depth_msg->height;floaty_radians_per_pixel=45.0/57.0/image_width;floatsin_pixel_y[image_height];for(inty=0;y//Signoppositexforyupvaluessin_pixel_y[y]=sin((image_height/2.0-y)*y_radians_per_pixel);}//X,Y,Zofthecentroid质心的xyzfloatx=0.0;floaty=0.0;floatz=1e6;//Numberofpointsobserved观察的点数unsignedintn=0;//Iteratethroughallthepointsintheregionandfindtheaverageoftheposition迭代通过该区域的所有点,找到位置的平均值constfloat*depth_row=reinterpret_cast(&depth_msg->data[0]);introw_step=depth_msg->step/sizeof(float);for(intv=0;v<(int)depth_msg->height;++v,depth_row+=row_step){for(intu=0;u<(int)depth_msg->width;++u){floatdepth=depth_image_proc::DepthTraits::toMeters(depth_row[u]);if(!depth_image_proc::DepthTraits::valid(depth)||depth>max_z_)continue;//不是有效的深度值或者深度超出max_z_floaty_val=sin_pixel_y[v]*depth;floatx_val=sin_pixel_x[u]*depth;if(y_val>min_y_&&y_valx_val>min_x_&&x_val{x+=x_val;y+=y_val;z=std::min(z,depth);//approximatedepthasforward.n++;}}}//Iftherearepoints,findthecentroidandcalculatethecommandgoal.//Iftherearenopoints,simplypublishastopgoal.//如果有点,找到质心并计算命令目标。如果没有点,只需发布停止消息。ROS_INFO_THROTTLE(1,"n==%d",n);if(n>4000){x/=n;y/=n;if(z>max_z_){ROS_INFO_THROTTLE(1,"Centroidtoofaraway%f,stoppingtherobot\n",z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}return;}ROS_INFO_THROTTLE(1,"Centroidat%f%f%fwith%dpoints",x,y,z,n);publishMarker(x,y,z);if(enabled_){geometry_msgs::TwistPtrcmd(newgeometry_msgs::Twist());cmd->linear.x=(z-goal_z_)*z_scale_;cmd->angular.z=-x*x_scale_;cmdpub_.publish(cmd);}}else{ROS_INFO_THROTTLE(1,"Notenoughpoints(%d)detected,stoppingtherobot",n);publishMarker(x,y,z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}}publishBbox();}boolchangeModeSrvCb(turtlebot_msgs::SetFollowState::Request&request,turtlebot_msgs::SetFollowState::Response&response){if((enabled_==true)&&(request.state==request.STOPPED)){ROS_INFO("Changemodeservicerequest:followingstopped");cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));enabled_=false;}elseif((enabled_==false)&&(request.state==request.FOLLOW)){ROS_INFO("Changemodeservicerequest:following(re)started");enabled_=true;}response.result=response.OK;returntrue;}//画一个圆点,这个圆点代表质心voidpublishMarker(doublex,doubley,doublez){visualization_msgs::Markermarker;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;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;//onlyifusingaMESH_RESOURCEmarkertype:markerpub_.publish(marker);}//画一个立方体,这个立方体代表感兴趣区域(RIO)voidpublishBbox(){doublex=(min_x_+max_x_)/2;doubley=(min_y_+max_y_)/2;doublez=(0+max_z_)/2;doublescale_x=(max_x_-x)*2;doublescale_y=(max_y_-y)*2;doublescale_z=(max_z_-z)*2;visualization_msgs::Markermarker;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;//onlyifusingaMESH_RESOURCEmarkertype:bboxpub_.publish(marker);}ros::Subscribersub_;ros::Publishercmdpub_;ros::Publishermarkerpub_;ros::Publisherbboxpub_;};PLUGINLIB_DECLARE_CLASS(turtlebot_follower,TurtlebotFollower,turtlebot_follower::TurtlebotFollower,nodelet::Nodelet);}接下来看launch文件follower.launch建议在修改前,将原先的代码注释掉,不要删掉。--Theturtlebotpeople(orwhatever)followernodelet.-->--Realrobot-->--modifyby2016.11.07启动我的机器人和摄像头,这里更换成你的机器人的启动文件和摄像头启动文件--> --将原先的注释掉--><
doublemin_x_;/**doublemax_x_;/**doublemax_z_;/**doublegoal_z_;/**doublez_scale_;/**doublex_scale_;/**boolenabled_;/**//Serviceforstart/stopfollowingros::ServiceServerswitch_srv_;//Dynamicreconfigureserver动态配置服务dynamic_reconfigure::Server:FollowerConfig>*config_srv_;/*!*@briefOnInitmethodfromnodehandle.*OnInitmethodfromnodehandle.Setsuptheparameters*andtopics.*初始化handle,参数,和话题*/virtualvoidonInit(){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:Twist>("/mobile_base/mobile_base_controller/cmd_vel",1);markerpub_=private_nh.advertise:Marker>("marker",1);bboxpub_=private_nh.advertise:Marker>("bbox",1);sub_=nh.subscribe:Image>("depth/image_rect",1,&TurtlebotFollower::imagecb,this);switch_srv_=private_nh.advertiseService("change_state",&TurtlebotFollower::changeModeSrvCb,this);config_srv_=newdynamic_reconfigure::Server:FollowerConfig>(private_nh);dynamic_reconfigure::Server:FollowerConfig>::CallbackTypef=boost::bind(&TurtlebotFollower::reconfigure,this,_1,_2);config_srv_->setCallback(f);}//设置默认值,详见catkin_ws/devel/include/turtlrbot_follower/FollowerConfig.hvoidreconfigure(turtlebot_follower::FollowerConfig&config,uint32_tlevel){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;}/*!*@briefCallbackforpointclouds.*Callbackfordepthimages.Itfindsthecentroid*ofthepointsinaboxinthecenteroftheimage.*它找到图像中心框中的点的质心*Publishescmd_velmessageswiththegoalfromtheimage.*发布图像中目标的cmd_vel消息*@paramcloudThepointcloudmessage.*参数:点云的消息*/voidimagecb(constsensor_msgs::ImageConstPtr&depth_msg){//Precomputethesinfunctionforeachrowandcolumnwangchao预计算每行每列的正弦函数uint32_timage_width=depth_msg->width;ROS_INFO_THROTTLE(1,"image_width=%d",image_width);floatx_radians_per_pixel=60.0/57.0/image_width;//每个像素的弧度floatsin_pixel_x[image_width];for(intx=0;x//求出正弦值sin_pixel_x[x]=sin((x-image_width/2.0)*x_radians_per_pixel);}uint32_timage_height=depth_msg->height;floaty_radians_per_pixel=45.0/57.0/image_width;floatsin_pixel_y[image_height];for(inty=0;y//Signoppositexforyupvaluessin_pixel_y[y]=sin((image_height/2.0-y)*y_radians_per_pixel);}//X,Y,Zofthecentroid质心的xyzfloatx=0.0;floaty=0.0;floatz=1e6;//Numberofpointsobserved观察的点数unsignedintn=0;//Iteratethroughallthepointsintheregionandfindtheaverageoftheposition迭代通过该区域的所有点,找到位置的平均值constfloat*depth_row=reinterpret_cast(&depth_msg->data[0]);introw_step=depth_msg->step/sizeof(float);for(intv=0;v<(int)depth_msg->height;++v,depth_row+=row_step){for(intu=0;u<(int)depth_msg->width;++u){floatdepth=depth_image_proc::DepthTraits::toMeters(depth_row[u]);if(!depth_image_proc::DepthTraits::valid(depth)||depth>max_z_)continue;//不是有效的深度值或者深度超出max_z_floaty_val=sin_pixel_y[v]*depth;floatx_val=sin_pixel_x[u]*depth;if(y_val>min_y_&&y_valx_val>min_x_&&x_val{x+=x_val;y+=y_val;z=std::min(z,depth);//approximatedepthasforward.n++;}}}//Iftherearepoints,findthecentroidandcalculatethecommandgoal.//Iftherearenopoints,simplypublishastopgoal.//如果有点,找到质心并计算命令目标。如果没有点,只需发布停止消息。ROS_INFO_THROTTLE(1,"n==%d",n);if(n>4000){x/=n;y/=n;if(z>max_z_){ROS_INFO_THROTTLE(1,"Centroidtoofaraway%f,stoppingtherobot\n",z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}return;}ROS_INFO_THROTTLE(1,"Centroidat%f%f%fwith%dpoints",x,y,z,n);publishMarker(x,y,z);if(enabled_){geometry_msgs::TwistPtrcmd(newgeometry_msgs::Twist());cmd->linear.x=(z-goal_z_)*z_scale_;cmd->angular.z=-x*x_scale_;cmdpub_.publish(cmd);}}else{ROS_INFO_THROTTLE(1,"Notenoughpoints(%d)detected,stoppingtherobot",n);publishMarker(x,y,z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}}publishBbox();}boolchangeModeSrvCb(turtlebot_msgs::SetFollowState::Request&request,turtlebot_msgs::SetFollowState::Response&response){if((enabled_==true)&&(request.state==request.STOPPED)){ROS_INFO("Changemodeservicerequest:followingstopped");cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));enabled_=false;}elseif((enabled_==false)&&(request.state==request.FOLLOW)){ROS_INFO("Changemodeservicerequest:following(re)started");enabled_=true;}response.result=response.OK;returntrue;}//画一个圆点,这个圆点代表质心voidpublishMarker(doublex,doubley,doublez){visualization_msgs::Markermarker;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;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;//onlyifusingaMESH_RESOURCEmarkertype:markerpub_.publish(marker);}//画一个立方体,这个立方体代表感兴趣区域(RIO)voidpublishBbox(){doublex=(min_x_+max_x_)/2;doubley=(min_y_+max_y_)/2;doublez=(0+max_z_)/2;doublescale_x=(max_x_-x)*2;doublescale_y=(max_y_-y)*2;doublescale_z=(max_z_-z)*2;visualization_msgs::Markermarker;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;//onlyifusingaMESH_RESOURCEmarkertype:bboxpub_.publish(marker);}ros::Subscribersub_;ros::Publishercmdpub_;ros::Publishermarkerpub_;ros::Publisherbboxpub_;};PLUGINLIB_DECLARE_CLASS(turtlebot_follower,TurtlebotFollower,turtlebot_follower::TurtlebotFollower,nodelet::Nodelet);}接下来看launch文件follower.launch建议在修改前,将原先的代码注释掉,不要删掉。--Theturtlebotpeople(orwhatever)followernodelet.-->--Realrobot-->--modifyby2016.11.07启动我的机器人和摄像头,这里更换成你的机器人的启动文件和摄像头启动文件--> --将原先的注释掉--><
doublemax_x_;/**doublemax_z_;/**doublegoal_z_;/**doublez_scale_;/**doublex_scale_;/**boolenabled_;/**//Serviceforstart/stopfollowingros::ServiceServerswitch_srv_;//Dynamicreconfigureserver动态配置服务dynamic_reconfigure::Server:FollowerConfig>*config_srv_;/*!*@briefOnInitmethodfromnodehandle.*OnInitmethodfromnodehandle.Setsuptheparameters*andtopics.*初始化handle,参数,和话题*/virtualvoidonInit(){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:Twist>("/mobile_base/mobile_base_controller/cmd_vel",1);markerpub_=private_nh.advertise:Marker>("marker",1);bboxpub_=private_nh.advertise:Marker>("bbox",1);sub_=nh.subscribe:Image>("depth/image_rect",1,&TurtlebotFollower::imagecb,this);switch_srv_=private_nh.advertiseService("change_state",&TurtlebotFollower::changeModeSrvCb,this);config_srv_=newdynamic_reconfigure::Server:FollowerConfig>(private_nh);dynamic_reconfigure::Server:FollowerConfig>::CallbackTypef=boost::bind(&TurtlebotFollower::reconfigure,this,_1,_2);config_srv_->setCallback(f);}//设置默认值,详见catkin_ws/devel/include/turtlrbot_follower/FollowerConfig.hvoidreconfigure(turtlebot_follower::FollowerConfig&config,uint32_tlevel){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;}/*!*@briefCallbackforpointclouds.*Callbackfordepthimages.Itfindsthecentroid*ofthepointsinaboxinthecenteroftheimage.*它找到图像中心框中的点的质心*Publishescmd_velmessageswiththegoalfromtheimage.*发布图像中目标的cmd_vel消息*@paramcloudThepointcloudmessage.*参数:点云的消息*/voidimagecb(constsensor_msgs::ImageConstPtr&depth_msg){//Precomputethesinfunctionforeachrowandcolumnwangchao预计算每行每列的正弦函数uint32_timage_width=depth_msg->width;ROS_INFO_THROTTLE(1,"image_width=%d",image_width);floatx_radians_per_pixel=60.0/57.0/image_width;//每个像素的弧度floatsin_pixel_x[image_width];for(intx=0;x//求出正弦值sin_pixel_x[x]=sin((x-image_width/2.0)*x_radians_per_pixel);}uint32_timage_height=depth_msg->height;floaty_radians_per_pixel=45.0/57.0/image_width;floatsin_pixel_y[image_height];for(inty=0;y//Signoppositexforyupvaluessin_pixel_y[y]=sin((image_height/2.0-y)*y_radians_per_pixel);}//X,Y,Zofthecentroid质心的xyzfloatx=0.0;floaty=0.0;floatz=1e6;//Numberofpointsobserved观察的点数unsignedintn=0;//Iteratethroughallthepointsintheregionandfindtheaverageoftheposition迭代通过该区域的所有点,找到位置的平均值constfloat*depth_row=reinterpret_cast(&depth_msg->data[0]);introw_step=depth_msg->step/sizeof(float);for(intv=0;v<(int)depth_msg->height;++v,depth_row+=row_step){for(intu=0;u<(int)depth_msg->width;++u){floatdepth=depth_image_proc::DepthTraits::toMeters(depth_row[u]);if(!depth_image_proc::DepthTraits::valid(depth)||depth>max_z_)continue;//不是有效的深度值或者深度超出max_z_floaty_val=sin_pixel_y[v]*depth;floatx_val=sin_pixel_x[u]*depth;if(y_val>min_y_&&y_valx_val>min_x_&&x_val{x+=x_val;y+=y_val;z=std::min(z,depth);//approximatedepthasforward.n++;}}}//Iftherearepoints,findthecentroidandcalculatethecommandgoal.//Iftherearenopoints,simplypublishastopgoal.//如果有点,找到质心并计算命令目标。如果没有点,只需发布停止消息。ROS_INFO_THROTTLE(1,"n==%d",n);if(n>4000){x/=n;y/=n;if(z>max_z_){ROS_INFO_THROTTLE(1,"Centroidtoofaraway%f,stoppingtherobot\n",z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}return;}ROS_INFO_THROTTLE(1,"Centroidat%f%f%fwith%dpoints",x,y,z,n);publishMarker(x,y,z);if(enabled_){geometry_msgs::TwistPtrcmd(newgeometry_msgs::Twist());cmd->linear.x=(z-goal_z_)*z_scale_;cmd->angular.z=-x*x_scale_;cmdpub_.publish(cmd);}}else{ROS_INFO_THROTTLE(1,"Notenoughpoints(%d)detected,stoppingtherobot",n);publishMarker(x,y,z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}}publishBbox();}boolchangeModeSrvCb(turtlebot_msgs::SetFollowState::Request&request,turtlebot_msgs::SetFollowState::Response&response){if((enabled_==true)&&(request.state==request.STOPPED)){ROS_INFO("Changemodeservicerequest:followingstopped");cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));enabled_=false;}elseif((enabled_==false)&&(request.state==request.FOLLOW)){ROS_INFO("Changemodeservicerequest:following(re)started");enabled_=true;}response.result=response.OK;returntrue;}//画一个圆点,这个圆点代表质心voidpublishMarker(doublex,doubley,doublez){visualization_msgs::Markermarker;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;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;//onlyifusingaMESH_RESOURCEmarkertype:markerpub_.publish(marker);}//画一个立方体,这个立方体代表感兴趣区域(RIO)voidpublishBbox(){doublex=(min_x_+max_x_)/2;doubley=(min_y_+max_y_)/2;doublez=(0+max_z_)/2;doublescale_x=(max_x_-x)*2;doublescale_y=(max_y_-y)*2;doublescale_z=(max_z_-z)*2;visualization_msgs::Markermarker;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;//onlyifusingaMESH_RESOURCEmarkertype:bboxpub_.publish(marker);}ros::Subscribersub_;ros::Publishercmdpub_;ros::Publishermarkerpub_;ros::Publisherbboxpub_;};PLUGINLIB_DECLARE_CLASS(turtlebot_follower,TurtlebotFollower,turtlebot_follower::TurtlebotFollower,nodelet::Nodelet);}接下来看launch文件follower.launch建议在修改前,将原先的代码注释掉,不要删掉。--Theturtlebotpeople(orwhatever)followernodelet.-->--Realrobot-->--modifyby2016.11.07启动我的机器人和摄像头,这里更换成你的机器人的启动文件和摄像头启动文件--> --将原先的注释掉--><
doublemax_z_;/**doublegoal_z_;/**doublez_scale_;/**doublex_scale_;/**boolenabled_;/**//Serviceforstart/stopfollowingros::ServiceServerswitch_srv_;//Dynamicreconfigureserver动态配置服务dynamic_reconfigure::Server:FollowerConfig>*config_srv_;/*!*@briefOnInitmethodfromnodehandle.*OnInitmethodfromnodehandle.Setsuptheparameters*andtopics.*初始化handle,参数,和话题*/virtualvoidonInit(){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:Twist>("/mobile_base/mobile_base_controller/cmd_vel",1);markerpub_=private_nh.advertise:Marker>("marker",1);bboxpub_=private_nh.advertise:Marker>("bbox",1);sub_=nh.subscribe:Image>("depth/image_rect",1,&TurtlebotFollower::imagecb,this);switch_srv_=private_nh.advertiseService("change_state",&TurtlebotFollower::changeModeSrvCb,this);config_srv_=newdynamic_reconfigure::Server:FollowerConfig>(private_nh);dynamic_reconfigure::Server:FollowerConfig>::CallbackTypef=boost::bind(&TurtlebotFollower::reconfigure,this,_1,_2);config_srv_->setCallback(f);}//设置默认值,详见catkin_ws/devel/include/turtlrbot_follower/FollowerConfig.hvoidreconfigure(turtlebot_follower::FollowerConfig&config,uint32_tlevel){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;}/*!*@briefCallbackforpointclouds.*Callbackfordepthimages.Itfindsthecentroid*ofthepointsinaboxinthecenteroftheimage.*它找到图像中心框中的点的质心*Publishescmd_velmessageswiththegoalfromtheimage.*发布图像中目标的cmd_vel消息*@paramcloudThepointcloudmessage.*参数:点云的消息*/voidimagecb(constsensor_msgs::ImageConstPtr&depth_msg){//Precomputethesinfunctionforeachrowandcolumnwangchao预计算每行每列的正弦函数uint32_timage_width=depth_msg->width;ROS_INFO_THROTTLE(1,"image_width=%d",image_width);floatx_radians_per_pixel=60.0/57.0/image_width;//每个像素的弧度floatsin_pixel_x[image_width];for(intx=0;x//求出正弦值sin_pixel_x[x]=sin((x-image_width/2.0)*x_radians_per_pixel);}uint32_timage_height=depth_msg->height;floaty_radians_per_pixel=45.0/57.0/image_width;floatsin_pixel_y[image_height];for(inty=0;y//Signoppositexforyupvaluessin_pixel_y[y]=sin((image_height/2.0-y)*y_radians_per_pixel);}//X,Y,Zofthecentroid质心的xyzfloatx=0.0;floaty=0.0;floatz=1e6;//Numberofpointsobserved观察的点数unsignedintn=0;//Iteratethroughallthepointsintheregionandfindtheaverageoftheposition迭代通过该区域的所有点,找到位置的平均值constfloat*depth_row=reinterpret_cast(&depth_msg->data[0]);introw_step=depth_msg->step/sizeof(float);for(intv=0;v<(int)depth_msg->height;++v,depth_row+=row_step){for(intu=0;u<(int)depth_msg->width;++u){floatdepth=depth_image_proc::DepthTraits::toMeters(depth_row[u]);if(!depth_image_proc::DepthTraits::valid(depth)||depth>max_z_)continue;//不是有效的深度值或者深度超出max_z_floaty_val=sin_pixel_y[v]*depth;floatx_val=sin_pixel_x[u]*depth;if(y_val>min_y_&&y_valx_val>min_x_&&x_val{x+=x_val;y+=y_val;z=std::min(z,depth);//approximatedepthasforward.n++;}}}//Iftherearepoints,findthecentroidandcalculatethecommandgoal.//Iftherearenopoints,simplypublishastopgoal.//如果有点,找到质心并计算命令目标。如果没有点,只需发布停止消息。ROS_INFO_THROTTLE(1,"n==%d",n);if(n>4000){x/=n;y/=n;if(z>max_z_){ROS_INFO_THROTTLE(1,"Centroidtoofaraway%f,stoppingtherobot\n",z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}return;}ROS_INFO_THROTTLE(1,"Centroidat%f%f%fwith%dpoints",x,y,z,n);publishMarker(x,y,z);if(enabled_){geometry_msgs::TwistPtrcmd(newgeometry_msgs::Twist());cmd->linear.x=(z-goal_z_)*z_scale_;cmd->angular.z=-x*x_scale_;cmdpub_.publish(cmd);}}else{ROS_INFO_THROTTLE(1,"Notenoughpoints(%d)detected,stoppingtherobot",n);publishMarker(x,y,z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}}publishBbox();}boolchangeModeSrvCb(turtlebot_msgs::SetFollowState::Request&request,turtlebot_msgs::SetFollowState::Response&response){if((enabled_==true)&&(request.state==request.STOPPED)){ROS_INFO("Changemodeservicerequest:followingstopped");cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));enabled_=false;}elseif((enabled_==false)&&(request.state==request.FOLLOW)){ROS_INFO("Changemodeservicerequest:following(re)started");enabled_=true;}response.result=response.OK;returntrue;}//画一个圆点,这个圆点代表质心voidpublishMarker(doublex,doubley,doublez){visualization_msgs::Markermarker;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;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;//onlyifusingaMESH_RESOURCEmarkertype:markerpub_.publish(marker);}//画一个立方体,这个立方体代表感兴趣区域(RIO)voidpublishBbox(){doublex=(min_x_+max_x_)/2;doubley=(min_y_+max_y_)/2;doublez=(0+max_z_)/2;doublescale_x=(max_x_-x)*2;doublescale_y=(max_y_-y)*2;doublescale_z=(max_z_-z)*2;visualization_msgs::Markermarker;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;//onlyifusingaMESH_RESOURCEmarkertype:bboxpub_.publish(marker);}ros::Subscribersub_;ros::Publishercmdpub_;ros::Publishermarkerpub_;ros::Publisherbboxpub_;};PLUGINLIB_DECLARE_CLASS(turtlebot_follower,TurtlebotFollower,turtlebot_follower::TurtlebotFollower,nodelet::Nodelet);}接下来看launch文件follower.launch建议在修改前,将原先的代码注释掉,不要删掉。--Theturtlebotpeople(orwhatever)followernodelet.-->--Realrobot-->--modifyby2016.11.07启动我的机器人和摄像头,这里更换成你的机器人的启动文件和摄像头启动文件--> --将原先的注释掉--><
doublegoal_z_;/**doublez_scale_;/**doublex_scale_;/**boolenabled_;/**//Serviceforstart/stopfollowingros::ServiceServerswitch_srv_;//Dynamicreconfigureserver动态配置服务dynamic_reconfigure::Server:FollowerConfig>*config_srv_;/*!*@briefOnInitmethodfromnodehandle.*OnInitmethodfromnodehandle.Setsuptheparameters*andtopics.*初始化handle,参数,和话题*/virtualvoidonInit(){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:Twist>("/mobile_base/mobile_base_controller/cmd_vel",1);markerpub_=private_nh.advertise:Marker>("marker",1);bboxpub_=private_nh.advertise:Marker>("bbox",1);sub_=nh.subscribe:Image>("depth/image_rect",1,&TurtlebotFollower::imagecb,this);switch_srv_=private_nh.advertiseService("change_state",&TurtlebotFollower::changeModeSrvCb,this);config_srv_=newdynamic_reconfigure::Server:FollowerConfig>(private_nh);dynamic_reconfigure::Server:FollowerConfig>::CallbackTypef=boost::bind(&TurtlebotFollower::reconfigure,this,_1,_2);config_srv_->setCallback(f);}//设置默认值,详见catkin_ws/devel/include/turtlrbot_follower/FollowerConfig.hvoidreconfigure(turtlebot_follower::FollowerConfig&config,uint32_tlevel){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;}/*!*@briefCallbackforpointclouds.*Callbackfordepthimages.Itfindsthecentroid*ofthepointsinaboxinthecenteroftheimage.*它找到图像中心框中的点的质心*Publishescmd_velmessageswiththegoalfromtheimage.*发布图像中目标的cmd_vel消息*@paramcloudThepointcloudmessage.*参数:点云的消息*/voidimagecb(constsensor_msgs::ImageConstPtr&depth_msg){//Precomputethesinfunctionforeachrowandcolumnwangchao预计算每行每列的正弦函数uint32_timage_width=depth_msg->width;ROS_INFO_THROTTLE(1,"image_width=%d",image_width);floatx_radians_per_pixel=60.0/57.0/image_width;//每个像素的弧度floatsin_pixel_x[image_width];for(intx=0;x//求出正弦值sin_pixel_x[x]=sin((x-image_width/2.0)*x_radians_per_pixel);}uint32_timage_height=depth_msg->height;floaty_radians_per_pixel=45.0/57.0/image_width;floatsin_pixel_y[image_height];for(inty=0;y//Signoppositexforyupvaluessin_pixel_y[y]=sin((image_height/2.0-y)*y_radians_per_pixel);}//X,Y,Zofthecentroid质心的xyzfloatx=0.0;floaty=0.0;floatz=1e6;//Numberofpointsobserved观察的点数unsignedintn=0;//Iteratethroughallthepointsintheregionandfindtheaverageoftheposition迭代通过该区域的所有点,找到位置的平均值constfloat*depth_row=reinterpret_cast(&depth_msg->data[0]);introw_step=depth_msg->step/sizeof(float);for(intv=0;v<(int)depth_msg->height;++v,depth_row+=row_step){for(intu=0;u<(int)depth_msg->width;++u){floatdepth=depth_image_proc::DepthTraits::toMeters(depth_row[u]);if(!depth_image_proc::DepthTraits::valid(depth)||depth>max_z_)continue;//不是有效的深度值或者深度超出max_z_floaty_val=sin_pixel_y[v]*depth;floatx_val=sin_pixel_x[u]*depth;if(y_val>min_y_&&y_valx_val>min_x_&&x_val{x+=x_val;y+=y_val;z=std::min(z,depth);//approximatedepthasforward.n++;}}}//Iftherearepoints,findthecentroidandcalculatethecommandgoal.//Iftherearenopoints,simplypublishastopgoal.//如果有点,找到质心并计算命令目标。如果没有点,只需发布停止消息。ROS_INFO_THROTTLE(1,"n==%d",n);if(n>4000){x/=n;y/=n;if(z>max_z_){ROS_INFO_THROTTLE(1,"Centroidtoofaraway%f,stoppingtherobot\n",z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}return;}ROS_INFO_THROTTLE(1,"Centroidat%f%f%fwith%dpoints",x,y,z,n);publishMarker(x,y,z);if(enabled_){geometry_msgs::TwistPtrcmd(newgeometry_msgs::Twist());cmd->linear.x=(z-goal_z_)*z_scale_;cmd->angular.z=-x*x_scale_;cmdpub_.publish(cmd);}}else{ROS_INFO_THROTTLE(1,"Notenoughpoints(%d)detected,stoppingtherobot",n);publishMarker(x,y,z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}}publishBbox();}boolchangeModeSrvCb(turtlebot_msgs::SetFollowState::Request&request,turtlebot_msgs::SetFollowState::Response&response){if((enabled_==true)&&(request.state==request.STOPPED)){ROS_INFO("Changemodeservicerequest:followingstopped");cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));enabled_=false;}elseif((enabled_==false)&&(request.state==request.FOLLOW)){ROS_INFO("Changemodeservicerequest:following(re)started");enabled_=true;}response.result=response.OK;returntrue;}//画一个圆点,这个圆点代表质心voidpublishMarker(doublex,doubley,doublez){visualization_msgs::Markermarker;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;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;//onlyifusingaMESH_RESOURCEmarkertype:markerpub_.publish(marker);}//画一个立方体,这个立方体代表感兴趣区域(RIO)voidpublishBbox(){doublex=(min_x_+max_x_)/2;doubley=(min_y_+max_y_)/2;doublez=(0+max_z_)/2;doublescale_x=(max_x_-x)*2;doublescale_y=(max_y_-y)*2;doublescale_z=(max_z_-z)*2;visualization_msgs::Markermarker;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;//onlyifusingaMESH_RESOURCEmarkertype:bboxpub_.publish(marker);}ros::Subscribersub_;ros::Publishercmdpub_;ros::Publishermarkerpub_;ros::Publisherbboxpub_;};PLUGINLIB_DECLARE_CLASS(turtlebot_follower,TurtlebotFollower,turtlebot_follower::TurtlebotFollower,nodelet::Nodelet);}接下来看launch文件follower.launch建议在修改前,将原先的代码注释掉,不要删掉。--Theturtlebotpeople(orwhatever)followernodelet.-->--Realrobot-->--modifyby2016.11.07启动我的机器人和摄像头,这里更换成你的机器人的启动文件和摄像头启动文件--> --将原先的注释掉--><
doublez_scale_;/**doublex_scale_;/**boolenabled_;/**//Serviceforstart/stopfollowingros::ServiceServerswitch_srv_;//Dynamicreconfigureserver动态配置服务dynamic_reconfigure::Server:FollowerConfig>*config_srv_;/*!*@briefOnInitmethodfromnodehandle.*OnInitmethodfromnodehandle.Setsuptheparameters*andtopics.*初始化handle,参数,和话题*/virtualvoidonInit(){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:Twist>("/mobile_base/mobile_base_controller/cmd_vel",1);markerpub_=private_nh.advertise:Marker>("marker",1);bboxpub_=private_nh.advertise:Marker>("bbox",1);sub_=nh.subscribe:Image>("depth/image_rect",1,&TurtlebotFollower::imagecb,this);switch_srv_=private_nh.advertiseService("change_state",&TurtlebotFollower::changeModeSrvCb,this);config_srv_=newdynamic_reconfigure::Server:FollowerConfig>(private_nh);dynamic_reconfigure::Server:FollowerConfig>::CallbackTypef=boost::bind(&TurtlebotFollower::reconfigure,this,_1,_2);config_srv_->setCallback(f);}//设置默认值,详见catkin_ws/devel/include/turtlrbot_follower/FollowerConfig.hvoidreconfigure(turtlebot_follower::FollowerConfig&config,uint32_tlevel){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;}/*!*@briefCallbackforpointclouds.*Callbackfordepthimages.Itfindsthecentroid*ofthepointsinaboxinthecenteroftheimage.*它找到图像中心框中的点的质心*Publishescmd_velmessageswiththegoalfromtheimage.*发布图像中目标的cmd_vel消息*@paramcloudThepointcloudmessage.*参数:点云的消息*/voidimagecb(constsensor_msgs::ImageConstPtr&depth_msg){//Precomputethesinfunctionforeachrowandcolumnwangchao预计算每行每列的正弦函数uint32_timage_width=depth_msg->width;ROS_INFO_THROTTLE(1,"image_width=%d",image_width);floatx_radians_per_pixel=60.0/57.0/image_width;//每个像素的弧度floatsin_pixel_x[image_width];for(intx=0;x//求出正弦值sin_pixel_x[x]=sin((x-image_width/2.0)*x_radians_per_pixel);}uint32_timage_height=depth_msg->height;floaty_radians_per_pixel=45.0/57.0/image_width;floatsin_pixel_y[image_height];for(inty=0;y//Signoppositexforyupvaluessin_pixel_y[y]=sin((image_height/2.0-y)*y_radians_per_pixel);}//X,Y,Zofthecentroid质心的xyzfloatx=0.0;floaty=0.0;floatz=1e6;//Numberofpointsobserved观察的点数unsignedintn=0;//Iteratethroughallthepointsintheregionandfindtheaverageoftheposition迭代通过该区域的所有点,找到位置的平均值constfloat*depth_row=reinterpret_cast(&depth_msg->data[0]);introw_step=depth_msg->step/sizeof(float);for(intv=0;v<(int)depth_msg->height;++v,depth_row+=row_step){for(intu=0;u<(int)depth_msg->width;++u){floatdepth=depth_image_proc::DepthTraits::toMeters(depth_row[u]);if(!depth_image_proc::DepthTraits::valid(depth)||depth>max_z_)continue;//不是有效的深度值或者深度超出max_z_floaty_val=sin_pixel_y[v]*depth;floatx_val=sin_pixel_x[u]*depth;if(y_val>min_y_&&y_valx_val>min_x_&&x_val{x+=x_val;y+=y_val;z=std::min(z,depth);//approximatedepthasforward.n++;}}}//Iftherearepoints,findthecentroidandcalculatethecommandgoal.//Iftherearenopoints,simplypublishastopgoal.//如果有点,找到质心并计算命令目标。如果没有点,只需发布停止消息。ROS_INFO_THROTTLE(1,"n==%d",n);if(n>4000){x/=n;y/=n;if(z>max_z_){ROS_INFO_THROTTLE(1,"Centroidtoofaraway%f,stoppingtherobot\n",z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}return;}ROS_INFO_THROTTLE(1,"Centroidat%f%f%fwith%dpoints",x,y,z,n);publishMarker(x,y,z);if(enabled_){geometry_msgs::TwistPtrcmd(newgeometry_msgs::Twist());cmd->linear.x=(z-goal_z_)*z_scale_;cmd->angular.z=-x*x_scale_;cmdpub_.publish(cmd);}}else{ROS_INFO_THROTTLE(1,"Notenoughpoints(%d)detected,stoppingtherobot",n);publishMarker(x,y,z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}}publishBbox();}boolchangeModeSrvCb(turtlebot_msgs::SetFollowState::Request&request,turtlebot_msgs::SetFollowState::Response&response){if((enabled_==true)&&(request.state==request.STOPPED)){ROS_INFO("Changemodeservicerequest:followingstopped");cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));enabled_=false;}elseif((enabled_==false)&&(request.state==request.FOLLOW)){ROS_INFO("Changemodeservicerequest:following(re)started");enabled_=true;}response.result=response.OK;returntrue;}//画一个圆点,这个圆点代表质心voidpublishMarker(doublex,doubley,doublez){visualization_msgs::Markermarker;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;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;//onlyifusingaMESH_RESOURCEmarkertype:markerpub_.publish(marker);}//画一个立方体,这个立方体代表感兴趣区域(RIO)voidpublishBbox(){doublex=(min_x_+max_x_)/2;doubley=(min_y_+max_y_)/2;doublez=(0+max_z_)/2;doublescale_x=(max_x_-x)*2;doublescale_y=(max_y_-y)*2;doublescale_z=(max_z_-z)*2;visualization_msgs::Markermarker;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;//onlyifusingaMESH_RESOURCEmarkertype:bboxpub_.publish(marker);}ros::Subscribersub_;ros::Publishercmdpub_;ros::Publishermarkerpub_;ros::Publisherbboxpub_;};PLUGINLIB_DECLARE_CLASS(turtlebot_follower,TurtlebotFollower,turtlebot_follower::TurtlebotFollower,nodelet::Nodelet);}接下来看launch文件follower.launch建议在修改前,将原先的代码注释掉,不要删掉。--Theturtlebotpeople(orwhatever)followernodelet.-->--Realrobot-->--modifyby2016.11.07启动我的机器人和摄像头,这里更换成你的机器人的启动文件和摄像头启动文件--> --将原先的注释掉--><
doublex_scale_;/**boolenabled_;/**//Serviceforstart/stopfollowingros::ServiceServerswitch_srv_;//Dynamicreconfigureserver动态配置服务dynamic_reconfigure::Server:FollowerConfig>*config_srv_;/*!*@briefOnInitmethodfromnodehandle.*OnInitmethodfromnodehandle.Setsuptheparameters*andtopics.*初始化handle,参数,和话题*/virtualvoidonInit(){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:Twist>("/mobile_base/mobile_base_controller/cmd_vel",1);markerpub_=private_nh.advertise:Marker>("marker",1);bboxpub_=private_nh.advertise:Marker>("bbox",1);sub_=nh.subscribe:Image>("depth/image_rect",1,&TurtlebotFollower::imagecb,this);switch_srv_=private_nh.advertiseService("change_state",&TurtlebotFollower::changeModeSrvCb,this);config_srv_=newdynamic_reconfigure::Server:FollowerConfig>(private_nh);dynamic_reconfigure::Server:FollowerConfig>::CallbackTypef=boost::bind(&TurtlebotFollower::reconfigure,this,_1,_2);config_srv_->setCallback(f);}//设置默认值,详见catkin_ws/devel/include/turtlrbot_follower/FollowerConfig.hvoidreconfigure(turtlebot_follower::FollowerConfig&config,uint32_tlevel){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;}/*!*@briefCallbackforpointclouds.*Callbackfordepthimages.Itfindsthecentroid*ofthepointsinaboxinthecenteroftheimage.*它找到图像中心框中的点的质心*Publishescmd_velmessageswiththegoalfromtheimage.*发布图像中目标的cmd_vel消息*@paramcloudThepointcloudmessage.*参数:点云的消息*/voidimagecb(constsensor_msgs::ImageConstPtr&depth_msg){//Precomputethesinfunctionforeachrowandcolumnwangchao预计算每行每列的正弦函数uint32_timage_width=depth_msg->width;ROS_INFO_THROTTLE(1,"image_width=%d",image_width);floatx_radians_per_pixel=60.0/57.0/image_width;//每个像素的弧度floatsin_pixel_x[image_width];for(intx=0;x//求出正弦值sin_pixel_x[x]=sin((x-image_width/2.0)*x_radians_per_pixel);}uint32_timage_height=depth_msg->height;floaty_radians_per_pixel=45.0/57.0/image_width;floatsin_pixel_y[image_height];for(inty=0;y//Signoppositexforyupvaluessin_pixel_y[y]=sin((image_height/2.0-y)*y_radians_per_pixel);}//X,Y,Zofthecentroid质心的xyzfloatx=0.0;floaty=0.0;floatz=1e6;//Numberofpointsobserved观察的点数unsignedintn=0;//Iteratethroughallthepointsintheregionandfindtheaverageoftheposition迭代通过该区域的所有点,找到位置的平均值constfloat*depth_row=reinterpret_cast(&depth_msg->data[0]);introw_step=depth_msg->step/sizeof(float);for(intv=0;v<(int)depth_msg->height;++v,depth_row+=row_step){for(intu=0;u<(int)depth_msg->width;++u){floatdepth=depth_image_proc::DepthTraits::toMeters(depth_row[u]);if(!depth_image_proc::DepthTraits::valid(depth)||depth>max_z_)continue;//不是有效的深度值或者深度超出max_z_floaty_val=sin_pixel_y[v]*depth;floatx_val=sin_pixel_x[u]*depth;if(y_val>min_y_&&y_valx_val>min_x_&&x_val{x+=x_val;y+=y_val;z=std::min(z,depth);//approximatedepthasforward.n++;}}}//Iftherearepoints,findthecentroidandcalculatethecommandgoal.//Iftherearenopoints,simplypublishastopgoal.//如果有点,找到质心并计算命令目标。如果没有点,只需发布停止消息。ROS_INFO_THROTTLE(1,"n==%d",n);if(n>4000){x/=n;y/=n;if(z>max_z_){ROS_INFO_THROTTLE(1,"Centroidtoofaraway%f,stoppingtherobot\n",z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}return;}ROS_INFO_THROTTLE(1,"Centroidat%f%f%fwith%dpoints",x,y,z,n);publishMarker(x,y,z);if(enabled_){geometry_msgs::TwistPtrcmd(newgeometry_msgs::Twist());cmd->linear.x=(z-goal_z_)*z_scale_;cmd->angular.z=-x*x_scale_;cmdpub_.publish(cmd);}}else{ROS_INFO_THROTTLE(1,"Notenoughpoints(%d)detected,stoppingtherobot",n);publishMarker(x,y,z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}}publishBbox();}boolchangeModeSrvCb(turtlebot_msgs::SetFollowState::Request&request,turtlebot_msgs::SetFollowState::Response&response){if((enabled_==true)&&(request.state==request.STOPPED)){ROS_INFO("Changemodeservicerequest:followingstopped");cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));enabled_=false;}elseif((enabled_==false)&&(request.state==request.FOLLOW)){ROS_INFO("Changemodeservicerequest:following(re)started");enabled_=true;}response.result=response.OK;returntrue;}//画一个圆点,这个圆点代表质心voidpublishMarker(doublex,doubley,doublez){visualization_msgs::Markermarker;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;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;//onlyifusingaMESH_RESOURCEmarkertype:markerpub_.publish(marker);}//画一个立方体,这个立方体代表感兴趣区域(RIO)voidpublishBbox(){doublex=(min_x_+max_x_)/2;doubley=(min_y_+max_y_)/2;doublez=(0+max_z_)/2;doublescale_x=(max_x_-x)*2;doublescale_y=(max_y_-y)*2;doublescale_z=(max_z_-z)*2;visualization_msgs::Markermarker;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;//onlyifusingaMESH_RESOURCEmarkertype:bboxpub_.publish(marker);}ros::Subscribersub_;ros::Publishercmdpub_;ros::Publishermarkerpub_;ros::Publisherbboxpub_;};PLUGINLIB_DECLARE_CLASS(turtlebot_follower,TurtlebotFollower,turtlebot_follower::TurtlebotFollower,nodelet::Nodelet);}接下来看launch文件follower.launch建议在修改前,将原先的代码注释掉,不要删掉。--Theturtlebotpeople(orwhatever)followernodelet.-->--Realrobot-->--modifyby2016.11.07启动我的机器人和摄像头,这里更换成你的机器人的启动文件和摄像头启动文件--> --将原先的注释掉--><
boolenabled_;/**//Serviceforstart/stopfollowingros::ServiceServerswitch_srv_;//Dynamicreconfigureserver动态配置服务dynamic_reconfigure::Server:FollowerConfig>*config_srv_;/*!*@briefOnInitmethodfromnodehandle.*OnInitmethodfromnodehandle.Setsuptheparameters*andtopics.*初始化handle,参数,和话题*/virtualvoidonInit(){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:Twist>("/mobile_base/mobile_base_controller/cmd_vel",1);markerpub_=private_nh.advertise:Marker>("marker",1);bboxpub_=private_nh.advertise:Marker>("bbox",1);sub_=nh.subscribe:Image>("depth/image_rect",1,&TurtlebotFollower::imagecb,this);switch_srv_=private_nh.advertiseService("change_state",&TurtlebotFollower::changeModeSrvCb,this);config_srv_=newdynamic_reconfigure::Server:FollowerConfig>(private_nh);dynamic_reconfigure::Server:FollowerConfig>::CallbackTypef=boost::bind(&TurtlebotFollower::reconfigure,this,_1,_2);config_srv_->setCallback(f);}//设置默认值,详见catkin_ws/devel/include/turtlrbot_follower/FollowerConfig.hvoidreconfigure(turtlebot_follower::FollowerConfig&config,uint32_tlevel){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;}/*!*@briefCallbackforpointclouds.*Callbackfordepthimages.Itfindsthecentroid*ofthepointsinaboxinthecenteroftheimage.*它找到图像中心框中的点的质心*Publishescmd_velmessageswiththegoalfromtheimage.*发布图像中目标的cmd_vel消息*@paramcloudThepointcloudmessage.*参数:点云的消息*/voidimagecb(constsensor_msgs::ImageConstPtr&depth_msg){//Precomputethesinfunctionforeachrowandcolumnwangchao预计算每行每列的正弦函数uint32_timage_width=depth_msg->width;ROS_INFO_THROTTLE(1,"image_width=%d",image_width);floatx_radians_per_pixel=60.0/57.0/image_width;//每个像素的弧度floatsin_pixel_x[image_width];for(intx=0;x//求出正弦值sin_pixel_x[x]=sin((x-image_width/2.0)*x_radians_per_pixel);}uint32_timage_height=depth_msg->height;floaty_radians_per_pixel=45.0/57.0/image_width;floatsin_pixel_y[image_height];for(inty=0;y//Signoppositexforyupvaluessin_pixel_y[y]=sin((image_height/2.0-y)*y_radians_per_pixel);}//X,Y,Zofthecentroid质心的xyzfloatx=0.0;floaty=0.0;floatz=1e6;//Numberofpointsobserved观察的点数unsignedintn=0;//Iteratethroughallthepointsintheregionandfindtheaverageoftheposition迭代通过该区域的所有点,找到位置的平均值constfloat*depth_row=reinterpret_cast(&depth_msg->data[0]);introw_step=depth_msg->step/sizeof(float);for(intv=0;v<(int)depth_msg->height;++v,depth_row+=row_step){for(intu=0;u<(int)depth_msg->width;++u){floatdepth=depth_image_proc::DepthTraits::toMeters(depth_row[u]);if(!depth_image_proc::DepthTraits::valid(depth)||depth>max_z_)continue;//不是有效的深度值或者深度超出max_z_floaty_val=sin_pixel_y[v]*depth;floatx_val=sin_pixel_x[u]*depth;if(y_val>min_y_&&y_valx_val>min_x_&&x_val{x+=x_val;y+=y_val;z=std::min(z,depth);//approximatedepthasforward.n++;}}}//Iftherearepoints,findthecentroidandcalculatethecommandgoal.//Iftherearenopoints,simplypublishastopgoal.//如果有点,找到质心并计算命令目标。如果没有点,只需发布停止消息。ROS_INFO_THROTTLE(1,"n==%d",n);if(n>4000){x/=n;y/=n;if(z>max_z_){ROS_INFO_THROTTLE(1,"Centroidtoofaraway%f,stoppingtherobot\n",z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}return;}ROS_INFO_THROTTLE(1,"Centroidat%f%f%fwith%dpoints",x,y,z,n);publishMarker(x,y,z);if(enabled_){geometry_msgs::TwistPtrcmd(newgeometry_msgs::Twist());cmd->linear.x=(z-goal_z_)*z_scale_;cmd->angular.z=-x*x_scale_;cmdpub_.publish(cmd);}}else{ROS_INFO_THROTTLE(1,"Notenoughpoints(%d)detected,stoppingtherobot",n);publishMarker(x,y,z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}}publishBbox();}boolchangeModeSrvCb(turtlebot_msgs::SetFollowState::Request&request,turtlebot_msgs::SetFollowState::Response&response){if((enabled_==true)&&(request.state==request.STOPPED)){ROS_INFO("Changemodeservicerequest:followingstopped");cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));enabled_=false;}elseif((enabled_==false)&&(request.state==request.FOLLOW)){ROS_INFO("Changemodeservicerequest:following(re)started");enabled_=true;}response.result=response.OK;returntrue;}//画一个圆点,这个圆点代表质心voidpublishMarker(doublex,doubley,doublez){visualization_msgs::Markermarker;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;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;//onlyifusingaMESH_RESOURCEmarkertype:markerpub_.publish(marker);}//画一个立方体,这个立方体代表感兴趣区域(RIO)voidpublishBbox(){doublex=(min_x_+max_x_)/2;doubley=(min_y_+max_y_)/2;doublez=(0+max_z_)/2;doublescale_x=(max_x_-x)*2;doublescale_y=(max_y_-y)*2;doublescale_z=(max_z_-z)*2;visualization_msgs::Markermarker;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;//onlyifusingaMESH_RESOURCEmarkertype:bboxpub_.publish(marker);}ros::Subscribersub_;ros::Publishercmdpub_;ros::Publishermarkerpub_;ros::Publisherbboxpub_;};PLUGINLIB_DECLARE_CLASS(turtlebot_follower,TurtlebotFollower,turtlebot_follower::TurtlebotFollower,nodelet::Nodelet);}接下来看launch文件follower.launch建议在修改前,将原先的代码注释掉,不要删掉。--Theturtlebotpeople(orwhatever)followernodelet.-->--Realrobot-->--modifyby2016.11.07启动我的机器人和摄像头,这里更换成你的机器人的启动文件和摄像头启动文件--> --将原先的注释掉--><
//Serviceforstart/stopfollowing
ros:
:
ServiceServerswitch_srv_;
//Dynamicreconfigureserver动态配置服务
dynamic_reconfigure:
Server:FollowerConfig>*config_srv_;/*!*@briefOnInitmethodfromnodehandle.*OnInitmethodfromnodehandle.Setsuptheparameters*andtopics.*初始化handle,参数,和话题*/virtualvoidonInit(){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:Twist>("/mobile_base/mobile_base_controller/cmd_vel",1);markerpub_=private_nh.advertise:Marker>("marker",1);bboxpub_=private_nh.advertise:Marker>("bbox",1);sub_=nh.subscribe:Image>("depth/image_rect",1,&TurtlebotFollower::imagecb,this);switch_srv_=private_nh.advertiseService("change_state",&TurtlebotFollower::changeModeSrvCb,this);config_srv_=newdynamic_reconfigure::Server:FollowerConfig>(private_nh);dynamic_reconfigure::Server:FollowerConfig>::CallbackTypef=boost::bind(&TurtlebotFollower::reconfigure,this,_1,_2);config_srv_->setCallback(f);}//设置默认值,详见catkin_ws/devel/include/turtlrbot_follower/FollowerConfig.hvoidreconfigure(turtlebot_follower::FollowerConfig&config,uint32_tlevel){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;}/*!*@briefCallbackforpointclouds.*Callbackfordepthimages.Itfindsthecentroid*ofthepointsinaboxinthecenteroftheimage.*它找到图像中心框中的点的质心*Publishescmd_velmessageswiththegoalfromtheimage.*发布图像中目标的cmd_vel消息*@paramcloudThepointcloudmessage.*参数:点云的消息*/voidimagecb(constsensor_msgs::ImageConstPtr&depth_msg){//Precomputethesinfunctionforeachrowandcolumnwangchao预计算每行每列的正弦函数uint32_timage_width=depth_msg->width;ROS_INFO_THROTTLE(1,"image_width=%d",image_width);floatx_radians_per_pixel=60.0/57.0/image_width;//每个像素的弧度floatsin_pixel_x[image_width];for(intx=0;x//求出正弦值sin_pixel_x[x]=sin((x-image_width/2.0)*x_radians_per_pixel);}uint32_timage_height=depth_msg->height;floaty_radians_per_pixel=45.0/57.0/image_width;floatsin_pixel_y[image_height];for(inty=0;y//Signoppositexforyupvaluessin_pixel_y[y]=sin((image_height/2.0-y)*y_radians_per_pixel);}//X,Y,Zofthecentroid质心的xyzfloatx=0.0;floaty=0.0;floatz=1e6;//Numberofpointsobserved观察的点数unsignedintn=0;//Iteratethroughallthepointsintheregionandfindtheaverageoftheposition迭代通过该区域的所有点,找到位置的平均值constfloat*depth_row=reinterpret_cast(&depth_msg->data[0]);introw_step=depth_msg->step/sizeof(float);for(intv=0;v<(int)depth_msg->height;++v,depth_row+=row_step){for(intu=0;u<(int)depth_msg->width;++u){floatdepth=depth_image_proc::DepthTraits::toMeters(depth_row[u]);if(!depth_image_proc::DepthTraits::valid(depth)||depth>max_z_)continue;//不是有效的深度值或者深度超出max_z_floaty_val=sin_pixel_y[v]*depth;floatx_val=sin_pixel_x[u]*depth;if(y_val>min_y_&&y_valx_val>min_x_&&x_val{x+=x_val;y+=y_val;z=std::min(z,depth);//approximatedepthasforward.n++;}}}//Iftherearepoints,findthecentroidandcalculatethecommandgoal.//Iftherearenopoints,simplypublishastopgoal.//如果有点,找到质心并计算命令目标。如果没有点,只需发布停止消息。ROS_INFO_THROTTLE(1,"n==%d",n);if(n>4000){x/=n;y/=n;if(z>max_z_){ROS_INFO_THROTTLE(1,"Centroidtoofaraway%f,stoppingtherobot\n",z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}return;}ROS_INFO_THROTTLE(1,"Centroidat%f%f%fwith%dpoints",x,y,z,n);publishMarker(x,y,z);if(enabled_){geometry_msgs::TwistPtrcmd(newgeometry_msgs::Twist());cmd->linear.x=(z-goal_z_)*z_scale_;cmd->angular.z=-x*x_scale_;cmdpub_.publish(cmd);}}else{ROS_INFO_THROTTLE(1,"Notenoughpoints(%d)detected,stoppingtherobot",n);publishMarker(x,y,z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}}publishBbox();}boolchangeModeSrvCb(turtlebot_msgs::SetFollowState::Request&request,turtlebot_msgs::SetFollowState::Response&response){if((enabled_==true)&&(request.state==request.STOPPED)){ROS_INFO("Changemodeservicerequest:followingstopped");cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));enabled_=false;}elseif((enabled_==false)&&(request.state==request.FOLLOW)){ROS_INFO("Changemodeservicerequest:following(re)started");enabled_=true;}response.result=response.OK;returntrue;}//画一个圆点,这个圆点代表质心voidpublishMarker(doublex,doubley,doublez){visualization_msgs::Markermarker;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;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;//onlyifusingaMESH_RESOURCEmarkertype:markerpub_.publish(marker);}//画一个立方体,这个立方体代表感兴趣区域(RIO)voidpublishBbox(){doublex=(min_x_+max_x_)/2;doubley=(min_y_+max_y_)/2;doublez=(0+max_z_)/2;doublescale_x=(max_x_-x)*2;doublescale_y=(max_y_-y)*2;doublescale_z=(max_z_-z)*2;visualization_msgs::Markermarker;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;//onlyifusingaMESH_RESOURCEmarkertype:bboxpub_.publish(marker);}ros::Subscribersub_;ros::Publishercmdpub_;ros::Publishermarkerpub_;ros::Publisherbboxpub_;};PLUGINLIB_DECLARE_CLASS(turtlebot_follower,TurtlebotFollower,turtlebot_follower::TurtlebotFollower,nodelet::Nodelet);}接下来看launch文件follower.launch建议在修改前,将原先的代码注释掉,不要删掉。--Theturtlebotpeople(orwhatever)followernodelet.-->--Realrobot-->--modifyby2016.11.07启动我的机器人和摄像头,这里更换成你的机器人的启动文件和摄像头启动文件--> --将原先的注释掉--><
FollowerConfig>*config_srv_;
/*!
*@briefOnInitmethodfromnodehandle.
*OnInitmethodfromnodehandle.Setsuptheparameters
*andtopics.
*初始化handle,参数,和话题
*/
virtualvoidonInit()
NodeHandle&nh=getNodeHandle();
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:Twist>("/mobile_base/mobile_base_controller/cmd_vel",1);markerpub_=private_nh.advertise:Marker>("marker",1);bboxpub_=private_nh.advertise:Marker>("bbox",1);sub_=nh.subscribe:Image>("depth/image_rect",1,&TurtlebotFollower::imagecb,this);switch_srv_=private_nh.advertiseService("change_state",&TurtlebotFollower::changeModeSrvCb,this);config_srv_=newdynamic_reconfigure::Server:FollowerConfig>(private_nh);dynamic_reconfigure::Server:FollowerConfig>::CallbackTypef=boost::bind(&TurtlebotFollower::reconfigure,this,_1,_2);config_srv_->setCallback(f);}//设置默认值,详见catkin_ws/devel/include/turtlrbot_follower/FollowerConfig.hvoidreconfigure(turtlebot_follower::FollowerConfig&config,uint32_tlevel){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;}/*!*@briefCallbackforpointclouds.*Callbackfordepthimages.Itfindsthecentroid*ofthepointsinaboxinthecenteroftheimage.*它找到图像中心框中的点的质心*Publishescmd_velmessageswiththegoalfromtheimage.*发布图像中目标的cmd_vel消息*@paramcloudThepointcloudmessage.*参数:点云的消息*/voidimagecb(constsensor_msgs::ImageConstPtr&depth_msg){//Precomputethesinfunctionforeachrowandcolumnwangchao预计算每行每列的正弦函数uint32_timage_width=depth_msg->width;ROS_INFO_THROTTLE(1,"image_width=%d",image_width);floatx_radians_per_pixel=60.0/57.0/image_width;//每个像素的弧度floatsin_pixel_x[image_width];for(intx=0;x//求出正弦值sin_pixel_x[x]=sin((x-image_width/2.0)*x_radians_per_pixel);}uint32_timage_height=depth_msg->height;floaty_radians_per_pixel=45.0/57.0/image_width;floatsin_pixel_y[image_height];for(inty=0;y//Signoppositexforyupvaluessin_pixel_y[y]=sin((image_height/2.0-y)*y_radians_per_pixel);}//X,Y,Zofthecentroid质心的xyzfloatx=0.0;floaty=0.0;floatz=1e6;//Numberofpointsobserved观察的点数unsignedintn=0;//Iteratethroughallthepointsintheregionandfindtheaverageoftheposition迭代通过该区域的所有点,找到位置的平均值constfloat*depth_row=reinterpret_cast(&depth_msg->data[0]);introw_step=depth_msg->step/sizeof(float);for(intv=0;v<(int)depth_msg->height;++v,depth_row+=row_step){for(intu=0;u<(int)depth_msg->width;++u){floatdepth=depth_image_proc::DepthTraits::toMeters(depth_row[u]);if(!depth_image_proc::DepthTraits::valid(depth)||depth>max_z_)continue;//不是有效的深度值或者深度超出max_z_floaty_val=sin_pixel_y[v]*depth;floatx_val=sin_pixel_x[u]*depth;if(y_val>min_y_&&y_valx_val>min_x_&&x_val{x+=x_val;y+=y_val;z=std::min(z,depth);//approximatedepthasforward.n++;}}}//Iftherearepoints,findthecentroidandcalculatethecommandgoal.//Iftherearenopoints,simplypublishastopgoal.//如果有点,找到质心并计算命令目标。如果没有点,只需发布停止消息。ROS_INFO_THROTTLE(1,"n==%d",n);if(n>4000){x/=n;y/=n;if(z>max_z_){ROS_INFO_THROTTLE(1,"Centroidtoofaraway%f,stoppingtherobot\n",z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}return;}ROS_INFO_THROTTLE(1,"Centroidat%f%f%fwith%dpoints",x,y,z,n);publishMarker(x,y,z);if(enabled_){geometry_msgs::TwistPtrcmd(newgeometry_msgs::Twist());cmd->linear.x=(z-goal_z_)*z_scale_;cmd->angular.z=-x*x_scale_;cmdpub_.publish(cmd);}}else{ROS_INFO_THROTTLE(1,"Notenoughpoints(%d)detected,stoppingtherobot",n);publishMarker(x,y,z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}}publishBbox();}boolchangeModeSrvCb(turtlebot_msgs::SetFollowState::Request&request,turtlebot_msgs::SetFollowState::Response&response){if((enabled_==true)&&(request.state==request.STOPPED)){ROS_INFO("Changemodeservicerequest:followingstopped");cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));enabled_=false;}elseif((enabled_==false)&&(request.state==request.FOLLOW)){ROS_INFO("Changemodeservicerequest:following(re)started");enabled_=true;}response.result=response.OK;returntrue;}//画一个圆点,这个圆点代表质心voidpublishMarker(doublex,doubley,doublez){visualization_msgs::Markermarker;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;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;//onlyifusingaMESH_RESOURCEmarkertype:markerpub_.publish(marker);}//画一个立方体,这个立方体代表感兴趣区域(RIO)voidpublishBbox(){doublex=(min_x_+max_x_)/2;doubley=(min_y_+max_y_)/2;doublez=(0+max_z_)/2;doublescale_x=(max_x_-x)*2;doublescale_y=(max_y_-y)*2;doublescale_z=(max_z_-z)*2;visualization_msgs::Markermarker;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;//onlyifusingaMESH_RESOURCEmarkertype:bboxpub_.publish(marker);}ros::Subscribersub_;ros::Publishercmdpub_;ros::Publishermarkerpub_;ros::Publisherbboxpub_;};PLUGINLIB_DECLARE_CLASS(turtlebot_follower,TurtlebotFollower,turtlebot_follower::TurtlebotFollower,nodelet::Nodelet);}接下来看launch文件follower.launch建议在修改前,将原先的代码注释掉,不要删掉。--Theturtlebotpeople(orwhatever)followernodelet.-->--Realrobot-->--modifyby2016.11.07启动我的机器人和摄像头,这里更换成你的机器人的启动文件和摄像头启动文件--> --将原先的注释掉--><
Twist>("/mobile_base/mobile_base_controller/cmd_vel",1);
markerpub_=private_nh.advertise:Marker>("marker",1);bboxpub_=private_nh.advertise:Marker>("bbox",1);sub_=nh.subscribe:Image>("depth/image_rect",1,&TurtlebotFollower::imagecb,this);switch_srv_=private_nh.advertiseService("change_state",&TurtlebotFollower::changeModeSrvCb,this);config_srv_=newdynamic_reconfigure::Server:FollowerConfig>(private_nh);dynamic_reconfigure::Server:FollowerConfig>::CallbackTypef=boost::bind(&TurtlebotFollower::reconfigure,this,_1,_2);config_srv_->setCallback(f);}//设置默认值,详见catkin_ws/devel/include/turtlrbot_follower/FollowerConfig.hvoidreconfigure(turtlebot_follower::FollowerConfig&config,uint32_tlevel){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;}/*!*@briefCallbackforpointclouds.*Callbackfordepthimages.Itfindsthecentroid*ofthepointsinaboxinthecenteroftheimage.*它找到图像中心框中的点的质心*Publishescmd_velmessageswiththegoalfromtheimage.*发布图像中目标的cmd_vel消息*@paramcloudThepointcloudmessage.*参数:点云的消息*/voidimagecb(constsensor_msgs::ImageConstPtr&depth_msg){//Precomputethesinfunctionforeachrowandcolumnwangchao预计算每行每列的正弦函数uint32_timage_width=depth_msg->width;ROS_INFO_THROTTLE(1,"image_width=%d",image_width);floatx_radians_per_pixel=60.0/57.0/image_width;//每个像素的弧度floatsin_pixel_x[image_width];for(intx=0;x//求出正弦值sin_pixel_x[x]=sin((x-image_width/2.0)*x_radians_per_pixel);}uint32_timage_height=depth_msg->height;floaty_radians_per_pixel=45.0/57.0/image_width;floatsin_pixel_y[image_height];for(inty=0;y//Signoppositexforyupvaluessin_pixel_y[y]=sin((image_height/2.0-y)*y_radians_per_pixel);}//X,Y,Zofthecentroid质心的xyzfloatx=0.0;floaty=0.0;floatz=1e6;//Numberofpointsobserved观察的点数unsignedintn=0;//Iteratethroughallthepointsintheregionandfindtheaverageoftheposition迭代通过该区域的所有点,找到位置的平均值constfloat*depth_row=reinterpret_cast(&depth_msg->data[0]);introw_step=depth_msg->step/sizeof(float);for(intv=0;v<(int)depth_msg->height;++v,depth_row+=row_step){for(intu=0;u<(int)depth_msg->width;++u){floatdepth=depth_image_proc::DepthTraits::toMeters(depth_row[u]);if(!depth_image_proc::DepthTraits::valid(depth)||depth>max_z_)continue;//不是有效的深度值或者深度超出max_z_floaty_val=sin_pixel_y[v]*depth;floatx_val=sin_pixel_x[u]*depth;if(y_val>min_y_&&y_valx_val>min_x_&&x_val{x+=x_val;y+=y_val;z=std::min(z,depth);//approximatedepthasforward.n++;}}}//Iftherearepoints,findthecentroidandcalculatethecommandgoal.//Iftherearenopoints,simplypublishastopgoal.//如果有点,找到质心并计算命令目标。如果没有点,只需发布停止消息。ROS_INFO_THROTTLE(1,"n==%d",n);if(n>4000){x/=n;y/=n;if(z>max_z_){ROS_INFO_THROTTLE(1,"Centroidtoofaraway%f,stoppingtherobot\n",z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}return;}ROS_INFO_THROTTLE(1,"Centroidat%f%f%fwith%dpoints",x,y,z,n);publishMarker(x,y,z);if(enabled_){geometry_msgs::TwistPtrcmd(newgeometry_msgs::Twist());cmd->linear.x=(z-goal_z_)*z_scale_;cmd->angular.z=-x*x_scale_;cmdpub_.publish(cmd);}}else{ROS_INFO_THROTTLE(1,"Notenoughpoints(%d)detected,stoppingtherobot",n);publishMarker(x,y,z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}}publishBbox();}boolchangeModeSrvCb(turtlebot_msgs::SetFollowState::Request&request,turtlebot_msgs::SetFollowState::Response&response){if((enabled_==true)&&(request.state==request.STOPPED)){ROS_INFO("Changemodeservicerequest:followingstopped");cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));enabled_=false;}elseif((enabled_==false)&&(request.state==request.FOLLOW)){ROS_INFO("Changemodeservicerequest:following(re)started");enabled_=true;}response.result=response.OK;returntrue;}//画一个圆点,这个圆点代表质心voidpublishMarker(doublex,doubley,doublez){visualization_msgs::Markermarker;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;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;//onlyifusingaMESH_RESOURCEmarkertype:markerpub_.publish(marker);}//画一个立方体,这个立方体代表感兴趣区域(RIO)voidpublishBbox(){doublex=(min_x_+max_x_)/2;doubley=(min_y_+max_y_)/2;doublez=(0+max_z_)/2;doublescale_x=(max_x_-x)*2;doublescale_y=(max_y_-y)*2;doublescale_z=(max_z_-z)*2;visualization_msgs::Markermarker;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;//onlyifusingaMESH_RESOURCEmarkertype:bboxpub_.publish(marker);}ros::Subscribersub_;ros::Publishercmdpub_;ros::Publishermarkerpub_;ros::Publisherbboxpub_;};PLUGINLIB_DECLARE_CLASS(turtlebot_follower,TurtlebotFollower,turtlebot_follower::TurtlebotFollower,nodelet::Nodelet);}接下来看launch文件follower.launch建议在修改前,将原先的代码注释掉,不要删掉。--Theturtlebotpeople(orwhatever)followernodelet.-->--Realrobot-->--modifyby2016.11.07启动我的机器人和摄像头,这里更换成你的机器人的启动文件和摄像头启动文件--> --将原先的注释掉--><
Marker>("marker",1);
bboxpub_=private_nh.advertise:Marker>("bbox",1);sub_=nh.subscribe:Image>("depth/image_rect",1,&TurtlebotFollower::imagecb,this);switch_srv_=private_nh.advertiseService("change_state",&TurtlebotFollower::changeModeSrvCb,this);config_srv_=newdynamic_reconfigure::Server:FollowerConfig>(private_nh);dynamic_reconfigure::Server:FollowerConfig>::CallbackTypef=boost::bind(&TurtlebotFollower::reconfigure,this,_1,_2);config_srv_->setCallback(f);}//设置默认值,详见catkin_ws/devel/include/turtlrbot_follower/FollowerConfig.hvoidreconfigure(turtlebot_follower::FollowerConfig&config,uint32_tlevel){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;}/*!*@briefCallbackforpointclouds.*Callbackfordepthimages.Itfindsthecentroid*ofthepointsinaboxinthecenteroftheimage.*它找到图像中心框中的点的质心*Publishescmd_velmessageswiththegoalfromtheimage.*发布图像中目标的cmd_vel消息*@paramcloudThepointcloudmessage.*参数:点云的消息*/voidimagecb(constsensor_msgs::ImageConstPtr&depth_msg){//Precomputethesinfunctionforeachrowandcolumnwangchao预计算每行每列的正弦函数uint32_timage_width=depth_msg->width;ROS_INFO_THROTTLE(1,"image_width=%d",image_width);floatx_radians_per_pixel=60.0/57.0/image_width;//每个像素的弧度floatsin_pixel_x[image_width];for(intx=0;x//求出正弦值sin_pixel_x[x]=sin((x-image_width/2.0)*x_radians_per_pixel);}uint32_timage_height=depth_msg->height;floaty_radians_per_pixel=45.0/57.0/image_width;floatsin_pixel_y[image_height];for(inty=0;y//Signoppositexforyupvaluessin_pixel_y[y]=sin((image_height/2.0-y)*y_radians_per_pixel);}//X,Y,Zofthecentroid质心的xyzfloatx=0.0;floaty=0.0;floatz=1e6;//Numberofpointsobserved观察的点数unsignedintn=0;//Iteratethroughallthepointsintheregionandfindtheaverageoftheposition迭代通过该区域的所有点,找到位置的平均值constfloat*depth_row=reinterpret_cast(&depth_msg->data[0]);introw_step=depth_msg->step/sizeof(float);for(intv=0;v<(int)depth_msg->height;++v,depth_row+=row_step){for(intu=0;u<(int)depth_msg->width;++u){floatdepth=depth_image_proc::DepthTraits::toMeters(depth_row[u]);if(!depth_image_proc::DepthTraits::valid(depth)||depth>max_z_)continue;//不是有效的深度值或者深度超出max_z_floaty_val=sin_pixel_y[v]*depth;floatx_val=sin_pixel_x[u]*depth;if(y_val>min_y_&&y_valx_val>min_x_&&x_val{x+=x_val;y+=y_val;z=std::min(z,depth);//approximatedepthasforward.n++;}}}//Iftherearepoints,findthecentroidandcalculatethecommandgoal.//Iftherearenopoints,simplypublishastopgoal.//如果有点,找到质心并计算命令目标。如果没有点,只需发布停止消息。ROS_INFO_THROTTLE(1,"n==%d",n);if(n>4000){x/=n;y/=n;if(z>max_z_){ROS_INFO_THROTTLE(1,"Centroidtoofaraway%f,stoppingtherobot\n",z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}return;}ROS_INFO_THROTTLE(1,"Centroidat%f%f%fwith%dpoints",x,y,z,n);publishMarker(x,y,z);if(enabled_){geometry_msgs::TwistPtrcmd(newgeometry_msgs::Twist());cmd->linear.x=(z-goal_z_)*z_scale_;cmd->angular.z=-x*x_scale_;cmdpub_.publish(cmd);}}else{ROS_INFO_THROTTLE(1,"Notenoughpoints(%d)detected,stoppingtherobot",n);publishMarker(x,y,z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}}publishBbox();}boolchangeModeSrvCb(turtlebot_msgs::SetFollowState::Request&request,turtlebot_msgs::SetFollowState::Response&response){if((enabled_==true)&&(request.state==request.STOPPED)){ROS_INFO("Changemodeservicerequest:followingstopped");cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));enabled_=false;}elseif((enabled_==false)&&(request.state==request.FOLLOW)){ROS_INFO("Changemodeservicerequest:following(re)started");enabled_=true;}response.result=response.OK;returntrue;}//画一个圆点,这个圆点代表质心voidpublishMarker(doublex,doubley,doublez){visualization_msgs::Markermarker;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;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;//onlyifusingaMESH_RESOURCEmarkertype:markerpub_.publish(marker);}//画一个立方体,这个立方体代表感兴趣区域(RIO)voidpublishBbox(){doublex=(min_x_+max_x_)/2;doubley=(min_y_+max_y_)/2;doublez=(0+max_z_)/2;doublescale_x=(max_x_-x)*2;doublescale_y=(max_y_-y)*2;doublescale_z=(max_z_-z)*2;visualization_msgs::Markermarker;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;//onlyifusingaMESH_RESOURCEmarkertype:bboxpub_.publish(marker);}ros::Subscribersub_;ros::Publishercmdpub_;ros::Publishermarkerpub_;ros::Publisherbboxpub_;};PLUGINLIB_DECLARE_CLASS(turtlebot_follower,TurtlebotFollower,turtlebot_follower::TurtlebotFollower,nodelet::Nodelet);}接下来看launch文件follower.launch建议在修改前,将原先的代码注释掉,不要删掉。--Theturtlebotpeople(orwhatever)followernodelet.-->--Realrobot-->--modifyby2016.11.07启动我的机器人和摄像头,这里更换成你的机器人的启动文件和摄像头启动文件--> --将原先的注释掉--><
Marker>("bbox",1);
sub_=nh.subscribe:Image>("depth/image_rect",1,&TurtlebotFollower::imagecb,this);switch_srv_=private_nh.advertiseService("change_state",&TurtlebotFollower::changeModeSrvCb,this);config_srv_=newdynamic_reconfigure::Server:FollowerConfig>(private_nh);dynamic_reconfigure::Server:FollowerConfig>::CallbackTypef=boost::bind(&TurtlebotFollower::reconfigure,this,_1,_2);config_srv_->setCallback(f);}//设置默认值,详见catkin_ws/devel/include/turtlrbot_follower/FollowerConfig.hvoidreconfigure(turtlebot_follower::FollowerConfig&config,uint32_tlevel){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;}/*!*@briefCallbackforpointclouds.*Callbackfordepthimages.Itfindsthecentroid*ofthepointsinaboxinthecenteroftheimage.*它找到图像中心框中的点的质心*Publishescmd_velmessageswiththegoalfromtheimage.*发布图像中目标的cmd_vel消息*@paramcloudThepointcloudmessage.*参数:点云的消息*/voidimagecb(constsensor_msgs::ImageConstPtr&depth_msg){//Precomputethesinfunctionforeachrowandcolumnwangchao预计算每行每列的正弦函数uint32_timage_width=depth_msg->width;ROS_INFO_THROTTLE(1,"image_width=%d",image_width);floatx_radians_per_pixel=60.0/57.0/image_width;//每个像素的弧度floatsin_pixel_x[image_width];for(intx=0;x//求出正弦值sin_pixel_x[x]=sin((x-image_width/2.0)*x_radians_per_pixel);}uint32_timage_height=depth_msg->height;floaty_radians_per_pixel=45.0/57.0/image_width;floatsin_pixel_y[image_height];for(inty=0;y//Signoppositexforyupvaluessin_pixel_y[y]=sin((image_height/2.0-y)*y_radians_per_pixel);}//X,Y,Zofthecentroid质心的xyzfloatx=0.0;floaty=0.0;floatz=1e6;//Numberofpointsobserved观察的点数unsignedintn=0;//Iteratethroughallthepointsintheregionandfindtheaverageoftheposition迭代通过该区域的所有点,找到位置的平均值constfloat*depth_row=reinterpret_cast(&depth_msg->data[0]);introw_step=depth_msg->step/sizeof(float);for(intv=0;v<(int)depth_msg->height;++v,depth_row+=row_step){for(intu=0;u<(int)depth_msg->width;++u){floatdepth=depth_image_proc::DepthTraits::toMeters(depth_row[u]);if(!depth_image_proc::DepthTraits::valid(depth)||depth>max_z_)continue;//不是有效的深度值或者深度超出max_z_floaty_val=sin_pixel_y[v]*depth;floatx_val=sin_pixel_x[u]*depth;if(y_val>min_y_&&y_valx_val>min_x_&&x_val{x+=x_val;y+=y_val;z=std::min(z,depth);//approximatedepthasforward.n++;}}}//Iftherearepoints,findthecentroidandcalculatethecommandgoal.//Iftherearenopoints,simplypublishastopgoal.//如果有点,找到质心并计算命令目标。如果没有点,只需发布停止消息。ROS_INFO_THROTTLE(1,"n==%d",n);if(n>4000){x/=n;y/=n;if(z>max_z_){ROS_INFO_THROTTLE(1,"Centroidtoofaraway%f,stoppingtherobot\n",z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}return;}ROS_INFO_THROTTLE(1,"Centroidat%f%f%fwith%dpoints",x,y,z,n);publishMarker(x,y,z);if(enabled_){geometry_msgs::TwistPtrcmd(newgeometry_msgs::Twist());cmd->linear.x=(z-goal_z_)*z_scale_;cmd->angular.z=-x*x_scale_;cmdpub_.publish(cmd);}}else{ROS_INFO_THROTTLE(1,"Notenoughpoints(%d)detected,stoppingtherobot",n);publishMarker(x,y,z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}}publishBbox();}boolchangeModeSrvCb(turtlebot_msgs::SetFollowState::Request&request,turtlebot_msgs::SetFollowState::Response&response){if((enabled_==true)&&(request.state==request.STOPPED)){ROS_INFO("Changemodeservicerequest:followingstopped");cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));enabled_=false;}elseif((enabled_==false)&&(request.state==request.FOLLOW)){ROS_INFO("Changemodeservicerequest:following(re)started");enabled_=true;}response.result=response.OK;returntrue;}//画一个圆点,这个圆点代表质心voidpublishMarker(doublex,doubley,doublez){visualization_msgs::Markermarker;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;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;//onlyifusingaMESH_RESOURCEmarkertype:markerpub_.publish(marker);}//画一个立方体,这个立方体代表感兴趣区域(RIO)voidpublishBbox(){doublex=(min_x_+max_x_)/2;doubley=(min_y_+max_y_)/2;doublez=(0+max_z_)/2;doublescale_x=(max_x_-x)*2;doublescale_y=(max_y_-y)*2;doublescale_z=(max_z_-z)*2;visualization_msgs::Markermarker;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;//onlyifusingaMESH_RESOURCEmarkertype:bboxpub_.publish(marker);}ros::Subscribersub_;ros::Publishercmdpub_;ros::Publishermarkerpub_;ros::Publisherbboxpub_;};PLUGINLIB_DECLARE_CLASS(turtlebot_follower,TurtlebotFollower,turtlebot_follower::TurtlebotFollower,nodelet::Nodelet);}接下来看launch文件follower.launch建议在修改前,将原先的代码注释掉,不要删掉。--Theturtlebotpeople(orwhatever)followernodelet.-->--Realrobot-->--modifyby2016.11.07启动我的机器人和摄像头,这里更换成你的机器人的启动文件和摄像头启动文件--> --将原先的注释掉--><
Image>("depth/image_rect",1,&TurtlebotFollower:
imagecb,this);
switch_srv_=private_nh.advertiseService("change_state",&TurtlebotFollower:
changeModeSrvCb,this);
config_srv_=newdynamic_reconfigure:
Server:FollowerConfig>(private_nh);dynamic_reconfigure::Server:FollowerConfig>::CallbackTypef=boost::bind(&TurtlebotFollower::reconfigure,this,_1,_2);config_srv_->setCallback(f);}//设置默认值,详见catkin_ws/devel/include/turtlrbot_follower/FollowerConfig.hvoidreconfigure(turtlebot_follower::FollowerConfig&config,uint32_tlevel){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;}/*!*@briefCallbackforpointclouds.*Callbackfordepthimages.Itfindsthecentroid*ofthepointsinaboxinthecenteroftheimage.*它找到图像中心框中的点的质心*Publishescmd_velmessageswiththegoalfromtheimage.*发布图像中目标的cmd_vel消息*@paramcloudThepointcloudmessage.*参数:点云的消息*/voidimagecb(constsensor_msgs::ImageConstPtr&depth_msg){//Precomputethesinfunctionforeachrowandcolumnwangchao预计算每行每列的正弦函数uint32_timage_width=depth_msg->width;ROS_INFO_THROTTLE(1,"image_width=%d",image_width);floatx_radians_per_pixel=60.0/57.0/image_width;//每个像素的弧度floatsin_pixel_x[image_width];for(intx=0;x//求出正弦值sin_pixel_x[x]=sin((x-image_width/2.0)*x_radians_per_pixel);}uint32_timage_height=depth_msg->height;floaty_radians_per_pixel=45.0/57.0/image_width;floatsin_pixel_y[image_height];for(inty=0;y//Signoppositexforyupvaluessin_pixel_y[y]=sin((image_height/2.0-y)*y_radians_per_pixel);}//X,Y,Zofthecentroid质心的xyzfloatx=0.0;floaty=0.0;floatz=1e6;//Numberofpointsobserved观察的点数unsignedintn=0;//Iteratethroughallthepointsintheregionandfindtheaverageoftheposition迭代通过该区域的所有点,找到位置的平均值constfloat*depth_row=reinterpret_cast(&depth_msg->data[0]);introw_step=depth_msg->step/sizeof(float);for(intv=0;v<(int)depth_msg->height;++v,depth_row+=row_step){for(intu=0;u<(int)depth_msg->width;++u){floatdepth=depth_image_proc::DepthTraits::toMeters(depth_row[u]);if(!depth_image_proc::DepthTraits::valid(depth)||depth>max_z_)continue;//不是有效的深度值或者深度超出max_z_floaty_val=sin_pixel_y[v]*depth;floatx_val=sin_pixel_x[u]*depth;if(y_val>min_y_&&y_valx_val>min_x_&&x_val{x+=x_val;y+=y_val;z=std::min(z,depth);//approximatedepthasforward.n++;}}}//Iftherearepoints,findthecentroidandcalculatethecommandgoal.//Iftherearenopoints,simplypublishastopgoal.//如果有点,找到质心并计算命令目标。如果没有点,只需发布停止消息。ROS_INFO_THROTTLE(1,"n==%d",n);if(n>4000){x/=n;y/=n;if(z>max_z_){ROS_INFO_THROTTLE(1,"Centroidtoofaraway%f,stoppingtherobot\n",z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}return;}ROS_INFO_THROTTLE(1,"Centroidat%f%f%fwith%dpoints",x,y,z,n);publishMarker(x,y,z);if(enabled_){geometry_msgs::TwistPtrcmd(newgeometry_msgs::Twist());cmd->linear.x=(z-goal_z_)*z_scale_;cmd->angular.z=-x*x_scale_;cmdpub_.publish(cmd);}}else{ROS_INFO_THROTTLE(1,"Notenoughpoints(%d)detected,stoppingtherobot",n);publishMarker(x,y,z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}}publishBbox();}boolchangeModeSrvCb(turtlebot_msgs::SetFollowState::Request&request,turtlebot_msgs::SetFollowState::Response&response){if((enabled_==true)&&(request.state==request.STOPPED)){ROS_INFO("Changemodeservicerequest:followingstopped");cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));enabled_=false;}elseif((enabled_==false)&&(request.state==request.FOLLOW)){ROS_INFO("Changemodeservicerequest:following(re)started");enabled_=true;}response.result=response.OK;returntrue;}//画一个圆点,这个圆点代表质心voidpublishMarker(doublex,doubley,doublez){visualization_msgs::Markermarker;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;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;//onlyifusingaMESH_RESOURCEmarkertype:markerpub_.publish(marker);}//画一个立方体,这个立方体代表感兴趣区域(RIO)voidpublishBbox(){doublex=(min_x_+max_x_)/2;doubley=(min_y_+max_y_)/2;doublez=(0+max_z_)/2;doublescale_x=(max_x_-x)*2;doublescale_y=(max_y_-y)*2;doublescale_z=(max_z_-z)*2;visualization_msgs::Markermarker;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;//onlyifusingaMESH_RESOURCEmarkertype:bboxpub_.publish(marker);}ros::Subscribersub_;ros::Publishercmdpub_;ros::Publishermarkerpub_;ros::Publisherbboxpub_;};PLUGINLIB_DECLARE_CLASS(turtlebot_follower,TurtlebotFollower,turtlebot_follower::TurtlebotFollower,nodelet::Nodelet);}接下来看launch文件follower.launch建议在修改前,将原先的代码注释掉,不要删掉。--Theturtlebotpeople(orwhatever)followernodelet.-->--Realrobot-->--modifyby2016.11.07启动我的机器人和摄像头,这里更换成你的机器人的启动文件和摄像头启动文件--> --将原先的注释掉--><
FollowerConfig>(private_nh);
Server:FollowerConfig>::CallbackTypef=boost::bind(&TurtlebotFollower::reconfigure,this,_1,_2);config_srv_->setCallback(f);}//设置默认值,详见catkin_ws/devel/include/turtlrbot_follower/FollowerConfig.hvoidreconfigure(turtlebot_follower::FollowerConfig&config,uint32_tlevel){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;}/*!*@briefCallbackforpointclouds.*Callbackfordepthimages.Itfindsthecentroid*ofthepointsinaboxinthecenteroftheimage.*它找到图像中心框中的点的质心*Publishescmd_velmessageswiththegoalfromtheimage.*发布图像中目标的cmd_vel消息*@paramcloudThepointcloudmessage.*参数:点云的消息*/voidimagecb(constsensor_msgs::ImageConstPtr&depth_msg){//Precomputethesinfunctionforeachrowandcolumnwangchao预计算每行每列的正弦函数uint32_timage_width=depth_msg->width;ROS_INFO_THROTTLE(1,"image_width=%d",image_width);floatx_radians_per_pixel=60.0/57.0/image_width;//每个像素的弧度floatsin_pixel_x[image_width];for(intx=0;x//求出正弦值sin_pixel_x[x]=sin((x-image_width/2.0)*x_radians_per_pixel);}uint32_timage_height=depth_msg->height;floaty_radians_per_pixel=45.0/57.0/image_width;floatsin_pixel_y[image_height];for(inty=0;y//Signoppositexforyupvaluessin_pixel_y[y]=sin((image_height/2.0-y)*y_radians_per_pixel);}//X,Y,Zofthecentroid质心的xyzfloatx=0.0;floaty=0.0;floatz=1e6;//Numberofpointsobserved观察的点数unsignedintn=0;//Iteratethroughallthepointsintheregionandfindtheaverageoftheposition迭代通过该区域的所有点,找到位置的平均值constfloat*depth_row=reinterpret_cast(&depth_msg->data[0]);introw_step=depth_msg->step/sizeof(float);for(intv=0;v<(int)depth_msg->height;++v,depth_row+=row_step){for(intu=0;u<(int)depth_msg->width;++u){floatdepth=depth_image_proc::DepthTraits::toMeters(depth_row[u]);if(!depth_image_proc::DepthTraits::valid(depth)||depth>max_z_)continue;//不是有效的深度值或者深度超出max_z_floaty_val=sin_pixel_y[v]*depth;floatx_val=sin_pixel_x[u]*depth;if(y_val>min_y_&&y_valx_val>min_x_&&x_val{x+=x_val;y+=y_val;z=std::min(z,depth);//approximatedepthasforward.n++;}}}//Iftherearepoints,findthecentroidandcalculatethecommandgoal.//Iftherearenopoints,simplypublishastopgoal.//如果有点,找到质心并计算命令目标。如果没有点,只需发布停止消息。ROS_INFO_THROTTLE(1,"n==%d",n);if(n>4000){x/=n;y/=n;if(z>max_z_){ROS_INFO_THROTTLE(1,"Centroidtoofaraway%f,stoppingtherobot\n",z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}return;}ROS_INFO_THROTTLE(1,"Centroidat%f%f%fwith%dpoints",x,y,z,n);publishMarker(x,y,z);if(enabled_){geometry_msgs::TwistPtrcmd(newgeometry_msgs::Twist());cmd->linear.x=(z-goal_z_)*z_scale_;cmd->angular.z=-x*x_scale_;cmdpub_.publish(cmd);}}else{ROS_INFO_THROTTLE(1,"Notenoughpoints(%d)detected,stoppingtherobot",n);publishMarker(x,y,z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}}publishBbox();}boolchangeModeSrvCb(turtlebot_msgs::SetFollowState::Request&request,turtlebot_msgs::SetFollowState::Response&response){if((enabled_==true)&&(request.state==request.STOPPED)){ROS_INFO("Changemodeservicerequest:followingstopped");cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));enabled_=false;}elseif((enabled_==false)&&(request.state==request.FOLLOW)){ROS_INFO("Changemodeservicerequest:following(re)started");enabled_=true;}response.result=response.OK;returntrue;}//画一个圆点,这个圆点代表质心voidpublishMarker(doublex,doubley,doublez){visualization_msgs::Markermarker;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;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;//onlyifusingaMESH_RESOURCEmarkertype:markerpub_.publish(marker);}//画一个立方体,这个立方体代表感兴趣区域(RIO)voidpublishBbox(){doublex=(min_x_+max_x_)/2;doubley=(min_y_+max_y_)/2;doublez=(0+max_z_)/2;doublescale_x=(max_x_-x)*2;doublescale_y=(max_y_-y)*2;doublescale_z=(max_z_-z)*2;visualization_msgs::Markermarker;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;//onlyifusingaMESH_RESOURCEmarkertype:bboxpub_.publish(marker);}ros::Subscribersub_;ros::Publishercmdpub_;ros::Publishermarkerpub_;ros::Publisherbboxpub_;};PLUGINLIB_DECLARE_CLASS(turtlebot_follower,TurtlebotFollower,turtlebot_follower::TurtlebotFollower,nodelet::Nodelet);}接下来看launch文件follower.launch建议在修改前,将原先的代码注释掉,不要删掉。--Theturtlebotpeople(orwhatever)followernodelet.-->--Realrobot-->--modifyby2016.11.07启动我的机器人和摄像头,这里更换成你的机器人的启动文件和摄像头启动文件--> --将原先的注释掉--><
FollowerConfig>:
CallbackTypef=
boost:
bind(&TurtlebotFollower:
reconfigure,this,_1,_2);
config_srv_->setCallback(f);
//设置默认值,详见catkin_ws/devel/include/turtlrbot_follower/FollowerConfig.h
voidreconfigure(turtlebot_follower:
FollowerConfig&config,uint32_tlevel)
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;
*@briefCallbackforpointclouds.
*Callbackfordepthimages.Itfindsthecentroid
*ofthepointsinaboxinthecenteroftheimage.
*它找到图像中心框中的点的质心
*Publishescmd_velmessageswiththegoalfromtheimage.
*发布图像中目标的cmd_vel消息
*@paramcloudThepointcloudmessage.
*参数:
点云的消息
voidimagecb(constsensor_msgs:
ImageConstPtr&depth_msg)
//Precomputethesinfunctionforeachrowandcolumnwangchao预计算每行每列的正弦函数
uint32_timage_width=depth_msg->width;
ROS_INFO_THROTTLE(1,"image_width=%d",image_width);
floatx_radians_per_pixel=60.0/57.0/image_width;//每个像素的弧度
floatsin_pixel_x[image_width];
for(intx=0;x//求出正弦值sin_pixel_x[x]=sin((x-image_width/2.0)*x_radians_per_pixel);}uint32_timage_height=depth_msg->height;floaty_radians_per_pixel=45.0/57.0/image_width;floatsin_pixel_y[image_height];for(inty=0;y//Signoppositexforyupvaluessin_pixel_y[y]=sin((image_height/2.0-y)*y_radians_per_pixel);}//X,Y,Zofthecentroid质心的xyzfloatx=0.0;floaty=0.0;floatz=1e6;//Numberofpointsobserved观察的点数unsignedintn=0;//Iteratethroughallthepointsintheregionandfindtheaverageoftheposition迭代通过该区域的所有点,找到位置的平均值constfloat*depth_row=reinterpret_cast(&depth_msg->data[0]);introw_step=depth_msg->step/sizeof(float);for(intv=0;v<(int)depth_msg->height;++v,depth_row+=row_step){for(intu=0;u<(int)depth_msg->width;++u){floatdepth=depth_image_proc::DepthTraits::toMeters(depth_row[u]);if(!depth_image_proc::DepthTraits::valid(depth)||depth>max_z_)continue;//不是有效的深度值或者深度超出max_z_floaty_val=sin_pixel_y[v]*depth;floatx_val=sin_pixel_x[u]*depth;if(y_val>min_y_&&y_valx_val>min_x_&&x_val{x+=x_val;y+=y_val;z=std::min(z,depth);//approximatedepthasforward.n++;}}}//Iftherearepoints,findthecentroidandcalculatethecommandgoal.//Iftherearenopoints,simplypublishastopgoal.//如果有点,找到质心并计算命令目标。如果没有点,只需发布停止消息。ROS_INFO_THROTTLE(1,"n==%d",n);if(n>4000){x/=n;y/=n;if(z>max_z_){ROS_INFO_THROTTLE(1,"Centroidtoofaraway%f,stoppingtherobot\n",z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}return;}ROS_INFO_THROTTLE(1,"Centroidat%f%f%fwith%dpoints",x,y,z,n);publishMarker(x,y,z);if(enabled_){geometry_msgs::TwistPtrcmd(newgeometry_msgs::Twist());cmd->linear.x=(z-goal_z_)*z_scale_;cmd->angular.z=-x*x_scale_;cmdpub_.publish(cmd);}}else{ROS_INFO_THROTTLE(1,"Notenoughpoints(%d)detected,stoppingtherobot",n);publishMarker(x,y,z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}}publishBbox();}boolchangeModeSrvCb(turtlebot_msgs::SetFollowState::Request&request,turtlebot_msgs::SetFollowState::Response&response){if((enabled_==true)&&(request.state==request.STOPPED)){ROS_INFO("Changemodeservicerequest:followingstopped");cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));enabled_=false;}elseif((enabled_==false)&&(request.state==request.FOLLOW)){ROS_INFO("Changemodeservicerequest:following(re)started");enabled_=true;}response.result=response.OK;returntrue;}//画一个圆点,这个圆点代表质心voidpublishMarker(doublex,doubley,doublez){visualization_msgs::Markermarker;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;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;//onlyifusingaMESH_RESOURCEmarkertype:markerpub_.publish(marker);}//画一个立方体,这个立方体代表感兴趣区域(RIO)voidpublishBbox(){doublex=(min_x_+max_x_)/2;doubley=(min_y_+max_y_)/2;doublez=(0+max_z_)/2;doublescale_x=(max_x_-x)*2;doublescale_y=(max_y_-y)*2;doublescale_z=(max_z_-z)*2;visualization_msgs::Markermarker;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;//onlyifusingaMESH_RESOURCEmarkertype:bboxpub_.publish(marker);}ros::Subscribersub_;ros::Publishercmdpub_;ros::Publishermarkerpub_;ros::Publisherbboxpub_;};PLUGINLIB_DECLARE_CLASS(turtlebot_follower,TurtlebotFollower,turtlebot_follower::TurtlebotFollower,nodelet::Nodelet);}接下来看launch文件follower.launch建议在修改前,将原先的代码注释掉,不要删掉。--Theturtlebotpeople(orwhatever)followernodelet.-->--Realrobot-->--modifyby2016.11.07启动我的机器人和摄像头,这里更换成你的机器人的启动文件和摄像头启动文件--> --将原先的注释掉--><
//求出正弦值
sin_pixel_x[x]=sin((x-image_width/2.0)*x_radians_per_pixel);
uint32_timage_height=depth_msg->height;
floaty_radians_per_pixel=45.0/57.0/image_width;
floatsin_pixel_y[image_height];
for(inty=0;y//Signoppositexforyupvaluessin_pixel_y[y]=sin((image_height/2.0-y)*y_radians_per_pixel);}//X,Y,Zofthecentroid质心的xyzfloatx=0.0;floaty=0.0;floatz=1e6;//Numberofpointsobserved观察的点数unsignedintn=0;//Iteratethroughallthepointsintheregionandfindtheaverageoftheposition迭代通过该区域的所有点,找到位置的平均值constfloat*depth_row=reinterpret_cast(&depth_msg->data[0]);introw_step=depth_msg->step/sizeof(float);for(intv=0;v<(int)depth_msg->height;++v,depth_row+=row_step){for(intu=0;u<(int)depth_msg->width;++u){floatdepth=depth_image_proc::DepthTraits::toMeters(depth_row[u]);if(!depth_image_proc::DepthTraits::valid(depth)||depth>max_z_)continue;//不是有效的深度值或者深度超出max_z_floaty_val=sin_pixel_y[v]*depth;floatx_val=sin_pixel_x[u]*depth;if(y_val>min_y_&&y_valx_val>min_x_&&x_val{x+=x_val;y+=y_val;z=std::min(z,depth);//approximatedepthasforward.n++;}}}//Iftherearepoints,findthecentroidandcalculatethecommandgoal.//Iftherearenopoints,simplypublishastopgoal.//如果有点,找到质心并计算命令目标。如果没有点,只需发布停止消息。ROS_INFO_THROTTLE(1,"n==%d",n);if(n>4000){x/=n;y/=n;if(z>max_z_){ROS_INFO_THROTTLE(1,"Centroidtoofaraway%f,stoppingtherobot\n",z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}return;}ROS_INFO_THROTTLE(1,"Centroidat%f%f%fwith%dpoints",x,y,z,n);publishMarker(x,y,z);if(enabled_){geometry_msgs::TwistPtrcmd(newgeometry_msgs::Twist());cmd->linear.x=(z-goal_z_)*z_scale_;cmd->angular.z=-x*x_scale_;cmdpub_.publish(cmd);}}else{ROS_INFO_THROTTLE(1,"Notenoughpoints(%d)detected,stoppingtherobot",n);publishMarker(x,y,z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}}publishBbox();}boolchangeModeSrvCb(turtlebot_msgs::SetFollowState::Request&request,turtlebot_msgs::SetFollowState::Response&response){if((enabled_==true)&&(request.state==request.STOPPED)){ROS_INFO("Changemodeservicerequest:followingstopped");cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));enabled_=false;}elseif((enabled_==false)&&(request.state==request.FOLLOW)){ROS_INFO("Changemodeservicerequest:following(re)started");enabled_=true;}response.result=response.OK;returntrue;}//画一个圆点,这个圆点代表质心voidpublishMarker(doublex,doubley,doublez){visualization_msgs::Markermarker;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;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;//onlyifusingaMESH_RESOURCEmarkertype:markerpub_.publish(marker);}//画一个立方体,这个立方体代表感兴趣区域(RIO)voidpublishBbox(){doublex=(min_x_+max_x_)/2;doubley=(min_y_+max_y_)/2;doublez=(0+max_z_)/2;doublescale_x=(max_x_-x)*2;doublescale_y=(max_y_-y)*2;doublescale_z=(max_z_-z)*2;visualization_msgs::Markermarker;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;//onlyifusingaMESH_RESOURCEmarkertype:bboxpub_.publish(marker);}ros::Subscribersub_;ros::Publishercmdpub_;ros::Publishermarkerpub_;ros::Publisherbboxpub_;};PLUGINLIB_DECLARE_CLASS(turtlebot_follower,TurtlebotFollower,turtlebot_follower::TurtlebotFollower,nodelet::Nodelet);}接下来看launch文件follower.launch建议在修改前,将原先的代码注释掉,不要删掉。--Theturtlebotpeople(orwhatever)followernodelet.-->--Realrobot-->--modifyby2016.11.07启动我的机器人和摄像头,这里更换成你的机器人的启动文件和摄像头启动文件--> --将原先的注释掉--><
//Signoppositexforyupvalues
sin_pixel_y[y]=sin((image_height/2.0-y)*y_radians_per_pixel);
//X,Y,Zofthecentroid质心的xyz
floatx=0.0;
floaty=0.0;
floatz=1e6;
//Numberofpointsobserved观察的点数
unsignedintn=0;
//Iteratethroughallthepointsintheregionandfindtheaverageoftheposition迭代通过该区域的所有点,找到位置的平均值
constfloat*depth_row=reinterpret_cast(&depth_msg->data[0]);
introw_step=depth_msg->step/sizeof(float);
for(intv=0;v<(int)depth_msg->height;++v,depth_row+=row_step)
for(intu=0;u<(int)depth_msg->width;++u)
floatdepth=depth_image_proc:
DepthTraits:
toMeters(depth_row[u]);
if(!
depth_image_proc:
valid(depth)||depth>max_z_)continue;//不是有效的深度值或者深度超出max_z_
floaty_val=sin_pixel_y[v]*depth;
floatx_val=sin_pixel_x[u]*depth;
if(y_val>min_y_&&y_valx_val>min_x_&&x_val{x+=x_val;y+=y_val;z=std::min(z,depth);//approximatedepthasforward.n++;}}}//Iftherearepoints,findthecentroidandcalculatethecommandgoal.//Iftherearenopoints,simplypublishastopgoal.//如果有点,找到质心并计算命令目标。如果没有点,只需发布停止消息。ROS_INFO_THROTTLE(1,"n==%d",n);if(n>4000){x/=n;y/=n;if(z>max_z_){ROS_INFO_THROTTLE(1,"Centroidtoofaraway%f,stoppingtherobot\n",z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}return;}ROS_INFO_THROTTLE(1,"Centroidat%f%f%fwith%dpoints",x,y,z,n);publishMarker(x,y,z);if(enabled_){geometry_msgs::TwistPtrcmd(newgeometry_msgs::Twist());cmd->linear.x=(z-goal_z_)*z_scale_;cmd->angular.z=-x*x_scale_;cmdpub_.publish(cmd);}}else{ROS_INFO_THROTTLE(1,"Notenoughpoints(%d)detected,stoppingtherobot",n);publishMarker(x,y,z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}}publishBbox();}boolchangeModeSrvCb(turtlebot_msgs::SetFollowState::Request&request,turtlebot_msgs::SetFollowState::Response&response){if((enabled_==true)&&(request.state==request.STOPPED)){ROS_INFO("Changemodeservicerequest:followingstopped");cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));enabled_=false;}elseif((enabled_==false)&&(request.state==request.FOLLOW)){ROS_INFO("Changemodeservicerequest:following(re)started");enabled_=true;}response.result=response.OK;returntrue;}//画一个圆点,这个圆点代表质心voidpublishMarker(doublex,doubley,doublez){visualization_msgs::Markermarker;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;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;//onlyifusingaMESH_RESOURCEmarkertype:markerpub_.publish(marker);}//画一个立方体,这个立方体代表感兴趣区域(RIO)voidpublishBbox(){doublex=(min_x_+max_x_)/2;doubley=(min_y_+max_y_)/2;doublez=(0+max_z_)/2;doublescale_x=(max_x_-x)*2;doublescale_y=(max_y_-y)*2;doublescale_z=(max_z_-z)*2;visualization_msgs::Markermarker;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;//onlyifusingaMESH_RESOURCEmarkertype:bboxpub_.publish(marker);}ros::Subscribersub_;ros::Publishercmdpub_;ros::Publishermarkerpub_;ros::Publisherbboxpub_;};PLUGINLIB_DECLARE_CLASS(turtlebot_follower,TurtlebotFollower,turtlebot_follower::TurtlebotFollower,nodelet::Nodelet);}接下来看launch文件follower.launch建议在修改前,将原先的代码注释掉,不要删掉。--Theturtlebotpeople(orwhatever)followernodelet.-->--Realrobot-->--modifyby2016.11.07启动我的机器人和摄像头,这里更换成你的机器人的启动文件和摄像头启动文件--> --将原先的注释掉--><
x_val>min_x_&&x_val{x+=x_val;y+=y_val;z=std::min(z,depth);//approximatedepthasforward.n++;}}}//Iftherearepoints,findthecentroidandcalculatethecommandgoal.//Iftherearenopoints,simplypublishastopgoal.//如果有点,找到质心并计算命令目标。如果没有点,只需发布停止消息。ROS_INFO_THROTTLE(1,"n==%d",n);if(n>4000){x/=n;y/=n;if(z>max_z_){ROS_INFO_THROTTLE(1,"Centroidtoofaraway%f,stoppingtherobot\n",z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}return;}ROS_INFO_THROTTLE(1,"Centroidat%f%f%fwith%dpoints",x,y,z,n);publishMarker(x,y,z);if(enabled_){geometry_msgs::TwistPtrcmd(newgeometry_msgs::Twist());cmd->linear.x=(z-goal_z_)*z_scale_;cmd->angular.z=-x*x_scale_;cmdpub_.publish(cmd);}}else{ROS_INFO_THROTTLE(1,"Notenoughpoints(%d)detected,stoppingtherobot",n);publishMarker(x,y,z);if(enabled_){cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));}}publishBbox();}boolchangeModeSrvCb(turtlebot_msgs::SetFollowState::Request&request,turtlebot_msgs::SetFollowState::Response&response){if((enabled_==true)&&(request.state==request.STOPPED)){ROS_INFO("Changemodeservicerequest:followingstopped");cmdpub_.publish(geometry_msgs::TwistPtr(newgeometry_msgs::Twist()));enabled_=false;}elseif((enabled_==false)&&(request.state==request.FOLLOW)){ROS_INFO("Changemodeservicerequest:following(re)started");enabled_=true;}response.result=response.OK;returntrue;}//画一个圆点,这个圆点代表质心voidpublishMarker(doublex,doubley,doublez){visualization_msgs::Markermarker;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;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;//onlyifusingaMESH_RESOURCEmarkertype:markerpub_.publish(marker);}//画一个立方体,这个立方体代表感兴趣区域(RIO)voidpublishBbox(){doublex=(min_x_+max_x_)/2;doubley=(min_y_+max_y_)/2;doublez=(0+max_z_)/2;doublescale_x=(max_x_-x)*2;doublescale_y=(max_y_-y)*2;doublescale_z=(max_z_-z)*2;visualization_msgs::Markermarker;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;//onlyifusingaMESH_RESOURCEmarkertype:bboxpub_.publish(marker);}ros::Subscribersub_;ros::Publishercmdpub_;ros::Publishermarkerpub_;ros::Publisherbboxpub_;};PLUGINLIB_DECLARE_CLASS(turtlebot_follower,TurtlebotFollower,turtlebot_follower::TurtlebotFollower,nodelet::Nodelet);}接下来看launch文件follower.launch建议在修改前,将原先的代码注释掉,不要删掉。--Theturtlebotpeople(orwhatever)followernodelet.-->--Realrobot-->--modifyby2016.11.07启动我的机器人和摄像头,这里更换成你的机器人的启动文件和摄像头启动文件--> --将原先的注释掉--><
x+=x_val;
y+=y_val;
z=std:
min(z,depth);//approximatedepthasforward.
n++;
//Iftherearepoints,findthecentroidandcalculatethecommandgoal.
//Iftherearenopoints,simplypublishastopgoal.
//如果有点,找到质心并计算命令目标。
如果没有点,只需发布停止消息。
ROS_INFO_THROTTLE(1,"n==%d",n);
if(n>4000)
x/=n;
y/=n;
if(z>max_z_){
ROS_INFO_THROTTLE(1,"Centroidtoofaraway%f,stoppingtherobot\n",z);
if(enabled_)
cmdpub_.publish(geometry_msgs:
TwistPtr(newgeometry_msgs:
Twist()));
return;
ROS_INFO_THROTTLE(1,"Centroidat%f%f%fwith%dpoints",x,y,z,n);
publishMarker(x,y,z);
geometry_msgs:
TwistPtrcmd(newgeometry_msgs:
Twist());
cmd->linear.x=(z-goal_z_)*z_scale_;
cmd->angular.z=-x*x_scale_;
cmdpub_.publish(cmd);
else
ROS_INFO_THROTTLE(1,"Notenoughpoints(%d)detected,stoppingtherobot",n);
publishBbox();
boolchangeModeSrvCb(turtlebot_msgs:
SetFollowState:
Request&request,
turtlebot_msgs:
Response&response)
if((enabled_==true)&&(request.state==request.STOPPED))
ROS_INFO("Changemodeservicerequest:
followingstopped");
enabled_=false;
elseif((enabled_==false)&&(request.state==request.FOLLOW))
following(re)started");
enabled_=true;
response.result=response.OK;
returntrue;
//画一个圆点,这个圆点代表质心
voidpublishMarker(doublex,doubley,doublez)
visualization_msgs:
Markermarker;
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:
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;
//onlyifusingaMESH_RESOURCEmarkertype:
markerpub_.publish(marker);
//画一个立方体,这个立方体代表感兴趣区域(RIO)
voidpublishBbox()
doublex=(min_x_+max_x_)/2;
doubley=(min_y_+max_y_)/2;
doublez=(0+max_z_)/2;
doublescale_x=(max_x_-x)*2;
doublescale_y=(max_y_-y)*2;
doublescale_z=(max_z_-z)*2;
marker.ns="my_namespace";
marker.id=1;
CUBE;
//设置标记物姿态
marker.pose.position.y=-y;
//设置标记物的尺寸大小
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;
bboxpub_.publish(marker);
Subscribersub_;
Publishercmdpub_;
Publishermarkerpub_;
Publisherbboxpub_;
};
PLUGINLIB_DECLARE_CLASS(turtlebot_follower,TurtlebotFollower,turtlebot_follower:
TurtlebotFollower,nodelet:
Nodelet);
接下来看launch文件follower.launch
建议在修改前,将原先的代码注释掉,不要删掉。
--
Theturtlebotpeople(orwhatever)followernodelet.
-->
--Realrobot-->--modifyby2016.11.07启动我的机器人和摄像头,这里更换成你的机器人的启动文件和摄像头启动文件--> --将原先的注释掉--><
--Realrobot-->
--modifyby2016.11.07启动我的机器人和摄像头,这里更换成你的机器人的启动文件和摄像头启动文件-->
--将原先的注释掉
<
copyright@ 2008-2022 冰豆网网站版权所有
经营许可证编号:鄂ICP备2022015515号-1