How to subscribe "/scan" topic, modify the messages and publish it again to the "/scan" topic?

Question

I would like to improve turtlebot3 LDS-01 sensor by applying some algorithm to the sensor. So my strategy is to improve and modify the value of Laserscan.ranges (which is the distance between the sensor to the obstacles) by subscribing the “/scan” topic, applying the algorithm to the Laserscan.ranges and publish it again to the “/scan” topic. How can I do that?. I have some idea in my mind like:

1. Create new topic, publish the sensor value to the new topic, subscribe the new topic and applying the algorithm and lastly, publish it to the “/scan” topic. But this first idea, I cannot find the cpp or python file that publish to “/scan” topic. Where can I find that file? So that I can change the coding to publish the Laserscan.ranges to the new topic.

2. Modify the sensor_msgs/LaserScan by applying the algorithm to the float32 range. But this second idea, I dont know is it the message can be modified? If can, how to modify the sensor_msgs/LaserScan to apply the algorithm?

And lastly, if there are any suggestion, tutorial or online courses that I can take related to this question, I would be very happy to hear.

This image is summary for the idea 1

Answer 1

You shouldn't ever publish back out on a sensor topic after modifying the message. If you do this other subscribers will just see a mixture of the modified and non-modified messages. Instead publish it out on a separate topic like this:

scan_msg_pub = rospy.Publisher('/scan/edited', LaserScan, queue_size=10)
def scan_msg_cb(msg):
    #Edit message here
    msg.header.frame_id = "new_frame_id" #You can change msg fields like this
    scan_msg_pub.publish(msg)

def main():
    rospy.Subscriber('/scan', LaserScan, scan_msg_cb)

    rospy.spin()

if __name__ == '__main__':
    rospy.init_node('scan_edit')
    main()