Merge pull request #508 from una-auxme/488-feature-split-up-acting--c…

…ontroller

Refs/heads/488 feature split up acting  controller

code/acting/launch/acting.launch

@@ -1,41 +1,21 @@
 <!---->
 <launch>
     <arg name="role_name" default="hero" />
-    <arg name="control_loop_rate" default="0.05" /> 
+    <arg name="control_loop_rate" default="0.05" />
 
-    <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)" />
-    </node>
-
-    <node pkg="acting" type="stanley_controller.py" name="stanley_controller" output="screen">
-        <param name="control_loop_rate" value="$(arg control_loop_rate)" />
+    <!--Passthrough
+    all messages the control package needs-->
+    <node pkg="acting" type="passthrough.py" name="passthrough" output="screen">
         <param name="role_name" value="$(arg role_name)" />
-    </node>
-
-    <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)" />
-    </node>
-
-     <node pkg="acting" type="vehicle_controller.py" name="vehicle_controller" output="screen">
-        <param name="control_loop_rate" value="$(arg control_loop_rate)"  /> <!-- leaderboard expects commands every 0.05 seconds OTHERWISE IT LAGS REALLY BADLY-->
-        <param name="role_name" value="$(arg role_name)" />
     </node>
 
     <node pkg="acting" type="MainFramePublisher.py" name="MainFramePublisher" output="screen">
-        <param name="control_loop_rate" value="$(arg control_loop_rate)"  />
-        <param name="role_name" value="$(arg role_name)" />
-    </node>
-
-    <!-- UNCOMMENT THIS TO USE THE DEBUG_NODE FOR ACTING-TESTING -->
-    <!-- <node pkg="acting" type="Acting_Debug_Node.py" name="Acting_Debug_Node" output="screen">
-        <param name="control_loop_rate" value="$(arg control_loop_rate)"  />
+        <param name="control_loop_rate" value="$(arg control_loop_rate)" />
         <param name="role_name" value="$(arg role_name)" />
     </node>
-     -->
-
-    <!-- If you want a live rqt_plots to show up automatically, include them like following example-plot for Velocity-Controller-Testing -->
-    <!--node pkg="rqt_plot" type="rqt_plot" output="screen" name="velocity_test" args="/carla/hero/Speed /paf/hero/target_velocity /paf/hero/throttle /paf/hero/brake"/-->
 
-</launch>
+    <include file="$(find control)/launch/control.launch">
+        <arg name="control_loop_rate" value="$(arg control_loop_rate)" />
+    </include>
+</launch>

code/acting/src/acting/passthrough.py

@@ -0,0 +1,87 @@
+#!/usr/bin/env python
+
+import ros_compatibility as roscomp
+from ros_compatibility.node import CompatibleNode
+from rospy import Publisher, Subscriber
+from std_msgs.msg import Float32
+from geometry_msgs.msg import PoseStamped
+from nav_msgs.msg import Path
+
+
+from dataclasses import dataclass
+from typing import Type, Dict
+
+
+@dataclass
+class TopicMapping:
+    pub_name: str
+    sub_name: str
+    topic_type: Type
+
+
+class Passthrough(CompatibleNode):
+    """This nodes sole purpose is to pass through all messages that control needs.
+    The purpose of this is that Control-Package should not have any global dependencies,
+    but is only dependent on the acting package.
+    """
+
+    role_name = "hero"  # Legacy will change soon
+
+    # Topics for velocity controller.
+    target_velocity = TopicMapping(
+        pub_name="/paf/acting/target_velocity",
+        sub_name=f"/paf/{role_name}/target_velocity",
+        topic_type=Float32,
+    )
+    # Topics for steering controllers
+    trajectory = TopicMapping(
+        pub_name="/paf/acting/trajectory",
+        sub_name=f"/paf/{role_name}/trajectory",
+        topic_type=Path,
+    )
+    position = TopicMapping(
+        pub_name="/paf/acting/current_pos",
+        sub_name=f"/paf/{role_name}/current_pos",
+        topic_type=PoseStamped,
+    )
+    heading = TopicMapping(
+        pub_name="/paf/acting/current_heading",
+        sub_name=f"/paf/{role_name}/current_heading",
+        topic_type=Float32,
+    )
+
+    mapped_topics = [target_velocity, trajectory, position, heading]
+
+    def __init__(self):
+        self.publishers: Dict[str, Publisher] = {}
+        self.subscribers: Dict[str, Subscriber] = {}
+        for topic in self.mapped_topics:
+            self.publishers[topic.pub_name] = self.new_publisher(
+                topic.topic_type, topic.pub_name, qos_profile=1
+            )
+
+            self.subscribers[topic.pub_name] = self.new_subscription(
+                topic.topic_type,
+                topic.sub_name,
+                callback=self.publishers[topic.pub_name].publish,
+                qos_profile=1,
+            )
+
+
+def main(args=None):
+    """Start the node.
+    This is the entry point, if called by a launch file.
+    """
+    roscomp.init("passthrough", args=args)
+
+    try:
+        node = Passthrough()
+        node.spin()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        roscomp.shutdown()
+
+
+if __name__ == "__main__":
+    main()

