Ask Your Question
1

problem with subscribing to a custom message publishing topic

asked 2014-08-17 16:19:35 -0500

Bhargav gravatar image

updated 2014-08-17 22:46:36 -0500

ahendrix gravatar image

Hi I was going thorough this tutorial ` navigation/Tutorials/RobotSetup/Odom. Here i want to pass the parameters vx, vy and vth from other topic (/arduVel) publishing a custom message of type /robot_setup_tf/Vel Format of Custom message is

float32 velX

float32 velY

float32 veltheta

Here is the code i have written

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

#include <nav_msgs/Odometry.h>

#include "robot_setup_tf/Vel.h"

double vx, vy, vth;

void OdomCallback(const robot_setup_tf::Vel::ConstPtr& msg)
{

vx = msg->velX;

vy = msg->velY;

vth = msg->theta;

}


int main(int argc, char** argv){
  ros::init(argc, argv, "odometry_publisher");

  ros::NodeHandle n;
  ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
  tf::TransformBroadcaster odom_broadcaster;



  double x = 0.0;
  double y = 0.0;
  double th = 0.0;

  //double vx = velX;
  //double vy = velY;
  //double vth =    velth;

  ros::Time current_time, last_time;
  current_time = ros::Time::now();
  last_time = ros::Time::now();

  ros::Rate r(1.0);




  while(n.ok())

{

    ros::Subscriber sub = n.subscribe("arduVel", 50, OdomCallback);

    ros::spinOnce();               // check for incoming messages

    current_time = ros::Time::now();

    //compute odometry in a typical way given the velocities of the robot
    double dt = (current_time - last_time).toSec();
    double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
    double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
    double delta_th = vth * dt;

    x += delta_x;
    y += delta_y;
    th += delta_th;

    //since all odometry is 6DOF we'll need a quaternion created from yaw
    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);

    //first, we'll publish the transform over tf
    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.stamp = current_time;
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "base_link";

    odom_trans.transform.translation.x = x;
    odom_trans.transform.translation.y = y;
    odom_trans.transform.translation.z = 0.0;
    odom_trans.transform.rotation = odom_quat;

    //send the transform
    odom_broadcaster.sendTransform(odom_trans);

    //next, we'll publish the odometry message over ROS
    nav_msgs::Odometry odom;
    odom.header.stamp = current_time;
    odom.header.frame_id = "odom";

    //set the position
    odom.pose.pose.position.x = x;
    odom.pose.pose.position.y = y;
    odom.pose.pose.position.z = 0.0;
    odom.pose.pose.orientation = odom_quat;

    //set the velocity
    odom.child_frame_id = "base_link";
    odom.twist.twist.linear.x = vx;
    odom.twist.twist.linear.y = vy;
    odom.twist.twist.angular.z = vth;

    //publish the message
    odom_pub.publish(odom);

    last_time = current_time;
    r.sleep();
  }
}

When i see the rqt_graph the nodes are subscribing correctly. Also the desired values from /arduVel topic is getting published. But this value is not reflected in /odom or /tf topic. All the values in these topics remain 0 even if the /arduVel is publishing the values.

Please help me. Thank you! :)

`

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
4

answered 2014-08-17 16:50:19 -0500

paulbovbel gravatar image

updated 2014-08-17 18:19:13 -0500

Okay, a few things here:

1) (Minor) You don't necessarily need a custom message type for this, a Twist message is made to transmit exactly this kind of information

2) Currently, you're running a loop that is constantly resubscribing to the topic, and then publishing odom/tf, whether you have received a message or not.

3_ In your main function, you should at most do the initial setup, configure the subscription callback, and call ros::spin(). Every time your message is received, the callback function will be processed, and that's where you can publish odometry message and transform.

Try refactoring the way I described in #3, and if it still doesn't work, update the question with your new code.

Also, please use the markdown formatting to make your code a little more readable too, it's very hard to understand when it's only partially formatted as above.

edit flag offensive delete link more

Comments

Hi paulbovbel, I am not very proficient with C++ coding, so i could not actually implement what you mentioned in 3rd point. I tried doing this

  1. shifting the subscriber in the main function ros::Subscriber sub = n.subscribe("arduVel", 50, OdomCallback);
  2. using a ros::spin() after that. This didn't help. Another thing that i tried,

  3. shifting the subscriber in main function and using ros::spinonce() in while loop. That didn't help either. Please suggest the section of code where should i make changes. Thanks a lot for supporting! :)

Bhargav gravatar image Bhargav  ( 2014-08-19 13:17:58 -0500 )edit

Thank you i solved it. Its now listening to topics! i used the subscriber in main function and everything as it is.

Bhargav gravatar image Bhargav  ( 2014-08-19 13:30:59 -0500 )edit
1

That's a start. Read about how c++ works in terms of: scope of variables, blocking functions, and class definitions.

paulbovbel gravatar image paulbovbel  ( 2014-08-19 14:48:01 -0500 )edit

Without getting into anything too fancy, here is a simple pattern you could use: https://gist.github.com/paulbovbel/63...

Here is something more fancy, though not perfect by any means: https://github.com/paulbovbel/nav2_pl...

paulbovbel gravatar image paulbovbel  ( 2014-08-19 14:49:22 -0500 )edit

Thank you for your help! :)

Bhargav gravatar image Bhargav  ( 2014-08-19 15:05:59 -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: 2014-08-17 16:19:35 -0500

Seen: 2,263 times

Last updated: Aug 17 '14