Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Refs/heads/488 feature split up acting controller #508

Merged
merged 7 commits into from
Nov 22, 2024
Merged

Refs/heads/488 feature split up acting controller #508

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
ed8b710
Split up Acting and Control
vinzenzm Nov 14, 2024
ae9f0e7
Added control documentation
vinzenzm Nov 16, 2024
4b90716
Updated acting and control documentation. No Code changes.
vinzenzm Nov 16, 2024
5760c6d
Minor documentation improvements on acting and control
vinzenzm Nov 16, 2024
4fb8afb
Made the linter happy with newlines and trailing spaces
vinzenzm Nov 17, 2024
9cb4c9e
Renamed passthrough dataclass to TopicMapping in passthrough
vinzenzm Nov 21, 2024
efee92c
Merge branch 'main' into 488-feature-split-up-acting--controller
seitzseb Nov 21, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
38 changes: 9 additions & 29 deletions code/acting/launch/acting.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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>
87 changes: 87 additions & 0 deletions code/acting/src/acting/passthrough.py
View file
Open in desktop
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

having a "passthrough" class in upper and lowercase is VERY confusing, but i see one is a dataclass and the other is not.
could you please explain why it is not possible to name the dataclass different ?

Original file line number Diff line number Diff line change
@@ -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
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

⚠️ Potential issue

Remove hardcoded role_name value

The comment indicates this is legacy code that will change soon. Consider making this configurable through ROS parameters instead of hardcoding.

-    role_name = "hero"  # Legacy will change soon
+    def __init__(self):
+        super().__init__('passthrough')
+        self.role_name = self.get_parameter_or('role_name', 'hero').value

Committable suggestion skipped: line range outside the PR's diff.


# 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,
)
Comment on lines +31 to +51
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

🛠️ Refactor suggestion

Improve topic configuration management

The topic paths are currently hardcoded with f-strings. Consider moving these configurations to a separate configuration file or ROS parameters for better maintainability.

Example approach:

def get_topic_mappings(self):
    return [
        TopicMapping(
            pub_name=self.get_parameter_or('target_velocity_pub', '/paf/acting/target_velocity').value,
            sub_name=self.get_parameter_or('target_velocity_sub', f'/paf/{self.role_name}/target_velocity').value,
            topic_type=Float32
        ),
        # ... other mappings
    ]


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()
18 changes: 18 additions & 0 deletions code/control/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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(
)

28 changes: 28 additions & 0 deletions code/control/launch/control.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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>
22 changes: 22 additions & 0 deletions code/control/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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>
6 changes: 6 additions & 0 deletions code/control/setup.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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)
Empty file added code/control/src/__init__.py
View file
Open in desktop
Empty file.
8 changes: 4 additions & 4 deletions ...ing/src/acting/pure_pursuit_controller.py → code/control/src/pure_pursuit_controller.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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,
)
Expand All @@ -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,
)
Expand Down
8 changes: 4 additions & 4 deletions code/acting/src/acting/stanley_controller.py → code/control/src/stanley_controller.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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!
Expand All @@ -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,
)
Expand All @@ -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,
)
Expand Down
0 code/acting/src/acting/vehicle_controller.py → code/control/src/vehicle_controller.py
View file
Open in desktop
File renamed without changes.
2 changes: 1 addition & 1 deletion .../acting/src/acting/velocity_controller.py → code/control/src/velocity_controller.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
)
Expand Down
10 changes: 4 additions & 6 deletions doc/acting/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
6 changes: 3 additions & 3 deletions doc/acting/acting_testing.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down
Loading
Loading