ROS c++ Class Subscriber doesn't work while publishing in constructor
Hi, The following code subscribes to topics containing data of individual wheel angular velocities, I am simply combining them and publishing the Odometry of the base_link.
I've been trying to do everything in the class itself but when I am running the While loop which is publishing the Odometry in the constructor the Subscriber Callback functions don't work. In the following code callback functions don't work. While rostopic info shows that I am subscribing to those topics but the callbacks just don't run.
When I remove the while loop from the constructor and place it inside one of the callbacks , Everything runs smoothly
#include "ros/ros.h"
#include "geometry_msgs/TwistStamped.h"
#include "geometry_msgs/Pose2D.h"
#include "nav_msgs/Odometry.h"
#include <stdio.h>
class Wheelodom
{
public:
Wheelodom()
{
pub1 = n_.advertise<nav_msgs::Odometry>("odom", 5);
sub1 = n_.subscribe<geometry_msgs::TwistStamped>("left_wheel_odom", 10, &Wheelodom::odomCallback_l, this);
sub2 = n_.subscribe<geometry_msgs::TwistStamped>("right_wheel_odom", 10, &Wheelodom::odomCallback_r, this);
initialize();
reset_pose();
ros::Rate loop_rate(20);
while(ros::ok())
{
calculate_vel();
ROS_INFO("%f", av_l);
calculate_delta_pose();
calculate_integrated_pose();
publish_odom();
loop_rate.sleep();
}
}
void initialize()
{
simba_link = "base_link";
odom_pose.x = 0.0;
odom_pose.y = 0.0;
odom_pose.theta = 0.0;
delta_odom_pose.x = 0.0;
delta_odom_pose.y = 0.0;
delta_odom_pose.theta = 0.0;
cur_time = ros::Time::now();
prev_time = ros::Time::now();
Wheel_radius_r = 0.065;
Wheel_radius_l = 0.065;
rear_axle = 0.21;
h = 0.07;
av_l = 0.0;
av_r = 0.0;
}
void reset_pose()
{
odom_pose.x = 0.0;
odom_pose.y = 0.0;
odom_pose.theta = 0.0;
delta_odom_pose.x = 0.0;
delta_odom_pose.y = 0.0;
delta_odom_pose.theta = 0.0;
}
void odomCallback_l(const geometry_msgs::TwistStamped::ConstPtr& data)
{
av_l = data->twist.linear.x;
ROS_INFO("data received");
}
void odomCallback_r(const geometry_msgs::TwistStamped::ConstPtr& data)
{
av_r = data->twist.linear.x;
}
void calculate_vel()
{
odom.twist.twist.linear.x = ((av_l*Wheel_radius_l) + (av_r*Wheel_radius_r))/2;
odom.twist.twist.angular.z = ((av_r*Wheel_radius_r) - (av_l*Wheel_radius_l))/rear_axle;
}
void calculate_delta_pose()
{
cur_time = ros::Time::now();
elapsed_time = cur_time-prev_time;
prev_time = cur_time;
delta_odom_pose.theta = odom.twist.twist.angular.z * elapsed_time.toSec();
delta_odom_pose.x = odom.twist.twist.linear.x * elapsed_time.toSec() * cos(odom_pose.theta + delta_odom_pose.theta * 0.5);
delta_odom_pose.y = odom.twist.twist.linear.x * elapsed_time.toSec() * sin(odom_pose.theta + delta_odom_pose.theta * 0.5);
}
void calculate_integrated_pose()
{
odom_pose.x += delta_odom_pose.x;
odom_pose.y += delta_odom_pose.y;
odom_pose.theta += delta_odom_pose.theta;
odom.pose.pose.position.x = odom_pose.x;
odom.pose.pose.position.y = odom_pose.y;
odom.pose.pose.position.z = h;
odom.pose.pose.orientation.x = 0.0;
odom.pose.pose.orientation.y = 0.0;
odom.pose.pose.orientation.z = sin(0.5 * odom_pose.theta);
odom.pose.pose.orientation.w = cos(0.5 * odom_pose.theta);
}
void publish_odom()
{
odom.header.frame_id = "w_odom";
odom.child_frame_id = simba_link;
odom.header.stamp = cur_time;
pub1.publish(odom);
}
private:
ros::NodeHandle n_;
ros::Publisher pub1;
ros::Subscriber sub1, sub2;
std::string simba_link;
nav_msgs::Odometry odom;
float Wheel_radius_r,Wheel_radius_l;
float rear_axle;
float av_l,av_r,h; //angular velocities
ros::Duration elapsed_time;
ros::Time cur_time;
ros::Time prev_time;
geometry_msgs::Pose2D delta_odom_pose;
geometry_msgs::Pose2D odom_pose;
};
int main(int argc, char **argv)
{
ros::init(argc, argv, "simba_wheel_odom");
Wheelodom odomObject;
//odomObject
// odomObject.initialize();
// odomObject.reset_pose ...
Never, ever put infinite whiles in constructors or callbacks. Ever. It leads to exactly the problems that you describe / experienced.
Re-enable the while in your
main(..)
. Or use anAsyncSpinner
.Uncommenting those portions inside main also leads to the same problem i.e : subscriber callbacks don't work.