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

update main

Browse source
This commit is contained in:
Ilya Uraev 2024-04-22 15:06:46 +03:00
parent d5e7768d90
commit e8ea09e020
20 changed files with 315 additions and 168 deletions
Download patch file Download diff file Expand all files Collapse all files

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__

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()

17
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 =
@ -37,7 +39,6 @@ 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;
@ -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

80
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();
@ -43,15 +52,9 @@ CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State &) {
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_transforms = m_config_loader->getTfDataAllPossible();
m_timer = getNode()->create_wall_timer(
100ms, std::bind(&GzEnviroment::broadcast_timer_callback, this));
return CallbackReturn::SUCCESS;
}
@ -60,11 +63,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);
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 +88,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;
}
@ -123,9 +124,7 @@ void GzEnviroment::onGzPoseSub(const gz::msgs::Pose_V &pose_v) {
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,55 +152,6 @@ 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;
//
// 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;
// }
} // namespace gz_enviroment
#include "pluginlib/class_list_macros.hpp"

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

17
rbs_bringup/launch/multi_robot.launch.py
View file

@ -73,7 +73,7 @@ def launch_setup(context, *args, **kwargs):
convert_types=False,
).merge_yamls()
for robot in robots["scene_config"]:
namespace = "/" + robot["name"]
namespace: str = "/" + robot["name"]
ld.append(IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
@ -106,9 +106,13 @@ def launch_setup(context, *args, **kwargs):
"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"])
"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()
))
@ -117,9 +121,12 @@ def launch_setup(context, *args, **kwargs):
executable='create',
arguments=[
'-name', robot_name,
'-x', "0",#str(robot["spawn_point"][0]),
'-y', "0",#str(robot["spawn_point"][1]),
'-z', "0",#str(robot["spawn_point"][2]),
# '-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))

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

@ -39,14 +39,18 @@ def launch_setup(context, *args, **kwargs):
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 = "/" + robot_name
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")]
# 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(
@ -89,8 +93,27 @@ def launch_setup(context, *args, **kwargs):
executable="robot_state_publisher",
namespace=namespace,
output="both",
remappings=remappings,
parameters=[{"use_sim_time": True}, robot_description],
# 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(
@ -182,7 +205,8 @@ def launch_setup(context, *args, **kwargs):
moveit,
skills,
task_planner,
perception
perception,
# rviz
]
return nodes_to_start
@ -337,6 +361,11 @@ def generate_launch_description():
default_value="false",
description="Launch gazebo with gui?")
)
declared_arguments.append(
DeclareLaunchArgument("namespace",
default_value="",
description="The ROS2 namespace of a robot")
)
declared_arguments.append(
DeclareLaunchArgument("x",
default_value="0.0",

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

@ -58,27 +58,31 @@ def launch_setup(context, *args, **kwargs):
condition=IfCondition(launch_simulation)
)
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,
# 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)
# 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),
'-x', "0.5",
'-y', "0.5",
'-z', "0.0",
'-topic', namespace + '/robot_description'],
output='screen',
condition=IfCondition(sim_gazebo))
@ -97,11 +101,11 @@ def launch_setup(context, *args, **kwargs):
"gripper_name": gripper_name,
"controllers_file": controllers_file,
"robot_type": robot_type,
"controllers_file": controller_paramfile,
"controllers_file": initial_joint_controllers_file_path,
"cartesian_controllers": cartesian_controllers,
"description_package": description_package,
"description_file": description_file,
"robot_name": robot_name,
"robot_name": robot_type,
"start_joint_controller": start_joint_controller,
"initial_joint_controller": initial_joint_controller,
"launch_simulation": launch_simulation,
@ -115,9 +119,9 @@ def launch_setup(context, *args, **kwargs):
"hardware": hardware,
"launch_controllers": "true",
"gazebo_gui": gazebo_gui,
"x": "0.5",
"y": "0.5",
"z": "0.5"
# "x": "0.5",
# "y": "0.5",
# "z": "0.5"
}.items()
)

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");

1
rbs_simulation/launch/simulation_gazebo.launch.py
View file

@ -69,6 +69,7 @@ def generate_launch_description():
env_manager = Node(
package="env_manager",
executable="run_env_manager",
parameters=[{'use_sim_time': True}],
condition=IfCondition(launch_env_manager)
)

13
rbs_simulation/worlds/asm2.sdf
View file

@ -92,16 +92,15 @@
</visual>
</link>
</model>
<!-- Manipulating objects -->
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>bishop</name> -->
<!-- <uri>model://bishop</uri> -->
<!-- <pose>0.35 0.0 0.0 0.0 0.0 0.0</pose> -->
<!-- </include> -->
</world>
</sdf>

1
rbs_skill_servers/CMakeLists.txt
View file

@ -152,6 +152,7 @@ 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}
)

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

@ -38,6 +38,12 @@ def launch_setup(context, *args, **kwargs):
]
)
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",
@ -89,6 +95,7 @@ def launch_setup(context, *args, **kwargs):
gripper_control_node,
move_cartesian_path_action_server,
move_joint_state_action_server,
move_to_pose,
# grasp_pose_loader
]
return nodes_to_start

1
rbs_skill_servers/scripts/test_cartesian_controller.py Normal file → Executable file
View file

@ -1,3 +1,4 @@
#!/usr/bin/python
import rclpy
from rclpy.node import Node
import argparse

11
rbs_utils/rbs_utils/include/rbs_utils/rbs_utils.hpp
View file

@ -12,13 +12,17 @@
#include "tf2_ros/static_transform_broadcaster.h"
#include <Eigen/Core>
#include <fstream>
#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");
@ -50,14 +54,17 @@ public:
&t_clock_node_interface);
tf2_msgs::msg::TFMessage getTfData(const std::string &model_name);
tf2_msgs::msg::TFMessage getTfDataAllPossible();
std::vector<std::string> getSceneModelNames();
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::vector<std::string> m_env_files;
@ -69,7 +76,7 @@ private:
rclcpp::Clock::SharedPtr m_clock;
void parseRbsDb(const std::string &filepath);
tf2_msgs::msg::TFMessage getWorkspace();
geometry_msgs::msg::PoseArray getWorkspace();
geometry_msgs::msg::Transform
createTransform(const geometry_msgs::msg::Pose &pose);
};

191
rbs_utils/rbs_utils/src/rbs_utils.cpp
View file

