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