sbpl_interface.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 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 
00036 #include <sbpl_interface/sbpl_interface.h>
00037 #include <planning_models/conversions.h>
00038 //#include <valgrind/callgrind.h>
00039 
00040 
00041 namespace sbpl_interface {
00042 
00043 bool SBPLInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
00044                           const moveit_msgs::GetMotionPlan::Request &req,
00045                           moveit_msgs::GetMotionPlan::Response &res,
00046                           const PlanningParameters& params) const
00047 {
00048   res.trajectory.joint_trajectory.points.clear();
00049   (const_cast<SBPLInterface*>(this))->last_planning_statistics_ = PlanningStatistics();
00050   planning_models::RobotState *start_state(planning_scene->getCurrentState());
00051   planning_models::robotStateMsgToRobotState(*planning_scene->getTransforms(), req.motion_plan_request.start_state, start_state);
00052 
00053   ros::WallTime wt = ros::WallTime::now();
00054   boost::shared_ptr<EnvironmentChain3D> env_chain(new EnvironmentChain3D(planning_scene));
00055   if(!env_chain->setupForMotionPlan(planning_scene,
00056                                     req,
00057                                     res,
00058                                     params)){
00059     //std::cerr << "Env chain setup failing" << std::endl;
00060     return false;
00061   }
00062   //std::cerr << "Creation with params " << params.use_bfs_ << " took " << (ros::WallTime::now()-wt).toSec() << std::endl;
00063   boost::this_thread::interruption_point();
00064 
00065   //DummyEnvironment* dummy_env = new DummyEnvironment();
00066   boost::shared_ptr<ARAPlanner> planner(new ARAPlanner(env_chain.get(), true));
00067   planner->set_initialsolution_eps(100.0);
00068   planner->set_search_mode(true);
00069   planner->force_planning_from_scratch();
00070   planner->set_start(env_chain->getPlanningData().start_hash_entry_->stateID);
00071   planner->set_goal(env_chain->getPlanningData().goal_hash_entry_->stateID);
00072   //std::cerr << "Creation took " << (ros::WallTime::now()-wt) << std::endl;
00073   std::vector<int> solution_state_ids;
00074   int solution_cost;
00075   wt = ros::WallTime::now();
00076   //CALLGRIND_START_INSTRUMENTATION;
00077   bool b_ret = planner->replan(10.0, &solution_state_ids, &solution_cost);
00078   //CALLGRIND_STOP_INSTRUMENTATION;
00079   double el = (ros::WallTime::now()-wt).toSec();
00080   std::cerr << "B ret is " << b_ret << " planning time " << el << std::endl;
00081   std::cerr << "Expansions " << env_chain->getPlanningStatistics().total_expansions_
00082             << " average time " << (env_chain->getPlanningStatistics().total_expansion_time_.toSec()/(env_chain->getPlanningStatistics().total_expansions_*1.0))
00083             << " hz " << 1.0/(env_chain->getPlanningStatistics().total_expansion_time_.toSec()/(env_chain->getPlanningStatistics().total_expansions_*1.0))
00084             << std::endl;
00085   std::cerr << "Total coll checks " << env_chain->getPlanningStatistics().coll_checks_ << " hz " << 1.0/(env_chain->getPlanningStatistics().total_coll_check_time_.toSec()/(env_chain->getPlanningStatistics().coll_checks_*1.0)) << std::endl;
00086   std::cerr << "Path length is " << solution_state_ids.size() << std::endl;
00087   if(!b_ret) {
00088     res.error_code.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
00089     return false;
00090   }
00091   if(solution_state_ids.size() == 0) {
00092     std::cerr << "Success but no path" << std::endl;
00093     res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN;
00094     return false;
00095   }
00096   if(!env_chain->populateTrajectoryFromStateIDSequence(solution_state_ids,
00097                                                        res.trajectory.joint_trajectory)) {
00098     std::cerr << "Success but path bad" << std::endl;
00099     res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN;
00100     return false;
00101   }
00102   ros::WallTime pre_short = ros::WallTime::now();
00103   //std::cerr << "Num traj points before " << res.trajectory.joint_trajectory.points.size() << std::endl;
00104   trajectory_msgs::JointTrajectory traj = res.trajectory.joint_trajectory;
00105   env_chain->attemptShortcut(traj, res.trajectory.joint_trajectory);
00106   //std::cerr << "Num traj points after " << res.trajectory.joint_trajectory.points.size() << std::endl;
00107   //std::cerr << "Time " << (ros::WallTime::now()-pre_short).toSec() << std::endl;
00108   //env_chain->getPlaneBFSMarker(mark, env_chain->getGoalPose().translation().z());
00109   res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00110   PlanningStatistics stats = env_chain->getPlanningStatistics();
00111   stats.total_planning_time_ = ros::WallDuration(el);
00112   (const_cast<SBPLInterface*>(this))->last_planning_statistics_ = stats;
00113   return true;
00114 }
00115 
00116 }


sbpl_interface
Author(s): Gil Jones
autogenerated on Sun Jan 17 2016 12:57:03