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 #include <sbpl_interface/sbpl_interface.h>
00037 #include <planning_models/conversions.h>
00038
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
00060 return false;
00061 }
00062
00063 boost::this_thread::interruption_point();
00064
00065
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
00073 std::vector<int> solution_state_ids;
00074 int solution_cost;
00075 wt = ros::WallTime::now();
00076
00077 bool b_ret = planner->replan(10.0, &solution_state_ids, &solution_cost);
00078
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
00104 trajectory_msgs::JointTrajectory traj = res.trajectory.joint_trajectory;
00105 env_chain->attemptShortcut(traj, res.trajectory.joint_trajectory);
00106
00107
00108
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 }