Bill Finger
a7b7225dd1
- Removed `assembly_config_service.py` node from launch configuration. - Added default `goal.duration` setting to `MoveToPose` and `MoveToPoseArray`. - Replaced `timeout_seconds` with `duration` in action definitions for `MoveitSendJointStates` and `MoveitSendPose`. - Removed dependencies on TinyXML2 and Gazebo/SDFormat, adding `controller_manager_msgs` and `control_msgs` to CMake configuration. - Added new action servers `cartesian_move_to_pose` and `move_to_joint_states`, registering them in CMakeLists file. - Introduced `SkillBase`, a template class for managing action-based skills, providing essential ROS 2 action server support and functionalities for handling goals, cancels, accepted actions, and controller management. - Implemented methods to load, configure, and switch required controllers with conflict detection for active controllers, along with parameter checking and asynchronous handling for required parameters. - Enhanced error handling for missing controllers, parameters, and resource conflicts. - Updated `skills.launch.py` to utilize `ComposableNodeContainer` for skill nodes, incorporating `MoveToJointStateActionServer` and `CartesianMoveToPose` as composable nodes. - Changed the executable name in `cartesian_move_to_pose_action_server` node configuration. - Added `cartesian_move_to_pose.cpp`, implementing the `CartesianMoveToPose` action server, including trajectory interpolation, pose adjustment, and controller management. - Updated `package.xml` to include `rclcpp_components` dependency. - Refactored `MoveToJointStateActionServer` to extend `SkillBase`, leveraging `FollowJointTrajectory` for joint trajectory execution, while removing redundant code and dependencies. - Implemented trajectory generation based on initial and target joint positions with parameterized interpolation for smoother execution, enhancing joint state handling to dynamically align current and target joint values.
403 lines
13 KiB
Python
403 lines
13 KiB
Python
import os
|
|
|
|
import xacro
|
|
from ament_index_python.packages import get_package_share_directory
|
|
from launch import LaunchDescription
|
|
from launch.actions import (
|
|
DeclareLaunchArgument,
|
|
IncludeLaunchDescription,
|
|
OpaqueFunction,
|
|
)
|
|
from launch.conditions import IfCondition
|
|
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
|
from launch.substitutions import (
|
|
Command,
|
|
FindExecutable,
|
|
LaunchConfiguration,
|
|
PathJoinSubstitution,
|
|
)
|
|
from launch_ros.actions import Node
|
|
from launch_ros.substitutions import FindPackageShare
|
|
|
|
|
|
def launch_setup(context, *args, **kwargs):
|
|
robot_type = LaunchConfiguration("robot_type")
|
|
with_gripper_condition = LaunchConfiguration("with_gripper")
|
|
controllers_file = LaunchConfiguration("controllers_file")
|
|
description_package = LaunchConfiguration("description_package")
|
|
description_file = LaunchConfiguration("description_file")
|
|
robot_name = LaunchConfiguration("robot_name")
|
|
use_moveit = LaunchConfiguration("use_moveit")
|
|
moveit_config_package = LaunchConfiguration("moveit_config_package")
|
|
moveit_config_file = LaunchConfiguration("moveit_config_file")
|
|
use_sim_time = LaunchConfiguration("use_sim_time")
|
|
hardware = LaunchConfiguration("hardware")
|
|
use_controllers = LaunchConfiguration("use_controllers")
|
|
gripper_name = LaunchConfiguration("gripper_name")
|
|
x_pos = LaunchConfiguration("x")
|
|
y_pos = LaunchConfiguration("y")
|
|
z_pos = LaunchConfiguration("z")
|
|
roll = LaunchConfiguration("roll")
|
|
pitch = LaunchConfiguration("pitch")
|
|
yaw = LaunchConfiguration("yaw")
|
|
namespace = LaunchConfiguration("namespace")
|
|
multi_robot = LaunchConfiguration("multi_robot")
|
|
robot_name = robot_name.perform(context)
|
|
namespace = namespace.perform(context)
|
|
robot_type = robot_type.perform(context)
|
|
description_package = description_package.perform(context)
|
|
description_file = description_file.perform(context)
|
|
controllers_file = controllers_file.perform(context)
|
|
multi_robot = multi_robot.perform(context)
|
|
robot_description = LaunchConfiguration("robot_description")
|
|
robot_description_semantic = LaunchConfiguration("robot_description_semantic")
|
|
control_space = LaunchConfiguration("control_space")
|
|
control_strategy = LaunchConfiguration("control_strategy")
|
|
|
|
remappings = []
|
|
if multi_robot == "true":
|
|
remappings.append([("/tf", "tf"), ("/tf_static", "tf_static")])
|
|
|
|
controllers_file = os.path.join(
|
|
get_package_share_directory(description_package), "config", controllers_file
|
|
)
|
|
|
|
if not robot_description.perform(context):
|
|
xacro_file = os.path.join(
|
|
get_package_share_directory(description_package), "urdf", description_file
|
|
)
|
|
robot_description_doc = xacro.process_file(
|
|
xacro_file,
|
|
mappings={
|
|
"gripper_name": gripper_name.perform(context),
|
|
"hardware": hardware.perform(context),
|
|
"simulation_controllers": controllers_file,
|
|
"namespace": namespace,
|
|
"x": x_pos.perform(context),
|
|
"y": y_pos.perform(context),
|
|
"z": z_pos.perform(context),
|
|
"roll": roll.perform(context),
|
|
"pitch": pitch.perform(context),
|
|
"yaw": yaw.perform(context),
|
|
},
|
|
)
|
|
|
|
robot_description_content = robot_description_doc.toprettyxml(indent=" ")
|
|
else:
|
|
robot_description_content = robot_description.perform(context)
|
|
|
|
robot_description = {"robot_description": robot_description_content}
|
|
|
|
if not robot_description_semantic.perform(context):
|
|
robot_description_semantic_content = Command(
|
|
[
|
|
PathJoinSubstitution([FindExecutable(name="xacro")]),
|
|
" ",
|
|
PathJoinSubstitution(
|
|
[
|
|
FindPackageShare(moveit_config_package),
|
|
"config/moveit",
|
|
"rbs_arm.srdf.xacro",
|
|
]
|
|
),
|
|
" ",
|
|
"name:=",
|
|
robot_type,
|
|
" ",
|
|
"with_gripper:=",
|
|
with_gripper_condition,
|
|
" ",
|
|
"gripper_name:=",
|
|
gripper_name,
|
|
" ",
|
|
]
|
|
)
|
|
else:
|
|
robot_description_semantic_content = robot_description_semantic
|
|
|
|
robot_description_semantic = {
|
|
"robot_description_semantic": robot_description_semantic_content
|
|
}
|
|
|
|
robot_description_kinematics = PathJoinSubstitution(
|
|
[FindPackageShare(moveit_config_package), "config", "kinematics.yaml"]
|
|
)
|
|
|
|
robot_description_kinematics = {
|
|
"robot_description_kinematics": robot_description_kinematics
|
|
}
|
|
|
|
robot_state_publisher = Node(
|
|
package="robot_state_publisher",
|
|
executable="robot_state_publisher",
|
|
namespace=namespace,
|
|
output="both",
|
|
remappings=remappings,
|
|
parameters=[{"use_sim_time": use_sim_time}, robot_description],
|
|
)
|
|
|
|
control = IncludeLaunchDescription(
|
|
PythonLaunchDescriptionSource(
|
|
[
|
|
PathJoinSubstitution(
|
|
[
|
|
FindPackageShare(description_package),
|
|
"launch",
|
|
"control.launch.py",
|
|
]
|
|
)
|
|
]
|
|
),
|
|
launch_arguments={
|
|
"control_space": control_space,
|
|
"control_strategy": control_strategy,
|
|
"has_gripper": with_gripper_condition,
|
|
"namespace": namespace,
|
|
}.items(),
|
|
condition=IfCondition(use_controllers),
|
|
)
|
|
|
|
moveit = IncludeLaunchDescription(
|
|
PythonLaunchDescriptionSource(
|
|
[
|
|
PathJoinSubstitution(
|
|
[
|
|
FindPackageShare(moveit_config_package),
|
|
"launch",
|
|
"moveit.launch.py",
|
|
]
|
|
)
|
|
]
|
|
),
|
|
launch_arguments={
|
|
"robot_description": robot_description_content,
|
|
"moveit_config_package": moveit_config_package,
|
|
"moveit_config_file": moveit_config_file,
|
|
"use_sim_time": use_sim_time,
|
|
"tf_prefix": robot_name,
|
|
"with_gripper": with_gripper_condition,
|
|
"robot_description_semantic": robot_description_semantic_content,
|
|
"namespace": namespace,
|
|
}.items(),
|
|
condition=IfCondition(use_moveit),
|
|
)
|
|
|
|
skills = IncludeLaunchDescription(
|
|
PythonLaunchDescriptionSource(
|
|
[
|
|
PathJoinSubstitution(
|
|
[
|
|
FindPackageShare("rbs_skill_servers"),
|
|
"launch",
|
|
"skills.launch.py",
|
|
]
|
|
)
|
|
]
|
|
),
|
|
launch_arguments={
|
|
"robot_description": robot_description_content,
|
|
"robot_description_semantic": robot_description_semantic_content,
|
|
"robot_description_kinematics": robot_description_kinematics,
|
|
"use_sim_time": use_sim_time,
|
|
"with_gripper_condition": with_gripper_condition,
|
|
"namespace": namespace,
|
|
"use_moveit": use_moveit,
|
|
}.items(),
|
|
)
|
|
|
|
nodes_to_start = [
|
|
robot_state_publisher,
|
|
control,
|
|
moveit,
|
|
skills,
|
|
]
|
|
return nodes_to_start
|
|
|
|
|
|
def generate_launch_description():
|
|
declared_arguments = []
|
|
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"robot_type",
|
|
description="Type of robot to launch, specified by name.",
|
|
choices=["rbs_arm", "ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
|
|
default_value="rbs_arm",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"controllers_file",
|
|
default_value="rbs_arm0_controllers.yaml",
|
|
description="YAML file containing configuration settings for the controllers.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"description_package",
|
|
default_value="rbs_arm",
|
|
description="Package containing the robot's URDF/XACRO description files. Can be set to use a custom description.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"description_file",
|
|
default_value="rbs_arm_modular.xacro",
|
|
description="URDF/XACRO file describing the robot's model.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"robot_name",
|
|
default_value="arm0",
|
|
description="Prefix for joint names to support multi-robot setups. If changed, ensure joint names in controllers configuration are updated accordingly.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"moveit_config_package",
|
|
default_value="rbs_arm",
|
|
description="Package containing the MoveIt configuration files (SRDF/XACRO) for the robot. Can be customized.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"moveit_config_file",
|
|
default_value="rbs_arm.srdf.xacro",
|
|
description="MoveIt SRDF/XACRO file for robot configuration.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"use_sim_time",
|
|
default_value="true",
|
|
description="Enables simulation time in MoveIt, needed for trajectory planning in simulation.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"gripper_name",
|
|
default_value="",
|
|
choices=["rbs_gripper", ""],
|
|
description="Specify gripper name (leave empty if none).",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"with_gripper",
|
|
default_value="true",
|
|
description="Specify if the robot includes a gripper.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"use_moveit",
|
|
default_value="true",
|
|
description="Specify if MoveIt should be launched.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"hardware",
|
|
choices=["gazebo", "mock"],
|
|
default_value="gazebo",
|
|
description="Specify the hardware interface to use (e.g., Gazebo or mock).",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"use_controllers",
|
|
default_value="true",
|
|
description="Specify if controllers should be launched.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"namespace",
|
|
default_value="",
|
|
description="ROS 2 namespace for the robot.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"x",
|
|
default_value="0.0",
|
|
description="X-coordinate for the robot's position in the world.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"y",
|
|
default_value="0.0",
|
|
description="Y-coordinate for the robot's position in the world.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"z",
|
|
default_value="0.0",
|
|
description="Z-coordinate for the robot's position in the world.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"roll",
|
|
default_value="0.0",
|
|
description="Roll orientation of the robot in the world.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"pitch",
|
|
default_value="0.0",
|
|
description="Pitch orientation of the robot in the world.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"yaw",
|
|
default_value="0.0",
|
|
description="Yaw orientation of the robot in the world.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"multi_robot",
|
|
default_value="false",
|
|
description="Specify if the setup is for multiple robots.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"robot_description",
|
|
default_value="",
|
|
description="Custom robot description that overrides the internal default.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"control_space",
|
|
default_value="task",
|
|
choices=["task", "joint"],
|
|
description="Specify the control space for the robot (e.g., task space).",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"control_strategy",
|
|
default_value="position",
|
|
choices=["position", "velocity", "effort"],
|
|
description="Specify the control strategy (e.g., position control).",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"robot_description_semantic",
|
|
default_value="",
|
|
description="Custom semantic robot description (SRDF) to override the default.",
|
|
)
|
|
)
|
|
|
|
return LaunchDescription(
|
|
declared_arguments + [OpaqueFunction(function=launch_setup)]
|
|
)
|