Qfyilyi

I want to subscribe to a geomtery message that is being published. But inside a node that is also a publisher under a condition.

My node publishes a cmd_vel message when an object is near the robot, to tell the robot to stop. and if not, then it moves to a constant speed.

But now I want the robot to subscribe to a cmd_vel that is being published when the condition is not met, for example, to be able to move it by using keyboard or others.

I have tried to write it but nothing is working, so I did not think it is useful to show to you what did not work. Because it does not work. Instead if you could give me help on how to do this, especially with the code part, I will be thankful.

I also want to ask if there is a way so that it publishes on the topic only when the condition is met, and just not publishing when it does not.

#!/usr/bin/env python



import rospy

from sensor_msgs.msg import LaserScan

from geometry_msgs.msg import Twist



class Obstacle():

def __init__(self):

    self.LIDAR_ERR = 0.05

    self._cmd_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)

    self.obstacle()



def get_scan(self):

    msg = rospy.wait_for_message("scan", LaserScan)

    self.scan_filter = 

    for i in range(360):

        if i <= 15 or i > 335:

            if msg.ranges[i] >= self.LIDAR_ERR:

                self.scan_filter.append(msg.ranges[i])



def obstacle(self):

    self.twist = Twist()

    while not rospy.is_shutdown():

        self.get_scan()



        if min(self.scan_filter) < 0.2:

            self.twist.linear.x = 0.0

            self.twist.angular.z = 0.0

            self._cmd_pub.publish(self.twist)

            rospy.loginfo('Stop!')



        else:

            self.twist.linear.x = 0.5

            self.twist.angular.z = 0.0

            rospy.loginfo('distance of the obstacle : %f', min(self.scan_filter))



        self._cmd_pub.publish(self.twist)



def main():

rospy.init_node('turtlebot3_obstacle')

try:

    obstacle = Obstacle()

except rospy.ROSInterruptException:

    pass



if __name__ == '__main__':

main()

Subscribing and unsubscribing just like publishing and unpublishing is not a optimal way since it takes time to construct and deconstruct them what you can do is set up both publisher and subscriber and control when they will be updated and published:

1. control when you publish by adding an if before self._cmd_pub.publish(self.twist).


2. set up your callback and update your variables with data from callback if your condition mets.

You have two options here and they differentiate in what you are trying to archive:

1. 
   

   You have different tasks for the robot to do. Take a look into the actionlib.

   
   
   

   E.g. the robot should drive to house x and park in the garage. Then you can split this into multiple tasks:

   
   
   
   
   
   • Drive to the garage.
   
   
   • Park in the garage.
   
   
   
   

   
   This split makes sense because the nature of the maneuvers are very different and you'll probably wont to write another node (or multiple nodes) for only this maneuver.

   
   


2. 
   

   From your question I'm a little bit confused because it seems like you are trying to implement a "safety" feature, which should be able to take control in all possible cases.

   
   
   

   Unfortunately, ROS has, as far as I know, no good way to differentiate between the publishers of a ROS message. A ROS message can have as many publishers and subscribers as you want and you are not able to select "listen here, but ignore these messages".

   
   
   

   The best way I found to do something like this, is to create a new subscriber in every cmd_vel publisher and if your "safety" node sends "stop" (or bool false/true in this case) the publisher stops publishing.

   
   

One last recommendation: Try to bundle up your navigation tools to one branch for every different task with only one end node, which publishes cmd_vel. And every safety and control mechanism you need is packed underneath and not "in parallel" so to speak.

This makes everything much easier and you can use the actionlib to its full potential. You can even share nodes between the branches, without interfering, because the end publisher will be silenced if not needed.