robossembler/runtime
7
0
Fork 0
Code Issues 4 Pull requests Projects Releases Packages Wiki Activity Actions

add external robot_description parameter for rbs_robot.launch.py

Browse source 
feat(robot): add support for handling scalar joint positions

- Updated `RobotData` and `RbsArm` to allow joint positions to be passed
as either a list or a scalar value.
- Refactored logic to dynamically handle scalar values and repeat them
across all joints.
- Added new `initial_arm_joint_positions` and
`initial_gripper_joint_positions` properties in `RbsArm`.
- Improved joint position initialization in the `Scene` class to
retrieve robot description asynchronously if not provided.
- Enhanced the launch script to configure environment variables based on
available CPU cores.
- Minor code cleanups and bug fixes, including terrain naming and
corrected joint defaultsd.

git commit --amend
This commit is contained in:
Ilya Uraev 2024-09-25 12:11:17 +03:00
parent bc8745abe5
commit e4e3e4e3af
18 changed files with 686 additions and 342 deletions
Download patch file Download diff file Expand all files Collapse all files

3
env_manager/env_manager/env_manager/models/configs/camera.py
View file

@ -1,9 +1,10 @@
from dataclasses import dataclass, field
import numpy as np
@dataclass
class CameraData:
name: str = field(default_factory=str)
name: str = field(default_factory=str)
enable: bool = field(default=True)
type: str = field(default="rgbd_camera")
relative_to: str = field(default="base_link")

1
env_manager/env_manager/env_manager/models/configs/light.py
View file

@ -1,5 +1,6 @@
from dataclasses import dataclass, field
@dataclass
class LightData:
type: str = field(default="sun")

5
env_manager/env_manager/env_manager/models/configs/robot.py
View file

@ -1,5 +1,6 @@
from dataclasses import dataclass, field
@dataclass
class RobotRandomizerData:
pose: bool = field(default=False)
@ -19,7 +20,7 @@ class RobotData:
spawn_quat_xyzw: tuple[float, float, float, float] = field(
default=(0.0, 0.0, 0.0, 1.0)
)
joint_positioins: list[float] = field(default_factory=list)
joint_positions: list[float] = field(default_factory=list)
with_gripper: bool = field(default=False)
gripper_jont_positions: list[float] = field(default_factory=list)
gripper_joint_positions: list[float] | float = field(default_factory=list)
randomizer: RobotRandomizerData = field(default_factory=RobotRandomizerData)

7
env_manager/env_manager/env_manager/models/configs/scene.py
View file

@ -1,4 +1,3 @@
from dataclasses import dataclass, field
from .objects import ObjectData
from .robot import RobotData
@ -6,6 +5,7 @@ from .terrain import TerrainData
from .light import LightData
from .camera import CameraData
@dataclass
class PluginsData:
scene_broadcaster: bool = field(default_factory=bool)
@ -13,6 +13,7 @@ class PluginsData:
fts_broadcaster: bool = field(default_factory=bool)
sensors_render_engine: str = field(default="ogre2")
@dataclass
class SceneData:
physics_rollouts_num: int = field(default=0)
@ -24,7 +25,3 @@ class SceneData:
objects: list[ObjectData] = field(default_factory=list)
camera: list[CameraData] = field(default_factory=list)
plugins: PluginsData = field(default_factory=PluginsData)

2
env_manager/env_manager/env_manager/models/configs/terrain.py
View file

@ -1,9 +1,9 @@
from dataclasses import dataclass, field
@dataclass
class TerrainData:
name: str = field(default="ground")
type: str = field(default="flat")
spawn_position: tuple[float, float, float] = field(default=(0, 0, 0))
spawn_quat_xyzw: tuple[float, float, float, float] = field(default=(0, 0, 0, 1))

50
env_manager/env_manager/env_manager/models/robots/rbs_arm_b.py
View file

@ -13,6 +13,7 @@ class RbsArm(RobotWrapper):
"""
Experimental class with `robot_builder`
"""
DEFAULT_ARM_JOINT_POSITIONS: list[float] = [
0.0,
0.5,
@ -41,12 +42,10 @@ class RbsArm(RobotWrapper):
name: str = "rbs_arm",
position: tuple[float, float, float] = (0.0, 0.0, 0.0),
orientation: tuple[float, float, float, float] = (1.0, 0, 0, 0),
initial_arm_joint_positions: list[float] = DEFAULT_ARM_JOINT_POSITIONS,
initial_gripper_joint_positions: list[float] = DEFAULT_GRIPPER_JOINT_POSITIONS,
initial_arm_joint_positions: list[float] | float = DEFAULT_ARM_JOINT_POSITIONS,
initial_gripper_joint_positions: list[float]
| float = DEFAULT_GRIPPER_JOINT_POSITIONS,
):
self.__initial_arm_joint_positions = initial_arm_joint_positions
self.__initial_gripper_joint_positions = initial_gripper_joint_positions
# Get a unique model name
model_name = get_unique_model_name(world, name)
@ -63,11 +62,19 @@ class RbsArm(RobotWrapper):
# Parse robot to get metadata
self._robot: Robot = URDF_parser.load_string(urdf_string)
node.get_logger().info("{}".format(self._robot.actuated_joint_names))
node.get_logger().info("{}".format(self._robot.get_gripper_mimic_joint_name))
node.get_logger().info("{}".format(self._robot.gripper_actuated_joint_names))
node.get_logger().info("{}".format(model.joint_names()))
self.__initial_gripper_joint_positions = (
[float(initial_gripper_joint_positions)]
* len(self._robot.gripper_actuated_joint_names)
if isinstance(initial_gripper_joint_positions, (int, float))
else initial_gripper_joint_positions
)
self.__initial_arm_joint_positions = (
[float(initial_arm_joint_positions)] * len(self._robot.actuated_joint_names)
if isinstance(initial_arm_joint_positions, (int, float))
else initial_arm_joint_positions
)
# Set initial joint configuration
self.set_initial_joint_positions(model)
@ -77,14 +84,23 @@ class RbsArm(RobotWrapper):
def robot(self) -> Robot:
return self._robot
@property
def initial_arm_joint_positions(self) -> list[float]:
return self.__initial_arm_joint_positions
@property
def initial_gripper_joint_positions(self) -> list[float]:
return self.__initial_gripper_joint_positions
def set_initial_joint_positions(self, model):
model = model.to_gazebo()
if not model.reset_joint_positions(
self.__initial_arm_joint_positions, self._robot.actuated_joint_names
):
raise RuntimeError("Failed to set initial positions of arm's joints")
if not model.reset_joint_positions(
self.__initial_gripper_joint_positions, [self._robot.get_gripper_mimic_joint_name]
):
raise RuntimeError("Failed to set initial positions of gripper's joints")
joint_position_data = [
(self.__initial_arm_joint_positions, self._robot.actuated_joint_names),
(self.__initial_gripper_joint_positions, self._robot.gripper_actuated_joint_names),
]
for positions, joint_names in joint_position_data:
print(f"Setting joint positions for {joint_names}: {positions}")
if not model.reset_joint_positions(positions, joint_names):
raise RuntimeError(f"Failed to set initial positions of {joint_names[0].split('_')[0]}'s joints")

