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

rbs_utils in env_manager and clear code

Browse source
This commit is contained in:
Ilya Uraev 2023-12-14 12:00:35 +03:00
parent 0588a5f6c6
commit 34c8961723
23 changed files with 536 additions and 237 deletions
Download patch file Download diff file Expand all files Collapse all files

2
Dockerfile
View file

@ -3,6 +3,8 @@ FROM osrf/ros:humble-desktop
ARG WSDIR=rbs_ws
ENV RBS_ASSEMBLY_DIR=/assembly
# COPY /home/bill-finger/assembly /assembly
ENV IGN_GAZEBO_RESOURCE_PATH=/${WSDIR}/install/rbs_simulation/share/rbs_simulation/
RUN apt update && apt install -y \

3
env_manager/env_interface/CMakeLists.txt
View file

@ -7,6 +7,9 @@ endif()
set(INCLUDE_DEPENDS
rclcpp_lifecycle
rbs_utils
tf2_ros
tf2_eigen
)
# find dependencies

5
env_manager/env_interface/include/env_interface/env_interface.hpp
View file

@ -1,4 +1,7 @@
#include "env_interface/env_interface_base.hpp"
#include "rbs_utils/rbs_utils.hpp"
#include <memory>
#include <vector>
namespace env_interface
{
@ -7,5 +10,7 @@ class EnvInterface : public EnvInterfaceBase
public:
EnvInterface() = default;
virtual ~EnvInterface() = default;
protected:
std::shared_ptr<std::vector<rbs_utils::EnvModels>> m_env_models;
};
} // namespace env_interface

1
env_manager/env_interface/package.xml
View file

@ -9,6 +9,7 @@
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rbs_utils</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

6
env_manager/env_interface/src/env_interface.cpp Normal file
View file

@ -0,0 +1,6 @@
#include "env_interface/env_interface.hpp"
namespace env_interface {
}

81
env_manager/env_manager/include/env_manager/env_manager.hpp
View file

