Merge pull request #162 from una-auxme/157-feature-create-behavior-le…

…ave-parking-space

157 feature create behavior leave parking space

.flake8

@@ -1,5 +1,4 @@
 [flake8]
-exclude= code/planning/behavior_agent/src/behavior_agent/behavior_tree.py,
-         code/planning/behavior_agent/src/behavior_agent/behaviours/__init__.py,
-         code/planning/src/behavior_agent/behavior_tree.py,
-         code/planning/src/behavior_agent/behaviours/__init__.py
+exclude= code/planning/src/behavior_agent/behavior_tree.py,
+         code/planning/src/behavior_agent/behaviours/__init__.py,
+         code/planning/src/behavior_agent/behaviours

build/docker-compose.yml

@@ -61,7 +61,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=carla-simulator --track=MAP"
     logging:
       driver: "local"

code/planning/launch/planning.launch

@@ -1,19 +1,19 @@
 <launch>
     <node pkg="planning" type="collision_check.py" name="CollisionCheck" output="screen">
            <param name="role_name" value="hero" />
-           <param name="control_loop_rate" value="1" />
+           <param name="control_loop_rate" value="0.3" />
     </node>
     <node pkg="planning" type="ACC.py" name="ACC" output="screen">
         <param name="role_name" value="hero" />
-        <param name="control_loop_rate" value="0.3" />
+        <param name="control_loop_rate" value="0.5" />
     </node>
     <!-- <node pkg="local_planner" type="local_planner/dev_collision_publisher.py" name="DevCollisionCheck" output="screen">
         <param name="role_name" value="hero" />
         <param name="control_loop_rate" value="0.3" />
     </node> -->
     <node pkg="planning" type="motion_planning.py" name="MotionPlanning" output="screen">
            <param name="role_name" value="hero" />
-           <param name="control_loop_rate" value="1" />
+           <param name="control_loop_rate" value="0.3" />
     </node>
 
     <!-- <node pkg="planning" type="dev_global_route.py" name="DevGlobalRoute" output="screen">

code/planning/src/behavior_agent/behavior_tree.py

