Arc 78 trajectory follower integration #2419

saina-ramyar · 2024-07-26T21:04:25Z

PR Details

Description

The Trajectory Follower Wrapper is being tested on a physical vehicle.
related PRs:
usdot-fhwa-stol/autoware.auto#26
https://github.com/usdot-fhwa-stol/carma-vehicle-calibration/pull/128

Related GitHub Issue

Related Jira Key

(https://usdot-carma.atlassian.net/browse/ARC-78)

Motivation and Context

Integrate the Trajectory Follower controller of Autoware, that has a MPC based lateral controller, to improve vehicle's performance.

How Has This Been Tested?

This controller is tested on Blue Lexus.
Notes for testing:

Ensure the calibration repo is on the correct branch, to receive configuration parameters
In Subsystem Controller, replace "pure_pursuit_wraper" with "trajectory_follower_wrapper"
Make sure to use the localization mode "GNSS only with NDT initialization" so /tf topic has correct information
Check the log to see if MPC is being solved at each step.

Types of changes

Defect fix (non-breaking change that fixes an issue)
New feature (non-breaking change that adds functionality)
Breaking change (fix or feature that cause existing functionality to change)

Checklist:

I have added any new packages to the sonar-scanner.properties file
My change requires a change to the documentation.
I have updated the documentation accordingly.
I have read the CONTRIBUTING document.
[] I have added tests to cover my changes.
All new and existing tests passed.

trajectory_follower_wrapper/src/trajectory_follower_wrapper_node.cpp

...ry_follower_wrapper/include/trajectory_follower_wrapper/trajectory_follower_wrapper_node.hpp

..._follower_wrapper/include/trajectory_follower_wrapper/trajectory_follower_wrapper_config.hpp

carma/launch/guidance.launch.py

basic_autonomy/src/basic_autonomy.cpp

MishkaMN · 2024-08-15T15:06:15Z

carma/launch/guidance.launch.py

+ trajectory_follower_container = ComposableNodeContainer(
+ package='carma_ros2_utils',
+ name='trajectory_follower_container',
+ executable='carma_component_container_mt',
+ namespace=GetCurrentNamespace(),
+ composable_node_descriptions=[
+ ComposableNode(
+ package='trajectory_follower_nodes',
+ plugin='autoware::motion::control::trajectory_follower_nodes::LatLonMuxer',
+ name='latlon_muxer_node',
+ extra_arguments=[
+ {'use_intra_process_comms': False},
+ {'--log-level' : GetLogLevel('latlon_muxer', env_log_levels) }
+ ],
+ parameters=[
+ {'timeout_thr_sec':0.5}
+ ]
+ ),
+ ComposableNode(
+ package='trajectory_follower_nodes',
+ plugin='autoware::motion::control::trajectory_follower_nodes::LateralController',
+ name='lateral_controller_node',
+ extra_arguments=[
+ {'use_intra_process_comms': True},
+ {'--log-level' : GetLogLevel('lateral_controller', env_log_levels) }
+ ],
+ remappings = [
+ ("output/lateral/control_cmd", "input/lateral/control_cmd")
+ ],
+ parameters = [
+ [vehicle_calibration_dir, "/mpc_follower/lateral_controller_defaults.yaml"]
+ ]
+ ),
+ ComposableNode(
+ package='trajectory_follower_nodes',
+ plugin='autoware::motion::control::trajectory_follower_nodes::LongitudinalController',
+ name='longitudinal_controller_node',
+ extra_arguments=[
+ {'use_intra_process_comms': False},
+ {'--log-level' : GetLogLevel('longitudinal_controller', env_log_levels) }
+ ],
+ remappings = [
+ ("output/longitudinal/control_cmd", "input/longitudinal/control_cmd"),
+ ("input/current_trajectory", "input/reference_trajectory"),
+ ("input/current_state", "input/current_kinematic_state")
+ ],
+ parameters = [
+ [vehicle_calibration_dir, "/mpc_follower/longitudinal_controller_defaults.yaml"]
+ ]
+ )
+ ]
+ )
+


I think this might be better to be in the plugins.launch.py close to its "main" node trajectory_follower_wrapper

I dont have a problem with that, but it seemed to me that the pluginns.launch.py has all the carma plugins that we developed. While external autoware nodes such as twist filter are in guidance.launch.py
What do you think?

It just seems easier if all the related nodes were in one file. Not a blocker though.

MishkaMN · 2024-08-15T15:18:17Z

carma/launch/guidance.launch.py

+ {'--log-level' : GetLogLevel('lateral_controller', env_log_levels) }
+ ],
+ remappings = [
+ ("output/lateral/control_cmd", "input/lateral/control_cmd")


I think it might be better if we drop the input output naming convention for the final name resulting from the mapping. for example, consider revising to:

("output/lateral/control_cmd", "lateral/control_cmd")

(above suggestion is assuming the final topic name includes the node's name if I remember correctly. I am imagining it should look like /guidance/lateral_controller_node/lateral/control_cmd)

This is mainly because the name "input" would be confusing if we inspect this specific node (instead of the main trajectory_follower) because it is still output for this one.

These are not topic names that I came up with. They are Autoware's nodes expected topics that are published by one, and consumed by another one. And I tried to minimize changes to Autoware nodes in case we want to update the package. Let me know what you think?

I agree, the input/output naming here is a little confusing. As long as we only remapping the topic names in the launch, it should still be the same effort to update the package?

@saina-ramyar yeah we don't need to change names in the autoware package itself. Just topic remapping for general naming here in the launch file would be good. Since we are already topic remapping from "output" to "input", which means every topic would end up being "input", why not drop it and rename it to something general.

"input/lateral/control_cmd" would be just sth like "lateral/control_cmd"

MishkaMN · 2024-08-15T15:19:33Z

carma/launch/guidance.launch.py

+ ("output/longitudinal/control_cmd", "input/longitudinal/control_cmd"),
+ ("input/current_trajectory", "input/reference_trajectory"),
+ ("input/current_state", "input/current_kinematic_state")


Same with above comment:

("output/longitudinal/control_cmd", "longitudinal/control_cmd"), ("input/current_trajectory", "reference_trajectory"), ("input/current_state", "current_kinematic_state")

MishkaMN · 2024-08-15T15:21:35Z

carma/launch/plugins.launch.py

+ ("input/current_kinematic_state", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''),"/input/current_kinematic_state"]),
+ ("input/reference_trajectory", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''),"/input/reference_trajectory"]),
+ ("output/control_cmd" , [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''),"/output/control_cmd"]),


