62 return "Fix Workspace Bounds";
68 std::vector<std::size_t>& added_path_index)
const 71 const moveit_msgs::WorkspaceParameters& wparams = req.workspace_parameters;
72 if (wparams.min_corner.x == wparams.max_corner.x && wparams.min_corner.x == 0.0 &&
73 wparams.min_corner.y == wparams.max_corner.y && wparams.min_corner.y == 0.0 &&
74 wparams.min_corner.z == wparams.max_corner.z && wparams.min_corner.z == 0.0)
76 ROS_DEBUG(
"It looks like the planning volume was not specified. Using default values.");
78 moveit_msgs::WorkspaceParameters& default_wp = req2.workspace_parameters;
79 default_wp.min_corner.x = default_wp.min_corner.y = default_wp.min_corner.z = -
workspace_extent_;
80 default_wp.max_corner.x = default_wp.max_corner.y = default_wp.max_corner.z =
workspace_extent_;
81 return planner(planning_scene, req2, res);
84 return planner(planning_scene, req, res);
CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::FixWorkspaceBounds, planning_request_adapter::PlanningRequestAdapter)
virtual bool adaptAndPlan(const PlannerFn &planner, const planning_scene::PlanningSceneConstPtr &planning_scene, const planning_interface::MotionPlanRequest &req, planning_interface::MotionPlanResponse &res, std::vector< std::size_t > &added_path_index) const
virtual std::string getDescription() const
moveit_msgs::MotionPlanRequest MotionPlanRequest
boost::function< bool(const planning_scene::PlanningSceneConstPtr &planning_scene, const planning_interface::MotionPlanRequest &req, planning_interface::MotionPlanResponse &res)> PlannerFn
static const std::string WBOUNDS_PARAM_NAME
#define ROS_INFO_STREAM(args)
bool getParam(const std::string &key, std::string &s) const