feat: Acting_DebuggerNode and Acting commitable

build/docker-compose.yml

@@ -54,8 +54,8 @@ 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/ && 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/ && 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

@@ -1,8 +1,8 @@
-<!-- TODO: Insert components of acting component-->
+<!---->
 <launch>
     <arg name="role_name" default="hero" />
     <arg name="control_loop_rate" default="0.1" />
-<!--
+
     <node pkg="acting" type="pure_pursuit_controller.py" name="pure_pursuit_controller" output="screen">
         <param name="control_loop_rate" value="$(arg control_loop_rate)" />
         <param name="role_name" value="$(arg role_name)" />
@@ -12,7 +12,7 @@
         <param name="control_loop_rate" value="$(arg control_loop_rate)" />
         <param name="role_name" value="$(arg role_name)" />
     </node>
--->
+
     <node pkg="acting" type="velocity_controller.py" name="velocity_controller" output="screen">
         <param name="control_loop_rate" value="$(arg control_loop_rate)" />
         <param name="role_name" value="$(arg role_name)" />
@@ -44,15 +44,15 @@
         <param name="enabled" value="False" /> <!-- set to True to publish dummy velocities for testing-->
     </node>
 
-<!--
     <node pkg="acting" type="acc.py" name="Acc" output="screen">
         <param name="control_loop_rate" value="$(arg control_loop_rate)" />
         <param name="role_name" value="$(arg role_name)" />
     </node>
--->
 
-    <node pkg="rqt_plot" type="rqt_plot" output="screen" name="rqt_plot_speed" args="/paf/hero/velocity_as_float /carla/hero/Speed /paf/hero/max_velocity /paf/hero/throttle /paf/hero/vel_diff"/>
+    <!-- Some plot nodes for debugging speed/steering etc. -->
+    <!--node pkg="rqt_plot" type="rqt_plot" output="screen" name="rqt_plot_speed" args="/paf/hero/velocity_as_float /carla/hero/Speed /paf/hero/max_velocity /paf/hero/throttle /paf/hero/vel_diff"/-->
     <!--node pkg="rqt_plot" type="rqt_plot" output="screen" name="rqt_plot_trajectoryfollowing" args="/paf/hero/current_x /paf/hero/current_target_wp"/-->
-    <!--node pkg="rqt_plot" type="rqt_plot" output="screen" name="rqt_plot_steering" args="/carla/hero/vehicle_control_cmd/steer /paf/hero/target_steering_debug"/-->
+    <!--node pkg="rqt_plot" type="rqt_plot" output="screen" name="rqt_plot_steering" args="/carla/hero/vehicle_control_cmd/steer /paf/hero/target_steering_debug /paf/hero/pid_point_debug"/-->
+    <!--node pkg="rqt_plot" type="rqt_plot" output="screen" name="rqt_plot_CONTROLLER" args="/paf/hero/controller"/-->
 
 </launch>

code/acting/src/acting/Acting_DebuggerNode.py

@@ -42,14 +42,20 @@
 # velocity = MAX_VELOCITY_LOW TODO: maybe use velocity publisher?
 # steering = STEERING_CONTROLLER_USED (see below)
 # trajectory = TRAJECTORY_TYPE (see below)
-TEST_TYPE = 0                       # aka. TT
+
+# 3: Test Emergency Breaks on TestType 1
+# TODO IMPLEMENT THIS TODO
+
+# 4: Test Steering-PID in vehicleController
+# TODO TODO
+TEST_TYPE = 2                       # aka. TT
 
 STEERING: float = 0.0               # for TT0: steering -> always straight
-MAX_VELOCITY_LOW: float = 10.0      # for TT0/TT1: low velocity
+MAX_VELOCITY_LOW: float = 5.0      # for TT0/TT1: low velocity
 MAX_VELOCITY_HIGH: float = 20.0     # for TT1: high velocity
 
-STEERING_CONTROLLER_USED = 2  # for TT1/TT2: 0 = both ; 1 = PP ; 2 = Stanley
-TRAJECTORY_TYPE = 0          # for TT2: 0 = Straight ; 1 = SineWave ; 2 = Curve
+STEERING_CONTROLLER_USED = 0  # for TT1/TT2: 0 = both ; 1 = PP ; 2 = Stanley
+TRAJECTORY_TYPE = 1          # for TT2: 0 = Straight ; 1 = SineWave ; 2 = Curve
 
 
 class Acting_Debugger(CompatibleNode):
@@ -68,25 +74,25 @@ def __init__(self):
         self.role_name = self.get_param('role_name', 'ego_vehicle')
         self.control_loop_rate = self.get_param('control_loop_rate', 0.05)
 