code/control/CMakeLists.txt

@@ -0,0 +1,18 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(control)
+
+
+find_package(catkin REQUIRED)
+
+catkin_python_setup()
+
+find_package(catkin REQUIRED COMPONENTS
+    rospy
+    std_msgs
+)
+catkin_package()
+
+
+include_directories(
+)
+

code/control/launch/control.launch

@@ -0,0 +1,28 @@
+<launch>
+    <arg name="role_name" default="hero" />
+    <arg name="control_loop_rate" default="0.05" />
+
+
+    <node pkg="control" 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)" />
+    </node>
+
+    <node pkg="control" type="stanley_controller.py" name="stanley_controller" output="screen">
+        <param name="control_loop_rate" value="$(arg control_loop_rate)" />
+        <param name="role_name" value="$(arg role_name)" />
+    </node>
+
+    <node pkg="control" 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)" />
+    </node>
+
+    <node pkg="control" type="vehicle_controller.py" name="vehicle_controller" output="screen">
+        <param name="control_loop_rate" value="$(arg control_loop_rate)" /> <!-- leaderboard expects
+    commands every 0.05 seconds OTHERWISE IT LAGS REALLY BADLY-->
+        <param name="role_name" value="$(arg role_name)" />
+    </node>
+
+</launch>

code/control/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>control</name>
+  <version>0.0.0</version>
+  <description>The control package for PAF Carla</description>
+
+  <maintainer email="[email protected]">Vinzenz Malke</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <exec_depend>message_runtime</exec_depend>
+  <buildtool_depend>catkin</buildtool_depend>
+
+
+  <export>
+  </export>
+</package>

code/control/setup.py

@@ -0,0 +1,6 @@
+#!/usr/bin/env python
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+setup_args = generate_distutils_setup(packages=["control"], package_dir={"": "src"})
+setup(**setup_args)

code/acting/src/acting/pure_pursuit_controller.py → code/control/src/pure_pursuit_controller.py

@@ -11,7 +11,7 @@
 from acting.msg import Debug
 import numpy as np
 
-from helper_functions import vector_angle, points_to_vector
+from acting.helper_functions import vector_angle, points_to_vector
 
 # Tuneable Values for PurePursuit-Algorithm
 K_LAD = 0.85  # optimal in dev-launch
@@ -33,12 +33,12 @@ def __init__(self):
         self.role_name = self.get_param("role_name", "ego_vehicle")
 
         self.position_sub: Subscriber = self.new_subscription(
-            Path, f"/paf/{self.role_name}/trajectory", self.__set_path, qos_profile=1
+            Path, "/paf/acting/trajectory", self.__set_path, qos_profile=1
         )
 
         self.path_sub: Subscriber = self.new_subscription(
             PoseStamped,
-            f"/paf/{self.role_name}/current_pos",
+            "/paf/acting/current_pos",
             self.__set_position,
             qos_profile=1,
         )
@@ -52,7 +52,7 @@ def __init__(self):
 
         self.heading_sub: Subscriber = self.new_subscription(
             Float32,
-            f"/paf/{self.role_name}/current_heading",
+            "/paf/acting/current_heading",
             self.__set_heading,
             qos_profile=1,
         )

