From dba3b8682a1c7f3aae9e2443d125527119ae99e3 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Mon, 14 Oct 2024 14:21:30 +0300 Subject: [PATCH] refactor: (env_manager, rbs_runtime) and add YAML config loading - Updated `camera.py` to use consistent float literals for default spawn positions. - Removed unused import statements and organized imports in `robot.py` and `scene.py`. - Removed debugging import from `scene.py` to clean up codebase. - Added a new YAML configuration file (`default-scene-config.yaml`) for scene settings. - Implemented a scene configuration loader function in `rbs_runtime/__init__.py` to read and parse YAML config files. - Modified `runtime.py` to utilize the newly added scene config loader and removed in-code default scene setup. - Updated `setup.py` to include `config` directory in package data. - Changed `rbs_robot.launch.py` to use position control strategy by default. - Refactored `skills.launch.py` with cleaner parameter definitions and reactivated commented-out nodes. --- .../env_manager/models/configs/camera.py | 5 +- .../env_manager/models/configs/robot.py | 1 - .../env_manager/models/configs/scene.py | 5 +- .../env_manager/env_manager/scene/scene.py | 1 - .../config/default-scene-config.yaml | 152 ++++++++++++++++++ .../rbs_runtime/rbs_runtime/__init__.py | 18 +++ .../rbs_runtime/scripts/runtime.py | 44 +++-- env_manager/rbs_runtime/setup.py | 4 + rbs_bringup/launch/rbs_robot.launch.py | 2 +- rbs_skill_servers/launch/skills.launch.py | 66 ++++---- 10 files changed, 226 insertions(+), 72 deletions(-) create mode 100644 env_manager/rbs_runtime/config/default-scene-config.yaml diff --git a/env_manager/env_manager/env_manager/models/configs/camera.py b/env_manager/env_manager/env_manager/models/configs/camera.py index 378937a..c76e8d9 100644 --- a/env_manager/env_manager/env_manager/models/configs/camera.py +++ b/env_manager/env_manager/env_manager/models/configs/camera.py @@ -1,4 +1,5 @@ from dataclasses import dataclass, field + import numpy as np @@ -93,9 +94,9 @@ class CameraData: publish_depth: bool = field(default=False) publish_points: bool = field(default=False) - spawn_position: tuple[float, float, float] = field(default=(0, 0, 1)) + spawn_position: tuple[float, float, float] = field(default=(0.0, 0.0, 1.0)) spawn_quat_xyzw: tuple[float, float, float, float] = field( - default=(0, 0.70710678118, 0, 0.70710678118) + default=(0.0, 0.70710678118, 0.0, 0.70710678118) ) random_pose_rollouts_num: int = field(default=1) diff --git a/env_manager/env_manager/env_manager/models/configs/robot.py b/env_manager/env_manager/env_manager/models/configs/robot.py index 0aac524..b2f90ca 100644 --- a/env_manager/env_manager/env_manager/models/configs/robot.py +++ b/env_manager/env_manager/env_manager/models/configs/robot.py @@ -34,7 +34,6 @@ class GripperEnum(Enum): vacuum = VacuumGripperData - @dataclass class RobotRandomizerData: """ diff --git a/env_manager/env_manager/env_manager/models/configs/scene.py b/env_manager/env_manager/env_manager/models/configs/scene.py index 61ebef4..94d078f 100644 --- a/env_manager/env_manager/env_manager/models/configs/scene.py +++ b/env_manager/env_manager/env_manager/models/configs/scene.py @@ -1,9 +1,10 @@ from dataclasses import dataclass, field + +from .camera import CameraData +from .light import LightData from .objects import ObjectData from .robot import RobotData from .terrain import TerrainData -from .light import LightData -from .camera import CameraData @dataclass diff --git a/env_manager/env_manager/env_manager/scene/scene.py b/env_manager/env_manager/env_manager/scene/scene.py index 705a436..8cdac9a 100644 --- a/env_manager/env_manager/env_manager/scene/scene.py +++ b/env_manager/env_manager/env_manager/scene/scene.py @@ -1,6 +1,5 @@ import asyncio from dataclasses import asdict -# import debugpy import numpy as np from rcl_interfaces.srv import GetParameters diff --git a/env_manager/rbs_runtime/config/default-scene-config.yaml b/env_manager/rbs_runtime/config/default-scene-config.yaml new file mode 100644 index 0000000..e81ed5b --- /dev/null +++ b/env_manager/rbs_runtime/config/default-scene-config.yaml @@ -0,0 +1,152 @@ +camera: +- clip_color: !tuple + - 0.01 + - 1000.0 + clip_depth: !tuple + - 0.05 + - 10.0 + enable: true + height: 128 + horizontal_fov: 1.0471975511965976 + image_format: R8G8B8 + name: robot_camera + noise_mean: null + noise_stddev: null + publish_color: true + publish_depth: true + publish_points: true + random_pose_focal_point_z_offset: 0.0 + random_pose_mode: orbit + random_pose_orbit_distance: 1.0 + random_pose_orbit_height_range: !tuple + - 0.1 + - 0.7 + random_pose_orbit_ignore_arc_behind_robot: 0.39269908169872414 + random_pose_rollout_counter: 0.0 + random_pose_rollouts_num: 1 + random_pose_select_position_options: [] + relative_to: base_link + spawn_position: !tuple + - 0.0 + - 0.0 + - 1.0 + spawn_quat_xyzw: !tuple + - 0.0 + - 0.70710678118 + - 0.0 + - 0.70710678118 + type: rgbd_camera + update_rate: 10 + vertical_fov: 1.0471975511965976 + width: 128 +gravity: !tuple +- 0.0 +- 0.0 +- -9.80665 +gravity_std: !tuple +- 0.0 +- 0.0 +- 0.0232 +light: + color: !tuple + - 1.0 + - 1.0 + - 1.0 + - 1.0 + direction: !tuple + - 0.5 + - -0.25 + - -0.75 + distance: 1000.0 + model_rollouts_num: 1 + radius: 25.0 + random_minmax_elevation: !tuple + - -0.15 + - -0.65 + type: sun + visual: true +objects: +- color: null + name: bishop + orientation: !tuple + - 1.0 + - 0.0 + - 0.0 + - 0.0 + position: !tuple + - 0.0 + - 1.0 + - 0.3 + 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' +physics_rollouts_num: 0 +plugins: + fts_broadcaster: false + scene_broadcaster: false + sensors_render_engine: ogre2 + user_commands: false +robot: + gripper_joint_positions: 0.0 + joint_positions: + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + name: rbs_arm + randomizer: + joint_positions: false + joint_positions_above_object_spawn: false + joint_positions_above_object_spawn_elevation: 0.2 + joint_positions_above_object_spawn_xy_randomness: 0.2 + joint_positions_std: 0.1 + pose: false + spawn_volume: !tuple + - 1.0 + - 1.0 + - 0.0 + spawn_position: !tuple + - 0.0 + - 0.0 + - 0.0 + spawn_quat_xyzw: !tuple + - 0.0 + - 0.0 + - 0.0 + - 1.0 + urdf_string: '' + with_gripper: true +terrain: + model_rollouts_num: 1 + name: ground + size: !tuple + - 1.5 + - 1.5 + spawn_position: !tuple + - 0 + - 0 + - 0 + spawn_quat_xyzw: !tuple + - 0 + - 0 + - 0 + - 1 + type: flat diff --git a/env_manager/rbs_runtime/rbs_runtime/__init__.py b/env_manager/rbs_runtime/rbs_runtime/__init__.py index e69de29..2659497 100644 --- a/env_manager/rbs_runtime/rbs_runtime/__init__.py +++ b/env_manager/rbs_runtime/rbs_runtime/__init__.py @@ -0,0 +1,18 @@ +from pathlib import Path + +import yaml +from dacite import from_dict +from env_manager.models.configs import SceneData + + +def scene_config_loader(file: str | Path) -> SceneData: + def tuple_constructor(loader, node): + return tuple(loader.construct_sequence(node)) + + with open(file, "r") as yaml_file: + yaml.SafeLoader.add_constructor("!tuple", tuple_constructor) + scene_cfg = yaml.load(yaml_file, Loader=yaml.SafeLoader) + + scene_data = from_dict(data_class=SceneData, data=scene_cfg) + + return scene_data diff --git a/env_manager/rbs_runtime/rbs_runtime/scripts/runtime.py b/env_manager/rbs_runtime/rbs_runtime/scripts/runtime.py index c99f0a9..56c9268 100755 --- a/env_manager/rbs_runtime/rbs_runtime/scripts/runtime.py +++ b/env_manager/rbs_runtime/rbs_runtime/scripts/runtime.py @@ -1,50 +1,42 @@ #!/usr/bin/python3 -from env_manager.models.configs.objects import ObjectRandomizerData -from rclpy.node import Node -from env_manager.scene import Scene -from env_manager.models.configs import ( - CameraData, - SceneData, - RobotData, - MeshData, - ModelData, -) import rclpy -from scenario.bindings.gazebo import GazeboSimulator +from ament_index_python.packages import get_package_share_directory +from env_manager.scene import Scene from env_manager_interfaces.srv import ResetEnv from rbs_assets_library import get_world_file +from rclpy.node import Node +from scenario.bindings.gazebo import GazeboSimulator - -DEFAULT_SCENE: SceneData = SceneData( - robot=RobotData( - name="rbs_arm", - spawn_position = (0.0, 0.0, 0.0), - with_gripper=True, - joint_positions=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - gripper_joint_positions=0.0, - ), - objects=[ - ModelData("bishop", position=(0.0, 1.0, 0.3)), - ], - camera=[CameraData("robot_camera", publish_color=True, publish_depth=True, publish_points=True)], -) +from .. import scene_config_loader class GazeboRuntime(Node): def __init__(self) -> None: super().__init__(node_name="rbs_gz_runtime") self.declare_parameter("robot_description", "") + self.declare_parameter( + "config_file", + get_package_share_directory("rbs_runtime") + + "/config/default-scene-config.yaml", + ) + self.gazebo = GazeboSimulator(step_size=0.001, rtf=1.0, steps_per_run=1) self.gazebo.insert_world_from_sdf(get_world_file("default")) if not self.gazebo.initialize(): raise RuntimeError("Gazebo cannot be initialized") + config_file = ( + self.get_parameter("config_file").get_parameter_value().string_value + ) + + scene_data = scene_config_loader(config_file) + self.scene = Scene( node=self, gazebo=self.gazebo, - scene=DEFAULT_SCENE, + scene=scene_data, robot_urdf_string=self.get_parameter("robot_description") .get_parameter_value() .string_value, diff --git a/env_manager/rbs_runtime/setup.py b/env_manager/rbs_runtime/setup.py index 6a47c74..b197204 100644 --- a/env_manager/rbs_runtime/setup.py +++ b/env_manager/rbs_runtime/setup.py @@ -16,6 +16,10 @@ setup( os.path.join("share", package_name, "launch"), glob(os.path.join("launch", "*launch.[pxy][yma]*")), ), + ( + os.path.join("share", package_name, "config"), + glob(os.path.join("config", "*config.[yma]*")), + ), ], install_requires=["setuptools"], zip_safe=True, diff --git a/rbs_bringup/launch/rbs_robot.launch.py b/rbs_bringup/launch/rbs_robot.launch.py index 5faa88a..1ffae1a 100644 --- a/rbs_bringup/launch/rbs_robot.launch.py +++ b/rbs_bringup/launch/rbs_robot.launch.py @@ -175,7 +175,7 @@ def launch_setup(context, *args, **kwargs): ), launch_arguments={ "control_space": "task", - "control_strategy": "effort", + "control_strategy": "position", "has_gripper": "true", "namespace": namespace, }.items(), diff --git a/rbs_skill_servers/launch/skills.launch.py b/rbs_skill_servers/launch/skills.launch.py index 38d349d..32c6a18 100644 --- a/rbs_skill_servers/launch/skills.launch.py +++ b/rbs_skill_servers/launch/skills.launch.py @@ -1,16 +1,11 @@ -from collections import namedtuple -from os import name from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - OpaqueFunction -) -from ament_index_python.packages import get_package_share_directory +from launch.actions import DeclareLaunchArgument, OpaqueFunction from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node from rbs_launch_utils.launch_common import load_yaml + def launch_setup(context, *args, **kwargs): robot_description_decl = LaunchConfiguration("robot_description") robot_description_semantic_decl = LaunchConfiguration("robot_description_semantic") @@ -19,12 +14,12 @@ def launch_setup(context, *args, **kwargs): with_gripper_condition = LaunchConfiguration("with_gripper_condition") points_params_filepath_decl = LaunchConfiguration("points_params_filepath") robot_description = {"robot_description": robot_description_decl} - robot_description_semantic = {"robot_description_semantic": robot_description_semantic_decl} + robot_description_semantic = { + "robot_description_semantic": robot_description_semantic_decl + } namespace = LaunchConfiguration("namespace") - points_params = load_yaml( - "rbs_skill_servers", "config/gripperPositions.yaml" - ) + points_params = load_yaml("rbs_skill_servers", "config/gripperPositions.yaml") kinematics_yaml = load_yaml("rbs_arm", "config/kinematics.yaml") @@ -39,20 +34,20 @@ def launch_setup(context, *args, **kwargs): 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= [ + parameters=[ robot_description, robot_description_semantic, robot_description_kinematics, {"use_sim_time": use_sim_time}, ], - condition=IfCondition(with_gripper_condition) + condition=IfCondition(with_gripper_condition), ) move_cartesian_path_action_server = Node( @@ -64,19 +59,16 @@ def launch_setup(context, *args, **kwargs): 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}], ) - # 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", @@ -86,30 +78,29 @@ def launch_setup(context, *args, **kwargs): robot_description_semantic, robot_description_kinematics, {"use_sim_time": use_sim_time}, - ] + ], ) assembly_config = Node( package="rbs_utils", executable="assembly_config_service.py", namespace=namespace, - output="screen" + output="screen", ) - - nodes_to_start =[ + 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, + cartesian_move_to_pose_action_server, # grasp_pose_loader ] return nodes_to_start + def generate_launch_description(): - declared_arguments = [] declared_arguments.append( @@ -125,18 +116,15 @@ 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="")) declared_arguments.append( DeclareLaunchArgument("with_gripper_condition", default_value="") ) declared_arguments.append( DeclareLaunchArgument("points_params_filepath", default_value="") ) - declared_arguments.append( - DeclareLaunchArgument("namespace", default_value="") + declared_arguments.append(DeclareLaunchArgument("namespace", default_value="")) + + return LaunchDescription( + declared_arguments + [OpaqueFunction(function=launch_setup)] ) - - - return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])