-        # Publisher for Dummy-Trajectory
+        # Publisher for Dummy Trajectory
         self.trajectory_pub: Publisher = self.new_publisher(
             Path,
             "/paf/" + self.role_name + "/trajectory",
             qos_profile=1)
 
-        # Publisher for TT0 and TT1 on max_velocity
+        # Publisher for Dummy Velocity
         self.velocity_pub: Publisher = self.new_publisher(
             Float32,
             f"/paf/{self.role_name}/max_velocity",
             qos_profile=1)
 
-        # Steer-SC: Publisher for TT0, constant Steer to stanley_steer
+        # Stanley: Publisher for Dummy Stanley-Steer
         self.stanley_steer_pub: Publisher = self.new_publisher(
             Float32,
             f"/paf/{self.role_name}/stanley_steer",
             qos_profile=1)
 
-        # Steer-PPC: Publisher for TT0, constant Steer to pure_pursuit_steer
+        # PurePursuit: Publisher for Dummy PP-Steer
         self.pure_pursuit_steer_pub: Publisher = self.new_publisher(
             Float32,
             f"/paf/{self.role_name}/pure_pursuit_steer",
@@ -99,32 +105,49 @@ def __init__(self):
             f"/paf/{self.role_name}/controller_selector_debug",
             qos_profile=1)
 
-        # Subscriber of current_pos, used for
+        # Subscriber of current_pos, used for TODO nothing yet
         self.current_pos_sub: Subscriber = self.new_subscription(
             msg_type=PoseStamped,
             topic="/paf/" + self.role_name + "/current_pos",
             callback=self.__current_position_callback,
             qos_profile=1)
 
-        # SUBSCRIBER FOR EVALUATION LISTS FOR TUNING
+        # ---> EVALUATION/TUNING: Subscribers for plotting
+        # Subscriber for max_velocity for plotting
         self.max_velocity_sub: Subscriber = self.new_subscription(
             Float32,
             f"/paf/{self.role_name}/max_velocity",
             self.__get_max_velocity,
             qos_profile=1)
 
+        # Subscriber for current_velocity for plotting
         self.current_velocity_sub: Subscriber = self.new_subscription(
             CarlaSpeedometer,
             f"/carla/{self.role_name}/Speed",
             self.__get_current_velocity,
             qos_profile=1)
 
+        # Subscriber for current_throttle for plotting
         self.current_throttle_sub: Subscriber = self.new_subscription(
             Float32,
             f"/paf/{self.role_name}/throttle",
             self.__get_throttle,
             qos_profile=1)
 
+        # Subscriber for PurePursuit_steer
+        self.pure_pursuit_steer_sub: Subscriber = self.new_subscription(
+            Float32,
+            f"/paf/{self.role_name}/pure_pursuit_steer",
+            self.__get_purepursuit_steer,
+            qos_profile=1)
+
+        # Subscriber for Stanley_Steer
+        self.stanley_steer_sub: Subscriber = self.new_subscription(
+            Float32,
+            f"/paf/{self.role_name}/stanley_steer",
+            self.__get_stanley_steer,
+            qos_profile=1)
+
         # Initialize all needed "global" variables here
         self.current_trajectory = []
         self.checkpoint_time = rospy.get_time()
@@ -136,12 +159,15 @@ def __init__(self):
         self.__throttles = []
         self.time_set = False
 
+        self.__purepursuit_steers = []
+        self.__stanley_steers = []
+
         self.path_msg = Path()
         self.path_msg.header.stamp = rospy.Time.now()
         self.path_msg.header.frame_id = "global"
 
         # Generate Trajectory as selected in TRAJECTORY_TYPE
-        # Spawncoords of the car at the simulationstart TODO: get from position
+        # Spawncoords at the simulationstart TODO: get from position
         startx = 984.5
         starty = -5442.0
 
@@ -247,6 +273,12 @@ def __get_current_velocity(self, data: CarlaSpeedometer):
     def __get_throttle(self, data: Float32):
         self.__throttles.append(float(data.data))
 
+    def __get_stanley_steer(self, data: Float32):
+        self.__stanley_steers.append(float(data.data))
+
+    def __get_purepursuit_steer(self, data: Float32):
+        self.__purepursuit_steers.append(float(data.data))
+
     def run(self):
         """
         Control loop
@@ -285,38 +317,32 @@ def loop(timer_event=None):
                 self.trajectory_pub.publish(self.path_msg)
                 self.velocity_pub.publish(self.driveVel)
 
+            elif (TEST_TYPE == 4):
+                self.drive_Vel = MAX_VELOCITY_LOW
+                self.stanley_steer_pub.publish(STEERING)
+                self.pure_pursuit_steer_pub.publish(STEERING)
+
             if (STEERING_CONTROLLER_USED == 1):
                 self.controller_selector_pub.publish(1)
             elif (STEERING_CONTROLLER_USED == 2):
                 self.controller_selector_pub.publish(2)
 
-            # set starttime to when the simulation is actually starting to run
+            """# set starttime to when simulation is actually starting to run
             # to really get 10 secs plots every time
             if not self.time_set:
                 self.checkpoint_time = rospy.get_time()
-                print(self.checkpoint_time)
                 self.time_set = True
 
             if (self.checkpoint_time < rospy.get_time() - 10.0):
                 self.checkpoint_time = rospy.get_time()
-                print(">>>>>>>>>>>> HIER <<<<<<<<<<<<<<")
-                print(self.checkpoint_time)
+                print(">>>>>>>>>>>> DATA <<<<<<<<<<<<<<")
                 print(self.__max_velocities)
                 print(self.__current_velocities)
                 print(self.__throttles)
                 print(len(self.__max_velocities))
                 print(len(self.__current_velocities))
                 print(len(self.__throttles))
-                """
-                TODO: THIS DOES NOT WORK WITHOUT SUDO PERMISSION
-                doc_folder = os.path.join(os.path.expanduser('~'), 'Documents')
-                data_folder = os.path.join(doc_folder, 'data')
-                os.makedirs(data_folder, exist_ok=True)
-                output_file = os.path.join(data_folder, 'cur_vel.txt')
-                with open (output_file, "w") as f:
-                    for wert in self.__current_velocities:
-                        f.write(str(wert) + "; ")
-                """
+                print(">>>>>>>>>>>> DATA <<<<<<<<<<<<<<")"""
 
         self.new_timer(self.control_loop_rate, loop)
         self.spin()

code/acting/src/acting/stanley_controller.py

@@ -24,7 +24,7 @@ def __init__(self):
         self.control_loop_rate = self.get_param('control_loop_rate', 0.05)
         self.role_name = self.get_param('role_name', 'ego_vehicle')
 
-        # Subscriber
+        # Subscribers
         self.position_sub: Subscriber = self.new_subscription(
             Path,
             f"/paf/{self.role_name}/trajectory",
@@ -49,7 +49,7 @@ def __init__(self):
             self.__set_heading,
             qos_profile=1)
 
-        # Publisher
+        # Publishers
         self.stanley_steer_pub: Publisher = self.new_publisher(
             Float32,
             f"/paf/{self.role_name}/stanley_steer",
@@ -60,6 +60,9 @@ def __init__(self):
             f"/paf/{self.role_name}/stanley_debug",
             qos_profile=1)
 
+        # Publishers for debugging StanleyController soon
+        # TODO: only works with perfect current_pos
+        # publish the target and the current position
         self.targetwp_publisher: Publisher = self.new_publisher(
             Float32,
             f"/paf/{self.role_name}/current_target_wp",
@@ -70,8 +73,8 @@ def __init__(self):
             f"/paf/{self.role_name}/current_x",
             qos_profile=1)
 
-        self.__position: (float, float) = None  # x, y
-        self.__last_pos: (float, float) = None
+        self.__position: (float, float) = None  # x , y
+        self.__last_pos: (float, float) = None  # x , y
         self.__path: Path = None
         self.__heading: float = None
         self.__velocity: float = None
@@ -163,10 +166,10 @@ def __set_velocity(self, data: CarlaSpeedometer):
     def __calculate_steer(self) -> float:
         """
         Calculates the steering angle based on the current information
-        using the Stanly algorithm
+        using the Stanley algorithm
         :return: steering angle
         """
-        k_ce = 0.10  # todo: tune
+        k_ce = 0.10  # TODO: tune
         k_v = 1.0
 
         current_velocity: float
@@ -187,7 +190,7 @@ def __calculate_steer(self) -> float:
                                             current_velocity * k_v)
         steering_angle *= - 1
 
-        # for debugging ->
+        # for debugging TODO: currently not that useful->
         debug_msg = StanleyDebug()
         debug_msg.heading = self.__heading
         debug_msg.path_heading = traj_heading
@@ -196,7 +199,6 @@ def __calculate_steer(self) -> float:
         debug_msg.steering_angle = steering_angle
         self.debug_publisher.publish(debug_msg)
         # <-
-
         # 2 more debugging messages: TODO: maybe put into DEBUGGER NODE?
         self.targetwp_publisher.publish((closest_point.pose.position.x-984.5))
         self.currentx_publisher.publish(self.__position[0]-984.5)