diff --git a/rbs_mill_assist/src/CMakeLists.txt b/rbs_mill_assist/src/CMakeLists.txt index f436668..48e8dcd 100644 --- a/rbs_mill_assist/src/CMakeLists.txt +++ b/rbs_mill_assist/src/CMakeLists.txt @@ -62,9 +62,11 @@ 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 @@ -93,3 +95,13 @@ install( LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME}) +# publish places frames + +add_executable(places_frame_publisher places_frame_publisher.cpp) +ament_target_dependencies(places_frame_publisher tf2_ros) +target_link_libraries(places_frame_publisher yaml-cpp) + +install( + TARGETS places_frame_publisher + DESTINATION lib/${PROJECT_NAME} +) diff --git a/rbs_mill_assist/src/places_frame_publisher.cpp b/rbs_mill_assist/src/places_frame_publisher.cpp new file mode 100644 index 0000000..7c16099 --- /dev/null +++ b/rbs_mill_assist/src/places_frame_publisher.cpp @@ -0,0 +1,86 @@ +#include +#include + +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "rbs_mill_assist/places_config.hpp" +#include "rbs_mill_assist/ref_frames_config.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2/LinearMath/Quaternion.h" +#include "tf2_ros/static_transform_broadcaster.h" + +class PlacesFramePublisher : public rclcpp::Node { +public: + explicit PlacesFramePublisher() : Node("static_turtle_tf2_broadcaster") { + tf_static_broadcaster_ = + std::make_shared(this); + this->declare_parameter("places_config_filepath", ""); + this->declare_parameter("ref_frames_config_filepath", ""); + auto places_config_filepath = + 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 ref_frames = + std::make_shared(ref_frames_config_filepath); + + this->publish_places_frames(places_config); + this->publish_ref_frames(ref_frames); + } + +private: + void publish_ref_frames(std::shared_ptr &ref_frames_config) { + for (size_t i = 0; i < ref_frames_config->getRefFrameNames().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]; + + geometry_msgs::msg::Pose tmp_pose; + tmp_pose = ref_frames_config->getRefFramePoses()[i]; + + t.transform.translation.x = tmp_pose.position.x; + t.transform.translation.y = tmp_pose.position.y; + t.transform.translation.z = tmp_pose.position.z; + t.transform.rotation.x = tmp_pose.orientation.x; + t.transform.rotation.y = tmp_pose.orientation.y; + t.transform.rotation.z = tmp_pose.orientation.z; + t.transform.rotation.w = tmp_pose.orientation.w; + + tf_static_broadcaster_->sendTransform(t); + } + } + + void publish_places_frames(std::shared_ptr &places_config) { + for (size_t i = 0; i < places_config->getModelNames().size(); i++) { + geometry_msgs::msg::TransformStamped t; + + t.header.stamp = this->get_clock()->now(); + t.header.frame_id = "world"; + t.child_frame_id = places_config->getModelNames()[i]; + + geometry_msgs::msg::Pose tmp_pose; + tmp_pose = places_config->getMeshPoses()[i]; + + t.transform.translation.x = tmp_pose.position.x; + t.transform.translation.y = tmp_pose.position.y; + t.transform.translation.z = tmp_pose.position.z; + t.transform.rotation.x = tmp_pose.orientation.x; + t.transform.rotation.y = tmp_pose.orientation.y; + t.transform.rotation.z = tmp_pose.orientation.z; + t.transform.rotation.w = tmp_pose.orientation.w; + + tf_static_broadcaster_->sendTransform(t); + } + } + + std::shared_ptr tf_static_broadcaster_; +}; + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/rbs_mill_assist/src/planning_scene_publisher.cpp b/rbs_mill_assist/src/planning_scene_publisher.cpp index cc343a1..492b370 100644 --- a/rbs_mill_assist/src/planning_scene_publisher.cpp +++ b/rbs_mill_assist/src/planning_scene_publisher.cpp @@ -1,47 +1,63 @@ #include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/quaternion.hpp" #include "moveit_msgs/msg/collision_object.hpp" #include "moveit_msgs/msg/planning_scene.hpp" #include "shape_msgs/msg/mesh.hpp" #include #include #include +#include #include #include +#include "rbs_mill_assist/places_config.hpp" +#include #include #include -#include #include -#include -#include +#include class PlanningScenePublisher : public rclcpp::Node { public: - PlanningScenePublisher() : Node("planning_scene_publisher"), object_published_(false) { - planning_scene_publisher_ = + PlanningScenePublisher() + : Node("planning_scene_publisher"), object_published_(false) { + this->declare_parameter("places_config", ""); + _planning_scene_publisher = this->create_publisher( "planning_scene", 10); - timer_ = this->create_wall_timer( + _timer = this->create_wall_timer( std::chrono::seconds(1), std::bind(&PlanningScenePublisher::publish_scene, this)); + auto places_config_filepath = + this->get_parameter("places_config").as_string(); + + try { + _places_config = std::make_shared(places_config_filepath); + } catch (const std::exception &e) { + RCLCPP_ERROR(this->get_logger(), "Failed to load config: %s", e.what()); + } + // _places_config = std::make_shared(this->shared_from_this(), + // places_config_filepath); } private: - rclcpp::Publisher::SharedPtr planning_scene_publisher_; - rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr + _planning_scene_publisher; + rclcpp::TimerBase::SharedPtr _timer; bool object_published_; + std::shared_ptr _places_config; void publish_scene() { if (object_published_) { return; } - + moveit_msgs::msg::PlanningScene planning_scene; moveit_msgs::msg::CollisionObject collision_object; - + collision_object.header.frame_id = "world"; collision_object.id = "table"; - + shape_msgs::msg::SolidPrimitive table; table.type = shape_msgs::msg::SolidPrimitive::BOX; table.dimensions = {1.2, 0.7, 0.8}; @@ -50,50 +66,40 @@ private: table_pose.position.x = 0.0; table_pose.position.y = 0.0; table_pose.position.z = -0.4; - + collision_object.primitives.push_back(table); collision_object.primitive_poses.push_back(table_pose); collision_object.operation = moveit_msgs::msg::CollisionObject::ADD; planning_scene.world.collision_objects.push_back(collision_object); - + moveit_msgs::msg::CollisionObject mesh_object; mesh_object.header.frame_id = "world"; mesh_object.id = "mesh_object"; - std::vector meshes = { - "package://rbs_mill_assist/assets/laser/meshes/laser.dae", - "package://rbs_mill_assist/assets/bunker/meshes/Bunker_50mm_height_for_labels_60х40.STL", - "package://rbs_mill_assist/assets/bunker_4_slots/meshes/Bunker_50mm_height_for_labels_60х40_4.STL" - }; - - std::vector> mesh_poses = { - {0.350, -0.170, 0.0, 0.0, 0.0, 1.0, 0.0}, - {-0.050, 0.250, 0.0, 0.0, 0.0, 0.707, 0.707}, - {-0.470, 0.250, 0.0, 0.0, 0.0, 0.707, 0.707}, - }; - - for (size_t i=0; i < meshes.size(); i++) { - auto mesh = getMeshCollisionObject(meshes[i]); - auto mesh_pose = arrayToPose(mesh_poses[i]); + for (size_t i = 0; i < _places_config->getMeshFilepaths().size(); i++) { + auto mesh = getMeshCollisionObject(_places_config->getMeshFilepaths()[i]); mesh_object.meshes.push_back(mesh); - mesh_object.mesh_poses.push_back(mesh_pose); } + mesh_object.mesh_poses = _places_config->getMeshPoses(); + mesh_object.operation = moveit_msgs::msg::CollisionObject::ADD; planning_scene.world.collision_objects.push_back(mesh_object); planning_scene.is_diff = true; - - planning_scene_publisher_->publish(planning_scene); - RCLCPP_INFO(this->get_logger(), "Published collision objects: table and mesh"); - + + _planning_scene_publisher->publish(planning_scene); + RCLCPP_INFO(this->get_logger(), + "Published collision objects: table and mesh"); + object_published_ = true; } - shape_msgs::msg::Mesh getMeshCollisionObject(const std::string &mesh_filepath) { + shape_msgs::msg::Mesh + getMeshCollisionObject(const std::string &mesh_filepath) { shape_msgs::msg::Mesh mesh; - shapes::Mesh* m = shapes::createMeshFromResource(mesh_filepath); + shapes::Mesh *m = shapes::createMeshFromResource(mesh_filepath); shapes::ShapeMsg mesh_msg; shapes::constructMsgFromShape(m, mesh_msg); mesh = boost::get(mesh_msg);