Merge pull request #9 from JACart/parameterized-stop

Parameterized stop

cart_endpoints/scripts/cart_health_monitor.py

@@ -73,6 +73,7 @@ def send_stop(self, stop, sender_id):
         stop_msg = Stop()
         stop_msg.stop = stop
         stop_msg.sender_id.data = sender_id
+        stop_msg.distance = -1
         self.stop_pub.publish(stop_msg)
 
 if __name__ == "__main__":

cart_endpoints/scripts/motor_endpoint.py

@@ -6,6 +6,7 @@
 from navigation_msgs.msg import VelAngle
 from std_msgs.msg import Int8, Bool
 import time
+import math
 cart_port = '/dev/ttyUSB9' #hardcoded depending on computer
 
 # STATES:
@@ -33,6 +34,15 @@ def __init__(self):
         self.state = STOPPED
         self.stopping_time = 0
         self.step_size = 255.0/(self.node_rate*self.brake_time)
+        self.obstacle_distance = -1
+        self.brake_time_used = 0
+        self.comfortable_stop_dist = 10.0
+        # init local values to store & change msg params
+        self.vel_curr = 0
+        self.vel_curr_cart_units = 0
+        self.vel = 0
+        self.vel_cart_units = 0
+        self.angle = 0
         """ Set up the node. """
         rospy.init_node('motor_endpoint')
         rospy.loginfo("Starting motor node!")
@@ -61,11 +71,29 @@ def __init__(self):
     def motion_callback(self, planned_vel_angle):
         self.cmd_msg = planned_vel_angle
 
-        if self.cmd_msg.vel > 0 and (self.state == STOPPED or self.state == BRAKING) and (time.time()- self.stopping_time) > 10:
+        # set local velocity from msg values
+        self.vel_curr = self.cmd_msg.vel_curr
+        self.vel = self.cmd_msg.vel
+        self.angle = self.cmd_msg.angle
+
+        if self.vel < 0:
+            # indicates an obstacle
+            self.obstacle_distance = abs(self.vel)
+            self.vel = 0
+            # reset brake time used
+            self.brake_time_used = 0
+            self.full_stop_count = 0
+        else:
+            # reset obstacle distance and brake time
+            self.obstacle_distance = -1
+            self.brake_time_used = 0
+            self.full_stop_count = 0
+
+        if self.vel > 0 and (self.state == STOPPED or self.state == BRAKING) and (time.time()- self.stopping_time) > 10:
             self.state = MOVING
             self.brake = 0 # make sure we take the foot off the brake
 
-        elif self.state == MOVING and self.cmd_msg.vel <= 0:  # Brakes are hit
+        elif self.state == MOVING and self.vel <= 0:  # Brakes are hit
             self.state = BRAKING
             self.brake = 0 # ramp up braking from 0
             self.stopping_time = time.time()
@@ -77,31 +105,36 @@ def debug_callback(self, msg):
 
 
     def endpoint_calc(self):
-        #The first time we get a new target speed and angle we must convert it
-
         if self.new_vel:
+            #The first time we get a new target speed and angle we must convert it        
+            self.vel_cart_units = self.vel
+            self.vel_curr_cart_units = self.vel_curr
+
             self.new_vel = False
-            self.cmd_msg.vel *= 50       # Rough conversion from m/s to cart controller units
-            self.cmd_msg.vel_curr *= 50  # Rough conversion from m/s to cart controller units
-            if self.cmd_msg.vel > 254:
-                self.cmd_msg.vel = 254
-            if self.cmd_msg.vel < -254:
-                self.cmd_msg.vel = -254
-            if self.cmd_msg.vel_curr > 254:
-                self.cmd_msg.vel_curr = 254
-            if self.cmd_msg.vel < 0:
+
+            self.vel_cart_units *= 50       # Rough conversion from m/s to cart controller units
+            self.vel_curr_cart_units *= 50  # Rough conversion from m/s to cart controller units
+            if self.vel_cart_units > 254:
+                self.vel_cart_units = 254
+            if self.vel_cart_units < -254:
+                self.vel_cart_units = -254
+            if self.vel_curr_cart_units > 254:
+                self.vel_curr_cart_units = 254
+            if self.vel_cart_units < 0:
                 rospy.logwarn("NEGATIVE VELOCITY REQUESTED FOR THE MOTOR ENDPOINT!")
-        target_speed = int(self.cmd_msg.vel) #float64
-        current_speed = int(self.cmd_msg.vel_curr) #float64
+
+        target_speed = int(self.vel_cart_units) #float64
+        current_speed = int(self.vel_curr_cart_units) #float64
+
         #adjust the target_angle range from (-45 <-> 45) to (0 <-> 100)
         # rospy.loginfo("Angle before adjustment: " + str(self.cmd_msg.angle))
