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

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

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

#include”turtlebot^follower/FollowerConfig.hM

depth-iinage_proc/depth-traits・h>

namespaceturtlcbot^follower

{

//*Theturtlebotfollowernodelet・

/**

*Theturtletotfollowernodelet.Subscribestopointclouds

*fromthe3dsensortprocessesthem,andpublishesconiniandvel

*messages・

*/

classTurtlcbotFollower:

publicnodelet:

:

Nodelet

public:

/*!

*briefTheconstruetorforthefollower・

*Constructorforthefollower・

TurtlebotFollower0:

min」_（0・l）,max_y_（0・5）,

min_x_（-0.2）,max_x_（0・2）,

max_z_（0.8）,goa1_z.（0.6）,

WORD版木

z.scale.a.0）,x_scale_（5・0）

-TurtlcbotFollowerO

deleteconfig_srv_;

最大z位置，以上四个字段用来设置框的大小*/

doublegoal_z_;

/**<

Thedistanceawayfromtherobottoholdthecentroid离机器人的距离>

以保持质心*/

doublez_scale.;

Thescalingfactorfortranslationalrobotspeed移动机器人速度的缩放系数*/

doublex_scale_;

Thescalingfactorforrotationalrobotspeed旋转机器人速度的缩放系数*/

boolenabled—；

Enable/disablefollowing;

justpreventsmotorcommands启用/禁用追踪；

只是阻止电机命令，置为false后，机器人不会移动，/inobi1e_base/mobile_base_controllcr/cmd_ve1topic为空*/

//Serviceforstart/stopfollowingros:

ScrviccScrverswitch_srv_;

//Dynamicreconfigureserver动态配置服务

dynaniic.reconfigure:

Scrver<

turtlcbot_follower:

FollowcrConfig>

*config_srv_;

*briefOnlnitmethodfromnodehandle.

*Onlnitmethodfromnodehandle・Setsuptheparameters

*andtopics・

*初始化handle‘参数‘和话题

virtualvoidonlnitO

ros:

NodcHandle&

nh=getNodcHandleO;

NodcHandle&

private^nh=gctPrivateNodeHand1e（）;

〃从参数服务器换取设置的参数（launch文件中设置数值）

private.nh・getParam（'

min_yn,n）in_y_）;

privatc_nh・gctParain（'

rinax^yn,max_y_）;

privatc_nh・gctParam（'

rinin-.xn,inin_x_）;

privatc_nh・getParainC1max_xn,inax_x_）;

privatc_nh・gctParani（hmax^zn,max_z_）;

goaLzn,goaLz.）;

private_nh・gctParani（,rz^scale11,z_scale.）;

private^nh・gctParaniC1x^scale11,x_scale.）;

private^nh・getParam（nenabled%enabled.）;

//设置机器人移动的话题（用于机器人移动）：

/mobi1e^basc/mobile_base_controllci7cmd_vc1（换成你的机器人的移动topic）

cmdpub_=private.nh・advertise<

gcometry_rosgs:

Twist>

C/mobile_base/mobile_base^controllcr/cnid^ver1,1）;

markerpub.=private__nh・advertisc<

visua1ization_msgs:

Marker〉（"

marker”,1）;

bboxpub_=private^nh・advcrtisc<

visualization_msgs:

Markcr>

（"

bboxF\1）;

sub_=nh・subscribe<

sensorjnsgs:

Iniage>

（n（lcpth/imagc_rectn,1,

&

TurtlebotFollower:

imageeb,this）;

switch_srv-=private^nh・advertiseService（uchangerstate"

changcModcSrvCb,this）;

config_srv-=new

dynamic^rcconfigure:

turtlebot_follower:

Fo11owcrConfig>

（private_nh）:

dynamic.rcconfigure:

Scrvcr<

CalIbackTypef

boost:

bind（&

reconfigure,this,_1,_2）;

config_srv--〉sctCallback（f）;

}

//设置默认值，详见catkin_ws/dcvel/inclu（lc/turtlrlx）t..follower/FollowerConfig.hvoidreconfigure（turtlcbot^follower:

FollowerConfig&

con仃g,uint32_tlevel）

min_y一=config・min_y;

max_y_=config・inax_y;

min_x_=config.min__x;

max_x_=config・max_x;

max_z_=config・inax_z;

goal_z_=config・goal_z;

z_scalc_=config・z_scalc;

x_scalc_=config.x_scalc;

*briefCallbackforpointclouds・

*Callbackfordepthimages・Itfindsthecentroid

*ofthepointsinaboxinthecenteroftheimage・

*它找到图像中心框中的点的质心

*Publishescmd^velmessageswiththegoalfromtheimage・

*发布图像中目标的cmd.vel消息

*paramcloudThepointcloudmessage・

*参数：

点云的消息

voidimagccb（constsensor^msgs:

ImagcConstPtrfidcpthjnsg）

//Precomputethesinfunctionforeachrowandcolumnwangchao预计算每行每列的正弦函数

uint32-tiiiiage_width=dcpthjnsg->

width;

ROS_INFO_THROHLE（1,"

iinage_width=%dn,imagc.width）;

floatx_radians_pcr_pixcl=60.0/57.0/image.width;

//每个像素的弧度

floatsin_pixel_x[iinage^width];

for（intx=0;

x<

imagesidth;

++x）{

//求出正弦值

sin』ixcl_x[x]=sin（（x-iinage.width/2.0）*x_radians_per_pixel）;

uint32_timage^height=dcpth_msg->

hcight;

floaty_radians_pcr_pixcl=45.0/57.0/image.width;

floatsin_pixc1_y[image^hcight];

for（inty=0;

y<

image.hcight;

++y）{

//Sigiloppositexforyupvalues

sin_pixcl_y[y]=sin（（image^height/2.0一y）*y_radians_per_pixcl）;

//X,Y,Zofthecentroid质心的xyz

floatx=0.0;

floaty=0.0;

floatz=lc6;

//Numberofpointsobserved观察的点数

unsignedintn=0;

//Iteratethroughallthepointsintheregionandfindtheaverageoftheposition迭代通过该区域的所有点，找到位置的平均值

constfloat*depth^row=reintcrpret.cast^onstfloat*X&

de