Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Unable to read LaserScan message from /scan #85

Open
sgawalsh opened this issue Aug 21, 2019 · 9 comments
Open

Unable to read LaserScan message from /scan #85

sgawalsh opened this issue Aug 21, 2019 · 9 comments

Comments

@sgawalsh
Copy link

I used this tutorial to set up my workspace and am trying to implement a q-learning environment using ROS2. I have written a simple subscriber to the /scan topic which has publisher /turtlebot3_laserscan, but do not get any results when running the script alongside the ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py command.

scanSub.py

import rclpy
from rclpy.qos import QoSProfile
from sensor_msgs.msg  import LaserScan

def chatter_callback(msg):
    #print('Heard {0}'.format(str(msg)))
    print('heard it')
    

def main(args=None):
    rclpy.init()

    node = rclpy.create_node('scanSubcriber')

    sub = node.create_subscription(LaserScan, 'scan', chatter_callback, QoSProfile(depth=10))
    assert sub  # prevent unused warning

    while rclpy.ok():
        rclpy.spin_once(node)

if __name__ == '__main__':
    main()

However, when I run my own simple publisher to publish custom scan messages, the messages are recieved.

testPub.py

from time import sleep
from sensor_msgs.msg  import LaserScan
import rclpy

from rclpy.qos import QoSProfile

def main(args=None):
    rclpy.init()

    node = rclpy.create_node('scanPublisher')

    chatter_pub = node.create_publisher(LaserScan, 'scan', QoSProfile(depth=10))

    msg = LaserScan()

    i = 1
    while True:
        msg.angle_min = float(i)
        i += 0.5
        print('Publishing: "{0}"'.format(msg.angle_min))
        chatter_pub.publish(msg)
        sleep(1)

if __name__ == '__main__':
    main()

The ros2 node info /turtlebot3_laserscan command gives the message type of /scan as sensor_msgs/msg/LaserScan which I believe is the type I am using on the custom subscriber and publisher.

My environment is

  • Ubuntu 18.04.3,
  • ros2 Dashing
@mkhansenbot
Copy link

I'm not seeing any data on the /scan topic either.

@sgawalsh - do you see anything when you run 'ros2 topic echo /scan'

@sgawalsh
Copy link
Author

sgawalsh commented Sep 7, 2019

I remember testing that at the time, and as I recall the LaserScan message was displayed as expected when running ros2 topic echo /scan

@routiful
Copy link

routiful commented Sep 8, 2019

@sgawalsh
/scan topic was published by gazebo ros ray sensor plugin.
That plugin set SensorQoS to publish scan topic.
Because your code was written to subscribe it with reliable QoS, you can't get any msgs from gazebo plugin.

sub = node.create_subscription(LaserScan, 'scan', chatter_callback, QoSProfile(depth=10))

I've attached reference about QoS Settings.
https://github.com/ros2/ros2_documentation/blob/master/source/Concepts/About-Quality-of-Service-Settings.rst

@mkhansenbot
Copy link

My issue is this one:
ros-simulation/gazebo_ros_pkgs#991

This means a change is needed in the turtlebot3 model files to match the new argument style.

@miguelvelezmj25
Copy link

miguelvelezmj25 commented Apr 27, 2020

What should be the quality of service to listen to /scan messages? That is, what should I put for qos in node.create_subscription(LaserScan, 'scan', chatter_callback, qos)

@sjev
Copy link

sjev commented Nov 6, 2020

I'm haiving the same issue - ros2 topic echo /scan works just fine, but subscribing to it from Python or rqt does not. Spending a couple of hours browsing through issue discussions and incomplete documentation on QoS did not help...
@robotis : please provide an example on how to subscribe to /scan data.

@goekce
Copy link

goekce commented Nov 12, 2020

@sjev

docs for create_subscription() can be found in: ROS2 docs: node

and for setting QoS: ROS2 docs: QoSProfile

import rclpy
from rclpy.node import Node
from rclpy.qos import ReliabilityPolicy, QoSProfile

from std_msgs.msg import String
from sensor_msgs.msg import LaserScan

class MinimalSubscriber(Node):

    def __init__(self):
        super().__init__('minimal_subscriber')
        self.subscription = self.create_subscription(
            LaserScan,
            'scan',
            self.listener_callback,
            QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT))
        self.subscription  # prevent unused variable warning

    def listener_callback(self, msg):
        dist_back = format(msg.ranges[180], '.2f')
        dist_left = format(msg.ranges[90], '.2f')
        dist_right = format(msg.ranges[270], '.2f')
        dist_head = format(msg.ranges[0], '.2f')
        self.get_logger().info(f'{dist_back} {dist_left} {dist_right} {dist_head}')


def main(args=None):
    rclpy.init(args=args)

ef main(args=None):
    rclpy.init(args=args)

    minimal_subscriber = MinimalSubscriber()

    rclpy.spin(minimal_subscriber)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    minimal_subscriber.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

I did not test it thoroughly, but the code snippet at least shows how to set BEST_EFFORT.

You should get:

[INFO] [1605203781.073661834] [minimal_subscriber]: 0.56 1.50 0.93 0.00
[INFO] [1605203781.274214020] [minimal_subscriber]: 0.56 1.50 0.92 0.00
...

@drfknoble
Copy link

I had a very similar issue. @goekce's solution of setting the QoSProfile's reliability to best effort, i.e. reliability=ReliabilityPolicy.BEST_EFFORT, fixed things for me.

@goekce
Copy link

goekce commented Dec 21, 2020

I found a more typing-friendly solution than the long option in my last post:

rclpy.qos has already predefined QoS profiles:

https://github.com/ros2/rclpy/blob/02fc987029c01c87676933cffa50bafb1f5a6d59/rclpy/rclpy/qos.py#L417-L430

Here is a minimal example using qos_profile_sensor_data:

import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import LaserScan


class Tb3(Node):
    def __init__(self):
        super().__init__('tb3')

        self.scan_sub = self.create_subscription(
                LaserScan,
                'scan',
                self.scan_callback,  # function to run upon message arrival
                qos_profile_sensor_data)  # allows packet loss

    def scan_callback(self, msg):
        print(msg.ranges[0])


def main(args=None):
    rclpy.init(args=args)

    tb3 = Tb3()

    rclpy.spin(tb3)  # execute tb3 node
    # blocks until the executor cannot work

    tb3.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

The output should be similar to:

0.5657613277435303
0.5455226898193359
...

Other examples are in turtlebot3_example package:

https://github.com/ROBOTIS-GIT/turtlebot3/tree/ros2/turtlebot3_example/turtlebot3_example

abhishek47kashyap added a commit to abhishek47kashyap/navigation2_dynamic that referenced this issue Feb 15, 2021
abhishek47kashyap added a commit to abhishek47kashyap/navigation2_dynamic that referenced this issue May 22, 2021
abhishek47kashyap added a commit to abhishek47kashyap/navigation2_dynamic that referenced this issue May 18, 2023
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Development

No branches or pull requests

7 participants