Trying to use Timesynchronizer...getting compile error
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 ...
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
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.
Yes...I tried adding it to several places....a working example would be great
@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.
Replacing the line for
sync.registerCallback()
with thesync.registerCallback(boost::bind(&PublishOdometry::handelerOdometry, this, _1, _2));
should be sufficient.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.