code/acting/src/acting/stanley_controller.py → code/control/src/stanley_controller.py

@@ -12,7 +12,7 @@
 from std_msgs.msg import Float32
 from acting.msg import StanleyDebug
 
-from helper_functions import vector_angle, points_to_vector
+from acting.helper_functions import vector_angle, points_to_vector
 
 
 K_CROSSERR = 0.4  # 1.24 was optimal in dev-launch!
@@ -28,12 +28,12 @@ def __init__(self):
 
         # Subscribers
         self.position_sub: Subscriber = self.new_subscription(
-            Path, f"/paf/{self.role_name}/trajectory", self.__set_path, qos_profile=1
+            Path, "/paf/acting/trajectory", self.__set_path, qos_profile=1
         )
 
         self.path_sub: Subscriber = self.new_subscription(
             PoseStamped,
-            f"/paf/{self.role_name}/current_pos",
+            "/paf/acting/current_pos",
             self.__set_position,
             qos_profile=1,
         )
@@ -47,7 +47,7 @@ def __init__(self):
 
         self.heading_sub: Subscriber = self.new_subscription(
             Float32,
-            f"/paf/{self.role_name}/current_heading",
+            "/paf/acting/current_heading",
             self.__set_heading,
             qos_profile=1,
         )

code/acting/src/acting/velocity_controller.py → code/control/src/velocity_controller.py

@@ -23,7 +23,7 @@ def __init__(self):
 
         self.target_velocity_sub: Subscriber = self.new_subscription(
             Float32,
-            f"/paf/{self.role_name}/target_velocity",
+            "/paf/acting/target_velocity",
             self.__get_target_velocity,
             qos_profile=1,
         )

doc/acting/README.md

@@ -2,9 +2,7 @@
 
 This folder contains the documentation of the acting component.
 
-1. [Architecture](./architecture_documentation.md)
-2. [Overview of the Velocity Controller](./velocity_controller.md)
-3. [Overview of the Steering Controllers](./steering_controllers.md)
-4. [Overview of the Vehicle Controller Component](./vehicle_controller.md)
-5. [How to test/tune acting components independedly](./acting_testing.md)
-6. [Main frame publisher](./mainframe_publisher.md)
+1. [Overview and Architecture](./architecture_documentation.md)
+2. [Passthrough](./passthrough.md)
+3. [Main frame publisher](./main_frame_publisher.md)
+4. [How to test/tune the acting component](./acting_testing.md)

doc/acting/acting_testing.md

@@ -8,15 +8,15 @@
 
 ## Acting_Debug_Node
 
-The [Acting_Debug_Node](../../code/acting/src/acting/Acting_Debug_Node.py) allows you to tune/test/verify all acting components independently of Planning-Inputs and also lets you easily output the data needed to plot/verify the tuning/testing you have done.
+The [Acting_Debug_Node](../../code/acting/src/acting/Acting_Debug_Node.py) allows you to tune/test/verify all acting and control components independently of Planning-Inputs and also lets you easily output the data needed to plot/verify the tuning/testing you have done.
 
 There is a dedicated [acting_debug.launch](../../code/acting/launch/acting_debug.launch) file which starts the acting component **as well as the necesarry perception components** automatically. It is recommended to first test the [Acting_Debug_Node](../../code/acting/src/acting/Acting_Debug_Node.py)
 with an empty road scenario. The following guide provides instructions on how to launch this setup.
 
 ### Setup for Testing with the Debug-Node
 
-To use the [Acting_Debug_Node](../../code/acting/src/acting/Acting_Debug_Node.py) you first have to edit a few files in your current branch: \\
-**! Make sure to revert these changes when pushing your branch!**
+To use the [Acting_Debug_Node](../../code/acting/src/acting/Acting_Debug_Node.py) you first have to edit a few files in your current branch:
+>[!WARNING]! Make sure to revert these changes when pushing your branch!
 
 - In [dev.launch](../../code/agent/launch/dev.launch) the [agent.launch](../../code/agent/launch/agent.launch) file is included. Change this to include [acting_debug.launch](../../code/acting/launch/acting_debug.launch) from the **acting** package.
 As mentioned above this includes everything we need.