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

ros2: turtlebot3 gazebo /scan topic subscriber is not calling the callback function

asked 2019-10-05 13:26:35 -0500

shivaang12 gravatar image

updated 2019-10-06 12:37:19 -0500

I am trying to simply subscribe to /scan topic after launching turtlebot3_world.py launch file in turtlebot3_gazebo pkg. For that I am creating a ros2 node which subscribes to /scan topic and logs it into the console.

Thanks in advance!

UPDATE:

/scan works from terminal using ros2 topic echo /scan but when run my pkg it simple not calling the callback function. This is how my subscriber class looks like.

class ScanPrinter:public rclcpp::Node{
public:

ScanPrinter():Node("scan_printer"){
    sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
    "scan",
    rclcpp::QoS(rclcpp::KeepLast(10)),
    std::bind(&ScanPrinter::LaserCallback, this, std::placeholders::_1)
    );
}

private:
void LaserCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg){
    RCLCPP_INFO(this->get_logger(), std::to_string(msg->ranges[90]).c_str());
}
rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr sub_;
};
  • Operating System:
    • Ubuntu 18.04
  • Version or commit hash:
    • Compiled from github
  • DDS implementation:
    • Default

Steps I do in order

  • export TURTLEBOT3_MODEL=burger
  • export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:<PATH/TO/MODELS>
  • I launch the turtlebot3_world.launch.py file which opens up the gazebo with the spawned turtle bot.

  • I try to run my subscriber node and it prints nothing on the console log, as it suppose to according to the LaseCallback function. By which I deduce that its not calling the call back function.

  • Then I tried ros2 topic echo /scan and it prints the message correctly.

  • Then I tried to see if my subscribers has really subscribed this topic by ros2 topic info /scan and then it prints publisher: 1, subscriber 1 by which I deduce it does.

  • Then I tried to subscribe custom string publisher by making necessary changes in the code, and it behaves perfectly.

I don't know what is wrong here.

edit retag flag offensive close merge delete

Comments

It's unlikely members here will analyse your complete package for you -- especially if you just post a link here. We don't run a debugging service here.

Please provide some excerpts of your code, especially the parts having to do with creating subscribers, an overview of the topics that do exist, a description of how you start everything (also: in what order), versions of things like Fast-RTPS (or any other DDS implementation you are using), etc, etc.

gvdhoorn gravatar image gvdhoorn  ( 2019-10-06 05:38:54 -0500 )edit

Sorry, I couldn't figured out earlier how to post code, which is why I provided the link. But I made the necessary changes as per you suggestion.

shivaang12 gravatar image shivaang12  ( 2019-10-06 12:35:08 -0500 )edit

3 Answers

Sort by ยป oldest newest most voted
3

answered 2019-10-08 09:48:54 -0500

shivaang12 gravatar image

I made it work at the end. The /scan topic publishes the data with Sensor QoS profile, qos_profile_sensor_data. So it is mandatory to use qos_profile_sensor_data QoS profile in the subscriber.

edit flag offensive delete link more

Comments

This worked for me! Thank you!

For those that need help with this in python it should be: rclpy.qos.qos_profile_sensor_data

For C++, it should be: rmw_qos_profile_sensor_data

zmk5 gravatar image zmk5  ( 2019-10-15 17:08:49 -0500 )edit
2

answered 2019-12-03 04:33:38 -0500

greggScutum gravatar image

You can also take a look at the Dolly demo for Gazebo (https://github.com/chapulina/dolly). They are using rclcpp::QoS(rclcpp::SystemDefaultsQoS()).

edit flag offensive delete link more
0

answered 2022-01-23 14:24:42 -0500

mac137 gravatar image

Python code for those who are still in doubt about how to implement the qos_profile_sensor_data for the subscriber of a 2d lidar (found here):

from sensor_msgs.msg import LaserScan

(...)

qos_policy = rclpy.qos.QoSProfile(reliability=rclpy.qos.ReliabilityPolicy.BEST_EFFORT, history=rclpy.qos.HistoryPolicy.KEEP_LAST, depth=1)

self.subscription = self.create_subscription(LaserScan, 'scan', self.listener_callback, qos_profile=qos_policy)

edit flag offensive delete link more

Question Tools

2 followers

Stats

Asked: 2019-10-05 13:26:35 -0500

Seen: 1,597 times

Last updated: Jan 23 '22