Robotics StackExchange | Archived questions

MoveIT2 segfaults when adding a Mesh Collision object.

I use tetgen to create an unbounded mesh. For now the points are manually specified:

            std::vector <geometry_msgs::msg::Point> meshPoints;
            geometry_msgs::msg::Point p1;
            p1.x = 0.652228;
            p1.y = -0.897207;
            p1.z =  1.16027;

            geometry_msgs::msg::Point p2;
            p2.x = 0.762599;
            p2.y = -0.897123;
            p2.z =  1.16035;

            geometry_msgs::msg::Point p3;
            p3.x = 0.762537;
            p3.y = -0.996545;
            p3.z =  1.16031;

            geometry_msgs::msg::Point p4;
            p4.x = 0.669485;
            p4.y = -0.996568;
            p4.z =  1.1602;

            geometry_msgs::msg::Point p5;
            p5.x = 0.669378;
            p5.y = -0.996639;
            p5.z =  1.22961;

            geometry_msgs::msg::Point p6;
            p6.x = 0.669379;
            p6.y = -0.899267;
            p6.z =   1.22973;

            geometry_msgs::msg::Point p7;
            p7.x = 0.762669;
            p7.y = -0.89918;
            p7.z =  1.22977;

            geometry_msgs::msg::Point p8;
            p8.x = 0.762651;
            p8.y = -0.937259;
            p8.z =  1.22973;
            meshPoints.push_back(p1);
            meshPoints.push_back(p2);
            meshPoints.push_back(p3);
            meshPoints.push_back(p4);
            meshPoints.push_back(p5);
            meshPoints.push_back(p6);
            meshPoints.push_back(p7);
            meshPoints.push_back(p8);

I then create a Collision object and attempt to add it to the scene:

            MeshBuilder m(meshPoints);
            shape_msgs::msg::Mesh mesh = m.getMesh();
            moveit_msgs::msg::CollisionObject collision_object;
            collision_object.id = "my_mesh_collision_object";
            collision_object.header.frame_id = move_group_->getPlanningFrame();
            collision_object.meshes.push_back(mesh);
            collision_object.mesh_poses.push_back(geometry_msgs::msg::Pose());
            collision_object.operation = moveit_msgs::msg::CollisionObject::ADD;
            moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
            planning_scene_interface.applyCollisionObject(collision_object);

However, as a result of this, MoveIt completely loses it and segfaults. The output is as follows:

[move_group-1] #9    Object "/home/max/ws_moveit2/install/moveit_ros_move_group/lib/libmoveit_move_group_default_capabilities.so.2.5.4", at 0x7fd67ed0dab3, in rclcpp::Service<moveit_msgs::srv::ApplyPlanningScene>::handle_request(std::shared_ptr<rmw_request_id_s>, std::shared_ptr<void>)
[move_group-1] #8    Object "/home/max/ws_moveit2/install/moveit_ros_move_group/lib/libmoveit_move_group_default_capabilities.so.2.5.4", at 0x7fd67ed0d64f, in rclcpp::AnyServiceCallbac
k<moveit_msgs::srv::ApplyPlanningScene>::dispatch(std::shared_ptr<rclcpp::Service<moveit_msgs::srv::ApplyPlanningScene> > const&, std::shared_ptr<rmw_request_id_s> const&, std::shared_ptr<moveit_msgs::srv::ApplyPlanningScene_Request_<std::allocator<void> > >)
[move_group-1] #7    Object "/home/max/ws_moveit2/install/moveit_ros_move_group/lib/libmoveit_move_group_default_capabilities.so.2.5.4", at 0x7fd67ed071a3, in move_group::ApplyPlanning
SceneService::applyScene(std::shared_ptr<rmw_request_id_s> const&, std::shared_ptr<moveit_msgs::srv::ApplyPlanningScene_Request_<std::allocator<void> > > const&, std::shared_ptr<moveit_msgs::srv::ApplyPlanningScene_Response_<std::allocator<void> > > const&)
[move_group-1] #6    Object "/home/max/ws_moveit2/install/moveit_ros_planning/lib/libmoveit_planning_scene_monitor.so.2.5.4", at 0x7fd6b27d7fdc, in planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneMessage(moveit_msgs::msg::PlanningScene_<std::allocator<void> > const&)
[move_group-1] #5    Object "/home/max/ws_moveit2/install/moveit_core/lib/libmoveit_planning_scene.so.2.5.4", at 0x7fd6b1b1a6b9, in planning_scene::PlanningScene::setPlanningSceneDiffMsg(moveit_msgs::msg::PlanningScene_<std::allocator<void> > const&)
[move_group-1] #4    Object "/home/max/ws_moveit2/install/moveit_core/lib/libmoveit_planning_scene.so.2.5.4", at 0x7fd6b1b19887, in planning_scene::PlanningScene::processCollisionObjectAdd(moveit_msgs::msg::CollisionObject_<std::allocator<void> > const&)
[move_group-1] #3    Object "/home/max/ws_moveit2/install/moveit_core/lib/libmoveit_planning_scene.so.2.5.4", at 0x7fd6b1b092b6, in planning_scene::PlanningScene::shapesAndPosesFromCol
lisionObjectMessage(moveit_msgs::msg::CollisionObject_<std::allocator<void> > const&, Eigen::Transform<double, 3, 1, 0>&, std::vector<std::shared_ptr<shapes::Shape const>, std::allocator<std::shared_ptr<shapes::Shape const> > >&, std::vector<Eigen::Transform<double, 3, 1, 0>, Eigen::aligned_allocator<Eigen::Transform<double, 3, 1, 0> > >&)
[move_group-1] #2    Object "/opt/ros/humble/lib/libgeometric_shapes.so.2.1.3", at 0x7fd6b18feb01, in shapes::constructShapeFromMsg(shape_msgs::msg::Mesh_<std::allocator<void> > const&)
[move_group-1] #1    Object "/opt/ros/humble/lib/libgeometric_shapes.so.2.1.3", at 0x7fd6b18fc7b3, in shapes::createMeshFromVertices(std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > > const&, std::vector<unsigned int, std::allocator<unsigned int> > const&)
[move_group-1] #0    Object "/opt/ros/humble/lib/libgeometric_shapes.so.2.1.3", at 0x7fd6b19019f1, in shapes::Mesh::computeVertexNormals()
[move_group-1] Segmentation fault (Invalid permissions for mapped object [0x7fd66430d278])

I've tried the example:

https://moveit.picknik.ai/humble/doc/tutorials/planning_around_objects/planning_around_objects.html

And this works flawlessly. I've also tried to comment out the line

                collision_object.meshes.push_back(mesh);

And nothing happens as well.

It seems like it doesnt like my mesh but instead of notifying me it segfaults. Does anyone know whats going on?

Asked by mdg on 2023-04-19 05:26:08 UTC

Comments

Answers