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

Callback not being called on first spinOnce()

asked 2020-12-26 17:46:28 -0500

kidRobot gravatar image

I am using Melodic on Ubuntu 18.04 and using the turtlesim_node from turtle_sim. The turtlesim_node publishes its pose on the topic of /turtle1/pose. I have a subscriber node that I wrote below:

#include "ros/ros.h"
#include <geometry_msgs/Twist.h>
#include <turtlesim/Pose.h>
#include <math.h>

class TurtleSimPose
{
public:
  TurtleSimPose(ros::NodeHandle& nh)
  {
    pose_subscriber = nh.subscribe<turtlesim::Pose>("/turtle1/pose", 1, 
    &TurtleSimPose::poseCallback, this);
  }

  void poseCallback(const turtlesim::Pose::ConstPtr & pose_message)
  {
    turtlesim_pose.x = pose_message->x;
    turtlesim_pose.y = pose_message->y;
    turtlesim_pose.theta = pose_message->theta;
    ROS_INFO("Callback function called");
  }

  ros::Subscriber pose_subscriber;
  turtlesim::Pose turtlesim_pose;
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "cleaner_node");
    ros::NodeHandle nh;

    ROS_INFO("Cleaner node has been initialized");
    TurtleSimPose turtleSimPose(nh);

    ros::Rate r(1);
    ros::spinOnce();
    r.sleep();
    ROS_INFO("poseX=%f", (double)turtleSimPose.turtlesim_pose.x);
    ros::spinOnce();
    ROS_INFO("poseX=%f", (double)turtleSimPose.turtlesim_pose.x);
    r.sleep();
}

When I run the node, the output is as follows

[ INFO] [1609025627.687273851]: Cleaner node has been initialized
[ INFO] [1609025637.689161925]: poseX=0.000000
[ INFO] [1609025637.689295814]: Callback function called
[ INFO] [1609025637.689356096]: poseX=5.544445

Why is the callback function not being called during the first spinOnce function call, but for the second function call, the class member variable is changed and the callback function is called. I am confused about this.

Also, when I change the rate to more than 1Hz, I noticed during the first second of the subscriber node being initialized that the callback function is not called, but only after that first second, will be it called.

Thank you in advance

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-12-27 07:24:55 -0500

kidRobot gravatar image

So I've figured it out. When I was debugging it, it seemed to me like that there was no connection to the turtlesim Pose publisher at first. That is why the pose values were not being updated within the node. The simple fix was to just add

int count = pose_subscriber.getNumPublishers(); //get number of publishers connected to this subscriber
    ros::Rate r(2);
    bool firstLoop = true;
    while (count < 1) //a while loop that will keep looping to ensure that the subscriber is connected to at least on publisher
    {
      //ROS_INFO("connecting");
      count = pose_subscriber.getNumPublishers();
      ros::spinOnce();
      r.sleep();
      if (firstLoop)
      {
        ROS_INFO("waiting to connect to at least one turtlesim pose publisher");
        firstLoop = false;
      }
    }
    ROS_INFO("connected!");

A very simple fix and it was just that the ROS master takes a while to establish the connection between the published and the subscriber, thus it makes sense to wait until that connection is established before proceeding.

edit flag offensive delete link more

Comments

it was just that the ROS master takes a while to establish the connection between the published and the subscriber

just to clarify: the ROS master does not establish any connections. Nodes communicate peer-to-peer.

gvdhoorn gravatar image gvdhoorn  ( 2020-12-29 02:20:16 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2020-12-26 17:46:28 -0500

Seen: 306 times

Last updated: Dec 26 '20