Ask Your Question
1

Problem with publihing velocity,Pioneer 3dx doesnt stop.

asked 2016-07-06 14:52:26 -0500

Tomm gravatar image

updated 2016-07-06 17:43:21 -0500

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.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2016-07-06 22:29:50 -0500

janindu gravatar image

updated 2016-07-06 22:31:22 -0500

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

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!

edit flag offensive delete link more

Comments

Thanks man. Code works !!

Tomm gravatar image Tomm  ( 2016-07-08 00:23:03 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2016-07-06 14:52:26 -0500

Seen: 107 times

Last updated: Jul 06 '16