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.
141 lines
5 KiB
Python
141 lines
5 KiB
Python
from launch import LaunchDescription
|
|
from launch.actions import DeclareLaunchArgument, OpaqueFunction
|
|
from launch.conditions import IfCondition
|
|
from launch.substitutions import LaunchConfiguration
|
|
from launch_ros.actions import ComposableNodeContainer, Node
|
|
from launch_ros.actions.composable_node_container import ComposableNode
|
|
from rbs_launch_utils.launch_common import load_yaml
|
|
|
|
|
|
def launch_setup(context, *args, **kwargs):
|
|
robot_description_decl = LaunchConfiguration("robot_description")
|
|
robot_description_semantic_decl = LaunchConfiguration("robot_description_semantic")
|
|
robot_description_kinematics = LaunchConfiguration("robot_description_kinematics")
|
|
use_sim_time = LaunchConfiguration("use_sim_time")
|
|
use_moveit = LaunchConfiguration("use_moveit")
|
|
# with_gripper_condition = LaunchConfiguration("with_gripper_condition")
|
|
|
|
robot_description = {"robot_description": robot_description_decl}
|
|
robot_description_semantic = {
|
|
"robot_description_semantic": robot_description_semantic_decl
|
|
}
|
|
namespace = LaunchConfiguration("namespace")
|
|
|
|
kinematics_yaml = load_yaml("rbs_arm", "config/kinematics.yaml")
|
|
|
|
robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml}
|
|
|
|
skills_container = ComposableNodeContainer(
|
|
name="skills",
|
|
namespace=namespace,
|
|
package="rclcpp_components",
|
|
# prefix=['gdbserver localhost:1234'],
|
|
executable="component_container_mt",
|
|
composable_node_descriptions=[
|
|
ComposableNode(
|
|
package="rbs_skill_servers",
|
|
plugin="rbs_skill_actions::MoveToJointStateActionServer",
|
|
name="mtjs_jtc",
|
|
parameters=[{"use_sim_time": use_sim_time}],
|
|
),
|
|
ComposableNode(
|
|
package="rbs_skill_servers",
|
|
plugin="rbs_skill_actions::CartesianMoveToPose",
|
|
name="mtp_cart",
|
|
parameters=[
|
|
{"use_sim_time": use_sim_time},
|
|
{"robot_name": namespace},
|
|
],
|
|
),
|
|
ComposableNode(
|
|
package="rbs_skill_servers",
|
|
plugin="rbs_skill_actions::MoveToPose",
|
|
name="mtp_jtc",
|
|
parameters=[
|
|
{"use_sim_time": use_sim_time},
|
|
{"robot_name": namespace},
|
|
robot_description,
|
|
],
|
|
),
|
|
ComposableNode(
|
|
package="rbs_skill_servers",
|
|
plugin="rbs_skill_actions::MoveToPoseJtcCartesian",
|
|
name="mtp_jtc_cart",
|
|
parameters=[
|
|
{"use_sim_time": use_sim_time},
|
|
{"robot_name": namespace},
|
|
robot_description,
|
|
],
|
|
),
|
|
ComposableNode(
|
|
package="rbs_skill_servers",
|
|
plugin="rbs_skill_actions::MoveitMtp",
|
|
name="mtp_moveit",
|
|
parameters=[
|
|
robot_description,
|
|
robot_description_semantic,
|
|
robot_description_kinematics,
|
|
{"use_sim_time": use_sim_time},
|
|
],
|
|
condition=IfCondition(use_moveit),
|
|
),
|
|
ComposableNode(
|
|
package="rbs_skill_servers",
|
|
plugin="rbs_skill_actions::MoveitMtpCart",
|
|
name="mtp_moveit_cart",
|
|
parameters=[
|
|
robot_description,
|
|
robot_description_semantic,
|
|
robot_description_kinematics,
|
|
{"use_sim_time": use_sim_time},
|
|
],
|
|
condition=IfCondition(use_moveit),
|
|
),
|
|
],
|
|
)
|
|
|
|
assembly_config = Node(
|
|
package="rbs_utils",
|
|
executable="assembly_config_service.py",
|
|
namespace=namespace,
|
|
output="screen",
|
|
)
|
|
|
|
nodes_to_start = [
|
|
assembly_config,
|
|
skills_container,
|
|
]
|
|
return nodes_to_start
|
|
|
|
|
|
def generate_launch_description():
|
|
declared_arguments = []
|
|
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"robot_description",
|
|
default_value="",
|
|
description="robot description string",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument("robot_description_semantic", default_value="")
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument("robot_description_kinematics", default_value="")
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument("use_sim_time", default_value="false")
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument("use_moveit", default_value="false")
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument("with_gripper_condition", default_value="")
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument("namespace", default_value="")
|
|
)
|
|
return LaunchDescription(
|
|
declared_arguments + [OpaqueFunction(function=launch_setup)]
|
|
)
|