From d2452e90e92e97a1c2d0b7b9bc174b4a6c6bd0e7 Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Thu, 11 Jul 2024 15:08:36 -0400 Subject: [PATCH] Port pinpoint to ros2 and update carma script (#228) # PR Details ## Description This PR supports migration of pinpoint driver to ROS2 in this PR: https://github.com/usdot-fhwa-stol/carma-torc-pinpoint-driver/pull/46 - It adds ROS2 version of pinpoint to the configs - It adds commands to include .env file in the image which started to be convention on this PR: https://github.com/usdot-fhwa-stol/carma-config/pull/352 ## Related GitHub Issue NA ## Related Jira Key https://usdot-carma.atlassian.net/browse/CAR-5907 ## Motivation and Context Migration of pinpoint to ROS2 ## How Has This Been Tested? Set the config and tried starting without issue. Just needs to test on actual Tahoe vehicle now. NOTE: image intentionally comments out the other components other than pinpoint ![image](https://github.com/usdot-fhwa-stol/carma-messenger/assets/20613282/8e656fa9-7638-48cf-966b-7c4c7618dfb0) ## 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: - [X] I have added any new packages to the sonar-scanner.properties file - [ ] My change requires a change to the documentation. - [X] I have updated the documentation accordingly. - [X] I have read the [**CONTRIBUTING**](https://github.com/usdot-fhwa-stol/carma-platform/blob/develop/Contributing.md) document. - [X] I have added tests to cover my changes. - [X] All new and existing tests passed. --- .../chevrolet_tahoe_2018/Dockerfile | 10 +- .../chevrolet_tahoe_2018/docker-compose.yml | 4 +- .../chevrolet_tahoe_2018/drivers.launch | 36 ---- .../chevrolet_tahoe_2018/drivers.launch.py | 163 ++++++++++++++---- carma-messenger-config/development/Dockerfile | 10 +- .../development/docker-compose.yml | 4 +- .../development/drivers.launch | 36 ---- .../development/drivers.launch.py | 163 ++++++++++++++---- 8 files changed, 276 insertions(+), 150 deletions(-) delete mode 100755 carma-messenger-config/chevrolet_tahoe_2018/drivers.launch delete mode 100755 carma-messenger-config/development/drivers.launch diff --git a/carma-messenger-config/chevrolet_tahoe_2018/Dockerfile b/carma-messenger-config/chevrolet_tahoe_2018/Dockerfile index c3baa8fad..5859310e4 100755 --- a/carma-messenger-config/chevrolet_tahoe_2018/Dockerfile +++ b/carma-messenger-config/chevrolet_tahoe_2018/Dockerfile @@ -1,11 +1,11 @@ -# Copyright (C) 2018-2021 LEIDOS. -# +# Copyright (C) 2018-2024 LEIDOS. +# # Licensed under the Apache License, Version 2.0 (the "License"); you may not # use this file except in compliance with the License. You may obtain a copy of # the License at -# +# # http://www.apache.org/licenses/LICENSE-2.0 -# +# # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, WITHOUT # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the @@ -35,5 +35,7 @@ LABEL org.label-schema.build-date=${BUILD_DATE} ADD . /root/vehicle/config VOLUME /opt/carma/vehicle/config +RUN mkdir -p /opt/carma/vehicle/config +RUN cp -a /root/vehicle/config/.env /opt/carma/vehicle/config/ CMD cp /root/vehicle/config/* /opt/carma/vehicle/config diff --git a/carma-messenger-config/chevrolet_tahoe_2018/docker-compose.yml b/carma-messenger-config/chevrolet_tahoe_2018/docker-compose.yml index 3ed05c265..c74fd5438 100755 --- a/carma-messenger-config/chevrolet_tahoe_2018/docker-compose.yml +++ b/carma-messenger-config/chevrolet_tahoe_2018/docker-compose.yml @@ -13,8 +13,6 @@ # the License. # Docker Compose Spec Version -version: '2' - services: roscore: image: ${DOCKER_ORG}/carma-base:${DOCKER_TAG} @@ -99,4 +97,4 @@ services: - /opt/carma/logs:/opt/carma/logs - /opt/carma/.ros:/opt/carma/.ros - /opt/carma/vehicle:/opt/carma/vehicle - command: bash -c 'wait-for-it.sh localhost:11311 -- roslaunch /opt/carma/vehicle/config/drivers.launch drivers:=pinpoint_driver' + command: bash -c 'source /opt/carma/install/setup.bash && ros2 launch /opt/carma/vehicle/config/drivers.launch.py drivers:=pinpoint_driver' diff --git a/carma-messenger-config/chevrolet_tahoe_2018/drivers.launch b/carma-messenger-config/chevrolet_tahoe_2018/drivers.launch deleted file mode 100755 index 6098c946d..000000000 --- a/carma-messenger-config/chevrolet_tahoe_2018/drivers.launch +++ /dev/null @@ -1,36 +0,0 @@ - - - - - - - - - - - - - - - - - diff --git a/carma-messenger-config/chevrolet_tahoe_2018/drivers.launch.py b/carma-messenger-config/chevrolet_tahoe_2018/drivers.launch.py index 74e594f2e..abd8dac7a 100644 --- a/carma-messenger-config/chevrolet_tahoe_2018/drivers.launch.py +++ b/carma-messenger-config/chevrolet_tahoe_2018/drivers.launch.py @@ -29,66 +29,165 @@ import launch.events import launch_ros.actions -import launch_ros.events +import launch_ros.events import launch_ros.events.lifecycle import lifecycle_msgs.msg + def generate_launch_description(): """ Launch desired CARMA Messenger drivers """ - env_log_levels = EnvironmentVariable('CARMA_ROS_LOGGING_CONFIG', default_value='{ "default_level" : "WARN" }') + env_log_levels = EnvironmentVariable( + "CARMA_ROS_LOGGING_CONFIG", default_value='{ "default_level" : "WARN" }' + ) - configuration_delay = LaunchConfiguration('configuration_delay') + configuration_delay = LaunchConfiguration("configuration_delay") declare_configuration_delay_arg = DeclareLaunchArgument( - name ='configuration_delay', default_value='4.0') + name="configuration_delay", default_value="4.0" + ) - drivers = LaunchConfiguration('drivers') + drivers = LaunchConfiguration("drivers") declare_drivers_arg = DeclareLaunchArgument( - name = 'drivers', default_value = 'dsrc_driver', description = "Desired drivers to launch specified by package name." + name="drivers", + default_value="dsrc_driver", + description="Desired drivers to launch specified by package name.", ) dsrc_group = GroupAction( - condition=IfCondition(PythonExpression(["'dsrc_driver' in '", drivers, "'.split()"])), + condition=IfCondition( + PythonExpression(["'dsrc_driver' in '", drivers, "'.split()"]) + ), actions=[ - PushRosNamespace(EnvironmentVariable('CARMA_INTR_NS', default_value='hardware_interface')), + PushRosNamespace( + EnvironmentVariable("CARMA_INTR_NS", default_value="hardware_interface") + ), IncludeLaunchDescription( - PythonLaunchDescriptionSource([ FindPackageShare('dsrc_driver'), '/launch/dsrc_driver.py']), - launch_arguments = { - 'log_level' : GetLogLevel('dsrc_driver', env_log_levels), - }.items() + PythonLaunchDescriptionSource( + [FindPackageShare("dsrc_driver"), "/launch/dsrc_driver.py"] + ), + launch_arguments={ + "log_level": GetLogLevel("dsrc_driver", env_log_levels), + }.items(), ), - ] + ], + ) + + pinpoint_group = GroupAction( + condition=IfCondition( + PythonExpression(["'pinpoint_driver' in '", drivers, "'.split()"]) + ), + actions=[ + PushRosNamespace( + EnvironmentVariable("CARMA_INTR_NS", default_value="hardware_interface") + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [FindPackageShare("pinpoint"), "/launch/pinpoint.launch.py"] + ), + launch_arguments={ + "log_level": GetLogLevel("pinpoint", env_log_levels), + }.items(), + ), + ], ) - ros2_cmd = launch.substitutions.FindExecutable(name='ros2') + ros2_cmd = launch.substitutions.FindExecutable(name="ros2") process_configure_dsrc_driver_node = launch.actions.ExecuteProcess( - cmd=[ros2_cmd, "lifecycle", "set", [ EnvironmentVariable('CARMA_INTR_NS', default_value='hardware_interface'), "/dsrc_driver_node" ], "configure"], + condition=IfCondition( + PythonExpression(["'dsrc_driver' in '", drivers, "'.split()"]) + ), + cmd=[ + ros2_cmd, + "lifecycle", + "set", + [ + EnvironmentVariable( + "CARMA_INTR_NS", default_value="hardware_interface" + ), + "/dsrc_driver_node", + ], + "configure", + ], ) - configuration_trigger = launch.actions.TimerAction( - period=configuration_delay, - actions=[ - process_configure_dsrc_driver_node - ] + configured_event_handler_dsrc_driver_node = launch.actions.RegisterEventHandler( + launch.event_handlers.OnExecutionComplete( + target_action=process_configure_dsrc_driver_node, + on_completion=[ + launch.actions.ExecuteProcess( + cmd=[ + ros2_cmd, + "lifecycle", + "set", + [ + EnvironmentVariable( + "CARMA_INTR_NS", default_value="hardware_interface" + ), + "/dsrc_driver_node", + ], + "activate", + ], + ) + ], + ) ) - configured_event_handler_dsrc_driver_node = launch.actions.RegisterEventHandler(launch.event_handlers.OnExecutionComplete( - target_action=process_configure_dsrc_driver_node, - on_completion=[ + process_configure_pinpoint_node = launch.actions.ExecuteProcess( + condition=IfCondition( + PythonExpression(["'pinpoint_driver' in '", drivers, "'.split()"]) + ), + cmd=[ + ros2_cmd, + "lifecycle", + "set", + [ + EnvironmentVariable( + "CARMA_INTR_NS", default_value="hardware_interface" + ), + "/pinpoint", + ], + "configure", + ], + ) + + configured_event_handler_pinpoint_node = launch.actions.RegisterEventHandler( + launch.event_handlers.OnExecutionComplete( + target_action=process_configure_pinpoint_node, + on_completion=[ launch.actions.ExecuteProcess( - cmd=[ros2_cmd, "lifecycle", "set", [ EnvironmentVariable('CARMA_INTR_NS', default_value='hardware_interface'), "/dsrc_driver_node" ], "activate"], + cmd=[ + ros2_cmd, + "lifecycle", + "set", + [ + EnvironmentVariable( + "CARMA_INTR_NS", default_value="hardware_interface" + ), + "/pinpoint", + ], + "activate", + ], ) - ] + ], ) ) - return LaunchDescription([ - declare_configuration_delay_arg, - declare_drivers_arg, - dsrc_group, - configuration_trigger, - configured_event_handler_dsrc_driver_node - ]) + configuration_trigger = launch.actions.TimerAction( + period=configuration_delay, + actions=[process_configure_dsrc_driver_node, process_configure_pinpoint_node], + ) + + return LaunchDescription( + [ + declare_configuration_delay_arg, + declare_drivers_arg, + dsrc_group, + pinpoint_group, + configuration_trigger, + configured_event_handler_dsrc_driver_node, + configured_event_handler_pinpoint_node, + ] + ) diff --git a/carma-messenger-config/development/Dockerfile b/carma-messenger-config/development/Dockerfile index c3baa8fad..5859310e4 100755 --- a/carma-messenger-config/development/Dockerfile +++ b/carma-messenger-config/development/Dockerfile @@ -1,11 +1,11 @@ -# Copyright (C) 2018-2021 LEIDOS. -# +# Copyright (C) 2018-2024 LEIDOS. +# # Licensed under the Apache License, Version 2.0 (the "License"); you may not # use this file except in compliance with the License. You may obtain a copy of # the License at -# +# # http://www.apache.org/licenses/LICENSE-2.0 -# +# # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, WITHOUT # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the @@ -35,5 +35,7 @@ LABEL org.label-schema.build-date=${BUILD_DATE} ADD . /root/vehicle/config VOLUME /opt/carma/vehicle/config +RUN mkdir -p /opt/carma/vehicle/config +RUN cp -a /root/vehicle/config/.env /opt/carma/vehicle/config/ CMD cp /root/vehicle/config/* /opt/carma/vehicle/config diff --git a/carma-messenger-config/development/docker-compose.yml b/carma-messenger-config/development/docker-compose.yml index 14441419d..8320238f6 100755 --- a/carma-messenger-config/development/docker-compose.yml +++ b/carma-messenger-config/development/docker-compose.yml @@ -13,8 +13,6 @@ # the License. # Docker Compose Spec Version -version: '2' - services: roscore: image: ${DOCKER_ORG}/carma-base:${DOCKER_TAG} @@ -86,4 +84,4 @@ services: volumes: - /opt/carma/logs:/opt/carma/logs - /opt/carma/.ros:/opt/carma/.ros - command: bash -c 'wait-for-it.sh localhost:11311 -- roslaunch /opt/carma/vehicle/config/drivers.launch drivers:=pinpoint_driver' + command: bash -c 'source /opt/carma/install/setup.bash && ros2 launch /opt/carma/vehicle/config/drivers.launch.py drivers:=pinpoint_driver' diff --git a/carma-messenger-config/development/drivers.launch b/carma-messenger-config/development/drivers.launch deleted file mode 100755 index 6098c946d..000000000 --- a/carma-messenger-config/development/drivers.launch +++ /dev/null @@ -1,36 +0,0 @@ - - - - - - - - - - - - - - - - - diff --git a/carma-messenger-config/development/drivers.launch.py b/carma-messenger-config/development/drivers.launch.py index 74e594f2e..abd8dac7a 100644 --- a/carma-messenger-config/development/drivers.launch.py +++ b/carma-messenger-config/development/drivers.launch.py @@ -29,66 +29,165 @@ import launch.events import launch_ros.actions -import launch_ros.events +import launch_ros.events import launch_ros.events.lifecycle import lifecycle_msgs.msg + def generate_launch_description(): """ Launch desired CARMA Messenger drivers """ - env_log_levels = EnvironmentVariable('CARMA_ROS_LOGGING_CONFIG', default_value='{ "default_level" : "WARN" }') + env_log_levels = EnvironmentVariable( + "CARMA_ROS_LOGGING_CONFIG", default_value='{ "default_level" : "WARN" }' + ) - configuration_delay = LaunchConfiguration('configuration_delay') + configuration_delay = LaunchConfiguration("configuration_delay") declare_configuration_delay_arg = DeclareLaunchArgument( - name ='configuration_delay', default_value='4.0') + name="configuration_delay", default_value="4.0" + ) - drivers = LaunchConfiguration('drivers') + drivers = LaunchConfiguration("drivers") declare_drivers_arg = DeclareLaunchArgument( - name = 'drivers', default_value = 'dsrc_driver', description = "Desired drivers to launch specified by package name." + name="drivers", + default_value="dsrc_driver", + description="Desired drivers to launch specified by package name.", ) dsrc_group = GroupAction( - condition=IfCondition(PythonExpression(["'dsrc_driver' in '", drivers, "'.split()"])), + condition=IfCondition( + PythonExpression(["'dsrc_driver' in '", drivers, "'.split()"]) + ), actions=[ - PushRosNamespace(EnvironmentVariable('CARMA_INTR_NS', default_value='hardware_interface')), + PushRosNamespace( + EnvironmentVariable("CARMA_INTR_NS", default_value="hardware_interface") + ), IncludeLaunchDescription( - PythonLaunchDescriptionSource([ FindPackageShare('dsrc_driver'), '/launch/dsrc_driver.py']), - launch_arguments = { - 'log_level' : GetLogLevel('dsrc_driver', env_log_levels), - }.items() + PythonLaunchDescriptionSource( + [FindPackageShare("dsrc_driver"), "/launch/dsrc_driver.py"] + ), + launch_arguments={ + "log_level": GetLogLevel("dsrc_driver", env_log_levels), + }.items(), ), - ] + ], + ) + + pinpoint_group = GroupAction( + condition=IfCondition( + PythonExpression(["'pinpoint_driver' in '", drivers, "'.split()"]) + ), + actions=[ + PushRosNamespace( + EnvironmentVariable("CARMA_INTR_NS", default_value="hardware_interface") + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [FindPackageShare("pinpoint"), "/launch/pinpoint.launch.py"] + ), + launch_arguments={ + "log_level": GetLogLevel("pinpoint", env_log_levels), + }.items(), + ), + ], ) - ros2_cmd = launch.substitutions.FindExecutable(name='ros2') + ros2_cmd = launch.substitutions.FindExecutable(name="ros2") process_configure_dsrc_driver_node = launch.actions.ExecuteProcess( - cmd=[ros2_cmd, "lifecycle", "set", [ EnvironmentVariable('CARMA_INTR_NS', default_value='hardware_interface'), "/dsrc_driver_node" ], "configure"], + condition=IfCondition( + PythonExpression(["'dsrc_driver' in '", drivers, "'.split()"]) + ), + cmd=[ + ros2_cmd, + "lifecycle", + "set", + [ + EnvironmentVariable( + "CARMA_INTR_NS", default_value="hardware_interface" + ), + "/dsrc_driver_node", + ], + "configure", + ], ) - configuration_trigger = launch.actions.TimerAction( - period=configuration_delay, - actions=[ - process_configure_dsrc_driver_node - ] + configured_event_handler_dsrc_driver_node = launch.actions.RegisterEventHandler( + launch.event_handlers.OnExecutionComplete( + target_action=process_configure_dsrc_driver_node, + on_completion=[ + launch.actions.ExecuteProcess( + cmd=[ + ros2_cmd, + "lifecycle", + "set", + [ + EnvironmentVariable( + "CARMA_INTR_NS", default_value="hardware_interface" + ), + "/dsrc_driver_node", + ], + "activate", + ], + ) + ], + ) ) - configured_event_handler_dsrc_driver_node = launch.actions.RegisterEventHandler(launch.event_handlers.OnExecutionComplete( - target_action=process_configure_dsrc_driver_node, - on_completion=[ + process_configure_pinpoint_node = launch.actions.ExecuteProcess( + condition=IfCondition( + PythonExpression(["'pinpoint_driver' in '", drivers, "'.split()"]) + ), + cmd=[ + ros2_cmd, + "lifecycle", + "set", + [ + EnvironmentVariable( + "CARMA_INTR_NS", default_value="hardware_interface" + ), + "/pinpoint", + ], + "configure", + ], + ) + + configured_event_handler_pinpoint_node = launch.actions.RegisterEventHandler( + launch.event_handlers.OnExecutionComplete( + target_action=process_configure_pinpoint_node, + on_completion=[ launch.actions.ExecuteProcess( - cmd=[ros2_cmd, "lifecycle", "set", [ EnvironmentVariable('CARMA_INTR_NS', default_value='hardware_interface'), "/dsrc_driver_node" ], "activate"], + cmd=[ + ros2_cmd, + "lifecycle", + "set", + [ + EnvironmentVariable( + "CARMA_INTR_NS", default_value="hardware_interface" + ), + "/pinpoint", + ], + "activate", + ], ) - ] + ], ) ) - return LaunchDescription([ - declare_configuration_delay_arg, - declare_drivers_arg, - dsrc_group, - configuration_trigger, - configured_event_handler_dsrc_driver_node - ]) + configuration_trigger = launch.actions.TimerAction( + period=configuration_delay, + actions=[process_configure_dsrc_driver_node, process_configure_pinpoint_node], + ) + + return LaunchDescription( + [ + declare_configuration_delay_arg, + declare_drivers_arg, + dsrc_group, + pinpoint_group, + configuration_trigger, + configured_event_handler_dsrc_driver_node, + configured_event_handler_pinpoint_node, + ] + )