ROS 에서 timed 실현out_and_back 기능

http://blog.csdn.net/scliu12345/article/details/44538927
#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>("/turtle1/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/s
    float goal_distance=1.0;//    1.0m
    float linear_duration=goal_distance/linear_speed;//     = 5s
    float angular_speed=1.0;//   1.0rad/s
    float goal_angle=M_PI;

	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+1<<"th circle!"<<"
"; // 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; }

좋은 웹페이지 즐겨찾기