Problem with publishing Twist message from a node
I wrote a piece of C++ code which should publish twist message and make a fake turtlebot start moving in Rviz. But after running it, the robot had no reaction.Here is my code.
#define PI 3.14
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
int main(int argc, char **argv)
{
ros::init(argc, argv, "test");
ros::NodeHandle n;
ros::Publisher cmd_vel_pub=
n.advertise<geometry_msgs::Twist>("cmd_vel",5);
int rate=50;
ros::Rate loop_rate(rate);
double linear_speed=0.2;
double goal_distance=1.0;
double linear_duration=goal_distance/linear_speed;
double angular_speed=1.0;
double goal_angle=PI;
double angular_duration=goal_angel/angular_speed;
geometry_msgs::Twist twist_msg;
twist_msg.linear.x=twist_msg.linear.y=twist_msg.linear.z=0;
twist_msg.angular.x=twist_msg.angular.y=twist_msg.angular.z=0;
while(ros::ok())
{
twist_msg.linear.x=linear_speed;
int ticks=static_cast<int>(linear_duration*rate);
for(int j=0;j<ticks;j++)
{
cmd_vel_pub.publish(twist_msg);
loop_rate.sleep();
}
twist_msg.linear.x=0;
cmd_vel_pub.publish(twist_msg);
ros::Duration(1).sleep();
twist_msg.angular.z=angular_speed;
ticks=static_cast<int>(goal_angle*rate);
for(int j=0;j<ticks;j++)
{
cmd_vel_pub.publish(twist_msg);
loop_rate.sleep();
}
twist_msg.angular.z=0;
cmd_vel_pub.publish(twist_msg);
ros::Duration(1).sleep();
}
twist_msg.linear.x=0;
twist_msg.angular.z=0;
cmd_vel_pub.publish(twist_msg);
ros::Duration(1).sleep();
return 0;
}
I also add the following code in CMakeList.txt
include_directories(include ${catkin_INCLUDE_DIRS})
add_executable(test src/talker.cpp)
target_link_libraries(test ${catkin_LIBRARIES})
Add this to package.xml
<build_depend>geometry_msgs</build_depend>
<run_depend>geometry_msgs</run_depend>