Ask Your Question
0

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

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

Salahuddin_Khan gravatar image

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

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

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 imagegvdhoorn ( 2018-07-30 06:07:06 -0600 )edit

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

Salahuddin_Khan gravatar imageSalahuddin_Khan ( 2018-07-30 06:10:46 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
1

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

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 imageSalahuddin_Khan ( 2018-07-30 06:25:16 -0600 )edit

It's working !!

Salahuddin_Khan gravatar imageSalahuddin_Khan ( 2018-07-30 06:29:44 -0600 )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: 2018-07-30 05:58:50 -0600

Seen: 205 times

Last updated: Jul 30 '18