diff --git a/moveit_ros/move_group/src/move_group.cpp b/moveit_ros/move_group/src/move_group.cpp index bb9629d191..355a650251 100644 --- a/moveit_ros/move_group/src/move_group.cpp +++ b/moveit_ros/move_group/src/move_group.cpp @@ -290,8 +290,26 @@ int main(int argc, char** argv) const auto moveit_cpp = std::make_shared(nh, moveit_cpp_options); const auto planning_scene_monitor = moveit_cpp->getPlanningSceneMonitorNonConst(); - if (planning_scene_monitor->getPlanningScene()) + if (auto ps = planning_scene_monitor->getPlanningScene()) { + if (nh->has_parameter("default_planning_scene")) + { + std::string path = nh->get_parameter("default_planning_scene").as_string(); + std::fstream file_stream; + file_stream.open(path, std::fstream::in); + planning_scene_monitor::LockedPlanningSceneRW locked_ps(planning_scene_monitor); + if (!file_stream.is_open() || !locked_ps->loadGeometryFromStream(file_stream)) + { + RCLCPP_ERROR(nh->get_logger(), std::string("Failed to load the planning scene geometry from the file specified " + "by the `default_planning_scene` parameter. The " + "`default_planning_scene` parameter was set to ") + .append(path) + .append(".") + .c_str()); + + return 0; + } + } bool debug = false; for (int i = 1; i < argc; ++i) {