Problem with publihing velocity,Pioneer 3dx doesnt stop

Question

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.

---

Originally posted by Tomm on ROS Answers with karma: 59 on 2016-07-06

Post score: 1

Answer 1

Yep, you are using ros.spin() wrong here. You can read up on it in the tutorial here : http://wiki.ros.org/roscpp/Overview/Callbacks%20and%20Spinning

Basically a call to ros.spin() will sleep until the node is killed. So your loop will not enter the second iteration. Also you are using the condition ros.ok() in your while loop. The problem with that is that it will not terminate until the ros node is terminated. So your cout will not be executed either.

And I see another logic error. Your global variable linearposx is tested in the while loop. Now, at the start linearposx = 0 therefore the velocity will be published. When it is published, linearposx is set to 0.02. Now, after that, neither linearposx == 0 or linearposx >= 0.1 will be evaluated true. You might want to rethink your design.

Hope this solves your problem!

---

Originally posted by janindu with karma: 849 on 2016-07-06

This answer was ACCEPTED on the original site

Post score: 1

---

Original comments

Comment by Tomm on 2016-07-08:
Thanks man. Code works !!