165 feature local trajectory planning with frenet trajectory planner

build/docker-compose.yml

@@ -62,7 +62,8 @@ services:
     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 && 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:
       driver: "local"
     environment:

build/docker/agent/Dockerfile

@@ -162,6 +162,19 @@ RUN source ~/.bashrc && pip install -r /workspace/requirements.txt
 # Add agent code
 COPY --chown=$USERNAME:$USERNAME ./code /workspace/code/
 
+# Install Frenet Optimal Trajectory Planner 
+RUN sudo mkdir -p /erdos/dependencies/frenet_optimal_trajectory_planner
+# Needed to resolve dependencies correctly inside freent_optimal_trajectory_planner 
+ENV PYLOT_HOME=/erdos
+
+ENV FREENET_HOME=/erdos/dependencies/frenet_optimal_trajectory_planner
+RUN sudo chown $USERNAME:$USERNAME $PYLOT_HOME
+RUN sudo chown $USERNAME:$USERNAME $FREENET_HOME
+
+RUN git clone https://github.com/erdos-project/frenet_optimal_trajectory_planner.git $FREENET_HOME
+RUN cd $FREENET_HOME && source ./build.sh
+
+ENV PYTHONPATH=$PYTHONPATH:/erdos/dependencies
 # Link code into catkin workspace
 RUN ln -s /workspace/code /catkin_ws/src
 
@@ -170,6 +183,8 @@ RUN source /opt/ros/noetic/setup.bash && catkin_make
 
 ADD ./build/docker/agent/entrypoint.sh /entrypoint.sh
 
+
+
 # set the default working directory to the code
 WORKDIR /workspace/code
 

code/agent/config/rviz_config.rviz

@@ -63,11 +63,11 @@ Visualization Manager:
       Unreliable: false
       Value: true
       Visibility:
-        Grid: false
+        Grid: true
         Imu: false
-        Path: false
+        Path: true
         PointCloud2: false
-        Value: false
+        Value: true
       Zoom Factor: 1
     - Class: rviz/Image
       Enabled: true
@@ -166,7 +166,7 @@ Visualization Manager:
       Offset:
         X: 0
         Y: 0
-        Z: 39
+        Z: 703
       Pose Color: 255; 85; 255
       Pose Style: None
       Queue Size: 10

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

@@ -0,0 +1,3 @@
+from . import intersection, lane_change, maneuvers, meta, road_features
+from . import topics2blackboard, traffic_objects
+from . import behavior_speed

code/planning/src/global_planner/global_planner.py

@@ -74,7 +74,7 @@ def __init__(self):
 
         self.path_pub = self.new_publisher(
             msg_type=Path,
-            topic='/paf/' + self.role_name + '/trajectory',
+            topic='/paf/' + self.role_name + '/trajectory_dummy',
             qos_profile=1)
 
         self.speed_limit_pub = self.new_publisher(
@@ -257,16 +257,16 @@ def run(self):
         :return:
         """
 
-        def loop(timer_event=None):
-            if len(self.path_backup.poses) < 1:
-                return
+        # def loop(timer_event=None):
+        #     if len(self.path_backup.poses) < 1:
+        #         return
 
-            # Continuously update paths time to update car position in rviz
-            # TODO: remove next lines when local planner exists
-            self.path_backup.header.stamp = rospy.Time.now()
-            self.path_pub.publish(self.path_backup)
+        #     # # Continuously update paths time to update car position in rviz
+        #     # # TODO: remove next lines when local planner exists
+        #     self.path_backup.header.stamp = rospy.Time.now()
+        #     self.path_pub.publish(self.path_backup)
 
-        self.new_timer(self.control_loop_rate, loop)
+        # self.new_timer(self.control_loop_rate, loop)
         self.spin()
 
 

code/planning/src/local_planner/ACC.py

@@ -47,7 +47,7 @@ def __init__(self):
         # Get trajectory to determine current speed limit
         self.trajectory_sub: Subscriber = self.new_subscription(
             Path,
-            f"/paf/{self.role_name}/trajectory",
+            f"/paf/{self.role_name}/trajectory_dummy",
             self.__set_trajectory,
             qos_profile=1)
 
@@ -66,6 +66,14 @@ def __init__(self):
             Float32,
             f"/paf/{self.role_name}/acc_velocity",
             qos_profile=1)
+        self.wp_publisher: Publisher = self.new_publisher(
+            Float32,
+            f"/paf/{self.role_name}/current_wp",
+            qos_profile=1)
+        self.speed_limit_publisher: Publisher = self.new_publisher(
+            Float32,
+            f"/paf/{self.role_name}/speed_limit",
+            qos_profile=1)
 
         # List of all speed limits, sorted by waypoint index
         self.__speed_limits_OD: [float] = []
@@ -88,7 +96,7 @@ def _set_distance(self, data: Float32):
         """Get min distance to object in front from perception
 
         Args:
-            data (MinDistance): Minimum Distance from LIDAR
+            data (Float32): Minimum Distance from LIDAR
         """
         self.obstacle_distance = data.data
 
@@ -148,8 +156,10 @@ def __current_position_callback(self, data: PoseStamped):
         if d_new < d_old:
             # update current waypoint and corresponding speed limit
             self.__current_wp_index += 1
+            self.wp_publisher.publish(self.__current_wp_index)
             self.speed_limit = \
                 self.__speed_limits_OD[self.__current_wp_index]
+            self.speed_limit_publisher.publish(self.speed_limit)
 
     def run(self):
         """