15
env_manager/env_manager/env_manager/models/robots/robot.py
View file

@ -12,8 +12,8 @@ class RobotWrapper(model_wrapper.ModelWrapper):
name: str | None = None,
position: tuple[float, float, float] | None = None,
orientation: tuple[float, float, float, float] | None = None,
initial_arm_joint_positions: list[float] = [],
initial_gripper_joint_positions: list[float] = [],
initial_arm_joint_positions: list[float] | float = [],
initial_gripper_joint_positions: list[float] | float = [],
model: model_wrapper.ModelWrapper | None = None,
**kwargs,
):
@ -27,6 +27,17 @@ class RobotWrapper(model_wrapper.ModelWrapper):
def robot(self) -> Robot:
pass
@property
@abstractmethod
def initial_gripper_joint_positions(self) -> list[float]:
pass
@property
@abstractmethod
def initial_arm_joint_positions(self) -> list[float]:
pass
@abstractmethod
def set_initial_joint_positions(self, model):
pass

55
env_manager/env_manager/env_manager/scene/scene.py
View file

@ -1,7 +1,7 @@
from dataclasses import asdict
import numpy as np
from rclpy.node import Node
from rclpy.node import Node, Parameter
from scenario.bindings.gazebo import GazeboSimulator, World, PhysicsEngine_dart
from scipy.spatial.transform import Rotation
from scipy.spatial import distance
@ -16,6 +16,8 @@ from ..utils.gazebo import (
transform_move_to_model_position,
)
from ..utils.types import Point, Pose
from rcl_interfaces.srv import GetParameters
import asyncio
# TODO: Split scene randomizer and scene
@ -52,7 +54,15 @@ class Scene:
self.__objects_unique_names = {}
self.__object_positions: dict[str, Point] = {}
self.__objects_workspace_centre: Point = (0.0, 0.0, 0.0)
scene.robot.urdf_string = robot_urdf_string
if not robot_urdf_string:
self.param_client = node.create_client(
GetParameters, "/robot_state_publisher/get_parameters"
)
asyncio.run(self.get_parameter_async())
scene.robot.urdf_string = node.get_parameter("robot_description").get_parameter_value().string_value
else:
scene.robot.urdf_string = robot_urdf_string
self.tf2_broadcaster = Tf2Broadcaster(node=node)
@ -74,6 +84,32 @@ class Scene:
self.__workspace_center[2] + half_depth,
)
async def get_parameter_async(self):
while not self.param_client.wait_for_service(timeout_sec=1.0):
self.node.get_logger().info(
"Waiting for parameter service [robot_state_publisher]..."
)
self.node.get_logger().info("Sending request for parameter...")
future = self.param_client.call_async(self.create_get_parameters_request())
response = await future
match response:
case None:
raise RuntimeError("robot_description parameter not found in robot_state_publisher")
case GetParameters.Response(values=[value]) if value.string_value:
self.node.get_logger().info(f"Got parameter: {value.string_value}")
param = Parameter(
"robot_description", Parameter.Type.STRING, value.string_value
)
self.node.set_parameters([param])
case _:
raise RuntimeError(f"I don't know what but the type of robot_description parameter is {type(response)}")
def create_get_parameters_request(self) -> GetParameters.Request:
return GetParameters.Request(names=["robot_description"])
def init_scene(self):
self.gazebo_paused_run()
@ -176,8 +212,8 @@ class Scene:
position=self.robot.spawn_position,
orientation=quat_to_wxyz(self.robot.spawn_quat_xyzw),
urdf_string=self.robot.urdf_string,
initial_arm_joint_positions=self.robot.joint_positioins,
initial_gripper_joint_positions=self.robot.gripper_jont_positions,
initial_arm_joint_positions=self.robot.joint_positions,
initial_gripper_joint_positions=self.robot.gripper_joint_positions,
)
# The desired name is passed as arg on creation, however, a different name might be selected to be unique
@ -361,6 +397,7 @@ class Scene:
# TODO: add another terrains
self.terrain_wrapper = Ground(
self.world,
name=self.terrain.name,
position=self.terrain.spawn_position,
orientation=quat_to_wxyz(self.terrain.spawn_quat_xyzw),
size=self.terrain.size,
@ -390,7 +427,7 @@ class Scene:
self.gazebo_paused_run()
def randomize_camera_pose(self, camera: CameraData):
# Get random camera pose, centred at object position (or centre of object spawn box)
# Get random camera pose, centered at object position (or center of object spawn box)
if "orbit" == camera.random_pose_mode:
camera_position, camera_quat_xyzw = self.get_random_camera_pose_orbit(
camera
@ -526,7 +563,7 @@ class Scene:
def get_random_camera_pose_sample_process(
self, camera: CameraData, position: Point
) -> Pose:
# Determine orientation such that camera faces the centre
# Determine orientation such that camera faces the center
rpy = [
0.0,
np.arctan2(
@ -584,7 +621,7 @@ class Scene:
def reset_robot_joint_positions(self):
gazebo_robot = self.robot_wrapper.to_gazebo()
arm_joint_positions = self.robot.joint_positioins
arm_joint_positions = self.robot_wrapper.initial_arm_joint_positions
# Add normal noise if desired
if self.robot.randomizer.joint_positions:
@ -610,7 +647,7 @@ class Scene:
and self.robot_wrapper.robot.gripper_actuated_joint_names
):
if not gazebo_robot.reset_joint_positions(
self.robot.gripper_jont_positions,
self.robot_wrapper.initial_gripper_joint_positions,
self.robot_wrapper.robot.gripper_actuated_joint_names,
):
raise RuntimeError("Failed to reset gripper joint positions")
@ -627,7 +664,7 @@ class Scene:
# if self.robot.with_gripper:
# if (
# self.robot_wrapper.CLOSED_GRIPPER_JOINT_POSITIONS
# == self.robot.gripper_jont_positions
# == self.robot.gripper_joint_positions
# ):
# self.gripper_controller.close()
# else:

BIN
env_manager/rbs_gym/img_color.png
View file

Binary file not shown.
Side by side

Before

Width:  |  Height:  |  Size: 62 KiB

28
env_manager/rbs_gym/launch/test_env.launch.py
View file

@ -440,14 +440,12 @@ def generate_launch_description():
)
)
# HER specifics
(
declared_arguments.append(
DeclareLaunchArgument(
"truncate_last_trajectory",
default_value="True",
description="When using HER with online sampling the last trajectory in the replay buffer will be truncated after) reloading the replay buffer.",
)
),
declared_arguments.append(
DeclareLaunchArgument(
"truncate_last_trajectory",
default_value="True",
description="When using HER with online sampling the last trajectory in the replay buffer will be truncated after) reloading the replay buffer.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
@ -456,11 +454,17 @@ def generate_launch_description():
description="The level of logging that is applied to all ROS 2 nodes launched by this script.",
)
)
cpu_c = cpu_count()
if cpu_c is not None:
env_variables = [
SetEnvironmentVariable(name="OMP_DYNAMIC", value="TRUE"),
SetEnvironmentVariable(name="OMP_NUM_THREADS", value=str(cpu_c // 2)),
]
else:
env_variables = [
SetEnvironmentVariable(name="OMP_DYNAMIC", value="TRUE"),
]
env_variables = [
SetEnvironmentVariable(name="OMP_DYNAMIC", value="TRUE"),
SetEnvironmentVariable(name="OMP_NUM_THREADS", value=str(cpu_count() // 2)),
]
return LaunchDescription(
declared_arguments + [OpaqueFunction(function=launch_setup)] + env_variables

4
env_manager/rbs_gym/rbs_gym/envs/__init__.py
View file

@ -103,9 +103,9 @@ SCENE_CONFIGURATION: SceneData = SceneData(
physics_rollouts_num=0,
robot=RobotData(
name="rbs_arm",
joint_positioins=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
joint_positions=[0.0, 0.5, 3.14159, 1.5, 0.0, 1.4, 0.0],
with_gripper=True,
gripper_jont_positions=[0],
gripper_joint_positions=0.064,
randomizer=RobotRandomizerData(joint_positions=True),
),
objects=[

51
env_manager/rbs_gym/rbs_gym/envs/tasks/manipulation.py
View file

@ -183,7 +183,6 @@ class Manipulation(Task, Node, abc.ABC):
# )
# Names of important models (in addition to robot model)
self.terrain_name = "terrain"
# Setup listener and broadcaster of transforms via tf2
self.tf2_listener = Tf2Listener(node=self)
@ -228,7 +227,8 @@ class Manipulation(Task, Node, abc.ABC):
raise NotImplementedError()
def reset_task(self):
self.__consume_parameter_overrides()
# self.__consume_parameter_overrides()
pass
# # Helper functions #
# def get_relative_ee_position(
@ -596,7 +596,8 @@ class Manipulation(Task, Node, abc.ABC):
Returns true if robot links are in collision with the ground.
"""
robot_name_len = len(self.robot.name)
terrain_model = self.world.get_model(self.terrain_name)
model_names = self.world.model_names()
terrain_model = self.world.get_model(self.scene.terrain.name)
for contact in terrain_model.contacts():
body_b = contact.body_b
@ -644,30 +645,30 @@ class Manipulation(Task, Node, abc.ABC):
# or object_position[2] > self.workspace_max_bound[2]
# )
def add_parameter_overrides(self, parameter_overrides: Dict[str, Any]):
self.add_task_parameter_overrides(parameter_overrides)
self.add_randomizer_parameter_overrides(parameter_overrides)
# def add_parameter_overrides(self, parameter_overrides: Dict[str, Any]):
# self.add_task_parameter_overrides(parameter_overrides)
# self.add_randomizer_parameter_overrides(parameter_overrides)
# #
# def add_task_parameter_overrides(self, parameter_overrides: Dict[str, Any]):
# self.__task_parameter_overrides.update(parameter_overrides)
# #
# def add_randomizer_parameter_overrides(self, parameter_overrides: Dict[str, Any]):
# self._randomizer_parameter_overrides.update(parameter_overrides)
#
def add_task_parameter_overrides(self, parameter_overrides: Dict[str, Any]):
self.__task_parameter_overrides.update(parameter_overrides)
# def __consume_parameter_overrides(self):
# for key, value in self.__task_parameter_overrides.items():
# if hasattr(self, key):
# setattr(self, key, value)
# elif hasattr(self, f"_{key}"):
# setattr(self, f"_{key}", value)
# elif hasattr(self, f"__{key}"):
# setattr(self, f"__{key}", value)
# else:
# self.get_logger().error(
# f"Override '{key}' is not supperted by the task."
# )
#
def add_randomizer_parameter_overrides(self, parameter_overrides: Dict[str, Any]):
self._randomizer_parameter_overrides.update(parameter_overrides)
def __consume_parameter_overrides(self):
for key, value in self.__task_parameter_overrides.items():
if hasattr(self, key):
setattr(self, key, value)
elif hasattr(self, f"_{key}"):
setattr(self, f"_{key}", value)
elif hasattr(self, f"__{key}"):
setattr(self, f"__{key}", value)
else:
self.get_logger().error(
f"Override '{key}' is not supperted by the task."
)
self.__task_parameter_overrides.clear()
# self.__task_parameter_overrides.clear()
@property
def robot(self) -> Robot:

9
env_manager/rbs_gym/rbs_gym/envs/tasks/reach/reach.py
View file

@ -53,7 +53,7 @@ class Reach(Manipulation, abc.ABC):
self._previous_distance: float = 0.0
# self._collision_reward: float = collision_reward
# self.initial_gripper_joint_positions = self.robot_data.gripper_jont_positions
# self.initial_gripper_joint_positions = self.robot_data.gripper_joint_positions
self.twist = TwistSubscriber(
self,
topic="/cartesian_force_controller/current_twist",
@ -122,10 +122,11 @@ class Reach(Manipulation, abc.ABC):
twist.twist.angular.z,
)
observation: Observation = np.concatenate([ee_position, target_position, twt], dtype=np.float32)
# Create the observation
observation = Observation(
np.concatenate([ee_position, target_position, twt], dtype=np.float32)
)
# observation = Observation(
# np.concatenate([ee_position, target_position, twt], dtype=np.float32)
# )
self.get_logger().debug(f"\nobservation: {observation}")

17
env_manager/rbs_runtime/scripts/asdict.py Normal file
View file

@ -0,0 +1,17 @@
from env_manager.models.configs import SceneData, RobotData, MeshObjectData, CameraData
from dataclasses import asdict
import json
scene: SceneData = SceneData(
robot=RobotData(
name="rbs_arm",
with_gripper=True,
joint_positions=[0, 0, 0, 0, 0, 0, 0],
gripper_joint_positions=[0],
),
objects=[MeshObjectData("bishop", position=(0.0, 1.0, 0.3))],
camera=[CameraData("robot_camera")],
)
with open("scene_config.json", "w") as file:
json.dump(asdict(scene), file)

4
env_manager/rbs_runtime/scripts/runtime.py
View file

@ -22,8 +22,8 @@ DEFAULT_SCENE: SceneData = SceneData(
robot=RobotData(
name="rbs_arm",
with_gripper=True,
joint_positioins=[0, 0, 0, 0, 0, 0, 0],
gripper_jont_positions=[0],
joint_positions=[0, 0, 0, 0, 0, 0, 0],
gripper_joint_positions=[0],
),
objects=[MeshObjectData("bishop", position=(0.0, 1.0, 0.3))],
camera=[CameraData("robot_camera")],

186
env_manager/scene_config.json Normal file
View file

@ -0,0 +1,186 @@
{
"physics_rollouts_num": 0,
"gravity": [
0.0,
0.0,
-9.80665
],
"gravity_std": [
0.0,
0.0,
0.0232
],
"robot": {
"name": "rbs_arm",
"urdf_string": "",
"spawn_position": [
0.0,
0.0,
0.0
],
"spawn_quat_xyzw": [
0.0,
0.0,
0.0,
1.0
],
"joint_positioins": [
0,
0,
0,
0,
0,
0,
0
],
"with_gripper": true,
"gripper_jont_positions": [
0
],
"randomizer": {
"pose": false,
"spawn_volume": [
1.0,
1.0,
0.0
],
"joint_positions": false,
"joint_positions_std": 0.1,
"joint_positions_above_object_spawn": false,
"joint_positions_above_object_spawn_elevation": 0.2,
"joint_positions_above_object_spawn_xy_randomness": 0.2
}
},
"terrain": {
"name": "ground",
"type": "flat",
"spawn_position": [
0,
0,
0
],
"spawn_quat_xyzw": [
0,
0,
0,
1
],
"size": [
1.5,
1.5
],
"model_rollouts_num": 1
},
"light": {
"type": "sun",
"direction": [
0.5,
-0.25,
-0.75
],
"random_minmax_elevation": [
-0.15,
-0.65
],
"color": [
1.0,
1.0,
1.0,
1.0
],
"distance": 1000.0,
"visual": true,
"radius": 25.0,
"model_rollouts_num": 1
},
"objects": [
{
"name": "bishop",
"type": "",
"relative_to": "world",
"position": [
0.0,
1.0,
0.3
],
"orientation": [
1.0,
0.0,
0.0,
0.0
],
"static": false,
"randomize": {
"count": 0,
"random_pose": false,
"random_position": false,
"random_orientation": false,
"random_model": false,
"random_spawn_position_segments": [],
"random_spawn_position_update_workspace_centre": false,
"random_spawn_volume": [
0.5,
0.5,
0.5
],
"models_rollouts_num": 0
},
"texture": []
}
],
"camera": [
{
"name": "robot_camera",
"enable": true,
"type": "rgbd_camera",
"relative_to": "base_link",
"width": 128,
"height": 128,
"image_format": "R8G8B8",
"update_rate": 10,
"horizontal_fov": 1.0471975511965976,
"vertical_fov": 1.0471975511965976,
"clip_color": [
0.01,
1000.0
],
"clip_depth": [
0.05,
10.0
],
"noise_mean": null,
"noise_stddev": null,
"publish_color": false,
"publish_depth": false,
"publish_points": false,
"spawn_position": [
0,
0,
1
],
"spawn_quat_xyzw": [
0,
0.70710678118,
0,
0.70710678118
],
"random_pose_rollouts_num": 1,
"random_pose_mode": "orbit",
"random_pose_orbit_distance": 1.0,
"random_pose_orbit_height_range": [
0.1,
0.7
],
"random_pose_orbit_ignore_arc_behind_robot": 0.39269908169872414,
"random_pose_select_position_options": [],
"random_pose_focal_point_z_offset": 0.0,
"random_pose_rollout_counter": 0.0
}
],
"plugins": {
"scene_broadcaster": false,
"user_commands": false,
"fts_broadcaster": false,
"sensors_render_engine": "ogre2"
}
}

415
rbs_bringup/launch/rbs_robot.launch.py
View file

@ -1,20 +1,23 @@
from launch import LaunchContext, LaunchDescription
import os
import xacro
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import (
DeclareLaunchArgument,
IncludeLaunchDescription,
RegisterEventHandler,
OpaqueFunction
OpaqueFunction,
)
from launch.event_handlers import OnProcessExit
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
from launch.substitutions import (
Command,
FindExecutable,
LaunchConfiguration,
PathJoinSubstitution,
)
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
import xacro
import os
from ament_index_python.packages import get_package_share_directory
from rbs_launch_utils.launch_common import load_yaml
def launch_setup(context, *args, **kwargs):
@ -54,31 +57,39 @@ def launch_setup(context, *args, **kwargs):
description_file = description_file.perform(context)
controllers_file = controllers_file.perform(context)
multi_robot = multi_robot.perform(context)
robot_description = LaunchConfiguration("robot_description")
remappings = []
if multi_robot == "true":
remappings.append([("/tf", "tf"), ("/tf_static", "tf_static")])
controllers_file = os.path.join(get_package_share_directory(description_package), "config", controllers_file)
xacro_file = os.path.join(get_package_share_directory(description_package), "urdf", description_file)
robot_description_doc = xacro.process_file(
xacro_file,
mappings={
"gripper_name": gripper_name.perform(context),
"hardware": hardware.perform(context),
"simulation_controllers": controllers_file,
"namespace": namespace,
"x": x_pos.perform(context),
"y": y_pos.perform(context),
"z": z_pos.perform(context),
"roll": roll.perform(context),
"pitch": pitch.perform(context),
"yaw": yaw.perform(context)
}
controllers_file = os.path.join(
get_package_share_directory(description_package), "config", controllers_file
)
robot_description_content = robot_description_doc.toprettyxml(indent=" ")
if robot_description.perform(context):
xacro_file = os.path.join(
get_package_share_directory(description_package), "urdf", description_file
)
robot_description_doc = xacro.process_file(
xacro_file,
mappings={
"gripper_name": gripper_name.perform(context),
"hardware": hardware.perform(context),
"simulation_controllers": controllers_file,
"namespace": namespace,
"x": x_pos.perform(context),
"y": y_pos.perform(context),
"z": z_pos.perform(context),
"roll": roll.perform(context),
"pitch": pitch.perform(context),
"yaw": yaw.perform(context),
},
)
robot_description_content = robot_description_doc.toprettyxml(indent=" ")
else:
robot_description_content = robot_description.perform(context)
robot_description = {"robot_description": robot_description_content}
@ -87,23 +98,37 @@ def launch_setup(context, *args, **kwargs):
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare(moveit_config_package), "config/moveit", "rbs_arm.srdf.xacro"]
[
FindPackageShare(moveit_config_package),
"config/moveit",
"rbs_arm.srdf.xacro",
]
),
" ",
"name:=",robot_type," ",
"with_gripper:=",with_gripper_condition, " ",
"gripper_name:=", gripper_name, " ",
"name:=",
robot_type,
" ",
"with_gripper:=",
with_gripper_condition,
" ",
"gripper_name:=",
gripper_name,
" ",
]
)
robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content}
robot_description_semantic = {
"robot_description_semantic": robot_description_semantic_content
}
robot_description_kinematics = PathJoinSubstitution(
[FindPackageShare(moveit_config_package), "config", "kinematics.yaml"]
)
# kinematics_yaml = load_yaml("rbs_arm", "config/kinematics.yaml")
robot_description_kinematics = {"robot_description_kinematics": robot_description_kinematics}
robot_description_kinematics = {
"robot_description_kinematics": robot_description_kinematics
}
rviz_config_file = PathJoinSubstitution(
[FindPackageShare("rbs_bringup"), "config", "rbs.rviz"]
)
@ -131,108 +156,119 @@ def launch_setup(context, *args, **kwargs):
robot_description,
robot_description_semantic,
robot_description_kinematics,
{'use_sim_time': use_sim_time}
{"use_sim_time": use_sim_time},
],
condition=IfCondition(launch_rviz)
condition=IfCondition(launch_rviz),
)
rviz = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
arguments=["-d", rviz_config_file],
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics
],
condition=IfCondition(launch_rviz))
control = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare(description_package),
'launch',
'control.launch.py'
])
]),
PythonLaunchDescriptionSource(
[
PathJoinSubstitution(
[
FindPackageShare(description_package),
"launch",
"control.launch.py",
]
)
]
),
launch_arguments={
'control_space': "task",
'control_strategy': "effort",
'is_gripper': "false",
'namespace': namespace,
"control_space": "task",
"control_strategy": "effort",
"has_gripper": "true",
"namespace": namespace,
}.items(),
condition=IfCondition(launch_controllers))
condition=IfCondition(launch_controllers),
)
moveit = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare(moveit_config_package),
'launch',
'moveit.launch.py'
])
]),
PythonLaunchDescriptionSource(
[
PathJoinSubstitution(
[
FindPackageShare(moveit_config_package),
"launch",
"moveit.launch.py",
]
)
]
),
launch_arguments={
'robot_description': robot_description_content,
'moveit_config_package': moveit_config_package,
'moveit_config_file': moveit_config_file,
'use_sim_time': use_sim_time,
'tf_prefix': robot_name,
'with_gripper': with_gripper_condition,
'robot_description_semantic': robot_description_semantic_content,
'namespace': namespace,
"robot_description": robot_description_content,
"moveit_config_package": moveit_config_package,
"moveit_config_file": moveit_config_file,
"use_sim_time": use_sim_time,
"tf_prefix": robot_name,
"with_gripper": with_gripper_condition,
"robot_description_semantic": robot_description_semantic_content,
"namespace": namespace,
}.items(),
condition=IfCondition(launch_moveit))
condition=IfCondition(launch_moveit),
)
skills = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare('rbs_skill_servers'),
'launch',
'skills.launch.py'
])
]),
PythonLaunchDescriptionSource(
[
PathJoinSubstitution(
[
FindPackageShare("rbs_skill_servers"),
"launch",
"skills.launch.py",
]
)
]
),
launch_arguments={
'robot_description': robot_description_content,
'robot_description_semantic': robot_description_semantic_content,
'robot_description_kinematics': robot_description_kinematics,
'use_sim_time': use_sim_time,
'with_gripper_condition': with_gripper_condition,
'namespace': namespace
}.items()
"robot_description": robot_description_content,
"robot_description_semantic": robot_description_semantic_content,
"robot_description_kinematics": robot_description_kinematics,
"use_sim_time": use_sim_time,
"with_gripper_condition": with_gripper_condition,
"namespace": namespace,
}.items(),
)
task_planner = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare('rbs_task_planner'),
'launch',
'task_planner.launch.py'
])
]),
PythonLaunchDescriptionSource(
[
PathJoinSubstitution(
[
FindPackageShare("rbs_task_planner"),
"launch",
"task_planner.launch.py",
]
)
]
),
launch_arguments={
# TBD
}.items(),
condition=IfCondition(launch_task_planner))
condition=IfCondition(launch_task_planner),
)
perception = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare('rbs_perception'),
'launch',
'perception.launch.py'
])
]),
PythonLaunchDescriptionSource(
[
PathJoinSubstitution(
[
FindPackageShare("rbs_perception"),
"launch",
"perception.launch.py",
]
)
]
),
launch_arguments={
# TBD
}.items(),
condition=IfCondition(launch_perception))
condition=IfCondition(launch_perception),
)
asm_config = Node(
package="rbs_utils",
namespace=namespace,
executable="assembly_config_service.py"
executable="assembly_config_service.py",
)
nodes_to_start = [
@ -243,7 +279,7 @@ def launch_setup(context, *args, **kwargs):
asm_config,
# task_planner,
# perception,
rviz
rviz,
]
return nodes_to_start
@ -254,7 +290,7 @@ def generate_launch_description():
DeclareLaunchArgument(
"robot_type",
description="Type of robot by name",
choices=["rbs_arm","ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
choices=["rbs_arm", "ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
default_value="rbs_arm",
)
)
@ -336,126 +372,145 @@ def generate_launch_description():
)
)
declared_arguments.append(
DeclareLaunchArgument("with_gripper",
default_value="true",
description="With gripper or not?")
DeclareLaunchArgument(
"with_gripper", default_value="true", description="With gripper or not?"
)
)
declared_arguments.append(
DeclareLaunchArgument("launch_rviz",
default_value="false",
description="Launch RViz?")
DeclareLaunchArgument(
"launch_rviz", default_value="false", description="Launch RViz?"
)
)
declared_arguments.append(
DeclareLaunchArgument("sim_gazebo",
default_value="true",
description="Gazebo Simulation")
DeclareLaunchArgument(
"sim_gazebo", default_value="true", description="Gazebo Simulation"
)
)
declared_arguments.append(
DeclareLaunchArgument("env_manager",
default_value="true",
description="Launch env_manager?")
DeclareLaunchArgument(
"env_manager", default_value="true", description="Launch env_manager?"
)
)
declared_arguments.append(
DeclareLaunchArgument("launch_sim",
default_value="true",
description="Launch simulator (Gazebo)?\
Most general arg")
DeclareLaunchArgument(
"launch_sim",
default_value="true",
description="Launch simulator (Gazebo)?\
Most general arg",
)
)
declared_arguments.append(
DeclareLaunchArgument("launch_moveit",
default_value="true",
description="Launch moveit?")
DeclareLaunchArgument(
"launch_moveit", default_value="true", description="Launch moveit?"
)
)
declared_arguments.append(
DeclareLaunchArgument("launch_perception",
default_value="false",
description="Launch perception?")
DeclareLaunchArgument(
"launch_perception", default_value="false", description="Launch perception?"
)
)
declared_arguments.append(
DeclareLaunchArgument("launch_task_planner",
default_value="false",
description="Launch task_planner?")
DeclareLaunchArgument(
"launch_task_planner",
default_value="false",
description="Launch task_planner?",
)
)
declared_arguments.append(
DeclareLaunchArgument("cartesian_controllers",
default_value="false",
description="Load cartesian\
controllers?")
DeclareLaunchArgument(
"cartesian_controllers",
default_value="false",
description="Load cartesian\
controllers?",
)
)
declared_arguments.append(
DeclareLaunchArgument("hardware",
choices=["gazebo", "mock"],
default_value="gazebo",
description="Choose your harware_interface")
DeclareLaunchArgument(
"hardware",
choices=["gazebo", "mock"],
default_value="gazebo",
description="Choose your harware_interface",
)
)
declared_arguments.append(
DeclareLaunchArgument("launch_controllers",
default_value="true",
description="Launch controllers?")
DeclareLaunchArgument(
"launch_controllers",
default_value="true",
description="Launch controllers?",
)
)
declared_arguments.append(
DeclareLaunchArgument("gazebo_gui",
default_value="false",
description="Launch gazebo with gui?")
DeclareLaunchArgument(
"gazebo_gui", default_value="false", description="Launch gazebo with gui?"
)
)
declared_arguments.append(
DeclareLaunchArgument("namespace",
default_value="",
description="The ROS2 namespace of a robot")
DeclareLaunchArgument(
"namespace", default_value="", description="The ROS2 namespace of a robot"
)
)
declared_arguments.append(
DeclareLaunchArgument("x",
default_value="0.0",
description="Position of robot in world by X")
DeclareLaunchArgument(
"x", default_value="0.0", description="Position of robot in world by X"
)
)
declared_arguments.append(
DeclareLaunchArgument("y",
default_value="0.0",
description="Position of robot in world by Y")
DeclareLaunchArgument(
"y", default_value="0.0", description="Position of robot in world by Y"
)
)
declared_arguments.append(
DeclareLaunchArgument("z",
default_value="0.0",
description="Position of robot in world by Z")
DeclareLaunchArgument(
"z", default_value="0.0", description="Position of robot in world by Z"
)
)
declared_arguments.append(
DeclareLaunchArgument("roll",
default_value="0.0",
description="Position of robot in world by Z")
DeclareLaunchArgument(
"roll", default_value="0.0", description="Position of robot in world by Z"
)
)
declared_arguments.append(
DeclareLaunchArgument("pitch",
default_value="0.0",
description="Position of robot in world by Z")
DeclareLaunchArgument(
"pitch", default_value="0.0", description="Position of robot in world by Z"
)
)
declared_arguments.append(
DeclareLaunchArgument("yaw",
default_value="0.0",
description="Position of robot in world by Z")
DeclareLaunchArgument(
"yaw", default_value="0.0", description="Position of robot in world by Z"
)
)
declared_arguments.append(
DeclareLaunchArgument("roll",
default_value="0.0",
description="Position of robot in world by Z")
DeclareLaunchArgument(
"roll", default_value="0.0", description="Position of robot in world by Z"
)
)
declared_arguments.append(
DeclareLaunchArgument("pitch",
default_value="0.0",
description="Position of robot in world by Z")
DeclareLaunchArgument(
"pitch", default_value="0.0", description="Position of robot in world by Z"
)
)
declared_arguments.append(
DeclareLaunchArgument("yaw",
default_value="0.0",
description="Position of robot in world by Z")
DeclareLaunchArgument(
"yaw", default_value="0.0", description="Position of robot in world by Z"
)
)
declared_arguments.append(
DeclareLaunchArgument(
"multi_robot",
default_value="false",
description="Flag if you use multi robot setup"
description="Flag if you use multi robot setup",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"robot_description",
default_value="",
description="Robot description that override internal robot desctioption",
)
)
return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])
return LaunchDescription(
declared_arguments + [OpaqueFunction(function=launch_setup)]
)

176
rbs_bringup/launch/single_robot.launch.py
View file

@ -1,17 +1,18 @@
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import (
DeclareLaunchArgument,
IncludeLaunchDescription,
OpaqueFunction
OpaqueFunction,
)
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
from launch_ros.actions import Node
import os
from ament_index_python.packages import get_package_share_directory
# from nav2_common.launch import RewrittenYaml
from launch_ros.substitutions import FindPackageShare
def launch_setup(context, *args, **kwargs):
# Initialize Arguments
@ -39,32 +40,35 @@ def launch_setup(context, *args, **kwargs):
gazebo_gui = LaunchConfiguration("gazebo_gui")
gripper_name = LaunchConfiguration("gripper_name")
sim_gazebo = LaunchConfiguration("sim_gazebo")
launch_simulation = LaunchConfiguration("launch_sim")
simulation = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare('rbs_simulation'),
'launch',
'simulation_gazebo.launch.py'
])
]),
PythonLaunchDescriptionSource(
[
PathJoinSubstitution(
[
FindPackageShare("rbs_simulation"),
"launch",
"simulation_gazebo.launch.py",
]
)
]
),
launch_arguments={
'sim_gazebo': sim_gazebo,
'debugger': "false",
'launch_env_manager': env_manager,
'gazebo_world_filename': "asm2.sdf"
"sim_gazebo": sim_gazebo,
"debugger": "false",
"launch_env_manager": env_manager,
"gazebo_world_filename": "asm2.sdf",
}.items(),
condition=IfCondition(launch_simulation)
condition=IfCondition(launch_simulation),
)
# FIXME: namespaces
# configured_params = RewrittenYaml(
# source_file=os.path.join(
# get_package_share_directory(
# description_package.perform(context)),
# "config",
# description_package.perform(context)),
# "config",
# controllers_file.perform(context)),
# root_key=robot_name.perform(context),
# param_rewrites={},
@ -72,34 +76,38 @@ def launch_setup(context, *args, **kwargs):
# )
initial_joint_controllers_file_path = os.path.join(
get_package_share_directory('rbs_arm'), 'config', 'rbs_arm0_controllers.yaml'
get_package_share_directory("rbs_arm"), "config", "rbs_arm0_controllers.yaml"
)
# controller_paramfile = configured_params.perform(context)
# controller_paramfile = PathJoinSubstitution([
# FindPackageShare(robot_type), "config", "rbs_arm0_controllers.yaml"
# ])
# namespace = "/" + robot_name.perform(context)
# namespace = "/" + robot_name.perform(context)
namespace = ""
gz_spawner = Node(
package='ros_gz_sim',
executable='create',
package="ros_gz_sim",
executable="create",
# prefix=['gdbserver localhost:1234'],
arguments=[
'-name', robot_name.perform(context),
'-topic', namespace + '/robot_description'],
output='screen',
condition=IfCondition(sim_gazebo))
"-name",
robot_name.perform(context),
"-topic",
namespace + "/robot_description",
],
output="screen",
condition=IfCondition(sim_gazebo),
)
single_robot_setup = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare('rbs_bringup'),
"launch",
"rbs_robot.launch.py"
])
]),
PythonLaunchDescriptionSource(
[
PathJoinSubstitution(
[FindPackageShare("rbs_bringup"), "launch", "rbs_robot.launch.py"]
)
]
),
launch_arguments={
"env_manager": env_manager,
"with_gripper": with_gripper_condition,
@ -127,14 +135,10 @@ def launch_setup(context, *args, **kwargs):
# "x": "0.5",
# "y": "0.5",
# "z": "0.5"
}.items()
}.items(),
)
nodes_to_start = [
simulation,
gz_spawner,
single_robot_setup
]
nodes_to_start = [simulation, gz_spawner, single_robot_setup]
return nodes_to_start
@ -144,7 +148,7 @@ def generate_launch_description():
DeclareLaunchArgument(
"robot_type",
description="Type of robot by name",
choices=["rbs_arm","ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
choices=["rbs_arm", "ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
default_value="rbs_arm",
)
)
@ -224,62 +228,74 @@ def generate_launch_description():
)
)
declared_arguments.append(
DeclareLaunchArgument("with_gripper",
default_value="true",
description="With gripper or not?")
DeclareLaunchArgument(
"with_gripper", default_value="true", description="With gripper or not?"
)
)
declared_arguments.append(
DeclareLaunchArgument("sim_gazebo",
default_value="true",
description="Gazebo Simulation")
DeclareLaunchArgument(
"sim_gazebo", default_value="true", description="Gazebo Simulation"
)
)
declared_arguments.append(
DeclareLaunchArgument("env_manager",
default_value="true",
description="Launch env_manager?")
DeclareLaunchArgument(
"env_manager", default_value="true", description="Launch env_manager?"
)
)
declared_arguments.append(
DeclareLaunchArgument("launch_sim",
default_value="true",
description="Launch simulator (Gazebo)?\
Most general arg")
DeclareLaunchArgument(
"launch_sim",
default_value="true",
description="Launch simulator (Gazebo)?\
Most general arg",
)
)
declared_arguments.append(
DeclareLaunchArgument("launch_moveit",
default_value="false",
description="Launch moveit?")
DeclareLaunchArgument(
"launch_moveit", default_value="false", description="Launch moveit?"
)
)
declared_arguments.append(
DeclareLaunchArgument("launch_perception",
default_value="false",
description="Launch perception?")
DeclareLaunchArgument(
"launch_perception", default_value="false", description="Launch perception?"
)
)
declared_arguments.append(
DeclareLaunchArgument("launch_task_planner",
default_value="false",
description="Launch task_planner?")
DeclareLaunchArgument(
"launch_task_planner",
default_value="false",
description="Launch task_planner?",
)
)
declared_arguments.append(
DeclareLaunchArgument("cartesian_controllers",
default_value="true",
description="Load cartesian\
controllers?")
DeclareLaunchArgument(
"cartesian_controllers",
default_value="true",
description="Load cartesian\
controllers?",
)
)
declared_arguments.append(
DeclareLaunchArgument("hardware",
choices=["gazebo", "mock"],
default_value="gazebo",
description="Choose your harware_interface")
DeclareLaunchArgument(
"hardware",
choices=["gazebo", "mock"],
default_value="gazebo",
description="Choose your harware_interface",
)
)
declared_arguments.append(
DeclareLaunchArgument("launch_controllers",
default_value="true",
description="Launch controllers?")
DeclareLaunchArgument(
"launch_controllers",
default_value="true",
description="Launch controllers?",
)
)
declared_arguments.append(
DeclareLaunchArgument("gazebo_gui",
default_value="true",
description="Launch gazebo with gui?")
DeclareLaunchArgument(
"gazebo_gui", default_value="true", description="Launch gazebo with gui?"
)
)
return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])
return LaunchDescription(
declared_arguments + [OpaqueFunction(function=launch_setup)]
)