How to use ros::spin() and values from call back function?
Hi everyone,
I am trying to move my pioneer 3dx robot. I have written a code and it compiles without any error but does not move my robot. I think it has something to do with the call back function of ros::spin(). Please have a look at my code and suggest any corrections : short term goal is to drive the robot to a point and come back to its initial position [straight path]. I want to get linearposx, linearposy and yaw, so that that I can use them in main function of any other function.
One more thing, when I run the code robot does not because of which all the value I print are zero.
#include "ros/ros.h"
#include "nav_msgs/Odometry.h"
#include <ros/console.h>
#include "geometry_msgs/Pose.h"
#include "geometry_msgs/Twist.h"
#include <tf/transform_datatypes.h>
void ComPoseCallback(const nav_msgs::Odometry::ConstPtr& msg);
double quatx;
double quaty;
double quatz;
double quatw;
double linearposx;
double linearposy;
int main(int argc, char **argv)
{
ros::init(argc, argv, "Test_FK");
ros::NodeHandle n;
ros::Subscriber ComPose_sub = n.subscribe("/RosAria/pose", 1000, ComPoseCallback);
ros::Publisher Velocity_pub = n.advertise<geometry_msgs::Twist>("/RosAria/cmd_vel", 1000);
geometry_msgs::Twist velocity;
velocity.linear.x=0.1;
if(linearposx>=0)
{
Velocity_pub.publish(velocity);
}
ros::spin();
}
void ComPoseCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
ROS_INFO("Seq: [%d]", msg->header.seq);
ROS_INFO("Position-> x: [%f], y: [%f], z: [%f]", msg->pose.pose.position.x,msg->pose.pose.position.y, msg->pose.pose.position.z);
ROS_INFO("Orientation-> x: [%f], y: [%f], z: [%f], w: [%f]", msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w);
linearposx=msg->pose.pose.position.x;
linearposy=msg->pose.pose.position.y;
quatx= msg->pose.pose.orientation.x;
quaty= msg->pose.pose.orientation.y;
quatz= msg->pose.pose.orientation.z;
quatw= msg->pose.pose.orientation.w;
tf::Quaternion q(quatx, quaty, quatz, quatw);
tf::Matrix3x3 m(q);
double roll, pitch, yaw;
m.getEulerYPR(yaw, pitch, roll); //getRPY
ROS_INFO("Yaw: [%f],Pitch: [%f],Roll: [%f]",yaw,pitch,roll);
return ;
}
EDIT: If I remove this part then also it does not run. I think the problem is in publisher.
if(linearposx>=0)
{
}