c++实现ros by example volume1 例子timed_out_and_back功能

之前没有查找相关资料，过后发现有相似功能例程，网址：http://blog.csdn.net/scliu12345/article/details/44538927

之前以为ros::Rate 时间片用完后，会重新从while(ros::ok())开始，后来证明该想法是不对的，应该是从断点处继续执行。上文的网址博文有源码中关键部分的注释，我的只是一个不同思路，但建议用上文博文的方法。

cpp源文件放在在beginer_turtorials/src下。

编辑功能包目录中的CMakeList,txt文件，加入下面两行：

add_executable(move_my_turtlebot src/move_my_turtlebot.cpp)

target_link_libraries(move_my_turtlebot ${catkin_LIBRARIES})

回到工作环境目录

$ cd ~/catkin_ws

编译

$ catkin_make

运行是先launch主节点，我的是roslaunch turtlebot_bringup minimal.launch

然后，rosrun beginner_turtorials move_my_turtlebot

<pre name="code" class="cpp">// move_my_turtlebot.cpp : 定义控制台应用程序的入口点。//#include "ros/ros.h"#include "geometry_msgs/Twist.h"#define pi 3.1415926int main(int argc, char **argv){ros::init(argc,argv,"move_my_turtlebot");ros::NodeHandle n;ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel_mux/input/safety_controller",1);unsigned char rate;rate = 50;ros::Rate loop_rate(rate);float linear_speed, goal_distance, linear_duration, angular_speed, goal_angle, angular_duration;linear_speed = 0.2;goal_distance = 1.0;linear_duration = goal_distance / linear_speed;angular_speed = 1.0;goal_angle = pi;angular_duration = goal_angle / angular_speed;int linear_ticks,angular_ticks;linear_ticks = angular_ticks = 0;linear_ticks =int(linear_duration*rate);angular_ticks =int(angular_duration*rate);geometry_msgs::Twist msg;while (ros::ok()){if(linear_ticks != 0)//先前行{msg.linear.x = linear_speed;vel_pub.publish(msg);msg.linear.x = 0;ros::spinOnce();linear_ticks--;loop_rate.sleep();}else if (angular_ticks != 0)//后旋转{msg.angular.z = angular_speed;vel_pub.publish(msg);msg.angular.z = 0;ros::spinOnce();angular_ticks--;loop_rate.sleep();}else//退出{msg.linear.x = 0;msg.angular.z = 0;vel_pub.publish(msg);ros::spinOnce();loop_rate.sleep();ROS_INFO("%s", "Stopping the robot ...");break;}}return 0;}

好的实现应该是如下的，完成走出及回来的过程。

// move_my_turtlebot2.cpp//#include "ros/ros.h"#include "geometry_msgs/Twist.h"#define pi 3.1415926int main(int argc, char **argv){ros::init(argc,argv,"move_my_turtlebot2");ros::NodeHandle n;ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel_mux/input/safety_controller",1);unsigned char rate;rate = 50;ros::Rate loop_rate(rate);float linear_speed, goal_distance, linear_duration, angular_speed, goal_angle, angular_duration;linear_speed = 0.2;goal_distance = 1.0;linear_duration = goal_distance / linear_speed;angular_speed = 1.0;goal_angle = pi;angular_duration = goal_angle / angular_speed;int ticks;geometry_msgs::Twist msg;while (ros::ok()){for(char re=0;re<2;re++){msg.linear.x = linear_speed;int ticks=int(linear_duration*rate);for(int i=0;i<ticks;i++){vel_pub.publish(msg);loop_rate.sleep();}msg.linear.x = 0;vel_pub.publish(msg);ros::Duration(1).sleep();msg.angular.z = angular_speed;ticks=int(angular_duration*rate);for(int i=0;i<ticks;i++){vel_pub.publish(msg);loop_rate.sleep();}msg.angular.z = 0;vel_pub.publish(msg);ros::Duration(1).sleep();}msg.linear.x = 0;msg.angular.z = 0;vel_pub.publish(msg);ROS_INFO("%s", "Stopping the robot ...");break;}return 0;}