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

Merge branch 'main-update' into 101-pose_estimation_DOPE

Browse source
This commit is contained in:
shalenikol 2024-05-13 12:26:39 +03:00
commit e4c3c0f1c7
58 changed files with 5387 additions and 905 deletions
Download patch file Download diff file Expand all files Collapse all files

9
.gitlab-ci.yml
View file

@ -13,15 +13,6 @@ build-colcon-job:
stage: build
script:
- apt-get update
- apt-get install wget
- wget https://github.com/nlohmann/json/archive/refs/tags/v3.11.3.tar.gz
- tar -xf v3.11.3.tar.gz
- cd json-3.11.3
- mkdir build
- cd build
- cmake ..
- make install
- cd ../..
- mkdir -p .src/robossembler-ros2
- mv * .src/robossembler-ros2
- mv .git .src/robossembler-ros2

2
README.md
View file

@ -6,7 +6,6 @@ Repo for ROS2 packages related to Robossembler
### Requirements
* OS: Ubuntu 22.04
* ROS 2 Humble
* nlohman/json
### Dependencies
These are the primary dependencies required to use this project.
@ -14,7 +13,6 @@ These are the primary dependencies required to use this project.
* MoveIt 2
> Install/build a version based on the selected ROS 2 release
* Gazebo Fortress
* [nlohman/json](https://github.com/nlohmann/json/releases/tag/v3.11.3)
### Build

2661
doc/Doxyfile Normal file
View file

File diff suppressed because it is too large Load diff

2
env_manager/env_manager/include/env_manager/env_manager.hpp
View file

@ -97,4 +97,4 @@ private:
std::shared_ptr<pluginlib::ClassLoader<env_interface::EnvInterface>> m_loader;
};
} // namespace env_manager
#endif // __ENV_MANAGER_HPP__
#endif // __ENV_MANAGER_HPP__

1
env_manager/env_manager/src/env_manager.cpp
View file

@ -1,5 +1,4 @@
#include "env_manager/env_manager.hpp"
#include "nlohmann/json.hpp"
#include <string>
static constexpr const char *ENV_INTERFACE_BASE_CLASS_NAMESPACE =

6
env_manager/gz_enviroment/CMakeLists.txt
View file

@ -67,6 +67,12 @@ install(TARGETS ${PROJECT_NAME}
LIBRARY DESTINATION lib
)
install(PROGRAMS
scripts/publish_asm_config.py
scripts/test_tf.py
DESTINATION lib/${PROJECT_NAME}
)
ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
# ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
ament_package()

19
env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp
View file

@ -2,12 +2,13 @@
#include "rclcpp_lifecycle/lifecycle_node.hpp"
// #include "ros_gz_bridge/convert.hpp"
#include "env_interface/env_interface.hpp"
#include "geometry_msgs/msg/pose_array.hpp"
#include "gz/msgs/pose_v.pb.h"
#include <geometry_msgs/msg/transform_stamped.hpp>
#include "geometry_msgs/msg/pose_array.hpp"
#include <gz/transport/Node.hh>
#include <memory>
#include <mutex>
#include <rclcpp/timer.hpp>
#include <string>
#include <tf2_msgs/msg/detail/tf_message__struct.hpp>
#include <tf2_msgs/msg/tf_message.hpp>
@ -16,6 +17,7 @@
#include <gz/sim/Entity.hh>
#include <gz/sim/EntityComponentManager.hh>
#include <gz/sim/components/Model.hh>
#include <thread>
namespace gz_enviroment {
using CallbackReturn =
@ -33,11 +35,10 @@ public:
protected:
void onGzPoseSub(const gz::msgs::Pose_V &pose_v);
bool doGzSpawn();
// bool doGzSpawn();
private:
std::unique_ptr<tf2_ros::TransformBroadcaster> m_tf2_broadcaster;
std::unique_ptr<tf2_ros::TransformBroadcaster> m_tf2_broadcaster_target;
std::shared_ptr<gz::transport::Node> m_gz_node;
std::vector<std::string> m_follow_frames;
std::string m_topic_name;
@ -46,6 +47,9 @@ private:
std::shared_ptr<rbs_utils::AssemblyConfigLoader> m_config_loader;
tf2_msgs::msg::TFMessage m_transforms;
tf2_msgs::msg::TFMessage m_target_places;
rclcpp::TimerBase::SharedPtr m_timer;
std::vector<geometry_msgs::msg::TransformStamped> m_all_transforms{};
std::string getGzWorldName();
std::string createGzModelString(const std::vector<double> &pose,
@ -53,5 +57,14 @@ private:
const double &mass,
const std::string &mesh_filepath,
const std::string &name);
void broadcast_timer_callback() {
if (!m_transforms.transforms.empty()) {
for (auto &transform : m_transforms.transforms) {
m_tf2_broadcaster->sendTransform(std::move(transform));
}
// m_transforms.transforms.clear();
}
}
};
} // namespace gz_enviroment

114
env_manager/gz_enviroment/scripts/publish_asm_config.py Executable file
View file

@ -0,0 +1,114 @@
#! /usr/bin/env python3
import yaml
import rclpy
from rclpy.node import Node
from rbs_utils_interfaces.msg import AssemblyConfig, NamedPose, RelativeNamedPose
from geometry_msgs.msg import Point
import os
class ConfigPublisherNode(Node):
def __init__(self):
super().__init__('config_publisher_node')
self.publisher_ = self.create_publisher(AssemblyConfig, '/assembly_config', 10)
timer_period = 1 # seconds
self.timer = self.create_timer(timer_period, self.timer_callback)
if 'RBS_ASSEMBLY_DIR' in os.environ:
assembly_dir = os.environ['RBS_ASSEMBLY_DIR']
else:
assembly_dir = '~/assembly/robossembler-stuff'
self.assembly_dir = os.path.expanduser(assembly_dir)
datafolder = "asp-example"
# Read data from YAML file
with open(f"{self.assembly_dir}/{datafolder}/rbs_db.yaml") as f:
self.data = yaml.safe_load(f)
def timer_callback(self):
# Create AssemblyConfig message
msg = AssemblyConfig()
# Fill in the data from the YAML file
msg.assets_db = self.data['assets_db']
# Populate workspace
for wp in self.data['workspace']:
point = Point()
point.x = wp["x"]
point.y = wp["y"]
point.z = wp["z"]
msg.workspace.append(point)
# Populate absolute_part
for part in self.data['absolute_part']:
named_pose = NamedPose()
named_pose.name = part['name']
pose = part['pose']
named_pose.pose.position.x = pose['position']['x']
named_pose.pose.position.y = pose['position']['y']
named_pose.pose.position.z = pose['position']['z']
named_pose.pose.orientation.x = pose['orientation']['x']
named_pose.pose.orientation.y = pose['orientation']['y']
named_pose.pose.orientation.z = pose['orientation']['z']
named_pose.pose.orientation.w = pose['orientation']['w']
msg.absolute_part.append(named_pose)
# Populate relative_part
for part in self.data['relative_part']:
relative_named_pose = RelativeNamedPose()
relative_named_pose.name = part['name']
relative_named_pose.relative_at = part['relative_at']
pose = part['pose']
relative_named_pose.pose.position.x = pose['position']['x']
relative_named_pose.pose.position.y = pose['position']['y']
relative_named_pose.pose.position.z = pose['position']['z']
relative_named_pose.pose.orientation.x = pose['orientation']['x']
relative_named_pose.pose.orientation.y = pose['orientation']['y']
relative_named_pose.pose.orientation.z = pose['orientation']['z']
relative_named_pose.pose.orientation.w = pose['orientation']['w']
msg.relative_part.append(relative_named_pose)
# Populate grasp_pose
for part in self.data['grasp_pose']:
relative_named_pose = RelativeNamedPose()
relative_named_pose.name = part['name']
relative_named_pose.relative_at = part['relative_at']
pose = part['pose']
relative_named_pose.pose.position.x = pose['position']['x']
relative_named_pose.pose.position.y = pose['position']['y']
relative_named_pose.pose.position.z = pose['position']['z']
relative_named_pose.pose.orientation.x = pose['orientation']['x']
relative_named_pose.pose.orientation.y = pose['orientation']['y']
relative_named_pose.pose.orientation.z = pose['orientation']['z']
relative_named_pose.pose.orientation.w = pose['orientation']['w']
msg.grasp_pose.append(relative_named_pose)
# Populate extra_poses
for part in self.data['extra_poses']:
named_pose = NamedPose()
named_pose.name = part['name']
pose = part['pose']
named_pose.pose.position.x = pose['position']['x']
named_pose.pose.position.y = pose['position']['y']
named_pose.pose.position.z = pose['position']['z']
named_pose.pose.orientation.x = pose['orientation']['x']
named_pose.pose.orientation.y = pose['orientation']['y']
named_pose.pose.orientation.z = pose['orientation']['z']
named_pose.pose.orientation.w = pose['orientation']['w']
msg.extra_poses.append(named_pose)
# Publish the message
self.publisher_.publish(msg)
def main(args=None):
rclpy.init(args=args)
config_publisher_node = ConfigPublisherNode()
rclpy.spin(config_publisher_node)
config_publisher_node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

75
env_manager/gz_enviroment/scripts/test_tf.py Executable file
View file

@ -0,0 +1,75 @@
#! /usr/bin/env python3
import rclpy
from rclpy.node import Node
from tf2_msgs.msg import TFMessage
from geometry_msgs.msg import TransformStamped
from rbs_utils_interfaces.msg import AssemblyConfig
from tf2_ros.transform_broadcaster import TransformBroadcaster
class TF2PublisherNode(Node):
def __init__(self):
super().__init__('tf2_publisher_node')
self.tfb_ = TransformBroadcaster(self)
self.subscription = self.create_subscription(
AssemblyConfig, '/assembly_config', self.config_callback, 10)
def config_callback(self, msg):
# Populate transforms for absolute_part
for part in msg.absolute_part:
ts = TransformStamped()
ts.header.stamp = self.get_clock().now().to_msg()
ts.header.frame_id = "world"
ts.child_frame_id = part.name
ts.transform.translation.x = part.pose.position.x
ts.transform.translation.y = part.pose.position.y
ts.transform.translation.z = part.pose.position.z
ts.transform.rotation.x = part.pose.orientation.x
ts.transform.rotation.y = part.pose.orientation.y
ts.transform.rotation.z = part.pose.orientation.z
ts.transform.rotation.w = part.pose.orientation.w
self.tfb_.sendTransform(ts)
# Populate transforms for relative_part
for part in msg.relative_part:
ts = TransformStamped()
ts.header.stamp = self.get_clock().now().to_msg()
ts.header.frame_id = part.relative_at
ts.child_frame_id = part.name
ts.transform.translation.x = part.pose.position.x
ts.transform.translation.y = part.pose.position.y
ts.transform.translation.z = part.pose.position.z
ts.transform.rotation.x = part.pose.orientation.x
ts.transform.rotation.y = part.pose.orientation.y
ts.transform.rotation.z = part.pose.orientation.z
ts.transform.rotation.w = part.pose.orientation.w
self.tfb_.sendTransform(ts)
# Populate transforms for grasp_pose
for part in msg.grasp_pose:
ts = TransformStamped()
ts.header.stamp = self.get_clock().now().to_msg()
ts.header.frame_id = part.relative_at
ts.child_frame_id = part.name
ts.transform.translation.x = part.pose.position.x
ts.transform.translation.y = part.pose.position.y
ts.transform.translation.z = part.pose.position.z
ts.transform.rotation.x = part.pose.orientation.x
ts.transform.rotation.y = part.pose.orientation.y
ts.transform.rotation.z = part.pose.orientation.z
ts.transform.rotation.w = part.pose.orientation.w
self.tfb_.sendTransform(ts)
def main(args=None):
rclpy.init(args=args)
tf2_publisher_node = TF2PublisherNode()
rclpy.spin(tf2_publisher_node)
tf2_publisher_node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

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

