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:
parent
b1e20696fe
commit
ef4b015491
6 changed files with 93 additions and 18 deletions
|
@ -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(),
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
|
@ -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)]
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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
|
||||||
|
|
40
rbs_utils/rbs_utils/launch/utils.launch.py
Normal file
40
rbs_utils/rbs_utils/launch/utils.launch.py
Normal 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)]
|
||||||
|
)
|
|
@ -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)
|
||||||
|
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue