Skip to content

Commit

Permalink
Add selected external command API (autowarefoundation#397)
Browse files Browse the repository at this point in the history
  • Loading branch information
isamu-takagi authored Sep 10, 2021
1 parent b3b834f commit dcd9a57
Show file tree
Hide file tree
Showing 3 changed files with 10 additions and 54 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@
<param name="type" value="autoware_api_msgs/msg/CrosswalkStatus"/>
</node>
<!-- TODO(Takagi, Isamu): workaround for topic check -->
<node pkg="topic_tools" exec="relay" name="initialpose">
<node pkg="topic_tools" exec="relay" name="initial_pose_2d">
<param name="input_topic" value="/initialpose"/>
<param name="output_topic" value="/initialpose2d"/>
<param name="type" value="geometry_msgs/msg/PoseWithCovarianceStamped"/>
Expand Down
59 changes: 9 additions & 50 deletions control_launch/launch/control.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -232,33 +232,14 @@ def launch_setup(context, *args, **kwargs):
)

# external cmd converter
external_cmd_converter_component = ComposableNode(
package='external_cmd_converter',
plugin='external_cmd_converter::ExternalCmdConverterNode',
name='external_cmd_converter',
remappings=[
('in/external_control_cmd', '/external/selected/external_control_cmd'),
('in/shift_cmd', '/external/selected/shift_cmd'),
('in/emergency_stop', '/remote/emergency_stop'),
('in/current_gate_mode', '/control/current_gate_mode'),
('in/twist', '/localization/twist'),
('out/control_cmd', '/external/selected/control_cmd'),
('out/latest_external_control_cmd', '/external/selected/latest_external_control_cmd'),
],
parameters=[
{
'csv_path_accel_map': LaunchConfiguration('csv_path_accel_map'),
'csv_path_brake_map': LaunchConfiguration('csv_path_brake_map'),

'ref_vel_gain': LaunchConfiguration('ref_vel_gain'),
'wait_for_first_topic': LaunchConfiguration('wait_for_first_topic'),
'control_command_timeout': LaunchConfiguration('control_command_timeout'),
'emergency_stop_timeout': LaunchConfiguration('emergency_stop_timeout'),
}
external_cmd_converter_loader = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
FindPackageShare('external_cmd_converter'), '/launch/external_cmd_converter.launch.py'
]),
launch_arguments=[
('use_intra_process', LaunchConfiguration('use_intra_process')),
('target_container', '/control/control_container'),
],
extra_arguments=[{
'use_intra_process_comms': LaunchConfiguration('use_intra_process')
}],
)

# set container to run all required components in the same process
Expand All @@ -273,7 +254,6 @@ def launch_setup(context, *args, **kwargs):
lane_departure_component,
shift_decider_component,
vehicle_cmd_gate_component,
external_cmd_converter_component,
],
)

Expand All @@ -297,6 +277,7 @@ def launch_setup(context, *args, **kwargs):
PushRosNamespace('control'),
container,
external_cmd_selector_loader,
external_cmd_converter_loader,
mpc_follower_loader,
pure_pursuit_loader
])
Expand Down Expand Up @@ -367,29 +348,7 @@ def add_launch_arg(name: str, default_value=None, description=None):
# external cmd selector
add_launch_arg('initial_selector_mode', 'remote', 'local or remote')

# remote cmd converter
add_launch_arg(
'csv_path_accel_map',
[
FindPackageShare('raw_vehicle_cmd_converter'),
'/data/default/accel_map.csv'
],
'csv file path for accel map'
)
add_launch_arg(
'csv_path_brake_map',
[
FindPackageShare('raw_vehicle_cmd_converter'),
'/data/default/brake_map.csv'
],
'csv file path for brake map'
)
add_launch_arg('ref_vel_gain', '3.0', 'gain for remote command accel')
add_launch_arg('wait_for_first_topic', 'true',
'disable topic disruption detection until subscribing first topics')
add_launch_arg('control_command_timeout', '1.0', 'remote control command timeout')
add_launch_arg('emergency_stop_timeout', '3.0', 'emergency stop timeout for remote heartbeat')

# component
add_launch_arg('use_intra_process', 'false', 'use ROS2 component container communication')
add_launch_arg('use_multithread', 'false', 'use multithread')
set_container_executable = SetLaunchConfiguration(
Expand Down
3 changes: 0 additions & 3 deletions control_launch/launch/control.launch.xml
Original file line number Diff line number Diff line change
Expand Up @@ -65,9 +65,6 @@

<!-- external_cmd_converter -->
<include file="$(find-pkg-share external_cmd_converter)/launch/external_cmd_converter.launch.xml">
<arg name="in/external_control_cmd" value="/external/selected/external_control_cmd" />
<arg name="in/shift_cmd" value="/external/selected/shift_cmd" />
<arg name="out/control_cmd" value="/external/selected/control_cmd" />
</include>
</group>
</launch>

0 comments on commit dcd9a57

Please sign in to comment.