@ -1,51 +1,53 @@
#ifndef __ENV_MANAGER_HPP__
#define __ENV_MANAGER_HPP__
#include "env_manager_interfaces/srv/configure_env.hpp"
#include "env_manager_interfaces/srv/load_env.hpp"
#include "env_manager_interfaces/srv/unload_env.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "env_manager_interfaces/srv/load_env.hpp"
#include "env_manager_interfaces/srv/configure_env.hpp"
#include "env_manager_interfaces/srv/unload_env.hpp"
#include "env_interface/env_interface.hpp"
#include "pluginlib/class_loader.hpp"
namespace env_manager
{
namespace env_manager {
using EnvInterface = env_interface::EnvInterface;
using EnvInterfaceSharedPtr = std::shared_ptr<env_interface::EnvInterface>;
using EnvStateReturnType = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
using EnvStateReturnType =
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
struct EnvSpec
{
struct EnvSpec {
std::string name;
std::string type;
EnvInterfaceSharedPtr env_ptr;
};
class EnvManager : public rclcpp::Node
{
class EnvManager : public rclcpp::Node {
public:
// EnvManager(rclcpp::Executor::SharedPtr executor,
// const std::string & json_config_path,
// const std::string & node_name = "env_manager",
// const std::string & node_namespace = "",
// rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()
// .allow_undeclared_parameters(true)
// .automatically_declare_parameters_from_overrides(true));
EnvManager(rclcpp::Executor::SharedPtr executor, const std::string& node_name = "env_manager",
const std::string& node_namespace = "",
rclcpp::NodeOptions& node_options = rclcpp::NodeOptions()
.allow_undeclared_parameters(true)
.automatically_declare_parameters_from_overrides(true));
// const std::string &asm_config,
// const std::string &node_name = "env_manager",
// const std::string &node_namespace = "",
// rclcpp::NodeOptions &node_options =
// rclcpp::NodeOptions()
// .allow_undeclared_parameters(true)
// .automatically_declare_parameters_from_overrides(true));
EnvManager(rclcpp::Executor::SharedPtr executor,
const std::string &node_name = "env_manager",
const std::string &node_namespace = "",
rclcpp::NodeOptions &node_options =
rclcpp::NodeOptions()
.allow_undeclared_parameters(true)
.automatically_declare_parameters_from_overrides(true));
virtual ~EnvManager() = default;
// EnvInterfaceSharedPtr loadEnv(const std::string& env_name);
EnvInterfaceSharedPtr loadEnv(const std::string& env_name, const std::string& env_class_type);
EnvStateReturnType unloadEnv(const std::string& env_name);
EnvStateReturnType configureEnv(const std::string& env_name);
EnvInterfaceSharedPtr addEnv(const EnvSpec& enviment);
EnvInterfaceSharedPtr loadEnv(const std::string &env_name,
const std::string &env_class_type);
EnvStateReturnType unloadEnv(const std::string &env_name);
EnvStateReturnType configureEnv(const std::string &env_name);
EnvInterfaceSharedPtr addEnv(const EnvSpec &enviment);
// TODO: Define the input data format
// Set Target for RL enviroment
@ -60,27 +62,34 @@ public:
protected:
void initServices();
rclcpp::Node::SharedPtr getNode();
void loadEnv_cb(const env_manager_interfaces::srv::LoadEnv::Request::SharedPtr request,
env_manager_interfaces::srv::LoadEnv_Response::SharedPtr response);
void loadEnv_cb(
const env_manager_interfaces::srv::LoadEnv::Request::SharedPtr request,
env_manager_interfaces::srv::LoadEnv_Response::SharedPtr response);
void configureEnv_cb(const env_manager_interfaces::srv::ConfigureEnv::Request::SharedPtr request,
env_manager_interfaces::srv::ConfigureEnv_Response::SharedPtr response);
void configureEnv_cb(
const env_manager_interfaces::srv::ConfigureEnv::Request::SharedPtr
request,
env_manager_interfaces::srv::ConfigureEnv_Response::SharedPtr response);
void unloadEnv_cb(const env_manager_interfaces::srv::UnloadEnv::Request::SharedPtr request,
env_manager_interfaces::srv::UnloadEnv::Response::SharedPtr response);
void unloadEnv_cb(
const env_manager_interfaces::srv::UnloadEnv::Request::SharedPtr request,
env_manager_interfaces::srv::UnloadEnv::Response::SharedPtr response);
private:
rclcpp::Node::SharedPtr m_node;
std::mutex m_env_mutex;
std::vector<EnvSpec> m_active_envs;
rclcpp::Service<env_manager_interfaces::srv::LoadEnv>::SharedPtr m_load_env_srv;
rclcpp::Service<env_manager_interfaces::srv::ConfigureEnv>::SharedPtr m_configure_env_srv;
rclcpp::Service<env_manager_interfaces::srv::UnloadEnv>::SharedPtr m_unload_env_srv;
rclcpp::Service<env_manager_interfaces::srv::LoadEnv>::SharedPtr
m_load_env_srv;
rclcpp::Service<env_manager_interfaces::srv::ConfigureEnv>::SharedPtr
m_configure_env_srv;
rclcpp::Service<env_manager_interfaces::srv::UnloadEnv>::SharedPtr
m_unload_env_srv;
rclcpp::CallbackGroup::SharedPtr m_callback_group;
rclcpp::Executor::SharedPtr m_executor;
std::shared_ptr<pluginlib::ClassLoader<env_interface::EnvInterface>> m_loader;
};
} // namespace env_manager
#endif // __ENV_MANAGER_HPP__
} // namespace env_manager
#endif // __ENV_MANAGER_HPP__

27
env_manager/env_manager/src/env_manager.cpp
View file

@ -1,5 +1,6 @@
#include "env_manager/env_manager.hpp"
#include "nlohmann/json.hpp"
#include <string>
static constexpr const char *ENV_INTERFACE_BASE_CLASS_NAMESPACE =
"env_interface";
@ -19,18 +20,17 @@ EnvManager::EnvManager(rclcpp::Executor::SharedPtr executor,
initServices();
}
// EnvManager::EnvManager(
// rclcpp::Executor::SharedPtr executor,
// const std::string & json_config_path,
// const std::string & node_name,
// const std::string & node_namespace,
// rclcpp::NodeOptions & options)
// : rclcpp::Node(node_name, node_namespace, options),
// m_executor(executor),
// m_loader(std::make_shared<pluginlib::ClassLoader<rclcpp_lifecycle::LifecycleNode>>(
// ENV_INTERFACE_BASE_CLASS_NAMESPACE, ENV_INTERFACE_BASE_CLASS_NAME))
// {
// initServices();
// EnvManager::EnvManager(rclcpp::Executor::SharedPtr executor,
// const std::string &asm_config,
// const std::string &node_name,
// const std::string &node_namespace,
// rclcpp::NodeOptions &options)
// : rclcpp::Node(node_name, node_namespace, options), m_executor(executor),
// m_loader(
// std::make_shared<pluginlib::ClassLoader<env_interface::EnvInterface>>(
// ENV_INTERFACE_BASE_CLASS_NAMESPACE,
// ENV_INTERFACE_BASE_CLASS_NAME)) {
// initServices();
// }
void EnvManager::initServices() {
@ -128,6 +128,7 @@ EnvStateReturnType EnvManager::unloadEnv(const std::string &env_name) {
[&env_name](const EnvSpec &env) { return env.name == env_name; });
if (it != m_active_envs.end()) {
it->env_ptr->getNode()->cleanup();
m_executor->remove_node(it->env_ptr->getNode()->get_node_base_interface());
m_active_envs.erase(it);
return EnvStateReturnType::SUCCESS;
@ -162,8 +163,6 @@ EnvStateReturnType EnvManager::configureEnv(const std::string &env_name) {
return EnvStateReturnType::FAILURE;
}
it->env_ptr->configure();
// it->env_ptr->getNode()->configure();
// it->env_ptr->activate();
it->env_ptr->getNode()->activate();
return EnvStateReturnType::SUCCESS;

3
env_manager/gz_enviroment/CMakeLists.txt
View file

@ -10,6 +10,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
rclcpp_lifecycle
tf2_ros
tf2_msgs
tf2_eigen
ros_gz
pluginlib
ignition-transport11
@ -67,5 +68,5 @@ install(TARGETS ${PROJECT_NAME}
)
ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
# ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
ament_package()

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

@ -1,7 +1,10 @@
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
// #include "ros_gz_bridge/convert.hpp"
#include <memory>
#include <mutex>
#include <string>
#include <tf2_msgs/msg/detail/tf_message__struct.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_msgs/msg/tf_message.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
@ -34,14 +37,19 @@ protected:
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;
std::string m_service_spawn;
std::string m_world_name;
std::shared_ptr<rbs_utils::AssemblyConfigLoader> m_config_loader;
tf2_msgs::msg::TFMessage m_transforms;
tf2_msgs::msg::TFMessage m_target_places;
std::string getGzWorldName();
std::string createGzModelString(const std::vector<double>& pose, const std::string& mesh_filepath,
std::string createGzModelString(const std::vector<double>& pose, const std::vector<double>& inertia, const double &mass, const std::string& mesh_filepath,
const std::string& name);
};
} // namespace gz_enviroment

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

@ -1,49 +1,51 @@
#include "gz_enviroment/gz_enviroment.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "ros_gz_bridge/convert.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "ros_gz_bridge/convert.hpp"
#include <memory>
#include <rclcpp/logging.hpp>
#include <ros_gz_interfaces/msg/detail/entity__builder.hpp>
#include <gz/sim/components.hh>
#include <string>
#include <tf2_msgs/msg/detail/tf_message__struct.hpp>
#include <tf2_ros/transform_broadcaster.h>
namespace gz_enviroment
{
namespace gz_enviroment {
GzEnviroment::GzEnviroment() : env_interface::EnvInterface()
{
}
GzEnviroment::GzEnviroment() : env_interface::EnvInterface() {}
CallbackReturn GzEnviroment::on_init()
{
CallbackReturn GzEnviroment::on_init() {
RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment in on_init()");
gz::sim::EntityComponentManager ecm;
// Получение всех сущностей, которые представляют модели
const auto modelEntities = ecm.EntitiesByComponents(gz::sim::components::Model());
const auto modelEntities =
ecm.EntitiesByComponents(gz::sim::components::Model());
// Вывод списка моделей
for (const auto entity : modelEntities)
{
for (const auto entity : modelEntities) {
const auto modelName = ecm.Component<gz::sim::components::Name>(entity);
if (modelName)
{
std::cout << "Model Name: " << modelName->Data() << std::endl;
if (modelName) {
RCLCPP_INFO_STREAM(getNode()->get_logger(), "Model Name: " << modelName->Data());
}
}
return CallbackReturn::SUCCESS;
}
CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State&)
{
CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State &) {
// Configuration of parameters
RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_configure");
m_gz_node = std::make_shared<gz::transport::Node>();
m_world_name = getGzWorldName();
m_topic_name = std::string("/world/") + m_world_name + "/dynamic_pose/info";
m_service_spawn = std::string("/world/") + m_world_name + "/create";
m_config_loader =
std::make_shared<rbs_utils::AssemblyConfigLoader>("asp-example2");
m_follow_frames = m_config_loader->getSceneModelNames();
// m_target_places = std::make_shared<tf2_msgs::msg::TFMessage>();
m_transforms = m_config_loader->getTfData("bishop");
for (auto &place : m_transforms.transforms) {
place.child_frame_id = place.child_frame_id + "_target";
}
// if (!doGzSpawn())
// {
@ -53,49 +55,53 @@ CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State&)
return CallbackReturn::SUCCESS;
}
CallbackReturn GzEnviroment::on_activate(const rclcpp_lifecycle::State&)
{
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);
m_tf2_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(getNode());
m_tf2_broadcaster =
std::make_unique<tf2_ros::TransformBroadcaster>(getNode());
m_tf2_broadcaster_target =
std::make_unique<tf2_ros::TransformBroadcaster>(getNode());
return CallbackReturn::SUCCESS;
}
CallbackReturn GzEnviroment::on_cleanup(const rclcpp_lifecycle::State&)
{
CallbackReturn GzEnviroment::on_cleanup(const rclcpp_lifecycle::State &) {
RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_cleanup");
m_tf2_broadcaster.reset();
m_tf2_broadcaster_target.reset();
m_gz_node.reset();
return CallbackReturn::SUCCESS;
}
CallbackReturn GzEnviroment::on_deactivate(const rclcpp_lifecycle::State&)
{
CallbackReturn GzEnviroment::on_deactivate(const rclcpp_lifecycle::State &) {
RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_deactivate");
// m_tf2_broadcaster.reset();
// m_tf2_broadcaster_target.reset();
// m_gz_node.reset();
return CallbackReturn::SUCCESS;
}
// TODO: Check do this via EntityComponentManager
void GzEnviroment::onGzPoseSub(const gz::msgs::Pose_V& pose_v)
{
// TODO: Check to do this via EntityComponentManager
void GzEnviroment::onGzPoseSub(const gz::msgs::Pose_V &pose_v) {
// TODO: Read from config
m_follow_frames = { "box1", "box2", "box3", "box4", "box5", "box6"};
for (const auto& it : pose_v.pose())
{
if (std::find(m_follow_frames.begin(), m_follow_frames.end(), it.name()) == m_follow_frames.end())
// m_follow_frames = {"box1", "box2", "box3", "box4", "box5", "box6"};
std::vector<geometry_msgs::msg::TransformStamped> all_transforms{};
for (const auto &it : pose_v.pose()) {
if (std::find(m_follow_frames.begin(), m_follow_frames.end(), it.name()) ==
m_follow_frames.end())
continue;
geometry_msgs::msg::PoseStamped rpose;
ros_gz_bridge::convert_gz_to_ros(it, rpose);
geometry_msgs::msg::TransformStamped t;
t.header.stamp = getNode()->get_clock()->now();
t.header.frame_id = "world";
t.child_frame_id = it.name();
@ -109,32 +115,37 @@ void GzEnviroment::onGzPoseSub(const gz::msgs::Pose_V& pose_v)
t.transform.rotation.z = rpose.pose.orientation.z;
t.transform.rotation.w = rpose.pose.orientation.w;
m_tf2_broadcaster->sendTransform(t);
all_transforms.push_back(t);
}
for (auto &place : m_transforms.transforms) {
all_transforms.push_back(place);
}
for (auto &transform : all_transforms) {
m_tf2_broadcaster->sendTransform(transform);
}
}
std::string GzEnviroment::getGzWorldName()
{
bool executed{ false };
bool result{ false };
unsigned int timeout{ 5000 };
std::string service{ "/gazebo/worlds" };
std::string GzEnviroment::getGzWorldName() {
bool executed{false};
bool result{false};
unsigned int timeout{5000};
std::string service{"/gazebo/worlds"};
gz::msgs::StringMsg_V worlds_msg;
while (rclcpp::ok() && !executed)
{
while (rclcpp::ok() && !executed) {
RCLCPP_INFO(getNode()->get_logger(), "Requesting list of world names.");
executed = m_gz_node->Request(service, timeout, worlds_msg, result);
}
if (!executed)
{
if (!executed) {
RCLCPP_INFO(getNode()->get_logger(), "Timed out when getting world names.");
return "";
}
if (!result || worlds_msg.data().empty())
{
if (!result || worlds_msg.data().empty()) {
RCLCPP_INFO(getNode()->get_logger(), "Failed to get world names.");
return "";
}
@ -142,39 +153,57 @@ std::string GzEnviroment::getGzWorldName()
return worlds_msg.data(0);
}
bool GzEnviroment::doGzSpawn()
{
bool GzEnviroment::doGzSpawn() {
gz::msgs::EntityFactory req;
gz::msgs::Boolean rep;
bool result;
unsigned int timeout = 5000;
// TODO: From Config
std::vector<double> pps{ 1.0, 0.0, 1.0, 0.0, 0.0 };
std::string mesh_file = "monkey.stl";
std::string mesh_name = "monkey";
// ENDTODO
std::string sdf_string = createGzModelString(pps, mesh_file, mesh_name);
req.set_sdf(sdf_string);
bool executed = m_gz_node->Request(m_service_spawn, req, timeout, rep, result);
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;
}
std::string GzEnviroment::createGzModelString(const std::vector<double>& pose, 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 + "'>" + "<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>" +
"<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>";
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
} // namespace gz_enviroment
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(gz_enviroment::GzEnviroment, env_interface::EnvInterface);
PLUGINLIB_EXPORT_CLASS(gz_enviroment::GzEnviroment,
env_interface::EnvInterface);

0
env_manager/gz_enviroment/test.json
View file

14
rbs_bt_executor/bt_trees/test_gripper.xml Normal file
View file

@ -0,0 +1,14 @@
<?xml version="1.0"?>
<root main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<Sequence>
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
</Sequence>
</BehaviorTree>
</root>

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

@ -51,7 +51,7 @@ def generate_launch_description():
executable='bt_engine',
#prefix=['xterm -e gdb -ex run --args'],
parameters=[
{'bt_file_path': os.path.join(get_package_share_directory('rbs_bt_executor'), 'bt_trees/test_tree.xml')},
{'bt_file_path': os.path.join(get_package_share_directory('rbs_bt_executor'), 'bt_trees/test_gripper.xml')},
{'plugins':["rbs_skill_move_topose_bt_action_client",
"rbs_skill_get_pick_place_pose_service_client",
"rbs_skill_gripper_move_bt_action_client",

4
rbs_bt_executor/src/MoveToJointStates.cpp
View file

@ -20,7 +20,7 @@ public:
MoveToJointStateAction::Goal populate_goal() override {
getInput<std::string>("robot_name", robot_name_);
getInput<std::vector<double>>("PrePlaceJointState", joint_values_);
getInput<std::vector<double>>("JointState", joint_values_);
auto goal = MoveToJointStateAction::Goal();
RCLCPP_INFO(_node->get_logger(), "Send goal for robot [%s]",
@ -42,7 +42,7 @@ public:
static PortsList providedPorts() {
return providedBasicPorts(
{InputPort<std::string>("robot_name"),
InputPort<std::vector<double>>("PrePlaceJointState")});
InputPort<std::vector<double>>("JointState")});
}
private:

8
rbs_bt_executor/src/gripper_move.cpp
View file

@ -19,9 +19,6 @@ public:
if (!_node->has_parameter("close")) {
_node->declare_parameter("close", position.close);
}
if (!_node->has_parameter("midPosition")) {
_node->declare_parameter("midPosition", position.closeFull);
}
}
GripperCommand::Goal populate_goal() override {
getInput<std::string>("pose", pose);
@ -44,9 +41,8 @@ public:
private:
struct {
double open = 0.2;
double closeFull = 0.8;
double close = 0.4;
double open = 0.008;
double close = 0.000;
} position;
std::string pose;

8
rbs_perception/src/perception_filter.cpp
View file

@ -5,7 +5,7 @@ rbs_perception::PCFilter::PCFilter() : Node("pc_filter", rclcpp::NodeOptions())
{
publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("rgbd_camera/filtered_points", 1);
subscriber_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
"rgbd_camera/points", 1, std::bind(&PCFilter::sub_callback, this, std::placeholders::_1)
"inner_rgbd_camera/points", 1, std::bind(&PCFilter::sub_callback, this, std::placeholders::_1)
);
tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
@ -16,7 +16,7 @@ void rbs_perception::PCFilter::sub_callback(const sensor_msgs::msg::PointCloud2
{
try
{
standform = tf_buffer_->lookupTransform(msg.header.frame_id, world_frame,
standform = tf_buffer_->lookupTransform(world_frame, "inner_rgbd_camera",
tf2::TimePointZero, tf2::durationFromSec(3));
}
@ -25,13 +25,13 @@ void rbs_perception::PCFilter::sub_callback(const sensor_msgs::msg::PointCloud2
RCLCPP_ERROR(this->get_logger(), "%s", ex.what());
}
sensor_msgs::msg::PointCloud2 transformed_cloud;
pcl_ros::transformPointCloud(world_frame, standform, msg, transformed_cloud);
pcl_ros::transformPointCloud("inner_rgbd_camera", standform, msg, transformed_cloud);
pcl::PointCloud<pcl::PointXYZ> cloud;
pcl::fromROSMsg(transformed_cloud, cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>(cloud));
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_voxel_filtered(new pcl::PointCloud<pcl::PointXYZ>());
voxel_filter.setInputCloud(cloud_ptr);
voxel_filter.setLeafSize(0.02, 0.02, 0.02);
voxel_filter.setLeafSize(0.01, 0.01, 0.01);
voxel_filter.filter(*cloud_voxel_filtered);
pub_callback(publisher_, *cloud_voxel_filtered);
}

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

@ -34,8 +34,9 @@ def generate_launch_description():
env_manager_cond = LaunchConfiguration("env_manager")
gazebo_gui = LaunchConfiguration("gazebo_gui")
# FIXME: To args when we'll have different files
print(os.getenv("IGN_GAZEBO_RESOURCE_PATH").__str__())
world_config_file = PathJoinSubstitution(
[FindPackageShare("rbs_simulation"), "worlds", "mir.sdf"]
[FindPackageShare("rbs_simulation"), "worlds", "asm2.sdf"]
)
# Gazebo nodes
@ -81,6 +82,18 @@ def generate_launch_description():
output='screen',
)
rgbd_bridge_in = Node(
package='ros_gz_bridge',
executable='parameter_bridge',
arguments=[
'/inner_rgbd_camera/image@sensor_msgs/msg/Image@gz.msgs.Image',
'/inner_rgbd_camera/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo',
'/inner_rgbd_camera/depth_image@sensor_msgs/msg/Image@gz.msgs.Image',
'/inner_rgbd_camera/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked'
],
output='screen',
)
nodes_to_start = [
@ -88,6 +101,7 @@ def generate_launch_description():
gazebo_server,
gazebo_spawn_robot,
env_manager,
rgbd_bridge_out
rgbd_bridge_out,
rgbd_bridge_in
]
return LaunchDescription(declared_arguments + nodes_to_start)

107
rbs_simulation/worlds/asm2.sdf Normal file
View file

@ -0,0 +1,107 @@
<?xml version="1.0"?>
<sdf version='1.9'>
<world name='asm2'>
<physics name='1ms' type='ignored'>
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
</physics>
<plugin name='ignition::gazebo::systems::Physics' filename='ignition-gazebo-physics-system'/>
<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 name="ignition::gazebo::systems::Sensors" filename="ignition-gazebo-sensors-system">
<render_engine>ogre2</render_engine>
</plugin>
<gravity>0 0 -9.8</gravity>
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
<atmosphere type='adiabatic'/>
<scene>
<ambient>0.4 0.4 0.4 1</ambient>
<background>0.7 0.7 0.7 1</background>
<shadows>false</shadows>
</scene>
<gui fullscreen="0">
<plugin filename="GzScene3D" name="3D View">
<ignition-gui>
<title>3D View</title>
<property type="bool" key="showTitleBar">false</property>
<property type="string" key="state">docked</property>
</ignition-gui>
<engine>ogre2</engine>
<scene>scene</scene>
<ambient_light>1.0 1.0 1.0</ambient_light>
<background_color>0.4 0.6 1.0</background_color>
<camera_pose>3.3 2.8 2.8 0 0.5 -2.4</camera_pose>
</plugin>
<plugin filename="WorldStats" name="World stats">
<ignition-gui>
<title>World stats</title>
<property type="bool" key="showTitleBar">false</property>
<property type="bool" key="resizable">false</property>
<property type="double" key="height">110</property>
<property type="double" key="width">290</property>
<property type="double" key="z">1</property>
<property type="string" key="state">floating</property>
<anchors target="3D View">
<line own="right" target="right"/>
<line own="bottom" target="bottom"/>
</anchors>
</ignition-gui>
<sim_time>true</sim_time>
<real_time>true</real_time>
<real_time_factor>true</real_time_factor>
<iterations>true</iterations>
</plugin>
</gui>
<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>
<model name='ground'>
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
</plane>
</geometry>
</collision>
<visual name="visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</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>
</world>
</sdf>

22
rbs_skill_servers/src/gripper_control_action_server.cpp
View file

@ -32,36 +32,24 @@ public:
std::placeholders::_1));
publisher = this->create_publisher<std_msgs::msg::Float64MultiArray>(
"/gripper_controller/commands", 10);
join_state_subscriber =
this->create_subscription<sensor_msgs::msg::JointState>(
"/joint_states", 10,
std::bind(&GripperControlActionServer::joint_state_callback, this,
std::placeholders::_1));
}
private:
rclcpp_action::Server<GripperCommand>::SharedPtr actionServer_;
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr publisher;
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr
join_state_subscriber;
double gripper_joint_state{1.0};
using ServerGoalHandle = rclcpp_action::ServerGoalHandle<GripperCommand>;
void joint_state_callback(const sensor_msgs::msg::JointState &msg) {
gripper_joint_state = msg.position.back();
}
rclcpp_action::GoalResponse
goal_callback(const rclcpp_action::GoalUUID &uuid,
std::shared_ptr<const GripperCommand::Goal> goal) {
RCLCPP_INFO(this->get_logger(),
"goal request recieved for gripper [%.2f m]", goal->position);
"goal request recieved for gripper [%.6f m]", goal->position);
(void)uuid;
if (goal->position > 0.9 or goal->position < 0) {
return rclcpp_action::GoalResponse::REJECT;
}
// if (goal->position > 0.008 or goal->position < 0) {
// return rclcpp_action::GoalResponse::REJECT;
// }
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
@ -91,8 +79,6 @@ private:
RCLCPP_ERROR(this->get_logger(), "Goal Canceled");
return;
}
RCLCPP_INFO(this->get_logger(), "Current gripper state %f",
gripper_joint_state);
std_msgs::msg::Float64MultiArray command;

24
rbs_skill_servers/src/pick_place_pose_loader.cpp
View file

@ -38,11 +38,6 @@ void GetGraspPlacePoseServer::handleServer(
response) {
std::string object_name =
request->object_name + "_place"; // TODO: replace with better name
// Load place pose from TF2
auto d = std::make_shared<rbs_utils::AssemblyConfigLoader>("asp-example");
d->setTfData();
// rbs_utils::processAsmFolderName("asp-example");
try {
place_pose_tf = tf_buffer_->lookupTransform("world", object_name.c_str(),
@ -66,10 +61,7 @@ void GetGraspPlacePoseServer::handleServer(
grasp_pose_tf.transform.translation.x,
grasp_pose_tf.transform.translation.y,
grasp_pose_tf.transform.translation.z);
// TODO: Here need to check the parameter
// We can use another parameter library from PickNik
// grasp_param_pose = this->get_parameter(request->object_name + ".GraspPose")
// .as_double_array();
RCLCPP_DEBUG(this->get_logger(),
"\nGrasp direction \n x: %.2f \n y: %.2f \n z: %.2f",
request->grasp_direction.x, request->grasp_direction.y,
@ -84,8 +76,7 @@ void GetGraspPlacePoseServer::handleServer(
Eigen::Vector3d scale_grasp(0, 0, 0.10);
Eigen::Vector3d scale_place(0, 0, 0.15);
auto path = std::getenv("RBS_ASSEMBLY_PATH");
RCLCPP_ERROR_STREAM(this->get_logger(), path);
response->grasp =
collectPose(grasp_pose, request->grasp_direction, scale_grasp);
response->place =
@ -128,13 +119,4 @@ std::vector<geometry_msgs::msg::Pose> GetGraspPlacePoseServer::collectPose(
poses.push_back(tf2::toMsg(postGraspPose));
return poses;
}
// std::vector<geometry_msgs::msg::Pose> GetGraspPlacePoseServer::getPlacePoseJson(const nlohmann::json& json)
// {
// std::vector<geometry_msgs::msg::Pose> place_pose;
// auto env_path = std::getenv("PATH");
// RCLCPP_INFO_STREAM(this->get_logger(), env_path);
// return place_pose;
// }
}

1
rbs_utils/CMakeLists.txt
View file

@ -56,6 +56,5 @@ if(BUILD_TESTING)
ament_lint_auto_find_test_dependencies()
endif()
# ament_export_dependencies()
ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
ament_package()

28
rbs_utils/include/rbs_utils/rbs_utils.hpp
View file

@ -8,6 +8,7 @@
#include <nlohmann/json.hpp>
#include <rclcpp/node.hpp>
#include <string>
#include <tf2/LinearMath/Vector3.h>
#include <tf2_eigen/tf2_eigen.hpp>
#include <tf2_msgs/msg/detail/tf_message__struct.hpp>
#include <unordered_map>
@ -17,29 +18,50 @@ const std::string env_dir = std::getenv("RBS_ASSEMBLY_DIR");
namespace rbs_utils {
struct EnvModels {
std::string model_name;
std::string mesh_path;
std::vector<double> model_inertia;
std::vector<double> model_pose;
double mass;
};
class AssemblyConfigLoader {
public:
AssemblyConfigLoader(const std::string &t_assembly_dir);
inline std::shared_ptr<std::unordered_map<std::string, nlohmann::json>>
getAssemblyFileData();
getAssemblyFileData() {
return m_env_vars;
}
void setTfData();
tf2_msgs::msg::TFMessage getTfData(const std::string &model_name);
inline std::shared_ptr<std::vector<EnvModels>> getEnvModels() {
return m_env_models;
}
std::vector<std::string> getSceneModelNames();
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<EnvModels>> m_env_models{};
std::string m_assembly_dir;
std::unique_ptr<tf2_ros::Buffer> m_tf_buffer;
void readAssemblyFileData(const std::string &filename,
const std::string &filepath);
tf2_msgs::msg::TFMessage parseJsonToTFMessage(const nlohmann::json &json);
tf2_msgs::msg::TFMessage parseJsonToTFMessage(const nlohmann::json &json,
const std::string &model_name);
void setTfFromDb(const std::string &filename);
double convertToDouble(const nlohmann::json &value);
void readModelConfigs();
EnvModels parseModelData(const std::string &jsonFilePath);
};
class StaticFramePublisher : public rclcpp::Node {

232
rbs_utils/src/rbs_utils.cpp
View file

@ -1,8 +1,10 @@
#include "rbs_utils/rbs_utils.hpp"
#include <fstream>
#include <iostream>
#include <iterator>
#include <memory>
#include <nlohmann/json_fwd.hpp>
#include <ostream>
#include <rclcpp/clock.hpp>
#include <rclcpp/logger.hpp>
#include <rclcpp/logging.hpp>
@ -10,21 +12,28 @@
#include <tf2_msgs/msg/detail/tf_message__struct.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <unordered_map>
#include <vector>
namespace rbs_utils {
AssemblyConfigLoader::AssemblyConfigLoader(const std::string &t_assembly_dir)
: m_env_vars(
std::make_shared<std::unordered_map<std::string, nlohmann::json>>()) {
if (!t_assembly_dir.empty()) {
std::make_shared<std::unordered_map<std::string, nlohmann::json>>()),
m_env_models(std::make_shared<std::vector<EnvModels>>()),
m_assembly_dir(t_assembly_dir) {
if (!m_assembly_dir.empty()) {
std::vector<std::string> filenames = {"robossembler_db", "sequences"};
for (auto &filename : filenames) {
std::string filepath =
env_dir + "/" + t_assembly_dir + "/" + filename + ".json";
env_dir + "/" + m_assembly_dir + "/" + filename + ".json";
m_env_files.push_back(filepath);
readAssemblyFileData(filename, filepath);
}
readModelConfigs();
} else {
RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"), "Assembly dir is not set");
}
}
@ -50,19 +59,112 @@ void AssemblyConfigLoader::readAssemblyFileData(const std::string &filename,
}
}
inline std::shared_ptr<std::unordered_map<std::string, nlohmann::json>>
AssemblyConfigLoader::getAssemblyFileData() {
return m_env_vars;
EnvModels
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(rclcpp::get_logger("rbs_utils"), "Failed to open file: %s",
json_file_path.c_str());
return EnvModels{};
}
json_content = nlohmann::json::parse(file);
} catch (const nlohmann::json::parse_error &e) {
RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"),
"Error parsing JSON in file %s: %s", json_file_path.c_str(),
e.what());
} catch (const std::exception &e) {
RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"),
"Exception reading file %s: %s", json_file_path.c_str(),
e.what());
}
EnvModels 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::setTfData() {
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") {
EnvModels 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"];
tf2_msgs::msg::TFMessage tf_msg = parseJsonToTFMessage(json);
// 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(rclcpp::get_logger("rbs_utils"),
"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_DEBUG_STREAM(
RCLCPP_INFO_STREAM(
rclcpp::get_logger("rbs_utils"),
"Frame ID: " << transform.header.frame_id << ", Child Frame ID: "
<< transform.child_frame_id << ", Translation: ["
@ -70,77 +172,91 @@ void AssemblyConfigLoader::setTfData() {
<< transform.transform.translation.y << ", "
<< transform.transform.translation.z << "]");
}
auto r = std::make_shared<StaticFramePublisher>(tf_msg);
// auto r = std::make_shared<StaticFramePublisher>(tf_msg);
} else {
RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"),
"Key 'robossembler_db' not found in m_env_vars.");
}
return tf_msg;
}
tf2_msgs::msg::TFMessage
AssemblyConfigLoader::parseJsonToTFMessage(const nlohmann::json &json) {
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").get<std::string>();
absolute_transform.transform.translation.x = -1.0;
absolute_transform.transform.translation.y = 0.0;
absolute_transform.transform.translation.z = 0.0;
absolute_transform.transform.rotation.w = 1.0;
absolute_transform.transform.rotation.x = 0.0;
absolute_transform.transform.rotation.y = 0.0;
absolute_transform.transform.rotation.z = 0.0;
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")) {
geometry_msgs::msg::TransformStamped relative_transform;
relative_transform.header.frame_id =
json.at("absolutePart").get<std::string>();
relative_transform.child_frame_id =
relative_part.at("name").get<std::string>();
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);
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>();
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")) {
geometry_msgs::msg::TransformStamped grasp_transform;
grasp_transform.header.frame_id =
json.at("absolutePart").get<std::string>();
grasp_transform.child_frame_id = grasp_pose.at("name").get<std::string>();
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);
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>();
grasp_transform.child_frame_id =
grasp_pose.at("name").get<std::string>() + "_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;