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
00038 #include <moveit/planning_request_adapter/planning_request_adapter.h>
00039 #include <moveit/robot_state/conversions.h>
00040 #include <class_loader/class_loader.h>
00041 #include <moveit/trajectory_processing/trajectory_tools.h>
00042 #include <ros/ros.h>
00043
00044 namespace default_planner_request_adapters
00045 {
00046 class FixStartStatePathConstraints : public planning_request_adapter::PlanningRequestAdapter
00047 {
00048 public:
00049 FixStartStatePathConstraints() : planning_request_adapter::PlanningRequestAdapter()
00050 {
00051 }
00052
00053 virtual std::string getDescription() const
00054 {
00055 return "Fix Start State Path Constraints";
00056 }
00057
00058 virtual bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene,
00059 const planning_interface::MotionPlanRequest& req,
00060 planning_interface::MotionPlanResponse& res,
00061 std::vector<std::size_t>& added_path_index) const
00062 {
00063 ROS_DEBUG("Running '%s'", getDescription().c_str());
00064
00065
00066 robot_state::RobotState start_state = planning_scene->getCurrentState();
00067 robot_state::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state);
00068
00069
00070 if (planning_scene->isStateValid(start_state, req.group_name) &&
00071 !planning_scene->isStateValid(start_state, req.path_constraints, req.group_name))
00072 {
00073 ROS_INFO("Path constraints not satisfied for start state...");
00074 planning_scene->isStateValid(start_state, req.path_constraints, req.group_name, true);
00075 ROS_INFO("Planning to path constraints...");
00076
00077 planning_interface::MotionPlanRequest req2 = req;
00078 req2.goal_constraints.resize(1);
00079 req2.goal_constraints[0] = req.path_constraints;
00080 req2.path_constraints = moveit_msgs::Constraints();
00081 planning_interface::MotionPlanResponse res2;
00082
00083
00084 std::vector<std::size_t> added_path_index_temp;
00085 added_path_index_temp.swap(added_path_index);
00086 bool solved1 = planner(planning_scene, req2, res2);
00087 added_path_index_temp.swap(added_path_index);
00088
00089 if (solved1)
00090 {
00091 planning_interface::MotionPlanRequest req3 = req;
00092 ROS_INFO("Planned to path constraints. Resuming original planning request.");
00093
00094
00095 robot_state::robotStateToRobotStateMsg(res2.trajectory_->getLastWayPoint(), req3.start_state);
00096 bool solved2 = planner(planning_scene, req3, res);
00097 res.planning_time_ += res2.planning_time_;
00098
00099 if (solved2)
00100 {
00101
00102 for (std::size_t i = 0; i < added_path_index.size(); ++i)
00103 added_path_index[i] += res2.trajectory_->getWayPointCount();
00104
00105
00106 for (std::size_t i = 0; i < res2.trajectory_->getWayPointCount(); ++i)
00107 added_path_index.push_back(i);
00108
00109
00110 res2.trajectory_->append(*res.trajectory_, 0.0);
00111 res2.trajectory_->swap(*res.trajectory_);
00112 return true;
00113 }
00114 else
00115 return false;
00116 }
00117 else
00118 {
00119 ROS_WARN("Unable to plan to path constraints. Running usual motion plan.");
00120 bool result = planner(planning_scene, req, res);
00121 res.planning_time_ += res2.planning_time_;
00122 return result;
00123 }
00124 }
00125 else
00126 {
00127 ROS_DEBUG("Path constraints are OK. Running usual motion plan.");
00128 return planner(planning_scene, req, res);
00129 }
00130 }
00131 };
00132 }
00133
00134 CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::FixStartStatePathConstraints,
00135 planning_request_adapter::PlanningRequestAdapter);