@@ -25,6 +25,7 @@ def grow_a_tree(role_name):
                 children=[
                     Selector("Road Features",
                              children=[
+                                 behaviours.maneuvers.LeaveParkingSpace("Leave Parking Space"),
                                  Sequence("Intersection",
                                           children=[
                                               behaviours.road_features.IntersectionAhead
@@ -93,14 +94,13 @@ def main():
         rospy.logerr("Tree Setup failed")
         sys.exit(1)
     rospy.loginfo("tree setup worked")
-    r = rospy.Rate(5)
+    r = rospy.Rate(5.3)
     while not rospy.is_shutdown():
         behaviour_tree.tick()
         try:
             r.sleep()
         except rospy.ROSInterruptException:
             pass
 
-
 if __name__ == "__main__":
     main()

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

@@ -9,6 +9,9 @@ def convert_to_ms(speed):
 Behavior = namedtuple("Behavior", ("name", "speed"))
 
 # Changed target_speed_pub to curr_behavior_pub
+# Leave Parking space
+
+parking = Behavior("parking", convert_to_ms(30.0))
 
 # Intersection - Behaviors
 

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

@@ -1,7 +1,7 @@
 import py_trees
 import rospy
 from std_msgs.msg import String
-
+import numpy as np
 from . import behavior_speed as bs
 # from behavior_agent.msg import BehaviorSpeed
 
@@ -10,6 +10,121 @@
 """
 
 
+class LeaveParkingSpace(py_trees.behaviour.Behaviour):
+    """
+    This behavior is triggered in the beginning when the vehicle needs
+    to leave the parking space.
+    """
+    def __init__(self, name):
+        """
+        Minimal one-time initialisation. A good rule of thumb is to only
+        include the initialisation relevant for being able to insert this
+        behaviour in a tree for offline rendering to dot graphs.
+
+         :param name: name of the behaviour
+        """
+        super(LeaveParkingSpace, self).__init__(name)
+        rospy.loginfo("LeaveParkingSpace started")
+        self.called = False
+
+    def setup(self, timeout):
+        """
+        Delayed one-time initialisation that would otherwise interfere with
+        offline rendering of this behaviour in a tree to dot graph or
+        validation of the behaviour's configuration.
+
+        This initializes the blackboard to be able to access data written to it
+        by the ROS topics and gathers the time to check how much time has
+        passed.
+        :param timeout: an initial timeout to see if the tree generation is
+        successful
+        :return: True, as there is nothing to set up.
+        """
+        self.curr_behavior_pub = rospy.Publisher("/paf/hero/"
+                                                 "curr_behavior",
+                                                 String, queue_size=1)
+        self.blackboard = py_trees.blackboard.Blackboard()
+        self.initPosition = None
+        return True
+
+    def initialise(self):
+        """
+        When is this called?
+        The first time your behaviour is ticked and anytime the status is not
+        RUNNING thereafter.
+
+        What to do here?
+            Any initialisation you need before putting your behaviour to work.
+        Get initial position to check how far vehicle has moved during
+        execution
+        """
+        self.initPosition = self.blackboard.get("/paf/hero/current_pos")
+
+    def update(self):
+        """
+        When is this called?
+        Every time your behaviour is ticked.
+
+        pose: 
+            position: 
+                x: 294.43757083094295
+                y: -1614.961812061094
+                z: 211.1994649671884
+        What to do here?
+            - Triggering, checking, monitoring. Anything...but do not block!
+            - Set a feedback message
+            - return a py_trees.common.Status.[RUNNING, SUCCESS, FAILURE]
+
+        This behaviour runs until the agent has left the parking space.
+        This is checked by calculating the euclidian distance thath the agent
+        has moved since the start
+
+        :return: py_trees.common.Status.RUNNING, while the agent is leaving
+                                            the parking space
+                 py_trees.common.Status.SUCCESS, never to continue with
+                                            intersection
+                 py_trees.common.Status.FAILURE, if not in parking
+                 lane
+        """
+        position = self.blackboard.get("/paf/hero/current_pos")
+        speed = self.blackboard.get("/carla/hero/Speed")
+        if self.called is False:
+            # calculate distance between start and current position
+            if position is not None and \
+                    self.initPosition is not None and \
+                    speed is not None:
+                startPos = np.array([position.pose.position.x,
+                                     position.pose.position.y])
+                endPos = np.array([self.initPosition.pose.position.x,
+                                   self.initPosition.pose.position.y])
+                distance = np.linalg.norm(startPos - endPos)
+                if distance < 1 or speed.speed < 2:
+                    self.curr_behavior_pub.publish(bs.parking.name)
+                    self.initPosition = position
+                    return py_trees.common.Status.RUNNING
+                else:
+                    self.called = True
+                    return py_trees.common.Status.FAILURE
+            else:
+                self.initPosition = position
+                return py_trees.common.Status.RUNNING
+        else:
+            return py_trees.common.Status.FAILURE
+
+    def terminate(self, new_status):
+        """
+        When is this called?
+        Whenever your behaviour switches to a non-running state.
+            - SUCCESS || FAILURE : your behaviour's work cycle has finished
+            - INVALID : a higher priority branch has interrupted, or shutting
+            down
+
+        writes a status message to the console when the behaviour terminates
+        """
+        self.logger.debug("  %s [Foo::terminate().terminate()][%s->%s]" %
+                          (self.name, self.status, new_status))
+
+
 class SwitchLaneLeft(py_trees.behaviour.Behaviour):
     """
     This behavior triggers the replanning of the path in the local planner to

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

@@ -7,6 +7,7 @@
 from std_msgs.msg import Float32, Bool
 from carla_msgs.msg import CarlaSpeedometer
 from sensor_msgs.msg import Range
+from geometry_msgs.msg import PoseStamped
 
 from mock.msg import Traffic_light, Stop_sign
 from perception.msg import Waypoint, LaneChange
@@ -47,7 +48,9 @@ def create_node(role_name):
         {'name': f"/paf/{role_name}/speed_limit", 'msg': Float32,
          'clearing-policy': py_trees.common.ClearingPolicy.NEVER},
         {'name': f"/paf/{role_name}/lane_change_distance", 'msg': LaneChange,
-         'clearing-policy': py_trees.common.ClearingPolicy.ON_INITIALISE}
+         'clearing-policy': py_trees.common.ClearingPolicy.ON_INITIALISE},
+        {'name': f"/paf/{role_name}/current_pos", 'msg': PoseStamped,
+         'clearing-policy': py_trees.common.ClearingPolicy.NEVER}
     ]
 
     topics2blackboard = py_trees.composites.Parallel("Topics to Blackboard")

code/planning/src/local_planner/ACC.py

@@ -10,7 +10,6 @@
 from std_msgs.msg import Float32MultiArray, Float32
 from collision_check import CollisionCheck
 import time
-from perception.msg import MinDistance
 
 
 class ACC(CompatibleNode):
@@ -22,7 +21,7 @@ def __init__(self):
         super(ACC, self).__init__('ACC')
         self.role_name = self.get_param("role_name", "hero")
         self.control_loop_rate = self.get_param("control_loop_rate", 1)
-        self.current_speed = 50 / 3.6  # m/ss
+        self.current_speed = None  # m/ss
 
         # Get current speed
         self.velocity_sub: Subscriber = self.new_subscription(
@@ -34,8 +33,8 @@ def __init__(self):
         # Subscriber for lidar distance
         # TODO: Change to real lidar distance
         self.lidar_dist = self.new_subscription(
-            MinDistance,
-            f"/paf/{self.role_name}/Center/min_distance",
+            Float32,
+            f"/carla/{self.role_name}/LIDAR_range",
             self._set_distance,
             qos_profile=1)
         # Get initial set of speed limits
@@ -52,7 +51,7 @@ def __init__(self):
             self.__set_trajectory,
             qos_profile=1)
 
-        self.emergency_sub: Subscriber = self.new_subscription(
+        self.pose_sub: Subscriber = self.new_subscription(
             msg_type=PoseStamped,
             topic="/paf/" + self.role_name + "/current_pos",
             callback=self.__current_position_callback,
@@ -85,13 +84,13 @@ def __init__(self):
 
         self.logdebug("ACC initialized")
 
-    def _set_distance(self, data: MinDistance):
+    def _set_distance(self, data: Float32):
         """Get min distance to object in front from perception
 
         Args:
             data (MinDistance): Minimum Distance from LIDAR
         """
-        self.obstacle_distance = data.distance
+        self.obstacle_distance = data.data
 
     def __approx_speed_callback(self, data: Float32):
         """Safe approximated speed form obstacle in front together with
@@ -101,6 +100,7 @@ def __approx_speed_callback(self, data: Float32):
         Args:
             data (Float32): Speed from obstacle in front
         """
+        # self.logerr("ACC: Approx speed recieved: " + str(data.data))
         self.obstacle_speed = (time.time(), data.data)
 
     def __get_current_velocity(self, data: CarlaSpeedometer):

code/planning/src/local_planner/behavior_speed.py

@@ -10,6 +10,9 @@ def convert_to_ms(speed):
 
 # Changed target_speed_pub to curr_behavior_pub
 
+# Leave Parking space
+
+parking = Behavior("parking", convert_to_ms(30.0))
 # Intersection - Behaviors
 
 # Approach