refactor(rbs_skill_servers): update action configuration, dependencies, and skills
- 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.
This commit is contained in:
parent
4dccc2b74c
commit
a7b7225dd1
24 changed files with 1864 additions and 1204 deletions
|
|
@ -2,7 +2,8 @@ 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 Node
|
||||
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
|
||||
|
||||
|
||||
|
|
@ -11,73 +12,85 @@ def launch_setup(context, *args, **kwargs):
|
|||
robot_description_semantic_decl = LaunchConfiguration("robot_description_semantic")
|
||||
robot_description_kinematics = LaunchConfiguration("robot_description_kinematics")
|
||||
use_sim_time = LaunchConfiguration("use_sim_time")
|
||||
with_gripper_condition = LaunchConfiguration("with_gripper_condition")
|
||||
points_params_filepath_decl = LaunchConfiguration("points_params_filepath")
|
||||
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")
|
||||
|
||||
points_params = load_yaml("rbs_skill_servers", "config/gripperPositions.yaml")
|
||||
|
||||
kinematics_yaml = load_yaml("rbs_arm", "config/kinematics.yaml")
|
||||
|
||||
robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml}
|
||||
|
||||
move_topose_action_server = Node(
|
||||
package="rbs_skill_servers",
|
||||
executable="move_topose_action_server",
|
||||
skills_container = ComposableNodeContainer(
|
||||
name="skills",
|
||||
namespace=namespace,
|
||||
parameters=[
|
||||
robot_description,
|
||||
robot_description_semantic,
|
||||
robot_description_kinematics,
|
||||
{"use_sim_time": use_sim_time},
|
||||
],
|
||||
)
|
||||
|
||||
gripper_control_node = Node(
|
||||
package="rbs_skill_servers",
|
||||
executable="gripper_control_action_server",
|
||||
namespace=namespace,
|
||||
parameters=[
|
||||
robot_description,
|
||||
robot_description_semantic,
|
||||
robot_description_kinematics,
|
||||
{"use_sim_time": use_sim_time},
|
||||
],
|
||||
condition=IfCondition(with_gripper_condition),
|
||||
)
|
||||
|
||||
move_cartesian_path_action_server = Node(
|
||||
package="rbs_skill_servers",
|
||||
executable="move_cartesian_path_action_server",
|
||||
namespace=namespace,
|
||||
parameters=[
|
||||
robot_description,
|
||||
robot_description_semantic,
|
||||
robot_description_kinematics,
|
||||
{"use_sim_time": use_sim_time},
|
||||
],
|
||||
)
|
||||
|
||||
cartesian_move_to_pose_action_server = Node(
|
||||
package="rbs_skill_servers",
|
||||
executable="move_to_pose.py",
|
||||
namespace=namespace,
|
||||
parameters=[{"use_sim_time": use_sim_time}, {"robot_name": namespace}],
|
||||
)
|
||||
|
||||
move_joint_state_action_server = Node(
|
||||
package="rbs_skill_servers",
|
||||
executable="move_to_joint_states_action_server",
|
||||
namespace=namespace,
|
||||
parameters=[
|
||||
robot_description,
|
||||
robot_description_semantic,
|
||||
robot_description_kinematics,
|
||||
{"use_sim_time": use_sim_time},
|
||||
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),
|
||||
),
|
||||
],
|
||||
)
|
||||
|
||||
|
|
@ -90,12 +103,7 @@ def launch_setup(context, *args, **kwargs):
|
|||
|
||||
nodes_to_start = [
|
||||
assembly_config,
|
||||
move_topose_action_server,
|
||||
gripper_control_node,
|
||||
move_cartesian_path_action_server,
|
||||
move_joint_state_action_server,
|
||||
cartesian_move_to_pose_action_server,
|
||||
# grasp_pose_loader
|
||||
skills_container,
|
||||
]
|
||||
return nodes_to_start
|
||||
|
||||
|
|
@ -116,15 +124,18 @@ def generate_launch_description():
|
|||
declared_arguments.append(
|
||||
DeclareLaunchArgument("robot_description_kinematics", default_value="")
|
||||
)
|
||||
declared_arguments.append(DeclareLaunchArgument("use_sim_time", 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("points_params_filepath", default_value="")
|
||||
DeclareLaunchArgument("namespace", default_value="")
|
||||
)
|
||||
declared_arguments.append(DeclareLaunchArgument("namespace", default_value=""))
|
||||
|
||||
return LaunchDescription(
|
||||
declared_arguments + [OpaqueFunction(function=launch_setup)]
|
||||
)
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue