ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

ROS c++ Class Subscriber doesn't work while publishing in constructor

asked 2018-07-30 05:58:50 -0500

Salahuddin_Khan gravatar image

updated 2018-07-30 06:03:36 -0500

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 ...
(more)
edit retag flag offensive close merge delete

Comments

1

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 an AsyncSpinner.

gvdhoorn gravatar image gvdhoorn  ( 2018-07-30 06:07:06 -0500 )edit

Uncommenting those portions inside main also leads to the same problem i.e : subscriber callbacks don't work.

Salahuddin_Khan gravatar image Salahuddin_Khan  ( 2018-07-30 06:10:46 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2018-07-30 06:15:30 -0500

gvdhoorn gravatar image

Which is expected, as you don't have a ros::spinOnce() anywhere in the while-loop.

If you need a separate while-loop, you have to call ros::spinOnce(). If you don't need the while-loop, use ros::spin().

edit flag offensive delete link more

Comments

Yeah your'e right . Thanks !

Salahuddin_Khan gravatar image Salahuddin_Khan  ( 2018-07-30 06:25:16 -0500 )edit

It's working !!

Salahuddin_Khan gravatar image Salahuddin_Khan  ( 2018-07-30 06:29:44 -0500 )edit

Could you post the definitive code that works? I am interested. Thanks!

xaru8145 gravatar image xaru8145  ( 2020-05-06 20:17:45 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2018-07-30 05:58:50 -0500

Seen: 976 times

Last updated: Jul 30 '18