-        if(self.cmd_msg.angle < -45):
-            self.cmd_msg.angle = -45
-        if(self.cmd_msg.angle > 45):
-            self.cmd_msg.angle = 45
-        target_angle = 100 - int(( (self.cmd_msg.angle + 45) / 90 ) * 100)
+
+        if(self.angle < -45):
+            self.angle = -45
+        if(self.angle > 45):
+            self.angle = 45
+        target_angle = 100 - int(( (self.angle + 45) / 90 ) * 100)
 
-
         #if debug printing is requested print speed and angle info
         if self.debug:
             self.delay_print -= 1
@@ -114,8 +147,39 @@ def endpoint_calc(self):
             self.brake = 0
             target_speed = 0
         elif self.state == BRAKING:
-            self.brake = float(min(255, self.brake + self.step_size))
-            if self.brake >= 255:  # We have reached maximum braking!
+            if self.obstacle_distance > 0:
+                # there exists an obstacle in the cart's path we need to stop for
+
+                self.brake_time_used += (1.0/self.node_rate) # 1 sec / rate per sec (10)
+                obstacle_brake_time = self.obstacle_distance/self.vel_curr - (1.0/self.node_rate) # we decrease by one node rate initially to account for rounding
+
+                y = (0.05) * ((5100) ** (self.brake_time_used/obstacle_brake_time))
+
+                rospy.loginfo("Brake Time: " + str(obstacle_brake_time + (1.0/self.node_rate)))
+                rospy.loginfo("obs distance: " + str(self.obstacle_distance ))
+                rospy.loginfo("vel curr: " + str(self.vel_curr))
+                rospy.loginfo("brake time used: " + str(self.brake_time_used))
+                rospy.loginfo("y: " + str(y))
+
+                if (y >= 255):
+                    self.full_stop_count += 1
+
+                self.brake = float(min(255, math.ceil(y)))
+                rospy.loginfo("Brake: " + str(int(self.brake)))
+            else:
+                # comfortable stop, no obstacle/deadline given
+
+                self.brake_time_used += (1.0/self.node_rate) # 1 sec / rate per sec (10)
+                brake_time = self.comfortable_stop_dist/self.vel_curr - (1.0/self.node_rate) # we decrease by one node rate initially to account for rounding
+
+                y = (0.05) * ((5100) ** (self.brake_time_used/brake_time))
+
+                if (y >= 255):
+                    self.full_stop_count += 1
+
+                self.brake = float(min(255, math.ceil(y)))
+                rospy.loginfo("Brake: " + str(self.brake))
+            if self.brake >= 255 and self.full_stop_count > 10:  # We have reached maximum braking!
                 self.state = STOPPED
             target_speed = 0
 

cart_endpoints/scripts/network_node.py

@@ -79,6 +79,7 @@ def on_pull_over():
     stop_msg = Stop()
     stop_msg.stop = True
     stop_msg.sender_id = 1
+    stop_msg.distance = -1
     stop_pub.publish(stop_msg)
 
 @sio.on('resume-driving',namespace='/ros')
@@ -87,6 +88,7 @@ def on_resume():
     stop_msg = Stop()
     stop_msg.stop = False
     stop_msg.sender_id = 1
+    stop_msg.distance = -1
     stop_pub.publish(stop_msg)
 
 @sio.on('stop',namespace='/ros')
@@ -95,6 +97,7 @@ def on_stop(data):
     stop_msg = Stop()
     stop_msg.stop = True
     stop_msg.sender_id = 1
+    stop_msg.distance = -1
     stop_pub.publish(stop_msg)    
 
 
@@ -161,6 +164,7 @@ def pullover_callback(msg):
         stop_msg = Stop()
         stop_msg.stop = True
         stop_msg.sender_id = 2
+        stop_msg.distance = -1
         stop_pub.publish(stop_msg)
         send_unsafe()
     else:
@@ -173,6 +177,7 @@ def passenger_safe_callback(msg):
         stop_msg = Stop()
         stop_msg.stop = True
         stop_msg.sender_id = 2
+        stop_msg.distance = -1
         stop_pub.publish(stop_msg)
         send_unsafe()
 

cart_planning/scripts/collision_detector.py

@@ -66,7 +66,7 @@ def __init__(self):
         # Minimum allowable transit time to an obstacle allowed before emergency stopping
         self.min_obstacle_time = rospy.get_param('min_obstacle_time', .5)
 
-        self.safe_obstacle_dist = rospy.get_param('safe_obstacle_dist', 3)
+        self.safe_obstacle_dist = rospy.get_param('safe_obstacle_dist', 10)
         self.safe_obstacle_time = rospy.get_param('safe_obstacle_time', 2)
 
         self.obstacle_sub = rospy.Subscriber('/obstacles', ObstacleArray, self.obstacle_callback, queue_size=10)
