How to call callback at a specific period?
I start learning ROS and these are my practice for subscribe callback. If my question is not clear, please let me know. I write a program to move robot for a certain distance and turn a certain angle. I have this part working. Beside that I would like to print out current information of robot before movement and after movement. I tried with two methods but none of them works.
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "turtlesim/Pose.h"
//#include "ros/callback_queue.h"
//to print current position
void poseCallback(const turtlesim::Pose& msg)
{
ROS_INFO_STREAM("Current Position: " << msg.x << ", " << msg.y << ", Direction: " << msg.theta);
}
int main(int argc, char **argv){
const double FORWARD_SPEED_MPS = 0.1;
const double PI = 3.14159265358979323846;
const double TURN_SPEED_RPS = PI/20;
ros::init(argc, argv, "move_turtle");
ros::NodeHandle node;
ros::Publisher pub = node.advertise<geometry_msgs::Twist>("turtle1/cmd_vel",10);
ros::Subscriber sub = node.subscribe("turtle1/pose",10,poseCallback);
geometry_msgs::Twist msg;
//ros::CallbackQueue my_queue;
//my_queue.callOne();
ros::spinOnce();
ros::Time start_time = ros::Time::now();
//Go straight 1m
ros::Rate rate(10);
while(ros::ok() && ros::Time::now() - start_time < ros::Duration(10)){
msg.linear.x = FORWARD_SPEED_MPS;
pub.publish(msg);
rate.sleep();
}
msg.linear.x = 0;
start_time = ros::Time::now();
//Turn 45deg
while(ros::ok() && ros::Time::now() - start_time < ros::Duration(5)){
msg.angular.z = TURN_SPEED_RPS;
pub.publish(msg);
rate.sleep();
}
msg.angular.z = 0;
//my_queue.callOne();
ros::spinOnce();
}
1st: I call spinOnce before and after while loop for moving and turning. With this it prints out a hole queue which includes 10 elements of pose information of robot before it move. But these information is printed out after it move.There is no pose information after robot move) Below is how it looks. Angle is start from 45deg so it has value 1.68.
viki@c3po:~/Documents/Workspace_IntroToRobotics$ rosrun turtle_move_package move_turtle [ INFO] [1458011783.446882695]: Current Position: 7.08517, 6.3085, Direction: 1.68138 [ INFO] [1458011783.447260117]: Current Position: 7.08517, 6.3085, Direction: 1.68389 [ INFO] [1458011783.447343927]: Current Position: 7.08517, 6.3085, Direction: 1.68641 [ INFO] [1458011783.447429971]: Current Position: 7.08517, 6.3085, Direction: 1.68892 [ INFO] [1458011783.447564346]: Current Position: 7.08517, 6.3085, Direction: 1.69143 [ INFO] [1458011783.447654022]: Current Position: 7.08517, 6.3085, Direction: 1.69395 [ INFO] [1458011783.447737273]: Current Position: 7.08517, 6.3085, Direction: 1.69646 [ INFO] [1458011783.447841755]: Current Position: 7.08517, 6.3085, Direction: 1.69897 [ INFO] [1458011783.447923889]: Current Position: 7.08517, 6.3085, Direction: 1.70149 [ INFO] [1458011783.447927520]: Current Position: 7.08517, 6.3085, Direction: 1.704
2nd: I tried to use callOne but it doesn't do anything. I comment it out in my above code.
Thank you for your time reading and answering my question.