别人克隆后没有 install，source 会直接失败。把展开后的 install 放进仓库，符号链接和指向本机 build 的 egg-link 收成真实文件，Python 入口改走当前环境的解释器，这样不用先 colcon build 也能启动。
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
@@ -0,0 +1,14 @@
|
||||
# generated from ament/cmake/core/templates/nameConfig-version.cmake.in
|
||||
set(PACKAGE_VERSION "1.9.1")
|
||||
|
||||
set(PACKAGE_VERSION_EXACT False)
|
||||
set(PACKAGE_VERSION_COMPATIBLE False)
|
||||
|
||||
if("${PACKAGE_FIND_VERSION}" VERSION_EQUAL "${PACKAGE_VERSION}")
|
||||
set(PACKAGE_VERSION_EXACT True)
|
||||
set(PACKAGE_VERSION_COMPATIBLE True)
|
||||
endif()
|
||||
|
||||
if("${PACKAGE_FIND_VERSION}" VERSION_LESS "${PACKAGE_VERSION}")
|
||||
set(PACKAGE_VERSION_COMPATIBLE True)
|
||||
endif()
|
||||
@@ -0,0 +1,42 @@
|
||||
# generated from ament/cmake/core/templates/nameConfig.cmake.in
|
||||
|
||||
# prevent multiple inclusion
|
||||
if(_flexiv_bringup_CONFIG_INCLUDED)
|
||||
# ensure to keep the found flag the same
|
||||
if(NOT DEFINED flexiv_bringup_FOUND)
|
||||
# explicitly set it to FALSE, otherwise CMake will set it to TRUE
|
||||
set(flexiv_bringup_FOUND FALSE)
|
||||
elseif(NOT flexiv_bringup_FOUND)
|
||||
# use separate condition to avoid uninitialized variable warning
|
||||
set(flexiv_bringup_FOUND FALSE)
|
||||
endif()
|
||||
return()
|
||||
endif()
|
||||
set(_flexiv_bringup_CONFIG_INCLUDED TRUE)
|
||||
|
||||
# output package information
|
||||
if(NOT flexiv_bringup_FIND_QUIETLY)
|
||||
message(STATUS "Found flexiv_bringup: 1.9.1 (${flexiv_bringup_DIR})")
|
||||
endif()
|
||||
|
||||
# warn when using a deprecated package
|
||||
if(NOT "" STREQUAL "")
|
||||
set(_msg "Package 'flexiv_bringup' is deprecated")
|
||||
# append custom deprecation text if available
|
||||
if(NOT "" STREQUAL "TRUE")
|
||||
set(_msg "${_msg} ()")
|
||||
endif()
|
||||
# optionally quiet the deprecation message
|
||||
if(NOT ${flexiv_bringup_DEPRECATED_QUIET})
|
||||
message(DEPRECATION "${_msg}")
|
||||
endif()
|
||||
endif()
|
||||
|
||||
# flag package as ament-based to distinguish it after being find_package()-ed
|
||||
set(flexiv_bringup_FOUND_AMENT_PACKAGE TRUE)
|
||||
|
||||
# include all config extra files
|
||||
set(_extras "")
|
||||
foreach(_extra ${_extras})
|
||||
include("${flexiv_bringup_DIR}/${_extra}")
|
||||
endforeach()
|
||||
@@ -0,0 +1,38 @@
|
||||
controller_manager:
|
||||
ros__parameters:
|
||||
update_rate: 100 # Hz
|
||||
|
||||
rizon_arm_controller:
|
||||
type: joint_trajectory_controller/JointTrajectoryController
|
||||
|
||||
joint_state_broadcaster:
|
||||
type: joint_state_broadcaster/JointStateBroadcaster
|
||||
|
||||
gpio_controller:
|
||||
type: gpio_controller/GPIOController
|
||||
|
||||
flexiv_robot_states_broadcaster:
|
||||
type: flexiv_robot_states_broadcaster/FlexivRobotStatesBroadcaster
|
||||
|
||||
rizon_arm_controller:
|
||||
ros__parameters:
|
||||
joints:
|
||||
- $(var external_axis_prefix)aico1_4_v1_joint2
|
||||
- $(var robot_sn)_joint1
|
||||
- $(var robot_sn)_joint2
|
||||
- $(var robot_sn)_joint3
|
||||
- $(var robot_sn)_joint4
|
||||
- $(var robot_sn)_joint5
|
||||
- $(var robot_sn)_joint6
|
||||
- $(var robot_sn)_joint7
|
||||
command_interfaces:
|
||||
- position
|
||||
state_interfaces:
|
||||
- position
|
||||
- velocity
|
||||
state_publish_rate: 100.0
|
||||
action_monitor_rate: 20.0
|
||||
allow_partial_joints_goal: true
|
||||
constraints:
|
||||
stopped_velocity_tolerance: 0.01
|
||||
goal_time: 0.0
|
||||
@@ -0,0 +1,40 @@
|
||||
controller_manager:
|
||||
ros__parameters:
|
||||
update_rate: 1000 # Hz
|
||||
|
||||
rizon_arm_controller:
|
||||
type: joint_trajectory_controller/JointTrajectoryController
|
||||
|
||||
joint_state_broadcaster:
|
||||
type: joint_state_broadcaster/JointStateBroadcaster
|
||||
|
||||
gpio_controller:
|
||||
type: gpio_controller/GPIOController
|
||||
|
||||
flexiv_robot_states_broadcaster:
|
||||
type: flexiv_robot_states_broadcaster/FlexivRobotStatesBroadcaster
|
||||
|
||||
rizon_arm_controller:
|
||||
ros__parameters:
|
||||
joints:
|
||||
- $(var external_axis_prefix)aico1_4_v2_joint1
|
||||
- $(var external_axis_prefix)aico1_4_v2_joint2
|
||||
- $(var external_axis_prefix)aico1_4_v2_joint_head
|
||||
- $(var robot_sn)_joint1
|
||||
- $(var robot_sn)_joint2
|
||||
- $(var robot_sn)_joint3
|
||||
- $(var robot_sn)_joint4
|
||||
- $(var robot_sn)_joint5
|
||||
- $(var robot_sn)_joint6
|
||||
- $(var robot_sn)_joint7
|
||||
command_interfaces:
|
||||
- position
|
||||
state_interfaces:
|
||||
- position
|
||||
- velocity
|
||||
state_publish_rate: 100.0
|
||||
action_monitor_rate: 20.0
|
||||
allow_partial_joints_goal: true
|
||||
constraints:
|
||||
stopped_velocity_tolerance: 0.01
|
||||
goal_time: 0.0
|
||||
@@ -0,0 +1,86 @@
|
||||
controller_manager:
|
||||
ros__parameters:
|
||||
update_rate: 1000 # Hz
|
||||
|
||||
left_rizon_arm_controller:
|
||||
type: joint_trajectory_controller/JointTrajectoryController
|
||||
|
||||
right_rizon_arm_controller:
|
||||
type: joint_trajectory_controller/JointTrajectoryController
|
||||
|
||||
joint_state_broadcaster:
|
||||
type: joint_state_broadcaster/JointStateBroadcaster
|
||||
|
||||
gpio_controller_left:
|
||||
type: gpio_controller/GPIOController
|
||||
|
||||
gpio_controller_right:
|
||||
type: gpio_controller/GPIOController
|
||||
|
||||
flexiv_robot_states_broadcaster_left:
|
||||
type: flexiv_robot_states_broadcaster/FlexivRobotStatesBroadcaster
|
||||
|
||||
flexiv_robot_states_broadcaster_right:
|
||||
type: flexiv_robot_states_broadcaster/FlexivRobotStatesBroadcaster
|
||||
|
||||
left_rizon_arm_controller:
|
||||
ros__parameters:
|
||||
joints:
|
||||
- $(var external_axis_prefix)aico2_10_v1_joint1
|
||||
- $(var external_axis_prefix)aico2_10_v1_joint2
|
||||
- $(var prefix_left)joint1
|
||||
- $(var prefix_left)joint2
|
||||
- $(var prefix_left)joint3
|
||||
- $(var prefix_left)joint4
|
||||
- $(var prefix_left)joint5
|
||||
- $(var prefix_left)joint6
|
||||
- $(var prefix_left)joint7
|
||||
command_interfaces:
|
||||
- position
|
||||
state_interfaces:
|
||||
- position
|
||||
- velocity
|
||||
state_publish_rate: 100.0
|
||||
action_monitor_rate: 20.0
|
||||
allow_partial_joints_goal: true
|
||||
constraints:
|
||||
stopped_velocity_tolerance: 0.01
|
||||
goal_time: 0.0
|
||||
|
||||
right_rizon_arm_controller:
|
||||
ros__parameters:
|
||||
joints:
|
||||
- $(var prefix_right)joint1
|
||||
- $(var prefix_right)joint2
|
||||
- $(var prefix_right)joint3
|
||||
- $(var prefix_right)joint4
|
||||
- $(var prefix_right)joint5
|
||||
- $(var prefix_right)joint6
|
||||
- $(var prefix_right)joint7
|
||||
command_interfaces:
|
||||
- position
|
||||
state_interfaces:
|
||||
- position
|
||||
- velocity
|
||||
state_publish_rate: 100.0
|
||||
action_monitor_rate: 20.0
|
||||
allow_partial_joints_goal: false
|
||||
constraints:
|
||||
stopped_velocity_tolerance: 0.01
|
||||
goal_time: 0.0
|
||||
|
||||
flexiv_robot_states_broadcaster_left:
|
||||
ros__parameters:
|
||||
robot_sn: left_$(var robot_sn_left)
|
||||
|
||||
flexiv_robot_states_broadcaster_right:
|
||||
ros__parameters:
|
||||
robot_sn: right_$(var robot_sn_right)
|
||||
|
||||
gpio_controller_left:
|
||||
ros__parameters:
|
||||
robot_sn: left_$(var robot_sn_left)
|
||||
|
||||
gpio_controller_right:
|
||||
ros__parameters:
|
||||
robot_sn: right_$(var robot_sn_right)
|
||||
@@ -0,0 +1,86 @@
|
||||
controller_manager:
|
||||
ros__parameters:
|
||||
update_rate: 1000 # Hz
|
||||
|
||||
left_rizon_arm_controller:
|
||||
type: joint_trajectory_controller/JointTrajectoryController
|
||||
|
||||
right_rizon_arm_controller:
|
||||
type: joint_trajectory_controller/JointTrajectoryController
|
||||
|
||||
joint_state_broadcaster:
|
||||
type: joint_state_broadcaster/JointStateBroadcaster
|
||||
|
||||
gpio_controller_left:
|
||||
type: gpio_controller/GPIOController
|
||||
|
||||
gpio_controller_right:
|
||||
type: gpio_controller/GPIOController
|
||||
|
||||
flexiv_robot_states_broadcaster_left:
|
||||
type: flexiv_robot_states_broadcaster/FlexivRobotStatesBroadcaster
|
||||
|
||||
flexiv_robot_states_broadcaster_right:
|
||||
type: flexiv_robot_states_broadcaster/FlexivRobotStatesBroadcaster
|
||||
|
||||
left_rizon_arm_controller:
|
||||
ros__parameters:
|
||||
joints:
|
||||
- $(var external_axis_prefix)aico2_4_v1_joint1
|
||||
- $(var external_axis_prefix)aico2_4_v1_joint2
|
||||
- $(var prefix_left)joint1
|
||||
- $(var prefix_left)joint2
|
||||
- $(var prefix_left)joint3
|
||||
- $(var prefix_left)joint4
|
||||
- $(var prefix_left)joint5
|
||||
- $(var prefix_left)joint6
|
||||
- $(var prefix_left)joint7
|
||||
command_interfaces:
|
||||
- position
|
||||
state_interfaces:
|
||||
- position
|
||||
- velocity
|
||||
state_publish_rate: 100.0
|
||||
action_monitor_rate: 20.0
|
||||
allow_partial_joints_goal: true
|
||||
constraints:
|
||||
stopped_velocity_tolerance: 0.01
|
||||
goal_time: 0.0
|
||||
|
||||
right_rizon_arm_controller:
|
||||
ros__parameters:
|
||||
joints:
|
||||
- $(var prefix_right)joint1
|
||||
- $(var prefix_right)joint2
|
||||
- $(var prefix_right)joint3
|
||||
- $(var prefix_right)joint4
|
||||
- $(var prefix_right)joint5
|
||||
- $(var prefix_right)joint6
|
||||
- $(var prefix_right)joint7
|
||||
command_interfaces:
|
||||
- position
|
||||
state_interfaces:
|
||||
- position
|
||||
- velocity
|
||||
state_publish_rate: 100.0
|
||||
action_monitor_rate: 20.0
|
||||
allow_partial_joints_goal: false
|
||||
constraints:
|
||||
stopped_velocity_tolerance: 0.01
|
||||
goal_time: 0.0
|
||||
|
||||
flexiv_robot_states_broadcaster_left:
|
||||
ros__parameters:
|
||||
robot_sn: left_$(var robot_sn_left)
|
||||
|
||||
flexiv_robot_states_broadcaster_right:
|
||||
ros__parameters:
|
||||
robot_sn: right_$(var robot_sn_right)
|
||||
|
||||
gpio_controller_left:
|
||||
ros__parameters:
|
||||
robot_sn: left_$(var robot_sn_left)
|
||||
|
||||
gpio_controller_right:
|
||||
ros__parameters:
|
||||
robot_sn: right_$(var robot_sn_right)
|
||||
@@ -0,0 +1,86 @@
|
||||
controller_manager:
|
||||
ros__parameters:
|
||||
update_rate: 1000 # Hz
|
||||
|
||||
left_rizon_arm_controller:
|
||||
type: joint_trajectory_controller/JointTrajectoryController
|
||||
|
||||
right_rizon_arm_controller:
|
||||
type: joint_trajectory_controller/JointTrajectoryController
|
||||
|
||||
joint_state_broadcaster:
|
||||
type: joint_state_broadcaster/JointStateBroadcaster
|
||||
|
||||
gpio_controller_left:
|
||||
type: gpio_controller/GPIOController
|
||||
|
||||
gpio_controller_right:
|
||||
type: gpio_controller/GPIOController
|
||||
|
||||
flexiv_robot_states_broadcaster_left:
|
||||
type: flexiv_robot_states_broadcaster/FlexivRobotStatesBroadcaster
|
||||
|
||||
flexiv_robot_states_broadcaster_right:
|
||||
type: flexiv_robot_states_broadcaster/FlexivRobotStatesBroadcaster
|
||||
|
||||
left_rizon_arm_controller:
|
||||
ros__parameters:
|
||||
joints:
|
||||
- $(var external_axis_prefix)aico2_4_v2_joint1
|
||||
- $(var external_axis_prefix)aico2_4_v2_joint2
|
||||
- $(var prefix_left)joint1
|
||||
- $(var prefix_left)joint2
|
||||
- $(var prefix_left)joint3
|
||||
- $(var prefix_left)joint4
|
||||
- $(var prefix_left)joint5
|
||||
- $(var prefix_left)joint6
|
||||
- $(var prefix_left)joint7
|
||||
command_interfaces:
|
||||
- position
|
||||
state_interfaces:
|
||||
- position
|
||||
- velocity
|
||||
state_publish_rate: 100.0
|
||||
action_monitor_rate: 20.0
|
||||
allow_partial_joints_goal: true
|
||||
constraints:
|
||||
stopped_velocity_tolerance: 0.01
|
||||
goal_time: 0.0
|
||||
|
||||
right_rizon_arm_controller:
|
||||
ros__parameters:
|
||||
joints:
|
||||
- $(var prefix_right)joint1
|
||||
- $(var prefix_right)joint2
|
||||
- $(var prefix_right)joint3
|
||||
- $(var prefix_right)joint4
|
||||
- $(var prefix_right)joint5
|
||||
- $(var prefix_right)joint6
|
||||
- $(var prefix_right)joint7
|
||||
command_interfaces:
|
||||
- position
|
||||
state_interfaces:
|
||||
- position
|
||||
- velocity
|
||||
state_publish_rate: 100.0
|
||||
action_monitor_rate: 20.0
|
||||
allow_partial_joints_goal: false
|
||||
constraints:
|
||||
stopped_velocity_tolerance: 0.01
|
||||
goal_time: 0.0
|
||||
|
||||
flexiv_robot_states_broadcaster_left:
|
||||
ros__parameters:
|
||||
robot_sn: left_$(var robot_sn_left)
|
||||
|
||||
flexiv_robot_states_broadcaster_right:
|
||||
ros__parameters:
|
||||
robot_sn: right_$(var robot_sn_right)
|
||||
|
||||
gpio_controller_left:
|
||||
ros__parameters:
|
||||
robot_sn: left_$(var robot_sn_left)
|
||||
|
||||
gpio_controller_right:
|
||||
ros__parameters:
|
||||
robot_sn: right_$(var robot_sn_right)
|
||||
+22
@@ -0,0 +1,22 @@
|
||||
publisher_joint_trajectory_controller:
|
||||
ros__parameters:
|
||||
|
||||
controller_name: "rizon_arm_controller"
|
||||
wait_sec_between_publish: 6
|
||||
|
||||
goal_names: ["pos1", "pos2", "pos3", "pos4"]
|
||||
pos1: [0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0]
|
||||
pos2: [0.698, -0.698, 0.698, 1.57, 0.698, 0.698, 0.698]
|
||||
pos3: [0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0]
|
||||
pos4: [-0.698, -0.698, 0.698, 1.57, 0.698, 0.698, 0.698]
|
||||
|
||||
joints:
|
||||
- $(var robot_sn)_joint1
|
||||
- $(var robot_sn)_joint2
|
||||
- $(var robot_sn)_joint3
|
||||
- $(var robot_sn)_joint4
|
||||
- $(var robot_sn)_joint5
|
||||
- $(var robot_sn)_joint6
|
||||
- $(var robot_sn)_joint7
|
||||
|
||||
check_starting_point: true
|
||||
@@ -0,0 +1,41 @@
|
||||
controller_manager:
|
||||
ros__parameters:
|
||||
update_rate: 1000 # Hz
|
||||
|
||||
rizon_arm_controller:
|
||||
type: joint_trajectory_controller/JointTrajectoryController
|
||||
|
||||
joint_state_broadcaster:
|
||||
type: joint_state_broadcaster/JointStateBroadcaster
|
||||
|
||||
gpio_controller:
|
||||
type: gpio_controller/GPIOController
|
||||
|
||||
flexiv_robot_states_broadcaster:
|
||||
type: flexiv_robot_states_broadcaster/FlexivRobotStatesBroadcaster
|
||||
|
||||
rizon_arm_controller:
|
||||
ros__parameters:
|
||||
joints:
|
||||
- $(var robot_sn)_joint1
|
||||
- $(var robot_sn)_joint2
|
||||
- $(var robot_sn)_joint3
|
||||
- $(var robot_sn)_joint4
|
||||
- $(var robot_sn)_joint5
|
||||
- $(var robot_sn)_joint6
|
||||
- $(var robot_sn)_joint7
|
||||
command_interfaces:
|
||||
- position
|
||||
state_interfaces:
|
||||
- position
|
||||
- velocity
|
||||
state_publish_rate: 100.0
|
||||
action_monitor_rate: 20.0
|
||||
allow_partial_joints_goal: false
|
||||
interpolation_method: splines
|
||||
allow_nonzero_velocity_at_trajectory_end: true
|
||||
# Stream from last command (no snap-back to encoder each preempt).
|
||||
open_loop_control: true
|
||||
constraints:
|
||||
stopped_velocity_tolerance: 0.25
|
||||
goal_time: 0.0
|
||||
@@ -0,0 +1,84 @@
|
||||
controller_manager:
|
||||
ros__parameters:
|
||||
update_rate: 1000 # Hz
|
||||
|
||||
left_rizon_arm_controller:
|
||||
type: joint_trajectory_controller/JointTrajectoryController
|
||||
|
||||
right_rizon_arm_controller:
|
||||
type: joint_trajectory_controller/JointTrajectoryController
|
||||
|
||||
joint_state_broadcaster:
|
||||
type: joint_state_broadcaster/JointStateBroadcaster
|
||||
|
||||
gpio_controller_left:
|
||||
type: gpio_controller/GPIOController
|
||||
|
||||
gpio_controller_right:
|
||||
type: gpio_controller/GPIOController
|
||||
|
||||
flexiv_robot_states_broadcaster_left:
|
||||
type: flexiv_robot_states_broadcaster/FlexivRobotStatesBroadcaster
|
||||
|
||||
flexiv_robot_states_broadcaster_right:
|
||||
type: flexiv_robot_states_broadcaster/FlexivRobotStatesBroadcaster
|
||||
|
||||
left_rizon_arm_controller:
|
||||
ros__parameters:
|
||||
joints:
|
||||
- $(var prefix_left)joint1
|
||||
- $(var prefix_left)joint2
|
||||
- $(var prefix_left)joint3
|
||||
- $(var prefix_left)joint4
|
||||
- $(var prefix_left)joint5
|
||||
- $(var prefix_left)joint6
|
||||
- $(var prefix_left)joint7
|
||||
command_interfaces:
|
||||
- position
|
||||
state_interfaces:
|
||||
- position
|
||||
- velocity
|
||||
state_publish_rate: 100.0
|
||||
action_monitor_rate: 20.0
|
||||
allow_partial_joints_goal: false
|
||||
constraints:
|
||||
stopped_velocity_tolerance: 0.01
|
||||
goal_time: 0.0
|
||||
|
||||
right_rizon_arm_controller:
|
||||
ros__parameters:
|
||||
joints:
|
||||
- $(var prefix_right)joint1
|
||||
- $(var prefix_right)joint2
|
||||
- $(var prefix_right)joint3
|
||||
- $(var prefix_right)joint4
|
||||
- $(var prefix_right)joint5
|
||||
- $(var prefix_right)joint6
|
||||
- $(var prefix_right)joint7
|
||||
command_interfaces:
|
||||
- position
|
||||
state_interfaces:
|
||||
- position
|
||||
- velocity
|
||||
state_publish_rate: 100.0
|
||||
action_monitor_rate: 20.0
|
||||
allow_partial_joints_goal: false
|
||||
constraints:
|
||||
stopped_velocity_tolerance: 0.01
|
||||
goal_time: 0.0
|
||||
|
||||
flexiv_robot_states_broadcaster_left:
|
||||
ros__parameters:
|
||||
robot_sn: left_$(var robot_sn_left)
|
||||
|
||||
flexiv_robot_states_broadcaster_right:
|
||||
ros__parameters:
|
||||
robot_sn: right_$(var robot_sn_right)
|
||||
|
||||
gpio_controller_left:
|
||||
ros__parameters:
|
||||
robot_sn: left_$(var robot_sn_left)
|
||||
|
||||
gpio_controller_right:
|
||||
ros__parameters:
|
||||
robot_sn: right_$(var robot_sn_right)
|
||||
@@ -0,0 +1 @@
|
||||
prepend-non-duplicate;AMENT_PREFIX_PATH;
|
||||
@@ -0,0 +1,4 @@
|
||||
# copied from
|
||||
# ament_cmake_core/cmake/environment_hooks/environment/ament_prefix_path.sh
|
||||
|
||||
ament_prepend_unique_value AMENT_PREFIX_PATH "$AMENT_CURRENT_PREFIX"
|
||||
@@ -0,0 +1 @@
|
||||
prepend-non-duplicate-if-exists;PATH;bin
|
||||
@@ -0,0 +1,5 @@
|
||||
# copied from ament_cmake_core/cmake/environment_hooks/environment/path.sh
|
||||
|
||||
if [ -d "$AMENT_CURRENT_PREFIX/bin" ]; then
|
||||
ament_prepend_unique_value PATH "$AMENT_CURRENT_PREFIX/bin"
|
||||
fi
|
||||
@@ -0,0 +1 @@
|
||||
prepend-non-duplicate;CMAKE_PREFIX_PATH;
|
||||
@@ -0,0 +1,3 @@
|
||||
# generated from colcon_powershell/shell/template/hook_prepend_value.ps1.em
|
||||
|
||||
colcon_prepend_unique_value CMAKE_PREFIX_PATH "$env:COLCON_CURRENT_PREFIX"
|
||||
@@ -0,0 +1,3 @@
|
||||
# generated from colcon_core/shell/template/hook_prepend_value.sh.em
|
||||
|
||||
_colcon_prepend_unique_value CMAKE_PREFIX_PATH "$COLCON_CURRENT_PREFIX"
|
||||
@@ -0,0 +1,420 @@
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import (
|
||||
DeclareLaunchArgument,
|
||||
EmitEvent,
|
||||
IncludeLaunchDescription,
|
||||
RegisterEventHandler,
|
||||
)
|
||||
from launch.conditions import IfCondition, UnlessCondition
|
||||
from launch.events import Shutdown
|
||||
from launch.event_handlers import OnProcessExit
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.parameter_descriptions import ParameterFile, ParameterValue
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
from launch.substitutions import (
|
||||
Command,
|
||||
FindExecutable,
|
||||
LaunchConfiguration,
|
||||
PathJoinSubstitution,
|
||||
PythonExpression,
|
||||
)
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
rizon_type_param_name = "rizon_type"
|
||||
robot_sn_param_name = "robot_sn"
|
||||
rdk_control_mode_param_name = "rdk_control_mode"
|
||||
start_rviz_param_name = "start_rviz"
|
||||
load_gripper_param_name = "load_gripper"
|
||||
gripper_name_param_name = "gripper_name"
|
||||
load_mounted_ft_sensor_param_name = "load_mounted_ft_sensor"
|
||||
use_fake_hardware_param_name = "use_fake_hardware"
|
||||
fake_sensor_commands_param_name = "fake_sensor_commands"
|
||||
robot_controller_param_name = "robot_controller"
|
||||
external_axis_type_param_name = "external_axis_type"
|
||||
external_axis_prefix_param_name = "external_axis_prefix"
|
||||
arm_prefix_param_name = "arm_prefix"
|
||||
|
||||
# Declare arguments
|
||||
declared_arguments = []
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
rizon_type_param_name,
|
||||
description="Type of the Flexiv Rizon robot.",
|
||||
default_value="Rizon4",
|
||||
choices=["Rizon4", "Rizon4s"],
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
robot_sn_param_name,
|
||||
description="Serial number of the robot to connect to. Remove any space, for example: Rizon4s-123456",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
rdk_control_mode_param_name,
|
||||
default_value="joint_position",
|
||||
description="RDK control mode for the ROS 2 control joint position and velocity interfaces. Options: joint_position, joint_impedance",
|
||||
choices=["joint_position", "joint_impedance"],
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
start_rviz_param_name,
|
||||
default_value="true",
|
||||
description="Start RViz automatically with the launch file",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
load_gripper_param_name,
|
||||
default_value="false",
|
||||
description="Flag to load the Flexiv Grav gripper as the end-effector of the robot.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
gripper_name_param_name,
|
||||
default_value="Flexiv-GN01",
|
||||
description="Full name of the gripper to be controlled, can be found in Flexiv Elements -> Settings -> Device",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
load_mounted_ft_sensor_param_name,
|
||||
default_value="false",
|
||||
description="Flag to load the mounted force torque sensor. Only available for Rizon4, Rizon4R and Rizon10.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
use_fake_hardware_param_name,
|
||||
default_value="false",
|
||||
description="Start robot with fake hardware mirroring command to its states.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
fake_sensor_commands_param_name,
|
||||
default_value="false",
|
||||
description="Enable fake command interfaces for sensors used for simple simulations. \
|
||||
Used only if 'use_fake_hardware' parameter is true.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
robot_controller_param_name,
|
||||
default_value="rizon_arm_controller",
|
||||
description="Robot controller to start. Available: rizon_arm_controller",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
external_axis_type_param_name,
|
||||
default_value="AICO1-4-V1",
|
||||
description="Type of the AICO1 platform.",
|
||||
choices=["AICO1-4-V1", "AICO1-4-V2"],
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
external_axis_prefix_param_name,
|
||||
default_value="",
|
||||
description="Prefix for the external axis links and joints.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
arm_prefix_param_name,
|
||||
default_value="",
|
||||
description="Prefix for the arm links and joints.",
|
||||
)
|
||||
)
|
||||
|
||||
# Initialize Arguments
|
||||
rizon_type = LaunchConfiguration(rizon_type_param_name)
|
||||
robot_sn = LaunchConfiguration(robot_sn_param_name)
|
||||
rdk_control_mode = LaunchConfiguration(rdk_control_mode_param_name)
|
||||
start_rviz = LaunchConfiguration(start_rviz_param_name)
|
||||
load_gripper = LaunchConfiguration(load_gripper_param_name)
|
||||
gripper_name = LaunchConfiguration(gripper_name_param_name)
|
||||
load_mounted_ft_sensor = LaunchConfiguration(load_mounted_ft_sensor_param_name)
|
||||
use_fake_hardware = LaunchConfiguration(use_fake_hardware_param_name)
|
||||
fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_param_name)
|
||||
robot_controller = LaunchConfiguration(robot_controller_param_name)
|
||||
external_axis_type = LaunchConfiguration(external_axis_type_param_name)
|
||||
external_axis_prefix = LaunchConfiguration(external_axis_prefix_param_name)
|
||||
arm_prefix = LaunchConfiguration(arm_prefix_param_name)
|
||||
gripper_ready_gate_condition = PythonExpression(
|
||||
[
|
||||
"'",
|
||||
load_gripper,
|
||||
"'.lower() in ['true', '1'] and '",
|
||||
use_fake_hardware,
|
||||
"'.lower() not in ['true', '1']",
|
||||
]
|
||||
)
|
||||
|
||||
# Get URDF via xacro
|
||||
flexiv_urdf_xacro = PathJoinSubstitution(
|
||||
[FindPackageShare("flexiv_description"), "urdf", "aico1.urdf.xacro"]
|
||||
)
|
||||
|
||||
# Get URDF via xacro
|
||||
robot_description_content = ParameterValue(
|
||||
Command(
|
||||
[
|
||||
PathJoinSubstitution([FindExecutable(name="xacro")]),
|
||||
" ",
|
||||
flexiv_urdf_xacro,
|
||||
" ",
|
||||
"robot_sn:=",
|
||||
robot_sn,
|
||||
" ",
|
||||
"rizon_type:=",
|
||||
rizon_type,
|
||||
" ",
|
||||
"ros2_control:=true ",
|
||||
"rdk_control_mode:=",
|
||||
rdk_control_mode,
|
||||
" ",
|
||||
"load_gripper:=",
|
||||
load_gripper,
|
||||
" ",
|
||||
"gripper_name:=",
|
||||
gripper_name,
|
||||
" ",
|
||||
"load_mounted_ft_sensor:=",
|
||||
load_mounted_ft_sensor,
|
||||
" ",
|
||||
"use_fake_hardware:=",
|
||||
use_fake_hardware,
|
||||
" ",
|
||||
"fake_sensor_commands:=",
|
||||
fake_sensor_commands,
|
||||
" ",
|
||||
"external_axis_type:=",
|
||||
PythonExpression(
|
||||
["'", external_axis_type, "'.lower().replace('-', '_')"]
|
||||
),
|
||||
" ",
|
||||
"external_axis_prefix:=",
|
||||
external_axis_prefix,
|
||||
" ",
|
||||
"arm_prefix:=",
|
||||
arm_prefix,
|
||||
]
|
||||
),
|
||||
value_type=str,
|
||||
)
|
||||
|
||||
robot_description = {"robot_description": robot_description_content}
|
||||
|
||||
# RViZ
|
||||
rviz_config_file = PathJoinSubstitution(
|
||||
[FindPackageShare("flexiv_description"), "rviz", "view_rizon.rviz"]
|
||||
)
|
||||
|
||||
rviz_node = Node(
|
||||
package="rviz2",
|
||||
executable="rviz2",
|
||||
name="rviz2",
|
||||
output="log",
|
||||
arguments=["-d", rviz_config_file],
|
||||
condition=IfCondition(start_rviz),
|
||||
)
|
||||
|
||||
# Robot controllers
|
||||
controller_file_name = PythonExpression(
|
||||
[
|
||||
"'aico1_4_v2_controllers.yaml' if '",
|
||||
external_axis_type,
|
||||
"' == 'AICO1-4-V2' else 'aico1_4_v1_controllers.yaml'",
|
||||
]
|
||||
)
|
||||
robot_controllers = PathJoinSubstitution(
|
||||
[
|
||||
FindPackageShare("flexiv_bringup"),
|
||||
"config",
|
||||
controller_file_name,
|
||||
]
|
||||
)
|
||||
|
||||
# Controller Manager
|
||||
ros2_control_node = Node(
|
||||
package="controller_manager",
|
||||
executable="ros2_control_node",
|
||||
parameters=[
|
||||
robot_description,
|
||||
ParameterFile(robot_controllers, allow_substs=True),
|
||||
{"robot_sn": robot_sn},
|
||||
{"rdk_control_mode": rdk_control_mode},
|
||||
{"external_axis_prefix": external_axis_prefix},
|
||||
],
|
||||
remappings=[("joint_states", "flexiv_rizon_arm/joint_states")],
|
||||
output="both",
|
||||
)
|
||||
|
||||
# Joint state publisher
|
||||
joint_state_publisher_node = Node(
|
||||
package="joint_state_publisher",
|
||||
executable="joint_state_publisher",
|
||||
name="joint_state_publisher",
|
||||
parameters=[
|
||||
{
|
||||
"source_list": [
|
||||
"flexiv_rizon_arm/joint_states",
|
||||
"flexiv_gripper_node/gripper_joint_states",
|
||||
],
|
||||
"rate": 30,
|
||||
}
|
||||
],
|
||||
)
|
||||
|
||||
# Robot state publisher
|
||||
robot_state_publisher_node = Node(
|
||||
package="robot_state_publisher",
|
||||
executable="robot_state_publisher",
|
||||
name="robot_state_publisher",
|
||||
output="both",
|
||||
parameters=[robot_description],
|
||||
)
|
||||
|
||||
# Run robot controller
|
||||
robot_controller_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=[robot_controller, "--controller-manager", "/controller_manager"],
|
||||
)
|
||||
|
||||
# Run joint state broadcaster
|
||||
joint_state_broadcaster_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=[
|
||||
"joint_state_broadcaster",
|
||||
"--controller-manager",
|
||||
"/controller_manager",
|
||||
],
|
||||
)
|
||||
|
||||
# Run Flexiv robot states broadcaster
|
||||
flexiv_robot_states_broadcaster_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=["flexiv_robot_states_broadcaster"],
|
||||
parameters=[{"robot_sn": robot_sn}],
|
||||
condition=UnlessCondition(use_fake_hardware),
|
||||
)
|
||||
|
||||
# Include gripper launch file
|
||||
load_gripper_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
PathJoinSubstitution(
|
||||
[
|
||||
FindPackageShare("flexiv_gripper"),
|
||||
"launch",
|
||||
"flexiv_gripper.launch.py",
|
||||
]
|
||||
)
|
||||
),
|
||||
launch_arguments={
|
||||
"robot_sn": robot_sn,
|
||||
"gripper_name": gripper_name,
|
||||
"use_fake_hardware": use_fake_hardware,
|
||||
"use_lite_rdk": "true",
|
||||
}.items(),
|
||||
condition=IfCondition(load_gripper),
|
||||
)
|
||||
|
||||
gripper_ready_waiter = Node(
|
||||
package="flexiv_gripper",
|
||||
executable="wait_for_gripper_ready",
|
||||
name="wait_for_gripper_ready",
|
||||
parameters=[{"ready_topic": "/flexiv_gripper_node/ready"}],
|
||||
output="screen",
|
||||
condition=IfCondition(gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
def launch_robot_controller_after_gripper_ready(event, context):
|
||||
if event.returncode == 0:
|
||||
return [robot_controller_spawner]
|
||||
return [
|
||||
EmitEvent(event=Shutdown(reason="flexiv_gripper_node did not report ready"))
|
||||
]
|
||||
|
||||
# Run gpio controller
|
||||
gpio_controller_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=["gpio_controller", "--controller-manager", "/controller_manager"],
|
||||
parameters=[{"robot_sn": robot_sn}],
|
||||
condition=UnlessCondition(use_fake_hardware),
|
||||
)
|
||||
|
||||
# Delay start of robot_controller after `joint_state_broadcaster`
|
||||
delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = (
|
||||
RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=joint_state_broadcaster_spawner,
|
||||
on_exit=[robot_controller_spawner],
|
||||
),
|
||||
condition=UnlessCondition(gripper_ready_gate_condition),
|
||||
)
|
||||
)
|
||||
|
||||
# Start gripper only after ros2_control has activated and the joint state broadcaster is up.
|
||||
delay_gripper_launch_after_joint_state_broadcaster_spawner = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=joint_state_broadcaster_spawner,
|
||||
on_exit=[load_gripper_launch],
|
||||
),
|
||||
condition=IfCondition(gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
delay_robot_controller_spawner_after_gripper_ready = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=gripper_ready_waiter,
|
||||
on_exit=launch_robot_controller_after_gripper_ready,
|
||||
),
|
||||
condition=IfCondition(gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
# Delay rviz start after `robot_controller_spawner`
|
||||
delay_rviz_after_robot_controller_spawner = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=robot_controller_spawner,
|
||||
on_exit=[rviz_node],
|
||||
)
|
||||
)
|
||||
|
||||
nodes = [
|
||||
ros2_control_node,
|
||||
joint_state_publisher_node,
|
||||
robot_state_publisher_node,
|
||||
gripper_ready_waiter,
|
||||
joint_state_broadcaster_spawner,
|
||||
flexiv_robot_states_broadcaster_spawner,
|
||||
gpio_controller_spawner,
|
||||
delay_gripper_launch_after_joint_state_broadcaster_spawner,
|
||||
delay_robot_controller_spawner_after_joint_state_broadcaster_spawner,
|
||||
delay_robot_controller_spawner_after_gripper_ready,
|
||||
delay_rviz_after_robot_controller_spawner,
|
||||
]
|
||||
|
||||
return LaunchDescription(declared_arguments + nodes)
|
||||
@@ -0,0 +1,608 @@
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
import os
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import (
|
||||
DeclareLaunchArgument,
|
||||
EmitEvent,
|
||||
IncludeLaunchDescription,
|
||||
OpaqueFunction,
|
||||
RegisterEventHandler,
|
||||
SetLaunchConfiguration,
|
||||
)
|
||||
from launch.conditions import IfCondition, UnlessCondition
|
||||
from launch.events import Shutdown
|
||||
from launch.event_handlers import OnProcessExit
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.parameter_descriptions import ParameterFile, ParameterValue
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
from launch.substitutions import (
|
||||
Command,
|
||||
FindExecutable,
|
||||
LaunchConfiguration,
|
||||
PathJoinSubstitution,
|
||||
PythonExpression,
|
||||
)
|
||||
|
||||
|
||||
def load_yaml(package_name, file_path, replacements=None):
|
||||
package_path = get_package_share_directory(package_name)
|
||||
absolute_file_path = os.path.join(package_path, file_path)
|
||||
|
||||
try:
|
||||
with open(absolute_file_path, "r") as file:
|
||||
file_content = file.read()
|
||||
if replacements:
|
||||
for key, value in replacements.items():
|
||||
file_content = file_content.replace(key, value)
|
||||
import yaml
|
||||
|
||||
return yaml.safe_load(file_content)
|
||||
except (
|
||||
EnvironmentError
|
||||
): # parent of IOError, OSError *and* WindowsError where available
|
||||
return None
|
||||
|
||||
|
||||
def launch_setup(context):
|
||||
rizon_type = LaunchConfiguration("rizon_type")
|
||||
robot_sn = LaunchConfiguration("robot_sn")
|
||||
rdk_control_mode = LaunchConfiguration("rdk_control_mode")
|
||||
start_rviz = LaunchConfiguration("start_rviz")
|
||||
load_gripper = LaunchConfiguration("load_gripper")
|
||||
gripper_name = LaunchConfiguration("gripper_name")
|
||||
load_mounted_ft_sensor = LaunchConfiguration("load_mounted_ft_sensor")
|
||||
use_fake_hardware = LaunchConfiguration("use_fake_hardware")
|
||||
fake_sensor_commands = LaunchConfiguration("fake_sensor_commands")
|
||||
robot_controller = LaunchConfiguration("robot_controller")
|
||||
external_axis_type = LaunchConfiguration("external_axis_type")
|
||||
external_axis_prefix = LaunchConfiguration("external_axis_prefix")
|
||||
arm_prefix = LaunchConfiguration("arm_prefix")
|
||||
gripper_ready_gate_condition = PythonExpression(
|
||||
[
|
||||
"'",
|
||||
load_gripper,
|
||||
"'.lower() in ['true', '1'] and '",
|
||||
use_fake_hardware,
|
||||
"'.lower() not in ['true', '1']",
|
||||
]
|
||||
)
|
||||
|
||||
robot_sn_str = robot_sn.perform(context)
|
||||
arm_prefix_str = arm_prefix.perform(context)
|
||||
external_axis_prefix_str = external_axis_prefix.perform(context)
|
||||
external_axis_type_str = external_axis_type.perform(context)
|
||||
|
||||
# Construct prefix
|
||||
prefix_str = ""
|
||||
|
||||
if arm_prefix_str and robot_sn_str:
|
||||
prefix_str = arm_prefix_str + "_" + robot_sn_str + "_"
|
||||
elif arm_prefix_str or robot_sn_str:
|
||||
prefix_str = arm_prefix_str + robot_sn_str + "_"
|
||||
|
||||
# Get URDF via xacro
|
||||
flexiv_urdf_xacro = PathJoinSubstitution(
|
||||
[FindPackageShare("flexiv_description"), "urdf", "aico1.urdf.xacro"]
|
||||
)
|
||||
|
||||
robot_description_content = ParameterValue(
|
||||
Command(
|
||||
[
|
||||
PathJoinSubstitution([FindExecutable(name="xacro")]),
|
||||
" ",
|
||||
flexiv_urdf_xacro,
|
||||
" ",
|
||||
"robot_sn:=",
|
||||
robot_sn,
|
||||
" ",
|
||||
"rizon_type:=",
|
||||
rizon_type,
|
||||
" ",
|
||||
"ros2_control:=true ",
|
||||
"rdk_control_mode:=",
|
||||
rdk_control_mode,
|
||||
" ",
|
||||
"load_gripper:=",
|
||||
load_gripper,
|
||||
" ",
|
||||
"gripper_name:=",
|
||||
gripper_name,
|
||||
" ",
|
||||
"load_mounted_ft_sensor:=",
|
||||
load_mounted_ft_sensor,
|
||||
" ",
|
||||
"use_fake_hardware:=",
|
||||
use_fake_hardware,
|
||||
" ",
|
||||
"fake_sensor_commands:=",
|
||||
fake_sensor_commands,
|
||||
" ",
|
||||
"external_axis_type:=",
|
||||
PythonExpression(
|
||||
["'", external_axis_type, "'.lower().replace('-', '_')"]
|
||||
),
|
||||
" ",
|
||||
"external_axis_prefix:=",
|
||||
external_axis_prefix,
|
||||
" ",
|
||||
"arm_prefix:=",
|
||||
arm_prefix,
|
||||
]
|
||||
),
|
||||
value_type=str,
|
||||
)
|
||||
|
||||
robot_description = {"robot_description": robot_description_content}
|
||||
|
||||
# MoveIt configuration
|
||||
flexiv_srdf_xacro = PathJoinSubstitution(
|
||||
[FindPackageShare("flexiv_moveit_config"), "srdf", "aico1.srdf.xacro"]
|
||||
)
|
||||
|
||||
robot_description_semantic_content = ParameterValue(
|
||||
Command(
|
||||
[
|
||||
PathJoinSubstitution([FindExecutable(name="xacro")]),
|
||||
" ",
|
||||
flexiv_srdf_xacro,
|
||||
" ",
|
||||
"robot_sn:=",
|
||||
robot_sn,
|
||||
" ",
|
||||
"load_gripper:=",
|
||||
load_gripper,
|
||||
" ",
|
||||
"load_mounted_ft_sensor:=",
|
||||
load_mounted_ft_sensor,
|
||||
" ",
|
||||
"external_axis_type:=",
|
||||
PythonExpression(
|
||||
["'", external_axis_type, "'.lower().replace('-', '_')"]
|
||||
),
|
||||
" ",
|
||||
"external_axis_prefix:=",
|
||||
external_axis_prefix,
|
||||
" ",
|
||||
"arm_prefix:=",
|
||||
arm_prefix,
|
||||
]
|
||||
),
|
||||
value_type=str,
|
||||
)
|
||||
robot_description_semantic = {
|
||||
"robot_description_semantic": robot_description_semantic_content
|
||||
}
|
||||
|
||||
publish_robot_description_semantic = {"publish_robot_description_semantic": True}
|
||||
|
||||
# Trajectory Execution Configuration
|
||||
replacements = {
|
||||
"$(var prefix)": prefix_str,
|
||||
"$(var external_axis_prefix)": external_axis_prefix_str,
|
||||
}
|
||||
|
||||
robot_description_kinematics_yaml = load_yaml(
|
||||
"flexiv_moveit_config", "config/aico/aico1_kinematics.yaml", replacements
|
||||
)
|
||||
robot_description_kinematics = {
|
||||
"robot_description_kinematics": robot_description_kinematics_yaml
|
||||
}
|
||||
|
||||
# Planning Configuration
|
||||
ompl_planning_pipeline_config = {
|
||||
"move_group": {
|
||||
"planning_plugin": "ompl_interface/OMPLPlanner",
|
||||
"request_adapters": "default_planner_request_adapters/AddTimeOptimalParameterization "
|
||||
"default_planner_request_adapters/ResolveConstraintFrames "
|
||||
"default_planner_request_adapters/FixWorkspaceBounds "
|
||||
"default_planner_request_adapters/FixStartStateBounds "
|
||||
"default_planner_request_adapters/FixStartStateCollision "
|
||||
"default_planner_request_adapters/FixStartStatePathConstraints",
|
||||
"start_state_max_bounds_error": 0.1,
|
||||
}
|
||||
}
|
||||
ompl_planning_yaml = load_yaml(
|
||||
"flexiv_moveit_config", "config/aico/aico1_ompl_planning.yaml", replacements
|
||||
)
|
||||
ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
|
||||
|
||||
controllers_file = "config/aico/aico1_4_v1_moveit_controllers.yaml"
|
||||
if external_axis_type_str == "AICO1-4-V2":
|
||||
controllers_file = "config/aico/aico1_4_v2_moveit_controllers.yaml"
|
||||
|
||||
moveit_simple_controllers_yaml = load_yaml(
|
||||
"flexiv_moveit_config",
|
||||
controllers_file,
|
||||
replacements,
|
||||
)
|
||||
|
||||
moveit_controllers = {
|
||||
"moveit_simple_controller_manager": moveit_simple_controllers_yaml,
|
||||
"moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager",
|
||||
}
|
||||
|
||||
trajectory_execution = {
|
||||
"moveit_manage_controllers": False,
|
||||
"trajectory_execution.allowed_execution_duration_scaling": 1.2,
|
||||
"trajectory_execution.allowed_goal_duration_margin": 0.5,
|
||||
"trajectory_execution.allowed_start_tolerance": 0.01,
|
||||
}
|
||||
|
||||
planning_scene_monitor_parameters = {
|
||||
"publish_planning_scene": True,
|
||||
"publish_geometry_updates": True,
|
||||
"publish_state_updates": True,
|
||||
"publish_transforms_updates": True,
|
||||
}
|
||||
|
||||
# Load joint limits
|
||||
joint_limits = load_yaml(
|
||||
"flexiv_moveit_config",
|
||||
"config/joint_limits.yaml",
|
||||
{"$(var robot_sn)": prefix_str.rstrip("_")},
|
||||
)
|
||||
|
||||
# Load external axis joint limits
|
||||
external_axis_joint_limits = load_yaml(
|
||||
"flexiv_moveit_config",
|
||||
"config/aico/aico_joint_limits.yaml",
|
||||
{"$(var external_axis_prefix)": external_axis_prefix_str},
|
||||
)
|
||||
|
||||
joint_limits_yaml = {"robot_description_planning": {"joint_limits": {}}}
|
||||
|
||||
if joint_limits and "joint_limits" in joint_limits:
|
||||
joint_limits_yaml["robot_description_planning"]["joint_limits"].update(
|
||||
joint_limits["joint_limits"]
|
||||
)
|
||||
|
||||
if external_axis_joint_limits and "joint_limits" in external_axis_joint_limits:
|
||||
joint_limits_yaml["robot_description_planning"]["joint_limits"].update(
|
||||
external_axis_joint_limits["joint_limits"]
|
||||
)
|
||||
|
||||
warehouse_ros_config = {
|
||||
"warehouse_plugin": "warehouse_ros_sqlite::DatabaseConnection",
|
||||
"warehouse_host": LaunchConfiguration("warehouse_sqlite_path"),
|
||||
}
|
||||
|
||||
# Start the actual move_group node/action server
|
||||
move_group_node = Node(
|
||||
package="moveit_ros_move_group",
|
||||
executable="move_group",
|
||||
output="screen",
|
||||
parameters=[
|
||||
robot_description,
|
||||
robot_description_semantic,
|
||||
publish_robot_description_semantic,
|
||||
robot_description_kinematics,
|
||||
joint_limits_yaml,
|
||||
ompl_planning_pipeline_config,
|
||||
trajectory_execution,
|
||||
moveit_controllers,
|
||||
planning_scene_monitor_parameters,
|
||||
warehouse_ros_config,
|
||||
],
|
||||
)
|
||||
|
||||
# RViz with MoveIt configuration
|
||||
rviz_config_file = PathJoinSubstitution(
|
||||
[FindPackageShare("flexiv_moveit_config"), "rviz", "moveit.rviz"]
|
||||
)
|
||||
|
||||
rviz_node = Node(
|
||||
package="rviz2",
|
||||
executable="rviz2",
|
||||
name="rviz2_moveit",
|
||||
output="log",
|
||||
arguments=["-d", rviz_config_file],
|
||||
parameters=[
|
||||
robot_description,
|
||||
robot_description_semantic,
|
||||
ompl_planning_pipeline_config,
|
||||
robot_description_kinematics,
|
||||
joint_limits_yaml,
|
||||
warehouse_ros_config,
|
||||
],
|
||||
condition=IfCondition(start_rviz),
|
||||
)
|
||||
|
||||
# Publish TF
|
||||
robot_state_publisher_node = Node(
|
||||
package="robot_state_publisher",
|
||||
executable="robot_state_publisher",
|
||||
name="robot_state_publisher",
|
||||
output="both",
|
||||
parameters=[robot_description],
|
||||
)
|
||||
|
||||
# Robot controllers
|
||||
ros2_controllers_file = "aico1_4_v1_controllers.yaml"
|
||||
if external_axis_type_str == "AICO1-4-V2":
|
||||
ros2_controllers_file = "aico1_4_v2_controllers.yaml"
|
||||
robot_controllers = PathJoinSubstitution(
|
||||
[FindPackageShare("flexiv_bringup"), "config", ros2_controllers_file]
|
||||
)
|
||||
|
||||
# Run controller manager
|
||||
ros2_control_node = Node(
|
||||
package="controller_manager",
|
||||
executable="ros2_control_node",
|
||||
parameters=[
|
||||
robot_description,
|
||||
ParameterFile(robot_controllers, allow_substs=True),
|
||||
{"robot_sn": robot_sn},
|
||||
{"rdk_control_mode": rdk_control_mode},
|
||||
],
|
||||
remappings=[("joint_states", "flexiv_rizon_arm/joint_states")],
|
||||
output="both",
|
||||
)
|
||||
|
||||
# Joint state publisher
|
||||
joint_state_publisher_node = Node(
|
||||
package="joint_state_publisher",
|
||||
executable="joint_state_publisher",
|
||||
name="joint_state_publisher",
|
||||
parameters=[
|
||||
{
|
||||
"source_list": [
|
||||
"flexiv_rizon_arm/joint_states",
|
||||
"flexiv_gripper_node/gripper_joint_states",
|
||||
],
|
||||
"rate": 30,
|
||||
}
|
||||
],
|
||||
)
|
||||
|
||||
# Run robot controller
|
||||
rizon_arm_controller_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=[
|
||||
robot_controller,
|
||||
"--controller-manager",
|
||||
"/controller_manager",
|
||||
],
|
||||
)
|
||||
|
||||
# Run joint state broadcaster
|
||||
joint_state_broadcaster_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=[
|
||||
"joint_state_broadcaster",
|
||||
"--controller-manager",
|
||||
"/controller_manager",
|
||||
],
|
||||
)
|
||||
|
||||
# Run Flexiv robot states broadcaster
|
||||
flexiv_robot_states_broadcaster_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=["flexiv_robot_states_broadcaster"],
|
||||
condition=UnlessCondition(use_fake_hardware),
|
||||
)
|
||||
|
||||
load_gripper_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
PathJoinSubstitution(
|
||||
[
|
||||
FindPackageShare("flexiv_gripper"),
|
||||
"launch",
|
||||
"flexiv_gripper.launch.py",
|
||||
]
|
||||
)
|
||||
),
|
||||
launch_arguments={
|
||||
"robot_sn": robot_sn,
|
||||
"gripper_name": gripper_name,
|
||||
"use_fake_hardware": use_fake_hardware,
|
||||
"use_lite_rdk": "true",
|
||||
}.items(),
|
||||
condition=IfCondition(load_gripper),
|
||||
)
|
||||
|
||||
gripper_ready_waiter = Node(
|
||||
package="flexiv_gripper",
|
||||
executable="wait_for_gripper_ready",
|
||||
name="wait_for_gripper_ready",
|
||||
parameters=[{"ready_topic": "/flexiv_gripper_node/ready"}],
|
||||
output="screen",
|
||||
condition=IfCondition(gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
def launch_controller_after_gripper_ready(event, context):
|
||||
if event.returncode == 0:
|
||||
return [rizon_arm_controller_spawner]
|
||||
return [
|
||||
EmitEvent(event=Shutdown(reason="flexiv_gripper_node did not report ready"))
|
||||
]
|
||||
|
||||
# Run gpio controller
|
||||
gpio_controller_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=[
|
||||
"gpio_controller",
|
||||
"--controller-manager",
|
||||
"/controller_manager",
|
||||
],
|
||||
condition=UnlessCondition(use_fake_hardware),
|
||||
)
|
||||
|
||||
# Delay start of controllers after `joint_state_broadcaster`
|
||||
delay_controller_after_jsb = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=joint_state_broadcaster_spawner,
|
||||
on_exit=[rizon_arm_controller_spawner],
|
||||
),
|
||||
condition=UnlessCondition(gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
# Start gripper only after ros2_control has activated and the joint state broadcaster is up.
|
||||
delay_gripper_launch_after_joint_state_broadcaster_spawner = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=joint_state_broadcaster_spawner,
|
||||
on_exit=[load_gripper_launch],
|
||||
),
|
||||
condition=IfCondition(gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
delay_controller_after_gripper_ready = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=gripper_ready_waiter,
|
||||
on_exit=launch_controller_after_gripper_ready,
|
||||
),
|
||||
condition=IfCondition(gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
# Delay rviz start after controller
|
||||
delay_rviz_after_controller = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=rizon_arm_controller_spawner,
|
||||
on_exit=[rviz_node],
|
||||
)
|
||||
)
|
||||
|
||||
nodes = [
|
||||
move_group_node,
|
||||
robot_state_publisher_node,
|
||||
ros2_control_node,
|
||||
joint_state_publisher_node,
|
||||
gripper_ready_waiter,
|
||||
joint_state_broadcaster_spawner,
|
||||
flexiv_robot_states_broadcaster_spawner,
|
||||
gpio_controller_spawner,
|
||||
delay_gripper_launch_after_joint_state_broadcaster_spawner,
|
||||
delay_controller_after_jsb,
|
||||
delay_controller_after_gripper_ready,
|
||||
delay_rviz_after_controller,
|
||||
]
|
||||
|
||||
return nodes
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
declared_arguments = []
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"rizon_type",
|
||||
description="Type of the Flexiv Rizon robot.",
|
||||
default_value="Rizon4",
|
||||
choices=["Rizon4", "Rizon4s"],
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"robot_sn",
|
||||
description="Serial number of the robot to connect to. Remove any space, for example: Rizon4s-123456",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"rdk_control_mode",
|
||||
default_value="joint_position",
|
||||
description="RDK control mode for the ROS 2 control joint position and velocity interfaces. Options: joint_position, joint_impedance",
|
||||
choices=["joint_position", "joint_impedance"],
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"start_rviz",
|
||||
default_value="true",
|
||||
description="Start RViz automatically with the launch file",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"load_gripper",
|
||||
default_value="false",
|
||||
description="Flag to load the Flexiv Grav gripper as the end-effector of the robot.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"gripper_name",
|
||||
default_value="Flexiv-GN01",
|
||||
description="Full name of the gripper to be controlled, can be found in Flexiv Elements -> Settings -> Device",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"load_mounted_ft_sensor",
|
||||
default_value="false",
|
||||
description="Flag to load the mounted force torque sensor. Only available for Rizon4, Rizon4R and Rizon10.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"use_fake_hardware",
|
||||
default_value="false",
|
||||
description="Start robot with fake hardware mirroring command to its states.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"fake_sensor_commands",
|
||||
default_value="false",
|
||||
description="Enable fake command interfaces for sensors used for simple simulations. \
|
||||
Used only if 'use_fake_hardware' parameter is true.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"robot_controller",
|
||||
default_value="rizon_arm_controller",
|
||||
description="Robot controller to start. Available: rizon_arm_controller",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"external_axis_type",
|
||||
default_value="AICO1-4-V1",
|
||||
description="Type of the AICO1 platform.",
|
||||
choices=["AICO1-4-V1", "AICO1-4-V2"],
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"external_axis_prefix",
|
||||
default_value="",
|
||||
description="Prefix for the external axis links and joints.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"arm_prefix",
|
||||
default_value="",
|
||||
description="Prefix for the arm links and joints.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"warehouse_sqlite_path",
|
||||
default_value=os.path.expanduser("~/.ros/warehouse_ros.sqlite"),
|
||||
description="Path to the warehouse database",
|
||||
)
|
||||
)
|
||||
|
||||
return LaunchDescription(
|
||||
declared_arguments + [OpaqueFunction(function=launch_setup)]
|
||||
)
|
||||
@@ -0,0 +1,591 @@
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import (
|
||||
DeclareLaunchArgument,
|
||||
EmitEvent,
|
||||
IncludeLaunchDescription,
|
||||
RegisterEventHandler,
|
||||
)
|
||||
from launch.conditions import IfCondition, UnlessCondition
|
||||
from launch.events import Shutdown
|
||||
from launch.event_handlers import OnProcessExit
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.parameter_descriptions import ParameterFile, ParameterValue
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
from launch.substitutions import (
|
||||
Command,
|
||||
FindExecutable,
|
||||
LaunchConfiguration,
|
||||
PathJoinSubstitution,
|
||||
PythonExpression,
|
||||
)
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
rizon_type_param_name = "rizon_type"
|
||||
robot_sn_left_param_name = "robot_sn_left"
|
||||
robot_sn_right_param_name = "robot_sn_right"
|
||||
rdk_control_mode_param_name = "rdk_control_mode"
|
||||
start_rviz_param_name = "start_rviz"
|
||||
use_fake_hardware_param_name = "use_fake_hardware"
|
||||
fake_sensor_commands_param_name = "fake_sensor_commands"
|
||||
load_gripper_left_param_name = "load_gripper_left"
|
||||
gripper_name_left_param_name = "gripper_name_left"
|
||||
load_gripper_right_param_name = "load_gripper_right"
|
||||
gripper_name_right_param_name = "gripper_name_right"
|
||||
load_mounted_ft_sensor_left_param_name = "load_mounted_ft_sensor_left"
|
||||
load_mounted_ft_sensor_right_param_name = "load_mounted_ft_sensor_right"
|
||||
external_axis_type_param_name = "external_axis_type"
|
||||
external_axis_prefix_param_name = "external_axis_prefix"
|
||||
|
||||
# Declare arguments
|
||||
declared_arguments = []
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
rizon_type_param_name,
|
||||
description="Type of the Flexiv Rizon robot.",
|
||||
default_value="Rizon4",
|
||||
choices=["Rizon4", "Rizon10"],
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
robot_sn_left_param_name,
|
||||
description="Serial number of the left robot.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
robot_sn_right_param_name,
|
||||
description="Serial number of the right robot.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
rdk_control_mode_param_name,
|
||||
default_value="joint_position",
|
||||
description="RDK control mode for the ROS 2 control joint position and velocity interfaces.",
|
||||
choices=["joint_position", "joint_impedance"],
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
start_rviz_param_name,
|
||||
default_value="true",
|
||||
description="Start RViz automatically with the launch file",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
use_fake_hardware_param_name,
|
||||
default_value="false",
|
||||
description="Start robot with fake hardware mirroring command to its states.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
fake_sensor_commands_param_name,
|
||||
default_value="false",
|
||||
description="Enable fake command interfaces for sensors used for simple simulations.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
load_gripper_left_param_name,
|
||||
default_value="false",
|
||||
description="Flag to load the Flexiv Grav gripper as the end-effector of the left robot.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
gripper_name_left_param_name,
|
||||
default_value="Flexiv-GN01",
|
||||
description="Full name of the left gripper to be controlled, can be found in Flexiv Elements -> Settings -> Device",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
load_gripper_right_param_name,
|
||||
default_value="false",
|
||||
description="Flag to load the Flexiv Grav gripper as the end-effector of the right robot.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
gripper_name_right_param_name,
|
||||
default_value="Flexiv-GN01",
|
||||
description="Full name of the right gripper to be controlled, can be found in Flexiv Elements -> Settings -> Device",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
load_mounted_ft_sensor_left_param_name,
|
||||
default_value="false",
|
||||
description="Flag to load the mounted force torque sensor for the left robot. Only available for Rizon4, Rizon4R and Rizon10.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
load_mounted_ft_sensor_right_param_name,
|
||||
default_value="false",
|
||||
description="Flag to load the mounted force torque sensor for the right robot. Only available for Rizon4, Rizon4R and Rizon10.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
external_axis_type_param_name,
|
||||
default_value="AICO2-4-V1",
|
||||
description="Type of the AICO2 platform.",
|
||||
choices=["AICO2-4-V1", "AICO2-4-V2", "AICO2-10-V1"],
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
external_axis_prefix_param_name,
|
||||
default_value="",
|
||||
description="Prefix for the external axis links and joints.",
|
||||
)
|
||||
)
|
||||
|
||||
# Initialize Arguments
|
||||
rizon_type = LaunchConfiguration(rizon_type_param_name)
|
||||
robot_sn_left = LaunchConfiguration(robot_sn_left_param_name)
|
||||
robot_sn_right = LaunchConfiguration(robot_sn_right_param_name)
|
||||
rdk_control_mode = LaunchConfiguration(rdk_control_mode_param_name)
|
||||
start_rviz = LaunchConfiguration(start_rviz_param_name)
|
||||
use_fake_hardware = LaunchConfiguration(use_fake_hardware_param_name)
|
||||
fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_param_name)
|
||||
load_gripper_left = LaunchConfiguration(load_gripper_left_param_name)
|
||||
gripper_name_left = LaunchConfiguration(gripper_name_left_param_name)
|
||||
load_gripper_right = LaunchConfiguration(load_gripper_right_param_name)
|
||||
gripper_name_right = LaunchConfiguration(gripper_name_right_param_name)
|
||||
load_mounted_ft_sensor_left = LaunchConfiguration(
|
||||
load_mounted_ft_sensor_left_param_name
|
||||
)
|
||||
load_mounted_ft_sensor_right = LaunchConfiguration(
|
||||
load_mounted_ft_sensor_right_param_name
|
||||
)
|
||||
external_axis_type = LaunchConfiguration(external_axis_type_param_name)
|
||||
external_axis_prefix = LaunchConfiguration(external_axis_prefix_param_name)
|
||||
left_gripper_ready_gate_condition = PythonExpression(
|
||||
[
|
||||
"'",
|
||||
load_gripper_left,
|
||||
"'.lower() in ['true', '1'] and '",
|
||||
use_fake_hardware,
|
||||
"'.lower() not in ['true', '1']",
|
||||
]
|
||||
)
|
||||
right_gripper_ready_gate_condition = PythonExpression(
|
||||
[
|
||||
"'",
|
||||
load_gripper_right,
|
||||
"'.lower() in ['true', '1'] and '",
|
||||
use_fake_hardware,
|
||||
"'.lower() not in ['true', '1']",
|
||||
]
|
||||
)
|
||||
|
||||
# Construct prefixes
|
||||
from launch.actions import SetLaunchConfiguration
|
||||
|
||||
set_prefix_left = SetLaunchConfiguration(
|
||||
name="prefix_left",
|
||||
value=PythonExpression(["'left_' + '", robot_sn_left, "' + '_'"]),
|
||||
)
|
||||
set_prefix_right = SetLaunchConfiguration(
|
||||
name="prefix_right",
|
||||
value=PythonExpression(["'right_' + '", robot_sn_right, "' + '_'"]),
|
||||
)
|
||||
|
||||
# Get URDF via xacro
|
||||
flexiv_urdf_xacro = PathJoinSubstitution(
|
||||
[FindPackageShare("flexiv_description"), "urdf", "aico2.urdf.xacro"]
|
||||
)
|
||||
|
||||
robot_description_content = ParameterValue(
|
||||
Command(
|
||||
[
|
||||
PathJoinSubstitution([FindExecutable(name="xacro")]),
|
||||
" ",
|
||||
flexiv_urdf_xacro,
|
||||
" ",
|
||||
"rizon_type:=",
|
||||
rizon_type,
|
||||
" ",
|
||||
"robot_sn_left:=",
|
||||
robot_sn_left,
|
||||
" ",
|
||||
"robot_sn_right:=",
|
||||
robot_sn_right,
|
||||
" ",
|
||||
"load_gripper_left:=",
|
||||
load_gripper_left,
|
||||
" ",
|
||||
"gripper_name_left:=",
|
||||
gripper_name_left,
|
||||
" ",
|
||||
"load_gripper_right:=",
|
||||
load_gripper_right,
|
||||
" ",
|
||||
"gripper_name_right:=",
|
||||
gripper_name_right,
|
||||
" ",
|
||||
"load_mounted_ft_sensor_left:=",
|
||||
load_mounted_ft_sensor_left,
|
||||
" ",
|
||||
"load_mounted_ft_sensor_right:=",
|
||||
load_mounted_ft_sensor_right,
|
||||
" ",
|
||||
"ros2_control:=true ",
|
||||
"rdk_control_mode:=",
|
||||
rdk_control_mode,
|
||||
" ",
|
||||
"use_fake_hardware:=",
|
||||
use_fake_hardware,
|
||||
" ",
|
||||
"fake_sensor_commands:=",
|
||||
fake_sensor_commands,
|
||||
" ",
|
||||
"external_axis_type:=",
|
||||
PythonExpression(
|
||||
["'", external_axis_type, "'.lower().replace('-', '_')"]
|
||||
),
|
||||
" ",
|
||||
"external_axis_prefix:=",
|
||||
external_axis_prefix,
|
||||
]
|
||||
),
|
||||
value_type=str,
|
||||
)
|
||||
|
||||
robot_description = {"robot_description": robot_description_content}
|
||||
|
||||
# RViZ
|
||||
rviz_config_file = PathJoinSubstitution(
|
||||
[FindPackageShare("flexiv_description"), "rviz", "view_rizon.rviz"]
|
||||
)
|
||||
|
||||
rviz_node = Node(
|
||||
package="rviz2",
|
||||
executable="rviz2",
|
||||
name="rviz2",
|
||||
output="log",
|
||||
arguments=["-d", rviz_config_file],
|
||||
condition=IfCondition(start_rviz),
|
||||
)
|
||||
|
||||
# Robot controllers
|
||||
controller_file_name = PythonExpression(
|
||||
[
|
||||
"'aico2_10_v1_controllers.yaml' if '",
|
||||
external_axis_type,
|
||||
"' == 'AICO2-10-V1' else ('aico2_4_v2_controllers.yaml' if '",
|
||||
external_axis_type,
|
||||
"' == 'AICO2-4-V2' else 'aico2_4_v1_controllers.yaml')",
|
||||
]
|
||||
)
|
||||
robot_controllers = PathJoinSubstitution(
|
||||
[
|
||||
FindPackageShare("flexiv_bringup"),
|
||||
"config",
|
||||
controller_file_name,
|
||||
]
|
||||
)
|
||||
|
||||
# Controller Manager
|
||||
ros2_control_node = Node(
|
||||
package="controller_manager",
|
||||
executable="ros2_control_node",
|
||||
parameters=[
|
||||
robot_description,
|
||||
ParameterFile(robot_controllers, allow_substs=True),
|
||||
{"robot_sn_left": robot_sn_left},
|
||||
{"robot_sn_right": robot_sn_right},
|
||||
{"prefix_left": LaunchConfiguration("prefix_left")},
|
||||
{"prefix_right": LaunchConfiguration("prefix_right")},
|
||||
{"rdk_control_mode": rdk_control_mode},
|
||||
{"external_axis_prefix": external_axis_prefix},
|
||||
],
|
||||
remappings=[("joint_states", "flexiv_dual_arm/joint_states")],
|
||||
output="both",
|
||||
)
|
||||
|
||||
# Joint state publisher
|
||||
joint_state_publisher_node = Node(
|
||||
package="joint_state_publisher",
|
||||
executable="joint_state_publisher",
|
||||
name="joint_state_publisher",
|
||||
parameters=[
|
||||
{
|
||||
"source_list": [
|
||||
"flexiv_dual_arm/joint_states",
|
||||
"left_gripper_node/gripper_joint_states",
|
||||
"right_gripper_node/gripper_joint_states",
|
||||
],
|
||||
"rate": 30,
|
||||
}
|
||||
],
|
||||
)
|
||||
|
||||
# Robot state publisher
|
||||
robot_state_publisher_node = Node(
|
||||
package="robot_state_publisher",
|
||||
executable="robot_state_publisher",
|
||||
name="robot_state_publisher",
|
||||
output="both",
|
||||
parameters=[robot_description],
|
||||
)
|
||||
|
||||
# Run joint state broadcaster
|
||||
joint_state_broadcaster_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=[
|
||||
"joint_state_broadcaster",
|
||||
"--controller-manager",
|
||||
"/controller_manager",
|
||||
],
|
||||
)
|
||||
|
||||
# Run left arm controller
|
||||
left_rizon_arm_controller_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=[
|
||||
"left_rizon_arm_controller",
|
||||
"--controller-manager",
|
||||
"/controller_manager",
|
||||
],
|
||||
)
|
||||
|
||||
# Run right arm controller
|
||||
right_rizon_arm_controller_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=[
|
||||
"right_rizon_arm_controller",
|
||||
"--controller-manager",
|
||||
"/controller_manager",
|
||||
],
|
||||
)
|
||||
|
||||
# Run Flexiv robot states broadcaster left
|
||||
flexiv_robot_states_broadcaster_left_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=["flexiv_robot_states_broadcaster_left"],
|
||||
condition=UnlessCondition(use_fake_hardware),
|
||||
)
|
||||
|
||||
# Run Flexiv robot states broadcaster right
|
||||
flexiv_robot_states_broadcaster_right_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=["flexiv_robot_states_broadcaster_right"],
|
||||
condition=UnlessCondition(use_fake_hardware),
|
||||
)
|
||||
|
||||
# Include gripper launch files
|
||||
load_gripper_left_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
PathJoinSubstitution(
|
||||
[
|
||||
FindPackageShare("flexiv_gripper"),
|
||||
"launch",
|
||||
"flexiv_gripper.launch.py",
|
||||
]
|
||||
)
|
||||
),
|
||||
launch_arguments={
|
||||
"gripper_node_name": "left_gripper_node",
|
||||
"robot_sn": robot_sn_left,
|
||||
"gripper_name": gripper_name_left,
|
||||
"use_fake_hardware": use_fake_hardware,
|
||||
"use_lite_rdk": "true",
|
||||
}.items(),
|
||||
condition=IfCondition(load_gripper_left),
|
||||
)
|
||||
load_gripper_right_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
PathJoinSubstitution(
|
||||
[
|
||||
FindPackageShare("flexiv_gripper"),
|
||||
"launch",
|
||||
"flexiv_gripper.launch.py",
|
||||
]
|
||||
)
|
||||
),
|
||||
launch_arguments={
|
||||
"gripper_node_name": "right_gripper_node",
|
||||
"robot_sn": robot_sn_right,
|
||||
"gripper_name": gripper_name_right,
|
||||
"use_fake_hardware": use_fake_hardware,
|
||||
"use_lite_rdk": "true",
|
||||
}.items(),
|
||||
condition=IfCondition(load_gripper_right),
|
||||
)
|
||||
|
||||
left_gripper_ready_waiter = Node(
|
||||
package="flexiv_gripper",
|
||||
executable="wait_for_gripper_ready",
|
||||
name="wait_for_left_gripper_ready",
|
||||
parameters=[{"ready_topic": "/left_gripper_node/ready"}],
|
||||
output="screen",
|
||||
condition=IfCondition(left_gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
right_gripper_ready_waiter = Node(
|
||||
package="flexiv_gripper",
|
||||
executable="wait_for_gripper_ready",
|
||||
name="wait_for_right_gripper_ready",
|
||||
parameters=[{"ready_topic": "/right_gripper_node/ready"}],
|
||||
output="screen",
|
||||
condition=IfCondition(right_gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
def launch_controller_after_gripper_ready(controller_spawner, gripper_node_name):
|
||||
def handle_gripper_ready(event, context):
|
||||
if event.returncode == 0:
|
||||
return [controller_spawner]
|
||||
return [
|
||||
EmitEvent(
|
||||
event=Shutdown(reason=f"{gripper_node_name} did not report ready")
|
||||
)
|
||||
]
|
||||
|
||||
return handle_gripper_ready
|
||||
|
||||
# Run gpio controllers
|
||||
gpio_controller_left_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=[
|
||||
"gpio_controller_left",
|
||||
"--controller-manager",
|
||||
"/controller_manager",
|
||||
],
|
||||
condition=UnlessCondition(use_fake_hardware),
|
||||
)
|
||||
gpio_controller_right_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=[
|
||||
"gpio_controller_right",
|
||||
"--controller-manager",
|
||||
"/controller_manager",
|
||||
],
|
||||
condition=UnlessCondition(use_fake_hardware),
|
||||
)
|
||||
|
||||
# Delay start of controllers after `joint_state_broadcaster`
|
||||
delay_left_controller_after_jsb = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=joint_state_broadcaster_spawner,
|
||||
on_exit=[left_rizon_arm_controller_spawner],
|
||||
),
|
||||
condition=UnlessCondition(left_gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
delay_left_gripper_launch_after_joint_state_broadcaster_spawner = (
|
||||
RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=joint_state_broadcaster_spawner,
|
||||
on_exit=[load_gripper_left_launch, left_gripper_ready_waiter],
|
||||
),
|
||||
condition=IfCondition(left_gripper_ready_gate_condition),
|
||||
)
|
||||
)
|
||||
|
||||
delay_right_gripper_launch_after_joint_state_broadcaster_spawner = (
|
||||
RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=joint_state_broadcaster_spawner,
|
||||
on_exit=[load_gripper_right_launch],
|
||||
),
|
||||
condition=IfCondition(right_gripper_ready_gate_condition),
|
||||
)
|
||||
)
|
||||
|
||||
delay_left_controller_after_gripper_ready = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=left_gripper_ready_waiter,
|
||||
on_exit=launch_controller_after_gripper_ready(
|
||||
left_rizon_arm_controller_spawner, "left_gripper_node"
|
||||
),
|
||||
),
|
||||
condition=IfCondition(left_gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
# Delay right controller start after left controller
|
||||
delay_right_controller_after_left_controller = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=left_rizon_arm_controller_spawner,
|
||||
on_exit=[right_rizon_arm_controller_spawner],
|
||||
),
|
||||
condition=UnlessCondition(right_gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
delay_right_gripper_ready_waiter_after_left_controller = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=left_rizon_arm_controller_spawner,
|
||||
on_exit=[right_gripper_ready_waiter],
|
||||
),
|
||||
condition=IfCondition(right_gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
delay_right_controller_after_gripper_ready = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=right_gripper_ready_waiter,
|
||||
on_exit=launch_controller_after_gripper_ready(
|
||||
right_rizon_arm_controller_spawner, "right_gripper_node"
|
||||
),
|
||||
),
|
||||
condition=IfCondition(right_gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
# Delay rviz start after right controller
|
||||
delay_rviz_after_right_controller = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=right_rizon_arm_controller_spawner,
|
||||
on_exit=[rviz_node],
|
||||
)
|
||||
)
|
||||
|
||||
nodes = [
|
||||
set_prefix_left,
|
||||
set_prefix_right,
|
||||
ros2_control_node,
|
||||
joint_state_publisher_node,
|
||||
robot_state_publisher_node,
|
||||
joint_state_broadcaster_spawner,
|
||||
flexiv_robot_states_broadcaster_left_spawner,
|
||||
flexiv_robot_states_broadcaster_right_spawner,
|
||||
gpio_controller_left_spawner,
|
||||
gpio_controller_right_spawner,
|
||||
delay_left_gripper_launch_after_joint_state_broadcaster_spawner,
|
||||
delay_right_gripper_launch_after_joint_state_broadcaster_spawner,
|
||||
delay_left_controller_after_jsb,
|
||||
delay_left_controller_after_gripper_ready,
|
||||
delay_right_controller_after_left_controller,
|
||||
delay_right_gripper_ready_waiter_after_left_controller,
|
||||
delay_right_controller_after_gripper_ready,
|
||||
delay_rviz_after_right_controller,
|
||||
]
|
||||
|
||||
return LaunchDescription(declared_arguments + nodes)
|
||||
@@ -0,0 +1,792 @@
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
import os
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import (
|
||||
DeclareLaunchArgument,
|
||||
EmitEvent,
|
||||
IncludeLaunchDescription,
|
||||
OpaqueFunction,
|
||||
RegisterEventHandler,
|
||||
SetLaunchConfiguration,
|
||||
)
|
||||
from launch.conditions import IfCondition, UnlessCondition
|
||||
from launch.events import Shutdown
|
||||
from launch.event_handlers import OnProcessExit
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.parameter_descriptions import ParameterFile, ParameterValue
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
from launch.substitutions import (
|
||||
Command,
|
||||
FindExecutable,
|
||||
LaunchConfiguration,
|
||||
PathJoinSubstitution,
|
||||
PythonExpression,
|
||||
)
|
||||
|
||||
|
||||
def load_yaml(package_name, file_path, replacements=None):
|
||||
package_path = get_package_share_directory(package_name)
|
||||
absolute_file_path = os.path.join(package_path, file_path)
|
||||
|
||||
try:
|
||||
with open(absolute_file_path, "r") as file:
|
||||
file_content = file.read()
|
||||
if replacements:
|
||||
for key, value in replacements.items():
|
||||
file_content = file_content.replace(key, value)
|
||||
import yaml
|
||||
|
||||
return yaml.safe_load(file_content)
|
||||
except (
|
||||
EnvironmentError
|
||||
): # parent of IOError, OSError *and* WindowsError where available
|
||||
return None
|
||||
|
||||
|
||||
def launch_setup(context):
|
||||
# Initialize Arguments
|
||||
rizon_type = LaunchConfiguration("rizon_type")
|
||||
robot_sn_left = LaunchConfiguration("robot_sn_left")
|
||||
robot_sn_right = LaunchConfiguration("robot_sn_right")
|
||||
|
||||
robot_sn_left_str = robot_sn_left.perform(context)
|
||||
robot_sn_right_str = robot_sn_right.perform(context)
|
||||
|
||||
rdk_control_mode = LaunchConfiguration("rdk_control_mode")
|
||||
start_rviz = LaunchConfiguration("start_rviz")
|
||||
use_fake_hardware = LaunchConfiguration("use_fake_hardware")
|
||||
fake_sensor_commands = LaunchConfiguration("fake_sensor_commands")
|
||||
|
||||
load_gripper_left = LaunchConfiguration("load_gripper_left")
|
||||
gripper_name_left = LaunchConfiguration("gripper_name_left")
|
||||
load_gripper_right = LaunchConfiguration("load_gripper_right")
|
||||
gripper_name_right = LaunchConfiguration("gripper_name_right")
|
||||
|
||||
load_mounted_ft_sensor_left = LaunchConfiguration("load_mounted_ft_sensor_left")
|
||||
load_mounted_ft_sensor_right = LaunchConfiguration("load_mounted_ft_sensor_right")
|
||||
left_gripper_ready_gate_condition = PythonExpression(
|
||||
[
|
||||
"'",
|
||||
load_gripper_left,
|
||||
"'.lower() in ['true', '1'] and '",
|
||||
use_fake_hardware,
|
||||
"'.lower() not in ['true', '1']",
|
||||
]
|
||||
)
|
||||
right_gripper_ready_gate_condition = PythonExpression(
|
||||
[
|
||||
"'",
|
||||
load_gripper_right,
|
||||
"'.lower() in ['true', '1'] and '",
|
||||
use_fake_hardware,
|
||||
"'.lower() not in ['true', '1']",
|
||||
]
|
||||
)
|
||||
|
||||
external_axis_type = LaunchConfiguration("external_axis_type")
|
||||
external_axis_prefix = LaunchConfiguration("external_axis_prefix")
|
||||
|
||||
warehouse_sqlite_path = LaunchConfiguration("warehouse_sqlite_path")
|
||||
|
||||
external_axis_type_str = external_axis_type.perform(context)
|
||||
external_axis_prefix_str = external_axis_prefix.perform(context)
|
||||
|
||||
# Construct prefixes
|
||||
prefix_left_str = "left_" + robot_sn_left_str + "_"
|
||||
|
||||
prefix_right_str = "right_" + robot_sn_right_str + "_"
|
||||
|
||||
set_prefix_left = SetLaunchConfiguration(name="prefix_left", value=prefix_left_str)
|
||||
set_prefix_right = SetLaunchConfiguration(
|
||||
name="prefix_right", value=prefix_right_str
|
||||
)
|
||||
|
||||
# Get URDF via xacro
|
||||
flexiv_urdf_xacro = PathJoinSubstitution(
|
||||
[FindPackageShare("flexiv_description"), "urdf", "aico2.urdf.xacro"]
|
||||
)
|
||||
|
||||
robot_description_content = ParameterValue(
|
||||
Command(
|
||||
[
|
||||
PathJoinSubstitution([FindExecutable(name="xacro")]),
|
||||
" ",
|
||||
flexiv_urdf_xacro,
|
||||
" ",
|
||||
"rizon_type:=",
|
||||
rizon_type,
|
||||
" ",
|
||||
"robot_sn_left:=",
|
||||
robot_sn_left,
|
||||
" ",
|
||||
"robot_sn_right:=",
|
||||
robot_sn_right,
|
||||
" ",
|
||||
"load_gripper_left:=",
|
||||
load_gripper_left,
|
||||
" ",
|
||||
"gripper_name_left:=",
|
||||
gripper_name_left,
|
||||
" ",
|
||||
"load_gripper_right:=",
|
||||
load_gripper_right,
|
||||
" ",
|
||||
"gripper_name_right:=",
|
||||
gripper_name_right,
|
||||
" ",
|
||||
"load_mounted_ft_sensor_left:=",
|
||||
load_mounted_ft_sensor_left,
|
||||
" ",
|
||||
"load_mounted_ft_sensor_right:=",
|
||||
load_mounted_ft_sensor_right,
|
||||
" ",
|
||||
"ros2_control:=true ",
|
||||
"rdk_control_mode:=",
|
||||
rdk_control_mode,
|
||||
" ",
|
||||
"use_fake_hardware:=",
|
||||
use_fake_hardware,
|
||||
" ",
|
||||
"fake_sensor_commands:=",
|
||||
fake_sensor_commands,
|
||||
" ",
|
||||
"external_axis_type:=",
|
||||
PythonExpression(
|
||||
["'", external_axis_type, "'.lower().replace('-', '_')"]
|
||||
),
|
||||
" ",
|
||||
"external_axis_prefix:=",
|
||||
external_axis_prefix,
|
||||
]
|
||||
),
|
||||
value_type=str,
|
||||
)
|
||||
|
||||
robot_description = {"robot_description": robot_description_content}
|
||||
|
||||
# MoveIt configuration
|
||||
flexiv_srdf_xacro = PathJoinSubstitution(
|
||||
[FindPackageShare("flexiv_moveit_config"), "srdf", "aico2.srdf.xacro"]
|
||||
)
|
||||
|
||||
robot_description_semantic_content = ParameterValue(
|
||||
Command(
|
||||
[
|
||||
PathJoinSubstitution([FindExecutable(name="xacro")]),
|
||||
" ",
|
||||
flexiv_srdf_xacro,
|
||||
" ",
|
||||
"robot_sn_left:=",
|
||||
robot_sn_left,
|
||||
" ",
|
||||
"robot_sn_right:=",
|
||||
robot_sn_right,
|
||||
" ",
|
||||
"load_gripper_left:=",
|
||||
load_gripper_left,
|
||||
" ",
|
||||
"load_gripper_right:=",
|
||||
load_gripper_right,
|
||||
" ",
|
||||
"load_mounted_ft_sensor_left:=",
|
||||
load_mounted_ft_sensor_left,
|
||||
" ",
|
||||
"load_mounted_ft_sensor_right:=",
|
||||
load_mounted_ft_sensor_right,
|
||||
" ",
|
||||
"arm_prefix_left:=",
|
||||
"left_",
|
||||
" ",
|
||||
"arm_prefix_right:=",
|
||||
"right_",
|
||||
" ",
|
||||
"external_axis_type:=",
|
||||
PythonExpression(
|
||||
["'", external_axis_type, "'.lower().replace('-', '_')"]
|
||||
),
|
||||
" ",
|
||||
"external_axis_prefix:=",
|
||||
external_axis_prefix,
|
||||
]
|
||||
),
|
||||
value_type=str,
|
||||
)
|
||||
robot_description_semantic = {
|
||||
"robot_description_semantic": robot_description_semantic_content
|
||||
}
|
||||
|
||||
publish_robot_description_semantic = {"publish_robot_description_semantic": True}
|
||||
|
||||
# Trajectory Execution Configuration
|
||||
replacements = {
|
||||
"$(var prefix_left)": prefix_left_str,
|
||||
"$(var prefix_right)": prefix_right_str,
|
||||
"$(var external_axis_prefix)": external_axis_prefix_str,
|
||||
}
|
||||
|
||||
robot_description_kinematics_yaml = load_yaml(
|
||||
"flexiv_moveit_config", "config/aico/aico2_kinematics.yaml", replacements
|
||||
)
|
||||
robot_description_kinematics = {
|
||||
"robot_description_kinematics": robot_description_kinematics_yaml
|
||||
}
|
||||
|
||||
# Planning Configuration
|
||||
ompl_planning_pipeline_config = {
|
||||
"move_group": {
|
||||
"planning_plugin": "ompl_interface/OMPLPlanner",
|
||||
"request_adapters": "default_planner_request_adapters/AddTimeOptimalParameterization "
|
||||
"default_planner_request_adapters/ResolveConstraintFrames "
|
||||
"default_planner_request_adapters/FixWorkspaceBounds "
|
||||
"default_planner_request_adapters/FixStartStateBounds "
|
||||
"default_planner_request_adapters/FixStartStateCollision "
|
||||
"default_planner_request_adapters/FixStartStatePathConstraints",
|
||||
"start_state_max_bounds_error": 0.1,
|
||||
}
|
||||
}
|
||||
ompl_planning_yaml = load_yaml(
|
||||
"flexiv_moveit_config", "config/aico/aico2_ompl_planning.yaml", replacements
|
||||
)
|
||||
ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
|
||||
|
||||
controllers_file = "config/aico/aico2_4_v1_moveit_controllers.yaml"
|
||||
if external_axis_type_str == "AICO2-10-V1":
|
||||
controllers_file = "config/aico/aico2_10_v1_moveit_controllers.yaml"
|
||||
elif external_axis_type_str == "AICO2-4-V2":
|
||||
controllers_file = "config/aico/aico2_4_v2_moveit_controllers.yaml"
|
||||
|
||||
moveit_simple_controllers_yaml = load_yaml(
|
||||
"flexiv_moveit_config",
|
||||
controllers_file,
|
||||
replacements,
|
||||
)
|
||||
|
||||
moveit_controllers = {
|
||||
"moveit_simple_controller_manager": moveit_simple_controllers_yaml,
|
||||
"moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager",
|
||||
}
|
||||
|
||||
trajectory_execution = {
|
||||
"moveit_manage_controllers": False,
|
||||
"trajectory_execution.allowed_execution_duration_scaling": 1.2,
|
||||
"trajectory_execution.allowed_goal_duration_margin": 0.5,
|
||||
"trajectory_execution.allowed_start_tolerance": 0.01,
|
||||
}
|
||||
|
||||
planning_scene_monitor_parameters = {
|
||||
"publish_planning_scene": True,
|
||||
"publish_geometry_updates": True,
|
||||
"publish_state_updates": True,
|
||||
"publish_transforms_updates": True,
|
||||
}
|
||||
|
||||
# Load for left arm
|
||||
joint_limits_left = load_yaml(
|
||||
"flexiv_moveit_config",
|
||||
"config/joint_limits.yaml",
|
||||
{"$(var robot_sn)": prefix_left_str.rstrip("_")},
|
||||
)
|
||||
joint_limits_right = load_yaml(
|
||||
"flexiv_moveit_config",
|
||||
"config/joint_limits.yaml",
|
||||
{"$(var robot_sn)": prefix_right_str.rstrip("_")},
|
||||
)
|
||||
|
||||
# Load platform joint limits
|
||||
external_axis_joint_limits = load_yaml(
|
||||
"flexiv_moveit_config",
|
||||
"config/aico/aico_joint_limits.yaml",
|
||||
{"$(var external_axis_prefix)": external_axis_prefix_str},
|
||||
)
|
||||
|
||||
joint_limits_yaml = {"robot_description_planning": {"joint_limits": {}}}
|
||||
|
||||
if joint_limits_left and "joint_limits" in joint_limits_left:
|
||||
joint_limits_yaml["robot_description_planning"]["joint_limits"].update(
|
||||
joint_limits_left["joint_limits"]
|
||||
)
|
||||
|
||||
if joint_limits_right and "joint_limits" in joint_limits_right:
|
||||
joint_limits_yaml["robot_description_planning"]["joint_limits"].update(
|
||||
joint_limits_right["joint_limits"]
|
||||
)
|
||||
|
||||
if external_axis_joint_limits and "joint_limits" in external_axis_joint_limits:
|
||||
joint_limits_yaml["robot_description_planning"]["joint_limits"].update(
|
||||
external_axis_joint_limits["joint_limits"]
|
||||
)
|
||||
|
||||
warehouse_ros_config = {
|
||||
"warehouse_plugin": "warehouse_ros_sqlite::DatabaseConnection",
|
||||
"warehouse_host": warehouse_sqlite_path,
|
||||
}
|
||||
|
||||
# Start the actual move_group node/action server
|
||||
move_group_node = Node(
|
||||
package="moveit_ros_move_group",
|
||||
executable="move_group",
|
||||
output="screen",
|
||||
parameters=[
|
||||
robot_description,
|
||||
robot_description_semantic,
|
||||
publish_robot_description_semantic,
|
||||
robot_description_kinematics,
|
||||
joint_limits_yaml,
|
||||
ompl_planning_pipeline_config,
|
||||
trajectory_execution,
|
||||
moveit_controllers,
|
||||
planning_scene_monitor_parameters,
|
||||
warehouse_ros_config,
|
||||
],
|
||||
)
|
||||
|
||||
# RViz with MoveIt configuration
|
||||
rviz_config_file = PathJoinSubstitution(
|
||||
[FindPackageShare("flexiv_moveit_config"), "rviz", "moveit.rviz"]
|
||||
)
|
||||
|
||||
rviz_node = Node(
|
||||
package="rviz2",
|
||||
executable="rviz2",
|
||||
name="rviz2_moveit",
|
||||
output="log",
|
||||
arguments=["-d", rviz_config_file],
|
||||
parameters=[
|
||||
robot_description,
|
||||
robot_description_semantic,
|
||||
ompl_planning_pipeline_config,
|
||||
robot_description_kinematics,
|
||||
joint_limits_yaml,
|
||||
warehouse_ros_config,
|
||||
],
|
||||
condition=IfCondition(start_rviz),
|
||||
)
|
||||
|
||||
# Publish TF
|
||||
robot_state_publisher_node = Node(
|
||||
package="robot_state_publisher",
|
||||
executable="robot_state_publisher",
|
||||
name="robot_state_publisher",
|
||||
output="both",
|
||||
parameters=[robot_description],
|
||||
)
|
||||
|
||||
# Robot controllers
|
||||
ros2_controllers_file = "aico2_4_v1_controllers.yaml"
|
||||
if external_axis_type_str == "AICO2-10-V1":
|
||||
ros2_controllers_file = "aico2_10_v1_controllers.yaml"
|
||||
elif external_axis_type_str == "AICO2-4-V2":
|
||||
ros2_controllers_file = "aico2_4_v2_controllers.yaml"
|
||||
robot_controllers = PathJoinSubstitution(
|
||||
[FindPackageShare("flexiv_bringup"), "config", ros2_controllers_file]
|
||||
)
|
||||
|
||||
# Run controller manager
|
||||
ros2_control_node = Node(
|
||||
package="controller_manager",
|
||||
executable="ros2_control_node",
|
||||
parameters=[
|
||||
robot_description,
|
||||
ParameterFile(robot_controllers, allow_substs=True),
|
||||
{"robot_sn_left": robot_sn_left},
|
||||
{"robot_sn_right": robot_sn_right},
|
||||
{"prefix_left": prefix_left_str},
|
||||
{"prefix_right": prefix_right_str},
|
||||
{"rdk_control_mode": rdk_control_mode},
|
||||
],
|
||||
remappings=[("joint_states", "flexiv_dual_arm/joint_states")],
|
||||
output="both",
|
||||
)
|
||||
|
||||
# Joint state publisher
|
||||
joint_state_publisher_node = Node(
|
||||
package="joint_state_publisher",
|
||||
executable="joint_state_publisher",
|
||||
name="joint_state_publisher",
|
||||
parameters=[
|
||||
{
|
||||
"source_list": [
|
||||
"flexiv_dual_arm/joint_states",
|
||||
"left_gripper_node/gripper_joint_states",
|
||||
"right_gripper_node/gripper_joint_states",
|
||||
],
|
||||
"rate": 30,
|
||||
}
|
||||
],
|
||||
)
|
||||
|
||||
# Run robot controller
|
||||
left_rizon_arm_controller_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=[
|
||||
"left_rizon_arm_controller",
|
||||
"--controller-manager",
|
||||
"/controller_manager",
|
||||
],
|
||||
)
|
||||
|
||||
right_rizon_arm_controller_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=[
|
||||
"right_rizon_arm_controller",
|
||||
"--controller-manager",
|
||||
"/controller_manager",
|
||||
],
|
||||
)
|
||||
|
||||
# Run joint state broadcaster
|
||||
joint_state_broadcaster_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=[
|
||||
"joint_state_broadcaster",
|
||||
"--controller-manager",
|
||||
"/controller_manager",
|
||||
],
|
||||
)
|
||||
|
||||
# Run Flexiv robot states broadcaster
|
||||
flexiv_robot_states_broadcaster_left_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=["flexiv_robot_states_broadcaster_left"],
|
||||
condition=UnlessCondition(use_fake_hardware),
|
||||
)
|
||||
|
||||
flexiv_robot_states_broadcaster_right_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=["flexiv_robot_states_broadcaster_right"],
|
||||
condition=UnlessCondition(use_fake_hardware),
|
||||
)
|
||||
|
||||
load_gripper_left_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
PathJoinSubstitution(
|
||||
[
|
||||
FindPackageShare("flexiv_gripper"),
|
||||
"launch",
|
||||
"flexiv_gripper.launch.py",
|
||||
]
|
||||
)
|
||||
),
|
||||
launch_arguments={
|
||||
"robot_sn": robot_sn_left,
|
||||
"gripper_name": gripper_name_left,
|
||||
"use_fake_hardware": use_fake_hardware,
|
||||
"gripper_node_name": "left_gripper_node",
|
||||
"use_lite_rdk": "true",
|
||||
}.items(),
|
||||
condition=IfCondition(load_gripper_left),
|
||||
)
|
||||
|
||||
load_gripper_right_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
PathJoinSubstitution(
|
||||
[
|
||||
FindPackageShare("flexiv_gripper"),
|
||||
"launch",
|
||||
"flexiv_gripper.launch.py",
|
||||
]
|
||||
)
|
||||
),
|
||||
launch_arguments={
|
||||
"robot_sn": robot_sn_right,
|
||||
"gripper_name": gripper_name_right,
|
||||
"use_fake_hardware": use_fake_hardware,
|
||||
"gripper_node_name": "right_gripper_node",
|
||||
"use_lite_rdk": "true",
|
||||
}.items(),
|
||||
condition=IfCondition(load_gripper_right),
|
||||
)
|
||||
|
||||
left_gripper_ready_waiter = Node(
|
||||
package="flexiv_gripper",
|
||||
executable="wait_for_gripper_ready",
|
||||
name="wait_for_left_gripper_ready",
|
||||
parameters=[{"ready_topic": "/left_gripper_node/ready"}],
|
||||
output="screen",
|
||||
condition=IfCondition(left_gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
right_gripper_ready_waiter = Node(
|
||||
package="flexiv_gripper",
|
||||
executable="wait_for_gripper_ready",
|
||||
name="wait_for_right_gripper_ready",
|
||||
parameters=[{"ready_topic": "/right_gripper_node/ready"}],
|
||||
output="screen",
|
||||
condition=IfCondition(right_gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
def launch_controller_after_gripper_ready(controller_spawner, gripper_node_name):
|
||||
def handle_gripper_ready(event, context):
|
||||
if event.returncode == 0:
|
||||
return [controller_spawner]
|
||||
return [
|
||||
EmitEvent(
|
||||
event=Shutdown(reason=f"{gripper_node_name} did not report ready")
|
||||
)
|
||||
]
|
||||
|
||||
return handle_gripper_ready
|
||||
|
||||
# Run gpio controllers
|
||||
gpio_controller_left_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=[
|
||||
"gpio_controller_left",
|
||||
"--controller-manager",
|
||||
"/controller_manager",
|
||||
],
|
||||
condition=UnlessCondition(use_fake_hardware),
|
||||
)
|
||||
gpio_controller_right_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=[
|
||||
"gpio_controller_right",
|
||||
"--controller-manager",
|
||||
"/controller_manager",
|
||||
],
|
||||
condition=UnlessCondition(use_fake_hardware),
|
||||
)
|
||||
|
||||
# Delay start of controllers after `joint_state_broadcaster`
|
||||
delay_left_controller_after_jsb = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=joint_state_broadcaster_spawner,
|
||||
on_exit=[left_rizon_arm_controller_spawner],
|
||||
),
|
||||
condition=UnlessCondition(left_gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
delay_left_gripper_launch_after_joint_state_broadcaster_spawner = (
|
||||
RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=joint_state_broadcaster_spawner,
|
||||
on_exit=[load_gripper_left_launch, left_gripper_ready_waiter],
|
||||
),
|
||||
condition=IfCondition(left_gripper_ready_gate_condition),
|
||||
)
|
||||
)
|
||||
|
||||
delay_right_gripper_launch_after_joint_state_broadcaster_spawner = (
|
||||
RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=joint_state_broadcaster_spawner,
|
||||
on_exit=[load_gripper_right_launch],
|
||||
),
|
||||
condition=IfCondition(right_gripper_ready_gate_condition),
|
||||
)
|
||||
)
|
||||
|
||||
delay_left_controller_after_gripper_ready = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=left_gripper_ready_waiter,
|
||||
on_exit=launch_controller_after_gripper_ready(
|
||||
left_rizon_arm_controller_spawner, "left_gripper_node"
|
||||
),
|
||||
),
|
||||
condition=IfCondition(left_gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
# Delay right controller start after left controller
|
||||
delay_right_controller_after_left_controller = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=left_rizon_arm_controller_spawner,
|
||||
on_exit=[right_rizon_arm_controller_spawner],
|
||||
),
|
||||
condition=UnlessCondition(right_gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
delay_right_gripper_ready_waiter_after_left_controller = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=left_rizon_arm_controller_spawner,
|
||||
on_exit=[right_gripper_ready_waiter],
|
||||
),
|
||||
condition=IfCondition(right_gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
delay_right_controller_after_gripper_ready = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=right_gripper_ready_waiter,
|
||||
on_exit=launch_controller_after_gripper_ready(
|
||||
right_rizon_arm_controller_spawner, "right_gripper_node"
|
||||
),
|
||||
),
|
||||
condition=IfCondition(right_gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
# Delay rviz start after right controller
|
||||
delay_rviz_after_right_controller = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=right_rizon_arm_controller_spawner,
|
||||
on_exit=[rviz_node],
|
||||
)
|
||||
)
|
||||
|
||||
nodes = [
|
||||
set_prefix_left,
|
||||
set_prefix_right,
|
||||
move_group_node,
|
||||
robot_state_publisher_node,
|
||||
ros2_control_node,
|
||||
joint_state_publisher_node,
|
||||
joint_state_broadcaster_spawner,
|
||||
flexiv_robot_states_broadcaster_left_spawner,
|
||||
flexiv_robot_states_broadcaster_right_spawner,
|
||||
gpio_controller_left_spawner,
|
||||
gpio_controller_right_spawner,
|
||||
delay_left_gripper_launch_after_joint_state_broadcaster_spawner,
|
||||
delay_right_gripper_launch_after_joint_state_broadcaster_spawner,
|
||||
delay_left_controller_after_jsb,
|
||||
delay_left_controller_after_gripper_ready,
|
||||
delay_right_controller_after_left_controller,
|
||||
delay_right_gripper_ready_waiter_after_left_controller,
|
||||
delay_right_controller_after_gripper_ready,
|
||||
delay_rviz_after_right_controller,
|
||||
]
|
||||
|
||||
return nodes
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
declared_arguments = []
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"rizon_type",
|
||||
default_value="Rizon4",
|
||||
description="Type of the Flexiv Rizon robot.",
|
||||
choices=["Rizon4", "Rizon10"],
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"robot_sn_left",
|
||||
description="Serial number of the left robot.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"robot_sn_right",
|
||||
description="Serial number of the right robot.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"rdk_control_mode",
|
||||
default_value="joint_position",
|
||||
description="RDK control mode for the ROS 2 control joint position and velocity interfaces.",
|
||||
choices=["joint_position", "joint_impedance"],
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"start_rviz",
|
||||
default_value="true",
|
||||
description="Start RViz automatically with the launch file",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"use_fake_hardware",
|
||||
default_value="false",
|
||||
description="Start robot with fake hardware mirroring command to its states.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"fake_sensor_commands",
|
||||
default_value="false",
|
||||
description="Enable fake command interfaces for sensors used for simple simulations.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"load_gripper_left",
|
||||
default_value="false",
|
||||
description="Flag to load the Flexiv Grav gripper as the end-effector of the left robot.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"gripper_name_left",
|
||||
default_value="Flexiv-GN01",
|
||||
description="Full name of the left gripper to be controlled, can be found in Flexiv Elements -> Settings -> Device",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"load_gripper_right",
|
||||
default_value="false",
|
||||
description="Flag to load the Flexiv Grav gripper as the end-effector of the right robot.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"gripper_name_right",
|
||||
default_value="Flexiv-GN01",
|
||||
description="Full name of the right gripper to be controlled, can be found in Flexiv Elements -> Settings -> Device",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"load_mounted_ft_sensor_left",
|
||||
default_value="false",
|
||||
description="Flag to load the mounted force torque sensor for the left robot. Only available for Rizon4, Rizon4R and Rizon10.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"load_mounted_ft_sensor_right",
|
||||
default_value="false",
|
||||
description="Flag to load the mounted force torque sensor for the right robot. Only available for Rizon4, Rizon4R and Rizon10.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"external_axis_type",
|
||||
default_value="AICO2-4-V1",
|
||||
description="Type of the AICO2 platform.",
|
||||
choices=["AICO2-4-V1", "AICO2-4-V2", "AICO2-10-V1"],
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"external_axis_prefix",
|
||||
default_value="",
|
||||
description="Prefix for the external axis links and joints.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"warehouse_sqlite_path",
|
||||
default_value=os.path.expanduser("~/.ros/warehouse_ros.sqlite"),
|
||||
description="Path to the warehouse database",
|
||||
)
|
||||
)
|
||||
|
||||
return LaunchDescription(
|
||||
declared_arguments + [OpaqueFunction(function=launch_setup)]
|
||||
)
|
||||
@@ -0,0 +1,366 @@
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import (
|
||||
DeclareLaunchArgument,
|
||||
EmitEvent,
|
||||
IncludeLaunchDescription,
|
||||
RegisterEventHandler,
|
||||
)
|
||||
from launch.conditions import IfCondition, UnlessCondition
|
||||
from launch.events import Shutdown
|
||||
from launch.event_handlers import OnProcessExit
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.parameter_descriptions import ParameterFile, ParameterValue
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
from launch.substitutions import (
|
||||
Command,
|
||||
FindExecutable,
|
||||
LaunchConfiguration,
|
||||
PathJoinSubstitution,
|
||||
PythonExpression,
|
||||
)
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
rizon_type_param_name = "rizon_type"
|
||||
robot_sn_param_name = "robot_sn"
|
||||
rdk_control_mode_param_name = "rdk_control_mode"
|
||||
start_rviz_param_name = "start_rviz"
|
||||
load_gripper_param_name = "load_gripper"
|
||||
gripper_name_param_name = "gripper_name"
|
||||
load_mounted_ft_sensor_param_name = "load_mounted_ft_sensor"
|
||||
use_fake_hardware_param_name = "use_fake_hardware"
|
||||
fake_sensor_commands_param_name = "fake_sensor_commands"
|
||||
robot_controller_param_name = "robot_controller"
|
||||
|
||||
# Declare arguments
|
||||
declared_arguments = []
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
rizon_type_param_name,
|
||||
description="Type of the Flexiv Rizon robot.",
|
||||
default_value="Rizon4",
|
||||
choices=["Rizon4", "Rizon4M", "Rizon4R", "Rizon4s", "Rizon10", "Rizon10s"],
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
robot_sn_param_name,
|
||||
description="Serial number of the robot to connect to. Remove any space, for example: Rizon4s-123456",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
rdk_control_mode_param_name,
|
||||
default_value="joint_position",
|
||||
description="RDK control mode for the ROS 2 control joint position and velocity interfaces. Options: joint_position, joint_impedance",
|
||||
choices=["joint_position", "joint_impedance"],
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
start_rviz_param_name,
|
||||
default_value="true",
|
||||
description="Start RViz automatically with the launch file",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
load_gripper_param_name,
|
||||
default_value="false",
|
||||
description="Flag to load the Flexiv Grav gripper as the end-effector of the robot.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
gripper_name_param_name,
|
||||
default_value="Flexiv-GN01",
|
||||
description="Full name of the gripper to be controlled, can be found in Flexiv Elements -> Settings -> Device",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
load_mounted_ft_sensor_param_name,
|
||||
default_value="false",
|
||||
description="Flag to load the mounted force torque sensor. Only available for Rizon4, Rizon4R and Rizon10.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
use_fake_hardware_param_name,
|
||||
default_value="false",
|
||||
description="Start robot with fake hardware mirroring command to its states.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
fake_sensor_commands_param_name,
|
||||
default_value="false",
|
||||
description="Enable fake command interfaces for sensors used for simple simulations. \
|
||||
Used only if 'use_fake_hardware' parameter is true.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
robot_controller_param_name,
|
||||
default_value="rizon_arm_controller",
|
||||
description="Robot controller to start. Available: rizon_arm_controller",
|
||||
)
|
||||
)
|
||||
|
||||
# Initialize Arguments
|
||||
rizon_type = LaunchConfiguration(rizon_type_param_name)
|
||||
robot_sn = LaunchConfiguration(robot_sn_param_name)
|
||||
rdk_control_mode = LaunchConfiguration(rdk_control_mode_param_name)
|
||||
start_rviz = LaunchConfiguration(start_rviz_param_name)
|
||||
load_gripper = LaunchConfiguration(load_gripper_param_name)
|
||||
gripper_name = LaunchConfiguration(gripper_name_param_name)
|
||||
load_mounted_ft_sensor = LaunchConfiguration(load_mounted_ft_sensor_param_name)
|
||||
use_fake_hardware = LaunchConfiguration(use_fake_hardware_param_name)
|
||||
fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_param_name)
|
||||
robot_controller = LaunchConfiguration(robot_controller_param_name)
|
||||
gripper_ready_gate_condition = PythonExpression(
|
||||
[
|
||||
"'",
|
||||
load_gripper,
|
||||
"'.lower() in ['true', '1'] and '",
|
||||
use_fake_hardware,
|
||||
"'.lower() not in ['true', '1']",
|
||||
]
|
||||
)
|
||||
|
||||
# Get URDF via xacro
|
||||
flexiv_urdf_xacro = PathJoinSubstitution(
|
||||
[FindPackageShare("flexiv_description"), "urdf", "rizon.urdf.xacro"]
|
||||
)
|
||||
|
||||
# Get URDF via xacro
|
||||
robot_description_content = ParameterValue(
|
||||
Command(
|
||||
[
|
||||
PathJoinSubstitution([FindExecutable(name="xacro")]),
|
||||
" ",
|
||||
flexiv_urdf_xacro,
|
||||
" ",
|
||||
"robot_sn:=",
|
||||
robot_sn,
|
||||
" ",
|
||||
"rizon_type:=",
|
||||
rizon_type,
|
||||
" ",
|
||||
"ros2_control:=true ",
|
||||
"rdk_control_mode:=",
|
||||
rdk_control_mode,
|
||||
" ",
|
||||
"load_gripper:=",
|
||||
load_gripper,
|
||||
" ",
|
||||
"gripper_name:=",
|
||||
gripper_name,
|
||||
" ",
|
||||
"load_mounted_ft_sensor:=",
|
||||
load_mounted_ft_sensor,
|
||||
" ",
|
||||
"use_fake_hardware:=",
|
||||
use_fake_hardware,
|
||||
" ",
|
||||
"fake_sensor_commands:=",
|
||||
fake_sensor_commands,
|
||||
]
|
||||
),
|
||||
value_type=str,
|
||||
)
|
||||
|
||||
robot_description = {"robot_description": robot_description_content}
|
||||
|
||||
# RViZ
|
||||
rviz_config_file = PathJoinSubstitution(
|
||||
[FindPackageShare("flexiv_description"), "rviz", "view_rizon.rviz"]
|
||||
)
|
||||
|
||||
rviz_node = Node(
|
||||
package="rviz2",
|
||||
executable="rviz2",
|
||||
name="rviz2",
|
||||
output="log",
|
||||
arguments=["-d", rviz_config_file],
|
||||
condition=IfCondition(start_rviz),
|
||||
)
|
||||
|
||||
# Robot controllers
|
||||
robot_controllers = PathJoinSubstitution(
|
||||
[FindPackageShare("flexiv_bringup"), "config", "rizon_controllers.yaml"]
|
||||
)
|
||||
|
||||
# Controller Manager
|
||||
ros2_control_node = Node(
|
||||
package="controller_manager",
|
||||
executable="ros2_control_node",
|
||||
parameters=[
|
||||
robot_description,
|
||||
ParameterFile(robot_controllers, allow_substs=True),
|
||||
{"robot_sn": robot_sn},
|
||||
{"rdk_control_mode": rdk_control_mode},
|
||||
],
|
||||
remappings=[("joint_states", "flexiv_rizon_arm/joint_states")],
|
||||
output="both",
|
||||
)
|
||||
|
||||
# Joint state publisher
|
||||
joint_state_publisher_node = Node(
|
||||
package="joint_state_publisher",
|
||||
executable="joint_state_publisher",
|
||||
name="joint_state_publisher",
|
||||
parameters=[
|
||||
{
|
||||
"source_list": [
|
||||
"flexiv_rizon_arm/joint_states",
|
||||
"flexiv_gripper_node/gripper_joint_states",
|
||||
],
|
||||
"rate": 30,
|
||||
}
|
||||
],
|
||||
)
|
||||
|
||||
# Robot state publisher
|
||||
robot_state_publisher_node = Node(
|
||||
package="robot_state_publisher",
|
||||
executable="robot_state_publisher",
|
||||
name="robot_state_publisher",
|
||||
output="both",
|
||||
parameters=[robot_description],
|
||||
)
|
||||
|
||||
# Run robot controller
|
||||
robot_controller_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=[robot_controller, "--controller-manager", "/controller_manager"],
|
||||
)
|
||||
|
||||
# Run joint state broadcaster
|
||||
joint_state_broadcaster_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=[
|
||||
"joint_state_broadcaster",
|
||||
"--controller-manager",
|
||||
"/controller_manager",
|
||||
],
|
||||
)
|
||||
|
||||
# Run Flexiv robot states broadcaster
|
||||
flexiv_robot_states_broadcaster_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=["flexiv_robot_states_broadcaster"],
|
||||
parameters=[{"robot_sn": robot_sn}],
|
||||
condition=UnlessCondition(use_fake_hardware),
|
||||
)
|
||||
|
||||
# Include gripper launch file
|
||||
load_gripper_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
PathJoinSubstitution(
|
||||
[
|
||||
FindPackageShare("flexiv_gripper"),
|
||||
"launch",
|
||||
"flexiv_gripper.launch.py",
|
||||
]
|
||||
)
|
||||
),
|
||||
launch_arguments={
|
||||
"robot_sn": robot_sn,
|
||||
"gripper_name": gripper_name,
|
||||
"use_fake_hardware": use_fake_hardware,
|
||||
"use_lite_rdk": "true",
|
||||
}.items(),
|
||||
condition=IfCondition(load_gripper),
|
||||
)
|
||||
|
||||
gripper_ready_waiter = Node(
|
||||
package="flexiv_gripper",
|
||||
executable="wait_for_gripper_ready",
|
||||
name="wait_for_gripper_ready",
|
||||
parameters=[{"ready_topic": "/flexiv_gripper_node/ready"}],
|
||||
output="screen",
|
||||
condition=IfCondition(gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
def launch_robot_controller_after_gripper_ready(event, context):
|
||||
if event.returncode == 0:
|
||||
return [robot_controller_spawner]
|
||||
return [
|
||||
EmitEvent(event=Shutdown(reason="flexiv_gripper_node did not report ready"))
|
||||
]
|
||||
|
||||
# Run gpio controller
|
||||
gpio_controller_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=["gpio_controller", "--controller-manager", "/controller_manager"],
|
||||
parameters=[{"robot_sn": robot_sn}],
|
||||
condition=UnlessCondition(use_fake_hardware),
|
||||
)
|
||||
|
||||
# Delay start of robot_controller after `joint_state_broadcaster`
|
||||
delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = (
|
||||
RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=joint_state_broadcaster_spawner,
|
||||
on_exit=[robot_controller_spawner],
|
||||
),
|
||||
condition=UnlessCondition(gripper_ready_gate_condition),
|
||||
)
|
||||
)
|
||||
|
||||
# Start gripper only after ros2_control has activated and the joint state broadcaster is up.
|
||||
delay_gripper_launch_after_joint_state_broadcaster_spawner = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=joint_state_broadcaster_spawner,
|
||||
on_exit=[load_gripper_launch],
|
||||
),
|
||||
condition=IfCondition(gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
delay_robot_controller_spawner_after_gripper_ready = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=gripper_ready_waiter,
|
||||
on_exit=launch_robot_controller_after_gripper_ready,
|
||||
),
|
||||
condition=IfCondition(gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
# Delay rviz start after `robot_controller_spawner`
|
||||
delay_rviz_after_robot_controller_spawner = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=robot_controller_spawner,
|
||||
on_exit=[rviz_node],
|
||||
)
|
||||
)
|
||||
|
||||
nodes = [
|
||||
ros2_control_node,
|
||||
joint_state_publisher_node,
|
||||
robot_state_publisher_node,
|
||||
gripper_ready_waiter,
|
||||
joint_state_broadcaster_spawner,
|
||||
flexiv_robot_states_broadcaster_spawner,
|
||||
gpio_controller_spawner,
|
||||
delay_gripper_launch_after_joint_state_broadcaster_spawner,
|
||||
delay_robot_controller_spawner_after_joint_state_broadcaster_spawner,
|
||||
delay_robot_controller_spawner_after_gripper_ready,
|
||||
delay_rviz_after_robot_controller_spawner,
|
||||
]
|
||||
|
||||
return LaunchDescription(declared_arguments + nodes)
|
||||
@@ -0,0 +1,562 @@
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import (
|
||||
DeclareLaunchArgument,
|
||||
EmitEvent,
|
||||
IncludeLaunchDescription,
|
||||
RegisterEventHandler,
|
||||
)
|
||||
from launch.conditions import IfCondition, UnlessCondition
|
||||
from launch.events import Shutdown
|
||||
from launch.event_handlers import OnProcessExit
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.parameter_descriptions import ParameterFile, ParameterValue
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
from launch.substitutions import (
|
||||
Command,
|
||||
FindExecutable,
|
||||
LaunchConfiguration,
|
||||
PathJoinSubstitution,
|
||||
PythonExpression,
|
||||
)
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
rizon_type_left_param_name = "rizon_type_left"
|
||||
rizon_type_right_param_name = "rizon_type_right"
|
||||
robot_sn_left_param_name = "robot_sn_left"
|
||||
robot_sn_right_param_name = "robot_sn_right"
|
||||
rdk_control_mode_param_name = "rdk_control_mode"
|
||||
start_rviz_param_name = "start_rviz"
|
||||
use_fake_hardware_param_name = "use_fake_hardware"
|
||||
fake_sensor_commands_param_name = "fake_sensor_commands"
|
||||
load_gripper_left_param_name = "load_gripper_left"
|
||||
gripper_name_left_param_name = "gripper_name_left"
|
||||
load_gripper_right_param_name = "load_gripper_right"
|
||||
gripper_name_right_param_name = "gripper_name_right"
|
||||
load_mounted_ft_sensor_left_param_name = "load_mounted_ft_sensor_left"
|
||||
load_mounted_ft_sensor_right_param_name = "load_mounted_ft_sensor_right"
|
||||
|
||||
# Declare arguments
|
||||
declared_arguments = []
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
rizon_type_left_param_name,
|
||||
description="Type of the left Flexiv Rizon robot.",
|
||||
default_value="Rizon4",
|
||||
choices=["Rizon4", "Rizon4M", "Rizon4R", "Rizon4s", "Rizon10", "Rizon10s"],
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
rizon_type_right_param_name,
|
||||
description="Type of the right Flexiv Rizon robot.",
|
||||
default_value="Rizon4R",
|
||||
choices=["Rizon4", "Rizon4M", "Rizon4R", "Rizon4s", "Rizon10", "Rizon10s"],
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
robot_sn_left_param_name,
|
||||
description="Serial number of the left robot.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
robot_sn_right_param_name,
|
||||
description="Serial number of the right robot.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
rdk_control_mode_param_name,
|
||||
default_value="joint_position",
|
||||
description="RDK control mode for the ROS 2 control joint position and velocity interfaces.",
|
||||
choices=["joint_position", "joint_impedance"],
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
start_rviz_param_name,
|
||||
default_value="true",
|
||||
description="Start RViz automatically with the launch file",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
use_fake_hardware_param_name,
|
||||
default_value="false",
|
||||
description="Start robot with fake hardware mirroring command to its states.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
fake_sensor_commands_param_name,
|
||||
default_value="false",
|
||||
description="Enable fake command interfaces for sensors used for simple simulations.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
load_gripper_left_param_name,
|
||||
default_value="false",
|
||||
description="Flag to load the Flexiv Grav gripper as the end-effector of the left robot.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
gripper_name_left_param_name,
|
||||
default_value="Flexiv-GN01",
|
||||
description="Full name of the left gripper to be controlled, can be found in Flexiv Elements -> Settings -> Device",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
load_gripper_right_param_name,
|
||||
default_value="false",
|
||||
description="Flag to load the Flexiv Grav gripper as the end-effector of the right robot.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
gripper_name_right_param_name,
|
||||
default_value="Flexiv-GN01",
|
||||
description="Full name of the right gripper to be controlled, can be found in Flexiv Elements -> Settings -> Device",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
load_mounted_ft_sensor_left_param_name,
|
||||
default_value="false",
|
||||
description="Flag to load the mounted force torque sensor for the left robot. Only available for Rizon4, Rizon4R and Rizon10.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
load_mounted_ft_sensor_right_param_name,
|
||||
default_value="false",
|
||||
description="Flag to load the mounted force torque sensor for the right robot. Only available for Rizon4, Rizon4R and Rizon10.",
|
||||
)
|
||||
)
|
||||
|
||||
# Initialize Arguments
|
||||
rizon_type_left = LaunchConfiguration(rizon_type_left_param_name)
|
||||
rizon_type_right = LaunchConfiguration(rizon_type_right_param_name)
|
||||
robot_sn_left = LaunchConfiguration(robot_sn_left_param_name)
|
||||
robot_sn_right = LaunchConfiguration(robot_sn_right_param_name)
|
||||
rdk_control_mode = LaunchConfiguration(rdk_control_mode_param_name)
|
||||
start_rviz = LaunchConfiguration(start_rviz_param_name)
|
||||
use_fake_hardware = LaunchConfiguration(use_fake_hardware_param_name)
|
||||
fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_param_name)
|
||||
load_gripper_left = LaunchConfiguration(load_gripper_left_param_name)
|
||||
gripper_name_left = LaunchConfiguration(gripper_name_left_param_name)
|
||||
load_gripper_right = LaunchConfiguration(load_gripper_right_param_name)
|
||||
gripper_name_right = LaunchConfiguration(gripper_name_right_param_name)
|
||||
load_mounted_ft_sensor_left = LaunchConfiguration(
|
||||
load_mounted_ft_sensor_left_param_name
|
||||
)
|
||||
load_mounted_ft_sensor_right = LaunchConfiguration(
|
||||
load_mounted_ft_sensor_right_param_name
|
||||
)
|
||||
left_gripper_ready_gate_condition = PythonExpression(
|
||||
[
|
||||
"'",
|
||||
load_gripper_left,
|
||||
"'.lower() in ['true', '1'] and '",
|
||||
use_fake_hardware,
|
||||
"'.lower() not in ['true', '1']",
|
||||
]
|
||||
)
|
||||
right_gripper_ready_gate_condition = PythonExpression(
|
||||
[
|
||||
"'",
|
||||
load_gripper_right,
|
||||
"'.lower() in ['true', '1'] and '",
|
||||
use_fake_hardware,
|
||||
"'.lower() not in ['true', '1']",
|
||||
]
|
||||
)
|
||||
|
||||
# Construct prefixes
|
||||
from launch.actions import SetLaunchConfiguration
|
||||
|
||||
set_prefix_left = SetLaunchConfiguration(
|
||||
name="prefix_left",
|
||||
value=PythonExpression(["'left_' + '", robot_sn_left, "' + '_'"]),
|
||||
)
|
||||
set_prefix_right = SetLaunchConfiguration(
|
||||
name="prefix_right",
|
||||
value=PythonExpression(["'right_' + '", robot_sn_right, "' + '_'"]),
|
||||
)
|
||||
|
||||
# Get URDF via xacro
|
||||
flexiv_urdf_xacro = PathJoinSubstitution(
|
||||
[FindPackageShare("flexiv_description"), "urdf", "rizon_dual.urdf.xacro"]
|
||||
)
|
||||
|
||||
robot_description_content = ParameterValue(
|
||||
Command(
|
||||
[
|
||||
PathJoinSubstitution([FindExecutable(name="xacro")]),
|
||||
" ",
|
||||
flexiv_urdf_xacro,
|
||||
" ",
|
||||
"robot_sn_left:=",
|
||||
robot_sn_left,
|
||||
" ",
|
||||
"robot_sn_right:=",
|
||||
robot_sn_right,
|
||||
" ",
|
||||
"rizon_type_left:=",
|
||||
rizon_type_left,
|
||||
" ",
|
||||
"rizon_type_right:=",
|
||||
rizon_type_right,
|
||||
" ",
|
||||
"load_gripper_left:=",
|
||||
load_gripper_left,
|
||||
" ",
|
||||
"gripper_name_left:=",
|
||||
gripper_name_left,
|
||||
" ",
|
||||
"load_gripper_right:=",
|
||||
load_gripper_right,
|
||||
" ",
|
||||
"gripper_name_right:=",
|
||||
gripper_name_right,
|
||||
" ",
|
||||
"load_mounted_ft_sensor_left:=",
|
||||
load_mounted_ft_sensor_left,
|
||||
" ",
|
||||
"load_mounted_ft_sensor_right:=",
|
||||
load_mounted_ft_sensor_right,
|
||||
" ",
|
||||
"ros2_control:=true ",
|
||||
"rdk_control_mode:=",
|
||||
rdk_control_mode,
|
||||
" ",
|
||||
"use_fake_hardware:=",
|
||||
use_fake_hardware,
|
||||
" ",
|
||||
"fake_sensor_commands:=",
|
||||
fake_sensor_commands,
|
||||
]
|
||||
),
|
||||
value_type=str,
|
||||
)
|
||||
|
||||
robot_description = {"robot_description": robot_description_content}
|
||||
|
||||
# RViZ
|
||||
rviz_config_file = PathJoinSubstitution(
|
||||
[FindPackageShare("flexiv_description"), "rviz", "view_rizon.rviz"]
|
||||
)
|
||||
|
||||
rviz_node = Node(
|
||||
package="rviz2",
|
||||
executable="rviz2",
|
||||
name="rviz2",
|
||||
output="log",
|
||||
arguments=["-d", rviz_config_file],
|
||||
condition=IfCondition(start_rviz),
|
||||
)
|
||||
|
||||
# Robot controllers
|
||||
robot_controllers = PathJoinSubstitution(
|
||||
[FindPackageShare("flexiv_bringup"), "config", "rizon_dual_controllers.yaml"]
|
||||
)
|
||||
|
||||
# Controller Manager
|
||||
ros2_control_node = Node(
|
||||
package="controller_manager",
|
||||
executable="ros2_control_node",
|
||||
parameters=[
|
||||
robot_description,
|
||||
ParameterFile(robot_controllers, allow_substs=True),
|
||||
{"robot_sn_left": robot_sn_left},
|
||||
{"robot_sn_right": robot_sn_right},
|
||||
{"prefix_left": LaunchConfiguration("prefix_left")},
|
||||
{"prefix_right": LaunchConfiguration("prefix_right")},
|
||||
{"rdk_control_mode": rdk_control_mode},
|
||||
],
|
||||
remappings=[("joint_states", "flexiv_dual_arm/joint_states")],
|
||||
output="both",
|
||||
)
|
||||
|
||||
# Joint state publisher
|
||||
joint_state_publisher_node = Node(
|
||||
package="joint_state_publisher",
|
||||
executable="joint_state_publisher",
|
||||
name="joint_state_publisher",
|
||||
parameters=[
|
||||
{
|
||||
"source_list": [
|
||||
"flexiv_dual_arm/joint_states",
|
||||
"left_gripper_node/gripper_joint_states",
|
||||
"right_gripper_node/gripper_joint_states",
|
||||
],
|
||||
"rate": 30,
|
||||
}
|
||||
],
|
||||
)
|
||||
|
||||
# Robot state publisher
|
||||
robot_state_publisher_node = Node(
|
||||
package="robot_state_publisher",
|
||||
executable="robot_state_publisher",
|
||||
name="robot_state_publisher",
|
||||
output="both",
|
||||
parameters=[robot_description],
|
||||
)
|
||||
|
||||
# Run joint state broadcaster
|
||||
joint_state_broadcaster_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=[
|
||||
"joint_state_broadcaster",
|
||||
"--controller-manager",
|
||||
"/controller_manager",
|
||||
],
|
||||
)
|
||||
|
||||
# Run left arm controller
|
||||
left_rizon_arm_controller_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=[
|
||||
"left_rizon_arm_controller",
|
||||
"--controller-manager",
|
||||
"/controller_manager",
|
||||
],
|
||||
)
|
||||
|
||||
# Run right arm controller
|
||||
right_rizon_arm_controller_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=[
|
||||
"right_rizon_arm_controller",
|
||||
"--controller-manager",
|
||||
"/controller_manager",
|
||||
],
|
||||
)
|
||||
|
||||
# Run Flexiv robot states broadcaster left
|
||||
flexiv_robot_states_broadcaster_left_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=["flexiv_robot_states_broadcaster_left"],
|
||||
condition=UnlessCondition(use_fake_hardware),
|
||||
)
|
||||
|
||||
# Run Flexiv robot states broadcaster right
|
||||
flexiv_robot_states_broadcaster_right_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=["flexiv_robot_states_broadcaster_right"],
|
||||
condition=UnlessCondition(use_fake_hardware),
|
||||
)
|
||||
|
||||
# Include gripper launch files
|
||||
load_gripper_left_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
PathJoinSubstitution(
|
||||
[
|
||||
FindPackageShare("flexiv_gripper"),
|
||||
"launch",
|
||||
"flexiv_gripper.launch.py",
|
||||
]
|
||||
)
|
||||
),
|
||||
launch_arguments={
|
||||
"gripper_node_name": "left_gripper_node",
|
||||
"robot_sn": robot_sn_left,
|
||||
"gripper_name": gripper_name_left,
|
||||
"use_fake_hardware": use_fake_hardware,
|
||||
"use_lite_rdk": "true",
|
||||
}.items(),
|
||||
condition=IfCondition(load_gripper_left),
|
||||
)
|
||||
load_gripper_right_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
PathJoinSubstitution(
|
||||
[
|
||||
FindPackageShare("flexiv_gripper"),
|
||||
"launch",
|
||||
"flexiv_gripper.launch.py",
|
||||
]
|
||||
)
|
||||
),
|
||||
launch_arguments={
|
||||
"gripper_node_name": "right_gripper_node",
|
||||
"robot_sn": robot_sn_right,
|
||||
"gripper_name": gripper_name_right,
|
||||
"use_fake_hardware": use_fake_hardware,
|
||||
"use_lite_rdk": "true",
|
||||
}.items(),
|
||||
condition=IfCondition(load_gripper_right),
|
||||
)
|
||||
|
||||
left_gripper_ready_waiter = Node(
|
||||
package="flexiv_gripper",
|
||||
executable="wait_for_gripper_ready",
|
||||
name="wait_for_left_gripper_ready",
|
||||
parameters=[{"ready_topic": "/left_gripper_node/ready"}],
|
||||
output="screen",
|
||||
condition=IfCondition(left_gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
right_gripper_ready_waiter = Node(
|
||||
package="flexiv_gripper",
|
||||
executable="wait_for_gripper_ready",
|
||||
name="wait_for_right_gripper_ready",
|
||||
parameters=[{"ready_topic": "/right_gripper_node/ready"}],
|
||||
output="screen",
|
||||
condition=IfCondition(right_gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
def launch_controller_after_gripper_ready(controller_spawner, gripper_node_name):
|
||||
def handle_gripper_ready(event, context):
|
||||
if event.returncode == 0:
|
||||
return [controller_spawner]
|
||||
return [
|
||||
EmitEvent(
|
||||
event=Shutdown(reason=f"{gripper_node_name} did not report ready")
|
||||
)
|
||||
]
|
||||
|
||||
return handle_gripper_ready
|
||||
|
||||
# Run gpio controllers
|
||||
gpio_controller_left_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=[
|
||||
"gpio_controller_left",
|
||||
"--controller-manager",
|
||||
"/controller_manager",
|
||||
],
|
||||
condition=UnlessCondition(use_fake_hardware),
|
||||
)
|
||||
gpio_controller_right_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=[
|
||||
"gpio_controller_right",
|
||||
"--controller-manager",
|
||||
"/controller_manager",
|
||||
],
|
||||
condition=UnlessCondition(use_fake_hardware),
|
||||
)
|
||||
|
||||
# Delay start of controllers after `joint_state_broadcaster`
|
||||
delay_left_controller_after_jsb = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=joint_state_broadcaster_spawner,
|
||||
on_exit=[left_rizon_arm_controller_spawner],
|
||||
),
|
||||
condition=UnlessCondition(left_gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
delay_left_gripper_launch_after_joint_state_broadcaster_spawner = (
|
||||
RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=joint_state_broadcaster_spawner,
|
||||
on_exit=[load_gripper_left_launch, left_gripper_ready_waiter],
|
||||
),
|
||||
condition=IfCondition(left_gripper_ready_gate_condition),
|
||||
)
|
||||
)
|
||||
|
||||
delay_right_gripper_launch_after_joint_state_broadcaster_spawner = (
|
||||
RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=joint_state_broadcaster_spawner,
|
||||
on_exit=[load_gripper_right_launch],
|
||||
),
|
||||
condition=IfCondition(right_gripper_ready_gate_condition),
|
||||
)
|
||||
)
|
||||
|
||||
delay_left_controller_after_gripper_ready = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=left_gripper_ready_waiter,
|
||||
on_exit=launch_controller_after_gripper_ready(
|
||||
left_rizon_arm_controller_spawner, "left_gripper_node"
|
||||
),
|
||||
),
|
||||
condition=IfCondition(left_gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
# Delay right controller start after left controller
|
||||
delay_right_controller_after_left_controller = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=left_rizon_arm_controller_spawner,
|
||||
on_exit=[right_rizon_arm_controller_spawner],
|
||||
),
|
||||
condition=UnlessCondition(right_gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
delay_right_gripper_ready_waiter_after_left_controller = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=left_rizon_arm_controller_spawner,
|
||||
on_exit=[right_gripper_ready_waiter],
|
||||
),
|
||||
condition=IfCondition(right_gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
delay_right_controller_after_gripper_ready = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=right_gripper_ready_waiter,
|
||||
on_exit=launch_controller_after_gripper_ready(
|
||||
right_rizon_arm_controller_spawner, "right_gripper_node"
|
||||
),
|
||||
),
|
||||
condition=IfCondition(right_gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
# Delay rviz start after right controller
|
||||
delay_rviz_after_right_controller = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=right_rizon_arm_controller_spawner,
|
||||
on_exit=[rviz_node],
|
||||
)
|
||||
)
|
||||
|
||||
nodes = [
|
||||
set_prefix_left,
|
||||
set_prefix_right,
|
||||
ros2_control_node,
|
||||
joint_state_publisher_node,
|
||||
robot_state_publisher_node,
|
||||
joint_state_broadcaster_spawner,
|
||||
flexiv_robot_states_broadcaster_left_spawner,
|
||||
flexiv_robot_states_broadcaster_right_spawner,
|
||||
gpio_controller_left_spawner,
|
||||
gpio_controller_right_spawner,
|
||||
delay_left_gripper_launch_after_joint_state_broadcaster_spawner,
|
||||
delay_right_gripper_launch_after_joint_state_broadcaster_spawner,
|
||||
delay_left_controller_after_jsb,
|
||||
delay_left_controller_after_gripper_ready,
|
||||
delay_right_controller_after_left_controller,
|
||||
delay_right_gripper_ready_waiter_after_left_controller,
|
||||
delay_right_controller_after_gripper_ready,
|
||||
delay_rviz_after_right_controller,
|
||||
]
|
||||
|
||||
return LaunchDescription(declared_arguments + nodes)
|
||||
@@ -0,0 +1,741 @@
|
||||
import os
|
||||
import yaml
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import (
|
||||
DeclareLaunchArgument,
|
||||
EmitEvent,
|
||||
IncludeLaunchDescription,
|
||||
OpaqueFunction,
|
||||
RegisterEventHandler,
|
||||
SetLaunchConfiguration,
|
||||
)
|
||||
from launch.conditions import IfCondition, UnlessCondition
|
||||
from launch.events import Shutdown
|
||||
from launch.event_handlers import OnProcessExit
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.parameter_descriptions import ParameterFile, ParameterValue
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
from launch.substitutions import (
|
||||
Command,
|
||||
FindExecutable,
|
||||
LaunchConfiguration,
|
||||
PathJoinSubstitution,
|
||||
)
|
||||
|
||||
|
||||
def load_yaml(package_name, file_path, replacements=None):
|
||||
package_path = get_package_share_directory(package_name)
|
||||
absolute_file_path = os.path.join(package_path, file_path)
|
||||
|
||||
try:
|
||||
with open(absolute_file_path, "r") as file:
|
||||
yaml_content = file.read()
|
||||
|
||||
if replacements:
|
||||
for key, value in replacements.items():
|
||||
yaml_content = yaml_content.replace(key, value)
|
||||
|
||||
return yaml.safe_load(yaml_content)
|
||||
except (
|
||||
EnvironmentError
|
||||
): # parent of IOError, OSError *and* WindowsError where available
|
||||
return None
|
||||
|
||||
|
||||
def launch_setup(context):
|
||||
# Initialize Arguments
|
||||
rizon_type_left = LaunchConfiguration("rizon_type_left")
|
||||
rizon_type_right = LaunchConfiguration("rizon_type_right")
|
||||
robot_sn_left = LaunchConfiguration("robot_sn_left")
|
||||
robot_sn_right = LaunchConfiguration("robot_sn_right")
|
||||
|
||||
robot_sn_left_str = robot_sn_left.perform(context)
|
||||
robot_sn_right_str = robot_sn_right.perform(context)
|
||||
|
||||
rdk_control_mode = LaunchConfiguration("rdk_control_mode")
|
||||
start_rviz = LaunchConfiguration("start_rviz")
|
||||
use_fake_hardware = LaunchConfiguration("use_fake_hardware")
|
||||
fake_sensor_commands = LaunchConfiguration("fake_sensor_commands")
|
||||
|
||||
load_gripper_left = LaunchConfiguration("load_gripper_left")
|
||||
gripper_name_left = LaunchConfiguration("gripper_name_left")
|
||||
load_gripper_right = LaunchConfiguration("load_gripper_right")
|
||||
gripper_name_right = LaunchConfiguration("gripper_name_right")
|
||||
|
||||
load_mounted_ft_sensor_left = LaunchConfiguration("load_mounted_ft_sensor_left")
|
||||
load_mounted_ft_sensor_right = LaunchConfiguration("load_mounted_ft_sensor_right")
|
||||
left_gripper_ready_gate_condition = PythonExpression(
|
||||
[
|
||||
"'",
|
||||
load_gripper_left,
|
||||
"'.lower() in ['true', '1'] and '",
|
||||
use_fake_hardware,
|
||||
"'.lower() not in ['true', '1']",
|
||||
]
|
||||
)
|
||||
right_gripper_ready_gate_condition = PythonExpression(
|
||||
[
|
||||
"'",
|
||||
load_gripper_right,
|
||||
"'.lower() in ['true', '1'] and '",
|
||||
use_fake_hardware,
|
||||
"'.lower() not in ['true', '1']",
|
||||
]
|
||||
)
|
||||
|
||||
warehouse_sqlite_path = LaunchConfiguration("warehouse_sqlite_path")
|
||||
|
||||
# Construct prefixes
|
||||
prefix_left_str = "left_" + robot_sn_left_str + "_"
|
||||
prefix_right_str = "right_" + robot_sn_right_str + "_"
|
||||
|
||||
set_prefix_left = SetLaunchConfiguration(name="prefix_left", value=prefix_left_str)
|
||||
set_prefix_right = SetLaunchConfiguration(
|
||||
name="prefix_right", value=prefix_right_str
|
||||
)
|
||||
|
||||
# Get URDF via xacro
|
||||
flexiv_urdf_xacro = PathJoinSubstitution(
|
||||
[FindPackageShare("flexiv_description"), "urdf", "rizon_dual.urdf.xacro"]
|
||||
)
|
||||
|
||||
robot_description_content = ParameterValue(
|
||||
Command(
|
||||
[
|
||||
PathJoinSubstitution([FindExecutable(name="xacro")]),
|
||||
" ",
|
||||
flexiv_urdf_xacro,
|
||||
" ",
|
||||
"robot_sn_left:=",
|
||||
robot_sn_left,
|
||||
" ",
|
||||
"robot_sn_right:=",
|
||||
robot_sn_right,
|
||||
" ",
|
||||
"rizon_type_left:=",
|
||||
rizon_type_left,
|
||||
" ",
|
||||
"rizon_type_right:=",
|
||||
rizon_type_right,
|
||||
" ",
|
||||
"load_gripper_left:=",
|
||||
load_gripper_left,
|
||||
" ",
|
||||
"gripper_name_left:=",
|
||||
gripper_name_left,
|
||||
" ",
|
||||
"load_gripper_right:=",
|
||||
load_gripper_right,
|
||||
" ",
|
||||
"gripper_name_right:=",
|
||||
gripper_name_right,
|
||||
" ",
|
||||
"load_mounted_ft_sensor_left:=",
|
||||
load_mounted_ft_sensor_left,
|
||||
" ",
|
||||
"load_mounted_ft_sensor_right:=",
|
||||
load_mounted_ft_sensor_right,
|
||||
" ",
|
||||
"ros2_control:=true ",
|
||||
"rdk_control_mode:=",
|
||||
rdk_control_mode,
|
||||
" ",
|
||||
"use_fake_hardware:=",
|
||||
use_fake_hardware,
|
||||
" ",
|
||||
"fake_sensor_commands:=",
|
||||
fake_sensor_commands,
|
||||
]
|
||||
),
|
||||
value_type=str,
|
||||
)
|
||||
|
||||
robot_description = {"robot_description": robot_description_content}
|
||||
|
||||
# MoveIt configuration
|
||||
flexiv_srdf_xacro = PathJoinSubstitution(
|
||||
[FindPackageShare("flexiv_moveit_config"), "srdf", "rizon_dual.srdf.xacro"]
|
||||
)
|
||||
|
||||
robot_description_semantic_content = ParameterValue(
|
||||
Command(
|
||||
[
|
||||
PathJoinSubstitution([FindExecutable(name="xacro")]),
|
||||
" ",
|
||||
flexiv_srdf_xacro,
|
||||
" ",
|
||||
"robot_sn_left:=",
|
||||
robot_sn_left,
|
||||
" ",
|
||||
"robot_sn_right:=",
|
||||
robot_sn_right,
|
||||
" ",
|
||||
"load_gripper_left:=",
|
||||
load_gripper_left,
|
||||
" ",
|
||||
"load_gripper_right:=",
|
||||
load_gripper_right,
|
||||
" ",
|
||||
"load_mounted_ft_sensor_left:=",
|
||||
load_mounted_ft_sensor_left,
|
||||
" ",
|
||||
"load_mounted_ft_sensor_right:=",
|
||||
load_mounted_ft_sensor_right,
|
||||
" ",
|
||||
"prefix_left:=",
|
||||
"left_",
|
||||
" ",
|
||||
"prefix_right:=",
|
||||
"right_",
|
||||
]
|
||||
),
|
||||
value_type=str,
|
||||
)
|
||||
robot_description_semantic = {
|
||||
"robot_description_semantic": robot_description_semantic_content
|
||||
}
|
||||
|
||||
publish_robot_description_semantic = {"publish_robot_description_semantic": True}
|
||||
|
||||
# Trajectory Execution Configuration
|
||||
replacements = {
|
||||
"$(var prefix_left)": prefix_left_str,
|
||||
"$(var prefix_right)": prefix_right_str,
|
||||
}
|
||||
|
||||
robot_description_kinematics_yaml = load_yaml(
|
||||
"flexiv_moveit_config", "config/dual_arm/kinematics_dual.yaml", replacements
|
||||
)
|
||||
robot_description_kinematics = {
|
||||
"robot_description_kinematics": robot_description_kinematics_yaml
|
||||
}
|
||||
|
||||
# Planning Configuration
|
||||
ompl_planning_pipeline_config = {
|
||||
"move_group": {
|
||||
"planning_plugin": "ompl_interface/OMPLPlanner",
|
||||
"request_adapters": "default_planner_request_adapters/AddTimeOptimalParameterization "
|
||||
"default_planner_request_adapters/ResolveConstraintFrames "
|
||||
"default_planner_request_adapters/FixWorkspaceBounds "
|
||||
"default_planner_request_adapters/FixStartStateBounds "
|
||||
"default_planner_request_adapters/FixStartStateCollision "
|
||||
"default_planner_request_adapters/FixStartStatePathConstraints",
|
||||
"start_state_max_bounds_error": 0.1,
|
||||
}
|
||||
}
|
||||
ompl_planning_yaml = load_yaml(
|
||||
"flexiv_moveit_config", "config/dual_arm/ompl_planning_dual.yaml", replacements
|
||||
)
|
||||
ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
|
||||
|
||||
moveit_simple_controllers_yaml = load_yaml(
|
||||
"flexiv_moveit_config",
|
||||
"config/dual_arm/moveit_controllers_dual.yaml",
|
||||
replacements,
|
||||
)
|
||||
|
||||
moveit_controllers = {
|
||||
"moveit_simple_controller_manager": moveit_simple_controllers_yaml,
|
||||
"moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager",
|
||||
}
|
||||
|
||||
trajectory_execution = {
|
||||
"moveit_manage_controllers": False,
|
||||
"trajectory_execution.allowed_execution_duration_scaling": 1.2,
|
||||
"trajectory_execution.allowed_goal_duration_margin": 0.5,
|
||||
"trajectory_execution.allowed_start_tolerance": 0.01,
|
||||
}
|
||||
|
||||
planning_scene_monitor_parameters = {
|
||||
"publish_planning_scene": True,
|
||||
"publish_geometry_updates": True,
|
||||
"publish_state_updates": True,
|
||||
"publish_transforms_updates": True,
|
||||
}
|
||||
|
||||
# Load for left arm
|
||||
joint_limits_left = load_yaml(
|
||||
"flexiv_moveit_config",
|
||||
"config/joint_limits.yaml",
|
||||
{"$(var robot_sn)": prefix_left_str.rstrip("_")},
|
||||
)
|
||||
joint_limits_right = load_yaml(
|
||||
"flexiv_moveit_config",
|
||||
"config/joint_limits.yaml",
|
||||
{"$(var robot_sn)": prefix_right_str.rstrip("_")},
|
||||
)
|
||||
|
||||
joint_limits_yaml = {"robot_description_planning": {"joint_limits": {}}}
|
||||
|
||||
if joint_limits_left and "joint_limits" in joint_limits_left:
|
||||
joint_limits_yaml["robot_description_planning"]["joint_limits"].update(
|
||||
joint_limits_left["joint_limits"]
|
||||
)
|
||||
|
||||
if joint_limits_right and "joint_limits" in joint_limits_right:
|
||||
joint_limits_yaml["robot_description_planning"]["joint_limits"].update(
|
||||
joint_limits_right["joint_limits"]
|
||||
)
|
||||
|
||||
warehouse_ros_config = {
|
||||
"warehouse_plugin": "warehouse_ros_sqlite::DatabaseConnection",
|
||||
"warehouse_host": warehouse_sqlite_path,
|
||||
}
|
||||
|
||||
# Start the actual move_group node/action server
|
||||
move_group_node = Node(
|
||||
package="moveit_ros_move_group",
|
||||
executable="move_group",
|
||||
output="screen",
|
||||
parameters=[
|
||||
robot_description,
|
||||
robot_description_semantic,
|
||||
publish_robot_description_semantic,
|
||||
robot_description_kinematics,
|
||||
joint_limits_yaml,
|
||||
ompl_planning_pipeline_config,
|
||||
trajectory_execution,
|
||||
moveit_controllers,
|
||||
planning_scene_monitor_parameters,
|
||||
warehouse_ros_config,
|
||||
],
|
||||
)
|
||||
|
||||
# RViz with MoveIt configuration
|
||||
rviz_config_file = PathJoinSubstitution(
|
||||
[FindPackageShare("flexiv_moveit_config"), "rviz", "moveit.rviz"]
|
||||
)
|
||||
|
||||
rviz_node = Node(
|
||||
package="rviz2",
|
||||
executable="rviz2",
|
||||
name="rviz2_moveit",
|
||||
output="log",
|
||||
arguments=["-d", rviz_config_file],
|
||||
parameters=[
|
||||
robot_description,
|
||||
robot_description_semantic,
|
||||
ompl_planning_pipeline_config,
|
||||
robot_description_kinematics,
|
||||
joint_limits_yaml,
|
||||
warehouse_ros_config,
|
||||
],
|
||||
condition=IfCondition(start_rviz),
|
||||
)
|
||||
|
||||
# Publish TF
|
||||
robot_state_publisher_node = Node(
|
||||
package="robot_state_publisher",
|
||||
executable="robot_state_publisher",
|
||||
name="robot_state_publisher",
|
||||
output="both",
|
||||
parameters=[robot_description],
|
||||
)
|
||||
|
||||
# Robot controllers
|
||||
robot_controllers = PathJoinSubstitution(
|
||||
[FindPackageShare("flexiv_bringup"), "config", "rizon_dual_controllers.yaml"]
|
||||
)
|
||||
|
||||
# Run controller manager
|
||||
ros2_control_node = Node(
|
||||
package="controller_manager",
|
||||
executable="ros2_control_node",
|
||||
parameters=[
|
||||
robot_description,
|
||||
ParameterFile(robot_controllers, allow_substs=True),
|
||||
{"robot_sn_left": robot_sn_left},
|
||||
{"robot_sn_right": robot_sn_right},
|
||||
{"prefix_left": prefix_left_str},
|
||||
{"prefix_right": prefix_right_str},
|
||||
{"rdk_control_mode": rdk_control_mode},
|
||||
],
|
||||
remappings=[("joint_states", "flexiv_dual_arm/joint_states")],
|
||||
output="both",
|
||||
)
|
||||
|
||||
# Joint state publisher
|
||||
joint_state_publisher_node = Node(
|
||||
package="joint_state_publisher",
|
||||
executable="joint_state_publisher",
|
||||
name="joint_state_publisher",
|
||||
parameters=[
|
||||
{
|
||||
"source_list": [
|
||||
"flexiv_dual_arm/joint_states",
|
||||
"left_gripper_node/gripper_joint_states",
|
||||
"right_gripper_node/gripper_joint_states",
|
||||
],
|
||||
"rate": 30,
|
||||
}
|
||||
],
|
||||
)
|
||||
|
||||
# Run robot controller
|
||||
left_rizon_arm_controller_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=[
|
||||
"left_rizon_arm_controller",
|
||||
"--controller-manager",
|
||||
"/controller_manager",
|
||||
],
|
||||
)
|
||||
|
||||
right_rizon_arm_controller_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=[
|
||||
"right_rizon_arm_controller",
|
||||
"--controller-manager",
|
||||
"/controller_manager",
|
||||
],
|
||||
)
|
||||
|
||||
# Run joint state broadcaster
|
||||
joint_state_broadcaster_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=[
|
||||
"joint_state_broadcaster",
|
||||
"--controller-manager",
|
||||
"/controller_manager",
|
||||
],
|
||||
)
|
||||
|
||||
# Run Flexiv robot states broadcaster
|
||||
flexiv_robot_states_broadcaster_left_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=["flexiv_robot_states_broadcaster_left"],
|
||||
condition=UnlessCondition(use_fake_hardware),
|
||||
)
|
||||
|
||||
flexiv_robot_states_broadcaster_right_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=["flexiv_robot_states_broadcaster_right"],
|
||||
condition=UnlessCondition(use_fake_hardware),
|
||||
)
|
||||
|
||||
load_gripper_left_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
PathJoinSubstitution(
|
||||
[
|
||||
FindPackageShare("flexiv_gripper"),
|
||||
"launch",
|
||||
"flexiv_gripper.launch.py",
|
||||
]
|
||||
)
|
||||
),
|
||||
launch_arguments={
|
||||
"robot_sn": robot_sn_left,
|
||||
"gripper_name": gripper_name_left,
|
||||
"use_fake_hardware": use_fake_hardware,
|
||||
"gripper_node_name": "left_gripper_node",
|
||||
"use_lite_rdk": "true",
|
||||
}.items(),
|
||||
condition=IfCondition(load_gripper_left),
|
||||
)
|
||||
|
||||
load_gripper_right_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
PathJoinSubstitution(
|
||||
[
|
||||
FindPackageShare("flexiv_gripper"),
|
||||
"launch",
|
||||
"flexiv_gripper.launch.py",
|
||||
]
|
||||
)
|
||||
),
|
||||
launch_arguments={
|
||||
"robot_sn": robot_sn_right,
|
||||
"gripper_name": gripper_name_right,
|
||||
"use_fake_hardware": use_fake_hardware,
|
||||
"gripper_node_name": "right_gripper_node",
|
||||
"use_lite_rdk": "true",
|
||||
}.items(),
|
||||
condition=IfCondition(load_gripper_right),
|
||||
)
|
||||
|
||||
left_gripper_ready_waiter = Node(
|
||||
package="flexiv_gripper",
|
||||
executable="wait_for_gripper_ready",
|
||||
name="wait_for_left_gripper_ready",
|
||||
parameters=[{"ready_topic": "/left_gripper_node/ready"}],
|
||||
output="screen",
|
||||
condition=IfCondition(left_gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
right_gripper_ready_waiter = Node(
|
||||
package="flexiv_gripper",
|
||||
executable="wait_for_gripper_ready",
|
||||
name="wait_for_right_gripper_ready",
|
||||
parameters=[{"ready_topic": "/right_gripper_node/ready"}],
|
||||
output="screen",
|
||||
condition=IfCondition(right_gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
def launch_controller_after_gripper_ready(controller_spawner, gripper_node_name):
|
||||
def handle_gripper_ready(event, context):
|
||||
if event.returncode == 0:
|
||||
return [controller_spawner]
|
||||
return [
|
||||
EmitEvent(
|
||||
event=Shutdown(reason=f"{gripper_node_name} did not report ready")
|
||||
)
|
||||
]
|
||||
|
||||
return handle_gripper_ready
|
||||
|
||||
# Run gpio controllers
|
||||
gpio_controller_left_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=[
|
||||
"gpio_controller_left",
|
||||
"--controller-manager",
|
||||
"/controller_manager",
|
||||
],
|
||||
condition=UnlessCondition(use_fake_hardware),
|
||||
)
|
||||
gpio_controller_right_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=[
|
||||
"gpio_controller_right",
|
||||
"--controller-manager",
|
||||
"/controller_manager",
|
||||
],
|
||||
condition=UnlessCondition(use_fake_hardware),
|
||||
)
|
||||
|
||||
# Delay start of controllers after `joint_state_broadcaster`
|
||||
delay_left_controller_after_jsb = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=joint_state_broadcaster_spawner,
|
||||
on_exit=[left_rizon_arm_controller_spawner],
|
||||
),
|
||||
condition=UnlessCondition(left_gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
delay_left_gripper_launch_after_joint_state_broadcaster_spawner = (
|
||||
RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=joint_state_broadcaster_spawner,
|
||||
on_exit=[load_gripper_left_launch, left_gripper_ready_waiter],
|
||||
),
|
||||
condition=IfCondition(left_gripper_ready_gate_condition),
|
||||
)
|
||||
)
|
||||
|
||||
delay_right_gripper_launch_after_joint_state_broadcaster_spawner = (
|
||||
RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=joint_state_broadcaster_spawner,
|
||||
on_exit=[load_gripper_right_launch],
|
||||
),
|
||||
condition=IfCondition(right_gripper_ready_gate_condition),
|
||||
)
|
||||
)
|
||||
|
||||
delay_left_controller_after_gripper_ready = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=left_gripper_ready_waiter,
|
||||
on_exit=launch_controller_after_gripper_ready(
|
||||
left_rizon_arm_controller_spawner, "left_gripper_node"
|
||||
),
|
||||
),
|
||||
condition=IfCondition(left_gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
# Delay right controller start after left controller
|
||||
delay_right_controller_after_left_controller = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=left_rizon_arm_controller_spawner,
|
||||
on_exit=[right_rizon_arm_controller_spawner],
|
||||
),
|
||||
condition=UnlessCondition(right_gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
delay_right_gripper_ready_waiter_after_left_controller = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=left_rizon_arm_controller_spawner,
|
||||
on_exit=[right_gripper_ready_waiter],
|
||||
),
|
||||
condition=IfCondition(right_gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
delay_right_controller_after_gripper_ready = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=right_gripper_ready_waiter,
|
||||
on_exit=launch_controller_after_gripper_ready(
|
||||
right_rizon_arm_controller_spawner, "right_gripper_node"
|
||||
),
|
||||
),
|
||||
condition=IfCondition(right_gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
# Delay rviz start after right controller
|
||||
delay_rviz_after_right_controller = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=right_rizon_arm_controller_spawner,
|
||||
on_exit=[rviz_node],
|
||||
)
|
||||
)
|
||||
|
||||
nodes = [
|
||||
set_prefix_left,
|
||||
set_prefix_right,
|
||||
move_group_node,
|
||||
robot_state_publisher_node,
|
||||
ros2_control_node,
|
||||
joint_state_publisher_node,
|
||||
joint_state_broadcaster_spawner,
|
||||
flexiv_robot_states_broadcaster_left_spawner,
|
||||
flexiv_robot_states_broadcaster_right_spawner,
|
||||
gpio_controller_left_spawner,
|
||||
gpio_controller_right_spawner,
|
||||
delay_left_gripper_launch_after_joint_state_broadcaster_spawner,
|
||||
delay_right_gripper_launch_after_joint_state_broadcaster_spawner,
|
||||
delay_left_controller_after_jsb,
|
||||
delay_left_controller_after_gripper_ready,
|
||||
delay_right_controller_after_left_controller,
|
||||
delay_right_gripper_ready_waiter_after_left_controller,
|
||||
delay_right_controller_after_gripper_ready,
|
||||
delay_rviz_after_right_controller,
|
||||
]
|
||||
|
||||
return nodes
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
declared_arguments = []
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"rizon_type_left",
|
||||
description="Type of the left Flexiv Rizon robot.",
|
||||
default_value="Rizon4",
|
||||
choices=["Rizon4", "Rizon4M", "Rizon4R", "Rizon4s", "Rizon10", "Rizon10s"],
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"rizon_type_right",
|
||||
description="Type of the right Flexiv Rizon robot.",
|
||||
default_value="Rizon4R",
|
||||
choices=["Rizon4", "Rizon4M", "Rizon4R", "Rizon4s", "Rizon10", "Rizon10s"],
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"robot_sn_left",
|
||||
description="Serial number of the left robot.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"robot_sn_right",
|
||||
description="Serial number of the right robot.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"rdk_control_mode",
|
||||
default_value="joint_position",
|
||||
description="RDK control mode for the ROS 2 control joint position and velocity interfaces.",
|
||||
choices=["joint_position", "joint_impedance"],
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"start_rviz",
|
||||
default_value="true",
|
||||
description="Start RViz automatically with the launch file",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"use_fake_hardware",
|
||||
default_value="false",
|
||||
description="Start robot with fake hardware mirroring command to its states.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"fake_sensor_commands",
|
||||
default_value="false",
|
||||
description="Enable fake command interfaces for sensors used for simple simulations.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"load_gripper_left",
|
||||
default_value="false",
|
||||
description="Flag to load the Flexiv Grav gripper as the end-effector of the left robot.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"gripper_name_left",
|
||||
default_value="Flexiv-GN01",
|
||||
description="Full name of the left gripper to be controlled, can be found in Flexiv Elements -> Settings -> Device",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"load_gripper_right",
|
||||
default_value="false",
|
||||
description="Flag to load the Flexiv Grav gripper as the end-effector of the right robot.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"gripper_name_right",
|
||||
default_value="Flexiv-GN01",
|
||||
description="Full name of the right gripper to be controlled, can be found in Flexiv Elements -> Settings -> Device",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"load_mounted_ft_sensor_left",
|
||||
default_value="false",
|
||||
description="Flag to load the mounted force torque sensor for the left robot. Only available for Rizon4, Rizon4R and Rizon10.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"load_mounted_ft_sensor_right",
|
||||
default_value="false",
|
||||
description="Flag to load the mounted force torque sensor for the right robot. Only available for Rizon4, Rizon4R and Rizon10.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"warehouse_sqlite_path",
|
||||
default_value=os.path.expanduser("~/.ros/warehouse_ros.sqlite"),
|
||||
description="Path to the warehouse database",
|
||||
)
|
||||
)
|
||||
|
||||
return LaunchDescription(
|
||||
declared_arguments + [OpaqueFunction(function=launch_setup)]
|
||||
)
|
||||
@@ -0,0 +1,549 @@
|
||||
import os
|
||||
import yaml
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import (
|
||||
DeclareLaunchArgument,
|
||||
EmitEvent,
|
||||
IncludeLaunchDescription,
|
||||
OpaqueFunction,
|
||||
RegisterEventHandler,
|
||||
)
|
||||
from launch.conditions import IfCondition, UnlessCondition
|
||||
from launch.events import Shutdown
|
||||
from launch.event_handlers import OnProcessExit
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.parameter_descriptions import ParameterFile, ParameterValue
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
from launch.substitutions import (
|
||||
Command,
|
||||
FindExecutable,
|
||||
LaunchConfiguration,
|
||||
PathJoinSubstitution,
|
||||
PythonExpression,
|
||||
)
|
||||
|
||||
|
||||
def load_yaml(package_name, file_path, replacements=None):
|
||||
package_path = get_package_share_directory(package_name)
|
||||
absolute_file_path = os.path.join(package_path, file_path)
|
||||
|
||||
try:
|
||||
with open(absolute_file_path, "r") as file:
|
||||
yaml_content = file.read()
|
||||
|
||||
if replacements:
|
||||
for placeholder, replacement in replacements.items():
|
||||
yaml_content = yaml_content.replace(placeholder, replacement)
|
||||
|
||||
return yaml.safe_load(yaml_content)
|
||||
except (
|
||||
EnvironmentError
|
||||
): # parent of IOError, OSError *and* WindowsError where available
|
||||
return None
|
||||
|
||||
|
||||
def launch_setup(context):
|
||||
# Initialize Arguments
|
||||
rizon_type = LaunchConfiguration("rizon_type")
|
||||
robot_sn = LaunchConfiguration("robot_sn")
|
||||
robot_sn_str = robot_sn.perform(context)
|
||||
rdk_control_mode = LaunchConfiguration("rdk_control_mode")
|
||||
start_rviz = LaunchConfiguration("start_rviz")
|
||||
load_gripper = LaunchConfiguration("load_gripper")
|
||||
gripper_name = LaunchConfiguration("gripper_name")
|
||||
load_mounted_ft_sensor = LaunchConfiguration("load_mounted_ft_sensor")
|
||||
use_fake_hardware = LaunchConfiguration("use_fake_hardware")
|
||||
fake_sensor_commands = LaunchConfiguration("fake_sensor_commands")
|
||||
warehouse_sqlite_path = LaunchConfiguration("warehouse_sqlite_path")
|
||||
start_servo = LaunchConfiguration("start_servo")
|
||||
gripper_ready_gate_condition = PythonExpression(
|
||||
[
|
||||
"'",
|
||||
load_gripper,
|
||||
"'.lower() in ['true', '1'] and '",
|
||||
use_fake_hardware,
|
||||
"'.lower() not in ['true', '1']",
|
||||
]
|
||||
)
|
||||
|
||||
# Get URDF via xacro
|
||||
flexiv_urdf_xacro = PathJoinSubstitution(
|
||||
[FindPackageShare("flexiv_description"), "urdf", "rizon.urdf.xacro"]
|
||||
)
|
||||
|
||||
# Get URDF via xacro
|
||||
robot_description_content = ParameterValue(
|
||||
Command(
|
||||
[
|
||||
PathJoinSubstitution([FindExecutable(name="xacro")]),
|
||||
" ",
|
||||
flexiv_urdf_xacro,
|
||||
" ",
|
||||
"robot_sn:=",
|
||||
robot_sn,
|
||||
" ",
|
||||
"rizon_type:=",
|
||||
rizon_type,
|
||||
" ",
|
||||
"ros2_control:=true ",
|
||||
"rdk_control_mode:=",
|
||||
rdk_control_mode,
|
||||
" ",
|
||||
"load_gripper:=",
|
||||
load_gripper,
|
||||
" ",
|
||||
"gripper_name:=",
|
||||
gripper_name,
|
||||
" ",
|
||||
"load_mounted_ft_sensor:=",
|
||||
load_mounted_ft_sensor,
|
||||
" ",
|
||||
"use_fake_hardware:=",
|
||||
use_fake_hardware,
|
||||
" ",
|
||||
"fake_sensor_commands:=",
|
||||
fake_sensor_commands,
|
||||
]
|
||||
),
|
||||
value_type=str,
|
||||
)
|
||||
robot_description = {"robot_description": robot_description_content}
|
||||
|
||||
# MoveIt configuration
|
||||
flexiv_srdf_xacro = PathJoinSubstitution(
|
||||
[FindPackageShare("flexiv_moveit_config"), "srdf", "rizon.srdf.xacro"]
|
||||
)
|
||||
|
||||
robot_description_semantic_content = ParameterValue(
|
||||
Command(
|
||||
[
|
||||
PathJoinSubstitution([FindExecutable(name="xacro")]),
|
||||
" ",
|
||||
flexiv_srdf_xacro,
|
||||
" ",
|
||||
"robot_sn:=",
|
||||
robot_sn,
|
||||
" ",
|
||||
"rizon_type:=",
|
||||
rizon_type,
|
||||
" ",
|
||||
"load_gripper:=",
|
||||
load_gripper,
|
||||
" ",
|
||||
"load_mounted_ft_sensor:=",
|
||||
load_mounted_ft_sensor,
|
||||
]
|
||||
),
|
||||
value_type=str,
|
||||
)
|
||||
robot_description_semantic = {
|
||||
"robot_description_semantic": robot_description_semantic_content
|
||||
}
|
||||
|
||||
publish_robot_description_semantic = {"publish_robot_description_semantic": True}
|
||||
|
||||
replacements = {"$(var robot_sn)": robot_sn_str}
|
||||
|
||||
robot_description_kinematics_yaml = load_yaml(
|
||||
"flexiv_moveit_config", "config/kinematics.yaml", replacements
|
||||
)
|
||||
robot_description_kinematics = {
|
||||
"robot_description_kinematics": robot_description_kinematics_yaml
|
||||
}
|
||||
|
||||
# Planning Configuration
|
||||
ompl_planning_pipeline_config = {
|
||||
"move_group": {
|
||||
"planning_plugin": "ompl_interface/OMPLPlanner",
|
||||
"request_adapters": "default_planner_request_adapters/AddTimeOptimalParameterization "
|
||||
"default_planner_request_adapters/ResolveConstraintFrames "
|
||||
"default_planner_request_adapters/FixWorkspaceBounds "
|
||||
"default_planner_request_adapters/FixStartStateBounds "
|
||||
"default_planner_request_adapters/FixStartStateCollision "
|
||||
"default_planner_request_adapters/FixStartStatePathConstraints",
|
||||
"start_state_max_bounds_error": 0.1,
|
||||
}
|
||||
}
|
||||
ompl_planning_yaml = load_yaml(
|
||||
"flexiv_moveit_config", "config/ompl_planning.yaml", replacements
|
||||
)
|
||||
ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
|
||||
|
||||
# Trajectory Execution Configuration
|
||||
moveit_simple_controllers_yaml = load_yaml(
|
||||
"flexiv_moveit_config", "config/moveit_controllers.yaml", replacements
|
||||
)
|
||||
|
||||
moveit_controllers = {
|
||||
"moveit_simple_controller_manager": moveit_simple_controllers_yaml,
|
||||
"moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager",
|
||||
}
|
||||
|
||||
trajectory_execution = {
|
||||
"moveit_manage_controllers": False,
|
||||
"trajectory_execution.allowed_execution_duration_scaling": 1.2,
|
||||
"trajectory_execution.allowed_goal_duration_margin": 0.5,
|
||||
"trajectory_execution.allowed_start_tolerance": 0.01,
|
||||
}
|
||||
|
||||
planning_scene_monitor_parameters = {
|
||||
"publish_planning_scene": True,
|
||||
"publish_geometry_updates": True,
|
||||
"publish_state_updates": True,
|
||||
"publish_transforms_updates": True,
|
||||
}
|
||||
|
||||
joint_limits_yaml = {
|
||||
"robot_description_planning": load_yaml(
|
||||
"flexiv_moveit_config", "config/joint_limits.yaml", replacements
|
||||
)
|
||||
}
|
||||
|
||||
warehouse_ros_config = {
|
||||
"warehouse_plugin": "warehouse_ros_sqlite::DatabaseConnection",
|
||||
"warehouse_host": warehouse_sqlite_path,
|
||||
}
|
||||
|
||||
# Start the actual move_group node/action server
|
||||
move_group_node = Node(
|
||||
package="moveit_ros_move_group",
|
||||
executable="move_group",
|
||||
output="screen",
|
||||
parameters=[
|
||||
robot_description,
|
||||
robot_description_semantic,
|
||||
publish_robot_description_semantic,
|
||||
robot_description_kinematics,
|
||||
joint_limits_yaml,
|
||||
ompl_planning_pipeline_config,
|
||||
trajectory_execution,
|
||||
moveit_controllers,
|
||||
planning_scene_monitor_parameters,
|
||||
warehouse_ros_config,
|
||||
],
|
||||
)
|
||||
|
||||
# RViz with MoveIt configuration
|
||||
rviz_config_file = PathJoinSubstitution(
|
||||
[FindPackageShare("flexiv_moveit_config"), "rviz", "moveit.rviz"]
|
||||
)
|
||||
|
||||
rviz_node = Node(
|
||||
package="rviz2",
|
||||
executable="rviz2",
|
||||
name="rviz2_moveit",
|
||||
output="log",
|
||||
arguments=["-d", rviz_config_file],
|
||||
parameters=[
|
||||
robot_description,
|
||||
robot_description_semantic,
|
||||
ompl_planning_pipeline_config,
|
||||
robot_description_kinematics,
|
||||
joint_limits_yaml,
|
||||
warehouse_ros_config,
|
||||
],
|
||||
condition=IfCondition(start_rviz),
|
||||
)
|
||||
|
||||
# Publish TF
|
||||
robot_state_publisher_node = Node(
|
||||
package="robot_state_publisher",
|
||||
executable="robot_state_publisher",
|
||||
name="robot_state_publisher",
|
||||
output="both",
|
||||
parameters=[robot_description],
|
||||
)
|
||||
|
||||
# Robot controllers
|
||||
robot_controllers = PathJoinSubstitution(
|
||||
[FindPackageShare("flexiv_bringup"), "config", "rizon_controllers.yaml"]
|
||||
)
|
||||
|
||||
# Run controller manager
|
||||
ros2_control_node = Node(
|
||||
package="controller_manager",
|
||||
executable="ros2_control_node",
|
||||
parameters=[
|
||||
robot_description,
|
||||
ParameterFile(robot_controllers, allow_substs=True),
|
||||
{"robot_sn": robot_sn},
|
||||
{"rdk_control_mode": rdk_control_mode},
|
||||
],
|
||||
remappings=[("joint_states", "flexiv_rizon_arm/joint_states")],
|
||||
output="both",
|
||||
)
|
||||
|
||||
# Joint state publisher
|
||||
joint_state_publisher_node = Node(
|
||||
package="joint_state_publisher",
|
||||
executable="joint_state_publisher",
|
||||
name="joint_state_publisher",
|
||||
parameters=[
|
||||
{
|
||||
"source_list": [
|
||||
"flexiv_rizon_arm/joint_states",
|
||||
"flexiv_gripper_node/gripper_joint_states",
|
||||
],
|
||||
"rate": 30,
|
||||
}
|
||||
],
|
||||
)
|
||||
|
||||
# Run robot controller
|
||||
robot_controller_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=[
|
||||
"rizon_arm_controller",
|
||||
"--controller-manager",
|
||||
"/controller_manager",
|
||||
],
|
||||
)
|
||||
|
||||
# Run joint state broadcaster
|
||||
joint_state_broadcaster_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=[
|
||||
"joint_state_broadcaster",
|
||||
"--controller-manager",
|
||||
"/controller_manager",
|
||||
],
|
||||
)
|
||||
|
||||
# Run Flexiv robot states broadcaster
|
||||
flexiv_robot_states_broadcaster_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=["flexiv_robot_states_broadcaster"],
|
||||
parameters=[{"robot_sn": robot_sn}],
|
||||
condition=UnlessCondition(use_fake_hardware),
|
||||
)
|
||||
|
||||
# Include gripper launch file
|
||||
load_gripper_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
PathJoinSubstitution(
|
||||
[
|
||||
FindPackageShare("flexiv_gripper"),
|
||||
"launch",
|
||||
"flexiv_gripper.launch.py",
|
||||
]
|
||||
)
|
||||
),
|
||||
launch_arguments={
|
||||
"robot_sn": robot_sn,
|
||||
"gripper_name": gripper_name,
|
||||
"use_fake_hardware": use_fake_hardware,
|
||||
"use_lite_rdk": "true",
|
||||
}.items(),
|
||||
condition=IfCondition(load_gripper),
|
||||
)
|
||||
|
||||
gripper_ready_waiter = Node(
|
||||
package="flexiv_gripper",
|
||||
executable="wait_for_gripper_ready",
|
||||
name="wait_for_gripper_ready",
|
||||
parameters=[{"ready_topic": "/flexiv_gripper_node/ready"}],
|
||||
output="screen",
|
||||
condition=IfCondition(gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
def launch_robot_controller_after_gripper_ready(event, context):
|
||||
if event.returncode == 0:
|
||||
return [robot_controller_spawner]
|
||||
return [
|
||||
EmitEvent(event=Shutdown(reason="flexiv_gripper_node did not report ready"))
|
||||
]
|
||||
|
||||
# Servo node for realtime control
|
||||
servo_yaml = load_yaml(
|
||||
"flexiv_moveit_config",
|
||||
"config/rizon_moveit_servo_config.yaml",
|
||||
replacements,
|
||||
)
|
||||
servo_params = {"moveit_servo": servo_yaml}
|
||||
servo_node = Node(
|
||||
package="moveit_servo",
|
||||
condition=IfCondition(start_servo),
|
||||
executable="servo_node_main",
|
||||
parameters=[
|
||||
servo_params,
|
||||
robot_description,
|
||||
robot_description_semantic,
|
||||
robot_description_kinematics,
|
||||
],
|
||||
output="screen",
|
||||
)
|
||||
|
||||
# Run gpio controller
|
||||
gpio_controller_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=["gpio_controller", "--controller-manager", "/controller_manager"],
|
||||
parameters=[{"robot_sn": robot_sn}],
|
||||
condition=UnlessCondition(use_fake_hardware),
|
||||
)
|
||||
|
||||
# Delay start of robot_controller after `joint_state_broadcaster` when gripper is not loaded
|
||||
delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = (
|
||||
RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=joint_state_broadcaster_spawner,
|
||||
on_exit=[robot_controller_spawner],
|
||||
),
|
||||
condition=UnlessCondition(gripper_ready_gate_condition),
|
||||
)
|
||||
)
|
||||
|
||||
delay_gripper_launch_after_joint_state_broadcaster_spawner = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=joint_state_broadcaster_spawner,
|
||||
on_exit=[load_gripper_launch],
|
||||
),
|
||||
condition=IfCondition(gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
delay_robot_controller_spawner_after_gripper_ready = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=gripper_ready_waiter,
|
||||
on_exit=launch_robot_controller_after_gripper_ready,
|
||||
),
|
||||
condition=IfCondition(gripper_ready_gate_condition),
|
||||
)
|
||||
|
||||
# Delay move_group start after `robot_controller_spawner`
|
||||
delay_move_group_after_robot_controller_spawner = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=robot_controller_spawner,
|
||||
on_exit=[move_group_node],
|
||||
)
|
||||
)
|
||||
|
||||
# Delay rviz start after `robot_controller_spawner`
|
||||
delay_rviz_after_robot_controller_spawner = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=robot_controller_spawner,
|
||||
on_exit=[rviz_node],
|
||||
)
|
||||
)
|
||||
|
||||
nodes = [
|
||||
ros2_control_node,
|
||||
joint_state_publisher_node,
|
||||
robot_state_publisher_node,
|
||||
gripper_ready_waiter,
|
||||
joint_state_broadcaster_spawner,
|
||||
flexiv_robot_states_broadcaster_spawner,
|
||||
gpio_controller_spawner,
|
||||
servo_node,
|
||||
delay_gripper_launch_after_joint_state_broadcaster_spawner,
|
||||
delay_robot_controller_spawner_after_joint_state_broadcaster_spawner,
|
||||
delay_robot_controller_spawner_after_gripper_ready,
|
||||
delay_move_group_after_robot_controller_spawner,
|
||||
delay_rviz_after_robot_controller_spawner,
|
||||
]
|
||||
|
||||
return nodes
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
# Declare command-line arguments
|
||||
declared_arguments = []
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"rizon_type",
|
||||
description="Type of the Flexiv Rizon robot.",
|
||||
default_value="Rizon4",
|
||||
choices=["Rizon4", "Rizon4M", "Rizon4R", "Rizon4s", "Rizon10", "Rizon10s"],
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"robot_sn",
|
||||
description="Serial number of the robot to connect to. Remove any space, for example: Rizon4s-123456",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"rdk_control_mode",
|
||||
default_value="joint_position",
|
||||
description="RDK control mode for the ROS 2 control joint position and velocity interfaces. Options: joint_position, joint_impedance",
|
||||
choices=["joint_position", "joint_impedance"],
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"start_rviz",
|
||||
default_value="true",
|
||||
description="Start RViz automatically with the launch file",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"load_gripper",
|
||||
default_value="false",
|
||||
description="Flag to load the Flexiv Grav gripper as the end-effector of the robot.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"gripper_name",
|
||||
default_value="Flexiv-GN01",
|
||||
description="Full name of the gripper to be controlled, can be found in Flexiv Elements -> Settings -> Device",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"load_mounted_ft_sensor",
|
||||
default_value="false",
|
||||
description="Flag to load the mounted force torque sensor. Only available for Rizon4, Rizon4R and Rizon10.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"use_fake_hardware",
|
||||
default_value="false",
|
||||
description="Start robot with fake hardware mirroring command to its states.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"fake_sensor_commands",
|
||||
default_value="false",
|
||||
description="Enable fake command interfaces for sensors used for simple simulations. \
|
||||
Used only if 'use_fake_hardware' parameter is true.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"warehouse_sqlite_path",
|
||||
default_value=os.path.expanduser("~/.ros/warehouse_ros.sqlite"),
|
||||
description="Path to the sqlite database used by the warehouse_ros package.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"start_servo",
|
||||
default_value="false",
|
||||
description="Start the MoveIt servo node?",
|
||||
)
|
||||
)
|
||||
|
||||
return LaunchDescription(
|
||||
declared_arguments + [OpaqueFunction(function=launch_setup)]
|
||||
)
|
||||
+37
@@ -0,0 +1,37 @@
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.parameter_descriptions import ParameterFile
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
robot_sn = LaunchConfiguration("robot_sn")
|
||||
|
||||
position_goals = PathJoinSubstitution(
|
||||
[
|
||||
FindPackageShare("flexiv_bringup"),
|
||||
"config",
|
||||
"joint_trajectory_position_publisher.yaml",
|
||||
]
|
||||
)
|
||||
|
||||
return LaunchDescription(
|
||||
[
|
||||
DeclareLaunchArgument(
|
||||
name="robot_sn",
|
||||
description="Serial number of the robot to connect to. Remove any space, for example: Rizon4s-123456",
|
||||
),
|
||||
Node(
|
||||
package="flexiv_test_nodes",
|
||||
executable="publisher_joint_trajectory_controller",
|
||||
name="publisher_joint_trajectory_controller",
|
||||
parameters=[ParameterFile(position_goals, allow_substs=True)],
|
||||
output={
|
||||
"stdout": "screen",
|
||||
"stderr": "screen",
|
||||
},
|
||||
),
|
||||
]
|
||||
)
|
||||
@@ -0,0 +1,46 @@
|
||||
# generated from ament_package/template/package_level/local_setup.bash.in
|
||||
|
||||
# source local_setup.sh from same directory as this file
|
||||
_this_path=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" && pwd)
|
||||
# provide AMENT_CURRENT_PREFIX to shell script
|
||||
AMENT_CURRENT_PREFIX=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`/../.." && pwd)
|
||||
# store AMENT_CURRENT_PREFIX to restore it before each environment hook
|
||||
_package_local_setup_AMENT_CURRENT_PREFIX=$AMENT_CURRENT_PREFIX
|
||||
|
||||
# trace output
|
||||
if [ -n "$AMENT_TRACE_SETUP_FILES" ]; then
|
||||
echo "# . \"$_this_path/local_setup.sh\""
|
||||
fi
|
||||
. "$_this_path/local_setup.sh"
|
||||
unset _this_path
|
||||
|
||||
# unset AMENT_ENVIRONMENT_HOOKS
|
||||
# if not appending to them for return
|
||||
if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then
|
||||
unset AMENT_ENVIRONMENT_HOOKS
|
||||
fi
|
||||
|
||||
# restore AMENT_CURRENT_PREFIX before evaluating the environment hooks
|
||||
AMENT_CURRENT_PREFIX=$_package_local_setup_AMENT_CURRENT_PREFIX
|
||||
# list all environment hooks of this package
|
||||
|
||||
# source all shell-specific environment hooks of this package
|
||||
# if not returning them
|
||||
if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then
|
||||
_package_local_setup_IFS=$IFS
|
||||
IFS=":"
|
||||
for _hook in $AMENT_ENVIRONMENT_HOOKS; do
|
||||
# restore AMENT_CURRENT_PREFIX for each environment hook
|
||||
AMENT_CURRENT_PREFIX=$_package_local_setup_AMENT_CURRENT_PREFIX
|
||||
# restore IFS before sourcing other files
|
||||
IFS=$_package_local_setup_IFS
|
||||
. "$_hook"
|
||||
done
|
||||
unset _hook
|
||||
IFS=$_package_local_setup_IFS
|
||||
unset _package_local_setup_IFS
|
||||
unset AMENT_ENVIRONMENT_HOOKS
|
||||
fi
|
||||
|
||||
unset _package_local_setup_AMENT_CURRENT_PREFIX
|
||||
unset AMENT_CURRENT_PREFIX
|
||||
@@ -0,0 +1,2 @@
|
||||
source;share/flexiv_bringup/environment/ament_prefix_path.sh
|
||||
source;share/flexiv_bringup/environment/path.sh
|
||||
@@ -0,0 +1,184 @@
|
||||
# generated from ament_package/template/package_level/local_setup.sh.in
|
||||
|
||||
# since this file is sourced use either the provided AMENT_CURRENT_PREFIX
|
||||
# or fall back to the destination set at configure time
|
||||
: ${AMENT_CURRENT_PREFIX:="/home/cc/ros2/install/flexiv_bringup"}
|
||||
if [ ! -d "$AMENT_CURRENT_PREFIX" ]; then
|
||||
if [ -z "$COLCON_CURRENT_PREFIX" ]; then
|
||||
echo "The compile time prefix path '$AMENT_CURRENT_PREFIX' doesn't " \
|
||||
"exist. Consider sourcing a different extension than '.sh'." 1>&2
|
||||
else
|
||||
AMENT_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
|
||||
fi
|
||||
fi
|
||||
|
||||
# function to append values to environment variables
|
||||
# using colons as separators and avoiding leading separators
|
||||
ament_append_value() {
|
||||
# arguments
|
||||
_listname="$1"
|
||||
_value="$2"
|
||||
#echo "listname $_listname"
|
||||
#eval echo "list value \$$_listname"
|
||||
#echo "value $_value"
|
||||
|
||||
# avoid leading separator
|
||||
eval _values=\"\$$_listname\"
|
||||
if [ -z "$_values" ]; then
|
||||
eval export $_listname=\"$_value\"
|
||||
#eval echo "set list \$$_listname"
|
||||
else
|
||||
# field separator must not be a colon
|
||||
_ament_append_value_IFS=$IFS
|
||||
unset IFS
|
||||
eval export $_listname=\"\$$_listname:$_value\"
|
||||
#eval echo "append list \$$_listname"
|
||||
IFS=$_ament_append_value_IFS
|
||||
unset _ament_append_value_IFS
|
||||
fi
|
||||
unset _values
|
||||
|
||||
unset _value
|
||||
unset _listname
|
||||
}
|
||||
|
||||
# function to append non-duplicate values to environment variables
|
||||
# using colons as separators and avoiding leading separators
|
||||
ament_append_unique_value() {
|
||||
# arguments
|
||||
_listname=$1
|
||||
_value=$2
|
||||
#echo "listname $_listname"
|
||||
#eval echo "list value \$$_listname"
|
||||
#echo "value $_value"
|
||||
|
||||
# check if the list contains the value
|
||||
eval _values=\$$_listname
|
||||
_duplicate=
|
||||
_ament_append_unique_value_IFS=$IFS
|
||||
IFS=":"
|
||||
if [ "$AMENT_SHELL" = "zsh" ]; then
|
||||
ament_zsh_to_array _values
|
||||
fi
|
||||
for _item in $_values; do
|
||||
# ignore empty strings
|
||||
if [ -z "$_item" ]; then
|
||||
continue
|
||||
fi
|
||||
if [ $_item = $_value ]; then
|
||||
_duplicate=1
|
||||
fi
|
||||
done
|
||||
unset _item
|
||||
|
||||
# append only non-duplicates
|
||||
if [ -z "$_duplicate" ]; then
|
||||
# avoid leading separator
|
||||
if [ -z "$_values" ]; then
|
||||
eval $_listname=\"$_value\"
|
||||
#eval echo "set list \$$_listname"
|
||||
else
|
||||
# field separator must not be a colon
|
||||
unset IFS
|
||||
eval $_listname=\"\$$_listname:$_value\"
|
||||
#eval echo "append list \$$_listname"
|
||||
fi
|
||||
fi
|
||||
IFS=$_ament_append_unique_value_IFS
|
||||
unset _ament_append_unique_value_IFS
|
||||
unset _duplicate
|
||||
unset _values
|
||||
|
||||
unset _value
|
||||
unset _listname
|
||||
}
|
||||
|
||||
# function to prepend non-duplicate values to environment variables
|
||||
# using colons as separators and avoiding trailing separators
|
||||
ament_prepend_unique_value() {
|
||||
# arguments
|
||||
_listname="$1"
|
||||
_value="$2"
|
||||
#echo "listname $_listname"
|
||||
#eval echo "list value \$$_listname"
|
||||
#echo "value $_value"
|
||||
|
||||
# check if the list contains the value
|
||||
eval _values=\"\$$_listname\"
|
||||
_duplicate=
|
||||
_ament_prepend_unique_value_IFS=$IFS
|
||||
IFS=":"
|
||||
if [ "$AMENT_SHELL" = "zsh" ]; then
|
||||
ament_zsh_to_array _values
|
||||
fi
|
||||
for _item in $_values; do
|
||||
# ignore empty strings
|
||||
if [ -z "$_item" ]; then
|
||||
continue
|
||||
fi
|
||||
if [ "$_item" = "$_value" ]; then
|
||||
_duplicate=1
|
||||
fi
|
||||
done
|
||||
unset _item
|
||||
|
||||
# prepend only non-duplicates
|
||||
if [ -z "$_duplicate" ]; then
|
||||
# avoid trailing separator
|
||||
if [ -z "$_values" ]; then
|
||||
eval export $_listname=\"$_value\"
|
||||
#eval echo "set list \$$_listname"
|
||||
else
|
||||
# field separator must not be a colon
|
||||
unset IFS
|
||||
eval export $_listname=\"$_value:\$$_listname\"
|
||||
#eval echo "prepend list \$$_listname"
|
||||
fi
|
||||
fi
|
||||
IFS=$_ament_prepend_unique_value_IFS
|
||||
unset _ament_prepend_unique_value_IFS
|
||||
unset _duplicate
|
||||
unset _values
|
||||
|
||||
unset _value
|
||||
unset _listname
|
||||
}
|
||||
|
||||
# unset AMENT_ENVIRONMENT_HOOKS
|
||||
# if not appending to them for return
|
||||
if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then
|
||||
unset AMENT_ENVIRONMENT_HOOKS
|
||||
fi
|
||||
|
||||
# list all environment hooks of this package
|
||||
ament_append_value AMENT_ENVIRONMENT_HOOKS "$AMENT_CURRENT_PREFIX/share/flexiv_bringup/environment/ament_prefix_path.sh"
|
||||
ament_append_value AMENT_ENVIRONMENT_HOOKS "$AMENT_CURRENT_PREFIX/share/flexiv_bringup/environment/path.sh"
|
||||
|
||||
# source all shell-specific environment hooks of this package
|
||||
# if not returning them
|
||||
if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then
|
||||
_package_local_setup_IFS=$IFS
|
||||
IFS=":"
|
||||
if [ "$AMENT_SHELL" = "zsh" ]; then
|
||||
ament_zsh_to_array AMENT_ENVIRONMENT_HOOKS
|
||||
fi
|
||||
for _hook in $AMENT_ENVIRONMENT_HOOKS; do
|
||||
if [ -f "$_hook" ]; then
|
||||
# restore IFS before sourcing other files
|
||||
IFS=$_package_local_setup_IFS
|
||||
# trace output
|
||||
if [ -n "$AMENT_TRACE_SETUP_FILES" ]; then
|
||||
echo "# . \"$_hook\""
|
||||
fi
|
||||
. "$_hook"
|
||||
fi
|
||||
done
|
||||
unset _hook
|
||||
IFS=$_package_local_setup_IFS
|
||||
unset _package_local_setup_IFS
|
||||
unset AMENT_ENVIRONMENT_HOOKS
|
||||
fi
|
||||
|
||||
# reset AMENT_CURRENT_PREFIX after each package
|
||||
# allowing to source multiple package-level setup files
|
||||
unset AMENT_CURRENT_PREFIX
|
||||
@@ -0,0 +1,59 @@
|
||||
# generated from ament_package/template/package_level/local_setup.zsh.in
|
||||
|
||||
AMENT_SHELL=zsh
|
||||
|
||||
# source local_setup.sh from same directory as this file
|
||||
_this_path=$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)
|
||||
# provide AMENT_CURRENT_PREFIX to shell script
|
||||
AMENT_CURRENT_PREFIX=$(builtin cd -q "`dirname "${(%):-%N}"`/../.." > /dev/null && pwd)
|
||||
# store AMENT_CURRENT_PREFIX to restore it before each environment hook
|
||||
_package_local_setup_AMENT_CURRENT_PREFIX=$AMENT_CURRENT_PREFIX
|
||||
|
||||
# function to convert array-like strings into arrays
|
||||
# to wordaround SH_WORD_SPLIT not being set
|
||||
ament_zsh_to_array() {
|
||||
local _listname=$1
|
||||
local _dollar="$"
|
||||
local _split="{="
|
||||
local _to_array="(\"$_dollar$_split$_listname}\")"
|
||||
eval $_listname=$_to_array
|
||||
}
|
||||
|
||||
# trace output
|
||||
if [ -n "$AMENT_TRACE_SETUP_FILES" ]; then
|
||||
echo "# . \"$_this_path/local_setup.sh\""
|
||||
fi
|
||||
# the package-level local_setup file unsets AMENT_CURRENT_PREFIX
|
||||
. "$_this_path/local_setup.sh"
|
||||
unset _this_path
|
||||
|
||||
# unset AMENT_ENVIRONMENT_HOOKS
|
||||
# if not appending to them for return
|
||||
if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then
|
||||
unset AMENT_ENVIRONMENT_HOOKS
|
||||
fi
|
||||
|
||||
# restore AMENT_CURRENT_PREFIX before evaluating the environment hooks
|
||||
AMENT_CURRENT_PREFIX=$_package_local_setup_AMENT_CURRENT_PREFIX
|
||||
# list all environment hooks of this package
|
||||
|
||||
# source all shell-specific environment hooks of this package
|
||||
# if not returning them
|
||||
if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then
|
||||
_package_local_setup_IFS=$IFS
|
||||
IFS=":"
|
||||
for _hook in $AMENT_ENVIRONMENT_HOOKS; do
|
||||
# restore AMENT_CURRENT_PREFIX for each environment hook
|
||||
AMENT_CURRENT_PREFIX=$_package_local_setup_AMENT_CURRENT_PREFIX
|
||||
# restore IFS before sourcing other files
|
||||
IFS=$_package_local_setup_IFS
|
||||
. "$_hook"
|
||||
done
|
||||
unset _hook
|
||||
IFS=$_package_local_setup_IFS
|
||||
unset _package_local_setup_IFS
|
||||
unset AMENT_ENVIRONMENT_HOOKS
|
||||
fi
|
||||
|
||||
unset _package_local_setup_AMENT_CURRENT_PREFIX
|
||||
unset AMENT_CURRENT_PREFIX
|
||||
@@ -0,0 +1,39 @@
|
||||
# generated from colcon_bash/shell/template/package.bash.em
|
||||
|
||||
# This script extends the environment for this package.
|
||||
|
||||
# a bash script is able to determine its own path if necessary
|
||||
if [ -z "$COLCON_CURRENT_PREFIX" ]; then
|
||||
# the prefix is two levels up from the package specific share directory
|
||||
_colcon_package_bash_COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`/../.." > /dev/null && pwd)"
|
||||
else
|
||||
_colcon_package_bash_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
|
||||
fi
|
||||
|
||||
# function to source another script with conditional trace output
|
||||
# first argument: the path of the script
|
||||
# additional arguments: arguments to the script
|
||||
_colcon_package_bash_source_script() {
|
||||
if [ -f "$1" ]; then
|
||||
if [ -n "$COLCON_TRACE" ]; then
|
||||
echo "# . \"$1\""
|
||||
fi
|
||||
. "$@"
|
||||
else
|
||||
echo "not found: \"$1\"" 1>&2
|
||||
fi
|
||||
}
|
||||
|
||||
# source sh script of this package
|
||||
_colcon_package_bash_source_script "$_colcon_package_bash_COLCON_CURRENT_PREFIX/share/flexiv_bringup/package.sh"
|
||||
|
||||
# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced scripts
|
||||
COLCON_CURRENT_PREFIX="$_colcon_package_bash_COLCON_CURRENT_PREFIX"
|
||||
|
||||
# source bash hooks
|
||||
_colcon_package_bash_source_script "$COLCON_CURRENT_PREFIX/share/flexiv_bringup/local_setup.bash"
|
||||
|
||||
unset COLCON_CURRENT_PREFIX
|
||||
|
||||
unset _colcon_package_bash_source_script
|
||||
unset _colcon_package_bash_COLCON_CURRENT_PREFIX
|
||||
@@ -0,0 +1,8 @@
|
||||
source;share/flexiv_bringup/hook/cmake_prefix_path.ps1
|
||||
source;share/flexiv_bringup/hook/cmake_prefix_path.dsv
|
||||
source;share/flexiv_bringup/hook/cmake_prefix_path.sh
|
||||
source;share/flexiv_bringup/local_setup.bash
|
||||
source;share/flexiv_bringup/local_setup.dsv
|
||||
source;share/flexiv_bringup/local_setup.ps1
|
||||
source;share/flexiv_bringup/local_setup.sh
|
||||
source;share/flexiv_bringup/local_setup.zsh
|
||||
@@ -0,0 +1,116 @@
|
||||
# generated from colcon_powershell/shell/template/package.ps1.em
|
||||
|
||||
# function to append a value to a variable
|
||||
# which uses colons as separators
|
||||
# duplicates as well as leading separators are avoided
|
||||
# first argument: the name of the result variable
|
||||
# second argument: the value to be prepended
|
||||
function colcon_append_unique_value {
|
||||
param (
|
||||
$_listname,
|
||||
$_value
|
||||
)
|
||||
|
||||
# get values from variable
|
||||
if (Test-Path Env:$_listname) {
|
||||
$_values=(Get-Item env:$_listname).Value
|
||||
} else {
|
||||
$_values=""
|
||||
}
|
||||
$_duplicate=""
|
||||
# start with no values
|
||||
$_all_values=""
|
||||
# iterate over existing values in the variable
|
||||
if ($_values) {
|
||||
$_values.Split(":") | ForEach {
|
||||
# not an empty string
|
||||
if ($_) {
|
||||
# not a duplicate of _value
|
||||
if ($_ -eq $_value) {
|
||||
$_duplicate="1"
|
||||
}
|
||||
if ($_all_values) {
|
||||
$_all_values="${_all_values}:$_"
|
||||
} else {
|
||||
$_all_values="$_"
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
# append only non-duplicates
|
||||
if (!$_duplicate) {
|
||||
# avoid leading separator
|
||||
if ($_all_values) {
|
||||
$_all_values="${_all_values}:${_value}"
|
||||
} else {
|
||||
$_all_values="${_value}"
|
||||
}
|
||||
}
|
||||
|
||||
# export the updated variable
|
||||
Set-Item env:\$_listname -Value "$_all_values"
|
||||
}
|
||||
|
||||
# function to prepend a value to a variable
|
||||
# which uses colons as separators
|
||||
# duplicates as well as trailing separators are avoided
|
||||
# first argument: the name of the result variable
|
||||
# second argument: the value to be prepended
|
||||
function colcon_prepend_unique_value {
|
||||
param (
|
||||
$_listname,
|
||||
$_value
|
||||
)
|
||||
|
||||
# get values from variable
|
||||
if (Test-Path Env:$_listname) {
|
||||
$_values=(Get-Item env:$_listname).Value
|
||||
} else {
|
||||
$_values=""
|
||||
}
|
||||
# start with the new value
|
||||
$_all_values="$_value"
|
||||
# iterate over existing values in the variable
|
||||
if ($_values) {
|
||||
$_values.Split(":") | ForEach {
|
||||
# not an empty string
|
||||
if ($_) {
|
||||
# not a duplicate of _value
|
||||
if ($_ -ne $_value) {
|
||||
# keep non-duplicate values
|
||||
$_all_values="${_all_values}:$_"
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
# export the updated variable
|
||||
Set-Item env:\$_listname -Value "$_all_values"
|
||||
}
|
||||
|
||||
# function to source another script with conditional trace output
|
||||
# first argument: the path of the script
|
||||
# additional arguments: arguments to the script
|
||||
function colcon_package_source_powershell_script {
|
||||
param (
|
||||
$_colcon_package_source_powershell_script
|
||||
)
|
||||
# source script with conditional trace output
|
||||
if (Test-Path $_colcon_package_source_powershell_script) {
|
||||
if ($env:COLCON_TRACE) {
|
||||
echo ". '$_colcon_package_source_powershell_script'"
|
||||
}
|
||||
. "$_colcon_package_source_powershell_script"
|
||||
} else {
|
||||
Write-Error "not found: '$_colcon_package_source_powershell_script'"
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
# a powershell script is able to determine its own path
|
||||
# the prefix is two levels up from the package specific share directory
|
||||
$env:COLCON_CURRENT_PREFIX=(Get-Item $PSCommandPath).Directory.Parent.Parent.FullName
|
||||
|
||||
colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX/share/flexiv_bringup/hook/cmake_prefix_path.ps1"
|
||||
colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX/share/flexiv_bringup/local_setup.ps1"
|
||||
|
||||
Remove-Item Env:\COLCON_CURRENT_PREFIX
|
||||
@@ -0,0 +1,87 @@
|
||||
# generated from colcon_core/shell/template/package.sh.em
|
||||
|
||||
# This script extends the environment for this package.
|
||||
|
||||
# function to prepend a value to a variable
|
||||
# which uses colons as separators
|
||||
# duplicates as well as trailing separators are avoided
|
||||
# first argument: the name of the result variable
|
||||
# second argument: the value to be prepended
|
||||
_colcon_prepend_unique_value() {
|
||||
# arguments
|
||||
_listname="$1"
|
||||
_value="$2"
|
||||
|
||||
# get values from variable
|
||||
eval _values=\"\$$_listname\"
|
||||
# backup the field separator
|
||||
_colcon_prepend_unique_value_IFS=$IFS
|
||||
IFS=":"
|
||||
# start with the new value
|
||||
_all_values="$_value"
|
||||
# workaround SH_WORD_SPLIT not being set in zsh
|
||||
if [ "$(command -v colcon_zsh_convert_to_array)" ]; then
|
||||
colcon_zsh_convert_to_array _values
|
||||
fi
|
||||
# iterate over existing values in the variable
|
||||
for _item in $_values; do
|
||||
# ignore empty strings
|
||||
if [ -z "$_item" ]; then
|
||||
continue
|
||||
fi
|
||||
# ignore duplicates of _value
|
||||
if [ "$_item" = "$_value" ]; then
|
||||
continue
|
||||
fi
|
||||
# keep non-duplicate values
|
||||
_all_values="$_all_values:$_item"
|
||||
done
|
||||
unset _item
|
||||
# restore the field separator
|
||||
IFS=$_colcon_prepend_unique_value_IFS
|
||||
unset _colcon_prepend_unique_value_IFS
|
||||
# export the updated variable
|
||||
eval export $_listname=\"$_all_values\"
|
||||
unset _all_values
|
||||
unset _values
|
||||
|
||||
unset _value
|
||||
unset _listname
|
||||
}
|
||||
|
||||
# since a plain shell script can't determine its own path when being sourced
|
||||
# either use the provided COLCON_CURRENT_PREFIX
|
||||
# or fall back to the build time prefix (if it exists)
|
||||
_colcon_package_sh_COLCON_CURRENT_PREFIX="/home/cc/ros2/install/flexiv_bringup"
|
||||
if [ -z "$COLCON_CURRENT_PREFIX" ]; then
|
||||
if [ ! -d "$_colcon_package_sh_COLCON_CURRENT_PREFIX" ]; then
|
||||
echo "The build time path \"$_colcon_package_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2
|
||||
unset _colcon_package_sh_COLCON_CURRENT_PREFIX
|
||||
return 1
|
||||
fi
|
||||
COLCON_CURRENT_PREFIX="$_colcon_package_sh_COLCON_CURRENT_PREFIX"
|
||||
fi
|
||||
unset _colcon_package_sh_COLCON_CURRENT_PREFIX
|
||||
|
||||
# function to source another script with conditional trace output
|
||||
# first argument: the path of the script
|
||||
# additional arguments: arguments to the script
|
||||
_colcon_package_sh_source_script() {
|
||||
if [ -f "$1" ]; then
|
||||
if [ -n "$COLCON_TRACE" ]; then
|
||||
echo "# . \"$1\""
|
||||
fi
|
||||
. "$@"
|
||||
else
|
||||
echo "not found: \"$1\"" 1>&2
|
||||
fi
|
||||
}
|
||||
|
||||
# source sh hooks
|
||||
_colcon_package_sh_source_script "$COLCON_CURRENT_PREFIX/share/flexiv_bringup/hook/cmake_prefix_path.sh"
|
||||
_colcon_package_sh_source_script "$COLCON_CURRENT_PREFIX/share/flexiv_bringup/local_setup.sh"
|
||||
|
||||
unset _colcon_package_sh_source_script
|
||||
unset COLCON_CURRENT_PREFIX
|
||||
|
||||
# do not unset _colcon_prepend_unique_value since it might be used by non-primary shell hooks
|
||||
@@ -0,0 +1,31 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>flexiv_bringup</name>
|
||||
<version>1.9.1</version>
|
||||
<description>Package with launch files and run-time configurations for Flexiv robots with `ros2_control`</description>
|
||||
<maintainer email="munseng.phoon@flexiv.com">Mun Seng Phoon</maintainer>
|
||||
<license>Apache License 2.0</license>
|
||||
<author email="munseng.phoon@flexiv.com">Mun Seng Phoon</author>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<exec_depend>controller_manager</exec_depend>
|
||||
<exec_depend>flexiv_description</exec_depend>
|
||||
<exec_depend>flexiv_gripper</exec_depend>
|
||||
<exec_depend>flexiv_hardware</exec_depend>
|
||||
<exec_depend>flexiv_robot_states_broadcaster</exec_depend>
|
||||
<exec_depend>flexiv_test_nodes</exec_depend>
|
||||
<exec_depend>forward_command_controller</exec_depend>
|
||||
<exec_depend>joint_state_broadcaster</exec_depend>
|
||||
<exec_depend>joint_state_publisher</exec_depend>
|
||||
<exec_depend>joint_state_publisher_gui</exec_depend>
|
||||
<exec_depend>joint_trajectory_controller</exec_depend>
|
||||
<exec_depend>robot_state_publisher</exec_depend>
|
||||
<exec_depend>rviz2</exec_depend>
|
||||
<exec_depend>xacro</exec_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,50 @@
|
||||
# generated from colcon_zsh/shell/template/package.zsh.em
|
||||
|
||||
# This script extends the environment for this package.
|
||||
|
||||
# a zsh script is able to determine its own path if necessary
|
||||
if [ -z "$COLCON_CURRENT_PREFIX" ]; then
|
||||
# the prefix is two levels up from the package specific share directory
|
||||
_colcon_package_zsh_COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`/../.." > /dev/null && pwd)"
|
||||
else
|
||||
_colcon_package_zsh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
|
||||
fi
|
||||
|
||||
# function to source another script with conditional trace output
|
||||
# first argument: the path of the script
|
||||
# additional arguments: arguments to the script
|
||||
_colcon_package_zsh_source_script() {
|
||||
if [ -f "$1" ]; then
|
||||
if [ -n "$COLCON_TRACE" ]; then
|
||||
echo "# . \"$1\""
|
||||
fi
|
||||
. "$@"
|
||||
else
|
||||
echo "not found: \"$1\"" 1>&2
|
||||
fi
|
||||
}
|
||||
|
||||
# function to convert array-like strings into arrays
|
||||
# to workaround SH_WORD_SPLIT not being set
|
||||
colcon_zsh_convert_to_array() {
|
||||
local _listname=$1
|
||||
local _dollar="$"
|
||||
local _split="{="
|
||||
local _to_array="(\"$_dollar$_split$_listname}\")"
|
||||
eval $_listname=$_to_array
|
||||
}
|
||||
|
||||
# source sh script of this package
|
||||
_colcon_package_zsh_source_script "$_colcon_package_zsh_COLCON_CURRENT_PREFIX/share/flexiv_bringup/package.sh"
|
||||
unset convert_zsh_to_array
|
||||
|
||||
# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced scripts
|
||||
COLCON_CURRENT_PREFIX="$_colcon_package_zsh_COLCON_CURRENT_PREFIX"
|
||||
|
||||
# source zsh hooks
|
||||
_colcon_package_zsh_source_script "$COLCON_CURRENT_PREFIX/share/flexiv_bringup/local_setup.zsh"
|
||||
|
||||
unset COLCON_CURRENT_PREFIX
|
||||
|
||||
unset _colcon_package_zsh_source_script
|
||||
unset _colcon_package_zsh_COLCON_CURRENT_PREFIX
|
||||
Reference in New Issue
Block a user