refactor and extend runtime configuration handling
- **Scene configuration:** - Replaced `bishop` object with `hole` and added a new `peg` object. - Enhanced object properties with new attributes like `randomize`, `relative_to`, `static`, and `texture`. - Updated `robot` joint positions in the default scene configuration. - **Launch file updates:** - Added handling for MoveIt SRDF file processing, including semantic descriptions. - Refactored unused parameters (`gripper_name`, `hardware`) and streamlined arguments. - Extended `robot_type` choices in the `generate_launch_description` method. - **Runtime initialization:** - Introduced `object_factory` to handle dynamic object instantiation based on type (`box`, `cylinder`, `mesh`, or `model`). - Enhanced `scene_config_loader` to process and instantiate objects using the factory function.
This commit is contained in:
parent
b71a2bdf55
commit
fbddca1b8b
3 changed files with 91 additions and 27 deletions
|
@ -67,7 +67,36 @@ light:
|
||||||
visual: true
|
visual: true
|
||||||
objects:
|
objects:
|
||||||
- color: null
|
- color: null
|
||||||
name: bishop
|
name: hole
|
||||||
|
orientation: !tuple
|
||||||
|
- 1.0
|
||||||
|
- 0.0
|
||||||
|
- 0.0
|
||||||
|
- 0.0
|
||||||
|
position: !tuple
|
||||||
|
- 0.1
|
||||||
|
- 0.3
|
||||||
|
- 0.1
|
||||||
|
randomize:
|
||||||
|
count: 0
|
||||||
|
models_rollouts_num: 0
|
||||||
|
random_color: false
|
||||||
|
random_model: false
|
||||||
|
random_orientation: false
|
||||||
|
random_pose: false
|
||||||
|
random_position: false
|
||||||
|
random_spawn_position_segments: []
|
||||||
|
random_spawn_position_update_workspace_centre: false
|
||||||
|
random_spawn_volume: !tuple
|
||||||
|
- 0.5
|
||||||
|
- 0.5
|
||||||
|
- 0.5
|
||||||
|
relative_to: world
|
||||||
|
static: false
|
||||||
|
texture: []
|
||||||
|
type: "model"
|
||||||
|
- color: null
|
||||||
|
name: peg
|
||||||
orientation: !tuple
|
orientation: !tuple
|
||||||
- 1.0
|
- 1.0
|
||||||
- 0.0
|
- 0.0
|
||||||
|
@ -75,8 +104,8 @@ objects:
|
||||||
- 0.0
|
- 0.0
|
||||||
position: !tuple
|
position: !tuple
|
||||||
- 0.0
|
- 0.0
|
||||||
- 1.0
|
|
||||||
- 0.3
|
- 0.3
|
||||||
|
- 0.1
|
||||||
randomize:
|
randomize:
|
||||||
count: 0
|
count: 0
|
||||||
models_rollouts_num: 0
|
models_rollouts_num: 0
|
||||||
|
@ -104,7 +133,7 @@ plugins:
|
||||||
robot:
|
robot:
|
||||||
gripper_joint_positions: 0.0
|
gripper_joint_positions: 0.0
|
||||||
joint_positions:
|
joint_positions:
|
||||||
- 1.57
|
- -1.57
|
||||||
- 0.5
|
- 0.5
|
||||||
- 3.14159
|
- 3.14159
|
||||||
- 1.5
|
- 1.5
|
||||||
|
|
|
@ -1,5 +1,6 @@
|
||||||
import os
|
import os
|
||||||
|
|
||||||
|
from launch.launch_introspector import indent
|
||||||
import xacro
|
import xacro
|
||||||
from ament_index_python.packages import get_package_share_directory
|
from ament_index_python.packages import get_package_share_directory
|
||||||
from launch import LaunchDescription
|
from launch import LaunchDescription
|
||||||
|
@ -26,13 +27,10 @@ def launch_setup(context, *args, **kwargs):
|
||||||
with_gripper_condition = LaunchConfiguration("with_gripper")
|
with_gripper_condition = LaunchConfiguration("with_gripper")
|
||||||
description_package = LaunchConfiguration("description_package")
|
description_package = LaunchConfiguration("description_package")
|
||||||
description_file = LaunchConfiguration("description_file")
|
description_file = LaunchConfiguration("description_file")
|
||||||
robot_name = LaunchConfiguration("robot_name")
|
|
||||||
use_moveit = LaunchConfiguration("use_moveit")
|
use_moveit = LaunchConfiguration("use_moveit")
|
||||||
moveit_config_package = LaunchConfiguration("moveit_config_package")
|
moveit_config_package = LaunchConfiguration("moveit_config_package")
|
||||||
moveit_config_file = LaunchConfiguration("moveit_config_file")
|
moveit_config_file = LaunchConfiguration("moveit_config_file")
|
||||||
use_sim_time = LaunchConfiguration("use_sim_time")
|
use_sim_time = LaunchConfiguration("use_sim_time")
|
||||||
hardware = LaunchConfiguration("hardware")
|
|
||||||
gripper_name = LaunchConfiguration("gripper_name")
|
|
||||||
scene_config_file = LaunchConfiguration("scene_config_file").perform(context)
|
scene_config_file = LaunchConfiguration("scene_config_file").perform(context)
|
||||||
|
|
||||||
ee_link_name = LaunchConfiguration("ee_link_name").perform(context)
|
ee_link_name = LaunchConfiguration("ee_link_name").perform(context)
|
||||||
|
@ -64,6 +62,7 @@ def launch_setup(context, *args, **kwargs):
|
||||||
|
|
||||||
xacro_config_file = f"{description_package_abs_path}/config/xacro_args.yaml"
|
xacro_config_file = f"{description_package_abs_path}/config/xacro_args.yaml"
|
||||||
|
|
||||||
|
|
||||||
# TODO: hide this to another place
|
# TODO: hide this to another place
|
||||||
# Load xacro_args
|
# Load xacro_args
|
||||||
def param_constructor(loader, node, local_vars):
|
def param_constructor(loader, node, local_vars):
|
||||||
|
@ -96,12 +95,29 @@ def launch_setup(context, *args, **kwargs):
|
||||||
|
|
||||||
robot_description_doc = xacro.process_file(xacro_file, mappings=mappings_data)
|
robot_description_doc = xacro.process_file(xacro_file, mappings=mappings_data)
|
||||||
|
|
||||||
|
robot_description_semantic_content = ""
|
||||||
|
|
||||||
|
if use_moveit.perform(context) == "true":
|
||||||
|
srdf_config_file = f"{description_package_abs_path}/config/srdf_xacro_args.yaml"
|
||||||
|
srdf_file = os.path.join(
|
||||||
|
get_package_share_directory(moveit_config_package.perform(context)),
|
||||||
|
"srdf",
|
||||||
|
moveit_config_file.perform(context),
|
||||||
|
)
|
||||||
|
srdf_mappings = load_xacro_args(srdf_config_file, locals())
|
||||||
|
robot_description_semantic_content = xacro.process_file(srdf_file, mappings=srdf_mappings)
|
||||||
|
robot_description_semantic_content = robot_description_semantic_content.toprettyxml(indent=" ")
|
||||||
|
control_space = "joint"
|
||||||
|
control_strategy = "position"
|
||||||
|
interactive = "false"
|
||||||
|
|
||||||
|
|
||||||
robot_description_content = robot_description_doc.toprettyxml(indent=" ")
|
robot_description_content = robot_description_doc.toprettyxml(indent=" ")
|
||||||
robot_description = {"robot_description": robot_description_content}
|
robot_description = {"robot_description": robot_description_content}
|
||||||
|
|
||||||
# Parse robot and configure controller's file for ControllerManager
|
# Parse robot and configure controller's file for ControllerManager
|
||||||
robot = URDF_parser.load_string(
|
robot = URDF_parser.load_string(
|
||||||
robot_description_content, ee_link_name="gripper_grasp_point"
|
robot_description_content, ee_link_name=ee_link_name
|
||||||
)
|
)
|
||||||
ControllerManager.save_to_yaml(
|
ControllerManager.save_to_yaml(
|
||||||
robot, description_package_abs_path, "controllers.yaml"
|
robot, description_package_abs_path, "controllers.yaml"
|
||||||
|
@ -117,7 +133,6 @@ def launch_setup(context, *args, **kwargs):
|
||||||
),
|
),
|
||||||
launch_arguments={
|
launch_arguments={
|
||||||
"with_gripper": with_gripper_condition,
|
"with_gripper": with_gripper_condition,
|
||||||
"gripper_name": gripper_name,
|
|
||||||
"controllers_file": simulation_controllers,
|
"controllers_file": simulation_controllers,
|
||||||
"robot_type": robot_type,
|
"robot_type": robot_type,
|
||||||
"description_package": description_package,
|
"description_package": description_package,
|
||||||
|
@ -127,9 +142,9 @@ def launch_setup(context, *args, **kwargs):
|
||||||
"moveit_config_package": moveit_config_package,
|
"moveit_config_package": moveit_config_package,
|
||||||
"moveit_config_file": moveit_config_file,
|
"moveit_config_file": moveit_config_file,
|
||||||
"use_sim_time": use_sim_time,
|
"use_sim_time": use_sim_time,
|
||||||
"hardware": hardware,
|
|
||||||
"use_controllers": "true",
|
"use_controllers": "true",
|
||||||
"robot_description": robot_description_content,
|
"robot_description": robot_description_content,
|
||||||
|
"robot_description_semantic": robot_description_semantic_content,
|
||||||
"base_link_name": base_link_name,
|
"base_link_name": base_link_name,
|
||||||
"ee_link_name": ee_link_name,
|
"ee_link_name": ee_link_name,
|
||||||
"control_space": control_space,
|
"control_space": control_space,
|
||||||
|
@ -153,7 +168,11 @@ def launch_setup(context, *args, **kwargs):
|
||||||
|
|
||||||
delay_robot_control_stack = TimerAction(period=10.0, actions=[rbs_robot_setup])
|
delay_robot_control_stack = TimerAction(period=10.0, actions=[rbs_robot_setup])
|
||||||
|
|
||||||
nodes_to_start = [rbs_runtime, clock_bridge, delay_robot_control_stack]
|
nodes_to_start = [
|
||||||
|
rbs_runtime,
|
||||||
|
clock_bridge,
|
||||||
|
delay_robot_control_stack
|
||||||
|
]
|
||||||
return nodes_to_start
|
return nodes_to_start
|
||||||
|
|
||||||
|
|
||||||
|
@ -163,7 +182,17 @@ def generate_launch_description():
|
||||||
DeclareLaunchArgument(
|
DeclareLaunchArgument(
|
||||||
"robot_type",
|
"robot_type",
|
||||||
description="Type of robot by name",
|
description="Type of robot by name",
|
||||||
choices=["rbs_arm", "ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
|
choices=[
|
||||||
|
"rbs_arm",
|
||||||
|
"ar4",
|
||||||
|
"ur3",
|
||||||
|
"ur3e",
|
||||||
|
"ur5",
|
||||||
|
"ur5e",
|
||||||
|
"ur10",
|
||||||
|
"ur10e",
|
||||||
|
"ur16e",
|
||||||
|
],
|
||||||
default_value="rbs_arm",
|
default_value="rbs_arm",
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
|
@ -212,14 +241,6 @@ def generate_launch_description():
|
||||||
This is needed for the trajectory planing in simulation.",
|
This is needed for the trajectory planing in simulation.",
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
declared_arguments.append(
|
|
||||||
DeclareLaunchArgument(
|
|
||||||
"gripper_name",
|
|
||||||
default_value="rbs_gripper",
|
|
||||||
choices=["rbs_gripper", ""],
|
|
||||||
description="choose gripper by name (leave empty if hasn't)",
|
|
||||||
)
|
|
||||||
)
|
|
||||||
declared_arguments.append(
|
declared_arguments.append(
|
||||||
DeclareLaunchArgument(
|
DeclareLaunchArgument(
|
||||||
"with_gripper", default_value="true", description="With gripper or not?"
|
"with_gripper", default_value="true", description="With gripper or not?"
|
||||||
|
@ -235,14 +256,6 @@ def generate_launch_description():
|
||||||
"launch_perception", default_value="false", description="Launch perception?"
|
"launch_perception", default_value="false", description="Launch perception?"
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
declared_arguments.append(
|
|
||||||
DeclareLaunchArgument(
|
|
||||||
"hardware",
|
|
||||||
choices=["gazebo", "mock"],
|
|
||||||
default_value="gazebo",
|
|
||||||
description="Choose your harware_interface",
|
|
||||||
)
|
|
||||||
)
|
|
||||||
declared_arguments.append(
|
declared_arguments.append(
|
||||||
DeclareLaunchArgument(
|
DeclareLaunchArgument(
|
||||||
"use_controllers",
|
"use_controllers",
|
||||||
|
|
|
@ -4,6 +4,24 @@ import yaml
|
||||||
from dacite import from_dict
|
from dacite import from_dict
|
||||||
from env_manager.models.configs import SceneData
|
from env_manager.models.configs import SceneData
|
||||||
|
|
||||||
|
from typing import Dict, Any
|
||||||
|
from env_manager.models.configs import (
|
||||||
|
BoxObjectData, CylinderObjectData, MeshData, ModelData, ObjectData
|
||||||
|
)
|
||||||
|
|
||||||
|
def object_factory(obj_data: Dict[str, Any]) -> ObjectData:
|
||||||
|
obj_type = obj_data.get('type', '')
|
||||||
|
|
||||||
|
if obj_type == 'box':
|
||||||
|
return BoxObjectData(**obj_data)
|
||||||
|
elif obj_type == 'cylinder':
|
||||||
|
return CylinderObjectData(**obj_data)
|
||||||
|
elif obj_type == 'mesh':
|
||||||
|
return MeshData(**obj_data)
|
||||||
|
elif obj_type == 'model':
|
||||||
|
return ModelData(**obj_data)
|
||||||
|
else:
|
||||||
|
return ObjectData(**obj_data)
|
||||||
|
|
||||||
def scene_config_loader(file: str | Path) -> SceneData:
|
def scene_config_loader(file: str | Path) -> SceneData:
|
||||||
def tuple_constructor(loader, node):
|
def tuple_constructor(loader, node):
|
||||||
|
@ -15,4 +33,8 @@ def scene_config_loader(file: str | Path) -> SceneData:
|
||||||
|
|
||||||
scene_data = from_dict(data_class=SceneData, data=scene_cfg)
|
scene_data = from_dict(data_class=SceneData, data=scene_cfg)
|
||||||
|
|
||||||
|
scene_data.objects = [object_factory(obj) for obj in scene_cfg.get('objects', [])]
|
||||||
|
|
||||||
|
print(scene_data)
|
||||||
|
|
||||||
return scene_data
|
return scene_data
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue