robossembler/cnc-graver-assist
7
0
Fork 0
Code Issues 5 Pull requests 1 Projects Releases Packages Wiki Activity Actions

refactor: add collision spawner and planning scene manager node

Browse source 
- refactored places config and ref_frames config parsers
- updated dependent files
This commit is contained in:
Ilya Uraev 2025-04-14 10:16:56 +03:00
parent c4f41a0322
commit 0525253fda
21 changed files with 2216 additions and 222 deletions
Download patch file Download diff file Expand all files Collapse all files

244
rbs_mill_assist/include/rbs_mill_assist/places_config.hpp
View file

@ -1,10 +1,12 @@
#include "geometry_msgs/msg/pose.hpp"
#include <filesystem>
#include <memory>
#include <rclcpp/logger.hpp>
#include <rclcpp/node.hpp>
#include <rclcpp/node_interfaces/node_base_interface.hpp>
#include <rclcpp/node_interfaces/node_logging_interface.hpp>
#include <string>
#include <tf2/LinearMath/Quaternion.hpp>
#include <utility>
#include <vector>
#include <yaml-cpp/node/node.h>
#include <yaml-cpp/node/parse.h>
@ -12,32 +14,140 @@
class PlacesConfig {
public:
PlacesConfig(const std::string &places_filepath) {
loadConfig(places_filepath);
PlacesConfig(const std::string &places_filepath, const rclcpp::Logger& logger)
: _logger(logger) {
objects_to_spawn_ = std::make_shared<Places>();
if (!loadPlacesFromYaml(places_filepath)) {
RCLCPP_FATAL(_logger, "Failed to load objects from YAML file: %s",
places_filepath.c_str());
rclcpp::shutdown();
return;
}
}
inline std::vector<std::string> getMeshFilenames() {
return _model_mesh_filenames;
}
inline std::vector<geometry_msgs::msg::Pose> getMeshPoses(){
return _model_poses;
}
inline std::vector<std::string> getMeshFilepaths() {
return _model_mesh_filepaths;
}
inline std::vector<std::string> getModelNames() {
return _model_names;
}
inline std::vector<std::string> getModelParents() {
return _model_parents;
}
PlacesConfig(const std::string &places_filepath, rclcpp::Node::SharedPtr node)
: PlacesConfig(places_filepath, node->get_logger()) {}
struct PlaceSpec {
std::string name;
std::string type;
std::string frame_id = "world";
bool has_dimensions;
std::vector<double> dimensions;
geometry_msgs::msg::Pose pose;
std::optional<std::string> mesh_filepath;
std::tuple<double, double, double> scale_xyz = {1.0, 1.0, 1.0};
};
struct Places {
std::optional<std::string> model_dir;
std::vector<PlaceSpec> models;
};
inline std::shared_ptr<Places> get() const { return objects_to_spawn_; }
private:
std::vector<std::string> _model_names;
std::vector<std::string> _model_mesh_filenames;
std::vector<geometry_msgs::msg::Pose> _model_poses;
std::vector<std::string> _model_mesh_filepaths;
std::vector<std::string> _model_parents;
const rclcpp::Logger& _logger;
bool loadPlacesFromYaml(const std::string &filepath) {
auto places_config_node = YAML::LoadFile(filepath);
if (auto node = places_config_node["model_dir"]; node && node.IsScalar()) {
objects_to_spawn_->model_dir = node.as<std::string>();
}
if (!places_config_node["models"] ||
!places_config_node["models"].IsSequence()) {
RCLCPP_FATAL(_logger, "No valid 'models' section found in config_file");
return false;
}
for (const auto &model : places_config_node["models"]) {
PlaceSpec object;
if (model["ps_ignore"] && model["ps_ignore"].as<bool>()) {
continue;
}
object.name = model["name"].as<std::string>();
object.type = model["type"].as<std::string>();
const auto &type = object.type;
if (type == "box" || type == "sphere" || type == "cylinder") {
if (!model["dimensions"]) {
RCLCPP_ERROR(_logger, "Model '%s' is missing 'dimensions'",
object.name.c_str());
return false;
}
auto d = model["dimensions"].as<std::vector<double>>();
if (type == "box" && d.size() == 3) {
object.dimensions = d;
} else if (type == "sphere" && d.size() == 1) {
object.dimensions = d;
} else if (type == "cylinder" && d.size() == 2) {
object.dimensions = d;
} else {
RCLCPP_ERROR(_logger, "Invalid dimensions count for type '%s'",
type.c_str());
return false;
}
} else if (type == "mesh") {
if (!model["mesh_filename"]) {
RCLCPP_ERROR(_logger, "mesh object missing mesh_filename");
return false;
}
std::string mesh_filename = model["mesh_filename"].as<std::string>();
if (objects_to_spawn_->model_dir.has_value()) {
object.mesh_filepath =
std::filesystem::path(objects_to_spawn_->model_dir->c_str()) /
object.name / "meshes" / mesh_filename;
RCLCPP_WARN(_logger, "mesh filepath is: %s",
object.mesh_filepath->c_str());
} else {
RCLCPP_ERROR(_logger, "mesh_filename in empty in config_file");
return false;
}
} else {
RCLCPP_ERROR(_logger, "Unrecognized object type: '%s'", type.c_str());
return false;
}
if (model["frame_id"]) {
object.frame_id = model["frame_id"].as<std::string>();
}
if (model["pose"]) {
auto pose = model["pose"];
if (pose["position"]) {
auto position = model["pose"]["position"];
object.pose.position.x = position["x"].as<double>();
object.pose.position.y = position["y"].as<double>();
object.pose.position.z = position["z"].as<double>();
}
if (pose["orientation"]) {
auto orientation = model["pose"]["orientation"];
if (!isQuat(orientation)) {
auto quat = rpyToQuat(orientation["r"].as<double>(),
orientation["p"].as<double>(),
orientation["y"].as<double>());
object.pose.orientation.x = quat.x;
object.pose.orientation.y = quat.y;
object.pose.orientation.z = quat.z;
object.pose.orientation.w = quat.w;
} else {
object.pose.orientation.x = orientation["x"].as<double>();
object.pose.orientation.y = orientation["y"].as<double>();
object.pose.orientation.z = orientation["z"].as<double>();
object.pose.orientation.w = orientation["w"].as<double>();
}
}
}
objects_to_spawn_->models.push_back(object);
}
return true;
}
bool isQuat(const YAML::Node &node) {
if (node["x"] && node["y"] && node["z"] && node["w"]) {
@ -49,82 +159,18 @@ private:
"Provided YAML Node is not quat or euler angles");
}
geometry_msgs::msg::Quaternion toQuat(const YAML::Node &node) {
geometry_msgs::msg::Quaternion quat;
if (isQuat(node)) {
quat.x = node["x"].as<double>();
quat.y = node["y"].as<double>();
quat.z = node["z"].as<double>();
quat.w = node["w"].as<double>();
} else {
tf2::Quaternion tf2_quat;
tf2_quat.setRPY(node["r"].as<double>(), node["p"].as<double>(),
node["y"].as<double>());
quat.x = tf2_quat.x();
quat.y = tf2_quat.y();
quat.z = tf2_quat.z();
quat.w = tf2_quat.w();
}
return quat;
}
void loadConfig(const std::string &places_config_filepath) {
YAML::Node places_config_node = YAML::LoadFile(places_config_filepath);
std::string model_dir;
if (auto node = places_config_node["model_dir"]; node && node.IsScalar()) {
model_dir = node.as<std::string>();
geometry_msgs::msg::Quaternion rpyToQuat(double roll, double pitch,
double yaw) {
tf2::Quaternion q;
q.setRPY(roll, pitch, yaw);
q.normalize();
geometry_msgs::msg::Quaternion q_msg;
q_msg.x = q.x();
q_msg.y = q.y();
q_msg.z = q.z();
q_msg.w = q.w();
return q_msg;
}
if (!places_config_node["models"] ||
!places_config_node["models"].IsSequence()) {
throw std::runtime_error(
"No valid 'models' section found in places.yaml");
}
for (const auto &model : places_config_node["models"]) {
if (!model["name"] || !model["mesh_filename"] || !model["pose"]) {
throw std::runtime_error(
"Missing required model parameters in places.yaml");
}
std::string model_name = model["name"].as<std::string>();
std::string model_parent = "world";
if (model["relative_to"].IsDefined()) {
model_parent = model["relative_to"].as<std::string>();
}
std::string model_mesh_filename =
model["mesh_filename"].as<std::string>();
_model_names.push_back(std::move(model_name));
_model_mesh_filenames.push_back(std::move(model_mesh_filename));
_model_parents.push_back(std::move(model_parent));
if (!model_dir.empty()) {
std::filesystem::path mesh_path = std::filesystem::path(model_dir) /
_model_names.back() / "meshes" /
_model_mesh_filenames.back();
_model_mesh_filepaths.push_back(mesh_path.string());
}
geometry_msgs::msg::Pose model_pose;
auto position = model["pose"]["position"];
if (!position) {
throw std::runtime_error("Define the model position in places.yaml");
}
model_pose.position.x = position["x"].as<double>();
model_pose.position.y = position["y"].as<double>();
model_pose.position.z = position["z"].as<double>();
if (auto orientation = model["pose"]["orientation"]; orientation) {
model_pose.orientation = toQuat(orientation);
} else {
RCLCPP_WARN(rclcpp::get_logger("planning_scene_loader"),
"Model orientation is not set. Using default values.");
}
_model_poses.push_back(std::move(model_pose));
}
}
std::shared_ptr<Places> objects_to_spawn_;
};

31
rbs_mill_assist/include/rbs_mill_assist/ref_frames_config.hpp
View file

@ -3,6 +3,7 @@
#include <rclcpp/logger.hpp>
#include <rclcpp/node.hpp>
#include <stdexcept>
#include <string>
#include <tf2/LinearMath/Quaternion.hpp>
#include <yaml-cpp/yaml.h>
@ -11,20 +12,20 @@ public:
RefFramesConfig(const std::string &ref_frames_filepath) {
loadConfig(ref_frames_filepath);
}
inline std::vector<std::string> getRefFrameNames() {
return _ref_frame_names;
}
inline std::vector<std::string> getRefFrameParents() {
return _ref_frame_parents;
}
inline std::vector<geometry_msgs::msg::Pose> getRefFramePoses() {
return _ref_frame_poses;
struct RefFrame {
std::string name;
std::string relative_to;
geometry_msgs::msg::Pose pose;
};
inline std::vector<RefFrame> get() const {
return _ref_frames;
}
private:
std::vector<std::string> _ref_frame_names;
std::vector<std::string> _ref_frame_parents;
std::vector<geometry_msgs::msg::Pose> _ref_frame_poses;
std::vector<RefFrame> _ref_frames;
geometry_msgs::msg::Quaternion toQuat(const YAML::Node &node) {
geometry_msgs::msg::Quaternion quat;
@ -47,12 +48,13 @@ private:
void parseRefFrames(const YAML::Node &node) {
for (const auto &frame : node) {
RefFrame ref_frame;
if (!frame["name"] || !frame["relative_to"] || !frame["pose"]) {
throw std::runtime_error(
"Missing required ref_frame parameters in YAML");
}
_ref_frame_names.push_back(frame["name"].as<std::string>());
_ref_frame_parents.push_back(frame["relative_to"].as<std::string>());
ref_frame.name = frame["name"].as<std::string>();
ref_frame.relative_to = frame["relative_to"].as<std::string>();
geometry_msgs::msg::Pose frame_pose;
frame_pose.position.x = frame["pose"]["position"]["x"].as<double>();
@ -62,7 +64,8 @@ private:
if (auto orientation = frame["pose"]["orientation"]; orientation) {
frame_pose.orientation = toQuat(orientation);
}
_ref_frame_poses.push_back(frame_pose);
ref_frame.pose = frame_pose;
_ref_frames.push_back(ref_frame);
}
}

40
rbs_mill_assist/launch/moveit.launch.py
View file

@ -132,10 +132,26 @@ def launch_setup(context, *args, **kwargs):
places_config_filepath = os.path.join(get_package_share_directory("rbs_mill_assist"), "world/config", "places.yaml")
planning_scene_publisher = Node(
# planning_scene_publisher = Node(
# package="rbs_mill_assist",
# executable="planning_scene_publisher",
# parameters=[moveit_config.to_dict(), {"use_sim_time": True}, {"places_config": places_config_filepath}],
# )
collision_spawner = Node(
package="rbs_mill_assist",
executable="planning_scene_publisher",
parameters=[moveit_config.to_dict(), {"use_sim_time": True}, {"places_config": places_config_filepath}],
# prefix=['gdbserver localhost:1234'],
executable="collision_spawner",
parameters=[{"config_file": places_config_filepath}]
)
ps_manager = Node(
package="rbs_mill_assist",
executable="ps_manager",
output="screen",
parameters=[{"frame_id": "world"}]
)
ref_frames_config_filepath = os.path.join(
@ -155,16 +171,6 @@ def launch_setup(context, *args, **kwargs):
],
)
handler = RegisterEventHandler(
OnProcessStart(
target_action=places_frame_publisher,
on_start=[planning_scene_publisher],
)
)
rviz_node = Node(
package="rviz2",
executable="rviz2",
@ -182,18 +188,14 @@ def launch_setup(context, *args, **kwargs):
],
)
delay_planning_scene = TimerAction(
actions=[planning_scene_publisher],
period=5.0
)
nodes_to_start = [
move_group_node,
rviz_node,
places_frame_publisher,
planning_scene_publisher,
# delay_planning_scene
# handler
collision_spawner,
ps_manager
]
return nodes_to_start

28
rbs_mill_assist/src/CMakeLists.txt
View file

@ -58,20 +58,20 @@ install(
# planning_scene_publisher
find_package(moveit_msgs REQUIRED)
find_package(moveit_core REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(shape_msgs REQUIRED)
find_package(yaml-cpp REQUIRED)
add_executable(planning_scene_publisher planning_scene_publisher.cpp)
ament_target_dependencies(planning_scene_publisher moveit_msgs moveit_core geometry_msgs)
target_link_libraries(planning_scene_publisher yaml-cpp)
install(
TARGETS planning_scene_publisher
DESTINATION lib/${PROJECT_NAME}
)
# find_package(moveit_msgs REQUIRED)
# find_package(moveit_core REQUIRED)
# find_package(geometry_msgs REQUIRED)
# find_package(shape_msgs REQUIRED)
# find_package(yaml-cpp REQUIRED)
#
# add_executable(planning_scene_publisher planning_scene_publisher.cpp)
# ament_target_dependencies(planning_scene_publisher moveit_msgs moveit_core geometry_msgs)
# target_link_libraries(planning_scene_publisher yaml-cpp)
#
# install(
# TARGETS planning_scene_publisher
# DESTINATION lib/${PROJECT_NAME}
# )
# publish_ee_pose

22
rbs_mill_assist/src/places_frame_publisher.cpp
View file

@ -22,7 +22,7 @@ public:
this->get_parameter("places_config_filepath").as_string();
auto ref_frames_config_filepath =
this->get_parameter("ref_frames_config_filepath").as_string();
auto places_config = std::make_shared<PlacesConfig>(places_config_filepath);
auto places_config = std::make_shared<PlacesConfig>(places_config_filepath, this->get_logger());
auto ref_frames =
std::make_shared<RefFramesConfig>(ref_frames_config_filepath);
@ -32,15 +32,16 @@ public:
private:
void publish_ref_frames(std::shared_ptr<RefFramesConfig> &ref_frames_config) {
for (size_t i = 0; i < ref_frames_config->getRefFrameNames().size(); i++) {
auto ref_frames = ref_frames_config->get();
for (size_t i = 0; i < ref_frames.size(); i++) {
geometry_msgs::msg::TransformStamped t;
t.header.stamp = this->get_clock()->now();
t.header.frame_id = ref_frames_config->getRefFrameParents()[i];
t.child_frame_id = ref_frames_config->getRefFrameNames()[i];
t.header.frame_id = ref_frames[i].relative_to;
t.child_frame_id = ref_frames[i].name;
geometry_msgs::msg::Pose tmp_pose;
tmp_pose = ref_frames_config->getRefFramePoses()[i];
tmp_pose = ref_frames[i].pose;
t.transform.translation.x = tmp_pose.position.x;
t.transform.translation.y = tmp_pose.position.y;
@ -54,16 +55,17 @@ private:
}
}
void publish_places_frames(std::shared_ptr<PlacesConfig> &places_config) {
for (size_t i = 0; i < places_config->getModelNames().size(); i++) {
void publish_places_frames(std::shared_ptr<PlacesConfig> places_config) {
auto places = places_config->get();
for (size_t i = 0; i < places->models.size(); i++) {
geometry_msgs::msg::TransformStamped t;
t.header.stamp = this->get_clock()->now();
t.header.frame_id = places_config->getModelParents()[i];
t.child_frame_id = places_config->getModelNames()[i];
t.header.frame_id = places->models[i].frame_id;
t.child_frame_id = places->models[i].name;
geometry_msgs::msg::Pose tmp_pose;
tmp_pose = places_config->getMeshPoses()[i];
tmp_pose = places->models[i].pose;
t.transform.translation.x = tmp_pose.position.x;
t.transform.translation.y = tmp_pose.position.y;

53
rbs_mill_assist/utils/CMakeLists.txt
View file

@ -0,0 +1,53 @@
find_package(rclcpp REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(shape_msgs REQUIRED)
find_package(moveit_msgs REQUIRED)
find_package(yaml-cpp REQUIRED)
find_package(geometric_shapes REQUIRED)
find_package(rbs_mill_interfaces REQUIRED)
include_directories(include)
add_executable(ps_manager ./src/ps_manager.cpp)
ament_target_dependencies(ps_manager
rclcpp
moveit_ros_planning_interface
tf2
tf2_ros
tf2_geometry_msgs
geometry_msgs
shape_msgs
moveit_msgs
geometric_shapes
rbs_mill_interfaces
)
add_executable(collision_spawner ./src/collision_spawner.cpp)
ament_target_dependencies(collision_spawner
rclcpp
moveit_ros_planning_interface
tf2
tf2_ros
tf2_geometry_msgs
geometry_msgs
shape_msgs
moveit_msgs
yaml-cpp
rbs_mill_interfaces
)
target_link_libraries(collision_spawner yaml-cpp)
install(TARGETS
ps_manager
collision_spawner
DESTINATION lib/${PROJECT_NAME}
)

27
rbs_mill_assist/utils/include/ps_manager.hpp
View file

@ -1,27 +0,0 @@
#include <rclcpp/node_options.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <moveit_msgs/msg/collision_object.hpp>
#include <shape_msgs/msg/solid_primitive.hpp>
#include <geometry_msgs/msg/quaternion.hpp>
// #include "rbs_mill_interfaces/msg/
namespace rbs_mill_utils {
class ObjectManager : public rclcpp::Node {
public:
ObjectManager(const rclcpp::NodeOptions options);
private:
void attachDetachCallback();
};
}

58
rbs_mill_assist/utils/include/rbs_mill_utils/collision_spawner.hpp Normal file
View file

@ -0,0 +1,58 @@
#include "geometry_msgs/msg/quaternion.hpp"
#include "rbs_mill_interfaces/action/add_collision_object.hpp"
#include "rbs_mill_interfaces/action/check_object_exists.hpp"
#include "rbs_mill_interfaces/action/remove_collision_object.hpp"
#include "rbs_mill_assist/places_config.hpp"
#include <geometry_msgs/msg/pose.hpp>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/client.hpp>
#include <tuple>
#include <yaml-cpp/node/node.h>
class CollisionSpawner : public rclcpp::Node {
public:
struct ObjectSpec {
std::string name;
std::string type;
std::string frame_id = "world";
bool has_dimensions;
std::vector<double> dimensions;
geometry_msgs::msg::Pose pose;
std::optional<std::string> mesh_filepath;
std::tuple<double, double, double> scale_xyz = {1.0, 1.0, 1.0};
};
struct Objects {
std::optional<std::string> model_dir;
std::vector<ObjectSpec> models;
};
using AddCollisionObject = rbs_mill_interfaces::action::AddCollisionObject;
using RemoveCollisionObject =
rbs_mill_interfaces::action::RemoveCollisionObject;
using CheckObjectExists = rbs_mill_interfaces::action::CheckObjectExists;
CollisionSpawner();
private:
void removeExistingObjects();
void sendRemoveGoal(const std::string &object_id);
void spawnObjects();
void sendAddGoal(const PlacesConfig::PlaceSpec &obj_spec);
bool isObjectExist(const std::string &object_id);
std::shared_ptr<PlacesConfig> _places_config;
rclcpp_action::Client<AddCollisionObject>::SharedPtr
add_object_action_client_;
rclcpp_action::Client<RemoveCollisionObject>::SharedPtr
remove_object_action_client_;
rclcpp_action::Client<CheckObjectExists>::SharedPtr
check_object_exists_action_client_;
std::string config_file_;
};

142
rbs_mill_assist/utils/include/rbs_mill_utils/ps_manager.hpp Normal file
View file

@ -0,0 +1,142 @@
#include "moveit_msgs/msg/collision_object.hpp"
#include "rbs_mill_interfaces/action/add_collision_object.hpp"
#include "rbs_mill_interfaces/action/attach_detach_object.hpp"
#include "rbs_mill_interfaces/action/check_object_exists.hpp"
#include "rbs_mill_interfaces/action/get_object_pose.hpp"
#include "rbs_mill_interfaces/action/remove_collision_object.hpp"
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/quaternion.hpp>
#include <moveit_msgs/msg/attached_collision_object.hpp>
#include <moveit_msgs/msg/collision_object.hpp>
#include <moveit_msgs/srv/get_planning_scene.hpp>
#include <rclcpp/callback_group.hpp>
#include <rclcpp/node_options.hpp>
#include <rclcpp/publisher.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/service.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <rclcpp_action/server.hpp>
#include <shape_msgs/msg/solid_primitive.hpp>
namespace rbs_mill_utils {
class PsManager : public rclcpp::Node {
public:
using AttachDetachObject = rbs_mill_interfaces::action::AttachDetachObject;
using AddCollisionObject = rbs_mill_interfaces::action::AddCollisionObject;
using RemoveCollisionObject =
rbs_mill_interfaces::action::RemoveCollisionObject;
using CheckObjectExists = rbs_mill_interfaces::action::CheckObjectExists;
using GetObjectPose = rbs_mill_interfaces::action::GetObjectPose;
using AddGoalHandle = rclcpp_action::ServerGoalHandle<AddCollisionObject>;
using RemoveGoalHandle =
rclcpp_action::ServerGoalHandle<RemoveCollisionObject>;
using CheckGoalHandle = rclcpp_action::ServerGoalHandle<CheckObjectExists>;
using AttachDetachGoalHandle =
rclcpp_action::ServerGoalHandle<AttachDetachObject>;
using GetObjectPoseGoalHandle =
rclcpp_action::ServerGoalHandle<GetObjectPose>;
PsManager();
private:
rclcpp_action::GoalResponse
handleAddGoal(const rclcpp_action::GoalUUID &uuid,
std::shared_ptr<const AddCollisionObject::Goal> goal);
rclcpp_action::CancelResponse
handleAddCancel(const std::shared_ptr<AddGoalHandle> goal_handle);
void handleAddExecute(const std::shared_ptr<AddGoalHandle> goal_handle);
rclcpp_action::GoalResponse
handleRemoveGoal(const rclcpp_action::GoalUUID &uuid,
std::shared_ptr<const RemoveCollisionObject::Goal> goal);
rclcpp_action::CancelResponse
handleRemoveCancel(const std::shared_ptr<RemoveGoalHandle> goal_handle);
void handleRemoveExecute(const std::shared_ptr<RemoveGoalHandle> goal_handle);
rclcpp_action::GoalResponse
handleCheckExistGoal(const rclcpp_action::GoalUUID &uuid,
std::shared_ptr<const CheckObjectExists::Goal> goal);
rclcpp_action::CancelResponse
handleCheckExistCancel(const std::shared_ptr<CheckGoalHandle> goal_handle);
void
handleCheckExistExecute(const std::shared_ptr<CheckGoalHandle> goal_handle);
rclcpp_action::GoalResponse
handleAttachDetachGoal(const rclcpp_action::GoalUUID &uuid,
std::shared_ptr<const AttachDetachObject::Goal> goal);
rclcpp_action::CancelResponse handleAttachDetachCancel(
const std::shared_ptr<AttachDetachGoalHandle> goal_handle);
void handleAttachDetachExecute(
const std::shared_ptr<AttachDetachGoalHandle> goal_handle);
rclcpp_action::GoalResponse
handleGetObjectPoseGoal(const rclcpp_action::GoalUUID &uuid,
std::shared_ptr<const GetObjectPose::Goal> goal);
rclcpp_action::CancelResponse handleGetObjectPoseCancel(
const std::shared_ptr<GetObjectPoseGoalHandle> goal_handle);
void handleGetObjectPoseExecute(
const std::shared_ptr<GetObjectPoseGoalHandle> goal_handle);
moveit_msgs::msg::CollisionObject createCollisionObject(
const std::string &id, const std::string &shape,
const std::vector<double> &dimensions, const std::string &frame_id,
const geometry_msgs::msg::Pose &pose, const std::string &mesh_file,
const double scale_mesh_x, const double scale_mesh_y,
const double scale_mesh_z) const;
bool attachedObjectExists(const std::string &id,
const std::string &link_name = "");
std::optional<std::string>
getAttachedObjectLinkById(const std::string &object_id);
bool objectExists(const std::string &id);
std::optional<moveit_msgs::msg::CollisionObject>
getObjectDataById(const std::string &object_id);
void printPlanningScene();
bool
validateShapeAndDimensions(const std::string &shape,
const std::vector<double> &dimensions,
const std::optional<std::string> &mesh_file) const;
rclcpp::Publisher<moveit_msgs::msg::CollisionObject>::SharedPtr
collision_object_publisher_;
rclcpp::Publisher<moveit_msgs::msg::AttachedCollisionObject>::SharedPtr
attached_collision_object_publisher_;
rclcpp::Client<moveit_msgs::srv::GetPlanningScene>::SharedPtr
get_planning_scene_client_;
rclcpp_action::Server<AddCollisionObject>::SharedPtr
add_object_action_server_;
rclcpp_action::Server<RemoveCollisionObject>::SharedPtr
remove_object_action_server_;
rclcpp_action::Server<CheckObjectExists>::SharedPtr
check_object_exists_action_server_;
rclcpp_action::Server<AttachDetachObject>::SharedPtr
attach_detach_object_action_server_;
rclcpp_action::Server<GetObjectPose>::SharedPtr
get_object_pose_action_server_;
std::string frame_id_;
rclcpp::CallbackGroup::SharedPtr service_callback_group_;
rclcpp::CallbackGroup::SharedPtr action_callback_group_;
};
} // namespace rbs_mill_utils

432
rbs_mill_assist/utils/src/collision_spawner.cpp Normal file
View file

@ -0,0 +1,432 @@
#include "rbs_mill_utils/collision_spawner.hpp"
#include <chrono>
#include <filesystem>
#include <memory>
#include <rclcpp/executors.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp/utilities.hpp>
#include <rclcpp_action/create_client.hpp>
#include <tf2/LinearMath/Quaternion.hpp>
#include <yaml-cpp/yaml.h>
CollisionSpawner::CollisionSpawner() : Node("collision_spawner") {
this->declare_parameter("config_file", "");
config_file_ = this->get_parameter("config_file").as_string();
if (config_file_.empty()) {
RCLCPP_FATAL(this->get_logger(), "Parameter 'config_file' if missing");
rclcpp::shutdown();
return;
}
RCLCPP_FATAL(this->get_logger(), "Starting loading yaml");
try {
_places_config =
std::make_shared<PlacesConfig>(config_file_, this->get_logger());
} catch (const std::exception &ex) {
RCLCPP_FATAL(this->get_logger(), "Failure because: %s", ex.what());
}
RCLCPP_FATAL(this->get_logger(), "Ended loading yaml");
if (_places_config->get()->models.empty()) {
RCLCPP_WARN(this->get_logger(), "No valid objects to spawn");
return;
}
add_object_action_client_ = rclcpp_action::create_client<AddCollisionObject>(
this, "add_collision_object");
remove_object_action_client_ =
rclcpp_action::create_client<RemoveCollisionObject>(
this, "remove_collision_object");
check_object_exists_action_client_ =
rclcpp_action::create_client<CheckObjectExists>(this,
"check_object_exists");
if (!add_object_action_client_->wait_for_action_server(
std::chrono::seconds(10))) {
RCLCPP_FATAL(this->get_logger(),
"AddCollisionObject action server not available. shutdown");
rclcpp::shutdown();
return;
}
if (!remove_object_action_client_->wait_for_action_server(
std::chrono::seconds(10))) {
RCLCPP_FATAL(this->get_logger(),
"RemoveCollisionObject action server not available. shutdown");
rclcpp::shutdown();
return;
}
if (!check_object_exists_action_client_->wait_for_action_server(
std::chrono::seconds(10))) {
RCLCPP_WARN(this->get_logger(), "CheckObjectExist is not available");
}
removeExistingObjects();
spawnObjects();
}
// bool CollisionSpawner::loadObjectsFromYaml(const std::string &filepath) {
// YAML::Node places_config_node = YAML::LoadFile(filepath);
//
// std::string model_dir;
// if (auto node = places_config_node["model_dir"]; node && node.IsScalar()) {
// objects_to_spawn_.model_dir = node.as<std::string>();
// }
//
// if (!places_config_node["models"] ||
// !places_config_node["models"].IsSequence()) {
// RCLCPP_FATAL(this->get_logger(),
// "No valid 'models' section found in config_file");
// return false;
// }
// for (const auto &model : places_config_node["models"]) {
// ObjectSpec object;
// if (model["ps_ignore"] && model["ps_ignore"].as<bool>()) {
// continue;
// }
// object.name = model["name"].as<std::string>();
// object.type = model["type"].as<std::string>();
// const auto &type = object.type;
// if (type == "box" || type == "sphere" || type == "cylinder") {
// if (!model["dimensions"]) {
// RCLCPP_ERROR(this->get_logger(), "Model '%s' is missing
// 'dimensions'",
// object.name.c_str());
// return false;
// }
// auto d = model["dimensions"].as<std::vector<double>>();
//
// if (type == "box" && d.size() == 3) {
// object.dimensions = d;
// } else if (type == "sphere" && d.size() == 1) {
// object.dimensions = d;
// } else if (type == "cylinder" && d.size() == 2) {
// object.dimensions = d;
// } else {
// RCLCPP_ERROR(this->get_logger(),
// "Invalid dimensions count for type '%s'", type.c_str());
// return false;
// }
//
// } else if (type == "mesh") {
// if (!model["mesh_filename"]) {
// RCLCPP_ERROR(this->get_logger(), "mesh object missing
// mesh_filename"); return false;
// }
// std::string mesh_filename = model["mesh_filename"].as<std::string>();
// if (objects_to_spawn_.model_dir.has_value()) {
// object.mesh_filepath =
// std::filesystem::path(objects_to_spawn_.model_dir->c_str()) /
// object.name / "meshes" / mesh_filename;
// RCLCPP_WARN(this->get_logger(), "mesh filepath is: %s",
// object.mesh_filepath->c_str());
// } else {
// RCLCPP_ERROR(this->get_logger(),
// "mesh_filename in empty in config_file");
// return false;
// }
//
// } else {
// RCLCPP_ERROR(this->get_logger(), "Unrecognized object type: '%s'",
// type.c_str());
// return false;
// }
// if (model["frame_id"]) {
// object.frame_id = model["frame_id"].as<std::string>();
// }
// if (model["pose"]) {
// auto pose = model["pose"];
//
// if (pose["position"]) {
// auto position = model["pose"]["position"];
// object.pose.position.x = position["x"].as<double>();
// object.pose.position.y = position["y"].as<double>();
// object.pose.position.z = position["z"].as<double>();
// }
// if (pose["orientation"]) {
// auto orientation = model["pose"]["orientation"];
// if (!isQuat(orientation)) {
// auto quat = rpyToQuat(orientation["r"].as<double>(),
// orientation["p"].as<double>(),
// orientation["y"].as<double>());
//
// object.pose.orientation.x = quat.x;
// object.pose.orientation.y = quat.y;
// object.pose.orientation.z = quat.z;
// object.pose.orientation.w = quat.w;
// } else {
// object.pose.orientation.x = orientation["x"].as<double>();
// object.pose.orientation.y = orientation["y"].as<double>();
// object.pose.orientation.z = orientation["z"].as<double>();
// object.pose.orientation.w = orientation["w"].as<double>();
// }
// }
// }
//
// objects_to_spawn_.models.push_back(object);
// }
//
// return true;
// }
void CollisionSpawner::removeExistingObjects() {
RCLCPP_INFO(this->get_logger(), "Removing existing objects...");
for (const auto &obj : _places_config->get()->models) {
auto object_id = obj.name;
// 1. Check if it exists
if (!isObjectExist(object_id)) {
RCLCPP_INFO(this->get_logger(),
"Object '%s' does not exist. Skipping removal.",
object_id.c_str());
continue;
}
// 2. Proceed with remove
sendRemoveGoal(object_id);
}
}
void CollisionSpawner::sendRemoveGoal(const std::string &object_id) {
auto goal = RemoveCollisionObject::Goal();
goal.id = object_id;
RCLCPP_INFO(this->get_logger(), "Sending remove goal for object '%s'.",
object_id.c_str());
auto send_goal_future = remove_object_action_client_->async_send_goal(goal);
// Wait for the goal to be accepted
if (rclcpp::spin_until_future_complete(
this->get_node_base_interface(), send_goal_future,
std::chrono::seconds(5)) != rclcpp::FutureReturnCode::SUCCESS) {
RCLCPP_WARN(this->get_logger(),
"Timeout while sending remove goal for object '%s'.",
object_id.c_str());
return;
}
auto goal_handle = send_goal_future.get();
if (!goal_handle) {
RCLCPP_ERROR(this->get_logger(),
"Remove goal was rejected for object '%s'.",
object_id.c_str());
return;
}
// Wait for the result
auto get_result_future =
remove_object_action_client_->async_get_result(goal_handle);
if (rclcpp::spin_until_future_complete(
this->get_node_base_interface(), get_result_future,
std::chrono::seconds(5)) != rclcpp::FutureReturnCode::SUCCESS) {
RCLCPP_WARN(this->get_logger(),
"Timeout while waiting for remove result for object '%s'.",
object_id.c_str());
return;
}
auto result = get_result_future.get();
switch (result.code) {
case rclcpp_action::ResultCode::SUCCEEDED:
RCLCPP_INFO(this->get_logger(), "Successfully removed object '%s'.",
object_id.c_str());
break;
case rclcpp_action::ResultCode::ABORTED:
RCLCPP_WARN(this->get_logger(),
"Remove action was aborted for object '%s'.",
object_id.c_str());
break;
case rclcpp_action::ResultCode::CANCELED:
RCLCPP_WARN(this->get_logger(),
"Remove action was canceled for object '%s'.",
object_id.c_str());
break;
default:
RCLCPP_WARN(this->get_logger(),
"Unknown result code for remove action of object '%s'.",
object_id.c_str());
break;
}
}
void CollisionSpawner::spawnObjects() {
for (const auto &obj : _places_config->get()->models) {
sendAddGoal(obj);
}
}
void CollisionSpawner::sendAddGoal(const PlacesConfig::PlaceSpec &obj_spec) {
auto goal = AddCollisionObject::Goal();
goal.id = obj_spec.name;
goal.shape = obj_spec.type;
goal.dimensions =
obj_spec.type == "mesh" ? std::vector<double>() : obj_spec.dimensions;
goal.pose = obj_spec.pose;
if (obj_spec.type == "mesh") {
if (!obj_spec.mesh_filepath.has_value()) {
RCLCPP_FATAL(this->get_logger(),
"Mesh filepath has no value for name: %s",
obj_spec.name.c_str());
}
goal.mesh_file = obj_spec.mesh_filepath.value();
auto [sx, sy, sz] = obj_spec.scale_xyz;
goal.scale_mesh_x = sx;
goal.scale_mesh_y = sy;
goal.scale_mesh_z = sz;
}
RCLCPP_INFO(this->get_logger(), "Sending add goal for object '%s'.",
obj_spec.name.c_str());
auto send_goal_future = add_object_action_client_->async_send_goal(goal);
// Wait for the goal to be accepted
if (rclcpp::spin_until_future_complete(
this->get_node_base_interface(), send_goal_future,
std::chrono::seconds(5)) != rclcpp::FutureReturnCode::SUCCESS) {
RCLCPP_WARN(this->get_logger(),
"Timeout while sending add goal for object '%s'.",
obj_spec.name.c_str());
return;
}
auto goal_handle = send_goal_future.get();
if (!goal_handle) {
RCLCPP_ERROR(this->get_logger(), "Add goal was rejected for object '%s'.",
obj_spec.name.c_str());
return;
}
// Wait for the result
auto get_result_future =
add_object_action_client_->async_get_result(goal_handle);
if (rclcpp::spin_until_future_complete(
this->get_node_base_interface(), get_result_future,
std::chrono::seconds(5)) != rclcpp::FutureReturnCode::SUCCESS) {
RCLCPP_WARN(this->get_logger(),
"Timeout while waiting for add result for object '%s'.",
obj_spec.name.c_str());
return;
}
auto result = get_result_future.get();
switch (result.code) {
case rclcpp_action::ResultCode::SUCCEEDED:
RCLCPP_INFO(this->get_logger(), "Successfully added object '%s'.",
obj_spec.name.c_str());
break;
case rclcpp_action::ResultCode::ABORTED:
RCLCPP_WARN(this->get_logger(), "Add action was aborted for object '%s'.",
obj_spec.name.c_str());
break;
case rclcpp_action::ResultCode::CANCELED:
RCLCPP_WARN(this->get_logger(), "Add action was canceled for object '%s'.",
obj_spec.name.c_str());
break;
default:
RCLCPP_WARN(this->get_logger(),
"Unknown result code for add action of object '%s'.",
obj_spec.name.c_str());
break;
}
}
bool CollisionSpawner::isObjectExist(const std::string &object_id) {
if (!check_object_exists_action_client_->action_server_is_ready()) {
RCLCPP_WARN(this->get_logger(),
"CheckObjectExists action server not ready. Assuming object "
"'%s' does not exist.",
object_id.c_str());
return false;
}
auto goal_msg = CheckObjectExists::Goal();
goal_msg.object_id = object_id;
RCLCPP_INFO(this->get_logger(),
"Sending CheckObjectExists goal for object '%s'.",
object_id.c_str());
auto send_goal_future =
check_object_exists_action_client_->async_send_goal(goal_msg);
// Wait for the goal to be accepted
if (rclcpp::spin_until_future_complete(
this->get_node_base_interface(), send_goal_future,
std::chrono::seconds(3)) != rclcpp::FutureReturnCode::SUCCESS) {
RCLCPP_WARN(this->get_logger(),
"Timeout while sending CheckObjectExists goal for object '%s'. "
"Assuming it does not exist.",
object_id.c_str());
return false;
}
auto goal_handle = send_goal_future.get();
if (!goal_handle) {
RCLCPP_ERROR(this->get_logger(),
"CheckObjectExists goal was rejected for object '%s'. "
"Assuming it does not exist.",
object_id.c_str());
return false;
}
// Wait for the result
auto get_result_future =
check_object_exists_action_client_->async_get_result(goal_handle);
if (rclcpp::spin_until_future_complete(
this->get_node_base_interface(), get_result_future,
std::chrono::seconds(3)) != rclcpp::FutureReturnCode::SUCCESS) {
RCLCPP_WARN(this->get_logger(),
"Timeout while waiting for CheckObjectExists result for object "
"'%s'. Assuming it does not exist.",
object_id.c_str());
return false;
}
auto result = get_result_future.get().result;
RCLCPP_INFO(this->get_logger(),
"CheckObjectExists result for object '%s': exists=%s",
object_id.c_str(), result->ok ? "true" : "false");
return result->ok;
}
// bool CollisionSpawner::isQuat(const YAML::Node &node) {
// if (node["x"] && node["y"] && node["z"] && node["w"]) {
// return true;
// } else if (node["r"] && node["p"] && node["y"]) {
// return false;
// }
// throw std::invalid_argument("Provided YAML Node is not quat or euler
// angles");
// }
// geometry_msgs::msg::Quaternion
// CollisionSpawner::rpyToQuat(double roll, double pitch, double yaw) {
// tf2::Quaternion q;
// q.setRPY(roll, pitch, yaw);
// q.normalize();
// geometry_msgs::msg::Quaternion q_msg;
// q_msg.x = q.x();
// q_msg.y = q.y();
// q_msg.z = q.z();
// q_msg.w = q.w();
// return q_msg;
// }
int main(int argc, char *argv[]) {
rclcpp::init(argc, argv);
auto node = std::make_shared<CollisionSpawner>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}

1165
rbs_mill_assist/utils/src/ps_manager.cpp
View file

File diff suppressed because it is too large Load diff

29
rbs_mill_assist/world/config/places.yaml
View file

@ -1,7 +1,19 @@
model_dir: package://rbs_mill_assist/assets/
models:
- name: table
type: box
dimensions: [1.2, 0.7, 0.8]
gz_ignore: true
pose:
position:
x: 0.0
y: 0.0
z: -0.4
- name: bunker
mesh_filename: Bunker_50mm_height_for_labels_60х40.STL
type: mesh
gz_ignore: true
#relative_to: world
pose:
position:
@ -15,6 +27,7 @@ models:
- name: bunker_4_slots
mesh_filename: Bunker_50mm_height_for_labels_60х40_4.STL
type: mesh
pose:
position:
x: -0.150
@ -27,6 +40,9 @@ models:
- name: aruco_1
mesh_filename: aruco_template.obj
type: mesh
ps_ignore: true
gz_ignore: true
pose:
position:
x: -0.235
@ -39,6 +55,9 @@ models:
- name: aruco_2
mesh_filename: aruco_template.obj
type: mesh
ps_ignore: true
gz_ignore: true
pose:
position:
x: -0.191
@ -51,6 +70,9 @@ models:
- name: aruco_3
mesh_filename: aruco_template.obj
type: mesh
ps_ignore: true
gz_ignore: true
pose:
position:
x: -0.147
@ -63,6 +85,9 @@ models:
- name: aruco_4
mesh_filename: aruco_template.obj
ps_ignore: true
gz_ignore: true
type: mesh
pose:
position:
x: -0.103
@ -87,14 +112,16 @@ models:
- name: conductor
mesh_filename: conductor.stl
type: mesh
pose:
position:
x: 0.220
x: 0.210
y: -0.170
z: 0.01
- name: laser
mesh_filename: laser.dae
type: mesh
pose:
position:
x: 0.250

37
rbs_mill_assist/world/config/ref_frames.yaml
View file

@ -40,6 +40,43 @@ ref_frames:
p: 0.0
y: 0.0
- name: preparing_grasp_slot
relative_to: bunker_4_slots
pose:
position:
x: 0.0
y: 0.0
z: 0.2
orientation:
r: 0.0
p: 3.14
y: 1.57
- name: preparing_place_slot
relative_to: bunker
pose:
position:
x: 0.0
y: 0.0
z: 0.2
orientation:
r: 0.0
p: 3.14
y: 1.57
- name: to_graver_prom
relative_to: world
pose:
position:
x: 0.06557199290989431
y: 0.13898459045940778
z: 0.12372277569863609
orientation:
x: 0.9350606047731165
y: -0.3537940621486448
z: 0.00805682110103744
w: 0.02065223047131592
- name: pregrasp_1
relative_to: slot_1
pose:

31
rbs_mill_assist/world/inc/from_places.xacro
View file

@ -4,48 +4,47 @@
<xacro:macro name="from_places" params="models:=^">
<xacro:if value="${models}">
<xacro:property name="model" value="${models.pop(0)}" />
<xacro:if value="${model.get('gz_ignore', False) is False}">
<xacro:model model_name="${model['name']}" static="true">
<xacro:if value="${model.get('relative_to', None) is not None}">
<pose relative_to="${model['relative_to']}">
${model["pose"]["position"]["x"]}
${model["pose"]["position"]["y"]}
${model["pose"]["position"]["z"]}
<xacro:if value="${'orientation' in model['pose']}">
<pose relative_to="${model['relative_to']}"> ${model["pose"]["position"]["x"]}
${model["pose"]["position"]["y"]} ${model["pose"]["position"]["z"]} <xacro:if
value="${'orientation' in model['pose']}">
<xacro:property name="orientation" value="${model['pose']['orientation']}" />
<xacro:if value="${'r' in orientation and 'p' in orientation and 'y' in orientation}">
<xacro:if
value="${'r' in orientation and 'p' in orientation and 'y' in orientation}">
${orientation['r']}
${orientation['p']}
${orientation['y']}
</xacro:if>
</xacro:if>
<xacro:unless value="${'orientation' in model['pose']}">
<xacro:unless
value="${'orientation' in model['pose']}">
0 0 0
</xacro:unless>
</pose>
</xacro:if>
<xacro:unless value="${model.get('relative_to', None) is not None}">
<pose>
${model["pose"]["position"]["x"]}
${model["pose"]["position"]["y"]}
${model["pose"]["position"]["z"]}
<xacro:if value="${'orientation' in model['pose']}">
<pose> ${model["pose"]["position"]["x"]} ${model["pose"]["position"]["y"]}
${model["pose"]["position"]["z"]} <xacro:if value="${'orientation' in model['pose']}">
<xacro:property name="orientation" value="${model['pose']['orientation']}" />
<xacro:if value="${'r' in orientation and 'p' in orientation and 'y' in orientation}">
<xacro:if
value="${'r' in orientation and 'p' in orientation and 'y' in orientation}">
${orientation['r']}
${orientation['p']}
${orientation['y']}
</xacro:if>
</xacro:if>
<xacro:unless value="${'orientation' in model['pose']}">
<xacro:unless
value="${'orientation' in model['pose']}">
0 0 0
</xacro:unless>
</pose>
</xacro:unless>
</xacro:model>
</xacro:if>
<xacro:from_places />
</xacro:if>
</xacro:macro>

5
rbs_mill_interfaces/CMakeLists.txt
View file

@ -11,6 +11,11 @@ find_package(rosidl_default_generators REQUIRED)
find_package(geometry_msgs REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"action/AttachDetachObject.action"
"action/GetObjectPose.action"
"action/CheckObjectExists.action"
"action/AddCollisionObject.action"
"action/RemoveCollisionObject.action"
"msg/GraspingPose.msg"
"srv/GetGraspingPose.srv"
DEPENDENCIES geometry_msgs

17
rbs_mill_interfaces/action/AddCollisionObject.action Normal file
View file

@ -0,0 +1,17 @@
string id
string shape
string mesh_file
float64[] dimensions
string frame_id 'world'
geometry_msgs/Pose pose
float64 scale_mesh_x
float64 scale_mesh_y
float64 scale_mesh_z
---
bool ok
string message
---
string status

9
rbs_mill_interfaces/action/AttachDetachObject.action Normal file
View file

@ -0,0 +1,9 @@
string object_id
string link_name
bool attach
string[] touch_links
---
bool ok
string message
---
string status

8
rbs_mill_interfaces/action/CheckObjectExists.action Normal file
View file

@ -0,0 +1,8 @@
string object_id
---
bool ok
bool is_attached
string link_name
string message
---

10
rbs_mill_interfaces/action/GetObjectPose.action Normal file
View file

@ -0,0 +1,10 @@
string object_id
string link_name
float64[] pre_transform_xyz_rpy
float64[] post_transform_xyz_rpy
---
geometry_msgs/Pose pose
bool ok
string message
---
string status

6
rbs_mill_interfaces/action/RemoveCollisionObject.action Normal file
View file

@ -0,0 +1,6 @@
string id
---
bool ok
string message
---
string status

0
rbs_mill_interfaces/srv/AttachDetachObject.srv
View file