@@ -202,6 +202,7 @@ def determine_collision(self):
                     self.cleared_confidence = 0
                     if not self.stopped:
                         self.stopped = True
+                        stop_msg.distance = distance
                         self.stop_pub.publish(stop_msg)
                         rospy.logwarn("Requesting a fast stop due to possible collision")
                     # Show a red obstacle, an obstacle worth stopping for
@@ -233,6 +234,7 @@ def determine_collision(self):
                 stop_msg = Stop()
                 stop_msg.stop = False
                 stop_msg.sender_id.data = "collision_detector"
+                stop_msg.distance = -1
                 self.stop_pub.publish(stop_msg)
 
         self.collision_pub.publish(collision_array)

cart_planning/scripts/local_planner.py

@@ -47,8 +47,8 @@ def __init__(self):
 
         self.current_state = VehicleState()
 
-        # To allow other nodes to make stop requests mapping like so: Sender_ID : [stop(boolean), stopfast(boolean)]
-        # To allow other nodes to make stop requests mapping like so: Sender_ID : [stop(boolean)]
+        # To allow other nodes to make stop requests mapping like so: Sender_ID : [stop(boolean), distance]
+        # If distance == -1, there is no obstacle
         self.stop_requests = {}
 
         # The points to use for a path, typically coming from global planner                                
@@ -113,8 +113,8 @@ def pose_callback(self, msg):
         self.global_pose = msg.pose
 
     def stop_callback(self, msg):
-        self.stop_requests[str(msg.sender_id.data).lower()] = [msg.stop]
-        rospy.loginfo(str(msg.sender_id.data).lower() + " requested stop: " + str(msg.stop))
+        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 vel_callback(self, msg):
         if msg.data < 1.0:

cart_simulator/teleop.py

@@ -30,6 +30,11 @@ def get_input(self, stdscr):
         d = 100
         x = 120
         y = 121
+        p = 112
+        o = 111
+        i = 105
+        u = 117
+        l = 108
 
         angle_max = 70
         angle_step = 0.5
@@ -38,9 +43,12 @@ def get_input(self, stdscr):
         stdscr.nodelay(True)
         rate = rospy.Rate(10) 
         stdscr.addstr(0,0,'Move with WASD, X for hard stop and Y for centering the wheel')
-        stdscr.addstr(1,0,'CTRL-C to exit')
-        stdscr.addstr(3,0,'TURNING       WHEEL ANGLE')
+        stdscr.addstr(1,0,'p= comfortable stop, o= obstacle 2m, i= obstacle 5m')
+        stdscr.addstr(2,0,'u= obstacle in 10m, l = start at 2.7s')
+        stdscr.addstr(3,0,'CTRL-C to exit')
+        stdscr.addstr(4,0,'TURNING       WHEEL ANGLE')
         stdscr.addstr(6,0,'FORWARD MOVEMENT')
+
         while not rospy.is_shutdown():
             keyval = stdscr.getch()
 
@@ -68,16 +76,36 @@ def get_input(self, stdscr):
             elif keyval == y:
                 self.msg.angle = 0.0
                 anglestr = "Center wheel  "
-
+            elif keyval == p:
+                self.msg.vel_curr = 2.7
+                self.msg.vel = 0
+                velstr = "comfortable stop          "
+            elif keyval == o:
+                self.msg.vel_curr = 2.7
+                self.msg.vel = -2
+                velstr = "obstacle in 2m - stop              "
+            elif keyval == i:
+                self.msg.vel_curr = 2.7
+                self.msg.vel = -5
+                velstr = "obstacle in 5m - stop              "
+            elif keyval == u:
+                self.msg.vel_curr = 2.7
+                self.msg.vel = -10
+                velstr = "obstacle in 10m - stop              "
+            elif keyval == l:
+                self.msg.vel_curr = 0.01
+                self.msg.vel = 2.7
+                velstr = "start moving at 2.7 m/s"
+
             self.prev_key = keyval
             self.motion_pub.publish(self.msg)
             outstr = anglestr +  velstr + str(self.msg.angle)
-            stdscr.addstr(4,0,anglestr + str(self.msg.angle))
+            stdscr.addstr(5,0,anglestr + str(self.msg.angle))
             stdscr.addstr(7,0,velstr)
             rate.sleep()
 
 if __name__ == "__main__":
     try:
         teleop()
     except rospy.ROSInterruptException:
-        pass
+        pass

navigation_msgs/msg/Stop.msg

@@ -1,3 +1,4 @@
 std_msgs/Header header
 bool stop
 std_msgs/String sender_id
+float64 distance