Uncaught exception in PlanningScene::shapesAndPosesFromCollisionObjectMessage when creating a shape with invalid dimensions

[schornakj]

Description

In these lines of code, geometric_shapes can throw an exception if the specified shape is invalid, for example if one of the dimensions is negative. We need to add a try/catch block here so we can return false if that happens instead of just crashing.

Your environment

• ROS Distro: Humble
• OS Version: Ubuntu 22.04
• Source build, 2.6.0
• CycloneDDS

Steps to reproduce

Add a shape with a negative dimension to the planning scene

Expected behaviour

Catch and return false so higher-level code can handle the failure to add the new shapes

Actual behaviour

Segfault and crash!

Backtrace or Console output

[move_group-1] terminate called after throwing an instance of 'std::runtime_error'                                                                                                        
[move_group-1]   what():  Box dimensions must be non-negative.                                                                                                                            
[move_group-1] Stack trace (most recent call last) in thread 292:                                                                                                                         
[move_group-1] #24   Object "", at 0xffffffffffffffff, in                                                                                                                                 
[move_group-1] #23   Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f1329c7f9ff, in                                                                                                  
[move_group-1] #22   Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f1329bedb42, in                                                                                                  
[move_group-1] #21   Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f1329e7d2b2, in                                                                                        
[move_group-1] #20   Object "/opt/ros/humble/lib/librclcpp.so", at 0x7f132a11d0b9, in rclcpp::executors::MultiThreadedExecutor::run(unsigned long)                                        
[move_group-1] #19   Object "/opt/ros/humble/lib/librclcpp.so", at 0x7f132a1150b5, in rclcpp::Executor::execute_any_executable(rclcpp::AnyExecutable&)                                    
[move_group-1] #18   Object "/opt/ros/humble/lib/librclcpp.so", at 0x7f132a114d49, in rclcpp::Executor::execute_service(std::shared_ptr<rclcpp::ServiceBase>)
[move_group-1] #17   Object "/opt/ros/humble/lib/librclcpp.so", at 0x7f132a117375, in                                                                                                     
[move_group-1] #16   Object "/opt/underlay_ws/install/lib/libmoveit_move_group_default_capabilities.so.2.6.0", at 0x7f12f66d8d83, in rclcpp::Service<moveit_msgs::srv::ApplyPlanningScene>
[move_group-1] #15   Object "/opt/underlay_ws/install/lib/libmoveit_move_group_default_capabilities.so.2.6.0", at 0x7f12f66d891f, in rclcpp::AnyServiceCallback<moveit_msgs::srv::ApplyPlared_ptr<moveit_msgs::srv::ApplyPlanningScene_Request_<std::allocator<void> > >)                                                                                                                                    
[move_group-1] #14   Object "/opt/underlay_ws/install/lib/libmoveit_move_group_default_capabilities.so.2.6.0", at 0x7f12f66d20d1, in std::_Function_handler<void (std::shared_ptr<rmw_requResponse_<std::allocator<void> > >), move_group::ApplyPlanningSceneService::initialize()::{lambda(std::shared_ptr<rmw_request_id_s>, std::shared_ptr<moveit_msgs::srv::ApplyPlanningScene_Request_<std::allocator<v<rmw_request_id_s>&&, std::shared_ptr<moveit_msgs::srv::ApplyPlanningScene_Request_<std::allocator<void> > >&&, std::shared_ptr<moveit_msgs::srv::ApplyPlanningScene_Response_<std::allocator<void> > >&&)         
[move_group-1] #13   Object "/opt/underlay_ws/install/lib/libmoveit_move_group_default_capabilities.so.2.6.0", at 0x7f12f66d1fe3, in move_group::ApplyPlanningSceneService::applyScene(std::ApplyPlanningScene_Response_<std::allocator<void> > >)                                                                                                                             
[move_group-1] #12   Object "/opt/underlay_ws/install/lib/libmoveit_planning_scene_monitor.so.2.6.0", at 0x7f132a45d90c, in planning_scene_monitor::PlanningSceneMonitor::newPlanningScene
[move_group-1] #11   Object "/opt/underlay_ws/install/lib/libmoveit_planning_scene.so.2.6.0", at 0x7f132979e699, in planning_scene::PlanningScene::setPlanningSceneDiffMsg(moveit_msgs::ms
[move_group-1] #10   Object "/opt/underlay_ws/install/lib/libmoveit_planning_scene.so.2.6.0", at 0x7f132979d867, in planning_scene::PlanningScene::processCollisionObjectAdd(moveit_msgs::
[move_group-1] #9    Object "/opt/underlay_ws/install/lib/libmoveit_planning_scene.so.2.6.0", at 0x7f132978d1b0, in planning_scene::PlanningScene::shapesAndPosesFromCollisionObjectMessage, 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] #8    Object "/opt/ros/humble/lib/libgeometric_shapes.so.2.1.3", at 0x7f132957f7e7, in shapes::constructShapeFromMsg(shape_msgs::msg::SolidPrimitive_<std::allocator<void> 
[move_group-1] #7    Object "/opt/ros/humble/lib/libgeometric_shapes.so.2.1.3", at 0x7f132956e8ef, in                                                                                     
[move_group-1] #6    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f1329e4f517, in __cxa_throw                                                                            
[move_group-1] #5    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f1329e4f2b6, in std::terminate()                                                                       
[move_group-1] #4    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f1329e4f24b, in                                                                                        
[move_group-1] #3    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f1329e43bbd, in                                                                                        
[move_group-1] #2    Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f1329b817f2, in abort                                                                                            
[move_group-1] #1    Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f1329b9b475, in raise                                                                                            
[move_group-1] #0    Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f1329befa7c, in pthread_kill                                                                                     
[move_group-1] Aborted (Signal sent by tkill() 78 0)     

[berkealgul]

Hello, Can I work on this issue, If no one is assinged to It.

[schornakj]

@berkealgul go for it, I don't think anyone else is working on fixing this yet.

[github-actions[bot]]

This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.

[schornakj]

@berkealgul did you have a chance to get started on this?

[github-actions[bot]]

This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.