If we follow comments on the guidance.launch.py, this would change to match as well

MishkaMN · 2024-08-15T15:25:12Z

carma/launch/guidance.launch.py

+ package='trajectory_follower_nodes',
+ plugin='autoware::motion::control::trajectory_follower_nodes::LatLonMuxer',
+ name='latlon_muxer_node',
+ extra_arguments=[
+ {'use_intra_process_comms': False},
+ {'--log-level' : GetLogLevel('latlon_muxer', env_log_levels) }
+ ],
+ parameters=[
+ {'timeout_thr_sec':0.5}
+ ]


If this node also has some output, input topic in the name. it should probably be remappaed to drop those as mentioned in my later comments

The output of this node are subscribed to by trajectory follower wrapper, and the remapping is implemented there

MishkaMN · 2024-08-16T19:15:19Z

carma/launch/guidance.launch.py

+ {'--log-level' : GetLogLevel('lateral_controller', env_log_levels) }
+ ],
+ remappings = [
+ ("output/lateral/control_cmd", "input/lateral/control_cmd")


@saina-ramyar yeah we don't need to change names in the autoware package itself. Just topic remapping for general naming here in the launch file would be good. Since we are already topic remapping from "output" to "input", which means every topic would end up being "input", why not drop it and rename it to something general.

"input/lateral/control_cmd" would be just sth like "lateral/control_cmd"

MishkaMN · 2024-08-16T19:21:27Z

carma/launch/guidance.launch.py

+ trajectory_follower_container = ComposableNodeContainer(
+ package='carma_ros2_utils',
+ name='trajectory_follower_container',
+ executable='carma_component_container_mt',
+ namespace=GetCurrentNamespace(),
+ composable_node_descriptions=[
+ ComposableNode(
+ package='trajectory_follower_nodes',
+ plugin='autoware::motion::control::trajectory_follower_nodes::LatLonMuxer',
+ name='latlon_muxer_node',
+ extra_arguments=[
+ {'use_intra_process_comms': False},
+ {'--log-level' : GetLogLevel('latlon_muxer', env_log_levels) }
+ ],
+ parameters=[
+ {'timeout_thr_sec':0.5}
+ ]
+ ),
+ ComposableNode(
+ package='trajectory_follower_nodes',
+ plugin='autoware::motion::control::trajectory_follower_nodes::LateralController',
+ name='lateral_controller_node',
+ extra_arguments=[
+ {'use_intra_process_comms': True},
+ {'--log-level' : GetLogLevel('lateral_controller', env_log_levels) }
+ ],
+ remappings = [
+ ("output/lateral/control_cmd", "input/lateral/control_cmd")
+ ],
+ parameters = [
+ [vehicle_calibration_dir, "/mpc_follower/lateral_controller_defaults.yaml"]
+ ]
+ ),
+ ComposableNode(
+ package='trajectory_follower_nodes',
+ plugin='autoware::motion::control::trajectory_follower_nodes::LongitudinalController',
+ name='longitudinal_controller_node',
+ extra_arguments=[
+ {'use_intra_process_comms': False},
+ {'--log-level' : GetLogLevel('longitudinal_controller', env_log_levels) }
+ ],
+ remappings = [
+ ("output/longitudinal/control_cmd", "input/longitudinal/control_cmd"),
+ ("input/current_trajectory", "input/reference_trajectory"),
+ ("input/current_state", "input/current_kinematic_state")
+ ],
+ parameters = [
+ [vehicle_calibration_dir, "/mpc_follower/longitudinal_controller_defaults.yaml"]
+ ]
+ )
+ ]
+ )
+


It just seems easier if all the related nodes were in one file. Not a blocker though.

sonarcloud · 2024-08-19T21:41:44Z

Quality Gate passed

Issues
4 New issues
0 Accepted issues

Measures
0 Security Hotspots
95.0% Coverage on New Code
0.0% Duplication on New Code

See analysis details on SonarCloud

MishkaMN · 2024-08-19T23:45:41Z

Looks good to me. Good stuff.
Remember to merge usdot-fhwa-stol/autoware.auto#26 first.

trajectory_follower_wrapper/src/trajectory_follower_wrapper_node.cpp

# PR Details ## Description This PR is part of the integration testing of the Trajectory Follower Wrapper on a physical vehicle. Related PR: usdot-fhwa-stol/carma-platform#2419 ## Related GitHub Issue      ## Related Jira Key https://usdot-carma.atlassian.net/browse/ARC-78 ## Motivation and Context Integrate the Trajectory Follower controller of Autoware, that has a MPC based lateral controller, to improve vehicle's performance. ## How Has This Been Tested? Integration tested on Blue Lexus    ## Types of changes  - [ ] Defect fix (non-breaking change that fixes an issue) - [x] New feature (non-breaking change that adds functionality) - [ ] Breaking change (fix or feature that cause existing functionality to change) ## Checklist:   - [ ] I have added any new packages to the sonar-scanner.properties file - [ ] My change requires a change to the documentation. - [ ] I have updated the documentation accordingly. - [ ] I have read the [**CONTRIBUTING**](https://github.com/usdot-fhwa-stol/carma-platform/blob/develop/Contributing.md) document. - [ ] I have added tests to cover my changes. - [x] All new and existing tests passed. --------- Co-authored-by: Misheel Bayartsengel <misheel.bayart@gmail.com>

saina-ramyar and others added 5 commits July 23, 2024 10:10

update launch file

6ffc3fa

add params temporarily

2822c71

integration test

3b464f8

clean up

e77809a

clean launch file

82ee5dc

saina-ramyar mentioned this pull request Jul 26, 2024

Arc 78 trajectory follower integration usdot-fhwa-stol/autoware.auto#26

Merged

9 tasks

MishkaMN and others added 7 commits August 7, 2024 16:48

fixes

ba41ff4

los

f5b9dbb

add frame

a02dd8d

add logs

beebbe2

cleanup

8b2be13

revert default controller

d3890b9

move yaw calc logic to basic autonomy

2bf26c1

saina-ramyar requested review from JonSmet, MishkaMN and adev4a August 13, 2024 20:34

saina-ramyar marked this pull request as ready for review August 13, 2024 20:34

MishkaMN requested changes Aug 15, 2024

View reviewed changes

address comments

ff1e5cd

MishkaMN requested changes Aug 16, 2024

View reviewed changes

MishkaMN self-requested a review August 16, 2024 19:24

rename topics and move trajectory follwer nodes to plugins launch file

c44f1d6

MishkaMN approved these changes Aug 19, 2024

View reviewed changes

adev4a reviewed Aug 20, 2024

View reviewed changes

trajectory_follower_wrapper/src/trajectory_follower_wrapper_node.cpp Show resolved Hide resolved

adev4a approved these changes Aug 20, 2024

View reviewed changes

saina-ramyar merged commit 05e2fc3 into develop Aug 20, 2024
4 checks passed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Arc 78 trajectory follower integration #2419

Arc 78 trajectory follower integration #2419

saina-ramyar commented Jul 26, 2024 •

edited

Loading

MishkaMN Aug 15, 2024

saina-ramyar Aug 16, 2024

MishkaMN Aug 16, 2024

MishkaMN Aug 15, 2024

saina-ramyar Aug 16, 2024 •

edited

Loading

adev4a Aug 16, 2024

MishkaMN Aug 16, 2024 •

edited

Loading

MishkaMN Aug 15, 2024

MishkaMN Aug 15, 2024

MishkaMN Aug 15, 2024

saina-ramyar Aug 16, 2024

MishkaMN Aug 16, 2024 •

edited

Loading

MishkaMN Aug 16, 2024

sonarcloud bot commented Aug 19, 2024

MishkaMN commented Aug 19, 2024 •

edited

Loading

Arc 78 trajectory follower integration #2419

Arc 78 trajectory follower integration #2419

Conversation

saina-ramyar commented Jul 26, 2024 • edited Loading

PR Details

Description

Related GitHub Issue

Related Jira Key

Motivation and Context

How Has This Been Tested?

Types of changes

Checklist:

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

saina-ramyar Aug 16, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

MishkaMN Aug 16, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

MishkaMN Aug 16, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

sonarcloud bot commented Aug 19, 2024

Quality Gate passed

MishkaMN commented Aug 19, 2024 • edited Loading

saina-ramyar commented Jul 26, 2024 •

edited

Loading

saina-ramyar Aug 16, 2024 •

edited

Loading

MishkaMN Aug 16, 2024 •

edited

Loading

MishkaMN Aug 16, 2024 •

edited

Loading

MishkaMN commented Aug 19, 2024 •

edited

Loading