refactor: move env_manager to new repo
This commit is contained in:
parent
988800abc0
commit
223807255b
125 changed files with 4 additions and 17930 deletions
|
|
@ -1,202 +0,0 @@
|
|||
|
||||
Apache License
|
||||
Version 2.0, January 2004
|
||||
http://www.apache.org/licenses/
|
||||
|
||||
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
|
||||
|
||||
1. Definitions.
|
||||
|
||||
"License" shall mean the terms and conditions for use, reproduction,
|
||||
and distribution as defined by Sections 1 through 9 of this document.
|
||||
|
||||
"Licensor" shall mean the copyright owner or entity authorized by
|
||||
the copyright owner that is granting the License.
|
||||
|
||||
"Legal Entity" shall mean the union of the acting entity and all
|
||||
other entities that control, are controlled by, or are under common
|
||||
control with that entity. For the purposes of this definition,
|
||||
"control" means (i) the power, direct or indirect, to cause the
|
||||
direction or management of such entity, whether by contract or
|
||||
otherwise, or (ii) ownership of fifty percent (50%) or more of the
|
||||
outstanding shares, or (iii) beneficial ownership of such entity.
|
||||
|
||||
"You" (or "Your") shall mean an individual or Legal Entity
|
||||
exercising permissions granted by this License.
|
||||
|
||||
"Source" form shall mean the preferred form for making modifications,
|
||||
including but not limited to software source code, documentation
|
||||
source, and configuration files.
|
||||
|
||||
"Object" form shall mean any form resulting from mechanical
|
||||
transformation or translation of a Source form, including but
|
||||
not limited to compiled object code, generated documentation,
|
||||
and conversions to other media types.
|
||||
|
||||
"Work" shall mean the work of authorship, whether in Source or
|
||||
Object form, made available under the License, as indicated by a
|
||||
copyright notice that is included in or attached to the work
|
||||
(an example is provided in the Appendix below).
|
||||
|
||||
"Derivative Works" shall mean any work, whether in Source or Object
|
||||
form, that is based on (or derived from) the Work and for which the
|
||||
editorial revisions, annotations, elaborations, or other modifications
|
||||
represent, as a whole, an original work of authorship. For the purposes
|
||||
of this License, Derivative Works shall not include works that remain
|
||||
separable from, or merely link (or bind by name) to the interfaces of,
|
||||
the Work and Derivative Works thereof.
|
||||
|
||||
"Contribution" shall mean any work of authorship, including
|
||||
the original version of the Work and any modifications or additions
|
||||
to that Work or Derivative Works thereof, that is intentionally
|
||||
submitted to Licensor for inclusion in the Work by the copyright owner
|
||||
or by an individual or Legal Entity authorized to submit on behalf of
|
||||
the copyright owner. For the purposes of this definition, "submitted"
|
||||
means any form of electronic, verbal, or written communication sent
|
||||
to the Licensor or its representatives, including but not limited to
|
||||
communication on electronic mailing lists, source code control systems,
|
||||
and issue tracking systems that are managed by, or on behalf of, the
|
||||
Licensor for the purpose of discussing and improving the Work, but
|
||||
excluding communication that is conspicuously marked or otherwise
|
||||
designated in writing by the copyright owner as "Not a Contribution."
|
||||
|
||||
"Contributor" shall mean Licensor and any individual or Legal Entity
|
||||
on behalf of whom a Contribution has been received by Licensor and
|
||||
subsequently incorporated within the Work.
|
||||
|
||||
2. Grant of Copyright License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
copyright license to reproduce, prepare Derivative Works of,
|
||||
publicly display, publicly perform, sublicense, and distribute the
|
||||
Work and such Derivative Works in Source or Object form.
|
||||
|
||||
3. Grant of Patent License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
(except as stated in this section) patent license to make, have made,
|
||||
use, offer to sell, sell, import, and otherwise transfer the Work,
|
||||
where such license applies only to those patent claims licensable
|
||||
by such Contributor that are necessarily infringed by their
|
||||
Contribution(s) alone or by combination of their Contribution(s)
|
||||
with the Work to which such Contribution(s) was submitted. If You
|
||||
institute patent litigation against any entity (including a
|
||||
cross-claim or counterclaim in a lawsuit) alleging that the Work
|
||||
or a Contribution incorporated within the Work constitutes direct
|
||||
or contributory patent infringement, then any patent licenses
|
||||
granted to You under this License for that Work shall terminate
|
||||
as of the date such litigation is filed.
|
||||
|
||||
4. Redistribution. You may reproduce and distribute copies of the
|
||||
Work or Derivative Works thereof in any medium, with or without
|
||||
modifications, and in Source or Object form, provided that You
|
||||
meet the following conditions:
|
||||
|
||||
(a) You must give any other recipients of the Work or
|
||||
Derivative Works a copy of this License; and
|
||||
|
||||
(b) You must cause any modified files to carry prominent notices
|
||||
stating that You changed the files; and
|
||||
|
||||
(c) You must retain, in the Source form of any Derivative Works
|
||||
that You distribute, all copyright, patent, trademark, and
|
||||
attribution notices from the Source form of the Work,
|
||||
excluding those notices that do not pertain to any part of
|
||||
the Derivative Works; and
|
||||
|
||||
(d) If the Work includes a "NOTICE" text file as part of its
|
||||
distribution, then any Derivative Works that You distribute must
|
||||
include a readable copy of the attribution notices contained
|
||||
within such NOTICE file, excluding those notices that do not
|
||||
pertain to any part of the Derivative Works, in at least one
|
||||
of the following places: within a NOTICE text file distributed
|
||||
as part of the Derivative Works; within the Source form or
|
||||
documentation, if provided along with the Derivative Works; or,
|
||||
within a display generated by the Derivative Works, if and
|
||||
wherever such third-party notices normally appear. The contents
|
||||
of the NOTICE file are for informational purposes only and
|
||||
do not modify the License. You may add Your own attribution
|
||||
notices within Derivative Works that You distribute, alongside
|
||||
or as an addendum to the NOTICE text from the Work, provided
|
||||
that such additional attribution notices cannot be construed
|
||||
as modifying the License.
|
||||
|
||||
You may add Your own copyright statement to Your modifications and
|
||||
may provide additional or different license terms and conditions
|
||||
for use, reproduction, or distribution of Your modifications, or
|
||||
for any such Derivative Works as a whole, provided Your use,
|
||||
reproduction, and distribution of the Work otherwise complies with
|
||||
the conditions stated in this License.
|
||||
|
||||
5. Submission of Contributions. Unless You explicitly state otherwise,
|
||||
any Contribution intentionally submitted for inclusion in the Work
|
||||
by You to the Licensor shall be under the terms and conditions of
|
||||
this License, without any additional terms or conditions.
|
||||
Notwithstanding the above, nothing herein shall supersede or modify
|
||||
the terms of any separate license agreement you may have executed
|
||||
with Licensor regarding such Contributions.
|
||||
|
||||
6. Trademarks. This License does not grant permission to use the trade
|
||||
names, trademarks, service marks, or product names of the Licensor,
|
||||
except as required for reasonable and customary use in describing the
|
||||
origin of the Work and reproducing the content of the NOTICE file.
|
||||
|
||||
7. Disclaimer of Warranty. Unless required by applicable law or
|
||||
agreed to in writing, Licensor provides the Work (and each
|
||||
Contributor provides its Contributions) on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
|
||||
implied, including, without limitation, any warranties or conditions
|
||||
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
|
||||
PARTICULAR PURPOSE. You are solely responsible for determining the
|
||||
appropriateness of using or redistributing the Work and assume any
|
||||
risks associated with Your exercise of permissions under this License.
|
||||
|
||||
8. Limitation of Liability. In no event and under no legal theory,
|
||||
whether in tort (including negligence), contract, or otherwise,
|
||||
unless required by applicable law (such as deliberate and grossly
|
||||
negligent acts) or agreed to in writing, shall any Contributor be
|
||||
liable to You for damages, including any direct, indirect, special,
|
||||
incidental, or consequential damages of any character arising as a
|
||||
result of this License or out of the use or inability to use the
|
||||
Work (including but not limited to damages for loss of goodwill,
|
||||
work stoppage, computer failure or malfunction, or any and all
|
||||
other commercial damages or losses), even if such Contributor
|
||||
has been advised of the possibility of such damages.
|
||||
|
||||
9. Accepting Warranty or Additional Liability. While redistributing
|
||||
the Work or Derivative Works thereof, You may choose to offer,
|
||||
and charge a fee for, acceptance of support, warranty, indemnity,
|
||||
or other liability obligations and/or rights consistent with this
|
||||
License. However, in accepting such obligations, You may act only
|
||||
on Your own behalf and on Your sole responsibility, not on behalf
|
||||
of any other Contributor, and only if You agree to indemnify,
|
||||
defend, and hold each Contributor harmless for any liability
|
||||
incurred by, or claims asserted against, such Contributor by reason
|
||||
of your accepting any such warranty or additional liability.
|
||||
|
||||
END OF TERMS AND CONDITIONS
|
||||
|
||||
APPENDIX: How to apply the Apache License to your work.
|
||||
|
||||
To apply the Apache License to your work, attach the following
|
||||
boilerplate notice, with the fields enclosed by brackets "[]"
|
||||
replaced with your own identifying information. (Don't include
|
||||
the brackets!) The text should be enclosed in the appropriate
|
||||
comment syntax for the file format. We also recommend that a
|
||||
file or class name and description of purpose be included on the
|
||||
same "printed page" as the copyright notice for easier
|
||||
identification within third-party archives.
|
||||
|
||||
Copyright [yyyy] [name of copyright owner]
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
|
|
@ -1,249 +0,0 @@
|
|||
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: star
|
||||
orientation: !tuple
|
||||
- 1.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
position: !tuple
|
||||
- -0.1
|
||||
- -0.40
|
||||
- 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: box
|
||||
mass: 1.0
|
||||
size: !tuple
|
||||
- 0.02
|
||||
- 0.02
|
||||
- 0.05
|
||||
orientation: !tuple
|
||||
- 1.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
position: !tuple
|
||||
- -0.1
|
||||
- -0.37
|
||||
- 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
|
||||
type: "box"
|
||||
|
||||
|
||||
- color: null
|
||||
name: cylinder
|
||||
mass: 1.0
|
||||
radius: 0.01
|
||||
length: 0.05
|
||||
orientation: !tuple
|
||||
- 1.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
position: !tuple
|
||||
- -0.1
|
||||
- -0.34
|
||||
- 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
|
||||
type: "cylinder"
|
||||
|
||||
- color: null
|
||||
name: board
|
||||
orientation: !tuple
|
||||
- 1.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
- 0.0
|
||||
position: !tuple
|
||||
- 0.0
|
||||
- -0.37
|
||||
- 0.01
|
||||
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: true
|
||||
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:
|
||||
- -1.57
|
||||
- 0.5
|
||||
- 3.14159
|
||||
- 1.5
|
||||
- 0.0
|
||||
- 1.4
|
||||
- 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
|
||||
|
|
@ -1,354 +0,0 @@
|
|||
import os
|
||||
|
||||
from launch.launch_introspector import indent
|
||||
import xacro
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import (
|
||||
DeclareLaunchArgument,
|
||||
IncludeLaunchDescription,
|
||||
OpaqueFunction,
|
||||
TimerAction,
|
||||
)
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
|
||||
from robot_builder.parser.urdf import URDF_parser
|
||||
from robot_builder.external.ros2_control import ControllerManager
|
||||
import yaml
|
||||
|
||||
|
||||
def launch_setup(context, *args, **kwargs):
|
||||
# Initialize Arguments
|
||||
robot_type = LaunchConfiguration("robot_type")
|
||||
# General arguments
|
||||
with_gripper_condition = LaunchConfiguration("with_gripper")
|
||||
description_package = LaunchConfiguration("description_package")
|
||||
description_file = LaunchConfiguration("description_file")
|
||||
use_moveit = LaunchConfiguration("use_moveit")
|
||||
moveit_config_package = LaunchConfiguration("moveit_config_package")
|
||||
moveit_config_file = LaunchConfiguration("moveit_config_file")
|
||||
use_sim_time = LaunchConfiguration("use_sim_time")
|
||||
scene_config_file = LaunchConfiguration("scene_config_file").perform(context)
|
||||
ee_link_name = LaunchConfiguration("ee_link_name").perform(context)
|
||||
base_link_name = LaunchConfiguration("base_link_name").perform(context)
|
||||
control_space = LaunchConfiguration("control_space").perform(context)
|
||||
control_strategy = LaunchConfiguration("control_strategy").perform(context)
|
||||
interactive = LaunchConfiguration("interactive").perform(context)
|
||||
real_robot = LaunchConfiguration("real_robot").perform(context)
|
||||
|
||||
use_rbs_utils = LaunchConfiguration("use_rbs_utils")
|
||||
assembly_config_name = LaunchConfiguration("assembly_config_name")
|
||||
|
||||
if not scene_config_file == "":
|
||||
config_file = {"config_file": scene_config_file}
|
||||
else:
|
||||
config_file = {}
|
||||
|
||||
description_package_abs_path = get_package_share_directory(
|
||||
description_package.perform(context)
|
||||
)
|
||||
|
||||
controllers_file = os.path.join(
|
||||
description_package_abs_path, "config", "controllers.yaml"
|
||||
)
|
||||
|
||||
xacro_file = os.path.join(
|
||||
description_package_abs_path,
|
||||
"urdf",
|
||||
description_file.perform(context),
|
||||
)
|
||||
|
||||
xacro_config_file = f"{description_package_abs_path}/config/xacro_args.yaml"
|
||||
|
||||
|
||||
# TODO: hide this to another place
|
||||
# Load xacro_args
|
||||
def param_constructor(loader, node, local_vars):
|
||||
value = loader.construct_scalar(node)
|
||||
return LaunchConfiguration(value).perform(
|
||||
local_vars.get("context", "Launch context if not defined")
|
||||
)
|
||||
|
||||
def variable_constructor(loader, node, local_vars):
|
||||
value = loader.construct_scalar(node)
|
||||
return local_vars.get(value, f"Variable '{value}' not found")
|
||||
|
||||
def load_xacro_args(yaml_file, local_vars):
|
||||
# Get valut from ros2 argument
|
||||
yaml.add_constructor(
|
||||
"!param", lambda loader, node: param_constructor(loader, node, local_vars)
|
||||
)
|
||||
|
||||
# Get value from local variable in this code
|
||||
# The local variable should be initialized before the loader was called
|
||||
yaml.add_constructor(
|
||||
"!variable",
|
||||
lambda loader, node: variable_constructor(loader, node, local_vars),
|
||||
)
|
||||
|
||||
with open(yaml_file, "r") as file:
|
||||
return yaml.load(file, Loader=yaml.FullLoader)
|
||||
|
||||
mappings_data = load_xacro_args(xacro_config_file, locals())
|
||||
|
||||
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 = {"robot_description": robot_description_content}
|
||||
|
||||
# Parse robot and configure controller's file for ControllerManager
|
||||
robot = URDF_parser.load_string(
|
||||
robot_description_content, ee_link_name=ee_link_name
|
||||
)
|
||||
ControllerManager.save_to_yaml(
|
||||
robot, description_package_abs_path, "controllers.yaml"
|
||||
)
|
||||
|
||||
rbs_robot_setup = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
[
|
||||
PathJoinSubstitution(
|
||||
[FindPackageShare("rbs_bringup"), "launch", "rbs_robot.launch.py"]
|
||||
)
|
||||
]
|
||||
),
|
||||
launch_arguments={
|
||||
"with_gripper": with_gripper_condition,
|
||||
"controllers_file": controllers_file,
|
||||
"robot_type": robot_type,
|
||||
"description_package": description_package,
|
||||
"description_file": description_file,
|
||||
"robot_name": robot_type,
|
||||
"use_moveit": use_moveit,
|
||||
"moveit_config_package": moveit_config_package,
|
||||
"moveit_config_file": moveit_config_file,
|
||||
"use_sim_time": use_sim_time,
|
||||
"use_controllers": "true",
|
||||
"robot_description": robot_description_content,
|
||||
"robot_description_semantic": robot_description_semantic_content,
|
||||
"base_link_name": base_link_name,
|
||||
"ee_link_name": ee_link_name,
|
||||
"control_space": control_space,
|
||||
"control_strategy": control_strategy,
|
||||
"interactive_control": interactive,
|
||||
"use_rbs_utils": use_rbs_utils,
|
||||
"assembly_config_name": assembly_config_name
|
||||
}.items(),
|
||||
)
|
||||
|
||||
if real_robot == "true":
|
||||
controller_manager_node = Node(
|
||||
package="controller_manager",
|
||||
executable="ros2_control_node",
|
||||
parameters=[
|
||||
robot_description,
|
||||
controllers_file,
|
||||
],
|
||||
output="screen",
|
||||
)
|
||||
return [rbs_robot_setup, controller_manager_node]
|
||||
|
||||
rbs_runtime = Node(
|
||||
package="rbs_runtime",
|
||||
executable="runtime",
|
||||
arguments=[('--ros-args --remap log_level:=debug')],
|
||||
parameters=[robot_description, config_file, {"use_sim_time": True}],
|
||||
)
|
||||
|
||||
clock_bridge = Node(
|
||||
package="ros_gz_bridge",
|
||||
executable="parameter_bridge",
|
||||
arguments=["/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock"],
|
||||
output="screen",
|
||||
)
|
||||
|
||||
delay_robot_control_stack = TimerAction(period=10.0, actions=[rbs_robot_setup])
|
||||
|
||||
nodes_to_start = [
|
||||
rbs_runtime,
|
||||
clock_bridge,
|
||||
delay_robot_control_stack
|
||||
]
|
||||
return nodes_to_start
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
declared_arguments = []
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"robot_type",
|
||||
description="Type of robot by name",
|
||||
choices=[
|
||||
"rbs_arm",
|
||||
"ar4",
|
||||
"ur3",
|
||||
"ur3e",
|
||||
"ur5",
|
||||
"ur5e",
|
||||
"ur10",
|
||||
"ur10e",
|
||||
"ur16e",
|
||||
],
|
||||
default_value="rbs_arm",
|
||||
)
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"description_package",
|
||||
default_value="rbs_arm",
|
||||
description="Description package with robot URDF/XACRO files. Usually the argument \
|
||||
is not set, it enables use of a custom description.",
|
||||
)
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"description_file",
|
||||
default_value="rbs_arm_modular.xacro",
|
||||
description="URDF/XACRO description file with the robot.",
|
||||
)
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"robot_name",
|
||||
default_value="arm0",
|
||||
description="Name for robot, used to apply namespace for specific robot in multirobot setup",
|
||||
)
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"moveit_config_package",
|
||||
default_value="rbs_arm",
|
||||
description="MoveIt config package with robot SRDF/XACRO files. Usually the argument \
|
||||
is not set, it enables use of a custom moveit config.",
|
||||
)
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"moveit_config_file",
|
||||
default_value="rbs_arm.srdf.xacro",
|
||||
description="MoveIt SRDF/XACRO description file with the robot.",
|
||||
)
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"use_sim_time",
|
||||
default_value="true",
|
||||
description="Make MoveIt to use simulation time.\
|
||||
This is needed for the trajectory planing in simulation.",
|
||||
)
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"with_gripper", default_value="true", description="With gripper or not?"
|
||||
)
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"use_moveit", default_value="false", description="Launch moveit?"
|
||||
)
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"launch_perception", default_value="false", description="Launch perception?"
|
||||
)
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"use_controllers",
|
||||
default_value="true",
|
||||
description="Launch controllers?",
|
||||
)
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"scene_config_file",
|
||||
default_value="",
|
||||
description="Path to a scene configuration file",
|
||||
)
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"ee_link_name",
|
||||
default_value="",
|
||||
description="End effector name of robot arm",
|
||||
)
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"base_link_name",
|
||||
default_value="",
|
||||
description="Base link name if robot arm",
|
||||
)
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"control_space",
|
||||
default_value="task",
|
||||
choices=["task", "joint"],
|
||||
description="Specify the control space for the robot (e.g., task space).",
|
||||
)
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"control_strategy",
|
||||
default_value="position",
|
||||
choices=["position", "velocity", "effort"],
|
||||
description="Specify the control strategy (e.g., position control).",
|
||||
)
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"interactive",
|
||||
default_value="true",
|
||||
description="Wheter to run the motion_control_handle controller",
|
||||
),
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"real_robot",
|
||||
default_value="false",
|
||||
description="Wheter to run on the real robot",
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"use_rbs_utils",
|
||||
default_value="true",
|
||||
description="Wheter to use rbs_utils",
|
||||
),
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"assembly_config_name",
|
||||
default_value="",
|
||||
description="Assembly config name from rbs_assets_library",
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
return LaunchDescription(
|
||||
declared_arguments + [OpaqueFunction(function=launch_setup)]
|
||||
)
|
||||
|
|
@ -1,18 +0,0 @@
|
|||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>rbs_runtime</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="ur.narmak@gmail.com">narmak</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
|
|
@ -1,41 +0,0 @@
|
|||
from pathlib import Path
|
||||
|
||||
import yaml
|
||||
from dacite import from_dict
|
||||
from env_manager.models.configs import SceneData
|
||||
|
||||
from typing import Dict, Any
|
||||
from env_manager.models.configs import (
|
||||
BoxObjectData, CylinderObjectData, MeshData, ModelData, ObjectData, ObjectRandomizerData
|
||||
)
|
||||
|
||||
def object_factory(obj_data: Dict[str, Any]) -> ObjectData:
|
||||
obj_type = obj_data.get('type', '')
|
||||
|
||||
if "randomize" in obj_data and isinstance(obj_data["randomize"], dict):
|
||||
obj_data["randomize"] = from_dict(data_class=ObjectRandomizerData, data=obj_data["randomize"])
|
||||
|
||||
if obj_type == 'box':
|
||||
return from_dict(data_class=BoxObjectData, data=obj_data)
|
||||
elif obj_type == 'cylinder':
|
||||
return from_dict(data_class=CylinderObjectData, data=obj_data)
|
||||
elif obj_type == 'mesh':
|
||||
return from_dict(data_class=MeshData, data=obj_data)
|
||||
elif obj_type == 'model':
|
||||
return from_dict(data_class=ModelData, data=obj_data)
|
||||
else:
|
||||
return from_dict(data_class=ObjectData, data=obj_data)
|
||||
|
||||
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)
|
||||
|
||||
scene_data.objects = [object_factory(obj) for obj in scene_cfg.get('objects', [])]
|
||||
|
||||
return scene_data
|
||||
|
|
@ -1,73 +0,0 @@
|
|||
#!/usr/bin/python3
|
||||
|
||||
import rclpy
|
||||
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
|
||||
from rclpy.executors import MultiThreadedExecutor, ExternalShutdownException
|
||||
|
||||
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=scene_data,
|
||||
robot_urdf_string=self.get_parameter("robot_description")
|
||||
.get_parameter_value()
|
||||
.string_value,
|
||||
)
|
||||
self.scene.init_scene()
|
||||
|
||||
self.reset_env_srv = self.create_service(
|
||||
ResetEnv, "/env_manager/reset_env", self.reset_env
|
||||
)
|
||||
self.is_env_reset = False
|
||||
self.timer = self.create_timer(self.gazebo.step_size(), self.gazebo_run)
|
||||
|
||||
def gazebo_run(self):
|
||||
if self.is_env_reset:
|
||||
self.scene.reset_scene()
|
||||
self.is_env_reset = False
|
||||
self.gazebo.run()
|
||||
|
||||
def reset_env(self, req, res):
|
||||
self.is_env_reset = True
|
||||
res.ok = self.is_env_reset
|
||||
return res
|
||||
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
|
||||
executor = MultiThreadedExecutor()
|
||||
node = GazeboRuntime()
|
||||
executor.add_node(node)
|
||||
try:
|
||||
executor.spin()
|
||||
except (KeyboardInterrupt, ExternalShutdownException):
|
||||
node.destroy_node()
|
||||
|
|
@ -1,4 +0,0 @@
|
|||
[develop]
|
||||
script_dir=$base/lib/rbs_runtime
|
||||
[install]
|
||||
install_scripts=$base/lib/rbs_runtime
|
||||
|
|
@ -1,36 +0,0 @@
|
|||
import os
|
||||
from glob import glob
|
||||
|
||||
from setuptools import find_packages, setup
|
||||
|
||||
package_name = "rbs_runtime"
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version="0.0.0",
|
||||
packages=find_packages(exclude=["test"]),
|
||||
data_files=[
|
||||
("share/ament_index/resource_index/packages", ["resource/" + package_name]),
|
||||
("share/" + package_name, ["package.xml"]),
|
||||
(
|
||||
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,
|
||||
maintainer="narmak",
|
||||
maintainer_email="ur.narmak@gmail.com",
|
||||
description="TODO: Package description",
|
||||
license="Apache-2.0",
|
||||
tests_require=["pytest"],
|
||||
entry_points={
|
||||
"console_scripts": [
|
||||
"runtime = rbs_runtime.scripts.runtime:main",
|
||||
],
|
||||
},
|
||||
)
|
||||
|
|
@ -1,25 +0,0 @@
|
|||
# Copyright 2015 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_copyright.main import main
|
||||
import pytest
|
||||
|
||||
|
||||
# Remove the `skip` decorator once the source file(s) have a copyright header
|
||||
@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
|
||||
@pytest.mark.copyright
|
||||
@pytest.mark.linter
|
||||
def test_copyright():
|
||||
rc = main(argv=['.', 'test'])
|
||||
assert rc == 0, 'Found errors'
|
||||
|
|
@ -1,25 +0,0 @@
|
|||
# Copyright 2017 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_flake8.main import main_with_errors
|
||||
import pytest
|
||||
|
||||
|
||||
@pytest.mark.flake8
|
||||
@pytest.mark.linter
|
||||
def test_flake8():
|
||||
rc, errors = main_with_errors(argv=[])
|
||||
assert rc == 0, \
|
||||
'Found %d code style errors / warnings:\n' % len(errors) + \
|
||||
'\n'.join(errors)
|
||||
|
|
@ -1,23 +0,0 @@
|
|||
# Copyright 2015 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_pep257.main import main
|
||||
import pytest
|
||||
|
||||
|
||||
@pytest.mark.linter
|
||||
@pytest.mark.pep257
|
||||
def test_pep257():
|
||||
rc = main(argv=['.', 'test'])
|
||||
assert rc == 0, 'Found code style errors / warnings'
|
||||
Loading…
Add table
Add a link
Reference in a new issue