chomp_planning_context.cpp
Go to the documentation of this file.
00001 /*
00002  * chomp_planning_context.cpp
00003  *
00004  *  Created on: 27-Jul-2016
00005  *      Author: ace
00006  */
00007 
00008 #include <chomp_interface/chomp_planning_context.h>
00009 
00010 namespace chomp_interface
00011 {
00012 CHOMPPlanningContext::CHOMPPlanningContext(const std::string &name, const std::string &group,
00013                                            const robot_model::RobotModelConstPtr &model)
00014   : planning_interface::PlanningContext(name, group), robot_model_(model)
00015 {
00016   chomp_interface_ = CHOMPInterfacePtr(new CHOMPInterface());
00017 
00018   boost::shared_ptr<collision_detection::CollisionDetectorAllocator> hybrid_cd(
00019       collision_detection::CollisionDetectorAllocatorHybrid::create());
00020 
00021   if (!this->getPlanningScene())
00022   {
00023     ROS_INFO_STREAM("Configuring New Planning Scene.");
00024     planning_scene::PlanningScenePtr planning_scene_ptr(new planning_scene::PlanningScene(model));
00025     planning_scene_ptr->setActiveCollisionDetector(hybrid_cd, true);
00026     setPlanningScene(planning_scene_ptr);
00027   }
00028 }
00029 
00030 CHOMPPlanningContext::~CHOMPPlanningContext()
00031 {
00032 }
00033 
00034 bool CHOMPPlanningContext::solve(planning_interface::MotionPlanDetailedResponse &res)
00035 {
00036   moveit_msgs::MotionPlanDetailedResponse res2;
00037   if (chomp_interface_->solve(planning_scene_, request_, chomp_interface_->getParams(), res2))
00038   {
00039     res.trajectory_.resize(1);
00040     res.trajectory_[0] =
00041         robot_trajectory::RobotTrajectoryPtr(new robot_trajectory::RobotTrajectory(robot_model_, getGroupName()));
00042 
00043     moveit::core::RobotState start_state(robot_model_);
00044     robot_state::robotStateMsgToRobotState(res2.trajectory_start, start_state);
00045     res.trajectory_[0]->setRobotTrajectoryMsg(start_state, res2.trajectory[0]);
00046 
00047     trajectory_processing::IterativeParabolicTimeParameterization itp;
00048     itp.computeTimeStamps(*res.trajectory_[0], request_.max_velocity_scaling_factor,
00049                           request_.max_acceleration_scaling_factor);
00050 
00051     res.description_.push_back("plan");
00052     res.processing_time_ = res2.processing_time;
00053     res.error_code_ = res2.error_code;
00054     return true;
00055   }
00056   else
00057   {
00058     res.error_code_ = res2.error_code;
00059     return false;
00060   }
00061 }
00062 
00063 bool CHOMPPlanningContext::solve(planning_interface::MotionPlanResponse &res)
00064 {
00065   planning_interface::MotionPlanDetailedResponse res_detailed;
00066   bool result = solve(res_detailed);
00067 
00068   res.error_code_ = res_detailed.error_code_;
00069   res.trajectory_ = res_detailed.trajectory_[0];
00070   res.planning_time_ = res_detailed.processing_time_[0];
00071 
00072   return result;
00073 }
00074 
00075 bool CHOMPPlanningContext::terminate()
00076 {
00077   // TODO - make interruptible
00078   return true;
00079 }
00080 
00081 void CHOMPPlanningContext::clear()
00082 {
00083 }
00084 
00085 } /* namespace chomp_interface */


chomp_interface
Author(s): Gil Jones
autogenerated on Thu Oct 27 2016 05:22:26