fix_start_state_path_constraints.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, Ioan A. Sucan
00005  *  Copyright (c) 2012, Willow Garage, Inc.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of Willow Garage nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 /* Author: Ioan Sucan */
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     // 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       // we call the planner for this additional request, but we do not want to include potential
00083       // index information from that call
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         // extract the last state of the computed motion plan and set it as the new start state
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           // since we add a prefix, we need to correct any existing index positions
00102           for (std::size_t i = 0; i < added_path_index.size(); ++i)
00103             added_path_index[i] += res2.trajectory_->getWayPointCount();
00104 
00105           // we mark the fact we insert a prefix path (we specify the index position we just added)
00106           for (std::size_t i = 0; i < res2.trajectory_->getWayPointCount(); ++i)
00107             added_path_index.push_back(i);
00108 
00109           // we need to append the solution paths.
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);


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Mon Jul 24 2017 02:21:19