diff --git a/rbs_mill_assist/assets/bunker/meshes/Bunker_50mm_height_for_labels_60х40.STL b/rbs_mill_assist/assets/bunker/meshes/Bunker_50mm_height_for_labels_60х40.STL new file mode 100644 index 0000000..f8e2518 Binary files /dev/null and b/rbs_mill_assist/assets/bunker/meshes/Bunker_50mm_height_for_labels_60х40.STL differ diff --git a/rbs_mill_assist/assets/bunker/meshes/Bunker_for_labels_60х40.STL b/rbs_mill_assist/assets/bunker/meshes/Bunker_for_labels_60х40.STL deleted file mode 100644 index 2ab7bf2..0000000 Binary files a/rbs_mill_assist/assets/bunker/meshes/Bunker_for_labels_60х40.STL and /dev/null differ diff --git a/rbs_mill_assist/assets/bunker/meshes/bunker.stl b/rbs_mill_assist/assets/bunker/meshes/bunker.stl deleted file mode 100644 index d4ef85c..0000000 Binary files a/rbs_mill_assist/assets/bunker/meshes/bunker.stl and /dev/null differ diff --git a/rbs_mill_assist/assets/bunker/model.sdf b/rbs_mill_assist/assets/bunker/model.sdf index 54faf4f..e832ca7 100644 --- a/rbs_mill_assist/assets/bunker/model.sdf +++ b/rbs_mill_assist/assets/bunker/model.sdf @@ -6,7 +6,7 @@ - model://bunker/meshes/Bunker_for_labels_60х40.STL + model://bunker/meshes/Bunker_50mm_height_for_labels_60х40.STL @@ -14,12 +14,16 @@ 1 1 1 1 0.5 0.5 0.5 1 0 0 0 1 + + + + - model://bunker/meshes/Bunker_for_labels_60х40.STL + model://bunker/meshes/Bunker_50mm_height_for_labels_60х40.STL diff --git a/rbs_mill_assist/assets/bunker_4_slots/meshes/Bunker_50mm_height_for_labels_60х40_4.STL b/rbs_mill_assist/assets/bunker_4_slots/meshes/Bunker_50mm_height_for_labels_60х40_4.STL new file mode 100644 index 0000000..2609b85 Binary files /dev/null and b/rbs_mill_assist/assets/bunker_4_slots/meshes/Bunker_50mm_height_for_labels_60х40_4.STL differ diff --git a/rbs_mill_assist/assets/bunker_4_slots/model.config b/rbs_mill_assist/assets/bunker_4_slots/model.config new file mode 100644 index 0000000..5db6d5c --- /dev/null +++ b/rbs_mill_assist/assets/bunker_4_slots/model.config @@ -0,0 +1,16 @@ + + + + bunker_4_slots + 0.1 + model.sdf + + + Bill Finger + ur.narmak@gmail.com + + + + Empty + + diff --git a/rbs_mill_assist/assets/bunker_4_slots/model.sdf b/rbs_mill_assist/assets/bunker_4_slots/model.sdf new file mode 100644 index 0000000..14e9398 --- /dev/null +++ b/rbs_mill_assist/assets/bunker_4_slots/model.sdf @@ -0,0 +1,53 @@ + + + + + + + + + model://bunker_4_slots/meshes/Bunker_50mm_height_for_labels_60х40_4.STL + + + + 1 1 1 1 + 1 1 1 1 + 0.5 0.5 0.5 1 + 0 0 0 1 + + + + + + + + + + model://bunker_4_slots/meshes/Bunker_50mm_height_for_labels_60х40_4.STL + + + + + + 1e6 + 1e3 + 0.1 + 0.001 + + + + 0.3 + + + + 0.61 + 0.47 + 0.0 + 0.0 + + + + + + + diff --git a/rbs_mill_assist/meshes/Link6.STL b/rbs_mill_assist/meshes/Link6.STL index ccea12c..f110c5c 100644 Binary files a/rbs_mill_assist/meshes/Link6.STL and b/rbs_mill_assist/meshes/Link6.STL differ diff --git a/rbs_mill_assist/meshes/Link7.STL b/rbs_mill_assist/meshes/Link7.STL index 9e180b2..23e4662 100644 Binary files a/rbs_mill_assist/meshes/Link7.STL and b/rbs_mill_assist/meshes/Link7.STL differ diff --git a/rbs_mill_assist/meshes/Link8.STL b/rbs_mill_assist/meshes/Link8.STL index 9db4181..89eabbd 100644 Binary files a/rbs_mill_assist/meshes/Link8.STL and b/rbs_mill_assist/meshes/Link8.STL differ diff --git a/rbs_mill_assist/src/planning_scene_publisher.cpp b/rbs_mill_assist/src/planning_scene_publisher.cpp index c837af7..cc343a1 100644 --- a/rbs_mill_assist/src/planning_scene_publisher.cpp +++ b/rbs_mill_assist/src/planning_scene_publisher.cpp @@ -1,3 +1,5 @@ +#include "geometry_msgs/msg/pose.hpp" +#include "moveit_msgs/msg/collision_object.hpp" #include "moveit_msgs/msg/planning_scene.hpp" #include "shape_msgs/msg/mesh.hpp" #include @@ -11,6 +13,7 @@ #include #include #include +#include class PlanningScenePublisher : public rclcpp::Node { public: @@ -30,7 +33,7 @@ private: void publish_scene() { if (object_published_) { - return; // Избегаем повторной публикации объекта + return; } moveit_msgs::msg::PlanningScene planning_scene; @@ -39,12 +42,10 @@ private: 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}; // Длина, ширина, высота + table.dimensions = {1.2, 0.7, 0.8}; - // Задаем позу объекта geometry_msgs::msg::Pose table_pose; table_pose.position.x = 0.0; table_pose.position.y = 0.0; @@ -56,50 +57,63 @@ private: 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"; - shape_msgs::msg::Mesh mesh; - shapes::Mesh* m = shapes::createMeshFromResource("package://rbs_mill_assist/assets/laser/meshes/laser.dae"); - shapes::ShapeMsg mesh_msg; - shapes::constructMsgFromShape(m, mesh_msg); - mesh = boost::get(mesh_msg); + 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" + }; - //0.30 0.0 0 0 0 3.14159 - geometry_msgs::msg::Pose mesh_pose; - mesh_pose.position.x = 0.30; - mesh_pose.position.y = 0.0; - mesh_pose.position.z = 0.0; + 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}, + }; - mesh_pose.orientation.x = 0.0; - mesh_pose.orientation.y = 0.0; - mesh_pose.orientation.z = 1.0; - mesh_pose.orientation.w = 0.0; + for (size_t i=0; i < meshes.size(); i++) { + auto mesh = getMeshCollisionObject(meshes[i]); + auto mesh_pose = arrayToPose(mesh_poses[i]); + mesh_object.meshes.push_back(mesh); + mesh_object.mesh_poses.push_back(mesh_pose); + } - moveit_msgs::msg::ObjectColor color; - color.id = mesh_object.id; - color.color.r = 0.0; - color.color.g = 1.0; - color.color.b = 0.0; - color.color.a = 1.0; - - - mesh_object.meshes.push_back(mesh); - mesh_object.mesh_poses.push_back(mesh_pose); mesh_object.operation = moveit_msgs::msg::CollisionObject::ADD; - // planning_scene.object_colors.push_back(color); - // planning_scene.object_colors.clear(); - 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"); - object_published_ = true; // Устанавливаем флаг, чтобы больше не публиковать объект + object_published_ = true; + } + + shape_msgs::msg::Mesh getMeshCollisionObject(const std::string &mesh_filepath) { + shape_msgs::msg::Mesh mesh; + shapes::Mesh* m = shapes::createMeshFromResource(mesh_filepath); + shapes::ShapeMsg mesh_msg; + shapes::constructMsgFromShape(m, mesh_msg); + mesh = boost::get(mesh_msg); + return mesh; + } + + geometry_msgs::msg::Pose arrayToPose(const std::vector arr) { + if (arr.size() != 7) { + throw std::invalid_argument("Size of array must be equal 7"); + } + geometry_msgs::msg::Pose pose; + pose.position.x = arr[0]; + pose.position.y = arr[1]; + pose.position.z = arr[2]; + + pose.orientation.x = arr[3]; + pose.orientation.y = arr[4]; + pose.orientation.z = arr[5]; + pose.orientation.w = arr[6]; + return pose; } }; diff --git a/rbs_mill_assist/urdf/rbs_arm.xacro b/rbs_mill_assist/urdf/rbs_arm.xacro index 205cd7b..baa0af8 100644 --- a/rbs_mill_assist/urdf/rbs_arm.xacro +++ b/rbs_mill_assist/urdf/rbs_arm.xacro @@ -7,7 +7,7 @@ - + diff --git a/rbs_mill_assist/urdf/rbs_arm_macro.xacro b/rbs_mill_assist/urdf/rbs_arm_macro.xacro index 8ef4316..1e650e8 100644 --- a/rbs_mill_assist/urdf/rbs_arm_macro.xacro +++ b/rbs_mill_assist/urdf/rbs_arm_macro.xacro @@ -342,51 +342,51 @@ velocity="10" /> - - - - - - - - - - - - - - - - - - - - - + name="Link6"> + + + + + + + + + + + + + + + + + + + + + + rpy="0 0 3.14" /> + value="0.0119295697834755" /> + ixx="4.27745590342647E-07" + ixy="2.68928440312099E-33" + ixz="1.44687693702654E-21" + iyy="4.2774559034265E-07" + iyz="3.0329437472821E-21" + izz="3.81314853129931E-07" /> @@ -453,64 +453,64 @@ xyz="0 0 0" /> - + name="Link8"> + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - + + + + @@ -611,7 +611,7 @@ - + diff --git a/rbs_mill_assist/world/default.sdf b/rbs_mill_assist/world/default.sdf index ab75071..4914e0c 100644 --- a/rbs_mill_assist/world/default.sdf +++ b/rbs_mill_assist/world/default.sdf @@ -41,7 +41,7 @@ - true + false 0 0 10 0 0 0 0.8 0.8 0.8 1 0.2 0.2 0.2 1 @@ -56,45 +56,51 @@ shildik_0 - -0.5 -0.2 0.02 0 0 1.57 + -0.403959 0.250 0.02 0 0 1.57 model://shildik shildik_1 - -0.5 -0.2 0.04 0 0 1.57 + -0.447918 0.250 0.02 0 0 1.57 model://shildik shildik_2 - -0.5 -0.2 0.05 0 0 1.57 + -0.491997 0.250 0.02 0 0 1.57 model://shildik shildik_3 - -0.5 -0.2 0.06 0 0 1.57 + -0.536033 0.250 0.02 0 0 1.57 model://shildik shildik_4 - -0.5 -0.2 0.07 0 0 1.57 + -0.491997 0.250 0.03 0 0 1.57 model://shildik true - 0.30 0.0 0 0 0 3.14159 + 0.350 -0.170 0 0 0 3.14159 model://laser true - -0.505 -0.20 0.06 0.0 0.0 1.57 + -0.050 0.250 0.0 0.0 0.0 1.57 model://bunker true - 0.20 0 0.01 0 0 1.57 + -0.470 0.250 0 0 0 1.57 + model://bunker_4_slots + + + + true + 0.350 -0.170 0.01 0 0 1.57 model://conductor