From ef4b0154917f5e8467b8e51bdeec9cb27a74e5bb Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Fri, 6 Dec 2024 12:14:29 +0300 Subject: [PATCH] Add assembly configuration integration and refactor launches - **Added support for assembly configuration in `rbs_bringup` and `rbs_robot` launches**: - Introduced new launch arguments: `use_rbs_utils` and `assembly_config_name`. - Integrated `rbs_utils`'s `utils.launch.py` for handling assembly configurations. - **Simplified `skills.launch.py`**: - Removed redundant `assembly_config` node setup. - **Enhanced `rbs_utils`**: - Added installation of `launch` files in `CMakeLists.txt`. - Created a new `utils.launch.py` for dynamically loading assembly configurations. - Refactored `assembly_config_service.py` to utilize `get_asm_config` for streamlined configuration file resolution. - Improved `rbs_bringup` setup to include additional parameters and nodes for assembly configuration. These changes centralize assembly configuration handling and enhance modularity across launch setups. --- rbs_bringup/launch/rbs_bringup.launch.py | 4 +- rbs_bringup/launch/rbs_robot.launch.py | 39 ++++++++++++++++++ rbs_skill_servers/launch/skills.launch.py | 9 ----- rbs_utils/rbs_utils/CMakeLists.txt | 5 +++ rbs_utils/rbs_utils/launch/utils.launch.py | 40 +++++++++++++++++++ .../scripts/assembly_config_service.py | 14 +++---- 6 files changed, 93 insertions(+), 18 deletions(-) create mode 100644 rbs_utils/rbs_utils/launch/utils.launch.py diff --git a/rbs_bringup/launch/rbs_bringup.launch.py b/rbs_bringup/launch/rbs_bringup.launch.py index b23d71c..50db4b6 100644 --- a/rbs_bringup/launch/rbs_bringup.launch.py +++ b/rbs_bringup/launch/rbs_bringup.launch.py @@ -35,7 +35,9 @@ def launch_setup(context, *args, **kwargs): "ee_link_name": "gripper_grasp_point", "control_space": "task", "control_strategy": "position", - "interactive": "false" + "interactive": "false", + "use_rbs_utils": "true", + "assembly_config_name": "board_pick_and_place" }.items(), ) diff --git a/rbs_bringup/launch/rbs_robot.launch.py b/rbs_bringup/launch/rbs_robot.launch.py index 2c2ff21..a36b396 100644 --- a/rbs_bringup/launch/rbs_robot.launch.py +++ b/rbs_bringup/launch/rbs_robot.launch.py @@ -48,6 +48,9 @@ def launch_setup(context, *args, **kwargs): ee_link_name = LaunchConfiguration("ee_link_name").perform(context) base_link_name = LaunchConfiguration("base_link_name").perform(context) + use_rbs_utils = LaunchConfiguration("use_sim_time") + assembly_config_name = LaunchConfiguration("assembly_config_name") + remappings = [] if multi_robot == "true": remappings.append([("/tf", "tf"), ("/tf_static", "tf_static")]) @@ -148,11 +151,33 @@ def launch_setup(context, *args, **kwargs): condition=IfCondition(use_skills), ) + + # assembly config loader + utils = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + PathJoinSubstitution( + [ + FindPackageShare("rbs_utils"), + "launch", + "utils.launch.py", + ] + ) + ] + ), + launch_arguments={ + "use_sim_time": use_sim_time, + "assembly_config_name": assembly_config_name + }.items(), + condition=IfCondition(use_rbs_utils), + ) + nodes_to_start = [ robot_state_publisher, control, moveit, skills, + utils, ] return nodes_to_start @@ -364,6 +389,20 @@ def generate_launch_description(): description="Base link name if robot arm", ) ) + declared_arguments.append( + DeclareLaunchArgument( + "assembly_config_name", + default_value="", + description="Assembly config name", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_rbs_utils", + default_value="true", + description="Whwther use utils", + ) + ) return LaunchDescription( declared_arguments + [OpaqueFunction(function=launch_setup)] diff --git a/rbs_skill_servers/launch/skills.launch.py b/rbs_skill_servers/launch/skills.launch.py index d971900..abd654f 100644 --- a/rbs_skill_servers/launch/skills.launch.py +++ b/rbs_skill_servers/launch/skills.launch.py @@ -99,16 +99,7 @@ def launch_setup(context, *args, **kwargs): ], ) - assembly_config = Node( - package="rbs_utils", - executable="assembly_config_service.py", - namespace=namespace, - parameters=[{"use_sim_time": use_sim_time}], - output="screen", - ) - nodes_to_start = [ - assembly_config, skills_container, ] return nodes_to_start diff --git a/rbs_utils/rbs_utils/CMakeLists.txt b/rbs_utils/rbs_utils/CMakeLists.txt index 756cfb6..92f9442 100644 --- a/rbs_utils/rbs_utils/CMakeLists.txt +++ b/rbs_utils/rbs_utils/CMakeLists.txt @@ -49,6 +49,11 @@ install( DESTINATION include ) +install( + DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + ament_target_dependencies(${PROJECT_NAME} PUBLIC ${deps}) # target_include_directories(asm_folder_process diff --git a/rbs_utils/rbs_utils/launch/utils.launch.py b/rbs_utils/rbs_utils/launch/utils.launch.py new file mode 100644 index 0000000..783efa4 --- /dev/null +++ b/rbs_utils/rbs_utils/launch/utils.launch.py @@ -0,0 +1,40 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def launch_setup(context, *args, **kwargs): + asm_config_name = LaunchConfiguration("assembly_config_name") + namespace = LaunchConfiguration("namespace") + use_sim_time = LaunchConfiguration("use_sim_time") + + assembly_config = Node( + package="rbs_utils", + executable="assembly_config_service.py", + namespace=namespace, + parameters=[{"use_sim_time": use_sim_time}, {"assembly_config_name": asm_config_name.perform(context)}], + output="screen", + ) + + nodes_to_start = [ + assembly_config, + ] + return nodes_to_start + + +def generate_launch_description(): + declared_arguments = [] + + declared_arguments.append( + DeclareLaunchArgument("assembly_config_name", default_value="") + ) + declared_arguments.append( + DeclareLaunchArgument("use_sim_time", default_value="false") + ) + declared_arguments.append( + DeclareLaunchArgument("namespace", default_value="") + ) + return LaunchDescription( + declared_arguments + [OpaqueFunction(function=launch_setup)] + ) diff --git a/rbs_utils/rbs_utils/scripts/assembly_config_service.py b/rbs_utils/rbs_utils/scripts/assembly_config_service.py index e932949..594e244 100755 --- a/rbs_utils/rbs_utils/scripts/assembly_config_service.py +++ b/rbs_utils/rbs_utils/scripts/assembly_config_service.py @@ -5,11 +5,9 @@ import rclpy from rclpy.node import Node import yaml from geometry_msgs.msg import Point, Pose, Quaternion -from visualization_msgs.msg import Marker, MarkerArray from rbs_utils_interfaces.msg import NamedPose, RelativeNamedPose, AssemblyConfig from rbs_utils_interfaces.srv import GetGraspPose, GetWorkspace -from tf2_ros import TransformListener, Buffer, TransformException -from rbs_assets_library import get_model_meshes_info +from rbs_assets_library import get_asm_config from env_manager.utils import Tf2Broadcaster @@ -18,12 +16,12 @@ class AssemblyConfigService(Node): super().__init__(node_name) # Initialize parameters - self.declare_parameter("db_folder", "asp-example") - db_folder = self.get_parameter("db_folder").get_parameter_value().string_value - - # Parse the YAML file + self.declare_parameter("assembly_config_name", "board_pick_and_place") + config_name = self.get_parameter("assembly_config_name").get_parameter_value().string_value + self.get_logger().info(f"Loading assembly config with name: {config_name}") + asm_config_filepath = get_asm_config(config_name) yaml_file = os.path.join( - os.getenv("RBS_ASSEMBLY_DIR", ""), db_folder, "rbs_db.yaml" + asm_config_filepath ) self.assembly_config = parse_yaml(yaml_file)