From 30cbbaaf96d69e80ff59b33f74191c01a8161511 Mon Sep 17 00:00:00 2001
From: samuelkuehnel <samuel.kuehnel@gmail.com>
Date: Thu, 18 Jan 2024 16:51:11 +0100
Subject: [PATCH] fix: testing with emergency brake

---
 code/planning/launch/planning.launch               |  6 +++---
 code/planning/src/local_planner/ACC.py             |  2 +-
 code/planning/src/local_planner/motion_planning.py |  4 ++--
 doc/03_research/03_planning/00_paf23/test_traj.py  | 13 ++++++++-----
 4 files changed, 14 insertions(+), 11 deletions(-)

diff --git a/code/planning/launch/planning.launch b/code/planning/launch/planning.launch
index ba842d1e..1942de46 100644
--- a/code/planning/launch/planning.launch
+++ b/code/planning/launch/planning.launch
@@ -1,11 +1,11 @@
 <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" />
@@ -13,7 +13,7 @@
     </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
index a2b8182c..4b7c2b9b 100755
--- 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
index c0217b66..952687ab 100755
--- 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
index d8e316ec..b4e1165a 100644
--- a/doc/03_research/03_planning/00_paf23/test_traj.py
+++ b/doc/03_research/03_planning/00_paf23/test_traj.py
@@ -2,7 +2,8 @@
 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,
@@ -10,7 +11,7 @@
     '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()