fix_start_state_path_constraints.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2012, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
00036 
00037 #include <moveit/planning_request_adapter/planning_request_adapter.h>
00038 #include <moveit/robot_state/conversions.h>
00039 #include <class_loader/class_loader.h>
00040 #include <moveit/trajectory_processing/trajectory_tools.h>
00041 #include <ros/ros.h>
00042 
00043 namespace default_planner_request_adapters
00044 {
00045 
00046 class FixStartStatePathConstraints : public planning_request_adapter::PlanningRequestAdapter
00047 {
00048 public:
00049 
00050   FixStartStatePathConstraints() : planning_request_adapter::PlanningRequestAdapter()
00051   {
00052   }
00053 
00054   virtual std::string getDescription() const { return "Fix Start State Path Constraints"; }
00055 
00056 
00057   virtual bool adaptAndPlan(const PlannerFn &planner,
00058                             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     // get the specified start state
00066     robot_state::RobotState start_state = planning_scene->getCurrentState();
00067     robot_state::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state);
00068 
00069     // if the start state is otherwise valid but does not meet path constraints
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       bool solved1 = planner(planning_scene, req2, res2);
00083 
00084       if (solved1)
00085       {
00086         planning_interface::MotionPlanRequest req3 = req;
00087         ROS_INFO("Planned to path constraints. Resuming original planning request.");
00088 
00089         // extract the last state of the computed motion plan and set it as the new start state
00090         robot_state::robotStateToRobotStateMsg(res2.trajectory_->getLastWayPoint(), req3.start_state);
00091         bool solved2 = planner(planning_scene, req3, res);
00092         res.planning_time_ += res2.planning_time_;
00093 
00094         if (solved2)
00095         {
00096           // we need to append the solution paths.
00097           res.trajectory_->append(*res2.trajectory_, 0.0);
00098           return true;
00099         }
00100         else
00101           return false;
00102       }
00103       else
00104       {
00105         ROS_WARN("Unable to plan to path constraints. Running usual motion plan.");
00106         bool result = planner(planning_scene, req, res);
00107         res.planning_time_ += res2.planning_time_;
00108         return result;
00109       }
00110     }
00111     else
00112     {
00113       ROS_DEBUG("Path constraints are OK. Running usual motion plan.");
00114       return planner(planning_scene, req, res);
00115     }
00116   }
00117 
00118 };
00119 
00120 }
00121 
00122 CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::FixStartStatePathConstraints,
00123                             planning_request_adapter::PlanningRequestAdapter);


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Mon Oct 6 2014 02:31:39