diff --git a/cart_planning/scripts/local_planner.py b/cart_planning/scripts/local_planner.py index abf7bb4..af0680f 100755 --- a/cart_planning/scripts/local_planner.py +++ b/cart_planning/scripts/local_planner.py @@ -67,6 +67,9 @@ def __init__(self): # Allow nodes to make stop requests self.stop_sub = rospy.Subscriber('/stop', Stop, self.stop_callback, queue_size=10) + # Allows changes in speed + self.set_speed_sub = rospy.Subscriber('/speed', Float32, self.speed_callback) + # Allow the sharing of the current staus of the vehicle driving self.vehicle_state_pub = rospy.Publisher('/vehicle_state', VehicleState, queue_size=10, latch=True) @@ -116,6 +119,10 @@ def stop_callback(self, msg): self.stop_requests[str(msg.sender_id.data).lower()] = [msg.stop, msg.distance] rospy.loginfo(str(msg.sender_id.data).lower() + " requested stop: " + str(msg.stop) + " with distance: " + str(msg.distance)) + def speed_callback(self, msg): + self.global_speed = msg.data / 3.6 + rospy.loginfo("Speed changed to " + str (self.global_speed)) + def vel_callback(self, msg): if msg.data < 1.0: self.cur_speed = 1.8 # Magic number however this is roughly the observed speed in realtime