@ -2,8 +2,13 @@
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "ros_gz_bridge/convert.hpp"
#include <algorithm>
#include <chrono>
#include <functional>
#include <iterator>
#include <memory>
#include <rclcpp/logging.hpp>
#include <rclcpp/utilities.hpp>
#include <rclcpp_lifecycle/lifecycle_node.hpp>
#include <ros_gz_interfaces/msg/detail/entity__builder.hpp>
@ -30,11 +35,15 @@ CallbackReturn GzEnviroment::on_init() {
"Model Name: " << modelName->Data());
}
}
// getNode()->declare_parameter("use_sim_time", true);
return CallbackReturn::SUCCESS;
}
CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State &) {
// Configuration of parameters
using namespace std::chrono_literals;
RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_configure");
m_gz_node = std::make_shared<gz::transport::Node>();
m_world_name = getGzWorldName();
@ -42,16 +51,9 @@ CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State &) {
m_service_spawn = std::string("/world/") + m_world_name + "/create";
m_config_loader = std::make_shared<rbs_utils::AssemblyConfigLoader>(
"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");
// TODO add workspace viewer in Rviz
// m_config_loader->printWorkspace();
// if (!doGzSpawn())
// {
// return CallbackReturn::ERROR;
// }
m_follow_frames = m_config_loader->getUniqueSceneModelNames();
m_transforms = m_config_loader->getGraspTfData("bishop");
// m_transforms = m_config_loader->getAllPossibleTfData();
return CallbackReturn::SUCCESS;
}
@ -60,11 +62,10 @@ CallbackReturn GzEnviroment::on_activate(const rclcpp_lifecycle::State &) {
// Setting up the subscribers and publishers
RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_activate");
m_gz_node->Subscribe(m_topic_name, &GzEnviroment::onGzPoseSub, this);
// Initialize tf broadcaster before use in subscriber
m_tf2_broadcaster =
std::make_unique<tf2_ros::TransformBroadcaster>(getNode());
m_tf2_broadcaster_target =
std::make_unique<tf2_ros::TransformBroadcaster>(getNode());
m_gz_node->Subscribe(m_topic_name, &GzEnviroment::onGzPoseSub, this);
return CallbackReturn::SUCCESS;
}
@ -86,7 +87,6 @@ CallbackReturn GzEnviroment::on_cleanup(const rclcpp_lifecycle::State &) {
CallbackReturn GzEnviroment::on_deactivate(const rclcpp_lifecycle::State &) {
RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_deactivate");
// What we should use here?
return CallbackReturn::SUCCESS;
}
@ -120,12 +120,11 @@ void GzEnviroment::onGzPoseSub(const gz::msgs::Pose_V &pose_v) {
}
for (auto &place : m_transforms.transforms) {
place.header.stamp = getNode()->get_clock()->now();
all_transforms.push_back(place);
}
for (auto &transform : all_transforms) {
m_tf2_broadcaster->sendTransform(transform);
}
// m_all_transforms.swap(all_transforms);
}
std::string GzEnviroment::getGzWorldName() {
@ -153,54 +152,54 @@ std::string GzEnviroment::getGzWorldName() {
return worlds_msg.data(0);
}
bool GzEnviroment::doGzSpawn() {
gz::msgs::EntityFactory req;
gz::msgs::Boolean rep;
bool result;
unsigned int timeout = 5000;
bool executed;
// bool GzEnviroment::doGzSpawn() {
// gz::msgs::EntityFactory req;
// gz::msgs::Boolean rep;
// bool result;
// unsigned int timeout = 5000;
// bool executed;
//
// auto env_models = m_config_loader->getEnvModels();
// for (auto &model : *env_models) {
// std::string sdf_string =
// createGzModelString(model.model_pose, model.model_inertia, model.mass,
// model.mesh_path, model.model_name);
// req.set_sdf(sdf_string);
// executed = m_gz_node->Request(m_service_spawn, req, timeout, rep, result);
// }
// return executed;
// }
auto env_models = m_config_loader->getEnvModels();
for (auto &model : *env_models) {
std::string sdf_string =
createGzModelString(model.model_pose, model.model_inertia, model.mass,
model.mesh_path, model.model_name);
req.set_sdf(sdf_string);
executed = m_gz_node->Request(m_service_spawn, req, timeout, rep, result);
}
return executed;
}
std::string GzEnviroment::createGzModelString(
const std::vector<double> &pose, const std::vector<double> &inertia,
const double &mass, const std::string &mesh_filepath,
const std::string &name) {
std::string model_template =
std::string("<sdf version='1.7'>") + "<model name='" + name + "'>" +
"<link name='link_" + name + "'>" + "<static>1</static>" + "<pose>" +
std::to_string(pose[0]) + " " + std::to_string(pose[1]) + " " +
std::to_string(pose[2]) + " " + std::to_string(pose[3]) + " " +
std::to_string(pose[4]) + " " + std::to_string(pose[5]) +
"</pose>"
// + "<inertial>"
// + "<inertia>"
// + "<ixx>" + std::to_string(inertia[0]) + "</ixx>"
// + "<ixy>" + std::to_string(inertia[1]) + "</ixy>"
// + "<ixz>" + std::to_string(inertia[2]) + "</ixz>"
// + "<iyy>" + std::to_string(inertia[3]) + "</iyy>"
// + "<iyz>" + std::to_string(inertia[4]) + "</iyz>"
// + "<izz>" + std::to_string(inertia[5]) + "</izz>"
// + "</inertia>"
// + "<mass>" + std::to_string(mass) + "</mass>"
// + "</inertial>"
+ "<visual name='visual_" + name + "'>" + "<geometry><mesh><uri>file://" +
mesh_filepath + "</uri></mesh></geometry>" + "</visual>" +
"<collision name='collision_" + name + "'>" +
"<geometry><mesh><uri>file://" + mesh_filepath +
"</uri></mesh></geometry>" + "</collision>" + "</link>" + "</model>" +
"</sdf>";
return model_template;
}
// std::string GzEnviroment::createGzModelString(
// const std::vector<double> &pose, const std::vector<double> &inertia,
// const double &mass, const std::string &mesh_filepath,
// const std::string &name) {
// std::string model_template =
// std::string("<sdf version='1.7'>") + "<model name='" + name + "'>" +
// "<link name='link_" + name + "'>" + "<static>1</static>" + "<pose>" +
// std::to_string(pose[0]) + " " + std::to_string(pose[1]) + " " +
// std::to_string(pose[2]) + " " + std::to_string(pose[3]) + " " +
// std::to_string(pose[4]) + " " + std::to_string(pose[5]) +
// "</pose>"
// // + "<inertial>"
// // + "<inertia>"
// // + "<ixx>" + std::to_string(inertia[0]) + "</ixx>"
// // + "<ixy>" + std::to_string(inertia[1]) + "</ixy>"
// // + "<ixz>" + std::to_string(inertia[2]) + "</ixz>"
// // + "<iyy>" + std::to_string(inertia[3]) + "</iyy>"
// // + "<iyz>" + std::to_string(inertia[4]) + "</iyz>"
// // + "<izz>" + std::to_string(inertia[5]) + "</izz>"
// // + "</inertia>"
// // + "<mass>" + std::to_string(mass) + "</mass>"
// // + "</inertial>"
// + "<visual name='visual_" + name + "'>" + "<geometry><mesh><uri>file://" +
// mesh_filepath + "</uri></mesh></geometry>" + "</visual>" +
// "<collision name='collision_" + name + "'>" +
// "<geometry><mesh><uri>file://" + mesh_filepath +
// "</uri></mesh></geometry>" + "</collision>" + "</link>" + "</model>" +
// "</sdf>";
// return model_template;
// }
} // namespace gz_enviroment

2
rbs_bringup/config/rbs.rviz
View file

@ -315,7 +315,7 @@ Visualization Manager:
Inertia: false
Mass: false
Name: RobotModel
TF Prefix: ""
TF tf_prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true

23
rbs_bringup/config/roboclone.yaml Normal file
View file

@ -0,0 +1,23 @@
scene_config:
- name: arm1
type: rbs_arm
pose:
position:
x: -0.45
y: -2.0
z: 1.6
orientation:
x: 3.14159
y: 0.0
z: 0.0
- name: arm2
type: rbs_arm
pose:
position:
x: 0.45
y: -2.0
z: 1.6
orientation:
x: 3.14159
y: 0.0
z: 0.0

57
rbs_bringup/config/robot_scene.yaml Normal file
View file

@ -0,0 +1,57 @@
scene_config:
- name: arm1
type: rbs_arm
tool_type: rbs_gripper
ndof: 6
parent: world
initial_position:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
pose:
position:
x: 0.0
y: 0.0
z: 0.0
orientation:
r: 0.0
p: 0.0
y: 0.0
- name: arm2
type: rbs_arm
tool_type: rbs_gripper
ndof: 6
parent: world
initial_position:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
pose:
position:
x: 0.0
y: 0.5
z: 0.0
orientation:
r: 0.0
p: 0.0
y: 0.0
- name: helper
type: rbs_gripper
parent: rbs_platform
initial_position:
- 0.0
pose:
position:
x: 0.0
y: 0.0
z: 0.0
orientation:
r: 0.0
p: 0.0
y: 0.0

254
rbs_bringup/launch/launch_env.launch.py Normal file
View file

@ -0,0 +1,254 @@
from launch import LaunchDescription
from launch.actions import (
DeclareLaunchArgument,
IncludeLaunchDescription,
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
def launch_setup(context, *args, **kwargs):
# Initialize Arguments
robot_type = LaunchConfiguration("robot_type")
# General arguments
with_gripper_condition = LaunchConfiguration("with_gripper")
controllers_file = LaunchConfiguration("controllers_file")
cartesian_controllers = LaunchConfiguration("cartesian_controllers")
description_package = LaunchConfiguration("description_package")
description_file = LaunchConfiguration("description_file")
robot_name = LaunchConfiguration("robot_name")
start_joint_controller = LaunchConfiguration("start_joint_controller")
initial_joint_controller = LaunchConfiguration("initial_joint_controller")
launch_simulation = LaunchConfiguration("launch_sim")
launch_moveit = LaunchConfiguration("launch_moveit")
launch_task_planner = LaunchConfiguration("launch_task_planner")
launch_perception = LaunchConfiguration("launch_perception")
moveit_config_package = LaunchConfiguration("moveit_config_package")
moveit_config_file = LaunchConfiguration("moveit_config_file")
use_sim_time = LaunchConfiguration("use_sim_time")
sim_gazebo = LaunchConfiguration("sim_gazebo")
hardware = LaunchConfiguration("hardware")
env_manager = LaunchConfiguration("env_manager")
launch_controllers = LaunchConfiguration("launch_controllers")
gazebo_gui = LaunchConfiguration("gazebo_gui")
gripper_name = LaunchConfiguration("gripper_name")
sim_gazebo = LaunchConfiguration("sim_gazebo")
launch_simulation = LaunchConfiguration("launch_sim")
configured_params = RewrittenYaml(
source_file=os.path.join(
get_package_share_directory(
description_package.perform(context)),
"config",
controllers_file.perform(context)),
root_key=robot_name.perform(context),
param_rewrites={},
convert_types=True,
)
initial_joint_controllers_file_path = os.path.join(
get_package_share_directory('rbs_arm'), 'config', 'rbs_arm0_controllers.yaml'
)
controller_paramfile = configured_params.perform(context)
namespace = "/" + robot_name.perform(context)
# spawner = Node(
# package="gz_enviroment_python",
# executable="spawner.py",
# namespace=namespace
# )
single_robot_setup = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare('rbs_bringup'),
"launch",
"rbs_robot.launch.py"
])
]),
launch_arguments={
"env_manager": env_manager,
"with_gripper": with_gripper_condition,
"gripper_name": gripper_name,
"controllers_file": controllers_file,
"robot_type": robot_type,
"controllers_file": initial_joint_controllers_file_path,
"cartesian_controllers": cartesian_controllers,
"description_package": description_package,
"description_file": description_file,
"robot_name": robot_name,
"start_joint_controller": start_joint_controller,
"initial_joint_controller": initial_joint_controller,
"launch_simulation": launch_simulation,
"launch_moveit": launch_moveit,
"launch_task_planner": launch_task_planner,
"launch_perception": launch_perception,
"moveit_config_package": moveit_config_package,
"moveit_config_file": moveit_config_file,
"use_sim_time": use_sim_time,
"sim_gazebo": sim_gazebo,
"hardware": hardware,
"launch_controllers": "true",
"gazebo_gui": gazebo_gui,
}.items()
)
nodes_to_start = [
# spawner,
single_robot_setup
]
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","ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
default_value="rbs_arm",
)
)
# General arguments
declared_arguments.append(
DeclareLaunchArgument(
"controllers_file",
default_value="rbs_arm_controllers_gazebosim.yaml",
description="YAML file with the controllers configuration.",
)
)
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(
"start_joint_controller",
default_value="false",
description="Enable headless mode for robot control",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"initial_joint_controller",
default_value="joint_trajectory_controller",
description="Robot controller to start.",
)
)
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(
"gripper_name",
default_value="rbs_gripper",
choices=["rbs_gripper", ""],
description="choose gripper by name (leave empty if hasn't)",
)
)
declared_arguments.append(
DeclareLaunchArgument("with_gripper",
default_value="true",
description="With gripper or not?")
)
declared_arguments.append(
DeclareLaunchArgument("sim_gazebo",
default_value="true",
description="Gazebo Simulation")
)
declared_arguments.append(
DeclareLaunchArgument("env_manager",
default_value="false",
description="Launch env_manager?")
)
declared_arguments.append(
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?")
)
declared_arguments.append(
DeclareLaunchArgument("launch_perception",
default_value="false",
description="Launch perception?")
)
declared_arguments.append(
DeclareLaunchArgument("launch_task_planner",
default_value="false",
description="Launch task_planner?")
)
declared_arguments.append(
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")
)
declared_arguments.append(
DeclareLaunchArgument("launch_controllers",
default_value="true",
description="Launch controllers?")
)
declared_arguments.append(
DeclareLaunchArgument("gazebo_gui",
default_value="true",
description="Launch gazebo with gui?")
)
return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])

292
rbs_bringup/launch/multi_robot.launch.py Normal file
View file

