Merge pull request #221 from una-auxme/212-feature-unstuck-routine

212 feature unstuck routine

build/docker-compose.yml

@@ -21,7 +21,7 @@ services:
 
       # based on https://github.com/ll7/paf21-1/blob/master/scenarios/docker-carla-sim-compose.yml
   carla-simulator:
-    command: /bin/bash CarlaUE4.sh -quality-level=Epic -world-port=2000 -resx=800 -resy=600 -nosound -carla-settings="/home/carla/CarlaUE4/Config/CustomCarlaSettings.ini"
+    command: /bin/bash CarlaUE4.sh -quality-level=High -world-port=2000 -resx=800 -resy=600 -nosound -carla-settings="/home/carla/CarlaUE4/Config/CustomCarlaSettings.ini"
     image: ghcr.io/una-auxme/paf23:leaderboard-2.0
     init: true
     deploy:
@@ -62,7 +62,7 @@ services:
     tty: true
     shm_size: 2gb
     #command: bash -c "sleep 10 && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/opt/leaderboard/leaderboard/autoagents/npc_agent.py --host=carla-simulator --track=SENSORS"
-    # command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch"
+    #command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch"
     command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/workspace/code/agent/src/agent/agent.py --host=paf23-carla-simulator-1 --track=MAP"
 
     logging:

code/acting/launch/acting.launch

@@ -24,12 +24,11 @@
     </node>
 
     <!-- UNCOMMENT THIS TO USE THE DEBUG_NODE FOR ACTING-TESTING -->
-    <!--
-    <node pkg="acting" type="Acting_Debug_Node.py" name="Acting_Debug_Node" output="screen">
+    <!-- <node pkg="acting" type="Acting_Debug_Node.py" name="Acting_Debug_Node" output="screen">
         <param name="control_loop_rate" value="0.05" />
         <param name="role_name" value="$(arg role_name)" />
     </node>
-    -->
+     -->
 
     <node pkg="acting" type="stanley_controller.py" name="stanley_controller" output="screen">
         <param name="control_loop_rate" value="$(arg control_loop_rate)" />

code/acting/src/acting/vehicle_controller.py

@@ -6,7 +6,7 @@
 from carla_msgs.msg import CarlaEgoVehicleControl, CarlaSpeedometer
 from rospy import Publisher, Subscriber
 from ros_compatibility.qos import QoSProfile, DurabilityPolicy
-from std_msgs.msg import Bool, Float32
+from std_msgs.msg import Bool, Float32, String
 
 
 class VehicleController(CompatibleNode):
@@ -26,6 +26,8 @@ def __init__(self):
         self.control_loop_rate = self.get_param('control_loop_rate', 0.05)
         self.role_name = self.get_param('role_name', 'ego_vehicle')
 
+        self.__curr_behavior = None  # only unstuck behavior is relevant here
+
         # Publisher for Carla Vehicle Control Commands
         self.control_publisher: Publisher = self.new_publisher(
             CarlaEgoVehicleControl,
@@ -58,6 +60,13 @@ def __init__(self):
                                    durability=DurabilityPolicy.TRANSIENT_LOCAL)
         )
 
+        # Subscribers
+        self.curr_behavior_sub: Subscriber = self.new_subscription(
+            String,
+            f"/paf/{self.role_name}/curr_behavior",
+            self.__set_curr_behavior,
+            qos_profile=1)
+
         self.emergency_sub: Subscriber = self.new_subscription(
             Bool,
             f"/paf/{self.role_name}/emergency",
@@ -84,6 +93,12 @@ def __init__(self):
             self.__set_brake,
             qos_profile=1)
 
+        self.reverse_sub: Subscriber = self.new_subscription(
+            Bool,
+            f"/paf/{self.role_name}/reverse",
+            self.__set_reverse,
+            qos_profile=1)
+
         self.pure_pursuit_steer_sub: Subscriber = self.new_subscription(
             Float32,
             f"/paf/{self.role_name}/pure_pursuit_steer",
@@ -96,6 +111,7 @@ def __init__(self):
             self.__set_stanley_steer,
             qos_profile=1)
 
+        self.__reverse: bool = False
         self.__emergency: bool = False
         self.__velocity: float = 0.0
         self.__brake: float = 0.0
@@ -127,9 +143,15 @@ def loop(timer_event=None) -> None:
             if self.__velocity > 5:
                 steer = self._s_steer
             else:
