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
00037 #include <moveit/ompl_interface/ompl_interface.h>
00038 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
00039 #include <tf/transform_listener.h>
00040 #include <moveit_msgs/DisplayTrajectory.h>
00041 #include <moveit/profiler/profiler.h>
00042 #include <moveit_msgs/GetMotionPlan.h>
00043
00044 static const std::string PLANNER_NODE_NAME="ompl_planning";
00045 static const std::string PLANNER_SERVICE_NAME="plan_kinematic_path";
00046 static const std::string ROBOT_DESCRIPTION="robot_description";
00047
00048 class OMPLPlannerService
00049 {
00050 public:
00051
00052 OMPLPlannerService(planning_scene_monitor::PlanningSceneMonitor &psm, bool debug = false) :
00053 nh_("~"), psm_(psm), ompl_interface_(psm.getPlanningScene()->getRobotModel()), debug_(debug)
00054 {
00055 plan_service_ = nh_.advertiseService(PLANNER_SERVICE_NAME, &OMPLPlannerService::computePlan, this);
00056 if (debug_)
00057 {
00058 pub_plan_ = nh_.advertise<moveit_msgs::DisplayTrajectory>("display_motion_plan", 100);
00059 pub_request_ = nh_.advertise<moveit_msgs::MotionPlanRequest>("motion_plan_request", 100);
00060 }
00061 }
00062
00063 bool computePlan(moveit_msgs::GetMotionPlan::Request &req, moveit_msgs::GetMotionPlan::Response &res)
00064 {
00065 ROS_INFO("Received new planning request...");
00066 if (debug_)
00067 pub_request_.publish(req.motion_plan_request);
00068 planning_interface::MotionPlanResponse response;
00069 bool result = ompl_interface_.solve(psm_.getPlanningScene(), req.motion_plan_request, response);
00070 res.motion_plan_response.error_code = response.error_code_;
00071
00072 if (debug_)
00073 {
00074 if (result)
00075 displaySolution(res.motion_plan_response);
00076 std::stringstream ss;
00077 ROS_INFO("%s", ss.str().c_str());
00078 }
00079 return result;
00080 }
00081
00082 void displaySolution(const moveit_msgs::MotionPlanResponse &mplan_res)
00083 {
00084 moveit_msgs::DisplayTrajectory d;
00085 d.model_id = psm_.getPlanningScene()->getRobotModel()->getName();
00086 d.trajectory_start = mplan_res.trajectory_start;
00087 d.trajectory.resize(1, mplan_res.trajectory);
00088 pub_plan_.publish(d);
00089 }
00090
00091 void status()
00092 {
00093 ompl_interface_.printStatus();
00094 ROS_INFO("Responding to planning and bechmark requests");
00095 if (debug_)
00096 ROS_INFO("Publishing debug information");
00097 }
00098
00099 private:
00100
00101 ros::NodeHandle nh_;
00102 planning_scene_monitor::PlanningSceneMonitor &psm_;
00103 ompl_interface::OMPLInterface ompl_interface_;
00104 ros::ServiceServer plan_service_;
00105 ros::ServiceServer display_states_service_;
00106 ros::Publisher pub_plan_;
00107 ros::Publisher pub_request_;
00108 bool debug_;
00109 };
00110
00111 int main(int argc, char **argv)
00112 {
00113 ros::init(argc, argv, PLANNER_NODE_NAME);
00114
00115 bool debug = false;
00116 for (int i = 1 ; i < argc ; ++i)
00117 if (strncmp(argv[i], "--debug", 7) == 0)
00118 debug = true;
00119
00120 ros::AsyncSpinner spinner(1);
00121 spinner.start();
00122
00123 boost::shared_ptr<tf::TransformListener> tf(new tf::TransformListener());
00124 planning_scene_monitor::PlanningSceneMonitor psm(ROBOT_DESCRIPTION, tf);
00125 if (psm.getPlanningScene())
00126 {
00127 psm.startWorldGeometryMonitor();
00128 psm.startSceneMonitor();
00129 psm.startStateMonitor();
00130
00131 OMPLPlannerService pservice(psm, debug);
00132 pservice.status();
00133 ros::waitForShutdown();
00134 }
00135 else
00136 ROS_ERROR("Planning scene not configured");
00137
00138 return 0;
00139 }