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.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;
}