140 feature create an integrated acc with the collision node

build/docker-compose.yml

@@ -57,8 +57,9 @@ 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 && 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"
+    # 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"
     environment:

code/acting/launch/acting.launch

@@ -29,10 +29,10 @@
         <param name="role_name" value="$(arg role_name)" />
     </node>
 
-    <node pkg="acting" type="DummyTrajectoryPublisher.py" name="DummyTrajectoryPublisher" output="screen">
+    <!-- <node pkg="acting" type="DummyTrajectoryPublisher.py" name="DummyTrajectoryPublisher" output="screen">
         <param name="control_loop_rate" value="1" />
         <param name="role_name" value="$(arg role_name)" />
-    </node>
+    </node> -->
 
 
     <node pkg="acting" type="MainFramePublisher.py" name="MainFramePublisher" output="screen">

code/planning/global_planner/launch/global_planner.launch

@@ -1,7 +1,7 @@
 <launch>
 
     <!-- <node pkg="planning" type="dev_global_route.py" name="DevGlobalRoute" output="screen">
-           <param name="from_txt" value="True" />
+           <param name="from_txt" value="False" />
            <param name="sampling_resolution" value="75.0" />
            <param name="routes" value="/opt/leaderboard/data/routes_devtest.xml" />
            <param name="global_route_txt" value="/code/planning/global_planner/src/global_route.txt" />

code/planning/local_planner/CMakeLists.txt

@@ -11,7 +11,9 @@ find_package(catkin REQUIRED)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
-
+find_package(catkin REQUIRED COMPONENTS
+  perception
+)
 
 ## Uncomment this if the package has a setup.py. This macro ensures
 ## modules and global scripts declared therein get installed
@@ -44,9 +46,7 @@ find_package(catkin REQUIRED)
 
 ## Generate messages in the 'msg' folder
 # add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
+#   MinDistance.msg
 # )
 
 ## Generate services in the 'srv' folder
@@ -65,8 +65,7 @@ find_package(catkin REQUIRED)
 
 ## Generate added messages and services with any dependencies listed here
 # generate_messages(
-#   DEPENDENCIES
-#   std_msgs  # Or other packages containing msgs
+#   perception
 # )
 
 ################################################
@@ -103,6 +102,7 @@ catkin_package(
 #  LIBRARIES planning
 #  CATKIN_DEPENDS other_catkin_pkg
 #  DEPENDS system_lib
+  CATKIN_DEPENDS perception
 )
 
 ###########

code/planning/local_planner/launch/local_planner.launch

@@ -3,7 +3,14 @@
            <param name="role_name" value="hero" />
            <param name="control_loop_rate" value="1" />
     </node>
-
+    <node pkg="local_planner" type="ACC.py" name="ACC" output="screen">
+        <param name="role_name" value="hero" />
+        <param name="control_loop_rate" value="0.3" />
+    </node>
+    <!-- <node pkg="local_planner" type="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="local_planner" type="motion_planning.py" name="MotionPlanning" output="screen">
            <param name="role_name" value="hero" />
            <param name="control_loop_rate" value="1" />

code/planning/local_planner/package.xml

@@ -32,7 +32,6 @@
   <!-- Dependencies can be catkin packages or system dependencies -->
   <!-- Examples: -->
   <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
-  <!--   <depend>roscpp</depend> -->
   <!--   Note that this is equivalent to the following: -->
   <!--   <build_depend>roscpp</build_depend> -->
   <!--   <exec_depend>roscpp</exec_depend> -->
@@ -48,12 +47,14 @@
   <!--   <test_depend>gtest</test_depend> -->
   <!-- Use doc_depend for packages you need only for building documentation: -->
   <!--   <doc_depend>doxygen</doc_depend> -->
+  <build_depend>perception</build_depend>
+  <build_export_depend>perception</build_export_depend>
+  <exec_depend>perception</exec_depend>
   <buildtool_depend>catkin</buildtool_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>
     <!-- Other tools can request additional information be placed here -->
-
   </export>
 </package>

code/planning/local_planner/src/ACC.py