@ -0,0 +1,292 @@
from launch import LaunchDescription
from launch.actions import (
DeclareLaunchArgument,
IncludeLaunchDescription,
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
from nav2_common.launch import RewrittenYaml
import os
from ament_index_python import get_package_share_directory
from rbs_launch_utils.merged_yaml import MergedYaml
from rbs_launch_utils.launch_common import load_yaml
import yaml
def launch_setup(context, *args, **kwargs):
# Initialize Arguments
robot_type = LaunchConfiguration("robot_type")
# General arguments
with_gripper_condition = LaunchConfiguration("with_gripper")
controllers_file = LaunchConfiguration("controllers_file")
cartesian_controllers = LaunchConfiguration("cartesian_controllers")
description_package = LaunchConfiguration("description_package")
description_file = LaunchConfiguration("description_file")
robot_name = LaunchConfiguration("robot_name")
start_joint_controller = LaunchConfiguration("start_joint_controller")
initial_joint_controller = LaunchConfiguration("initial_joint_controller")
launch_simulation = LaunchConfiguration("launch_sim")
launch_moveit = LaunchConfiguration("launch_moveit")
launch_task_planner = LaunchConfiguration("launch_task_planner")
launch_perception = LaunchConfiguration("launch_perception")
moveit_config_package = LaunchConfiguration("moveit_config_package")
moveit_config_file = LaunchConfiguration("moveit_config_file")
use_sim_time = LaunchConfiguration("use_sim_time")
sim_gazebo = LaunchConfiguration("sim_gazebo")
hardware = LaunchConfiguration("hardware")
env_manager = LaunchConfiguration("env_manager")
launch_controllers = LaunchConfiguration("launch_controllers")
gazebo_gui = LaunchConfiguration("gazebo_gui")
gripper_name = LaunchConfiguration("gripper_name")
robots_config_file = LaunchConfiguration("robots_config_file")
gazebo_world_filename = LaunchConfiguration("gazebo_world_filename")
ld = []
ld.append(IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare('rbs_simulation'),
'launch',
'simulation_gazebo.launch.py'
])
]),
launch_arguments={
'sim_gazebo': sim_gazebo,
'debugger': "false",
'launch_env_manager': "false",
"gazebo_world_filename": gazebo_world_filename
}.items(),
condition=IfCondition(launch_simulation)
))
scene_file = robots_config_file.perform(context)
robots = load_yaml("rbs_bringup", "config/" + scene_file)
description_package = description_package.perform(context)
controllers_file = controllers_file.perform(context)
config = MergedYaml(context,
os.path.join(get_package_share_directory(description_package), "config", controllers_file),
root_keys=[i["name"] for i in robots["scene_config"]],
param_rewrites={},
convert_types=False,
).merge_yamls()
for robot in robots["scene_config"]:
namespace: str = "/" + robot["name"]
ld.append(IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare('rbs_bringup'),
"launch",
"rbs_robot.launch.py"
])
]),
launch_arguments={
"env_manager": env_manager,
"with_gripper": with_gripper_condition,
"gripper_name": gripper_name,
"controllers_file": controllers_file,
"robot_type": robot["type"],
"controllers_file": config,
"cartesian_controllers": cartesian_controllers,
"description_package": description_package,
"description_file": description_file,
"robot_name": robot["name"],
"start_joint_controller": start_joint_controller,
"initial_joint_controller": initial_joint_controller,
"launch_simulation": launch_simulation,
"launch_moveit": launch_moveit,
"launch_task_planner": launch_task_planner,
"launch_perception": launch_perception,
"moveit_config_package": moveit_config_package,
"moveit_config_file": moveit_config_file,
"use_sim_time": use_sim_time,
"sim_gazebo": sim_gazebo,
"hardware": hardware,
"launch_controllers": launch_controllers,
"gazebo_gui": gazebo_gui,
"namespace": namespace,
"x": str(robot["pose"]["position"]["x"]),
"y": str(robot["pose"]["position"]["y"]),
"z": str(robot["pose"]["position"]["z"]),
"roll": str(robot["pose"]["orientation"]["x"]),
"pitch": str(robot["pose"]["orientation"]["y"]),
"yaw": str(robot["pose"]["orientation"]["z"]),
}.items()
))
gz_spawner = Node(
package='ros_gz_sim',
executable='create',
arguments=[
'-name', robot_name,
# '-x', str(robot["pose"]["position"]["x"]),
# '-y', str(robot["pose"]["position"]["y"]),
# '-z', str(robot["pose"]["position"]["z"]),
# '-R', str(robot["pose"]["orientation"]["x"]),
# '-P', str(robot["pose"]["orientation"]["y"]),
# '-Y', str(robot["pose"]["orientation"]["z"]),
'-topic', namespace + '/robot_description'],
output='screen',
condition=IfCondition(sim_gazebo))
ld.append(gz_spawner)
return ld
def generate_launch_description():
declared_arguments = []
declared_arguments.append(
DeclareLaunchArgument(
"robot_type",
description="Type of robot by name",
choices=["rbs_arm","ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
default_value="rbs_arm",
)
)
# General arguments
declared_arguments.append(
DeclareLaunchArgument(
"controllers_file",
default_value="rbs_arm_controllers_gazebosim.yaml",
description="YAML file with the controllers configuration.",
)
)
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(
"start_joint_controller",
default_value="false",
description="Enable headless mode for robot control",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"initial_joint_controller",
default_value="joint_trajectory_controller",
description="Robot controller to start.",
)
)
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(
"gripper_name",
default_value="rbs_gripper",
choices=["rbs_gripper", ""],
description="choose gripper by name (leave empty if hasn't)",
)
)
declared_arguments.append(
DeclareLaunchArgument("with_gripper",
default_value="true",
description="With gripper or not?")
)
declared_arguments.append(
DeclareLaunchArgument("sim_gazebo",
default_value="true",
description="Gazebo Simulation")
)
declared_arguments.append(
DeclareLaunchArgument("env_manager",
default_value="false",
description="Launch env_manager?")
)
declared_arguments.append(
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?")
)
declared_arguments.append(
DeclareLaunchArgument("launch_perception",
default_value="false",
description="Launch perception?")
)
declared_arguments.append(
DeclareLaunchArgument("launch_task_planner",
default_value="false",
description="Launch task_planner?")
)
declared_arguments.append(
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")
)
declared_arguments.append(
DeclareLaunchArgument("launch_controllers",
default_value="true",
description="Launch controllers?")
)
declared_arguments.append(
DeclareLaunchArgument("gazebo_gui",
default_value="true",
description="Launch gazebo with gui?")
)
declared_arguments.append(
DeclareLaunchArgument("gazebo_world_filename",
default_value="asm2.sdf",
description="Filename of Gazebo world file to launch")
)
declared_arguments.append(
DeclareLaunchArgument("robots_config_file",
default_value="robot_scene",
description="Filename for config file with robots in scene")
)
return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])

457
rbs_bringup/launch/bringup.launch.py → rbs_bringup/launch/rbs_robot.launch.py
View file

@ -1,19 +1,232 @@
from launch import LaunchDescription, condition
from launch import LaunchContext, LaunchDescription
from launch.actions import (
DeclareLaunchArgument,
IncludeLaunchDescription,
RegisterEventHandler,
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_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_arm import RbsBuilder
def launch_setup(context, *args, **kwargs):
# Initialize Arguments
robot_type = LaunchConfiguration("robot_type")
# General arguments
with_gripper_condition = LaunchConfiguration("with_gripper")
controllers_file = LaunchConfiguration("controllers_file")
cartesian_controllers = LaunchConfiguration("cartesian_controllers")
description_package = LaunchConfiguration("description_package")
description_file = LaunchConfiguration("description_file")
robot_name = LaunchConfiguration("robot_name")
start_joint_controller = LaunchConfiguration("start_joint_controller")
initial_joint_controller = LaunchConfiguration("initial_joint_controller")
launch_moveit = LaunchConfiguration("launch_moveit")
launch_task_planner = LaunchConfiguration("launch_task_planner")
launch_perception = LaunchConfiguration("launch_perception")
moveit_config_package = LaunchConfiguration("moveit_config_package")
moveit_config_file = LaunchConfiguration("moveit_config_file")
use_sim_time = LaunchConfiguration("use_sim_time")
hardware = LaunchConfiguration("hardware")
launch_controllers = LaunchConfiguration("launch_controllers")
gripper_name = LaunchConfiguration("gripper_name")
x_pos = LaunchConfiguration("x")
y_pos = LaunchConfiguration("y")
z_pos = LaunchConfiguration("z")
roll = LaunchConfiguration("roll")
pitch = LaunchConfiguration("pitch")
yaw = LaunchConfiguration("yaw")
namespace = LaunchConfiguration("namespace")
robot_name = robot_name.perform(context)
namespace = namespace.perform(context)
robot_type = robot_type.perform(context)
description_package = description_package.perform(context)
description_file = description_file.perform(context)
controllers_file = controllers_file.perform(context)
# remappings = [("/tf", "tf"), ("/tf_static", "tf_static")]
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)
#TODO: add rotation and add probably via dict
}
)
# robot = RbsBuilder(6, "arm0", "world", "rbs_gripper")
# robot.base()
# robot.gripper()
# robot.ros2_control()
# robot.moveit()
robot_description_content = robot_description_doc.toprettyxml(indent=" ")
robot_description = {"robot_description": robot_description_content}
robot_description_semantic_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare(moveit_config_package), "config/moveit", "rbs_arm.srdf.xacro"]
),
" ",
"name:=",robot_name," ",
"with_gripper:=",with_gripper_condition, " ",
"gripper_name:=", gripper_name, " ",
]
)
robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content}
robot_description_kinematics = PathJoinSubstitution(
[FindPackageShare(moveit_config_package), "config", "kinematics.yaml"]
)
robot_state_publisher = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
namespace=namespace,
output="both",
# remappings=remappings,
parameters=[{"use_sim_time": use_sim_time}, robot_description],
)
rviz_config_file = PathJoinSubstitution(
[FindPackageShare("rbs_bringup"), "config", "rbs.rviz"]
)
rviz = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
namespace=namespace,
arguments=["-d", rviz_config_file],
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
{'use_sim_time': use_sim_time}
]
)
control = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare(description_package),
'launch',
'control.launch.py'
])
]),
launch_arguments={
'with_gripper': with_gripper_condition,
'robot_description': robot_description_content,
'start_joint_controller': start_joint_controller,
'initial_joint_controller': initial_joint_controller,
'controllers_file': controllers_file,
'cartesian_controllers': cartesian_controllers,
'namespace': namespace,
}.items(),
condition=IfCondition(launch_controllers))
moveit = IncludeLaunchDescription(
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,
}.items(),
condition=IfCondition(launch_moveit))
skills = IncludeLaunchDescription(
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()
)
task_planner = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare('rbs_task_planner'),
'launch',
'task_planner.launch.py'
])
]),
launch_arguments={
# TBD
}.items(),
condition=IfCondition(launch_task_planner))
perception = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare('rbs_perception'),
'launch',
'perception.launch.py'
])
]),
launch_arguments={
# TBD
}.items(),
condition=IfCondition(launch_perception))
nodes_to_start = [
robot_state_publisher,
control,
moveit,
skills,
task_planner,
perception,
# rviz
]
return nodes_to_start
def generate_launch_description():
declared_arguments = []
declared_arguments.append(
DeclareLaunchArgument(
"rbs_robot_type",
"robot_type",
description="Type of robot by name",
choices=["rbs_arm","ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
default_value="rbs_arm",
@ -44,9 +257,9 @@ def generate_launch_description():
)
declared_arguments.append(
DeclareLaunchArgument(
"prefix",
default_value='""',
description="Prefix of the joint names, useful for \
"robot_name",
default_value="arm0",
description="tf_prefix of the joint names, useful for \
multi-robot setup. If changed than also joint names in the controllers' configuration \
have to be updated.",
)
@ -88,6 +301,14 @@ def generate_launch_description():
This is needed for the trajectory planing in simulation.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"gripper_name",
default_value="",
choices=["rbs_gripper", ""],
description="choose gripper by name (leave empty if hasn't)",
)
)
declared_arguments.append(
DeclareLaunchArgument("with_gripper",
default_value="true",
@ -151,197 +372,41 @@ def generate_launch_description():
default_value="false",
description="Launch gazebo with gui?")
)
# Initialize Arguments
rbs_robot_type = LaunchConfiguration("rbs_robot_type")
# General arguments
with_gripper_condition = LaunchConfiguration("with_gripper")
controllers_file = LaunchConfiguration("controllers_file")
cartesian_controllers = LaunchConfiguration("cartesian_controllers")
description_package = LaunchConfiguration("description_package")
description_file = LaunchConfiguration("description_file")
prefix = LaunchConfiguration("prefix")
start_joint_controller = LaunchConfiguration("start_joint_controller")
initial_joint_controller = LaunchConfiguration("initial_joint_controller")
launch_rviz = LaunchConfiguration("launch_rviz")
launch_simulation = LaunchConfiguration("launch_sim")
launch_moveit = LaunchConfiguration("launch_moveit")
launch_task_planner = LaunchConfiguration("launch_task_planner")
launch_perception = LaunchConfiguration("launch_perception")
moveit_config_package = LaunchConfiguration("moveit_config_package")
moveit_config_file = LaunchConfiguration("moveit_config_file")
use_sim_time = LaunchConfiguration("use_sim_time")
sim_gazebo = LaunchConfiguration("sim_gazebo")
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]
declared_arguments.append(
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")
)
declared_arguments.append(
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")
)
declared_arguments.append(
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")
)
declared_arguments.append(
DeclareLaunchArgument("yaw",
default_value="0.0",
description="Position of robot in world by Z")
)
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]),
" ",
"gripper_name:=", "", " ",
"prefix:=", prefix, " ",
"hardware:=", hardware, " ",
"simulation_controllers:=", initial_joint_controllers_file_path, " ",
]
)
robot_description = {"robot_description": robot_description_content}
robot_description_semantic_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[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"]
)
robot_state_publisher = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
output="both",
parameters=[{"use_sim_time": True}, robot_description],
)
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('rbs_arm'),
'launch',
'control.launch.py'
])
]),
launch_arguments={
'with_gripper': with_gripper_condition,
'robot_description': robot_description_content,
'start_joint_controller': start_joint_controller,
'initial_joint_controller': initial_joint_controller,
'controllers_file': controllers_file,
"cartesian_controllers": cartesian_controllers
}.items(),
condition=IfCondition(launch_controllers))
simulation = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare('rbs_simulation'),
'launch',
'simulation.launch.py'
])
]),
launch_arguments={
'sim_gazebo': sim_gazebo,
'gazebo_gui': gazebo_gui,
'rbs_robot_type': rbs_robot_type,
'env_manager': env_manager,
'debugger': "false"
}.items(),
condition=IfCondition(launch_simulation))
moveit = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare('rbs_arm'),
'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,
'prefix': prefix,
'with_gripper': with_gripper_condition,
'robot_description_semantic': robot_description_semantic_content
}.items(),
condition=IfCondition(launch_moveit))
skills = IncludeLaunchDescription(
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,
}.items()
)
task_planner = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare('rbs_task_planner'),
'launch',
'task_planner.launch.py'
])
]),
launch_arguments={
# TBD
}.items(),
condition=IfCondition(launch_task_planner))
perception = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare('rbs_perception'),
'launch',
'perception.launch.py'
])
]),
launch_arguments={
# TBD
}.items(),
condition=IfCondition(launch_perception))
nodes_to_start = [
robot_state_publisher,
rviz,
control,
simulation,
moveit,
skills,
task_planner,
perception
]
return LaunchDescription(declared_arguments + nodes_to_start)
return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])

280
rbs_bringup/launch/single_robot.launch.py Normal file
View file

