Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <moveit/planning_request_adapter/planning_request_adapter.h>
00038 #include <class_loader/class_loader.h>
00039 #include <ros/ros.h>
00040
00041 namespace default_planner_request_adapters
00042 {
00043
00044 class FixWorkspaceBounds : public planning_request_adapter::PlanningRequestAdapter
00045 {
00046 public:
00047
00048 static const std::string WBOUNDS_PARAM_NAME;
00049
00050 FixWorkspaceBounds() : planning_request_adapter::PlanningRequestAdapter(), nh_("~")
00051 {
00052 if (!nh_.getParam(WBOUNDS_PARAM_NAME, workspace_extent_))
00053 {
00054 workspace_extent_ = 10.0;
00055 ROS_INFO_STREAM("Param '" << WBOUNDS_PARAM_NAME << "' was not set. Using default value: " << workspace_extent_);
00056 }
00057 else
00058 ROS_INFO_STREAM("Param '" << WBOUNDS_PARAM_NAME << "' was set to " << workspace_extent_);
00059 workspace_extent_ /= 2.0;
00060 }
00061
00062 virtual std::string getDescription() const { return "Fix Workspace Bounds"; }
00063
00064 virtual bool adaptAndPlan(const PlannerFn &planner,
00065 const planning_scene::PlanningSceneConstPtr& planning_scene,
00066 const planning_interface::MotionPlanRequest &req,
00067 planning_interface::MotionPlanResponse &res,
00068 std::vector<std::size_t> &added_path_index) const
00069 {
00070 ROS_DEBUG("Running '%s'", getDescription().c_str());
00071 const moveit_msgs::WorkspaceParameters &wparams = req.workspace_parameters;
00072 if (wparams.min_corner.x == wparams.max_corner.x && wparams.min_corner.x == 0.0 &&
00073 wparams.min_corner.y == wparams.max_corner.y && wparams.min_corner.y == 0.0 &&
00074 wparams.min_corner.z == wparams.max_corner.z && wparams.min_corner.z == 0.0)
00075 {
00076 ROS_DEBUG("It looks like the planning volume was not specified. Using default values.");
00077 planning_interface::MotionPlanRequest req2 = req;
00078 moveit_msgs::WorkspaceParameters &default_wp = req2.workspace_parameters;
00079 default_wp.min_corner.x = default_wp.min_corner.y = default_wp.min_corner.z = -workspace_extent_;
00080 default_wp.max_corner.x = default_wp.max_corner.y = default_wp.max_corner.z = workspace_extent_;
00081 return planner(planning_scene, req2, res);
00082 }
00083 else
00084 return planner(planning_scene, req, res);
00085 }
00086
00087 private:
00088 ros::NodeHandle nh_;
00089 double workspace_extent_;
00090 };
00091
00092 const std::string FixWorkspaceBounds::WBOUNDS_PARAM_NAME = "default_workspace_bounds";
00093
00094 }
00095
00096 CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::FixWorkspaceBounds,
00097 planning_request_adapter::PlanningRequestAdapter);