Skip to content

Commit

Permalink
Merge pull request #237 from una-auxme/220-feature-documentation-acting
Browse files Browse the repository at this point in the history 
220 feature documentation acting
  • Loading branch information
@hellschwalex
hellschwalex authored Mar 28, 2024
2 parents c113052 + ccaa2a5 commit dec7592
Show file tree
Hide file tree
Showing 72 changed files with 881 additions and 2,621 deletions.
37 changes: 9 additions & 28 deletions code/acting/launch/acting.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,12 +3,17 @@
<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">
<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="velocity_controller.py" name="velocity_controller" output="screen">
<node pkg="acting" 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="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>
Expand All @@ -30,31 +35,7 @@
</node>
-->

<node pkg="acting" 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>

<!-- ______OBSOLETES______
<node pkg="acting" type="acting_velocity_publisher.py" name="acting_velocity_publisher" output="screen">
<param name="role_name" value="$(arg role_name)" />
<param name="control_loop_rate" value="0.2" />
<param name="enabled" value="False" />
</node>
<node pkg="acting" type="acc_distance_publisher_dummy.py" name="AccDistancePublisherDummy" output="screen">
<param name="role_name" value="$(arg role_name)" />
<param name="enabled" value="False" />
</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>
-->

<!-- Some plot nodes for debugging speed/steering etc. -->
<!--node pkg="rqt_plot" type="rqt_plot" output="screen" name="rqt_plot_speed" args="/carla/hero/Speed /paf/hero/target_velocity /paf/hero/throttle /paf/hero/brake"/-->
<!--node pkg="rqt_plot" type="rqt_plot" output="screen" name="rqt_plot_steering" args="/paf/hero/pure_pursuit_steer /carla/hero/vehicle_control_cmd/steer /paf/hero/pure_p_debug/l_distance"/-->
<!-- 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>
68 changes: 34 additions & 34 deletions code/acting/readme.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,62 +6,62 @@

## Authors

Julian Graf

Gabriel Schwald
Alexander Hellmann

## Date

29.03.2023
01.04.2024

---