@ -1,12 +1,15 @@
#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>
@ -27,7 +30,7 @@ AssemblyConfigLoader::AssemblyConfigLoader(
m_logger(t_logging_interface->get_logger()),
m_clock(t_clock_interface->get_clock()) {
if (!m_assembly_dir.empty()) {
std::vector<std::string> filenames = {"robossembler_db"};
std::vector<std::string> filenames = {"rbs_db"};
for (auto &filename : filenames) {
std::string filepath =
env_dir + "/" + m_assembly_dir + "/" + filename + ".yaml";
@ -49,18 +52,22 @@ void AssemblyConfigLoader::parseRbsDb(const std::string &filepath) {
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);
void *ros_msg = reinterpret_cast<void *>(&m_assembly_config);
dynmsg::cpp::yaml_and_typeinfo_to_rosmsg(rosmsg.type_info,
asm_config_string, ros_msg);
// auto file_cont = rbs_utils_interfaces::msg::to_yaml(m_assembly_config);
// std::ofstream file("rbs_db.yaml");
// if (file.is_open()) {
// file << file_cont;
// file.close();
// }
} 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() {
}
[[deprecated("Not Implemented")]] void AssemblyConfigLoader::saveRbsConfig() {}
std::vector<std::string> AssemblyConfigLoader::getSceneModelNames() {
std::vector<std::string> model_names;
@ -79,37 +86,121 @@ std::vector<std::string> AssemblyConfigLoader::getSceneModelNames() {
return model_names;
}
tf2_msgs::msg::TFMessage AssemblyConfigLoader::getAdditionalPoses() {
tf2_msgs::msg::TFMessage tp;
if (m_assembly_config.extra_poses.empty()) {
RCLCPP_ERROR(m_logger, "Extra poses is empty");
return tp;
}
for (auto &point : m_assembly_config.extra_poses) {
tp.transforms.emplace_back().child_frame_id = point.name;
tp.transforms.emplace_back().transform = createTransform(point.pose);
tp.transforms.emplace_back().header.stamp = m_clock->now();
tp.transforms.emplace_back().header.frame_id = "world";
}
return tp;
}
tf2_msgs::msg::TFMessage AssemblyConfigLoader::getTfDataAllPossible() {
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());
}
if (!m_assembly_config.relative_part.empty()) {
for (const auto &relative_part : m_assembly_config.relative_part) {
geometry_msgs::msg::TransformStamped relative_transform_stamped;
relative_transform_stamped.transform =
createTransform(relative_part.pose);
relative_transform_stamped.child_frame_id = relative_part.name;
relative_transform_stamped.header.frame_id = relative_part.relative_at;
relative_transform_stamped.header.stamp = m_clock->now();
tp.transforms.push_back(relative_transform_stamped);
RCLCPP_INFO(m_logger, "Model name [%s]",
relative_part.relative_at.c_str());
}
}
if (!m_assembly_config.grasp_pose.empty()) {
for (const auto &grasp_pose : m_assembly_config.grasp_pose) {
geometry_msgs::msg::TransformStamped grasp_transform_stamped;
grasp_transform_stamped.transform = createTransform(grasp_pose.pose);
grasp_transform_stamped.child_frame_id = grasp_pose.name;
grasp_transform_stamped.header.stamp = m_clock->now();
grasp_transform_stamped.header.frame_id = grasp_pose.relative_at;
tp.transforms.push_back(grasp_transform_stamped);
}
}
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()) {
const auto &absolute_part_pose = m_assembly_config.absolute_part.pose;
tp.transforms.emplace_back().transform = createTransform(absolute_part_pose);
tp.transforms.emplace_back().child_frame_id = "world";
tp.transforms.emplace_back().header.stamp = m_clock->now();
for (const auto &relative_part : m_assembly_config.relative_part) {
// Find our model data
if (relative_part.name == model_name) {
tp.transforms.emplace_back().transform = createTransform(relative_part.pose);
tp.transforms.emplace_back().child_frame_id = relative_part.relative_at;
tp.transforms.emplace_back().header.stamp = m_clock->now();
geometry_msgs::msg::TransformStamped relative_transform_stamped;
relative_transform_stamped.transform =
createTransform(relative_part.pose);
relative_transform_stamped.child_frame_id = relative_part.name;
relative_transform_stamped.header.frame_id = relative_part.relative_at;
relative_transform_stamped.header.stamp = m_clock->now();
tp.transforms.push_back(relative_transform_stamped);
found_model = true;
}
RCLCPP_INFO(m_logger, "Model name [%s]", relative_part.name.c_str());
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());
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.name == model_name) {
tp.transforms.emplace_back().transform = createTransform(grasp_pose.pose);
tp.transforms.emplace_back().child_frame_id = grasp_pose.name;
tp.transforms.emplace_back().header.stamp = m_clock->now();
found_grasp_pose = true;
if (!m_assembly_config.grasp_pose.empty()) {
for (const auto &grasp_pose : m_assembly_config.grasp_pose) {
if (grasp_pose.name == model_name) {
geometry_msgs::msg::TransformStamped grasp_transform_stamped;
grasp_transform_stamped.transform = createTransform(grasp_pose.pose);
grasp_transform_stamped.child_frame_id = grasp_pose.name + "_grasp";
grasp_transform_stamped.header.stamp = m_clock->now();
grasp_transform_stamped.header.frame_id = grasp_pose.name;
tp.transforms.push_back(grasp_transform_stamped);
found_grasp_pose = true;
}
}
}
@ -117,7 +208,8 @@ AssemblyConfigLoader::getTfData(const std::string &model_name) {
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());
RCLCPP_ERROR(m_logger, "Grasp pose not found for model %s",
model_name.c_str());
}
return tp;
@ -136,42 +228,41 @@ AssemblyConfigLoader::createTransform(const geometry_msgs::msg::Pose &pose) {
return transform;
}
tf2_msgs::msg::TFMessage AssemblyConfigLoader::getWorkspace() {
tf2_msgs::msg::TFMessage tf_msg;
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 tf_msg;
return pose_array;
}
const std::string child_frame_id = "world";
pose_array.poses.reserve(m_assembly_config.workspace.size());
const double default_rotation_value = 0.0;
const double default_rotation_w = 1.0;
tf_msg.transforms.reserve(m_assembly_config.workspace.size());
for (std::size_t i = 0; i < m_assembly_config.workspace.size(); ++i) {
tf_msg.transforms[i].transform.translation.x =
m_assembly_config.workspace[i].x;
tf_msg.transforms[i].transform.translation.y =
m_assembly_config.workspace[i].y;
tf_msg.transforms[i].transform.translation.z =
m_assembly_config.workspace[i].z;
tf_msg.transforms[i].child_frame_id = child_frame_id;
tf_msg.transforms[i].transform.rotation.x = default_rotation_value;
tf_msg.transforms[i].transform.rotation.y = default_rotation_value;
tf_msg.transforms[i].transform.rotation.z = default_rotation_value;
tf_msg.transforms[i].transform.rotation.w = default_rotation_w;
tf_msg.transforms[i].header.stamp = m_clock->now();
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 tf_msg;
}
return pose_array;
}
geometry_msgs::msg::PoseArray
AssemblyConfigLoader::getWorkspaceInspectorTrajectory() {
geometry_msgs::msg::PoseArray pose_array;
pose_array.header.frame_id = "world";
auto workspace = getWorkspace();
for (auto &point : workspace.transforms) {
for (auto &point : workspace.poses) {
auto pose = transformTrajectory(point);
pose_array.poses.push_back(pose);
}
@ -179,13 +270,15 @@ AssemblyConfigLoader::getWorkspaceInspectorTrajectory() {
}
geometry_msgs::msg::Pose AssemblyConfigLoader::transformTrajectory(
const geometry_msgs::msg::TransformStamped &pose) {
auto pose_eigen = tf2::transformToEigen(pose.transform);
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;
auto pose_msg = tf2::toMsg(pose_eigen);
return pose_msg;
return tf2::toMsg(pose_eigen);
}
} // namespace rbs_utils

5
rbs_utils/rbs_utils_interfaces/msg/AssemblyConfig.msg
View file

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