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.
This commit is contained in:
Ilya Uraev 2024-12-06 12:14:29 +03:00
parent b1e20696fe
commit ef4b015491
6 changed files with 93 additions and 18 deletions

View file

@ -35,7 +35,9 @@ def launch_setup(context, *args, **kwargs):
"ee_link_name": "gripper_grasp_point", "ee_link_name": "gripper_grasp_point",
"control_space": "task", "control_space": "task",
"control_strategy": "position", "control_strategy": "position",
"interactive": "false" "interactive": "false",
"use_rbs_utils": "true",
"assembly_config_name": "board_pick_and_place"
}.items(), }.items(),
) )

View file

@ -48,6 +48,9 @@ def launch_setup(context, *args, **kwargs):
ee_link_name = LaunchConfiguration("ee_link_name").perform(context) ee_link_name = LaunchConfiguration("ee_link_name").perform(context)
base_link_name = LaunchConfiguration("base_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 = [] remappings = []
if multi_robot == "true": if multi_robot == "true":
remappings.append([("/tf", "tf"), ("/tf_static", "tf_static")]) remappings.append([("/tf", "tf"), ("/tf_static", "tf_static")])
@ -148,11 +151,33 @@ def launch_setup(context, *args, **kwargs):
condition=IfCondition(use_skills), 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 = [ nodes_to_start = [
robot_state_publisher, robot_state_publisher,
control, control,
moveit, moveit,
skills, skills,
utils,
] ]
return nodes_to_start return nodes_to_start
@ -364,6 +389,20 @@ def generate_launch_description():
description="Base link name if robot arm", 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( return LaunchDescription(
declared_arguments + [OpaqueFunction(function=launch_setup)] declared_arguments + [OpaqueFunction(function=launch_setup)]

View file

@ -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 = [ nodes_to_start = [
assembly_config,
skills_container, skills_container,
] ]
return nodes_to_start return nodes_to_start

View file

@ -49,6 +49,11 @@ install(
DESTINATION include DESTINATION include
) )
install(
DIRECTORY launch
DESTINATION share/${PROJECT_NAME}
)
ament_target_dependencies(${PROJECT_NAME} PUBLIC ${deps}) ament_target_dependencies(${PROJECT_NAME} PUBLIC ${deps})
# target_include_directories(asm_folder_process # target_include_directories(asm_folder_process

View file

@ -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)]
)

View file

@ -5,11 +5,9 @@ import rclpy
from rclpy.node import Node from rclpy.node import Node
import yaml import yaml
from geometry_msgs.msg import Point, Pose, Quaternion 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.msg import NamedPose, RelativeNamedPose, AssemblyConfig
from rbs_utils_interfaces.srv import GetGraspPose, GetWorkspace from rbs_utils_interfaces.srv import GetGraspPose, GetWorkspace
from tf2_ros import TransformListener, Buffer, TransformException from rbs_assets_library import get_asm_config
from rbs_assets_library import get_model_meshes_info
from env_manager.utils import Tf2Broadcaster from env_manager.utils import Tf2Broadcaster
@ -18,12 +16,12 @@ class AssemblyConfigService(Node):
super().__init__(node_name) super().__init__(node_name)
# Initialize parameters # Initialize parameters
self.declare_parameter("db_folder", "asp-example") self.declare_parameter("assembly_config_name", "board_pick_and_place")
db_folder = self.get_parameter("db_folder").get_parameter_value().string_value 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}")
# Parse the YAML file asm_config_filepath = get_asm_config(config_name)
yaml_file = os.path.join( 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) self.assembly_config = parse_yaml(yaml_file)