-                steer = self._p_steer
+                # while doing the unstuck routine we don't want to steer
+                if self.__curr_behavior == "us_unstuck" or \
+                   self.__curr_behavior == "us_stop":
+                    steer = 0
+                else:
+                    steer = self._p_steer
+
             message = CarlaEgoVehicleControl()
-            message.reverse = False
+            message.reverse = self.__reverse
             message.hand_brake = False
             message.manual_gear_shift = False
             message.gear = 1
@@ -143,6 +165,14 @@ def loop(timer_event=None) -> None:
         self.new_timer(self.control_loop_rate, loop)
         self.spin()
 
+    def __set_curr_behavior(self, data: String) -> None:
+        """
+        Sets the current behavior
+        :param data:
+        :return:
+        """
+        self.__curr_behavior = data.data
+
     def __set_emergency(self, data) -> None:
         """
         In case of an emergency set the emergency flag to True ONCE
@@ -213,6 +243,9 @@ def __set_throttle(self, data):
     def __set_brake(self, data):
         self.__brake = data.data
 
+    def __set_reverse(self, data):
+        self.__reverse = data.data
+
     def __set_pure_pursuit_steer(self, data: Float32):
         r = (math.pi / 2)  # convert from RAD to [-1;1]
         self._p_steer = (data.data / r)

code/acting/src/acting/velocity_controller.py

@@ -4,8 +4,9 @@
 from ros_compatibility.node import CompatibleNode
 from rospy import Publisher, Subscriber
 from simple_pid import PID
-from std_msgs.msg import Float32
+from std_msgs.msg import Float32, Bool
 from nav_msgs.msg import Path
+import rospy
 
 # TODO put back to 36 when controller can handle it
 SPEED_LIMIT_DEFAULT: float = 0  # 36.0
@@ -48,6 +49,11 @@ def __init__(self):
             f"/paf/{self.role_name}/brake",
             qos_profile=1)
 
+        self.reverse_pub: Publisher = self.new_publisher(
+            Bool,
+            f"/paf/{self.role_name}/reverse",
+            qos_profile=1)
+
         # needed to prevent the car from driving before a path to follow is
         # available. Might be needed later to slow down in curves
         self.trajectory_sub: Subscriber = self.new_subscription(
@@ -102,14 +108,30 @@ def loop(timer_event=None):
                 return
             """
             if self.__target_velocity < 0:
-                self.logerr("VelocityController doesn't support backward "
-                            "driving yet.")
-                return
+                # self.logerr("VelocityController doesn't support backward "
+                #             "driving yet.")
+                if self.__target_velocity == -3:
+                    #  -3 is the signal for reverse driving
+
+                    reverse = True
+                    throttle = 1
+                    brake = 0
+                    rospy.loginfo("VelocityController: reverse driving")
+
+                else:
+                    #  other negative values only lead to braking
+                    reverse = False
+                    brake = 1
+                    throttle = 0
+
             # very low target_velocities -> stand
-            if self.__target_velocity < 1:
+            elif self.__target_velocity < 1:
+                reverse = False
                 brake = 1
                 throttle = 0
             else:
+                reverse = False
+
                 v = self.__target_velocity
                 pid_t.setpoint = v
                 throttle = pid_t(self.__current_velocity)
@@ -118,6 +140,8 @@ def loop(timer_event=None):
                     throttle = 0
                 else:
                     brake = 0
+
+            self.reverse_pub.publish(reverse)
             self.brake_pub.publish(brake)
             self.throttle_pub.publish(throttle)
 

code/planning/src/behavior_agent/behavior_tree.py

@@ -23,6 +23,7 @@ def grow_a_tree(role_name):
             Selector
             ("Priorities",
                 children=[
+                    behaviours.maneuvers.UnstuckRoutine("Unstuck Routine"),
                     Selector("Road Features",
                              children=[
                                  behaviours.maneuvers.LeaveParkingSpace("Leave Parking Space"),

code/planning/src/behavior_agent/behaviours/behavior_speed.py

@@ -79,3 +79,8 @@ def convert_to_ms(speed):
 # Cruise
 
 cruise = Behavior("Cruise", -1)
+
+# Unstuck Routine
+us_unstuck = Behavior("us_unstuck", -3)
+us_stop = Behavior("us_stop", 0)
+us_overtake = Behavior("us_overtake", 0)