<!-- TOC -->
* [Acting](#acting)
* [Authors](#authors)
* [Date](#date)
* [Basics](#basics)
* [Content of this package](#content-of-this-package)
* [Longitudinal controllers](#longitudinal-controllers)
* [Lateral controllers](#lateral-controllers)
* [Vehicle controller](#vehicle-controller)
* [Visualization](#visualization)
* [more Content](#more-content)
- [Acting](#acting)
- [Authors](#authors)
- [Date](#date)
- [Acting Documentation](#acting-documentation)
- [Test/Debug/Tune Acting-Components](#testdebugtune-acting-components)
- [Longitudinal controllers (Velocity Controller)](#longitudinal-controllers-velocity-controller)
- [Lateral controllers (Steering Controllers)](#lateral-controllers-steering-controllers)
- [Vehicle controller](#vehicle-controller)
- [Visualization of the HeroFrame in rviz](#visualization-of-the-heroframe-in-rviz)
<!-- TOC -->

## Basics
## Acting Documentation

In order to further understand the general idea of our approach to the acting component please refer to the documentation of our [research](../../doc/03_research/01_acting) and our planned [general definition](../../doc/01_general/04_architecture.md#acting).
In order to further understand the general idea of the taken approach to the acting component please refer to the documentation of the [research](../../doc/03_research/01_acting/Readme.md) done and see the planned [general definition](../../doc/01_general/04_architecture.md#acting).

---
It is also highly recommended to go through the indepth [Acting-Documentation](../../doc/05_acting/Readme.md)!

## Test/Debug/Tune Acting-Components

The Acting_Debug_Node can be used as a simulated Planning package, publishing adjustable target velocities, steerings and trajectories as needed.

## Content of this package
For more information about this node and how to use it, please read the [documentation](../../doc/05_acting/05_acting_testing.md).
You can also find more information in the commented [code](./src/acting/Acting_Debug_Node.py).

This package contain all acting nodes. For general information on what the acting does, see the [planned architecture](./../../doc/01_general/04_architecture.md#acting).
## Longitudinal controllers (Velocity Controller)

### Longitudinal controllers
The longitudinal controller is implemented as a PID velocity controller.

The longitudinal controller is implemented as a PID velocity controller ([VelocityController](./src/acting/velocity_controller.py)). The acting also publishes a max velocity based onn upcoming curves ([ActingVelocityPublisher](./src/acting/acting_velocity_publisher.py)).
There also is an [ACC](../../doc/05_acting/02_acc.md) implemented in acting ([Acc](./src/acting/acc.py)).
For more information about this controller, either read the [documentation](../../doc/05_acting/02_velocity_controller.md) or go through the commented [code](./src/acting/velocity_controller.py).

### Lateral controllers
## Lateral controllers (Steering Controllers)

There are two steering controllers implemented in the acting:
There are two steering controllers currently implemented, both providing live telemetry via Debug-Messages:

* Pure Persuit Controller
* Stanley Controller
- Pure Persuit Controller (paf/hero/pure_p_debug)
- Stanley Controller (paf/hero/stanley_debug)

For further information see [lateral controllers](./../../doc/05_acting/03_lateral_controller.md).
Both controllers provide live telemetry (on paf/hero/purep_debug and paf/hero/stanley_debug).
For further information about the steering controllers, either read the [documentation](./../../doc/05_acting/03_steering_controllers.md) or go through the commented code of [stanley_controller](./src/acting/stanley_controller.py) or [purepursuit_controller](./src/acting/pure_pursuit_controller.py).

### Vehicle controller
## Vehicle controller

The [VehicleController](./src/acting/vehicle_controller.py) collects all necessary msgs and publishes the [CarlaEgoVehicleControl](https://carla.readthedocs.io/en/0.9.8/ros_msgs/#carlaegovehiclecontrol) for the [Carla ros bridge](https://github.com/carla-simulator/ros-bridge).
The VehicleController collects all necessary msgs from the other controllers and publishes the [CarlaEgoVehicleControl](https://carla.readthedocs.io/en/0.9.8/ros_msgs/#carlaegovehiclecontrol) for the [Carla ros bridge](https://github.com/carla-simulator/ros-bridge).

### Visualization
It also executes emergency-brakes and the unstuck-routine, if detected.

For information about vizualizing the upcomming path in rviz see [Main frame publisher](../../doc/05_acting/04_main_frame_publisher.md)
For more information about this controller, either read the [documentation](../../doc/05_acting/04_vehicle_controller.md) or go through the commented [code](./src/acting/vehicle_controller.py).

## more Content
## Visualization of the HeroFrame in rviz

For more detailed information on all relevant components of the acting domain please refer to [the acting documentation](../../doc/05_acting).
For information about vizualizing the upcomming path in rviz see [Main frame publisher](../../doc/05_acting/06_main_frame_publisher.md)
109 changes: 67 additions & 42 deletions code/acting/src/acting/Acting_Debug_Node.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,11 +3,10 @@
"""
This node provides full testability for all Acting
components by offering different testcases
to hopefully fully implement and tune Acting without
to fully implement, test and tune Acting without
the need of working Perception and Planning components.
This also generates Lists of all the important values
to be saved in a file and plotted again.
TODO: emergency brake behavior
"""

import math
Expand All @@ -21,38 +20,44 @@
from rospy import Publisher, Subscriber
from carla_msgs.msg import CarlaSpeedometer, CarlaEgoVehicleControl

from trajectory_interpolation import interpolate_route

# TEST_TYPE to choose which kind of Test to run:
# 0: Test Velocity Controller with constant one velocity
# const. velocity = TARGET_VELOCITY_1
# const. steering = 0
# no trajectory
# TURN OFF PP Controller in acting.launch!

# 1: Test Velocity Controller with changing velocity
# velocity = alternate all 20 secs: TARGET_VELOCITY_1/_2
# const. steering = 0
# no trajectory
# TURN OFF PP Controller in acting.launch!

# 2: Test Steering Controller on chooseable trajectory
# velocity = TARGET_VELOCITY_1
# steering = STEERING_CONTROLLER_USED (see below)
# trajectory = TRAJECTORY_TYPE (see below)

# 3: Test Emergency Breaks on TestType 1
# const velocity = TARGET_VELOCITY_1
# const steering = 0
# no trajectory
# Triggers emergency break after 15 Seconds
from helper_functions import interpolate_route

"""
TEST_TYPE to choose which kind of Test to run:
- 0: Test Velocity Controller with constant one velocity
const. velocity = TARGET_VELOCITY_1
const. steering = 0
no trajectory
TURN OFF PP Controller in acting.launch!
- 1: Test Velocity Controller with changing velocity
velocity = alternate all 20 secs: TARGET_VELOCITY_1/_2
const. steering = 0
no trajectory
TURN OFF PP Controller in acting.launch!
- 2: Test Steering Controller on chooseable trajectory
velocity = TARGET_VELOCITY_1
steering = use acting.launch to selcet controller
trajectory = TRAJECTORY_TYPE (see below)
- 3: Test Emergency Breaks on TestType 1
const velocity = TARGET_VELOCITY_1
const steering = 0
no trajectory
Triggers emergency break after 15 Seconds
"""
TEST_TYPE = 2
FIXED_STEERING: float = 0 # if fixed steering needed
TARGET_VELOCITY_1: float = 10 # standard velocity
TARGET_VELOCITY_2: float = 0 # second velocity to switch to
# 0 = Straight ; 1 = Curve ; 2 = SineWave ; 3 = Overtake
TRAJECTORY_TYPE = 3

# This Component also prints collected data to the terminal,
# if wanted. Use the following Variables to select what to print.
# see lines 395 - 421 if you want to edit these!
PRINT_AFTER_TIME = 10.0 # How long after Simulationstart to print data
PRINT_TRAJECTORY = False # True = prints the published trajectory
PRINT_VELOCITY_DATA = False # True = print target and current velocities
PRINT_STEERING_DATA = False # True = print stanley and pp steerings


class Acting_Debug_Node(CompatibleNode):
Expand Down Expand Up @@ -104,27 +109,34 @@ def __init__(self):
self.__get_target_velocity,
qos_profile=1)

# Subscriber for current_headng for plotting
# Subscriber for current_heading
self.heading_sub: Subscriber = self.new_subscription(
Float32,
f"/paf/{self.role_name}/current_heading",
self.__get_heading,
qos_profile=1)

# Subscriber for current_velocity for plotting
# Subscriber for current_velocity
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
# Subscriber for current_throttle
self.current_throttle_sub: Subscriber = self.new_subscription(
Float32,
f"/paf/{self.role_name}/throttle",
self.__get_throttle,
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)

# Subscriber for PurePursuit_steer
self.pure_pursuit_steer_sub: Subscriber = self.new_subscription(
Float32,
Expand Down Expand Up @@ -158,8 +170,8 @@ def __init__(self):
self.__throttles = []
self.__current_headings = []
self.__purepursuit_steers = []
self.__stanley_steers = []
self.__vehicle_steers = []
self.stanley_cross_errors = []
self.positions = []

# Generate Trajectory as selected in TRAJECTORY_TYPE
Expand Down Expand Up @@ -286,6 +298,7 @@ def updated_trajectory(self, target_trajectory):
pos.pose.orientation.w = 0
self.path_msg.poses.append(pos)

# ALL SUBSCRIBER-FUNCTIONS HERE
def __current_position_callback(self, data: PoseStamped):
agent = data.pose.position
self.x = agent.x
Expand All @@ -308,6 +321,11 @@ 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):
r = 1 / (math.pi / 2)
steering_float = float(data.data) * r
self.__stanley_steers.append(steering_float)

def __get_purepursuit_steer(self, data: Float32):
r = 1 / (math.pi / 2)
steering_float = float(data.data) * r
Expand Down Expand Up @@ -378,23 +396,30 @@ def loop(timer_event=None):
if not self.time_set:
self.checkpoint_time = rospy.get_time()
self.time_set = True
# print(">>>>>>>>>>>> TRAJECTORY <<<<<<<<<<<<<<")
# print(self.current_trajectory)
# print(">>>>>>>>>>>> TRAJECTORY <<<<<<<<<<<<<<")
if PRINT_TRAJECTORY:
print(">>>>>>>>>>>> TRAJECTORY <<<<<<<<<<<<<<")
print(self.current_trajectory)
print(">>>>>>>>>>>> TRAJECTORY <<<<<<<<<<<<<<")

# Uncomment the prints of the data you want to plot
if (self.checkpoint_time < rospy.get_time() - PRINT_AFTER_TIME):
self.checkpoint_time = rospy.get_time()
print(">>>>>>>>>>>> DATA <<<<<<<<<<<<<<")
# print(self.__max_velocities)
# print(self.__current_velocities)
# print(self.__throttles)
# print(self.__purepursuit_steers)
# print(self.__vehicle_steers)
# print(self.__current_headings)
print(self.positions)
if PRINT_VELOCITY_DATA:
print(">> TARGET VELOCITIES <<")
print(self.__max_velocities)
print(">> CURRENT VELOCITIES <<")
print(self.__current_velocities)
print(">> THROTTLES <<")
print(self.__throttles)
if PRINT_STEERING_DATA:
print(">> PUREPURSUIT STEERS <<")
print(self.__purepursuit_steers)
print(">> STANLEY STEERS <<")
print(self.__stanley_steers)
print(">> ACTUAL POSITIONS <<")
print(self.positions)
print(">>>>>>>>>>>> DATA <<<<<<<<<<<<<<")

self.new_timer(self.control_loop_rate, loop)
self.spin()

Expand Down
Loading

0 comments on commit dec7592

Please sign in to comment.