@ -0,0 +1,280 @@
from launch import LaunchDescription
from launch.actions import (
DeclareLaunchArgument,
IncludeLaunchDescription,
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
def launch_setup(context, *args, **kwargs):
# Initialize Arguments
robot_type = LaunchConfiguration("robot_type")
# General arguments
with_gripper_condition = LaunchConfiguration("with_gripper")
controllers_file = LaunchConfiguration("controllers_file")
cartesian_controllers = LaunchConfiguration("cartesian_controllers")
description_package = LaunchConfiguration("description_package")
description_file = LaunchConfiguration("description_file")
robot_name = LaunchConfiguration("robot_name")
start_joint_controller = LaunchConfiguration("start_joint_controller")
initial_joint_controller = LaunchConfiguration("initial_joint_controller")
launch_simulation = LaunchConfiguration("launch_sim")
launch_moveit = LaunchConfiguration("launch_moveit")
launch_task_planner = LaunchConfiguration("launch_task_planner")
launch_perception = LaunchConfiguration("launch_perception")
moveit_config_package = LaunchConfiguration("moveit_config_package")
moveit_config_file = LaunchConfiguration("moveit_config_file")
use_sim_time = LaunchConfiguration("use_sim_time")
sim_gazebo = LaunchConfiguration("sim_gazebo")
hardware = LaunchConfiguration("hardware")
env_manager = LaunchConfiguration("env_manager")
launch_controllers = LaunchConfiguration("launch_controllers")
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'
])
]),
launch_arguments={
'sim_gazebo': sim_gazebo,
'debugger': "false",
'launch_env_manager': "false"
}.items(),
condition=IfCondition(launch_simulation)
)
# FIXME: namespaces
# configured_params = RewrittenYaml(
# source_file=os.path.join(
# get_package_share_directory(
# description_package.perform(context)),
# "config",
# controllers_file.perform(context)),
# root_key=robot_name.perform(context),
# param_rewrites={},
# convert_types=True,
# )
initial_joint_controllers_file_path = os.path.join(
get_package_share_directory('rbs_arm'), 'config', 'rbs_arm0_controllers.yaml'
)
# controller_paramfile = configured_params.perform(context)
# namespace = "/" + robot_name.perform(context)
namespace = ""
gz_spawner = Node(
package='ros_gz_sim',
executable='create',
arguments=[
'-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"
])
]),
launch_arguments={
"env_manager": env_manager,
"with_gripper": with_gripper_condition,
"gripper_name": gripper_name,
"controllers_file": controllers_file,
"robot_type": robot_type,
"controllers_file": initial_joint_controllers_file_path,
"cartesian_controllers": cartesian_controllers,
"description_package": description_package,
"description_file": description_file,
"robot_name": robot_type,
"start_joint_controller": start_joint_controller,
"initial_joint_controller": initial_joint_controller,
"launch_simulation": launch_simulation,
"launch_moveit": launch_moveit,
"launch_task_planner": launch_task_planner,
"launch_perception": launch_perception,
"moveit_config_package": moveit_config_package,
"moveit_config_file": moveit_config_file,
"use_sim_time": use_sim_time,
"sim_gazebo": sim_gazebo,
"hardware": hardware,
"launch_controllers": "true",
"gazebo_gui": gazebo_gui,
# "x": "0.5",
# "y": "0.5",
# "z": "0.5"
}.items()
)
nodes_to_start = [
simulation,
gz_spawner,
single_robot_setup
]
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","ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
default_value="rbs_arm",
)
)
# General arguments
declared_arguments.append(
DeclareLaunchArgument(
"controllers_file",
default_value="rbs_arm_controllers_gazebosim.yaml",
description="YAML file with the controllers configuration.",
)
)
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(
"start_joint_controller",
default_value="false",
description="Enable headless mode for robot control",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"initial_joint_controller",
default_value="joint_trajectory_controller",
description="Robot controller to start.",
)
)
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(
"gripper_name",
default_value="rbs_gripper",
choices=["rbs_gripper", ""],
description="choose gripper by name (leave empty if hasn't)",
)
)
declared_arguments.append(
DeclareLaunchArgument("with_gripper",
default_value="true",
description="With gripper or not?")
)
declared_arguments.append(
DeclareLaunchArgument("sim_gazebo",
default_value="true",
description="Gazebo Simulation")
)
declared_arguments.append(
DeclareLaunchArgument("env_manager",
default_value="false",
description="Launch env_manager?")
)
declared_arguments.append(
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?")
)
declared_arguments.append(
DeclareLaunchArgument("launch_perception",
default_value="false",
description="Launch perception?")
)
declared_arguments.append(
DeclareLaunchArgument("launch_task_planner",
default_value="false",
description="Launch task_planner?")
)
declared_arguments.append(
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")
)
declared_arguments.append(
DeclareLaunchArgument("launch_controllers",
default_value="true",
description="Launch controllers?")
)
declared_arguments.append(
DeclareLaunchArgument("gazebo_gui",
default_value="true",
description="Launch gazebo with gui?")
)
return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])

2
rbs_bringup/package.xml
View file

@ -9,6 +9,8 @@
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>nav2_common</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

62
rbs_bringup/rbs_launch_utils/launch_common.py
View file

@ -1,58 +1,30 @@
#!/usr/bin/env python3
# Copyright (c) 2021 PickNik, Inc.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
#
# * Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in the
# documentation and/or other materials provided with the distribution.
#
# * Neither the name of the {copyright_holder} nor the names of its
# contributors may be used to endorse or promote products derived from
# this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Author: Lovro Ivanov
import math
import os
import sys
import yaml
from typing import Dict
from ament_index_python.packages import get_package_share_directory
def print_error(message: str) -> None:
"""Print an error message in red color."""
print("\033[91m{}\033[0m".format(message), file=sys.stderr)
def construct_angle_radians(loader, node):
def construct_angle_radians(loader, node) -> float:
"""Utility function to construct radian values from yaml."""
value = loader.construct_scalar(node)
try:
return float(value)
except SyntaxError:
raise Exception("invalid expression: %s" % value)
raise Exception("Invalid expression: %s" % value)
def construct_angle_degrees(loader, node):
def construct_angle_degrees(loader, node) -> float:
"""Utility function for converting degrees into radians from yaml."""
return math.radians(construct_angle_radians(loader, node))
def load_yaml(package_name, file_path):
def load_yaml(package_name: str, file_path: str) -> Dict:
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)
@ -60,25 +32,27 @@ def load_yaml(package_name, file_path):
yaml.SafeLoader.add_constructor("!radians", construct_angle_radians)
yaml.SafeLoader.add_constructor("!degrees", construct_angle_degrees)
except Exception:
raise Exception("yaml support not available; install python-yaml")
print_error("Error: YAML support not available; install python-yaml")
sys.exit(1)
try:
with open(absolute_file_path) as file:
return yaml.safe_load(file)
except OSError: # parent of IOError, OSError *and* WindowsError where available
return None
def load_yaml_abs(absolute_file_path):
print_error("Error: Unable to open the file {}".format(absolute_file_path))
sys.exit(1)
def load_yaml_abs(absolute_file_path: str) -> Dict:
try:
yaml.SafeLoader.add_constructor("!radians", construct_angle_radians)
yaml.SafeLoader.add_constructor("!degrees", construct_angle_degrees)
except Exception:
raise Exception("yaml support not available; install python-yaml")
print_error("Error: YAML support not available; install python-yaml")
sys.exit(1)
try:
with open(absolute_file_path) as file:
return yaml.safe_load(file)
except OSError: # parent of IOError, OSError *and* WindowsError where available
return None
print_error("Error: Unable to open the file {}".format(absolute_file_path))
sys.exit(1)

51
rbs_bringup/rbs_launch_utils/merged_yaml.py Normal file
View file

@ -0,0 +1,51 @@
from typing import Dict
from typing import List
from typing import Text
from typing import Optional
import yaml
import tempfile
import launch
from nav2_common.launch import RewrittenYaml
from rbs_launch_utils.launch_common import load_yaml_abs
class MergedYaml:
"""
Apply RewrittenYaml for list of config files and combine it in one file
"""
def __init__(self, context: launch.LaunchContext,
source_file: Text = "",
root_keys: List = [],
param_rewrites: Dict = {},
convert_types = False,
key_rewrites: Dict = {}):
self.__root_keys = root_keys
self.__context = context
self.__param_rewrites = param_rewrites
self.__convet_types = convert_types
#TODO: work with multiple source files
# for different robots
self.__source_file = source_file
self.__key_rewrites = key_rewrites
def get_config_file(self) -> List[Text]:
config_list = []
for key in self.__root_keys:
yaml = RewrittenYaml(
self.__source_file,
self.__param_rewrites,
key,
self.__key_rewrites,
self.__convet_types)
config_list.append(yaml.perform(self.__context))
return config_list
def merge_yamls(self) -> Text:
configs = self.get_config_file()
yamls = []
for file in configs:
yamls.append(load_yaml_abs(file))
merged_yaml = tempfile.NamedTemporaryFile(mode='w', delete=False, suffix=".yaml")
yaml.dump_all(yamls, merged_yaml)
return merged_yaml.name

36
rbs_bt_executor/bt_trees/test_roboclone.xml Normal file
View file

@ -0,0 +1,36 @@
<?xml version="1.0"?>
<root main_tree_to_execute="MainTree">
<!-- ////////// -->
<BehaviorTree ID="MainTree">
<Sequence>
<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="rbs_arm" />
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<BehaviorTree ID="WorkspaceInspection">
<Sequence>
<Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
pose_vec_out="{goal_pose}"
robot_name="{robot_name}" server_name="/arm1/move_topose" 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="/arm1/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="/arm1/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="/arm1/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="/arm/move_cartesian" server_timeout="5000" />
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<include path="./nodes_interfaces/general.xml"/>
<!-- ////////// -->
</root>

10
rbs_bt_executor/bt_trees/test_tree.xml
View file

@ -14,19 +14,19 @@
<Sequence>
<Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
pose_vec_out="{goal_pose}"
robot_name="{robot_name}" server_name="/move_topose" server_timeout="5000" />
robot_name="{robot_name}" server_name="/cartesian_move_to_pose" 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" />
robot_name="{robot_name}" server_name="/cartesian_move_to_pose" 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" />
robot_name="{robot_name}" server_name="/cartesian_move_to_pose" 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" />
robot_name="{robot_name}" server_name="/cartesian_move_to_pose" 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" />
robot_name="{robot_name}" server_name="/cartesian_move_to_pose" server_timeout="5000" />
</Sequence>
</BehaviorTree>

11
rbs_bt_executor/launch/rbs_executor.launch.py
View file

@ -27,11 +27,16 @@ def generate_launch_description():
Node(
package='behavior_tree',
executable='bt_engine',
# prefix=['gdbserver localhost:3000'],
# prefix=['gdbserver localhost:1234'],
parameters=[
btfile_param,
bt_skills_param
]
bt_skills_param,
{'use_sim_time': True}
],
# arguments=[
# "--ros-args",
# "--log-level", "debug",
# ],
)
]

2
rbs_bt_executor/src/EnvManager.cpp
View file

@ -35,7 +35,7 @@ public:
if (response->ok) {
// TODO We need better perfomance for call it in another place for all BT nodes
// - Make it via shared_ptr forward throught blackboard
auto t = std::make_shared<rbs_utils::AssemblyConfigLoader>("asp-example2", _node);
auto t = std::make_shared<rbs_utils::AssemblyConfigLoader>("asp-example", _node);
auto p = t->getWorkspaceInspectorTrajectory();
setOutput("workspace", p);
return BT::NodeStatus::SUCCESS;

4
rbs_bt_executor/src/MoveToPose.cpp
View file

@ -29,8 +29,8 @@ class MoveToPose : public BtAction<MoveToPoseAction> {
goal.robot_name = robot_name_;
goal.target_pose = pose_des;
goal.end_effector_acceleration = 0.5;
goal.end_effector_velocity = 0.5;
goal.end_effector_acceleration = 1.0;
goal.end_effector_velocity = 1.0;
RCLCPP_INFO(_node->get_logger(), "Goal populated");

2
rbs_simulation/launch/get_urdf.launch.py
View file

@ -17,7 +17,7 @@ def generate_launch_description():
"safety_k_position": "20",
"name": "ur",
"ur_type": "ur5e",
"prefix": "",
"tf_prefix": "",
"sim_mujoco": "true",
"simulation_controllers": str(initial_joint_controllers_file_path),
"with_gripper": "true"

16
rbs_simulation/launch/rbs_simulation.launch.py
View file

@ -94,9 +94,9 @@ def generate_launch_description():
)
declared_arguments.append(
DeclareLaunchArgument(
"prefix",
"tf_prefix",
default_value='""',
description="Prefix of the joint names, useful for \
description="tf_prefix of the joint names, useful for \
multi-robot setup. If changed than also joint names in the controllers' configuration \
have to be updated.",
)
@ -173,7 +173,7 @@ def generate_launch_description():
description_package = LaunchConfiguration("description_package")
description_file = LaunchConfiguration("description_file")
prefix = LaunchConfiguration("prefix")
tf_prefix = LaunchConfiguration("tf_prefix")
start_joint_controller = LaunchConfiguration("start_joint_controller")
initial_joint_controller = LaunchConfiguration("initial_joint_controller")
initial_gripper_controller = LaunchConfiguration("initial_gripper_controller")
@ -221,7 +221,7 @@ def generate_launch_description():
"safety_k_position:=", safety_k_position, " ",
"name:=", "ur", " ",
"ur_type:=", rbs_robot_type, " ",
"prefix:=", prefix, " ",
"tf_prefix:=", tf_prefix, " ",
"sim_mujoco:=", sim_mujoco, " ",
"sim_gazebo:=", sim_gazebo, " ",
"sim_fake:=", sim_fake, " ",
@ -325,8 +325,8 @@ def generate_launch_description():
"name:=",
"ur",
" ",
"prefix:=",
prefix,
"tf_prefix:=",
tf_prefix,
" ",
"with_gripper:=",
with_gripper_condition
@ -460,7 +460,7 @@ def generate_launch_description():
executable="assemble_state_service_server",
output="screen",
parameters=[
{'assemble_prefix': 'ASSEMBLE_'},
{'assemble_tf_prefix': 'ASSEMBLE_'},
{'assemble_dir': assemble_dir}
]
)
@ -529,7 +529,7 @@ def generate_launch_description():
# package="rbs_perception",
# executable="pc_filter",
# output="screen",
# #prefix=['xterm -e gdb -ex run --args'],
# #tf_prefix=['xterm -e gdb -ex run --args'],
# )
grasp_marker = Node(

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

@ -37,12 +37,18 @@ def generate_launch_description():
default_value="false",
description="launch Gazebo with debugger?")
)
declared_arguments.append(
DeclareLaunchArgument("vision_bridge",
default_value="false",
description="Create bridge for rgbd cameras")
)
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")
vision_bridge = LaunchConfiguration("vision_bridge")
# FIXME: To args when we'll have different files
# TODO: Use global variable to find world file in robossembler_db
@ -98,7 +104,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) and IfCondition(vision_bridge))
rgbd_bridge_in = Node(
package='ros_gz_bridge',
@ -110,7 +116,7 @@ def generate_launch_description():
'/inner_rgbd_camera/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked'
],
output='screen',
condition=IfCondition(sim_gazebo))
condition=IfCondition(sim_gazebo) and IfCondition(vision_bridge))
clock_bridge = Node(
package='ros_gz_bridge',

90
rbs_simulation/launch/simulation_gazebo.launch.py Normal file
View file

@ -0,0 +1,90 @@
import os
from launch_ros.actions import Node
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch_ros.substitutions import FindPackageShare
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
from launch.conditions import IfCondition, UnlessCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
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")
)
declared_arguments.append(
DeclareLaunchArgument("gazebo_gui",
default_value="true",
description="Launch env_manager?")
)
declared_arguments.append(
DeclareLaunchArgument("debugger",
default_value="false",
description="launch Gazebo with debugger?")
)
declared_arguments.append(
DeclareLaunchArgument("launch_env_manager",
default_value="false",
description="Launch env_manager?")
)
declared_arguments.append(
DeclareLaunchArgument("gazebo_world_filename",
default_value="",
description="Launch env_manager?")
)
sim_gazebo = LaunchConfiguration("sim_gazebo")
gazebo_gui = LaunchConfiguration("gazebo_gui")
debugger = LaunchConfiguration("debugger")
launch_env_manager = LaunchConfiguration("launch_env_manager")
gazebo_world_filename = LaunchConfiguration("gazebo_world_filename")
world_config_file = PathJoinSubstitution(
[FindPackageShare("rbs_simulation"), "worlds", gazebo_world_filename]
)
gazebo_server = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[os.path.join(get_package_share_directory('ros_gz_sim'),
'launch', 'gz_sim.launch.py')]),
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={
'gz_args': [' -r ',world_config_file],
"debugger": debugger,
}.items(),
condition=IfCondition(gazebo_gui))
env_manager = Node(
package="env_manager",
executable="run_env_manager",
parameters=[{'use_sim_time': True}],
condition=IfCondition(launch_env_manager)
)
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,
gazebo_server,
clock_bridge,
env_manager
]
return LaunchDescription(declared_arguments + nodes_to_start)

