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

Merge remote-tracking branch 'origin/main' into 101-pose_estimation_DOPE

Browse source
This commit is contained in:
shalenikol 2024-04-02 11:04:57 +03:00
commit b4952d0234
20 changed files with 450 additions and 340 deletions
Download patch file Download diff file Expand all files Collapse all files

2
.gitlab-ci.yml
View file

@ -26,7 +26,7 @@ build-colcon-job:
- mv * .src/robossembler-ros2
- mv .git .src/robossembler-ros2
- mv .src src
- vcs import src < src/robossembler-ros2/rbs.sim.repos
- vcs import src < src/robossembler-ros2/repos/sim.rbs.repos
- rosdep install -y -r -q --from-paths src --ignore-src --rosdistro humble
- colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release
rules:

15
README.md
View file

@ -26,20 +26,27 @@ Prepare workspace & install dependencies (So far only tested with UR robot arm)
```bash
mkdir -p ~/robossembler_ws/src && cd ~/robossembler_ws/src
git clone https://gitlab.com/robosphere/robossembler-ros2
vcs import . < robossembler-ros2/rbs.sim.repos
vcs import . < robossembler-ros2/repos/sim.rbs.repos
cd ..
rosdep install -y -r -q --from-paths src --ignore-src --rosdistro humble
colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install
```
<!-- For simulation with Mujoco, pls [ref](https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers/blob/fdaa9a2dd0ce15fc949b127dd1164095504273fb/cartesian_controller_simulation/README.md) build and install section. -->
Additionally, if you want to use Cartesian controllers, such as stiffness or others, you need to execute the following:
```
# in directory robossembler_ws/src
vcs import . < robossembler-ros2/repos/cartesian_controllers.repos
```
This will also install `ros2_control` and `gz_ros2_control` as packages, so it is recommended to delete global packages if they have been installed.
### Set Gazebo enviroment variables
Replace `[WS_FOLDER]` with your workspace folder
```bash
echo "export IGN_GAZEBO_RESOURCE_PATH=${IGN_GAZEBO_RESOURCE_PATH}:~/[WS_FOLDER]/install/robotiq_description/share/:~/[WS_FOLDER]/install/rbs_simulation/share/rbs_simulation/" >> ~/.bashrc
echo "export IGN_GAZEBO_RESOURCE_PATH=${IGN_GAZEBO_RESOURCE_PATH}:~/[WS_FOLDER]/install/rbs_simulation/share/rbs_simulation/" >> ~/.bashrc
# or if you have alredy built the workspace
echo "export IGN_GAZEBO_RESOURCE_PATH=${IGN_GAZEBO_RESOURCE_PATH}:~/$(ros2 pkg prefix rbs_simulation)/share/rbs_simulation/" >> ~/.bashrc
```
### Examples
Activate current ROS2 enviroment:
```

4
env_manager/gz_enviroment/src/gz_enviroment.cpp
View file

@ -41,7 +41,7 @@ CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State &) {
m_topic_name = std::string("/world/") + m_world_name + "/dynamic_pose/info";
m_service_spawn = std::string("/world/") + m_world_name + "/create";
m_config_loader = std::make_shared<rbs_utils::AssemblyConfigLoader>(
"asp-example2", getNode());
"asp-example", getNode());
m_follow_frames = m_config_loader->getSceneModelNames();
// m_target_places = std::make_shared<tf2_msgs::msg::TFMessage>();
m_transforms = m_config_loader->getTfData("bishop");
@ -92,8 +92,6 @@ CallbackReturn GzEnviroment::on_deactivate(const rclcpp_lifecycle::State &) {
// TODO: Check to do this via EntityComponentManager
void GzEnviroment::onGzPoseSub(const gz::msgs::Pose_V &pose_v) {
// TODO: Read from config
// m_follow_frames = {"box1", "box2", "box3", "box4", "box5", "box6"};
std::vector<geometry_msgs::msg::TransformStamped> all_transforms{};
for (const auto &it : pose_v.pose()) {

281
rbs_bringup/config/rbs.rviz
View file

@ -6,9 +6,8 @@ Panels:
Expanded:
- /Global Options1
- /TF1/Frames1
- /MarkerArray2
Splitter Ratio: 0.49496981501579285
Tree Height: 981
Tree Height: 985
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
@ -76,62 +75,37 @@ Visualization Manager:
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
base:
Alpha: 1
Show Axes: false
Show Trail: false
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
base_link_inertia:
Value: true
ee_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
flange:
Alpha: 1
Show Axes: false
Show Trail: false
forearm_link:
fork0_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
ft_frame:
Alpha: 1
Show Axes: false
Show Trail: false
inner_rgbd_camera:
fork1_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
outer_rgbd_camera:
fork2_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
schunk_grasp_point:
Alpha: 1
Show Axes: false
Show Trail: false
schunk_gripper_base_link:
main0_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
schunk_l_finger_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
schunk_r_finger_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
shoulder_link:
main1_link:
Alpha: 1
Show Axes: false
Show Trail: false
@ -140,77 +114,37 @@ Visualization Manager:
Alpha: 1
Show Axes: false
Show Trail: false
upper_arm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
world:
Alpha: 1
Show Axes: false
Show Trail: false
wrist_1_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wrist_2_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wrist_3_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Robot Alpha: 1
Show Robot Collision: false
Show Robot Visual: true
Show Robot Visual: false
Value: true
- Class: rviz_default_plugins/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: false
base:
Value: false
base_link:
Value: false
base_link_inertia:
Value: false
flange:
Value: false
forearm_link:
Value: false
ft_frame:
Value: false
inner_rgbd_camera:
Value: false
outer_rgbd_camera:
Value: false
schunk_grasp_point:
ee_link:
Value: true
schunk_gripper_base_link:
fork0_link:
Value: true
schunk_l_finger_link:
fork1_link:
Value: true
schunk_r_finger_link:
fork2_link:
Value: true
main0_link:
Value: true
main1_link:
Value: true
shoulder_link:
Value: false
tool0:
Value: false
upper_arm_link:
Value: false
world:
Value: false
wrist_1_link:
Value: false
wrist_2_link:
Value: false
wrist_3_link:
Value: false
Marker Scale: 0.4000000059604645
Name: TF
Show Arrows: true
@ -219,30 +153,14 @@ Visualization Manager:
Tree:
world:
base_link:
base:
{}
base_link_inertia:
shoulder_link:
upper_arm_link:
forearm_link:
wrist_1_link:
wrist_2_link:
wrist_3_link:
flange:
tool0:
inner_rgbd_camera:
{}
schunk_grasp_point:
{}
schunk_gripper_base_link:
schunk_l_finger_link:
{}
schunk_r_finger_link:
{}
ft_frame:
{}
outer_rgbd_camera:
{}
fork0_link:
main0_link:
fork1_link:
main1_link:
fork2_link:
ee_link:
tool0:
{}
Update Interval: 0
Value: true
- Class: moveit_rviz_plugin/Trajectory
@ -255,62 +173,37 @@ Visualization Manager:
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
base:
Alpha: 1
Show Axes: false
Show Trail: false
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
base_link_inertia:
Value: true
ee_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
flange:
Alpha: 1
Show Axes: false
Show Trail: false
forearm_link:
fork0_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
ft_frame:
Alpha: 1
Show Axes: false
Show Trail: false
inner_rgbd_camera:
fork1_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
outer_rgbd_camera:
fork2_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
schunk_grasp_point:
Alpha: 1
Show Axes: false
Show Trail: false
schunk_gripper_base_link:
main0_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
schunk_l_finger_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
schunk_r_finger_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
shoulder_link:
main1_link:
Alpha: 1
Show Axes: false
Show Trail: false
@ -319,30 +212,10 @@ Visualization Manager:
Alpha: 1
Show Axes: false
Show Trail: false
upper_arm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
world:
Alpha: 1
Show Axes: false
Show Trail: false
wrist_1_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wrist_2_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wrist_3_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Loop Animation: false
Name: Trajectory
Robot Alpha: 0.5
@ -368,6 +241,84 @@ Visualization Manager:
Reliability Policy: Reliable
Value: workspace
Value: true
- Class: rviz_default_plugins/InteractiveMarkers
Enable Transparency: true
Enabled: true
Interactive Markers Namespace: /motion_controller_handle
Name: InteractiveMarkers
Show Axes: false
Show Descriptions: true
Show Visual Aids: false
Value: true
- Alpha: 1
Class: rviz_default_plugins/RobotModel
Collision Enabled: false
Description File: ""
Description Source: Topic
Description Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /robot_description
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
ee_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fork0_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fork1_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fork2_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
main0_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
main1_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
tool0:
Alpha: 1
Show Axes: false
Show Trail: false
world:
Alpha: 1
Show Axes: false
Show Trail: false
Mass Properties:
Inertia: false
Mass: false
Name: RobotModel
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
@ -414,7 +365,7 @@ Visualization Manager:
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 6.619869709014893
Distance: 2.528358221054077
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
@ -429,10 +380,10 @@ Visualization Manager:
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.5753980875015259
Pitch: 0.5803980827331543
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 1.1453965902328491
Yaw: 1.0903966426849365
Saved: ~
Window Geometry:
Displays:
@ -440,7 +391,7 @@ Window Geometry:
Height: 1385
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 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
QMainWindow State: 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
Selection:
collapsed: false
Tool Properties:
@ -450,5 +401,5 @@ Window Geometry:
Views:
collapsed: false
Width: 2307
X: 436
Y: 66
X: 507
Y: 315

166
rbs_bringup/launch/bringup.launch.py
View file

@ -1,5 +1,4 @@
import os
from launch import LaunchDescription
from launch import LaunchDescription, condition
from launch.actions import (
DeclareLaunchArgument,
IncludeLaunchDescription,
@ -9,7 +8,6 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
# from rbs_launch_utils.launch_common import load_yaml_abs
def generate_launch_description():
declared_arguments = []
@ -17,50 +15,22 @@ def generate_launch_description():
DeclareLaunchArgument(
"rbs_robot_type",
description="Type of robot by name",
choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
default_value="ur5e",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"safety_limits",
default_value="true",
description="Enables the safety limits controller if true.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"safety_pos_margin",
default_value="0.15",
description="The margin to lower and upper limits in the safety controller.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"safety_k_position",
default_value="20",
description="k-position factor in the safety controller.",
choices=["rbs_arm","ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
default_value="rbs_arm",
)
)
# General arguments
declared_arguments.append(
DeclareLaunchArgument(
"controllers_file",
default_value="ur_plus_gripper_controllers.yaml",
default_value="rbs_arm_controllers_gazebosim.yaml",
description="YAML file with the controllers configuration.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"controllers_with_gripper_file",
default_value="ur_plus_gripper_controllers.yaml",
description="YAML file with the UR + gripper_controller configuration.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"description_package",
default_value="ur_description",
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.",
)
@ -68,7 +38,7 @@ def generate_launch_description():
declared_arguments.append(
DeclareLaunchArgument(
"description_file",
default_value="ur.urdf.xacro",
default_value="rbs_arm_modular.xacro",
description="URDF/XACRO description file with the robot.",
)
)
@ -95,19 +65,10 @@ def generate_launch_description():
description="Robot controller to start.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"initial_gripper_controller",
default_value="gripper_controller",
description="Robot controller to start.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"moveit_config_package",
default_value="ur_moveit_config",
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.",
)
@ -115,7 +76,7 @@ def generate_launch_description():
declared_arguments.append(
DeclareLaunchArgument(
"moveit_config_file",
default_value="ur.srdf.xacro",
default_value="rbs_arm.srdf.xacro",
description="MoveIt SRDF/XACRO description file with the robot.",
)
)
@ -123,48 +84,75 @@ def generate_launch_description():
DeclareLaunchArgument(
"use_sim_time",
default_value="true",
description="Make MoveIt to use simulation time. This is needed for the trajectory planing in simulation.",
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?")
DeclareLaunchArgument("with_gripper",
default_value="true",
description="With gripper or not?")
)
declared_arguments.append(
DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?")
DeclareLaunchArgument("launch_rviz",
default_value="true",
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("sim_mujoco", default_value="false", description="Mujoco Simulation")
DeclareLaunchArgument("env_manager",
default_value="true",
description="Launch env_manager?")
)
declared_arguments.append(
DeclareLaunchArgument("sim_fake", default_value="false", description="Mock contollers")
DeclareLaunchArgument("launch_sim",
default_value="true",
description="Launch simulator (Gazebo)?\
Most general arg")
)
declared_arguments.append(
DeclareLaunchArgument("env_manager", default_value="true", description="Launch env_manager?")
DeclareLaunchArgument("launch_moveit",
default_value="true",
description="Launch moveit?")
)
declared_arguments.append(
DeclareLaunchArgument("launch_sim", default_value="true", description="Launch simulator (Gazebo)? Most general arg")
DeclareLaunchArgument("launch_perception",
default_value="false",
description="Launch perception?")
)
declared_arguments.append(
DeclareLaunchArgument("launch_moveit", default_value="true", description="Launch moveit?")
DeclareLaunchArgument("launch_task_planner",
default_value="false",
description="Launch task_planner?")
)
declared_arguments.append(
DeclareLaunchArgument("launch_perception", default_value="false", description="Launch perception?")
DeclareLaunchArgument("cartesian_controllers",
default_value="false",
description="Load cartesian\
controllers?")
)
declared_arguments.append(
DeclareLaunchArgument("launch_task_planner", default_value="false", description="Launch task_planner?")
DeclareLaunchArgument("hardware",
choices=["gazebo", "mock"],
default_value="gazebo",
description="Choose your harware_interface")
)
declared_arguments.append(
DeclareLaunchArgument("cartesian_controllers", default_value="false", description="Load cartesian controllers?")
DeclareLaunchArgument("launch_controllers",
default_value="true",
description="Launch controllers?")
)
declared_arguments.append(
DeclareLaunchArgument("gazebo_gui",
default_value="false",
description="Launch gazebo with gui?")
)
# Initialize Arguments
rbs_robot_type = LaunchConfiguration("rbs_robot_type")
safety_limits = LaunchConfiguration("safety_limits")
safety_pos_margin = LaunchConfiguration("safety_pos_margin")
safety_k_position = LaunchConfiguration("safety_k_position")
# General arguments
with_gripper_condition = LaunchConfiguration("with_gripper")
controllers_file = LaunchConfiguration("controllers_file")
@ -183,10 +171,10 @@ def generate_launch_description():
moveit_config_file = LaunchConfiguration("moveit_config_file")
use_sim_time = LaunchConfiguration("use_sim_time")
sim_gazebo = LaunchConfiguration("sim_gazebo")
sim_mujoco = LaunchConfiguration("sim_mujoco")
sim_fake = LaunchConfiguration("sim_fake")
hardware = LaunchConfiguration("hardware")
env_manager = LaunchConfiguration("env_manager")
launch_controllers = LaunchConfiguration("launch_controllers")
gazebo_gui = LaunchConfiguration("gazebo_gui")
initial_joint_controllers_file_path = PathJoinSubstitution(
[FindPackageShare(description_package), "config", controllers_file]
@ -195,44 +183,36 @@ def generate_launch_description():
rviz_config_file = PathJoinSubstitution(
[FindPackageShare("rbs_bringup"), "config", "rbs.rviz"]
)
robot_description_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare(description_package), "urdf", description_file]
),
PathJoinSubstitution([FindPackageShare(description_package), "urdf", description_file]),
" ",
"safety_limits:=", safety_limits, " ",
"safety_pos_margin:=", safety_pos_margin, " ",
"safety_k_position:=", safety_k_position, " ",
"name:=", "ur", " ",
"ur_type:=", rbs_robot_type, " ",
"gripper_name:=", "", " ",
"prefix:=", prefix, " ",
"sim_mujoco:=", sim_mujoco, " ",
"sim_gazebo:=", sim_gazebo, " ",
"sim_fake:=", sim_fake, " ",
"hardware:=", hardware, " ",
"simulation_controllers:=", initial_joint_controllers_file_path, " ",
"with_gripper:=", with_gripper_condition, " ",
]
)
robot_description = {"robot_description": robot_description_content}
# MoveIt Configuration
robot_description_semantic_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare(moveit_config_package), "srdf", moveit_config_file]
), " ",
"name:=", "ur", " ",
"prefix:=", prefix, " ",
"with_gripper:=", with_gripper_condition
[FindPackageShare(moveit_config_package), "config/moveit", "rbs_arm.srdf.xacro"]
),
" ",
"name:=","rbs_arm"," ",
"prefix:=",prefix," ",
]
)
robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content}
robot_description_kinematics = PathJoinSubstitution(
[FindPackageShare(moveit_config_package), "config", "kinematics.yaml"]
@ -260,7 +240,7 @@ def generate_launch_description():
control = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare('ur_description'),
FindPackageShare('rbs_arm'),
'launch',
'control.launch.py'
])
@ -272,7 +252,8 @@ def generate_launch_description():
'initial_joint_controller': initial_joint_controller,
'controllers_file': controllers_file,
"cartesian_controllers": cartesian_controllers
}.items())
}.items(),
condition=IfCondition(launch_controllers))
simulation = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
@ -284,15 +265,17 @@ def generate_launch_description():
]),
launch_arguments={
'sim_gazebo': sim_gazebo,
'gazebo_gui': gazebo_gui,
'rbs_robot_type': rbs_robot_type,
'env_manager': env_manager
'env_manager': env_manager,
'debugger': "false"
}.items(),
condition=IfCondition(launch_simulation))
moveit = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare('ur_moveit_config'),
FindPackageShare('rbs_arm'),
'launch',
'moveit.launch.py'
])
@ -322,7 +305,6 @@ def generate_launch_description():
'robot_description_kinematics': robot_description_kinematics,
'use_sim_time': use_sim_time,
'with_gripper_condition': with_gripper_condition,
'points_params_filepath': "gripperPositions.yaml"
}.items()
)
@ -363,7 +345,3 @@ def generate_launch_description():
perception
]
return LaunchDescription(declared_arguments + nodes_to_start)

19
rbs_bt_executor/bt_trees/example_skills/env_manager_starter.xml Normal file
View file

@ -0,0 +1,19 @@
<?xml version="1.0" encoding="UTF-8"?>
<root main_tree_to_execute="Main">
<Action ID="EnvStarter"
env_class="gz_enviroment::GzEnviroment"
env_name="gz_enviroment"
server_name="/env_manager/start_env"
server_timeout="1000"
workspace="{workspace}" />
<TreeNodesModel>
<Action ID="EnvStarter">
<input_port name="env_class" />
<input_port name="env_name" />
<input_port name="server_name" />
<input_port name="server_timeout" />
<output_port name="workspace" />
</Action>
</TreeNodesModel>
</root>

23
rbs_bt_executor/bt_trees/example_skills/move_to_joint_states.xml Normal file
View file

@ -0,0 +1,23 @@
<?xml version="1.0" encoding="UTF-8"?>
<root main_tree_to_execute="Main">
<Action ID="MoveToPose"
robot_name="{robot_name}"
JointState="{joint_state}"
server_name="{action_server}"
server_timeout="{time_s}"
cancel_timeout="{time_c}" />
<TreeNodesModel>
<Action ID="MoveToPose">
<!-- Массив положений суставов робота-->
<!-- std::vector<double>-->
<input_port name="JointState" />
<!-- std::string название робота. Нужен для MoveIt2 -->
<input_port name="robot_name" />
<!-- Для ros2_action-->
<input_port name="cancel_timeout" />
<input_port name="server_name" />
<input_port name="server_timeout" />
</Action>
</TreeNodesModel>
</root>

23
rbs_bt_executor/bt_trees/example_skills/move_to_pose.xml Normal file
View file

@ -0,0 +1,23 @@
<?xml version="1.0" encoding="UTF-8"?>
<root main_tree_to_execute="Main">
<Action ID="MoveToPose"
robot_name="{robot_name}"
pose="{pose}"
server_name="{action_server}"
server_timeout="{time_s}"
cancel_timeout="{time_c}" />
<TreeNodesModel>
<Action ID="MoveToPose">
<!-- Непосредственно сама позиция в формате ROS2 сообщения-->
<!-- geometry_msgs::msg::Pose-->
<input_port name="pose" />
<!-- std::string название робота. Нужен для MoveIt2 -->
<input_port name="robot_name" />
<!-- Для ros2_action-->
<input_port name="cancel_timeout" />
<input_port name="server_name" />
<input_port name="server_timeout" />
</Action>
</TreeNodesModel>
</root>

26
rbs_bt_executor/bt_trees/example_skills/move_to_pose_array.xml Normal file
View file

@ -0,0 +1,26 @@
<?xml version="1.0" encoding="UTF-8"?>
<root main_tree_to_execute="Main">
<Action ID="MoveToPoseArray"
cancel_timeout="5000"
pose_vec_in="{goal_pose}"
pose_vec_out="{goal_pose}"
robot_name="{robot_name}"
server_name="{server_name}"
server_timeout="5000" />
<TreeNodesModel>
<Action ID="MoveToPose">
<!-- Непосредственно сама позиция в формате ROS2 сообщения-->
<!-- geometry_msgs::msgs::PoseArray-->
<input_port name="pose_vec_in" />
<!-- Выдает вычтенную первую позицию из массива-->
<!-- geometry_msgs::msgs::PoseArray-->
<output_port name="pose_vec_out" />
<!-- std::string название робота. Нужен для MoveIt2 -->
<input_port name="robot_name" />
<!-- Для ros2_action-->
<input_port name="cancel_timeout" />
<input_port name="server_name" />
<input_port name="server_timeout" />
</Action>
</TreeNodesModel>
</root>

58
rbs_bt_executor/bt_trees/nodes_interfaces/general.xml Normal file
View file

@ -0,0 +1,58 @@
<root>
<TreeNodesModel>
<Action ID="EnvStarter">
<input_port name="env_class" />
<input_port name="env_name" />
<input_port name="server_name" />
<input_port name="server_timeout" />
<output_port name="workspace" />
</Action>
<Action ID="MoveToJointState">
<!-- Массив положений суставов робота, целевое состояние -->
<!-- std::vector<double>-->
<input_port name="JointState" />
<!-- std::string название робота. Нужен для MoveIt2 -->
<input_port name="robot_name" />
<!-- Параметры ros2_action (скрывать от пользователя по умолчанию) -->
<!-- Время необходимое для выполнения действия, мс -->
<input_port name="cancel_timeout" default="2000" />
<!-- Имя Action server, топик/строка -->
<input_port name="server_name" default="/move_topose" />
<!-- Время, в течение которого Action Client будет ожидать ответа Action Server, мс -->
<input_port name="server_timeout" default="1000" />
</Action>
<Action ID="MoveToPose">
<!-- geometry_msgs::msg::Pose - целевая позиция конечного звена -->
<input_port name="pose" />
<!-- std::string название робота. Нужен для MoveIt2 -->
<input_port name="robot_name" />
<!-- Время необходимое для выполнения действия, мс -->
<input_port name="cancel_timeout" default="2000" />
<!-- Имя Action server, топик/строка -->
<input_port name="server_name" default="/move_topose" />
<!-- Время, в течение которого Action Client будет ожидать ответа Action Server, мс -->
<input_port name="server_timeout" default="1000" />
</Action>
<Action ID="MoveToPoseArray">
<!-- Непосредственно сама позиция в формате ROS2 сообщения-->
<!-- geometry_msgs::msgs::PoseArray-->
<input_port name="pose_vec_in" />
<!-- Выдает вычтенную первую позицию из массива-->
<!-- geometry_msgs::msgs::PoseArray-->
<output_port name="pose_vec_out" />
<!-- std::string название робота. Нужен для MoveIt2 -->
<input_port name="robot_name" />
<!-- Для ros2_action-->
<!-- Время необходимое для выполнения действия, мс -->
<input_port name="cancel_timeout" default="2000" />
<!-- Имя Action server, топик/строка -->
<input_port name="server_name" default="/move_topose" />
<!-- Время, в течение которого Action Client будет ожидать ответа Action Server, мс -->
<input_port name="server_timeout" default="1000" />
</Action>
</TreeNodesModel>
</root>

32
rbs_bt_executor/bt_trees/test_tree.xml
View file

@ -6,7 +6,7 @@
<Action ID="EnvStarter" env_class="gz_enviroment::GzEnviroment" env_name="gz_enviroment"
server_name="/env_manager/start_env" server_timeout="1000" workspace="{workspace}" />
<SubTreePlus ID="WorkspaceInspection" __autoremap="1" goal_pose="{workspace}"
robot_name="ur_manipulator" />
robot_name="rbs_arm" />
</Sequence>
</BehaviorTree>
<!-- ////////// -->
@ -24,33 +24,13 @@
<Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
pose_vec_out="{goal_pose}"
robot_name="{robot_name}" server_name="/move_cartesian" server_timeout="5000" />
<Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
pose_vec_out="{goal_pose}"
robot_name="{robot_name}" server_name="/move_cartesian" server_timeout="5000" />
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Action ID="EnvStarter">
<inout_port name="env_class" />
<inout_port name="env_name" />
<inout_port name="server_name" />
<inout_port name="server_timeout" />
<inout_port name="workspace" />
</Action>
<Action ID="MoveToPose">
<inout_port name="cancel_timeout" />
<inout_port name="pose" />
<inout_port name="robot_name" />
<inout_port name="server_name" />
<inout_port name="server_timeout" />
</Action>
<Action ID="MoveToPoseArray">
<inout_port name="cancel_timeout" />
<inout_port name="pose_vec_in" />
<inout_port name="robot_name" />
<inout_port name="server_name" />
<inout_port name="server_timeout" />
</Action>
<SubTree ID="PoseEstimation" />
<SubTree ID="WorkspaceInspection" />
</TreeNodesModel>
<include path="./nodes_interfaces/general.xml"/>
<!-- ////////// -->
</root>

12
rbs_bt_executor/src/MoveToPoseArray.cpp
View file

@ -30,7 +30,8 @@ public:
goal.target_pose = target_pose_vec_.poses.at(0);
target_pose_vec_.poses.erase(target_pose_vec_.poses.begin());
setOutput<geometry_msgs::msg::PoseArray>("pose_vec_out", target_pose_vec_);
setOutput<geometry_msgs::msg::PoseArray>("pose_vec_out",
target_pose_vec_);
} else {
RCLCPP_WARN(_node->get_logger(), "Target pose vector empty");
}
@ -39,11 +40,10 @@ public:
}
static BT::PortsList providedPorts() {
return providedBasicPorts({
BT::InputPort<std::string>("robot_name"),
BT::InputPort<geometry_msgs::msg::PoseArray>("pose_vec_in"),
BT::OutputPort<geometry_msgs::msg::PoseArray>("pose_vec_out")
});
return providedBasicPorts(
{BT::InputPort<std::string>("robot_name"),
BT::InputPort<geometry_msgs::msg::PoseArray>("pose_vec_in"),
BT::OutputPort<geometry_msgs::msg::PoseArray>("pose_vec_out")});
}
private:

79
rbs_simulation/launch/simulation.launch.py
View file

@ -12,27 +12,38 @@ from ament_index_python.packages import get_package_share_directory
def generate_launch_description():
declared_arguments = []
declared_arguments.append(
DeclareLaunchArgument("sim_gazebo", default_value="false", description="Gazebo Simulation")
DeclareLaunchArgument("sim_gazebo",
default_value="false",
description="Gazebo Simulation")
)
declared_arguments.append(
DeclareLaunchArgument(
"rbs_robot_type",
description="Type of robot by name",
choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
default_value="ur5e",
)
DeclareLaunchArgument("rbs_robot_type",
description="Type of robot by name",
choices=["rbs_arm" ,"ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
default_value="rbs_arm",)
)
declared_arguments.append(
DeclareLaunchArgument("env_manager", default_value="false", description="Launch env_manager?")
DeclareLaunchArgument("env_manager",
default_value="false",
description="Launch env_manager?")
)
declared_arguments.append(
DeclareLaunchArgument("gazebo_gui", default_value="false", description="Launch env_manager?")
DeclareLaunchArgument("gazebo_gui",
default_value="true",
description="Launch env_manager?")
)
declared_arguments.append(
DeclareLaunchArgument("debugger",
default_value="false",
description="launch Gazebo with debugger?")
)
sim_gazebo = LaunchConfiguration("sim_gazebo")
rbs_robot_type = LaunchConfiguration("rbs_robot_type")
env_manager_cond = LaunchConfiguration("env_manager")
gazebo_gui = LaunchConfiguration("gazebo_gui")
debugger = LaunchConfiguration("debugger")
# FIXME: To args when we'll have different files
# TODO: Use global variable to find world file in robossembler_db
world_config_file = PathJoinSubstitution(
@ -44,31 +55,38 @@ def generate_launch_description():
PythonLaunchDescriptionSource(
[os.path.join(get_package_share_directory('ros_gz_sim'),
'launch', 'gz_sim.launch.py')]),
launch_arguments=[('ign_args', [' -r ',world_config_file, " -s"])],
launch_arguments={
'gz_args': [' -r ',world_config_file, " -s"],
"debugger": debugger,
}.items(),
condition=UnlessCondition(gazebo_gui))
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[os.path.join(get_package_share_directory('ros_gz_sim'),
'launch', 'gz_sim.launch.py')]),
launch_arguments=[('ign_args', [' -r ',world_config_file])],
launch_arguments={
'gz_args': [' -r ',world_config_file],
"debugger": debugger,
}.items(),
condition=IfCondition(gazebo_gui))
# Spawn robot
gazebo_spawn_robot = Node(package='ros_ign_gazebo', executable='create',
arguments=[
'-name', rbs_robot_type,
'-x', '0.0',
'-z', '0.0',
'-y', '0.0',
'-topic', '/robot_description'],
output='screen',
condition=IfCondition(sim_gazebo))
gazebo_spawn_robot = Node(
package='ros_gz_sim',
executable='create',
arguments=[
'-name', rbs_robot_type,
'-x', '0.0',
'-z', '0.0',
'-y', '0.0',
'-topic', '/robot_description'],
output='screen',
condition=IfCondition(sim_gazebo))
env_manager = Node(package="env_manager",
executable="run_env_manager",
condition=IfCondition(env_manager_cond)
)
condition=IfCondition(env_manager_cond))
# Bridge
rgbd_bridge_out = Node(
package='ros_gz_bridge',
@ -80,8 +98,7 @@ def generate_launch_description():
'/outer_rgbd_camera/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked'
],
output='screen',
condition=IfCondition(sim_gazebo)
)
condition=IfCondition(sim_gazebo))
rgbd_bridge_in = Node(
package='ros_gz_bridge',
@ -93,9 +110,14 @@ def generate_launch_description():
'/inner_rgbd_camera/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked'
],
output='screen',
)
condition=IfCondition(sim_gazebo))
clock_bridge = Node(
package='ros_gz_bridge',
executable='parameter_bridge',
arguments=['/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock'],
output='screen',
condition=IfCondition(sim_gazebo))
nodes_to_start = [
gazebo,
@ -103,6 +125,7 @@ def generate_launch_description():
gazebo_spawn_robot,
env_manager,
rgbd_bridge_out,
rgbd_bridge_in
rgbd_bridge_in,
clock_bridge,
]
return LaunchDescription(declared_arguments + nodes_to_start)

5
rbs_simulation/mujoco_model/current_mj.xml
View file

@ -2,6 +2,9 @@
<compiler angle="radian" meshdir="meshes/"/>
<default/>
<asset>
<texture type="skybox" builtin="gradient" rgb1="0.9 0.9 0.9" rgb2="0.5 0.5 0.5" width="512" height="512"/>
<texture name="texplane" type="2d" builtin="checker" rgb1=".25 .25 .25" rgb2=".3 .3 .3" width="512" height="512" mark="cross" markrgb=".8 .8 .8"/>
<material name="matplane" reflectance="0.2" texture="texplane" texrepeat="1 1" texuniform="true"/>
<mesh name="base" file="base.stl"/>
<mesh name="shoulder" file="shoulder.stl"/>
<mesh name="upperarm" file="upperarm.stl"/>
@ -11,6 +14,8 @@
<mesh name="wrist3" file="wrist3.stl"/>
</asset>
<worldbody>
<light directional="true" diffuse=".8 .8 .8" specular=".2 .2 .2" pos="0 0 5" dir="0 0 -1" castshadow="false"/>
<geom name="floor" pos="0 0 -0.0" size="0 0 1" type="plane" material="matplane"/>
<geom quat="-1 0 0 0" type="mesh" contype="0" conaffinity="0" group="1" density="0" mesh="base"/>
<geom quat="-1 0 0 0" type="mesh" mesh="base"/>
<body name="shoulder_link" pos="0 0 0.1625" quat="0 0 0 1" >

3
rbs_simulation/worlds/mir.sdf
View file

@ -12,7 +12,8 @@
<plugin name='ignition::gazebo::systems::Contact' filename='ignition-gazebo-contact-system'/>
<plugin name="ignition::gazebo::systems::Sensors" filename="ignition-gazebo-sensors-system">
<render_engine>ogre2</render_engine>
</plugin>
</plugin>
<plugin name='ignition::gazebo::systems::ForceTorque' filename='ignition-gazebo-forcetorque-system'/>
<gravity>0 0 -9.8</gravity>
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
<atmosphere type='adiabatic'/>

8
rbs_utils/src/rbs_utils.cpp
View file

@ -349,10 +349,10 @@ AssemblyConfigLoader::getWorkspaceInspectorTrajectory() {
if (json.contains("workspace")) {
auto workspace = readWorkspaceJson(json);
for (auto &point : workspace.transforms) {
geometry_msgs::msg::Pose pose;
pose = transformTrajectory(point);
auto pose = transformTrajectory(point);
pose_array.poses.push_back(pose);
}
pose_array.poses.push_back(transformTrajectory(workspace.transforms[0]));
}
}
return pose_array;
@ -361,9 +361,9 @@ AssemblyConfigLoader::getWorkspaceInspectorTrajectory() {
geometry_msgs::msg::Pose AssemblyConfigLoader::transformTrajectory(
const geometry_msgs::msg::TransformStamped &pose) {
auto pose_eigen = tf2::transformToEigen(pose.transform);
Eigen::AngleAxisd rotation(M_PI, Eigen::Vector3d::UnitX());
Eigen::AngleAxisd rotation(M_PI, Eigen::Vector3d::UnitY());
pose_eigen.rotate(rotation);
pose_eigen.translation().z() += 0.5;
pose_eigen.translation().z() += 0.35;
auto pose_msg = tf2::toMsg(pose_eigen);
return pose_msg;
}

13
repos/cartesian_controllers.repos Normal file
View file

@ -0,0 +1,13 @@
repositories:
cartesian-controllers:
type: git
url: https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers.git
version: ros2
ros2_control:
type: git
url: https://github.com/solid-sinusoid/gz_ros2_control.git
version: gz-ros2-cartesian-controllers
gz_ros2_control:
type: git
url: https://github.com/solid-sinusoid/gz_ros2_control.git
version: pass-robot-description

4
rbs.real.repos → repos/real.repos
View file

@ -15,7 +15,3 @@ repositories:
type: git
url: https://gitlab.com/robossembler/arm-tools/urdf-model-shrunk-gripper-egp-40-n-n-b.git
version: 2-add-ros2-control
gz_ros2_control:
type: git
url: https://github.com/solid-sinusoid/gz_ros2_control.git
version: ft-sensor-broadcaster

13
repos/sim.rbs.repos Normal file
View file

@ -0,0 +1,13 @@
repositories:
rbs_arm:
type: git
url: https://github.com/solid-sinusoid/rbs-arm.git
version: main
rbs_gripper:
type: git
url: https://github.com/solid-sinusoid/rbs-gripper.git
version: main
behavior_tree:
type: git
url: https://github.com/solid-sinusoid/behavior_tree.git
version: master

4
rbs.sim.repos → repos/sim.ur.repos
View file

@ -15,7 +15,3 @@ repositories:
type: git
url: https://gitlab.com/robossembler/arm-tools/urdf-model-shrunk-gripper-egp-40-n-n-b.git
version: 2-add-ros2-control
gz_ros2_control:
type: git
url: https://github.com/solid-sinusoid/gz_ros2_control.git
version: ft-sensor-broadcaster