Problem with publihing velocity,Pioneer 3dx doesnt stop.

edit

Hi everyone,

I have been trying to move my pioneer from point A to B and return back to point A. I have written a code which starts publishing velocity when the position (x,y,z) is zero. I want it to move to a distance say 0.2m in x direction and stop and then turn and come back to initial position. But the problem is after publishing first velocity command it does not publishes second command i.e. command to stop.

if (linearposx==0)
   {
     Velocity_pub.publish(velocity);
   }
    if(linearposx<=-0.1)
   {
      Velocity_pub.publish(Stop);
   }

Here is my complete code:

#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>





double quatx;
double quaty;
double quatz;
double quatw;
double linearposx;
double linearposy;



class PioneerData
{
 public: 
 void ComPoseCallback(const nav_msgs::Odometry::ConstPtr& msg);
};



int main(int argc, char **argv)
{
    PioneerData pioneer_Data;
    ros::init(argc, argv, "Test_FK"); 
    ros::NodeHandle n;                 
    ros::Publisher Velocity_pub = n.advertise<geometry_msgs::Twist>("/RosAria/cmd_vel",1000);
    ros::Subscriber ComPose_sub = n.subscribe("/RosAria/pose", 1000, &PioneerData::ComPoseCallback, &pioneer_Data);


    geometry_msgs::Twist velocity;
    velocity.linear.x =0.02;
    velocity.linear.y =0;
    velocity.linear.z =0;
    velocity.angular.x =0;
    velocity.angular.y =0;
    velocity.angular.z =0;

    geometry_msgs::Twist Stop;
    Stop.linear.x =0;
    Stop.linear.y =0;
    Stop.linear.z =0;
    Stop.angular.x =0;
    Stop.angular.y =0;
    Stop.angular.z =0;




   //ros::Rate loop_rate(10);
  //ros::Rate rate (1);

  while (ros::ok())
   {
    if(Velocity_pub.getNumSubscribers()>0)
    {
    if (linearposx==0)
    {

    Velocity_pub.publish(velocity);

    }
    if(linearposx>=0.1)
    {
      Velocity_pub.publish(Stop);
    }
    ros::spin();

    //sleep(100);
   //break;
    }
   }
  std::cout<<"xPosition"<<" "<<linearposx<<std::endl;   //ROS_INFO("Position-> x: [%f], y: [%f]",linearposx,linearposy);//return 0;
}

void PioneerData::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 1:

Through trial and error I came to know that I am not using ros::spin() or ros::spinOnce correctly. I am getting confused as to where to call ros::spin() or ros::spinOnce.