76
rbs_simulation/launch/simulation_spawn.launch.py Normal file
View file

@ -0,0 +1,76 @@
from launch_ros.actions import Node
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction
from launch.substitutions import LaunchConfiguration
from launch.conditions import IfCondition
def launch_setup(context, *args, **kwargs):
sim_gazebo = LaunchConfiguration("sim_gazebo")
rbs_robot_name= LaunchConfiguration("rbs_robot_name")
namespace = LaunchConfiguration("namespace")
x_pos = LaunchConfiguration("x_pos")
y_pos = LaunchConfiguration("y_pos")
z_pos = LaunchConfiguration("z_pos")
# Convert LaunchConfiguration to string values
namespace = namespace.perform(context)
# Spawn robot
# TODO: add full pose support include rotation
gazebo_spawn_robot = Node(
package='ros_gz_sim',
executable='create',
arguments=[
'-name', rbs_robot_name,
'-x', x_pos.perform(context),
'-y', y_pos.perform(context),
'-z', z_pos.perform(context),
'-topic', namespace + '/robot_description'],
output='screen',
condition=IfCondition(sim_gazebo))
nodes_to_start = [
gazebo_spawn_robot,
]
return nodes_to_start
def generate_launch_description():
declared_arguments = []
declared_arguments.append(
DeclareLaunchArgument("sim_gazebo",
default_value="false",
description="Gazebo Simulation")
)
declared_arguments.append(
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("gazebo_gui",
default_value="true",
description="Launch env_manager?")
)
declared_arguments.append(
DeclareLaunchArgument("namespace",
default_value="",
description="namespace for spawn a robot")
)
declared_arguments.append(
DeclareLaunchArgument("x_pos",
default_value="0.0",
description="robot's position along X")
)
declared_arguments.append(
DeclareLaunchArgument("y_pos",
default_value="0.0",
description="robot's position along Y")
)
declared_arguments.append(
DeclareLaunchArgument("z_pos",
default_value="0.0",
description="robot's position along Z")
)
return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])

23
rbs_simulation/worlds/asm2.sdf
View file

@ -10,6 +10,7 @@
<plugin name='ignition::gazebo::systems::UserCommands' filename='ignition-gazebo-user-commands-system'/>
<plugin name='ignition::gazebo::systems::SceneBroadcaster' filename='ignition-gazebo-scene-broadcaster-system'/>
<plugin name='ignition::gazebo::systems::Contact' filename='ignition-gazebo-contact-system'/>
<plugin filename="ignition-gazebo-forcetorque-system" name="ignition::gazebo::systems::ForceTorque"/>
<plugin name="ignition::gazebo::systems::Sensors" filename="ignition-gazebo-sensors-system">
<render_engine>ogre2</render_engine>
</plugin>
@ -93,15 +94,15 @@
</link>
</model>
<!-- Manipulating objects -->
<include>
<name>board</name>
<uri>model://board</uri>
<pose>0.45 0.0 0.0 0.0 0.0 0.0</pose>
</include>
<include>
<name>bishop</name>
<uri>model://bishop</uri>
<pose>0.35 0.0 0.0 0.0 0.0 0.0</pose>
</include>
<!-- <include> -->
<!-- <name>board</name> -->
<!-- <uri>model://board</uri> -->
<!-- <pose>0.45 0.0 0.0 0.0 0.0 0.0</pose> -->
<!-- </include> -->
<!-- <include> -->
<!-- <name>bishop</name> -->
<!-- <uri>model://bishop</uri> -->
<!-- <pose>0.35 0.0 0.0 0.0 0.0 0.0</pose> -->
<!-- </include> -->
</world>
</sdf>
</sdf>

37
rbs_skill_servers/CMakeLists.txt
View file

@ -33,7 +33,6 @@ find_package(tf2_msgs REQUIRED)
find_package(tinyxml2_vendor REQUIRED)
find_package(TinyXML2 REQUIRED)
find_package(Eigen3 3.3 REQUIRED)
find_package(nlohmann_json 3.2.0 REQUIRED)
find_package(rbs_utils REQUIRED)
# Default to Fortress
@ -116,7 +115,7 @@ target_include_directories(
$<INSTALL_INTERFACE:include>)
target_compile_definitions(pick_place_pose_loader
PRIVATE "PICK_PLACE_POSE_LOADER_CPP_BUILDING_DLL")
ament_target_dependencies(pick_place_pose_loader ${deps} Eigen3 nlohmann_json
ament_target_dependencies(pick_place_pose_loader ${deps} Eigen3
rbs_utils)
rclcpp_components_register_node(
pick_place_pose_loader PLUGIN "rbs_skill_actions::GetGraspPickPoseServer"
@ -134,30 +133,6 @@ add_executable(move_cartesian_path_action_server
src/move_cartesian_path_action_server.cpp)
ament_target_dependencies(move_cartesian_path_action_server ${deps})
# add_executable(add_planning_scene_object_service
# src/add_planning_scene_objects_service.cpp)
# ament_target_dependencies(add_planning_scene_object_service ${deps})
# add_library(assemble_state_server SHARED src/assemble_state_server.cpp)
# target_compile_definitions(assemble_state_server
# PRIVATE "ASSEMBLE_STATE_SERVER_CPP_BUILDING_DLL")
# ament_target_dependencies(assemble_state_server ${deps})
# target_link_libraries(assemble_state_server ${TINYXML2_LIBRARY})
# rclcpp_components_register_node(
# assemble_state_server PLUGIN "AssembleStateServer" EXECUTABLE
# assemble_state_service_server)
# add_library(moveit_update_planning_scene_server SHARED
# src/moveit_update_planning_scene.cpp)
# target_compile_definitions(
# moveit_update_planning_scene_server
# PRIVATE "MOVEIT_UPDATE_PLANNING_SCENE_SERVER_CPP_BUILDING_DLL")
# ament_target_dependencies(moveit_update_planning_scene_server ${deps})
# target_link_libraries(moveit_update_planning_scene_server ${TINYXML2_LIBRARY})
# rclcpp_components_register_node(
# moveit_update_planning_scene_server PLUGIN "UpdatePlanningSceneServer"
# EXECUTABLE moveit_update_planning_scene_service_server)
#
install(DIRECTORY include/ DESTINATION include)
install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME})
@ -168,13 +143,17 @@ install(
pick_place_pose_loader
move_to_joint_states_action_server
move_cartesian_path_action_server
# add_planning_scene_object_service
# assemble_state_server
# moveit_update_planning_scene_server
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION lib/${PROJECT_NAME})
ament_export_include_directories(include)
ament_python_install_package(${PROJECT_NAME})
install(PROGRAMS
scripts/test_cartesian_controller.py
scripts/move_to_pose.py
DESTINATION lib/${PROJECT_NAME}
)
ament_package()

8
rbs_skill_servers/include/rbs_skill_servers/pick_place_pose_loader.hpp
View file

@ -3,8 +3,6 @@
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_components/register_node_macro.hpp"
#include <Eigen/Core>
#include <nlohmann/json.hpp>
#include <rclcpp/rclcpp.hpp>
#include <tf2_eigen/tf2_eigen.hpp>
#include <tf2_ros/transform_listener.h>
@ -24,8 +22,8 @@ private:
geometry_msgs::msg::TransformStamped place_pose_tf;
geometry_msgs::msg::TransformStamped grasp_pose_tf;
std::vector<geometry_msgs::msg::Pose>
getPlacePoseJson(const nlohmann::json &json);
// std::vector<geometry_msgs::msg::Pose>
// getPlacePoseJson(const nlohmann::json &json);
void handleServer(
const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr
request,
@ -36,4 +34,4 @@ private:
};
} // namespace rbs_skill_actions
RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::GetGraspPickPoseServer);
RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::GetGraspPickPoseServer);

175
rbs_skill_servers/launch/skills.launch.py
View file

@ -1,19 +1,104 @@
import os
from launch import LaunchDescription, LaunchContext
from collections import namedtuple
from os import name
from launch import LaunchDescription
from launch.actions import (
DeclareLaunchArgument,
IncludeLaunchDescription,
ExecuteProcess,
OpaqueFunction
)
from ament_index_python.packages import get_package_share_directory
from launch.conditions import IfCondition, UnlessCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from rbs_launch_utils.launch_common import load_yaml
def launch_setup(context, *args, **kwargs):
robot_description_decl = LaunchConfiguration("robot_description")
robot_description_semantic_decl = LaunchConfiguration("robot_description_semantic")
robot_description_kinematics = LaunchConfiguration("robot_description_kinematics")
use_sim_time = LaunchConfiguration("use_sim_time")
with_gripper_condition = LaunchConfiguration("with_gripper_condition")
points_params_filepath_decl = LaunchConfiguration("points_params_filepath")
robot_description = {"robot_description": robot_description_decl}
robot_description_semantic = {"robot_description_semantic": robot_description_semantic_decl}
namespace = LaunchConfiguration("namespace")
points_params = load_yaml(
"rbs_skill_servers", "config/gripperPositions.yaml"
)
move_topose_action_server = Node(
package="rbs_skill_servers",
executable="move_topose_action_server",
namespace=namespace,
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
{"use_sim_time": use_sim_time},
]
)
move_to_pose = Node(
package="rbs_skill_servers",
executable="move_to_pose.py",
namespace=namespace
)
gripper_control_node = Node(
package="rbs_skill_servers",
executable="gripper_control_action_server",
namespace=namespace,
parameters= [
robot_description,
robot_description_semantic,
robot_description_kinematics,
{"use_sim_time": use_sim_time},
],
condition=IfCondition(with_gripper_condition)
)
move_cartesian_path_action_server = Node(
package="rbs_skill_servers",
executable="move_cartesian_path_action_server",
namespace=namespace,
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
{"use_sim_time": use_sim_time},
]
)
# FIXME: The name of this node, "move_topose,"
# is intended to be different from the actual MoveToPose node.
move_joint_state_action_server = Node(
package="rbs_skill_servers",
executable="move_to_joint_states_action_server",
namespace=namespace,
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
{"use_sim_time": use_sim_time},
]
)
grasp_pose_loader = Node(
package="rbs_skill_servers",
executable="pick_place_pose_loader_service_server",
namespace=namespace,
output="screen"
)
nodes_to_start =[
move_topose_action_server,
gripper_control_node,
move_cartesian_path_action_server,
move_joint_state_action_server,
move_to_pose,
# grasp_pose_loader
]
return nodes_to_start
def generate_launch_description():
@ -41,77 +126,9 @@ def generate_launch_description():
declared_arguments.append(
DeclareLaunchArgument("points_params_filepath", default_value="")
)
robot_description_decl = LaunchConfiguration("robot_description")
robot_description_semantic_decl = LaunchConfiguration("robot_description_semantic")
robot_description_kinematics = LaunchConfiguration("robot_description_kinematics")
use_sim_time = LaunchConfiguration("use_sim_time")
with_gripper_condition = LaunchConfiguration("with_gripper_condition")
points_params_filepath_decl = LaunchConfiguration("points_params_filepath")
robot_description = {"robot_description": robot_description_decl}
robot_description_semantic = {"robot_description_semantic": robot_description_semantic_decl}
points_params = load_yaml(
"rbs_skill_servers", "config/gripperPositions.yaml"
declared_arguments.append(
DeclareLaunchArgument("namespace", default_value="")
)
move_topose_action_server = Node(
package="rbs_skill_servers",
executable="move_topose_action_server",
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
{"use_sim_time": use_sim_time},
]
)
gripper_control_node = Node(
package="rbs_skill_servers",
executable="gripper_control_action_server",
parameters= [
robot_description,
robot_description_semantic,
robot_description_kinematics,
{"use_sim_time": use_sim_time},
],
condition=IfCondition(with_gripper_condition)
)
move_cartesian_path_action_server = Node(
package="rbs_skill_servers",
executable="move_cartesian_path_action_server",
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
{"use_sim_time": use_sim_time},
]
)
move_joint_state_action_server = Node(
package="rbs_skill_servers",
executable="move_to_joint_states_action_server",
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
{"use_sim_time": use_sim_time},
]
)
grasp_pose_loader = Node(
package="rbs_skill_servers",
executable="pick_place_pose_loader_service_server",
output="screen"
)
nodes_to_start =[
move_topose_action_server,
gripper_control_node,
move_cartesian_path_action_server,
move_joint_state_action_server,
grasp_pose_loader
]
return LaunchDescription(declared_arguments + nodes_to_start)
return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])

0
rbs_skill_servers/rbs_skill_servers/__init__.py Normal file
View file

