ROSturtlebotfollower让机器人跟随我们移动.docx

ROSturtlebotfollower让机器人跟随我们移动.docx

《ROSturtlebotfollower让机器人跟随我们移动.docx》由会员分享，可在线阅读，更多相关《ROSturtlebotfollower让机器人跟随我们移动.docx（17页珍藏版）》请在冰点文库上搜索。

ROSturtlebotfollower让机器人跟随我们移动

ROS-turtlebot-follower-：

让机器人跟随我们移动

ROSturtlebot_follower：

让机器人跟随我们移动

ROSturtlebot_follower学习

首先在catkin_ws/src目录下载源码

了解代码见注释（其中有些地方我也不是很明白）

follower.cpp

#include

#include

#include

#include

#include

#include

#include

#include"dynamic_reconfigure/server.h"

#include"turtlebot_follower/FollowerConfig.h"

#include

namespaceturtlebot_follower

{

//*Theturtlebotfollowernodelet.

/**

*Theturtlebotfollowernodelet.Subscribestopointclouds

*fromthe3dsensor,processesthem,andpublishescommandvel

*messages.

*/

classTurtlebotFollower:

publicnodelet:

:

Nodelet

{

public:

/*!

*@briefTheconstructorforthefollower.

*Constructorforthefollower.

*/

TurtlebotFollower（）:

min_y_（0.1）,max_y_（0.5）,

min_x_（-0.2）,max_x_（0.2）,

max_z_（0.8）,goal_z_（0.6）,

z_scale_（1.0）,x_scale_（5.0）

{

}

~TurtlebotFollower（）

{

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（&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_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-