Ask Your Question
0

How to use ros::spin() and values from call back function?

asked 2016-06-28 14:02:40 -0500

Tomm gravatar image

updated 2016-06-28 14:34:00 -0500

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)
        {
         }
edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
1

answered 2016-06-28 18:28:17 -0500

jacobperron gravatar image

updated 2016-06-28 18:29:11 -0500

I believe there is a race condition between setting up the publisher and attempting to publish the message.

For example if you add a 1 second delay between advertising the topic and publishing you should see a successfully published message:

usleep(1000000);

You could also latch the topic (meaning it will always publish the last message sent to new subscribers), but maybe not the best idea for velocity commands. Add true as the last parameter:

ros::Publisher Velocity_pub = n.advertise<geometry_msgs::Twist>("/RosAria/cmd_vel", 1000, true);

But, if you are going to adaptively change the velocity based on the position anyways, you might want to have a loop somewhere inside you're main function that calls ros::spinOnce() instead of the blocking ros::spin(). See the roscpp Tutorial.

edit flag offensive delete link more

Comments

Thank you Jacobperron.Adding ros::spinOnce() sloved the problem.

Tomm gravatar image Tomm  ( 2016-06-29 13:01:02 -0500 )edit
1

answered 2016-06-28 14:21:03 -0500

billy gravatar image

So you don't publish unless linearposx >= 0, and your callback won't run without the publisher publishing (linearposx >= 0), and your callback sets linearposx. Am I reading that correctly? Catch 22?

Does C++ automatically initialize global doubles to 0? Even if it does, it seems like poor practice to not explicitly initialize it yourself.

Does the callback execute at all?

edit flag offensive delete link more

Comments

Oh that was a silly mistake. But then again when I try to publish the velocity, it does not move. Thanks for pointing out the mistake Billy.

Tomm gravatar image Tomm  ( 2016-06-28 14:30:58 -0500 )edit

IAnd yeah regarding callback function,I have checked it before (before publishing linear velocity) and it works fine. I used this command to publish velocity [Rosaria tutorial] rostopic pub -1 /RosAria/cmd_vel geometry_msgs/Twist '[0.1, 0.0, 0.0]' '[0.0, 0.0, 0.0]'

Tomm gravatar image Tomm  ( 2016-06-28 14:37:45 -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-06-28 14:02:40 -0500

Seen: 479 times

Last updated: Jun 28 '16