Ask Your Question
2

Trying to use Timesynchronizer...getting compile error

asked 2013-09-25 11:21:39 -0500

rnunziata gravatar image

updated 2013-09-26 15:14:14 -0500

Trying to use Timesynchronizer...getting compile error. Do not know what the error is telling me. Changed pgm to use ConstPtrs still same error:

It seem based on this reference http://stackoverflow.com/questions/2304203/how-to-use-boost-bind-with-a-member-function Timesycronizer and other boost type templates are not officially supported by ROS within a class member function. Is my understand correct? If this is true what other interface to these functions is supported? Can the tutorial be updated to show how to use these in member functions or state that they are not available.

find_package(catkin REQUIRED COMPONENTS
  roscpp
  tf2_ros
  std_msgs
  geometry_msgs
  sensor_msgs
  )

catkin_package(
  DEPENDS boost
  LIBRARIES
  CATKIN_DEPENDS
  tf2_ros
  std_msgs
  geometry_msgs
  sensor_msgs
  )

Here is my code:

#include <string>
#include <ros/ros.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <std_msgs/String.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Quaternion.h>
#include <control_msgs/JointControllerState.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <sensor_msgs/JointState.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>


using namespace control_msgs;
using namespace message_filters;

class PublishOdometry
{
public:
  PublishOdometry()
  {

     count=0;
     motor_position_joint1_new =0;
     motor_position_joint1_old =0;
     motor_position_joint2_new =0;
     motor_position_joint2_old =0;
     x = 0;               
     y = 0;
     theta = 0;

     pubOdometry = n.advertise<nav_msgs::Odometry>("odom", 10);

     // Listen and wait for initial pose from nav
     subInitialPose = n.subscribe("/initialpose", 1, 
           &PublishOdometry::handelerInitialPose,this);
  }

void handelerOdometry(const JointControllerStateConstPtr& msg1, const JointControllerStateConstPtr& msg2)
{
     double wheel_track = 0.4;   //distance between two wheels

     motor_position_joint1_new = msg1->process_value;
     motor_position_joint2_new = msg2->process_value;

     ROS_INFO("J1PV=%f  J2PV=%f",motor_position_joint1_new, motor_position_joint2_new);


     if (timeold.toSec()==0)  // skip first set of joint cmd
     {
       timeold = msg1->header.stamp;
       motor_position_joint1_old = motor_position_joint1_new;
       motor_position_joint2_old = motor_position_joint2_new;
       return; 
     }

     float j1 = (motor_position_joint1_new - motor_position_joint1_old); 
     float j2 = (motor_position_joint2_new - motor_position_joint2_old);

     motor_position_joint1_old = motor_position_joint1_new;
     motor_position_joint2_old = motor_position_joint2_new;

     double  dt = (msg1->header.stamp - timeold).toSec();  // elapsed time in seconds
     float   distance =    ( ((j1 + j2)*dt) / 2.0);             // distance in meters
     float   dtheta   =    ( ((j1 - j2)*dt) / wheel_track);     // rotation

     double dx = (distance*sin(dtheta));
     double dy = (distance*cos(dtheta)); 

     theta =  (theta+dtheta); //fmodf((theta+dtheta),(2*M_PI));                   
     x = x + dx;
     y = y + dy;

     ROS_INFO("x=%e  y=%e theta=%f ddtheta=%f ",x,y,theta, dtheta);

     timeold = msg1->header.stamp;    // new old time

     geometry_msgs::Quaternion  quaternion;
     quaternion.x = 0.0;
     quaternion.y = 0.0; 
     quaternion.z = sin(theta/2);          // set yaw
     quaternion.w = cos(theta/2);

     geometry_msgs::TransformStamped odom_trans;
     odom_trans.header.frame_id = "odom";
     odom_trans.child_frame_id = "base_link";
     odom_trans.header.stamp =  timeold;
     odom_trans.transform.translation.x = x;
     odom_trans.transform.translation.y = y;
     odom_trans.transform.translation.z = 0.0; 
     odom_trans.transform.rotation = quaternion;

     odom_broadcaster.sendTransform(odom_trans);

     nav_msgs::Odometry odometry;
     odometry.header.frame_id = "odom";
     odometry.child_frame_id  = "base_link";
     odometry.header.stamp    = timeold;

     odometry.pose.pose.position.x  = distance;
     odometry.pose.pose.position.y  = y;
     odometry.pose.pose.position.z  = 0;
     odometry.pose.pose.orientation = quaternion;
     odometry.twist.twist.linear.x  = x;
     odometry.twist.twist.linear.y  = 0;
     odometry.twist.twist.angular.z = ((theta)*(M_PI/180));  // convert to radians

     pubOdometry.publish(odometry);

     calculateJointState(motor_position_joint1_old,"joint1");
     calculateJointState(motor_position_joint2_old,"joint2");
}



void calculateJointState(const float pv, const std::string name)
{
     sensor_msgs::JointState jointStateMsg;
     jointStateMsg.name.resize(1);
     jointStateMsg.position.resize(1);
     jointStateMsg.velocity.resize(1 ...
(more)
edit retag flag offensive close merge delete

Comments

I think your problem might be related to how boost::bind works with member functions: http://stackoverflow.com/questions/2304203/how-to-use-boost-bind-with-a-member-function

georgebrindeiro gravatar imagegeorgebrindeiro ( 2013-09-26 10:22:15 -0500 )edit

It seem based on this reference Timesycronizer and other boost type templates are not officially supported by ROS within a class member function. Is my understand correct? If this is true what other interface to these functions is supported.

rnunziata gravatar imagernunziata ( 2013-09-26 12:37:21 -0500 )edit

Yes...I tried adding it to several places....a working example would be great

rnunziata gravatar imagernunziata ( 2013-09-26 13:54:51 -0500 )edit

@georgebrindeiro is right. `this` is missing in the `bind()`. Every **method of a class** actually has extra argument which is the pointer to the instance of that class. When the method is called within another method of a class, `this` is passed implicitly. ROS require callback functions to have certain number of arguments, but it has no idea about the host class. So, when the callback is called the only parameters to be passed there are the messages and `this` is not provided. However your function can not work without `this`, because it will have no access to any of the class members. So `boost::bind` simply allows you to pass it as additional argument. Since you didn't do it the compiler complained.

Boris gravatar imageBoris ( 2013-09-26 17:13:38 -0500 )edit

Replacing the line for sync.registerCallback() with the sync.registerCallback(boost::bind(&PublishOdometry::handelerOdometry, this, _1, _2)); should be sufficient.

Boris gravatar imageBoris ( 2013-09-26 17:19:19 -0500 )edit

This solution worked and the code now compiles...thank you. How can this solution get into the tutorial so others like myself who are not steeped in c++ will not repeat this. Also can you make this an Answer.

rnunziata gravatar imagernunziata ( 2013-09-27 02:31:06 -0500 )edit

2 Answers

Sort by ยป oldest newest most voted
2

answered 2013-09-26 12:43:39 -0500

updated 2013-09-27 06:28:23 -0500

Have you tried adding this to the boost::bind function call? That's what they usually require for binding member functions. That might be the only thing missing.

Citing Boris' comment, this is how you would do it:

sync.registerCallback(boost::bind(&PublishOdometry::handelerOdometry, this, _1, _2));
edit flag offensive delete link more

Comments

Wow thank you! Saved my day!

soroosh129 gravatar imagesoroosh129 ( 2019-07-05 12:19:03 -0500 )edit
2

answered 2013-09-25 15:21:28 -0500

updated 2017-10-03 20:25:11 -0500

130s gravatar image

From a quick glance you are specifying wrong argument types for calculateOdometry(). Instead of const JointControllerState, you should use const JointControllerStateConstPtr& (note ConstPtr& at the end). So, all the message callbacks in ROS accepting boost::shared_ptr as the arguments.

Although passing arguments by reference is not mandatory, it is recommended and considered as a good style.

Please also note that template arguments should not be pointers, but the types of the messages themselves. So simply fixing the callback function arguments will be enough to make it compile.

edit flag offensive delete link more

Comments

I have made these changes and I am still getting a compile error. Can this be some kind of compiler environment issue?

rnunziata gravatar imagernunziata ( 2013-09-26 03:49:04 -0500 )edit

What I learn from my past experience (and I didn't manage to make it work), the timestamps of those to be synchronized have to match 'exactly'. Perhaps this is also the culprit in your case.

alfa_80 gravatar imagealfa_80 ( 2013-09-26 19:53:30 -0500 )edit

@alfa_80, the problem that you are talking about is a runtime problem, but the question is about compilation. Anyway, you are right, the time stamps should be exactly same. To deal with misaligned time stamps there is ApproximateTime message filter: http://wiki.ros.org/message_filters#ApproximateTime_Policy

Boris gravatar imageBoris ( 2013-09-26 20:43:13 -0500 )edit

@Boris: You're right.

alfa_80 gravatar imagealfa_80 ( 2013-09-27 12:10:52 -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

3 followers

Stats

Asked: 2013-09-25 11:21:39 -0500

Seen: 822 times

Last updated: Oct 03 '17