fix: testing with emergency brake

una-auxme · Jan 18, 2024 · 30cbbaa · 30cbbaa
1 parent 1fa0acb
commit 30cbbaa
Show file tree

Hide file tree

Showing 4 changed files with 14 additions and 11 deletions.
diff --git a/code/planning/launch/planning.launch b/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="1" />
+        <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="global_planner/dev_global_route.py" name="DevGlobalRoute" output="screen">

diff --git a/code/planning/src/local_planner/ACC.py b/code/planning/src/local_planner/ACC.py
@@ -100,7 +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.logerr("ACC: Approx speed recieved: " + str(data.data))
         self.obstacle_speed = (time.time(), data.data)
 
     def __get_current_velocity(self, data: CarlaSpeedometer):

diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py
@@ -91,9 +91,9 @@ def __check_emergency(self, data: Bool):
         Args:
             data (Bool): True if emergency stop detected by collision check
         """
-        self.logerr("Emergency stop detected")
+        # self.logerr("Emergency stop detected")
         if not self.__curr_behavior == bs.parking.name:
-            self.logerr("Emergency stop detected and executed")
+            # self.logerr("Emergency stop detected and executed")
             self.emergency_pub.publish(data)
 
     def update_target_speed(self, acc_speed, behavior):

diff --git a/doc/03_research/03_planning/00_paf23/test_traj.py b/doc/03_research/03_planning/00_paf23/test_traj.py
@@ -2,15 +2,16 @@
 import numpy as np
 import matplotlib.pyplot as plt
 
-wp = wp = np.r_[[np.full((50), 983.5889666959667)], [np.linspace(5370.016106881272, 5399.016106881272, 50)]].T
+wp = wp = np.r_[[np.full((50), 983.5889666959667)],
+                [np.linspace(5370.016106881272, 5399.016106881272, 50)]].T
 initial_conditions = {
     'ps': 0,
     'target_speed': 6,
     'pos': np.array([983.5807552562393, 5370.014637890163]),
     'vel': np.array([5, 1]),
     'wp': wp,
     'obs': np.array([[983.568124548765, 5386.0219828457075,
-             983.628124548765, 5386.0219828457075]])
+                      983.628124548765, 5386.0219828457075]])
 }
 
 hyperparameters = {
@@ -47,11 +48,13 @@
     print("result_y: ", result_y)
     fig, ax = plt.subplots(1, 2)
 
-    ax[0].scatter(wp[:,0], wp[:,1], label="original")
-    ax[0].scatter([983.568124548765, 983.628124548765], [5386.0219828457075, 5386.0219828457075], label="object")
+    ax[0].scatter(wp[:, 0], wp[:, 1], label="original")
+    ax[0].scatter([983.568124548765, 983.628124548765],
+                  [5386.0219828457075, 5386.0219828457075], label="object")
     ax[0].set_xticks([983.518124548765, 983.598124548765])
     ax[1].scatter(result_x, result_y, label="frenet")
-    ax[1].scatter([983.568124548765, 983.628124548765], [5386.0219828457075, 5386.0219828457075], label="object")
+    ax[1].scatter([983.568124548765, 983.628124548765],
+                  [5386.0219828457075, 5386.0219828457075], label="object")
     ax[1].set_xticks([983.518124548765, 983.598124548765])
     plt.legend()
     plt.show()