ROS下实现timed_out_and_back功能

来源：互联网发布：淘宝商家怎么贷款编辑：程序博客网时间：2024/06/11 03:01

借鉴《ROS by example》里边的timed_out_and_back.py实例，在CPP下复现了一下：(基本流程参考发布节点过程)

中途参考了：

http://stackoverflow.com/questions/13213917/turtlebot-ros-moving-using-twist

http://www.clearpathrobotics.com/blog/ros-101-creating-node/

#include "ros/ros.h"//#include "std_msgs/String.h"//geometry_msgs#include "geometry_msgs/Twist.h"//包含elocity space消息#include "math.h"#include <sstream>#include <iostream>int main(int argc, char **argv){ros::init(argc,argv,"out_and_back");//指定节点“out_and_back”ros::NodeHandle n;//创造一个节点句柄ros::Publisher cmd_vel_pub=n.advertise<geometry_msgs::Twist>("/cmd_vel",1000);//将在/cmd_vel话题上发布一个geometry_msgs::Twist消息int rate=50;//定义更新频率ros::Rate loop_rate(rate);//更新频率50Hz，它会追踪记录自上一次调用Rate::sleep()后时间的流逝，并休眠直到一个频率周期的时间//初始化操作float linear_speed=0.2;//向前的线速度0.2m/sfloat goal_distance=1.0;//行进记录1.0mfloat linear_duration=goal_distance/linear_speed;//行进时间float angular_speed=1.0;//角度素1.0rad/sfloat goal_angle=M_PIl;//more /usr/include/math.h | grep pi可以查看pi的定义float angular_duration=goal_angle/angular_speed;//旋转时间geometry_msgs::Twist move_cmd;//定义消息对象move_cmd.linear.x=move_cmd.linear.y=move_cmd.linear.z=0;move_cmd.angular.x=move_cmd.angular.y=move_cmd.angular.z=0;int count=0;//记录循环次数while(ros::ok())//等待键盘ctrl+C操作则停止{std::cout<<"Hello world:"<<"The "<<count<<"th circle!"<<"\n";//向前运动move_cmd.linear.x=linear_speed;int ticks=int(linear_duration*rate);for(int i=0;i<ticks;i++){cmd_vel_pub.publish(move_cmd);loop_rate.sleep();}//旋转前停止move_cmd.linear.x=0;cmd_vel_pub.publish(move_cmd);ros::Duration(1).sleep(); //休眠1s//旋转180度move_cmd.angular.z=angular_speed;//设置角速度ticks=int(goal_angle*rate);for(int i=0;i<ticks;i++){cmd_vel_pub.publish(move_cmd);loop_rate.sleep();}//停止move_cmd.angular.z=0;cmd_vel_pub.publish(move_cmd);ros::Duration(1).sleep(); count++;} //使停止move_cmd.linear.x=0;move_cmd.angular.z=0;cmd_vel_pub.publish(move_cmd);ros::Duration(1).sleep(); return 0;}

最终结果：

Hello world:The 0th circle!
Hello world:The 1th circle!
Hello world:The 2th circle!
Hello world:The 3th circle!
Hello world:The 4th circle!

0 0