108
rbs_skill_servers/scripts/move_to_pose.py Executable file
View file

@ -0,0 +1,108 @@
#!/usr/bin/python
import rclpy
from rclpy.action import ActionServer
from rclpy.node import Node
import numpy as np
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.executors import MultiThreadedExecutor
from geometry_msgs.msg import Pose, PoseStamped
from rbs_skill_interfaces.action import MoveitSendPose
class PoseSubscriber(Node):
def __init__(self, parent=None):
super().__init__('pose_subscriber')
self.parent = parent
self._sub = self.create_subscription(PoseStamped,
"/cartesian_motion_controller/current_pose",
self.parent.on_pose_callback, 1,
callback_group=self.parent._callback_group)
self.get_logger().info('PoseSubscriber node initialized')
class CartesianMoveToPose(Node):
def __init__(self):
super().__init__('cartesian_move_to_pose')
self._callback_group = ReentrantCallbackGroup()
self._action_server = ActionServer(
self,
MoveitSendPose,
'cartesian_move_to_pose',
self.execute_callback, callback_group=self._callback_group)
self._pub = self.create_publisher(PoseStamped,
"/cartesian_motion_controller/target_frame", 1,
callback_group=self._callback_group)
self.current_pose = None
self.goal_tolerance = 0.05
def on_pose_callback(self, msg: PoseStamped):
if isinstance(msg, PoseStamped):
self.current_pose = msg
def execute_callback(self, goal_handle):
self.get_logger().debug(f"Executing goal {goal_handle.request.target_pose}")
tp = PoseStamped()
tp.pose = goal_handle.request.target_pose
tp.header.stamp = self.get_clock().now().to_msg()
tp.header.frame_id = "base_link"
while self.get_distance_to_target(tp.pose) >= self.goal_tolerance:
self._pub.publish(tp)
goal_handle.succeed()
result = MoveitSendPose.Result()
result.success = True
return result
def get_distance_to_target(self, target_pose: Pose):
if self.current_pose is None or self.current_pose.pose is None:
self.get_logger().warn("Current pose is not available")
return None
current_pose = self.current_pose.pose
current_position = np.array([
current_pose.position.x,
current_pose.position.y,
current_pose.position.z,
current_pose.orientation.x,
current_pose.orientation.y,
current_pose.orientation.z
])
target_position = np.array([
target_pose.position.x,
target_pose.position.y,
target_pose.position.z,
target_pose.orientation.x,
target_pose.orientation.y,
target_pose.orientation.z
])
# Проверка на наличие значений в массивах координат
if np.any(np.isnan(current_position)) or np.any(np.isnan(target_position)):
self.get_logger().error("Invalid coordinates")
return None
# Вычисляем расстояние между текущей и целевой позициями
distance = np.linalg.norm(current_position - target_position)
return distance
def main(args=None):
rclpy.init(args=args)
cartesian_move_to_pose = CartesianMoveToPose()
pose_subscriber = PoseSubscriber(parent=cartesian_move_to_pose)
executor = MultiThreadedExecutor()
executor.add_node(cartesian_move_to_pose)
executor.add_node(pose_subscriber)
executor.spin()
rclpy.shutdown()
if __name__ == '__main__':
main()

48
rbs_skill_servers/scripts/test_cartesian_controller.py Executable file
View file

@ -0,0 +1,48 @@
#!/usr/bin/python
import rclpy
from rclpy.node import Node
import argparse
from geometry_msgs.msg import PoseStamped
class CartesianControllerPublisher(Node):
def __init__(self, robot_name: str):
super().__init__("cartesian_controller_pose_publisher")
self.publisher_ = self.create_publisher(
PoseStamped,
"/" + robot_name + "/cartesian_motion_controller/target_frame", 10)
timer_period = 0.5 # seconds
self.timer = self.create_timer(timer_period, self.timer_callback)
def timer_callback(self):
msg = PoseStamped()
msg.header.stamp = self.get_clock().now().to_msg()
msg.header.frame_id = "base_link"
msg.pose.position.x = 0.7
msg.pose.position.y = 0.0
msg.pose.position.z = 0.45
msg.pose.orientation.x = 0.0
msg.pose.orientation.y = 0.707
msg.pose.orientation.z = 0.0
msg.pose.orientation.w = 0.707
self.publisher_.publish(msg)
def main(args=None):
rclpy.init(args=args)
parser = argparse.ArgumentParser(description='ROS2 Minimal Publisher')
parser.add_argument('--robot-name', type=str, default='arm0', help='Specify the robot name')
args = parser.parse_args()
minimal_publisher = CartesianControllerPublisher(args.robot_name)
rclpy.spin(minimal_publisher)
minimal_publisher.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

0
rbs_skill_servers/scripts/test_cartesian_force.py Normal file
View file

69
rbs_skill_servers/scripts/test_move_to_pose.py Normal file
View file

@ -0,0 +1,69 @@
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
from rbs_skill_interfaces.action import MoveitSendPose
class TestMoveToPose(Node):
def __init__(self, robot_name: str):
super().__init__('test_move_to_pose')
self._action_client = ActionClient(self, MoveitSendPose, "/"+ robot_name + "/move_topose")
def send_goal(self, msg):
goal_msg = MoveitSendPose.Goal()
# goal_msg.target_pose
goal_msg.target_pose.position.x = msg["target_pose"]["position"]["x"]
goal_msg.target_pose.position.y = msg["target_pose"]["position"]["y"]
goal_msg.target_pose.position.z = msg["target_pose"]["position"]["z"]
goal_msg.target_pose.orientation.x = msg["target_pose"]["orientation"]["x"]
goal_msg.target_pose.orientation.y = msg["target_pose"]["orientation"]["y"]
goal_msg.target_pose.orientation.z = msg["target_pose"]["orientation"]["z"]
goal_msg.target_pose.orientation.w = msg["target_pose"]["orientation"]["w"]
goal_msg.robot_name = msg["robot_name"]
goal_msg.end_effector_velocity = msg["end_effector_velocity"]
goal_msg.end_effector_acceleration = msg["end_effector_acceleration"]
self._action_client.wait_for_server()
return self._action_client.send_goal_async(goal_msg)
def main(args=None):
rclpy.init(args=args)
robot_name = "arm0"
action_client = TestMoveToPose(robot_name)
msg = {
"constraint_mode": 0,
"target_pose": {
"position": {
"x": 0.2,
"y": 0.2,
"z": 0.35,
},
"orientation": {
"x": 0.0,
"y": 0.0,
"z": 0.0,
"w": 1.0,
},
},
"robot_name": robot_name,
"end_effector_velocity": 1.0,
"end_effector_acceleration": 1.0,
"timeout_seconds": 0.0
}
future = action_client.send_goal(msg)
rclpy.spin_until_future_complete(action_client, future)
if __name__ == '__main__':
main()

9
rbs_skill_servers/src/gripper_control_action_server.cpp
View file

@ -31,7 +31,7 @@ public:
std::bind(&GripperControlActionServer::accepted_callback, this,
std::placeholders::_1));
publisher = this->create_publisher<std_msgs::msg::Float64MultiArray>(
"/gripper_controller/commands", 10);
"~/gripper_controller/commands", 10);
}
private:
@ -96,10 +96,3 @@ private:
} // namespace rbs_skill_actions
RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::GripperControlActionServer);
// int main(int argc, char ** argv) {
// rclcpp::init(argc, argv);
// rbs_skill_actions::GripperControlActionServer server;
// rclcpp::spin(server);
// return 0;
// }

3
rbs_skill_servers/src/move_to_joint_states_action_server.cpp
View file

