之前没有查找相关资料,过后发现有相似功能例程,网址: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.1415926 int 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.1415926 int 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; }