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

ROS2 : Subscribing to LaserScan

asked 2022-04-28 10:34:19 -0500

Fredtzu gravatar image

updated 2022-04-28 10:58:24 -0500

Hello everyone , noobie question :

I'm trying to write a subscriber to LaserScan from the Dolly robot (https://github.com/chapulina/dolly) . I've written the launch file , for the Gazebo simulation,then wrote the subscriber but i can't get feedback from the laserscan.

So basically i've got this so far :

import rclpy
from rclpy.node import Node


import sensor_msgs.msg


class ReadingLaser(Node):

    def __init__(self):
        super().__init__('reading_laser')

        self.subscription= self.create_subscription(
            sensor_msgs.msg.LaserScan,
            '/dolly/laser_scan'
            self.listener_callback,
            qos_profile= 10

        )


    def listener_callback(self, msg):
        self.get_logger().info('I heard "%f"' %msg.data[100] )


def main(args=None):
    rclpy.init()
    reading_laser = ReadingLaser()                  
    reading_laser.get_logger().info("Hello friend!")
    rclpy.spin(reading_laser)

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

if __name__ == '__main__':
    main()

After I build it, I run it then i get the log message ("hello friend") , but no LaserScan content. What am I doing wrong here?

Thanks in advance!

(EDIT: i finally got it to spit something , but it seems I don't defined well the QoS Profile, since now i've got : [WARN] [1651161136.977626994] [reading_laser]: New publisher discovered on topic '/dolly/laser_scan', offering incompatible QoS. No messages will be received from it. Last incompatible policy: RELIABILITY -> how do I define this QoS Profile? )

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
2

answered 2022-04-28 13:05:08 -0500

Fredtzu gravatar image

After some more reading i think i solved the issue . First i had to define the QoS (after importing necessary modules) i simply defined the QoSprofile with depth 10, and after checking the verbose of the topic that i wanted to subscribe (ros2 topic info /dolly/laser_scan --verbose) i simply matched the corresponding parameters. I also made some mistakes on the msg that i wanted to subscribe. Only for testing i checked a couple of ranges values :

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
from rclpy.qos import QoSProfile

from sensor_msgs.msg import LaserScan


class ReadingLaser(Node):

    def __init__(self):
        super().__init__('reading_laser')

        qos_profile = QoSProfile(depth=10)
        qos_profile.reliability = QoSReliabilityPolicy.BEST_EFFORT
        qos_profile.durability = QoSDurabilityPolicy.VOLATILE



        self.subscription= self.create_subscription(
            LaserScan,
            '/dolly/laser_scan',
            self.listener_callback,
            qos_profile,
        )


    def listener_callback(self,msg):
        self.get_logger().info('I heard : Range[0] "%f" Ranges[50]: "%f"' %(msg.ranges[0] ,msg.ranges[50]))


def main(args=None):
    rclpy.init()
    reading_laser = ReadingLaser()                  
    reading_laser.get_logger().info("Hello friend!")
    rclpy.spin(reading_laser)

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

if __name__ == '__main__':
    main()

I don't know if there is a simpler , more elegant way to do this, but this is what i got so far :)

edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2022-04-28 10:34:19 -0500

Seen: 1,541 times

Last updated: Apr 28 '22