@ -148,7 +148,8 @@ int main(int argc, char **argv) {
rclcpp::init(argc, argv);
rclcpp::NodeOptions node_options;
node_options.automatically_declare_parameters_from_overrides(true);
auto node = rclcpp::Node::make_shared("move_topose", "", node_options);
auto node =
rclcpp::Node::make_shared("move_to_joint_states", "", node_options);
rbs_skill_actions::MoveToJointStateActionServer server(node);
std::thread run_server([&server]() {

3
rbs_skill_servers/src/pick_place_pose_loader.cpp
View file

@ -7,7 +7,6 @@
#include <fstream>
#include <geometry_msgs/msg/detail/pose__struct.hpp>
#include <memory>
#include <nlohmann/json_fwd.hpp>
#include <rclcpp/logging.hpp>
#include <sdf/Geometry.hh>
#include <tf2_eigen/tf2_eigen.hpp>
@ -119,4 +118,4 @@ std::vector<geometry_msgs::msg::Pose> GetGraspPlacePoseServer::collectPose(
poses.push_back(tf2::toMsg(postGraspPose));
return poses;
}
}

7
rbs_utils/CMakeLists.txt → rbs_utils/rbs_utils/CMakeLists.txt
View file

@ -13,6 +13,9 @@ find_package(tf2_ros REQUIRED)
find_package(tf2_eigen REQUIRED)
find_package(rviz_visual_tools REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(rbs_utils_interfaces REQUIRED)
find_package(dynmsg REQUIRED)
set(deps
rclcpp
@ -21,10 +24,10 @@ set(deps
tf2_eigen
rviz_visual_tools
geometry_msgs
rbs_utils_interfaces
dynmsg
)
add_library(${PROJECT_NAME} SHARED src/rbs_utils.cpp)
install(

0
rbs_utils/LICENSE → rbs_utils/rbs_utils/LICENSE
View file

70
rbs_utils/include/rbs_utils/rbs_utils.hpp → rbs_utils/rbs_utils/include/rbs_utils/rbs_utils.hpp
View file

@ -1,18 +1,28 @@
#include <Eigen/Core>
#include "dynmsg/message_reading.hpp"
#include "dynmsg/msg_parser.hpp"
#include "dynmsg/typesupport.hpp"
#include "dynmsg/yaml_utils.hpp"
#include "geometry_msgs/msg/pose.hpp"
#include "geometry_msgs/msg/pose_array.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "rviz_visual_tools/rviz_visual_tools.hpp"
#include "tf2_msgs/msg/tf_message.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_msgs/msg/tf_message.hpp"
#include "tf2_ros/buffer.h"
#include <tf2/convert.h>
#include "tf2_ros/static_transform_broadcaster.h"
#include <Eigen/Core>
#include <fstream>
#include <nlohmann/json.hpp>
#include <geometry_msgs/msg/detail/pose__struct.hpp>
#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
#include <rbs_utils_interfaces/msg/detail/assembly_config__struct.hpp>
#include <rclcpp/clock.hpp>
#include <rclcpp/logger.hpp>
#include <rclcpp/node.hpp>
#include <rclcpp/node_interfaces/node_clock_interface.hpp>
#include <rclcpp/time.hpp>
#include <tf2/convert.h>
#include <tf2_eigen/tf2_eigen.hpp>
#include <tf2_msgs/msg/detail/tf_message__struct.hpp>
#include <unordered_map>
const std::string env_dir = std::getenv("RBS_ASSEMBLY_DIR");
@ -27,64 +37,48 @@ struct EnvModel {
double mass;
};
struct Workspace {
std::string name;
geometry_msgs::msg::PoseArray poses;
};
class AssemblyConfigLoader {
public:
template <typename NodePtr>
explicit AssemblyConfigLoader(const std::string &t_assembly_dir,
const NodePtr &t_node)
: AssemblyConfigLoader(t_assembly_dir,
t_node->get_node_logging_interface()) {}
t_node->get_node_logging_interface(),
t_node->get_node_clock_interface()) {}
explicit AssemblyConfigLoader(
const std::string &t_assembly_dir,
const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr
&t_logger_node_interface);
inline std::shared_ptr<std::unordered_map<std::string, nlohmann::json>>
getAssemblyFileData() {
return m_env_vars;
}
&t_logger_node_interface,
const rclcpp::node_interfaces::NodeClockInterface::SharedPtr
&t_clock_node_interface);
tf2_msgs::msg::TFMessage getAllPossibleTfData();
tf2_msgs::msg::TFMessage getTfData(const std::string &model_name);
inline std::shared_ptr<std::vector<EnvModel>> getEnvModels() {
return m_env_models;
}
tf2_msgs::msg::TFMessage getGraspTfData(const std::string &model_name);
std::vector<std::string> getSceneModelNames();
std::vector<std::string> getUniqueSceneModelNames();
void printWorkspace();
geometry_msgs::msg::PoseArray getWorkspaceInspectorTrajectory();
geometry_msgs::msg::Pose
transformTrajectory(const geometry_msgs::msg::TransformStamped &pose);
transformTrajectory(const geometry_msgs::msg::Pose &pose);
void saveRbsConfig();
tf2_msgs::msg::TFMessage getAdditionalPoses();
private:
std::shared_ptr<std::unordered_map<std::string, nlohmann::json>> m_env_vars;
std::vector<std::string> m_env_files;
std::vector<std::ifstream> m_env_paths;
std::shared_ptr<std::vector<EnvModel>> m_env_models{};
std::string m_assembly_dir;
std::unique_ptr<tf2_ros::Buffer> m_tf_buffer;
// rviz_visual_tools::RvizVisualToolsPtr m_visual_tools;
rbs_utils_interfaces::msg::AssemblyConfig m_assembly_config;
rclcpp::Logger m_logger;
void readAssemblyFileData(const std::string &filename,
const std::string &filepath);
rclcpp::Clock::SharedPtr m_clock;
void parseRbsDb(const std::string &filepath);
tf2_msgs::msg::TFMessage readWorkspaceJson(const nlohmann::json &json);
tf2_msgs::msg::TFMessage parseJsonToTFMessage(const nlohmann::json &json,
const std::string &model_name);
void publishWorkspace(const tf2_msgs::msg::TFMessage &tf_msg);
void setTfFromDb(const std::string &filename);
double convertToDouble(const nlohmann::json &value);
void readModelConfigs();
EnvModel parseModelData(const std::string &jsonFilePath);
geometry_msgs::msg::PoseArray getWorkspace();
geometry_msgs::msg::Transform
createTransform(const geometry_msgs::msg::Pose &pose);
};
class StaticFramePublisher : public rclcpp::Node {

11
rbs_utils/package.xml → rbs_utils/rbs_utils/package.xml
View file

@ -8,7 +8,18 @@
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>rclcpp_lifecycle</depend>
<depend>tf2_ros</depend>
<depend>tf2_eigen</depend>
<depend>rviz_visual_tools</depend>
<depend>geometry_msgs</depend>
<depend>rosidl_default_generators</depend>
<depend>std_msgs</depend>
<depend>rbs_utils_interfaces</depend>
<depend>dynmsg</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

297
rbs_utils/rbs_utils/src/rbs_utils.cpp Normal file
View file

@ -0,0 +1,297 @@
#include "rbs_utils/rbs_utils.hpp"
#include <Eigen/src/Geometry/Transform.h>
#include <dynmsg/typesupport.hpp>
#include <fstream>
#include <geometry_msgs/msg/detail/pose__struct.hpp>
#include <geometry_msgs/msg/detail/transform_stamped__struct.hpp>
#include <rbs_utils_interfaces/msg/detail/assembly_config__struct.hpp>
#include <rbs_utils_interfaces/msg/detail/assembly_config__traits.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp/node_interfaces/node_clock_interface.hpp>
#include <string>
#include <strstream>
#include <tf2/LinearMath/Transform.h>
#include <tf2/convert.h>
#include <tf2_eigen/tf2_eigen.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2_msgs/msg/detail/tf_message__struct.hpp>
#include <tf2_ros/buffer_interface.h>
#include <yaml-cpp/emitter.h>
#include <yaml-cpp/node/parse.h>
namespace rbs_utils {
AssemblyConfigLoader::AssemblyConfigLoader(
const std::string &t_assembly_dir,
const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr
&t_logging_interface,
const rclcpp::node_interfaces::NodeClockInterface::SharedPtr
&t_clock_interface)
: m_assembly_dir(t_assembly_dir),
m_logger(t_logging_interface->get_logger()),
m_clock(t_clock_interface->get_clock()) {
if (!m_assembly_dir.empty()) {
std::vector<std::string> filenames = {"rbs_db"};
for (auto &filename : filenames) {
std::string filepath =
env_dir + "/" + m_assembly_dir + "/" + filename + ".yaml";
m_env_files.push_back(filepath);
parseRbsDb(filepath);
}
} else {
RCLCPP_ERROR(m_logger, "Assembly dir is not set");
}
}
void AssemblyConfigLoader::parseRbsDb(const std::string &filepath) {
try {
YAML::Node asm_config = YAML::LoadFile(filepath);
std::string asm_config_string = dynmsg::yaml_to_string(asm_config);
RosMessage_Cpp rosmsg;
InterfaceTypeName interface{"rbs_utils_interfaces", "AssemblyConfig"};
rosmsg.type_info = dynmsg::cpp::get_type_info(interface);
void *ros_msg = reinterpret_cast<void *>(&m_assembly_config);
dynmsg::cpp::yaml_and_typeinfo_to_rosmsg(rosmsg.type_info,
asm_config_string, ros_msg);
} catch (const std::exception &e) {
RCLCPP_ERROR(m_logger, "Exception reading file %s: %s", filepath.c_str(),
e.what());
}
}
[[deprecated("Not Implemented")]] void AssemblyConfigLoader::saveRbsConfig() {}
std::vector<std::string> AssemblyConfigLoader::getUniqueSceneModelNames() {
std::vector<std::string> model_names;
if (m_assembly_config.relative_part.size() != 0) {
for (auto &t : m_assembly_config.relative_part) {
model_names.push_back(t.name);
}
} else {
return model_names;
}
// Sort and remove duplicates
std::sort(model_names.begin(), model_names.end());
model_names.erase(std::unique(model_names.begin(), model_names.end()),
model_names.end());
return model_names;
}
tf2_msgs::msg::TFMessage AssemblyConfigLoader::getAllPossibleTfData() {
tf2_msgs::msg::TFMessage tp;
// Get absolute parts
for (auto &abs_poses : m_assembly_config.absolute_part) {
geometry_msgs::msg::TransformStamped tmp;
tmp.transform = createTransform(abs_poses.pose);
tmp.child_frame_id = abs_poses.name;
tmp.header.frame_id = "world";
tmp.header.stamp = m_clock->now();
tp.transforms.push_back(tmp);
}
// Get relative parts
for (const auto &relative_part : m_assembly_config.relative_part) {
geometry_msgs::msg::TransformStamped tmp;
tmp.transform = createTransform(relative_part.pose);
tmp.child_frame_id = relative_part.name;
tmp.header.frame_id = relative_part.relative_at;
tmp.header.stamp = m_clock->now();
tp.transforms.push_back(tmp);
RCLCPP_INFO(m_logger, "Model name [%s]", relative_part.name.c_str());
}
// Get grasp poses
for (const auto &grasp_pose : m_assembly_config.grasp_pose) {
geometry_msgs::msg::TransformStamped tmp;
tmp.transform = createTransform(grasp_pose.pose);
tmp.child_frame_id = grasp_pose.name;
tmp.header.frame_id = grasp_pose.relative_at;
tmp.header.stamp = m_clock->now();
tp.transforms.push_back(tmp);
}
return tp;
}
tf2_msgs::msg::TFMessage
AssemblyConfigLoader::getGraspTfData(const std::string &model_name) {
tf2_msgs::msg::TFMessage tp;
bool found_grasp_pose = false;
if (!m_assembly_config.relative_part.empty()) {
for (auto &abs_poses : m_assembly_config.absolute_part) {
geometry_msgs::msg::TransformStamped tmp;
tmp.transform = createTransform(abs_poses.pose);
tmp.child_frame_id = abs_poses.name;
tmp.header.frame_id = "world";
tmp.header.stamp = m_clock->now();
tp.transforms.push_back(tmp);
}
} else {
RCLCPP_ERROR(m_logger, "Relative parts is empty size: %zu",
m_assembly_config.relative_part.size());
}
for (const auto &grasp_pose : m_assembly_config.grasp_pose) {
if (grasp_pose.relative_at == model_name) {
geometry_msgs::msg::TransformStamped tmp;
tmp.transform = createTransform(grasp_pose.pose);
tmp.child_frame_id = grasp_pose.name;
tmp.header.frame_id = grasp_pose.relative_at;
tmp.header.stamp = m_clock->now();
tp.transforms.push_back(tmp);
found_grasp_pose = true;
}
}
if (!found_grasp_pose) {
RCLCPP_ERROR(m_logger, "Grasp pose not found for model %s",
model_name.c_str());
}
return tp;
}
tf2_msgs::msg::TFMessage
AssemblyConfigLoader::getTfData(const std::string &model_name) {
tf2_msgs::msg::TFMessage tp;
if (!m_assembly_config.absolute_part.empty()) {
for (auto &abs_part : m_assembly_config.absolute_part) {
geometry_msgs::msg::TransformStamped abs_transrorm_stamped;
abs_transrorm_stamped.transform = createTransform(abs_part.pose);
abs_transrorm_stamped.child_frame_id = abs_part.name;
abs_transrorm_stamped.header.frame_id = "world";
abs_transrorm_stamped.header.stamp = m_clock->now();
tp.transforms.push_back(abs_transrorm_stamped);
}
} else {
RCLCPP_ERROR(m_logger, "Absolute parts is empty size: %zu",
m_assembly_config.absolute_part.size());
}
bool found_model = false;
bool found_grasp_pose = false;
if (!m_assembly_config.relative_part.empty()) {
for (auto &abs_poses : m_assembly_config.absolute_part) {
geometry_msgs::msg::TransformStamped tmp;
tmp.transform = createTransform(abs_poses.pose);
tmp.child_frame_id = abs_poses.name;
tmp.header.frame_id = "world";
tmp.header.stamp = m_clock->now();
tp.transforms.push_back(tmp);
}
for (const auto &relative_part : m_assembly_config.relative_part) {
// Find our model data
if (relative_part.name == model_name) {
geometry_msgs::msg::TransformStamped tmp;
tmp.transform = createTransform(relative_part.pose);
tmp.child_frame_id = relative_part.name;
tmp.header.frame_id = relative_part.relative_at;
tmp.header.stamp = m_clock->now();
tp.transforms.push_back(tmp);
found_model = true;
}
RCLCPP_INFO(m_logger, "Model name [%s]",
relative_part.relative_at.c_str());
}
} else {
RCLCPP_ERROR(m_logger, "Relative parts is empty size: %zu",
m_assembly_config.relative_part.size());
}
for (const auto &grasp_pose : m_assembly_config.grasp_pose) {
if (grasp_pose.relative_at == model_name) {
geometry_msgs::msg::TransformStamped tmp;
tmp.transform = createTransform(grasp_pose.pose);
tmp.child_frame_id = grasp_pose.name;
tmp.header.frame_id = grasp_pose.relative_at;
tmp.header.stamp = m_clock->now();
tp.transforms.push_back(tmp);
found_grasp_pose = true;
}
}
if (!found_model) {
RCLCPP_ERROR(m_logger, "Model %s not found in config", model_name.c_str());
}
if (!found_grasp_pose) {
RCLCPP_ERROR(m_logger, "Grasp pose not found for model %s",
model_name.c_str());
}
return tp;
}
geometry_msgs::msg::Transform
AssemblyConfigLoader::createTransform(const geometry_msgs::msg::Pose &pose) {
geometry_msgs::msg::Transform transform;
transform.translation.x = pose.position.x;
transform.translation.y = pose.position.y;
transform.translation.z = pose.position.z;
transform.rotation.x = pose.orientation.x;
transform.rotation.y = pose.orientation.y;
transform.rotation.z = pose.orientation.z;
transform.rotation.w = pose.orientation.w;
return transform;
}
geometry_msgs::msg::PoseArray AssemblyConfigLoader::getWorkspace() {
geometry_msgs::msg::PoseArray pose_array;
pose_array.header.frame_id = "world";
if (m_assembly_config.workspace.empty()) {
RCLCPP_WARN(m_logger, "Workspace is empty, check your robossembler_db");
return pose_array;
}
pose_array.poses.reserve(m_assembly_config.workspace.size());
const double default_rotation_value = 0.0;
const double default_rotation_w = 1.0;
for (const auto &point : m_assembly_config.workspace) {
geometry_msgs::msg::Pose pose;
pose.position.x = point.x;
pose.position.y = point.y;
pose.position.z = point.z;
pose.orientation.x = default_rotation_value;
pose.orientation.y = default_rotation_value;
pose.orientation.z = default_rotation_value;
pose.orientation.w = default_rotation_w;
pose_array.poses.push_back(pose);
}
return pose_array;
}
geometry_msgs::msg::PoseArray
AssemblyConfigLoader::getWorkspaceInspectorTrajectory() {
geometry_msgs::msg::PoseArray pose_array;
pose_array.header.frame_id = "world";
auto workspace_poses = getWorkspace();
for (const auto &pose : workspace_poses.poses) {
pose_array.poses.push_back(transformTrajectory(pose));
}
return pose_array;
}
geometry_msgs::msg::Pose AssemblyConfigLoader::transformTrajectory(
const geometry_msgs::msg::Pose &pose) {
Eigen::Isometry3d pose_eigen;
tf2::fromMsg(pose, pose_eigen);
Eigen::AngleAxisd rotation(M_PI, Eigen::Vector3d::UnitY());
pose_eigen.rotate(rotation);
pose_eigen.translation().z() += 0.35;
return tf2::toMsg(pose_eigen);
}
} // namespace rbs_utils

38
rbs_utils/rbs_utils_interfaces/CMakeLists.txt Normal file
View file

@ -0,0 +1,38 @@
cmake_minimum_required(VERSION 3.8)
project(rbs_utils_interfaces)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(std_msgs REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/AssemblyConfig.msg"
"msg/NamedPose.msg"
"msg/RelativeNamedPose.msg"
DEPENDENCIES std_msgs geometry_msgs
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# comment the line when this package is in a git repo and when
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()

202
rbs_utils/rbs_utils_interfaces/LICENSE Normal file
View file

@ -0,0 +1,202 @@
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.

7
rbs_utils/rbs_utils_interfaces/msg/AssemblyConfig.msg Normal file
View file

@ -0,0 +1,7 @@
string assets_db
geometry_msgs/Point[] workspace
rbs_utils_interfaces/NamedPose[] absolute_part
rbs_utils_interfaces/RelativeNamedPose[] relative_part
rbs_utils_interfaces/RelativeNamedPose[] grasp_pose
rbs_utils_interfaces/NamedPose[] extra_poses

2
rbs_utils/rbs_utils_interfaces/msg/NamedPose.msg Normal file
View file

@ -0,0 +1,2 @@
string name
geometry_msgs/Pose pose

3
rbs_utils/rbs_utils_interfaces/msg/RelativeNamedPose.msg Normal file
View file

@ -0,0 +1,3 @@
string name
string relative_at
geometry_msgs/Pose pose

19
rbs_utils/rbs_utils_interfaces/package.xml Normal file
View file

@ -0,0 +1,19 @@
<?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_utils_interfaces</name>
<version>0.0.0</version>
<description>Interfaces for read robossembler_db config</description>
<maintainer email="ur.narmak@gmail.com">bill-finger</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

386
rbs_utils/src/rbs_utils.cpp
View file

@ -1,386 +0,0 @@
#include "rbs_utils/rbs_utils.hpp"
#include <geometry_msgs/msg/detail/pose__struct.hpp>
#include <tf2/LinearMath/Transform.h>
#include <tf2/convert.h>
#include <tf2_eigen/tf2_eigen.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2_ros/buffer_interface.h>
namespace rbs_utils {
AssemblyConfigLoader::AssemblyConfigLoader(
const std::string &t_assembly_dir,
const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr
&t_logging_interface)
: m_env_vars(
std::make_shared<std::unordered_map<std::string, nlohmann::json>>()),
m_env_models(std::make_shared<std::vector<EnvModel>>()),
m_assembly_dir(t_assembly_dir),
m_logger(t_logging_interface->get_logger()) {
// m_visual_tools.reset(new
// rviz_visual_tools::RvizVisualTools("world","/rviz_visual_markers",
// m_node));
if (!m_assembly_dir.empty()) {
std::vector<std::string> filenames = {"robossembler_db", "sequences"};
for (auto &filename : filenames) {
std::string filepath =
env_dir + "/" + m_assembly_dir + "/" + filename + ".json";
m_env_files.push_back(filepath);
readAssemblyFileData(filename, filepath);
}
readModelConfigs();
} else {
RCLCPP_ERROR(m_logger, "Assembly dir is not set");
}
}
void AssemblyConfigLoader::readAssemblyFileData(const std::string &filename,
const std::string &filepath) {
try {
std::ifstream i(filepath);
if (!i.is_open()) {
RCLCPP_ERROR(m_logger, "Failed to open file: %s", filepath.c_str());
return;
}
nlohmann::json json = nlohmann::json::parse(i);
m_env_vars->insert({filename, json});
} catch (const nlohmann::json::parse_error &e) {
RCLCPP_ERROR(m_logger, "Error parsing JSON in file %s: %s",
filepath.c_str(), e.what());
} catch (const std::exception &e) {
RCLCPP_ERROR(m_logger, "Exception reading file %s: %s", filepath.c_str(),
e.what());
}
}
EnvModel
AssemblyConfigLoader::parseModelData(const std::string &json_file_path) {
nlohmann::json json_content{};
try {
std::ifstream file(json_file_path);
if (!file.is_open()) {
RCLCPP_ERROR(m_logger, "Failed to open file: %s", json_file_path.c_str());
return EnvModel{};
}
json_content = nlohmann::json::parse(file);
} catch (const nlohmann::json::parse_error &e) {
RCLCPP_ERROR(m_logger, "Error parsing JSON in file %s: %s",
json_file_path.c_str(), e.what());
} catch (const std::exception &e) {
RCLCPP_ERROR(m_logger, "Exception reading file %s: %s",
json_file_path.c_str(), e.what());
}
EnvModel envModel;
if (!json_content.empty()) {
envModel.model_name = json_content.at("name").get<std::string>();
envModel.mesh_path = env_dir + "/" + m_assembly_dir + "/" +
json_content.at("stl").get<std::string>();
envModel.model_inertia = {convertToDouble(json_content.at("ixx")),
convertToDouble(json_content.at("ixy")),
convertToDouble(json_content.at("iyy")),
convertToDouble(json_content.at("ixz")),
convertToDouble(json_content.at("izz")),
convertToDouble(json_content.at("iyz"))};
envModel.model_pose = {convertToDouble(json_content.at("posX")),
convertToDouble(json_content.at("posY")),
convertToDouble(json_content.at("posZ")),
convertToDouble(json_content.at("eulerX")),
convertToDouble(json_content.at("eulerY")),
convertToDouble(json_content.at("eulerZ"))};
envModel.mass = convertToDouble(json_content.at("massSDF"));
}
return envModel;
}
void AssemblyConfigLoader::readModelConfigs() {
if (m_env_vars->find("robossembler_db") != m_env_vars->end()) {
const nlohmann::json &db_json = (*m_env_vars)["robossembler_db"];
const std::string assets_folder =
env_dir + "/" + m_assembly_dir + "/" +
db_json.at("assets_db").get<std::string>();
for (const auto &entry :
std::filesystem::directory_iterator(assets_folder)) {
if (entry.is_regular_file() && entry.path().extension() == ".json") {
EnvModel model = parseModelData(entry.path().string());
m_env_models->push_back(model);
}
}
}
}
std::vector<std::string> AssemblyConfigLoader::getSceneModelNames() {
std::vector<std::string> model_names;
if (m_env_vars->find("robossembler_db") != m_env_vars->end()) {
const nlohmann::json &json = (*m_env_vars)["robossembler_db"];
// Extract names from "relativeParts"
if (json.find("relativeParts") != json.end()) {
const auto &relative_parts = json["relativeParts"];
for (const auto &part : relative_parts) {
model_names.push_back(part["name"]);
}
}
// Extract names from "graspPoseModels"
if (json.find("graspPoseModels") != json.end()) {
const auto &grasp_pose_models = json["graspPoseModels"];
for (const auto &pose_model : grasp_pose_models) {
model_names.push_back(pose_model["name"]);
}
}
} else {
RCLCPP_ERROR(m_logger, "Key 'robossembler_db' not found in m_env_vars.");
return model_names;
}
// Sort and remove duplicates
std::sort(model_names.begin(), model_names.end());
model_names.erase(std::unique(model_names.begin(), model_names.end()),
model_names.end());
return model_names;
}
tf2_msgs::msg::TFMessage
AssemblyConfigLoader::getTfData(const std::string &model_name) {
tf2_msgs::msg::TFMessage tf_msg{};
if (m_env_vars->find("robossembler_db") != m_env_vars->end()) {
const nlohmann::json &json = (*m_env_vars)["robossembler_db"];
tf_msg = parseJsonToTFMessage(json, model_name);
// Output position information to console
for (const auto &transform : tf_msg.transforms) {
RCLCPP_INFO_STREAM(
m_logger,
"Frame ID: " << transform.header.frame_id << ", Child Frame ID: "
<< transform.child_frame_id << ", Translation: ["
<< transform.transform.translation.x << ", "
<< transform.transform.translation.y << ", "
<< transform.transform.translation.z << "]");
}
// auto r = std::make_shared<StaticFramePublisher>(tf_msg);
} else {
RCLCPP_ERROR(m_logger, "Key 'robossembler_db' not found in m_env_vars.");
}
return tf_msg;
}
tf2_msgs::msg::TFMessage
AssemblyConfigLoader::parseJsonToTFMessage(const nlohmann::json &json,
const std::string &model_name) {
tf2_msgs::msg::TFMessage tf_msg;
// Add absolute part to TFMessage
geometry_msgs::msg::TransformStamped absolute_transform;
absolute_transform.header.frame_id = "world";
absolute_transform.child_frame_id =
json.at("absolutePart").at("name").get<std::string>();
absolute_transform.transform.translation.x =
convertToDouble(json.at("absolutePart").at("position").at("x"));
absolute_transform.transform.translation.y =
convertToDouble(json.at("absolutePart").at("position").at("y"));
absolute_transform.transform.translation.z =
convertToDouble(json.at("absolutePart").at("position").at("z"));
absolute_transform.transform.rotation.w =
convertToDouble(json.at("absolutePart").at("quaternion").at("qw"));
absolute_transform.transform.rotation.x =
convertToDouble(json.at("absolutePart").at("quaternion").at("qx"));
absolute_transform.transform.rotation.y =
convertToDouble(json.at("absolutePart").at("quaternion").at("qy"));
absolute_transform.transform.rotation.z =
convertToDouble(json.at("absolutePart").at("quaternion").at("qz"));
tf_msg.transforms.push_back(absolute_transform);
// Add relative parts to TFMessage
for (const auto &relative_part : json.at("relativeParts")) {
if (relative_part.at("name").get<std::string>() == model_name) {
geometry_msgs::msg::TransformStamped relative_transform;
relative_transform.header.frame_id =
relative_part.at("relativeAt").get<std::string>();
relative_transform.child_frame_id =
relative_part.at("name").get<std::string>() + "_target";
relative_transform.transform.translation.x =
convertToDouble(relative_part.at("position").at("x"));
relative_transform.transform.translation.y =
convertToDouble(relative_part.at("position").at("y"));
relative_transform.transform.translation.z =
convertToDouble(relative_part.at("position").at("z"));
relative_transform.transform.rotation.w =
convertToDouble(relative_part.at("quaternion").at("qw"));
relative_transform.transform.rotation.x =
convertToDouble(relative_part.at("quaternion").at("qx"));
relative_transform.transform.rotation.y =
convertToDouble(relative_part.at("quaternion").at("qy"));
relative_transform.transform.rotation.z =
convertToDouble(relative_part.at("quaternion").at("qz"));
tf_msg.transforms.push_back(relative_transform);
}
}
// Add grasp pose models to TFMessage
for (const auto &grasp_pose : json.at("graspPoseModels")) {
if (grasp_pose.at("name").get<std::string>() == model_name) {
geometry_msgs::msg::TransformStamped grasp_transform;
grasp_transform.header.frame_id =
grasp_pose.at("name").get<std::string>() + "_target";
grasp_transform.child_frame_id =
grasp_pose.at("name").get<std::string>() + "_target" + "_grasp";
grasp_transform.transform.translation.x =
convertToDouble(grasp_pose.at("position").at("x"));
grasp_transform.transform.translation.y =
convertToDouble(grasp_pose.at("position").at("y"));
grasp_transform.transform.translation.z =
convertToDouble(grasp_pose.at("position").at("z"));
grasp_transform.transform.rotation.w =
convertToDouble(grasp_pose.at("quaternion").at("qw"));
grasp_transform.transform.rotation.x =
convertToDouble(grasp_pose.at("quaternion").at("qx"));
grasp_transform.transform.rotation.y =
convertToDouble(grasp_pose.at("quaternion").at("qy"));
grasp_transform.transform.rotation.z =
convertToDouble(grasp_pose.at("quaternion").at("qz"));
tf_msg.transforms.push_back(grasp_transform);
}
}
return tf_msg;
}
tf2_msgs::msg::TFMessage
AssemblyConfigLoader::readWorkspaceJson(const nlohmann::json &json) {
tf2_msgs::msg::TFMessage tf_msg;
int i{0};
if (json.contains("workspace") && json.at("workspace").is_array()) {
for (auto &point : json.at("workspace")) {
if (point.contains("position") && point.at("position").is_object()) {
geometry_msgs::msg::TransformStamped workspace_point;
workspace_point.header.frame_id = "world";
workspace_point.child_frame_id = "bbox_" + std::to_string(i);
i++;
if (point["position"].contains("x") &&
point["position"].contains("y") &&
point["position"].contains("z")) {
workspace_point.transform.translation.x =
point["position"]["x"].get<double>();
workspace_point.transform.translation.y =
point["position"]["y"].get<double>();
workspace_point.transform.translation.z =
point["position"]["z"].get<double>();
tf_msg.transforms.push_back(workspace_point);
} else {
RCLCPP_ERROR(m_logger,
"Cannot find key 'x', 'y', 'z' in 'position' or 'name' "
"isn't found in key 'workspace'");
}
} else {
RCLCPP_ERROR(m_logger,
"Cannot find key 'position', 'name', or 'position' isn't "
"an object in key 'workspace'");
}
}
} else {
RCLCPP_ERROR(m_logger, "Cannot find key 'workspace' in robossembler_db");
}
return tf_msg;
}
void AssemblyConfigLoader::publishWorkspace(
const tf2_msgs::msg::TFMessage &tf_msg) {
// if (tf_msg.transforms.size() >= 4) {
// auto pose1 =
// Eigen::Vector3d(tf_msg.transforms[0].transform.translation.x,
// tf_msg.transforms[0].transform.translation.y,
// tf_msg.transforms[0].transform.translation.z);
//
// auto pose2 =
// Eigen::Vector3d(tf_msg.transforms[1].transform.translation.x,
// tf_msg.transforms[1].transform.translation.y,
// tf_msg.transforms[1].transform.translation.z);
//
// auto pose3 =
// Eigen::Vector3d(tf_msg.transforms[2].transform.translation.x,
// tf_msg.transforms[2].transform.translation.y,
// tf_msg.transforms[2].transform.translation.z);
//
// auto pose4 =
// Eigen::Vector3d(tf_msg.transforms[3].transform.translation.x,
// tf_msg.transforms[3].transform.translation.y,
// tf_msg.transforms[3].transform.translation.z);
//
// // Connect workspace points as box
// m_visual_tools->publishLine(pose1, pose2, rviz_visual_tools::GREEN);
// m_visual_tools->publishLine(pose2, pose3, rviz_visual_tools::GREEN);
// m_visual_tools->publishLine(pose3, pose4, rviz_visual_tools::GREEN);
// m_visual_tools->publishLine(pose4, pose1, rviz_visual_tools::GREEN);
// } else {
// RCLCPP_ERROR(m_logger,
// "Not enough points to form a square");
// }
}
void AssemblyConfigLoader::printWorkspace() {
if (m_env_vars->find("robossembler_db") != m_env_vars->end()) {
const nlohmann::json json = (*m_env_vars)["robossembler_db"];
if (json.contains("workspace")) {
auto workspace = readWorkspaceJson(json);
publishWorkspace(workspace);
} else {
RCLCPP_WARN(m_logger, "Workspace isn't set in robossembler_db");
}
} else {
RCLCPP_ERROR(m_logger, "Cannot find robossembler_db");
}
}
geometry_msgs::msg::PoseArray
AssemblyConfigLoader::getWorkspaceInspectorTrajectory() {
geometry_msgs::msg::PoseArray pose_array;
pose_array.header.frame_id = "world";
if (m_env_vars->find("robossembler_db") != m_env_vars->end()) {
const nlohmann::json json = (*m_env_vars)["robossembler_db"];
if (json.contains("workspace")) {
auto workspace = readWorkspaceJson(json);
for (auto &point : workspace.transforms) {
auto pose = transformTrajectory(point);
pose_array.poses.push_back(pose);
}
pose_array.poses.push_back(transformTrajectory(workspace.transforms[0]));
}
}
return pose_array;
}
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::UnitY());
pose_eigen.rotate(rotation);
pose_eigen.translation().z() += 0.35;
auto pose_msg = tf2::toMsg(pose_eigen);
return pose_msg;
}
double AssemblyConfigLoader::convertToDouble(const nlohmann::json &value) {
if (value.is_number()) {
return value.get<double>();
} else if (value.is_string()) {
try {
return std::stod(value.get<std::string>());
} catch (const std::exception &e) {
RCLCPP_ERROR(m_logger, "Error converting string to double: %s", e.what());
throw std::runtime_error("Error converting string to double");
}
}
throw std::runtime_error("Invalid JSON type for conversion to double");
}
} // namespace rbs_utils

2
repos/cartesian_controllers.repos
View file

@ -5,7 +5,7 @@ repositories:
version: ros2
ros2_control:
type: git
url: https://github.com/solid-sinusoid/gz_ros2_control.git
url: https://github.com/solid-sinusoid/ros2_control.git
version: gz-ros2-cartesian-controllers
gz_ros2_control:
type: git

4
repos/real.repos
View file

@ -15,3 +15,7 @@ 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
dynamic_message_introspection:
type: git
url: https://github.com/osrf/dynamic_message_introspection.git
version: main

4
repos/sim.rbs.repos
View file

@ -11,3 +11,7 @@ repositories:
type: git
url: https://github.com/solid-sinusoid/behavior_tree.git
version: master
dynamic_message_introspection:
type: git
url: https://github.com/osrf/dynamic_message_introspection.git
version: main

4
repos/sim.ur.repos
View file

@ -15,3 +15,7 @@ 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
dynamic_message_introspection:
type: git
url: https://github.com/osrf/dynamic_message_introspection.git
version: main