ROSturtlebotfollower让机器人跟随我们移动.docx
- 文档编号:24758904
- 上传时间:2023-06-01
- 格式:DOCX
- 页数:17
- 大小:377.24KB
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/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.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 //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 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_val 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启动我的机器人和摄像头,这里更换成你的机器人的启动文件和摄像头启动文件--> --将原先的注释掉 --> <
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- ROSturtlebotfollower 机器人 跟随 我们 移动