diff --git a/rbs_mill_assist/include/rbs_mill_assist/places_config.hpp b/rbs_mill_assist/include/rbs_mill_assist/places_config.hpp index db7ce36..10a411f 100644 --- a/rbs_mill_assist/include/rbs_mill_assist/places_config.hpp +++ b/rbs_mill_assist/include/rbs_mill_assist/places_config.hpp @@ -1,10 +1,12 @@ #include "geometry_msgs/msg/pose.hpp" #include +#include #include #include +#include +#include #include #include -#include #include #include #include @@ -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(); + 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 getMeshFilenames() { - return _model_mesh_filenames; - } - inline std::vector getMeshPoses(){ - return _model_poses; - } - inline std::vector getMeshFilepaths() { - return _model_mesh_filepaths; - } - inline std::vector getModelNames() { - return _model_names; - } - inline std::vector 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 dimensions; + geometry_msgs::msg::Pose pose; + std::optional mesh_filepath; + std::tuple scale_xyz = {1.0, 1.0, 1.0}; + }; + + struct Places { + std::optional model_dir; + std::vector models; + }; + + inline std::shared_ptr get() const { return objects_to_spawn_; } private: - std::vector _model_names; - std::vector _model_mesh_filenames; - std::vector _model_poses; - std::vector _model_mesh_filepaths; - std::vector _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(); + } + + 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()) { + continue; + } + object.name = model["name"].as(); + object.type = model["type"].as(); + 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>(); + + 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(); + 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(); + } + if (model["pose"]) { + auto pose = model["pose"]; + + if (pose["position"]) { + auto position = model["pose"]["position"]; + object.pose.position.x = position["x"].as(); + object.pose.position.y = position["y"].as(); + object.pose.position.z = position["z"].as(); + } + if (pose["orientation"]) { + auto orientation = model["pose"]["orientation"]; + if (!isQuat(orientation)) { + auto quat = rpyToQuat(orientation["r"].as(), + orientation["p"].as(), + orientation["y"].as()); + + 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(); + object.pose.orientation.y = orientation["y"].as(); + object.pose.orientation.z = orientation["z"].as(); + object.pose.orientation.w = orientation["w"].as(); + } + } + } + + 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(); - quat.y = node["y"].as(); - quat.z = node["z"].as(); - quat.w = node["w"].as(); - } else { - tf2::Quaternion tf2_quat; - tf2_quat.setRPY(node["r"].as(), node["p"].as(), - node["y"].as()); - quat.x = tf2_quat.x(); - quat.y = tf2_quat.y(); - quat.z = tf2_quat.z(); - quat.w = tf2_quat.w(); - } - return quat; + 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; } - 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(); - } - - 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 model_parent = "world"; - if (model["relative_to"].IsDefined()) { - model_parent = model["relative_to"].as(); - } - std::string model_mesh_filename = - model["mesh_filename"].as(); - - _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(); - model_pose.position.y = position["y"].as(); - model_pose.position.z = position["z"].as(); - - 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 objects_to_spawn_; }; diff --git a/rbs_mill_assist/include/rbs_mill_assist/ref_frames_config.hpp b/rbs_mill_assist/include/rbs_mill_assist/ref_frames_config.hpp index b3f233a..0755f52 100644 --- a/rbs_mill_assist/include/rbs_mill_assist/ref_frames_config.hpp +++ b/rbs_mill_assist/include/rbs_mill_assist/ref_frames_config.hpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include @@ -11,20 +12,20 @@ public: RefFramesConfig(const std::string &ref_frames_filepath) { loadConfig(ref_frames_filepath); } - inline std::vector getRefFrameNames() { - return _ref_frame_names; - } - inline std::vector getRefFrameParents() { - return _ref_frame_parents; - } - inline std::vector getRefFramePoses() { - return _ref_frame_poses; + + struct RefFrame { + std::string name; + std::string relative_to; + geometry_msgs::msg::Pose pose; + }; + + inline std::vector get() const { + return _ref_frames; } + private: - std::vector _ref_frame_names; - std::vector _ref_frame_parents; - std::vector _ref_frame_poses; + std::vector _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()); - _ref_frame_parents.push_back(frame["relative_to"].as()); + ref_frame.name = frame["name"].as(); + ref_frame.relative_to = frame["relative_to"].as(); geometry_msgs::msg::Pose frame_pose; frame_pose.position.x = frame["pose"]["position"]["x"].as(); @@ -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); } } diff --git a/rbs_mill_assist/launch/moveit.launch.py b/rbs_mill_assist/launch/moveit.launch.py index 803e22b..c4b1424 100644 --- a/rbs_mill_assist/launch/moveit.launch.py +++ b/rbs_mill_assist/launch/moveit.launch.py @@ -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 diff --git a/rbs_mill_assist/src/CMakeLists.txt b/rbs_mill_assist/src/CMakeLists.txt index 48e8dcd..0c5e78d 100644 --- a/rbs_mill_assist/src/CMakeLists.txt +++ b/rbs_mill_assist/src/CMakeLists.txt @@ -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 diff --git a/rbs_mill_assist/src/places_frame_publisher.cpp b/rbs_mill_assist/src/places_frame_publisher.cpp index faa6a98..1f4a8ae 100644 --- a/rbs_mill_assist/src/places_frame_publisher.cpp +++ b/rbs_mill_assist/src/places_frame_publisher.cpp @@ -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(places_config_filepath); + auto places_config = std::make_shared(places_config_filepath, this->get_logger()); auto ref_frames = std::make_shared(ref_frames_config_filepath); @@ -32,15 +32,16 @@ public: private: void publish_ref_frames(std::shared_ptr &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 &places_config) { - for (size_t i = 0; i < places_config->getModelNames().size(); i++) { + void publish_places_frames(std::shared_ptr 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; diff --git a/rbs_mill_assist/utils/CMakeLists.txt b/rbs_mill_assist/utils/CMakeLists.txt index e69de29..afcb9d6 100644 --- a/rbs_mill_assist/utils/CMakeLists.txt +++ b/rbs_mill_assist/utils/CMakeLists.txt @@ -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} +) + diff --git a/rbs_mill_assist/utils/include/ps_manager.hpp b/rbs_mill_assist/utils/include/ps_manager.hpp deleted file mode 100644 index 89c776f..0000000 --- a/rbs_mill_assist/utils/include/ps_manager.hpp +++ /dev/null @@ -1,27 +0,0 @@ - -#include -#include -#include -#include -#include -#include -#include -// #include "rbs_mill_interfaces/msg/ - - -namespace rbs_mill_utils { - -class ObjectManager : public rclcpp::Node { - - public: - - ObjectManager(const rclcpp::NodeOptions options); - - private: - - void attachDetachCallback(); - -}; - - -} diff --git a/rbs_mill_assist/utils/include/rbs_mill_utils/collision_spawner.hpp b/rbs_mill_assist/utils/include/rbs_mill_utils/collision_spawner.hpp new file mode 100644 index 0000000..485daa1 --- /dev/null +++ b/rbs_mill_assist/utils/include/rbs_mill_utils/collision_spawner.hpp @@ -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 +#include +#include +#include +#include +#include + +class CollisionSpawner : public rclcpp::Node { + +public: + struct ObjectSpec { + std::string name; + std::string type; + std::string frame_id = "world"; + bool has_dimensions; + std::vector dimensions; + geometry_msgs::msg::Pose pose; + std::optional mesh_filepath; + std::tuple scale_xyz = {1.0, 1.0, 1.0}; + }; + + struct Objects { + std::optional model_dir; + std::vector 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 _places_config; + + rclcpp_action::Client::SharedPtr + add_object_action_client_; + rclcpp_action::Client::SharedPtr + remove_object_action_client_; + rclcpp_action::Client::SharedPtr + check_object_exists_action_client_; + std::string config_file_; +}; diff --git a/rbs_mill_assist/utils/include/rbs_mill_utils/ps_manager.hpp b/rbs_mill_assist/utils/include/rbs_mill_utils/ps_manager.hpp new file mode 100644 index 0000000..c512b88 --- /dev/null +++ b/rbs_mill_assist/utils/include/rbs_mill_utils/ps_manager.hpp @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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; + using RemoveGoalHandle = + rclcpp_action::ServerGoalHandle; + using CheckGoalHandle = rclcpp_action::ServerGoalHandle; + using AttachDetachGoalHandle = + rclcpp_action::ServerGoalHandle; + using GetObjectPoseGoalHandle = + rclcpp_action::ServerGoalHandle; + + PsManager(); + +private: + rclcpp_action::GoalResponse + handleAddGoal(const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal); + + rclcpp_action::CancelResponse + handleAddCancel(const std::shared_ptr goal_handle); + void handleAddExecute(const std::shared_ptr goal_handle); + rclcpp_action::GoalResponse + handleRemoveGoal(const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal); + rclcpp_action::CancelResponse + handleRemoveCancel(const std::shared_ptr goal_handle); + void handleRemoveExecute(const std::shared_ptr goal_handle); + rclcpp_action::GoalResponse + handleCheckExistGoal(const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal); + rclcpp_action::CancelResponse + handleCheckExistCancel(const std::shared_ptr goal_handle); + void + handleCheckExistExecute(const std::shared_ptr goal_handle); + + rclcpp_action::GoalResponse + handleAttachDetachGoal(const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal); + + rclcpp_action::CancelResponse handleAttachDetachCancel( + const std::shared_ptr goal_handle); + + void handleAttachDetachExecute( + const std::shared_ptr goal_handle); + + rclcpp_action::GoalResponse + handleGetObjectPoseGoal(const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal); + + rclcpp_action::CancelResponse handleGetObjectPoseCancel( + const std::shared_ptr goal_handle); + + void handleGetObjectPoseExecute( + const std::shared_ptr goal_handle); + + moveit_msgs::msg::CollisionObject createCollisionObject( + const std::string &id, const std::string &shape, + const std::vector &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 + getAttachedObjectLinkById(const std::string &object_id); + + bool objectExists(const std::string &id); + + std::optional + getObjectDataById(const std::string &object_id); + + void printPlanningScene(); + + bool + validateShapeAndDimensions(const std::string &shape, + const std::vector &dimensions, + const std::optional &mesh_file) const; + + rclcpp::Publisher::SharedPtr + collision_object_publisher_; + + rclcpp::Publisher::SharedPtr + attached_collision_object_publisher_; + rclcpp::Client::SharedPtr + get_planning_scene_client_; + + rclcpp_action::Server::SharedPtr + add_object_action_server_; + + rclcpp_action::Server::SharedPtr + remove_object_action_server_; + + rclcpp_action::Server::SharedPtr + check_object_exists_action_server_; + + rclcpp_action::Server::SharedPtr + attach_detach_object_action_server_; + + rclcpp_action::Server::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 diff --git a/rbs_mill_assist/utils/src/collision_spawner.cpp b/rbs_mill_assist/utils/src/collision_spawner.cpp new file mode 100644 index 0000000..21c6a66 --- /dev/null +++ b/rbs_mill_assist/utils/src/collision_spawner.cpp @@ -0,0 +1,432 @@ +#include "rbs_mill_utils/collision_spawner.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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(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( + this, "add_collision_object"); + remove_object_action_client_ = + rclcpp_action::create_client( + this, "remove_collision_object"); + check_object_exists_action_client_ = + rclcpp_action::create_client(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(); +// } +// +// 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()) { +// continue; +// } +// object.name = model["name"].as(); +// object.type = model["type"].as(); +// 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>(); +// +// 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(); +// 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(); +// } +// if (model["pose"]) { +// auto pose = model["pose"]; +// +// if (pose["position"]) { +// auto position = model["pose"]["position"]; +// object.pose.position.x = position["x"].as(); +// object.pose.position.y = position["y"].as(); +// object.pose.position.z = position["z"].as(); +// } +// if (pose["orientation"]) { +// auto orientation = model["pose"]["orientation"]; +// if (!isQuat(orientation)) { +// auto quat = rpyToQuat(orientation["r"].as(), +// orientation["p"].as(), +// orientation["y"].as()); +// +// 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(); +// object.pose.orientation.y = orientation["y"].as(); +// object.pose.orientation.z = orientation["z"].as(); +// object.pose.orientation.w = orientation["w"].as(); +// } +// } +// } +// +// 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() : 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(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/rbs_mill_assist/utils/src/ps_manager.cpp b/rbs_mill_assist/utils/src/ps_manager.cpp index e69de29..67c6810 100644 --- a/rbs_mill_assist/utils/src/ps_manager.cpp +++ b/rbs_mill_assist/utils/src/ps_manager.cpp @@ -0,0 +1,1165 @@ +#include "rbs_mill_utils/ps_manager.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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rbs_mill_utils { + +PsManager::PsManager() : Node("ps_manager") { + this->declare_parameter("frame_id", "world"); + frame_id_ = this->get_parameter("frame_id").as_string(); + + collision_object_publisher_ = + this->create_publisher( + "/collision_object", 10); + attached_collision_object_publisher_ = + this->create_publisher( + "/attached_collision_object", 10); + + service_callback_group_ = + this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + get_planning_scene_client_ = + this->create_client( + "/get_planning_scene", rclcpp::QoS(5), service_callback_group_); + + RCLCPP_INFO(this->get_logger(), + "Waiting for /get_planning_scene service ..."); + if (!get_planning_scene_client_->wait_for_service(std::chrono::seconds(10))) { + + RCLCPP_FATAL(this->get_logger(), + "/get_planning_scene service is not available. Shutting down"); + rclcpp::shutdown(); + return; + } + RCLCPP_INFO(this->get_logger(), "/get_planning_scene service is available"); + + printPlanningScene(); + + action_callback_group_ = + this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + auto serverOptions = rcl_action_server_get_default_options(); + serverOptions.result_service_qos = rmw_qos_profile_services_default; + // Initialize action servers + add_object_action_server_ = rclcpp_action::create_server( + this, "add_collision_object", + std::bind(&PsManager::handleAddGoal, this, std::placeholders::_1, + std::placeholders::_2), + std::bind(&PsManager::handleAddCancel, this, std::placeholders::_1), + std::bind(&PsManager::handleAddExecute, this, std::placeholders::_1), + serverOptions, action_callback_group_); + + remove_object_action_server_ = + rclcpp_action::create_server( + this, "remove_collision_object", + std::bind(&PsManager::handleRemoveGoal, this, std::placeholders::_1, + std::placeholders::_2), + std::bind(&PsManager::handleRemoveCancel, this, + std::placeholders::_1), + std::bind(&PsManager::handleRemoveExecute, this, + std::placeholders::_1), + serverOptions, action_callback_group_); + + // Initialize CheckObjectExists action server (New Action Server) + check_object_exists_action_server_ = + rclcpp_action::create_server( + this, "check_object_exists", + std::bind(&PsManager::handleCheckExistGoal, this, + std::placeholders::_1, std::placeholders::_2), + std::bind(&PsManager::handleCheckExistCancel, this, + std::placeholders::_1), + std::bind(&PsManager::handleCheckExistExecute, this, + std::placeholders::_1), + serverOptions, action_callback_group_); + + attach_detach_object_action_server_ = + rclcpp_action::create_server( + this, "attach_detach_object", + std::bind(&PsManager::handleAttachDetachGoal, this, + std::placeholders::_1, std::placeholders::_2), + std::bind(&PsManager::handleAttachDetachCancel, this, + std::placeholders::_1), + std::bind(&PsManager::handleAttachDetachExecute, this, + std::placeholders::_1), + serverOptions, action_callback_group_); + + get_object_pose_action_server_ = rclcpp_action::create_server( + this, "get_object_pose", + std::bind(&PsManager::handleGetObjectPoseGoal, this, + std::placeholders::_1, std::placeholders::_2), + std::bind(&PsManager::handleGetObjectPoseCancel, this, + std::placeholders::_1), + std::bind(&PsManager::handleGetObjectPoseExecute, this, + std::placeholders::_1), + serverOptions, action_callback_group_); + + RCLCPP_INFO(this->get_logger(), + "Object Manager Node initialized with frame_id: %s.", + frame_id_.c_str()); +} + +rclcpp_action::GoalResponse PsManager::handleCheckExistGoal( + [[maybe_unused]] const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal) { + RCLCPP_INFO(this->get_logger(), + "Received request to check existence of object: %s", + goal->object_id.c_str()); + + // Always accept the goal as it's a simple check + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse PsManager::handleCheckExistCancel( + const std::shared_ptr goal_handle) { + (void)goal_handle; // Suppress unused parameter warning + RCLCPP_INFO(this->get_logger(), "CheckExist operation canceled."); + return rclcpp_action::CancelResponse::ACCEPT; +} + +void PsManager::handleCheckExistExecute( + const std::shared_ptr goal_handle) { + auto goal = goal_handle->get_goal(); + auto result = std::make_shared(); + + // Check if the object exists + bool collision_object_exists = objectExists(goal->object_id); + bool attached_object_exists = attachedObjectExists(goal->object_id); + bool exists = (collision_object_exists || attached_object_exists); + + result->ok = exists; + result->is_attached = attached_object_exists; + + if (attached_object_exists) { + result->link_name = getAttachedObjectLinkById(goal->object_id)->c_str(); + + std::ostringstream result_message; + result_message << "Object exists in the planning scene, attached to '" + << result->link_name.c_str() << "'."; + + result->message = (result_message.str()); + + RCLCPP_INFO(this->get_logger(), + "CheckExist: Object '%s' exists attached to link '%s'.", + goal->object_id.c_str(), result->link_name.c_str()); + } else { + result->link_name = ""; + result->message = exists ? "Object exists in the planning scene." + : "Object not found in the planning scene."; + + RCLCPP_INFO(this->get_logger(), + "CheckExist: Object '%s' %s in the planning scene.", + goal->object_id.c_str(), exists ? "exists" : "does NOT exist"); + } + + // Set the result and succeed the goal + goal_handle->succeed(result); +} + +void PsManager::printPlanningScene() { + + auto request = + std::make_shared(); + request->components.components = + moveit_msgs::msg::PlanningSceneComponents::WORLD_OBJECT_NAMES | + moveit_msgs::msg::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS; + + RCLCPP_INFO(this->get_logger(), + "Requesting Planning Scene with components: %d", + request->components.components); + + auto future_response = + get_planning_scene_client_->async_send_request(request); + + // Spin the node manually to process service callbacks + auto start_time = std::chrono::steady_clock::now(); + const auto timeout = std::chrono::seconds(5); + + while (std::chrono::steady_clock::now() - start_time < timeout) { + rclcpp::spin_some(this->get_node_base_interface()); + if (future_response.wait_for(std::chrono::milliseconds(100)) == + std::future_status::ready) { + break; + } + } + + // Handle the service response + if (future_response.wait_for(std::chrono::milliseconds(0)) == + std::future_status::ready) { + auto response = future_response.get(); + if (response) { + RCLCPP_INFO(this->get_logger(), "Received Planning Scene:"); + const auto &collision_objects = response->scene.world.collision_objects; + + if (collision_objects.empty()) { + RCLCPP_INFO(this->get_logger(), + " No collision objects found in the planning scene."); + } else { + RCLCPP_INFO(this->get_logger(), " Found %zu collision object(s):", + collision_objects.size()); + for (const auto &obj : collision_objects) { + RCLCPP_INFO(this->get_logger(), " - ID: '%s'", obj.id.c_str()); + } + } + + const auto &attached_collision_object = + response->scene.robot_state.attached_collision_objects; + + if (attached_collision_object.empty()) { + RCLCPP_INFO( + this->get_logger(), + " No attached collision objects found in the planning scene."); + } else { + RCLCPP_INFO(this->get_logger(), + " Found %zu attached collision object(s):", + attached_collision_object.size()); + for (const auto &attached_obj : attached_collision_object) { + RCLCPP_INFO(this->get_logger(), " - ID: '%s'", + attached_obj.object.id.c_str()); + } + } + } else { + RCLCPP_ERROR(this->get_logger(), + "Received null response from /get_planning_scene."); + } + } else { + RCLCPP_ERROR(this->get_logger(), + "Timeout while waiting for /get_planning_scene response."); + } +} + +rclcpp_action::GoalResponse +PsManager::handleAddGoal([[maybe_unused]] const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal) { + RCLCPP_INFO(this->get_logger(), "Received request to add object: %s", + goal->id.c_str()); + + // if (!validateShapeAndDimensions(goal->shape, goal->dimensions, + // goal->mesh_file)) { + // RCLCPP_WARN(this->get_logger(), + // "Invalid shape/dimensions or missing mesh file for object."); + // return rclcpp_action::GoalResponse::REJECT; + // } else + if (objectExists(goal->id)) { + RCLCPP_WARN(this->get_logger(), "Object already in the planning scene."); + return rclcpp_action::GoalResponse::REJECT; + } else if (attachedObjectExists(goal->id)) { + RCLCPP_WARN(this->get_logger(), + "Object already in the planning scene, attached to %s.", + getAttachedObjectLinkById(goal->id)->c_str()); + return rclcpp_action::GoalResponse::REJECT; + } + + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse PsManager::handleAddCancel( + [[maybe_unused]] const std::shared_ptr goal_handle) { + RCLCPP_INFO(this->get_logger(), "Add operation canceled."); + return rclcpp_action::CancelResponse::ACCEPT; +} + +void PsManager::handleAddExecute( + const std::shared_ptr goal_handle) { + auto goal = goal_handle->get_goal(); + auto result = std::make_shared(); + + // Publish the collision object + auto collision_object = createCollisionObject( + goal->id, goal->shape, goal->dimensions, goal->frame_id, goal->pose, goal->mesh_file, + goal->scale_mesh_x, goal->scale_mesh_y, goal->scale_mesh_z); + collision_object_publisher_->publish(collision_object); + RCLCPP_INFO(this->get_logger(), "Published ADD operation for object '%s'.", + goal->id.c_str()); + + // Publish feedback + auto feedback = std::make_shared(); + feedback->status = "Collision object published."; + goal_handle->publish_feedback(feedback); + + // Retry mechanism + const int max_retries = 5; + const int delay_ms = 50; + bool exists = false; + + for (int attempt = 1; attempt <= max_retries; ++attempt) { + rclcpp::sleep_for(std::chrono::milliseconds(delay_ms * attempt)); + + // Check if the object exists + exists = objectExists(goal->id); + if (exists) { + result->ok = true; + result->message = "Object added successfully."; + goal_handle->succeed(result); + RCLCPP_INFO(this->get_logger(), + "Object '%s' added successfully after %d attempt(s).", + goal->id.c_str(), attempt); + return; + } + + RCLCPP_WARN(this->get_logger(), + "Attempt %d: Object '%s' not found in planning scene yet.", + attempt, goal->id.c_str()); + } + + // If we reach here, the object was not found + result->ok = false; + result->message = "Failed to verify addition of object."; + goal_handle->abort(result); + RCLCPP_ERROR(this->get_logger(), + "Failed to verify addition of object '%s' after %d attempts.", + goal->id.c_str(), max_retries); +} + +rclcpp_action::GoalResponse PsManager::handleRemoveGoal( + [[maybe_unused]] const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal) { + RCLCPP_INFO(this->get_logger(), "Received request to remove object: %s", + goal->id.c_str()); + + // Check if the object is attached before trying to remove + if (attachedObjectExists(goal->id)) { + RCLCPP_WARN( + this->get_logger(), + "Object '%s' is attached to link '%s', detach it before removing it.", + goal->id.c_str(), getAttachedObjectLinkById(goal->id)->c_str()); + return rclcpp_action::GoalResponse::REJECT; + } + + // Check if the object exists before trying to remove + if ((!objectExists(goal->id)) && (!attachedObjectExists(goal->id))) { + RCLCPP_WARN(this->get_logger(), "Object '%s' does not exist.", + goal->id.c_str()); + return rclcpp_action::GoalResponse::REJECT; + } + + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse PsManager::handleRemoveCancel( + const std::shared_ptr goal_handle) { + (void)goal_handle; // Suppress unused parameter warning + RCLCPP_INFO(this->get_logger(), "Remove operation canceled."); + return rclcpp_action::CancelResponse::ACCEPT; +} + +void PsManager::handleRemoveExecute( + const std::shared_ptr goal_handle) { + auto goal = goal_handle->get_goal(); + auto result = std::make_shared(); + + // Create a CollisionObject message with REMOVE + moveit_msgs::msg::CollisionObject collision_object; + collision_object.id = goal->id; + collision_object.header.frame_id = frame_id_; + collision_object.operation = moveit_msgs::msg::CollisionObject::REMOVE; + + // Publish the remove operation + collision_object_publisher_->publish(collision_object); + RCLCPP_INFO(this->get_logger(), "Published REMOVE operation for object '%s'.", + goal->id.c_str()); + + // Publish feedback + auto feedback = std::make_shared(); + feedback->status = "Collision object remove command published."; + goal_handle->publish_feedback(feedback); + + // Retry mechanism + const int max_retries = 5; + const int initial_delay_ms = 50; + int delay_ms = initial_delay_ms; + bool exists = true; + + for (int attempt = 1; attempt <= max_retries; ++attempt) { + rclcpp::sleep_for(std::chrono::milliseconds(delay_ms)); + + // Check if the object still exists + exists = objectExists(goal->id); + if (!exists) { + result->ok = true; + result->message = "Object removed successfully."; + goal_handle->succeed(result); + RCLCPP_INFO(this->get_logger(), + "Object '%s' removed successfully after %d attempt(s).", + goal->id.c_str(), attempt); + return; + } + + RCLCPP_WARN(this->get_logger(), + "Attempt %d: Object '%s' still exists in planning scene.", + attempt, goal->id.c_str()); + + // Exponential backoff + delay_ms *= 2; + } + + // If we reach here, the object was not removed + result->ok = false; + result->message = "Failed to verify removal of object."; + goal_handle->abort(result); + RCLCPP_ERROR(this->get_logger(), + "Failed to verify removal of object '%s' after %d attempts.", + goal->id.c_str(), max_retries); +} + +rclcpp_action::GoalResponse PsManager::handleAttachDetachGoal( + [[maybe_unused]] const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal) { + // I call once the search functions to avoid dalays + auto attached_object_exists = attachedObjectExists(goal->object_id); + auto collision_object_exists = objectExists(goal->object_id); + + RCLCPP_INFO(this->get_logger(), + "Received request to %s object: %s to link: %s", + goal->attach ? "attach" : "detach", goal->object_id.c_str(), + goal->link_name.c_str()); + + // Basic validation + if (goal->object_id.empty() || (goal->attach && goal->link_name.empty())) { + RCLCPP_WARN(this->get_logger(), + "Invalid object_id or link_name in the request."); + return rclcpp_action::GoalResponse::REJECT; + } + + // Check if object exists at all, if not it wouldn't make sense to continue + // either for attaching or detaching + if ((!attached_object_exists && !collision_object_exists)) { + RCLCPP_WARN(this->get_logger(), + "Object '%s' does not exist in the planning scene.", + goal->object_id.c_str()); + return rclcpp_action::GoalResponse::REJECT; + } + + // Check if the object exists for attach operation + if (goal->attach && (attached_object_exists)) { + RCLCPP_WARN(this->get_logger(), + "Cannot attach. Object '%s' already attached to link %s.", + goal->object_id.c_str(), + getAttachedObjectLinkById(goal->object_id)->c_str()); + return rclcpp_action::GoalResponse::REJECT; + } + + // Check if the object is already attached somewhere + if (goal->attach && (!collision_object_exists || attached_object_exists)) { + RCLCPP_WARN(this->get_logger(), + "Cannot attach. Object '%s' already attached .", + goal->object_id.c_str()); + return rclcpp_action::GoalResponse::REJECT; + } + + // Check if the object is attached for detach operation + if (!goal->attach && !attached_object_exists) { + RCLCPP_WARN(this->get_logger(), + "Cannot detach. Object '%s' is not attached to link '%s'.", + goal->object_id.c_str(), goal->link_name.c_str()); + return rclcpp_action::GoalResponse::REJECT; + } + + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse PsManager::handleAttachDetachCancel( + [[maybe_unused]] const std::shared_ptr + goal_handle) { + RCLCPP_INFO(this->get_logger(), "AttachDetach operation canceled."); + return rclcpp_action::CancelResponse::ACCEPT; +} + +void PsManager::handleAttachDetachExecute( + const std::shared_ptr goal_handle) { + auto goal = goal_handle->get_goal(); + auto result = std::make_shared(); + + // Create a CollisionObject message for attaching/detaching + moveit_msgs::msg::AttachedCollisionObject attached_object; + attached_object.object.id = goal->object_id; + attached_object.link_name = goal->link_name; + attached_object.object.header.frame_id = frame_id_; + + std::optional collision_object_opt; + + if (goal->attach) { + attached_object.object.operation = moveit_msgs::msg::CollisionObject::ADD; + attached_object.detach_posture = trajectory_msgs::msg::JointTrajectory(); + + if (!goal->touch_links.empty()) { + attached_object.touch_links = goal->touch_links; + } + } else { + attached_object.object.operation = + moveit_msgs::msg::CollisionObject::REMOVE; + + // Retrieve object data from planning scene + collision_object_opt = getObjectDataById(goal->object_id); + if (!collision_object_opt) { + RCLCPP_ERROR(this->get_logger(), "Failed to retrieve object '%s' data.", + goal->object_id.c_str()); + + result->ok = false; + result->message = goal->attach ? "Failed to attach object." + : "Failed to detach object."; + goal_handle->abort(result); + RCLCPP_ERROR(this->get_logger(), + "%s operation failed for object '%s' and link '%s'.", + goal->attach ? "Attach" : "Detach", goal->object_id.c_str(), + goal->link_name.c_str()); + return; + } + } + + // Publish the attach/detach command + attached_collision_object_publisher_->publish(attached_object); + RCLCPP_INFO(this->get_logger(), + "%s operation published for object '%s' to link '%s'.", + goal->attach ? "Attach" : "Detach", goal->object_id.c_str(), + goal->link_name.c_str()); + + // Publish feedback + auto feedback = std::make_shared(); + feedback->status = + goal->attach ? "Attach command published." : "Detach command published."; + goal_handle->publish_feedback(feedback); + + // Retry mechanism to verify the operation + const int max_retries = 5; + const int delay_ms = 50; + bool operation_successful = false; + + for (int attempt = 1; attempt <= max_retries; ++attempt) { + rclcpp::sleep_for(std::chrono::milliseconds(delay_ms * attempt)); + + // Check the planning scene + auto request = + std::make_shared(); + request->components.components = + moveit_msgs::msg::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS; + + auto future_response = + get_planning_scene_client_->async_send_request(request); + + // Wait for the response with timeout + if (future_response.wait_for(std::chrono::milliseconds(500)) == + std::future_status::ready) { + auto response = future_response.get(); + if (response) { + bool found = attachedObjectExists(goal->object_id); + + if (goal->attach && found) { + RCLCPP_INFO(this->get_logger(), + "Object '%s' successfully attached to link '%s'.", + goal->object_id.c_str(), goal->link_name.c_str()); + operation_successful = true; + break; + } else if (!goal->attach && !found) { + RCLCPP_INFO(this->get_logger(), + "Object '%s' successfully detached from link '%s'.", + goal->object_id.c_str(), goal->link_name.c_str()); + + // If detaching, ensure the object is back in the planning scene + if (!objectExists(goal->object_id)) { + RCLCPP_INFO(this->get_logger(), + "Object '%s' not found in the planning scene after " + "detaching. Adding it back.", + goal->object_id.c_str()); + + const auto &collision_object = collision_object_opt.value(); + + // Publisher for collision objects + collision_object_publisher_->publish(collision_object); + RCLCPP_INFO(this->get_logger(), + "Re-added object '%s' to the planning scene.", + goal->object_id.c_str()); + + // Wait briefly to allow the planning scene to update + rclcpp::sleep_for(std::chrono::milliseconds(500)); + + // Verify the object is added + if (objectExists(goal->object_id)) { + RCLCPP_INFO( + this->get_logger(), + "Object '%s' re-added to the planning scene successfully.", + goal->object_id.c_str()); + operation_successful = true; + break; + } else { + RCLCPP_WARN(this->get_logger(), + "Failed to re-add object '%s' to the planning scene.", + goal->object_id.c_str()); + } + } else { + operation_successful = true; + break; + } + } + } else { + RCLCPP_ERROR(this->get_logger(), + "Received null response from /get_planning_scene."); + } + } else { + RCLCPP_ERROR(this->get_logger(), + "Timeout while waiting for /get_planning_scene response."); + } + + RCLCPP_WARN(this->get_logger(), "Attempt %d: Operation not confirmed yet.", + attempt); + } + + if (operation_successful) { + result->ok = true; + result->message = goal->attach ? "Object attached successfully." + : "Object detached successfully."; + goal_handle->succeed(result); + RCLCPP_INFO( + this->get_logger(), + "%s operation completed successfully for object '%s' and link '%s'.", + goal->attach ? "Attach" : "Detach", goal->object_id.c_str(), + goal->link_name.c_str()); + } else { + result->ok = false; + result->message = + goal->attach ? "Failed to attach object." : "Failed to detach object."; + goal_handle->abort(result); + RCLCPP_ERROR(this->get_logger(), + "%s operation failed for object '%s' and link '%s'.", + goal->attach ? "Attach" : "Detach", goal->object_id.c_str(), + goal->link_name.c_str()); + } +} + +rclcpp_action::GoalResponse PsManager::handleGetObjectPoseGoal( + [[maybe_unused]] const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal) { + RCLCPP_INFO(this->get_logger(), + "Received GetObjectPose goal for object: %s, reference link: %s", + goal->object_id.c_str(), goal->link_name.c_str()); + + if (goal->object_id.empty()) { + RCLCPP_WARN(this->get_logger(), "Invalid object_id: cannot be empty."); + return rclcpp_action::GoalResponse::REJECT; + } + + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse PsManager::handleGetObjectPoseCancel( + [[maybe_unused]] const std::shared_ptr + goal_handle) { + RCLCPP_INFO(this->get_logger(), "GetObjectPose operation canceled."); + return rclcpp_action::CancelResponse::ACCEPT; +} + +void PsManager::handleGetObjectPoseExecute( + const std::shared_ptr goal_handle) { + auto goal = goal_handle->get_goal(); + auto result = std::make_shared(); + + // 1. Retrieve the CollisionObject from the planning scene + auto collision_object_opt = getObjectDataById(goal->object_id); + if (!collision_object_opt) { + RCLCPP_ERROR(this->get_logger(), + "Failed to retrieve object '%s' from planning scene.", + goal->object_id.c_str()); + result->ok = false; + result->message = "Object not found in planning scene."; + goal_handle->abort(result); + return; + } + + // 2. The object's pose is stored in either "world" (frame_id_) or + // the attached link's frame if attached + geometry_msgs::msg::Pose object_pose = collision_object_opt->pose; + + bool is_attached = attachedObjectExists(goal->object_id); + std::string object_frame = frame_id_; // assume 'world' by default + if (is_attached) { + auto maybe_link = getAttachedObjectLinkById(goal->object_id); + if (!maybe_link) { + RCLCPP_ERROR(this->get_logger(), + "Failed to get attached link for object '%s'.", + goal->object_id.c_str()); + result->ok = false; + result->message = "Failed to determine attached link."; + goal_handle->abort(result); + return; + } + // If attached, the object_pose is in this link's frame + object_frame = maybe_link.value(); + } + + // 3. If needed, transform from 'object_frame' => 'goal->link_name' + // only if the user specifies a different link_name + if (!goal->link_name.empty() && goal->link_name != object_frame) { + tf2_ros::Buffer tfBuffer(this->get_clock()); + tf2_ros::TransformListener tfListener(tfBuffer, this, true); + + try { + geometry_msgs::msg::TransformStamped transformStamped = + tfBuffer.lookupTransform(goal->link_name, // target frame + object_frame, // source frame + tf2::TimePointZero, + tf2::durationFromSec(1.0)); + + // Apply that transform + geometry_msgs::msg::Pose transformed_pose; + tf2::doTransform(object_pose, transformed_pose, transformStamped); + object_pose = transformed_pose; + } catch (const tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), + "Failed to lookup transform from '%s' to '%s': %s", + object_frame.c_str(), goal->link_name.c_str(), ex.what()); + result->ok = false; + result->message = "Transform error."; + goal_handle->abort(result); + return; + } + } + + // 4. Now we apply the pre-/post- transformations to the pose in the final + // frame + + try { + // Validate arrays + if (goal->pre_transform_xyz_rpy.size() != 6) { + throw std::invalid_argument("pre_transform_xyz_rpy must have exactly 6 " + "elements (x,y,z,roll,pitch,yaw)."); + } + if (goal->post_transform_xyz_rpy.size() != 6) { + throw std::invalid_argument("post_transform_xyz_rpy must have exactly 6 " + "elements (x,y,z,roll,pitch,yaw)."); + } + + // Steps 1..8 from the original snippet + // ------------------------------------ + // Step 1: Start with identity + tf2::Transform combined_transform; + combined_transform.setIdentity(); + + // Step 2: Rotation from pre_transform_xyz_rpy (roll, pitch, yaw) + tf2::Quaternion pre_rot; + pre_rot.setRPY(goal->pre_transform_xyz_rpy[3], + goal->pre_transform_xyz_rpy[4], + goal->pre_transform_xyz_rpy[5]); + tf2::Transform pre_rot_tf; + pre_rot_tf.setRotation(pre_rot); + pre_rot_tf.setOrigin(tf2::Vector3(0, 0, 0)); + combined_transform = pre_rot_tf * combined_transform; + + // Step 3: Translation from pre_transform_xyz_rpy (x, y, z) + tf2::Vector3 pre_trans(goal->pre_transform_xyz_rpy[0], + goal->pre_transform_xyz_rpy[1], + goal->pre_transform_xyz_rpy[2]); + tf2::Transform pre_trans_tf; + pre_trans_tf.setIdentity(); + pre_trans_tf.setOrigin(pre_trans); + combined_transform = pre_trans_tf * combined_transform; + + // Step 4: Rotation from post_transform_xyz_rpy (roll, pitch, yaw) + tf2::Quaternion post_rot; + post_rot.setRPY(goal->post_transform_xyz_rpy[3], + goal->post_transform_xyz_rpy[4], + goal->post_transform_xyz_rpy[5]); + tf2::Transform post_rot_tf; + post_rot_tf.setRotation(post_rot); + post_rot_tf.setOrigin(tf2::Vector3(0, 0, 0)); + combined_transform = post_rot_tf * combined_transform; + + // Step 7: Translation from post_transform_xyz_rpy (x, y, z) + tf2::Vector3 post_trans(goal->post_transform_xyz_rpy[0], + goal->post_transform_xyz_rpy[1], + goal->post_transform_xyz_rpy[2]); + tf2::Transform post_trans_tf; + post_trans_tf.setIdentity(); + post_trans_tf.setOrigin(post_trans); + combined_transform = post_trans_tf * combined_transform; + + // Step 5: Rotation from the object's original orientation + tf2::Quaternion obj_quat( + object_pose.orientation.x, object_pose.orientation.y, + object_pose.orientation.z, object_pose.orientation.w); + tf2::Transform obj_rot_tf; + obj_rot_tf.setRotation(obj_quat); + obj_rot_tf.setOrigin(tf2::Vector3(0, 0, 0)); + combined_transform = obj_rot_tf * combined_transform; + + // Step 6: Translation from the object's original position + tf2::Vector3 obj_trans(object_pose.position.x, object_pose.position.y, + object_pose.position.z); + tf2::Transform obj_trans_tf; + obj_trans_tf.setIdentity(); + obj_trans_tf.setOrigin(obj_trans); + combined_transform = obj_trans_tf * combined_transform; + + // Step 8: Extract final pose + geometry_msgs::msg::Pose final_pose; + final_pose.position.x = combined_transform.getOrigin().x(); + final_pose.position.y = combined_transform.getOrigin().y(); + final_pose.position.z = combined_transform.getOrigin().z(); + + tf2::Quaternion final_quat = combined_transform.getRotation(); + final_quat.normalize(); + final_pose.orientation.x = final_quat.x(); + final_pose.orientation.y = final_quat.y(); + final_pose.orientation.z = final_quat.z(); + final_pose.orientation.w = final_quat.w(); + + // 5. Set and return result + result->pose = final_pose; + result->ok = true; + result->message = + "Pose updated successfully with updated transformation logic."; + goal_handle->succeed(result); + + RCLCPP_INFO( + this->get_logger(), + "Object '%s' updated successfully. Final pose: " + "position (%.3f, %.3f, %.3f), orientation (%.3f, %.3f, %.3f, %.3f)", + goal->object_id.c_str(), final_pose.position.x, final_pose.position.y, + final_pose.position.z, final_pose.orientation.x, + final_pose.orientation.y, final_pose.orientation.z, + final_pose.orientation.w); + } catch (const std::exception &e) { + RCLCPP_ERROR(this->get_logger(), "%s", e.what()); + result->ok = false; + result->message = e.what(); + goal_handle->abort(result); + } +} + +bool PsManager::objectExists(const std::string &id) { + auto request = + std::make_shared(); + request->components.components = + moveit_msgs::msg::PlanningSceneComponents::WORLD_OBJECT_NAMES; + + RCLCPP_INFO(this->get_logger(), + "Checking if object '%s' exists in the planning scene...", + id.c_str()); + + auto future_response = + get_planning_scene_client_->async_send_request(request); + + // Wait for the future to complete without additional spinning + if (future_response.wait_for(std::chrono::seconds(5)) == + std::future_status::ready) { + auto response = future_response.get(); + if (response) { + for (const auto &obj : response->scene.world.collision_objects) { + if (obj.id == id) { + RCLCPP_INFO(this->get_logger(), + "Object '%s' found in the planning scene.", id.c_str()); + return true; + } + } + } else { + RCLCPP_ERROR(this->get_logger(), + "Received null response from /get_planning_scene."); + } + } else { + RCLCPP_ERROR(this->get_logger(), + "Timeout while waiting for /get_planning_scene response."); + } + + RCLCPP_INFO(this->get_logger(), + "Object '%s' not found in the planning scene.", id.c_str()); + return false; +} + +bool PsManager::attachedObjectExists(const std::string &id, + const std::string &link_name) { + auto request = + std::make_shared(); + request->components.components = + moveit_msgs::msg::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS; + + if (link_name.size() > 0) { + RCLCPP_INFO(this->get_logger(), + "Checking if object '%s' is attached to link '%s'...", + id.c_str(), link_name.c_str()); + } else { + RCLCPP_INFO(this->get_logger(), + "Checking if object '%s' is attached to any link...", + id.c_str()); + } + + auto future_response = + get_planning_scene_client_->async_send_request(request); + + if (future_response.wait_for(std::chrono::seconds(5)) == + std::future_status::ready) { + auto response = future_response.get(); + if (response) { + for (const auto &attached_obj : + response->scene.robot_state.attached_collision_objects) { + if (attached_obj.object.id == id) { + if (link_name.empty() || attached_obj.link_name == link_name) { + RCLCPP_INFO(this->get_logger(), + "Object '%s' is attached to link '%s'.", id.c_str(), + attached_obj.link_name.c_str()); + return true; + } + } + } + } else { + RCLCPP_ERROR(this->get_logger(), + "Received null response from /get_planning_scene."); + } + } else { + RCLCPP_ERROR(this->get_logger(), + "Timeout while waiting for /get_planning_scene response."); + } + + RCLCPP_WARN(this->get_logger(), "Object '%s' is not attached to link '%s'.", + id.c_str(), link_name.empty() ? "any link" : link_name.c_str()); + return false; +} + +std::optional +PsManager::getObjectDataById(const std::string &object_id) { + // Request planning scene world data + auto request = + std::make_shared(); + request->components.components = + moveit_msgs::msg::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY | + moveit_msgs::msg::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS; + + auto future_response = + get_planning_scene_client_->async_send_request(request); + + // Wait for the service response + if (future_response.wait_for(std::chrono::seconds(5)) == + std::future_status::ready) { + auto response = future_response.get(); + if (response) { + for (const auto &collision_object : + response->scene.world.collision_objects) { + if (collision_object.id == object_id) { + // Object found, return it + RCLCPP_INFO(this->get_logger(), + "Object '%s' data retrieved from planning scene.", + object_id.c_str()); + return collision_object; + } + } + for (const auto &attached_collision_object : + response->scene.robot_state.attached_collision_objects) { + if (attached_collision_object.object.id == object_id) { + // Object found, return it + RCLCPP_INFO(this->get_logger(), + "Object '%s' data retrieved from attached objects in " + "planning scene.", + object_id.c_str()); + return attached_collision_object.object; + } + } + RCLCPP_WARN(this->get_logger(), + "Object '%s' not found in planning scene.", + object_id.c_str()); + } else { + RCLCPP_ERROR(this->get_logger(), + "Null response received from /get_planning_scene."); + } + } else { + RCLCPP_ERROR(this->get_logger(), + "Timeout while waiting for /get_planning_scene response."); + } + + // Return empty if not found + return std::nullopt; +} + +std::optional +PsManager::getAttachedObjectLinkById(const std::string &object_id) { + // Request planning scene world data + auto request = + std::make_shared(); + request->components.components = + moveit_msgs::msg::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS; + + auto future_response = + get_planning_scene_client_->async_send_request(request); + + // Wait for the service response + if (future_response.wait_for(std::chrono::seconds(5)) == + std::future_status::ready) { + auto response = future_response.get(); + if (response) { + for (const auto &attached_collision_object : + response->scene.robot_state.attached_collision_objects) { + if (attached_collision_object.object.id == object_id) { + // Object found, return it + RCLCPP_INFO(this->get_logger(), + "Object '%s' link retrieved from attached objects in " + "planning scene.", + object_id.c_str()); + return attached_collision_object.link_name; + } + } + RCLCPP_WARN(this->get_logger(), "Link '%s' not found in planning scene.", + object_id.c_str()); + } else { + RCLCPP_ERROR(this->get_logger(), + "Null response received from /get_planning_scene."); + } + } else { + RCLCPP_ERROR(this->get_logger(), + "Timeout while waiting for /get_planning_scene response."); + } + + // Return empty if not found + return std::nullopt; +} + +moveit_msgs::msg::CollisionObject PsManager::createCollisionObject( + const std::string &id, const std::string &shape, + const std::vector &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 { + moveit_msgs::msg::CollisionObject collision_object; + collision_object.header.frame_id = frame_id; + collision_object.id = id; + + if (shape == "mesh") { + // Attempt to load the mesh from the specified resource + shapes::Mesh *mesh = shapes::createMeshFromResource(mesh_file); + if (!mesh) { + RCLCPP_ERROR(this->get_logger(), "Failed to load mesh from file: %s", + mesh_file.c_str()); + return collision_object; // Exit early if mesh loading fails + } + + try { + // Apply scaling to the mesh vertices + if (scale_mesh_x != 1.0 && scale_mesh_y != 1.0 && scale_mesh_z != 1.0) { + for (unsigned int i = 0; i < mesh->vertex_count; ++i) { + mesh->vertices[3 * i + 0] *= scale_mesh_x; // Scale X + mesh->vertices[3 * i + 1] *= scale_mesh_y; // Scale Y + mesh->vertices[3 * i + 2] *= scale_mesh_z; // Scale Z + } + } + // Convert the scaled mesh into a shape_msgs::Mesh + shape_msgs::msg::Mesh mesh_msg; + shapes::ShapeMsg shape_msg; + + // Convert the shapes::Mesh into a ShapeMsg (a Boost variant) + shapes::constructMsgFromShape(mesh, shape_msg); + + // Extract the shape_msgs::msg::Mesh from the ShapeMsg + if (auto mesh_ptr = boost::get(&shape_msg)) { + // Successfully extracted the mesh + mesh_msg = *mesh_ptr; + collision_object.meshes.push_back(mesh_msg); + collision_object.mesh_poses.push_back(pose); + } else { + RCLCPP_ERROR(this->get_logger(), + "Failed to extract mesh from ShapeMsg for object '%s'.", + id.c_str()); + } + } catch (const std::exception &e) { + RCLCPP_ERROR(this->get_logger(), "Exception while processing mesh: %s", + e.what()); + } + + // Clean up dynamically allocated mesh to prevent memory leaks + delete mesh; + } else { + shape_msgs::msg::SolidPrimitive primitive; + if (shape == "box") { + primitive.type = primitive.BOX; + primitive.dimensions.resize(3); + if (dimensions.size() >= 3) { + primitive.dimensions[0] = dimensions[0]; + primitive.dimensions[1] = dimensions[1]; + primitive.dimensions[2] = dimensions[2]; + } else { + RCLCPP_WARN(this->get_logger(), + "Insufficient dimensions for box. Using default size."); + primitive.dimensions[0] = 0.1; + primitive.dimensions[1] = 0.1; + primitive.dimensions[2] = 0.1; + } + } else if (shape == "cylinder") { + primitive.type = primitive.CYLINDER; + primitive.dimensions.resize(2); + if (dimensions.size() >= 2) { + primitive.dimensions[0] = dimensions[0]; // height + primitive.dimensions[1] = dimensions[1]; // radius + } else { + RCLCPP_WARN( + this->get_logger(), + "Insufficient dimensions for cylinder. Using default size."); + primitive.dimensions[0] = 0.1; + primitive.dimensions[1] = 0.05; + } + } else if (shape == "sphere") { + primitive.type = primitive.SPHERE; + primitive.dimensions.resize(1); + if (dimensions.size() >= 1) { + primitive.dimensions[0] = dimensions[0]; // radius + } else { + RCLCPP_WARN(this->get_logger(), + "Insufficient dimensions for sphere. Using default size."); + primitive.dimensions[0] = 0.05; + } + } else { + RCLCPP_WARN(this->get_logger(), + "Unsupported shape type: %s. Defaulting to box.", + shape.c_str()); + primitive.type = primitive.BOX; + primitive.dimensions.resize(3); + primitive.dimensions[0] = 0.1; + primitive.dimensions[1] = 0.1; + primitive.dimensions[2] = 0.1; + } + + collision_object.primitives.push_back(primitive); + collision_object.primitive_poses.push_back(pose); + collision_object.operation = moveit_msgs::msg::CollisionObject::ADD; + } + + return collision_object; +} + +bool PsManager::validateShapeAndDimensions( + const std::string &shape, const std::vector &dimensions, + const std::optional &mesh_file) const { + if (shape == "box" && dimensions.size() == 3) + return true; + if (shape == "cylinder" && dimensions.size() == 2) + return true; + if (shape == "sphere" && dimensions.size() == 1) + return true; + if (shape == "mesh" && mesh_file.has_value()) + return true; + + RCLCPP_WARN(this->get_logger(), + "Invalid shape '%s' or missing required dimensions/mesh file.", + shape.c_str()); + RCLCPP_WARN(this->get_logger(), "Dimensions size '%li'.", dimensions.size()); + return false; +} + +} // namespace rbs_mill_utils + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + + auto node = std::make_shared(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/rbs_mill_assist/world/config/places.yaml b/rbs_mill_assist/world/config/places.yaml index 0714bc9..5f0de4a 100644 --- a/rbs_mill_assist/world/config/places.yaml +++ b/rbs_mill_assist/world/config/places.yaml @@ -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 diff --git a/rbs_mill_assist/world/config/ref_frames.yaml b/rbs_mill_assist/world/config/ref_frames.yaml index efeb661..1a3573e 100644 --- a/rbs_mill_assist/world/config/ref_frames.yaml +++ b/rbs_mill_assist/world/config/ref_frames.yaml @@ -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: diff --git a/rbs_mill_assist/world/inc/from_places.xacro b/rbs_mill_assist/world/inc/from_places.xacro index 3eceda5..06c4aed 100644 --- a/rbs_mill_assist/world/inc/from_places.xacro +++ b/rbs_mill_assist/world/inc/from_places.xacro @@ -4,48 +4,47 @@ - - - - ${model["pose"]["position"]["x"]} - ${model["pose"]["position"]["y"]} - ${model["pose"]["position"]["z"]} - - - - - ${orientation['r']} - ${orientation['p']} - ${orientation['y']} + + + + ${model["pose"]["position"]["x"]} + ${model["pose"]["position"]["y"]} ${model["pose"]["position"]["z"]} + + + ${orientation['r']} + ${orientation['p']} + ${orientation['y']} + - - - 0 0 0 - - - - - - ${model["pose"]["position"]["x"]} - ${model["pose"]["position"]["y"]} - ${model["pose"]["position"]["z"]} - - - - - ${orientation['r']} - ${orientation['p']} - ${orientation['y']} + + 0 0 0 + + + + + ${model["pose"]["position"]["x"]} ${model["pose"]["position"]["y"]} + ${model["pose"]["position"]["z"]} + + + ${orientation['r']} + ${orientation['p']} + ${orientation['y']} + - - - 0 0 0 - - - - + + 0 0 0 + + + + + diff --git a/rbs_mill_interfaces/CMakeLists.txt b/rbs_mill_interfaces/CMakeLists.txt index ac47416..21c255e 100644 --- a/rbs_mill_interfaces/CMakeLists.txt +++ b/rbs_mill_interfaces/CMakeLists.txt @@ -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 diff --git a/rbs_mill_interfaces/action/AddCollisionObject.action b/rbs_mill_interfaces/action/AddCollisionObject.action new file mode 100644 index 0000000..2b0b053 --- /dev/null +++ b/rbs_mill_interfaces/action/AddCollisionObject.action @@ -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 diff --git a/rbs_mill_interfaces/action/AttachDetachObject.action b/rbs_mill_interfaces/action/AttachDetachObject.action new file mode 100644 index 0000000..01662cf --- /dev/null +++ b/rbs_mill_interfaces/action/AttachDetachObject.action @@ -0,0 +1,9 @@ +string object_id +string link_name +bool attach +string[] touch_links +--- +bool ok +string message +--- +string status diff --git a/rbs_mill_interfaces/action/CheckObjectExists.action b/rbs_mill_interfaces/action/CheckObjectExists.action new file mode 100644 index 0000000..a6b8ece --- /dev/null +++ b/rbs_mill_interfaces/action/CheckObjectExists.action @@ -0,0 +1,8 @@ +string object_id +--- +bool ok +bool is_attached +string link_name +string message +--- + diff --git a/rbs_mill_interfaces/action/GetObjectPose.action b/rbs_mill_interfaces/action/GetObjectPose.action new file mode 100644 index 0000000..907b727 --- /dev/null +++ b/rbs_mill_interfaces/action/GetObjectPose.action @@ -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 diff --git a/rbs_mill_interfaces/action/RemoveCollisionObject.action b/rbs_mill_interfaces/action/RemoveCollisionObject.action new file mode 100644 index 0000000..efcd5ec --- /dev/null +++ b/rbs_mill_interfaces/action/RemoveCollisionObject.action @@ -0,0 +1,6 @@ +string id +--- +bool ok +string message +--- +string status diff --git a/rbs_mill_interfaces/srv/AttachDetachObject.srv b/rbs_mill_interfaces/srv/AttachDetachObject.srv deleted file mode 100644 index e69de29..0000000