chomp_plugin.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, 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 <planning_interface/planning_interface.h>
00037 #include <planning_scene/planning_scene.h>
00038 #include <planning_models/robot_model.h>
00039 #include <moveit_msgs/GetMotionPlan.h>
00040 #include <chomp_interface_ros/chomp_interface_ros.h>
00041 
00042 #include <boost/shared_ptr.hpp>
00043 
00044 #include <pluginlib/class_list_macros.h>
00045 
00046 namespace chomp_interface_ros
00047 {
00048 
00049 class CHOMPPlanner : public planning_interface::Planner
00050 {
00051 public:
00052   void init(const planning_models::RobotModelConstPtr& model)
00053   {
00054     chomp_interface_.reset(new CHOMPInterfaceROS(model));
00055   }
00056 
00057   bool canServiceRequest(const moveit_msgs::GetMotionPlan::Request &req,
00058                          planning_interface::PlannerCapability &capabilities) const
00059   {
00060     // TODO: this is a dummy implementation
00061     //      capabilities.dummy = false;
00062     return true;
00063   }
00064 
00065   bool solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
00066              const moveit_msgs::GetMotionPlan::Request &req,
00067              moveit_msgs::GetMotionPlan::Response &res) const
00068   {
00069     return chomp_interface_->solve(planning_scene, req,
00070                                    chomp_interface_->getParams(),res);
00071   }
00072 
00073   bool solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
00074              const moveit_msgs::GetMotionPlan::Request &req,
00075              moveit_msgs::MotionPlanDetailedResponse &res) const
00076   {
00077     moveit_msgs::GetMotionPlan::Response res2;
00078     if (chomp_interface_->solve(planning_scene, req,
00079                                 chomp_interface_->getParams(),res2))
00080     {
00081       res.trajectory_start = res2.trajectory_start;
00082       res.trajectory.push_back(res2.trajectory);
00083       res.description.push_back("plan");
00084       res.processing_time.push_back(res2.planning_time);
00085       return true;
00086     }
00087     else
00088       return false;
00089   }
00090 
00091   std::string getDescription() const { return "CHOMP"; }
00092 
00093   void getPlanningAlgorithms(std::vector<std::string> &algs) const
00094   {
00095     algs.resize(1);
00096     algs[0] = "CHOMP";
00097   }
00098 
00099   void terminate() const
00100   {
00101     //TODO - make interruptible
00102   }
00103 
00104 private:
00105   boost::shared_ptr<CHOMPInterfaceROS> chomp_interface_;
00106 };
00107 
00108 } // ompl_interface_ros
00109 
00110 PLUGINLIB_EXPORT_CLASS( chomp_interface_ros::CHOMPPlanner, planning_interface::Planner);


chomp_interface_ros
Author(s): Gil Jones
autogenerated on Wed Sep 16 2015 04:42:52