@@ -0,0 +1,207 @@
+#!/usr/bin/env python
+import ros_compatibility as roscomp
+# import tf.transformations
+from ros_compatibility.node import CompatibleNode
+from rospy import Subscriber, Publisher
+from geometry_msgs.msg import PoseStamped
+from carla_msgs.msg import CarlaSpeedometer   # , CarlaWorldInfo
+from nav_msgs.msg import Path
+# from std_msgs.msg import String
+from std_msgs.msg import Float32MultiArray, Float32
+from collision_check import CollisionCheck
+import time
+from perception.msg import MinDistance
+
+
+class ACC(CompatibleNode):
+    """
+    This node recieves a possible collision and
+    """
+
+    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
+
+        # Get current speed
+        self.velocity_sub: Subscriber = self.new_subscription(
+            CarlaSpeedometer,
+            f"/carla/{self.role_name}/Speed",
+            self.__get_current_velocity,
+            qos_profile=1)
+
+        # 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",
+            self._set_distance,
+            qos_profile=1)
+        # Get initial set of speed limits
+        self.speed_limit_OD_sub: Subscriber = self.new_subscription(
+            Float32MultiArray,
+            f"/paf/{self.role_name}/speed_limits_OpenDrive",
+            self.__set_speed_limits_opendrive,
+            qos_profile=1)
+
+        # Get trajectory to determine current speed limit
+        self.trajectory_sub: Subscriber = self.new_subscription(
+            Path,
+            f"/paf/{self.role_name}/trajectory",
+            self.__set_trajectory,
+            qos_profile=1)
+
+        self.emergency_sub: Subscriber = self.new_subscription(
+            msg_type=PoseStamped,
+            topic="/paf/" + self.role_name + "/current_pos",
+            callback=self.__current_position_callback,
+            qos_profile=1)
+        self.approx_speed_sub = self.new_subscription(
+            Float32,
+            f"/paf/{self.role_name}/cc_speed",
+            self.__approx_speed_callback,
+            qos_profile=1)
+        # Publish desired speed to acting
+        self.velocity_pub: Publisher = self.new_publisher(
+            Float32,
+            f"/paf/{self.role_name}/acc_velocity",
+            qos_profile=1)
+
+        # List of all speed limits, sorted by waypoint index
+        self.__speed_limits_OD: [float] = []
+        # Current Trajectory
+        self.__trajectory: Path = None
+        # Current index from waypoint
+        self.__current_wp_index: int = 0
+        # Current speed
+        self.__current_velocity: float = None
+        # Distance and speed from possible collsion object
+        self.obstacle_speed: tuple = None
+        # Obstalce distance
+        self.obstacle_distance = None
+        # Current speed limit
+        self.speed_limit: float = None  # m/s
+
+    def _set_distance(self, data: MinDistance):
+        """Get min distance to object in front from perception
+
+        Args:
+            data (MinDistance): Minimum Distance from LIDAR
+        """
+        self.obstacle_distance = data.distance
+
+    def __approx_speed_callback(self, data: Float32):
+        """Safe approximated speed form obstacle in front together with
+        timestamp when recieved.
+        Timestamp is needed to check wether we still have a vehicle in front
+
+        Args:
+            data (Float32): Speed from obstacle in front
+        """
+        self.obstacle_speed = (time.time(), data.data)
+
+    def __get_current_velocity(self, data: CarlaSpeedometer):
+        """_summary_
+
+        Args:
+            data (CarlaSpeedometer): _description_
+        """
+        self.__current_velocity = float(data.speed)
+
+    def __set_trajectory(self, data: Path):
+        """Recieve trajectory from global planner
+
+        Args:
+            data (Path): Trajectory path
+        """
+        self.__trajectory = data
+
+    def __set_speed_limits_opendrive(self, data: Float32MultiArray):
+        """Recieve speed limits from OpenDrive via global planner
+
+        Args:
+            data (Float32MultiArray): speed limits per waypoint
+        """
+        self.__speed_limits_OD = data.data
+
+    def __current_position_callback(self, data: PoseStamped):
+        """Get current position and check if next waypoint is reached
+            If yes -> update current waypoint and speed limit
+
+        Args:
+            data (PoseStamped): Current position from perception
+        """
+        if len(self.__speed_limits_OD) < 1 or self.__trajectory is None:
+            return
+
+        agent = data.pose.position
+        current_wp = self.__trajectory.poses[self.__current_wp_index].\
+            pose.position
+        next_wp = self.__trajectory.poses[self.__current_wp_index + 1].\
+            pose.position
+        # distances from agent to current and next waypoint
+        d_old = abs(agent.x - current_wp.x) + abs(agent.y - current_wp.y)
+        d_new = abs(agent.x - next_wp.x) + abs(agent.y - next_wp.y)
+        if d_new < d_old:
+            # update current waypoint and corresponding speed limit
+            self.__current_wp_index += 1
+            self.speed_limit = \
+                self.__speed_limits_OD[self.__current_wp_index]
+
+    def run(self):
+        """
+        Control loop
+        :return:
+        """
+        def loop(timer_event=None):
+            """
+            Checks if distance to a possible object is too small and
+            publishes the desired speed to motion planning
+            """
+            if self.obstacle_speed is not None:
+                # Check if too much time has passed since last speed update
+                if self.obstacle_speed[0] + 0.5 < time.time():
+                    self.obstacle_speed = None
+
+            if self.obstacle_distance is not None and \
+                    self.obstacle_speed is not None and \
+                    self.__current_velocity is not None:
+                # If we have obstalce speed and distance, we can
+                # calculate the safe speed
+                safety_distance = CollisionCheck.calculate_rule_of_thumb(
+                    False, self.__current_velocity)
+                if self.obstacle_distance < safety_distance:
+                    # If safety distance is reached, we want to reduce the
+                    # speed to meet the desired distance
+                    safe_speed = self.obstacle_speed[1] * \
+                        (self.obstacle_distance / safety_distance)
+                    self.velocity_pub.publish(safe_speed)
+                else:
+                    # If safety distance is reached, drive with same speed as
+                    # Object in front
+                    self.velocity_pub.publish(self.obstacle_speed[1])
+
+            elif self.speed_limit is not None:
+                # If we have no obstacle, we want to drive with the current
+                # speed limit
+                self.velocity_pub.publish(self.speed_limit)
+
+        self.new_timer(self.control_loop_rate, loop)
+        self.spin()
+
+
+if __name__ == "__main__":
+    """
+    main function starts the ACC node
+    :param args:
+    """
+    roscomp.init('ACC')
+
+    try:
+        node = ACC()
+        node.run()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        roscomp.shutdown()