OMPL Plannification
Path planning with OMPL
ROS_OMPL_Planner.cpp
1 
8 #include "ROS_OMPL_Planner.hpp"
9 
10 ROS_OMPL_Planner::ROS_OMPL_Planner(int dimension) : planningDimension(dimension), planner(dimension), _obstacle_flag(true)
11 {
12  //=======================================================================
13  // Subscribers.
14  //=======================================================================
15  //Navigation data
16  odom_sub_ = _node_hand.subscribe("/odom", 1, &ROS_OMPL_Planner::odomCallback, this);
17 
18  //=======================================================================
19  // Publishers (to visualize the resulting path).
20  //=======================================================================
21  visual_traj_pub_ = _node_hand.advertise<visualization_msgs::Marker> ("/controller_turtlebot/solution_path", 1, true);
22  path_pub_ = _node_hand.advertise<geometry_msgs::PoseArray> ("/Planning_Module/path", 1, true);
23 
24  //=======================================================================
25  // Service
26  //=======================================================================
27  service_ = _node_hand.advertiseService("/controller_turtlebot/find_path_to_goal", &ROS_OMPL_Planner::findPathToGoal, this);
28 
29  planning_bounds_x_.resize(2);
30  planning_bounds_y_.resize(2);
31  planning_bounds_z_.resize(2);
32  current_position_.resize(3);
33  goal_state.resize(3);
34 
35  //=======================================================================
36  // Get parameters from configuration file.
37  //=======================================================================
38  _node_hand.getParam("planning_bounds_x", planning_bounds_x_);
39  _node_hand.getParam("planning_bounds_y", planning_bounds_y_);
40  _node_hand.getParam("planning_bounds_z", planning_bounds_z_);
41 
42  _node_hand.getParam("solving_time", solving_time_);
43  _node_hand.getParam("planner_name", planner_name_);
44 
45  _node_hand.getParam("collision_mask_x", collision_mask.x);
46  _node_hand.getParam("collision_mask_y", collision_mask.y);
47 
48  if(dimension==2)
49  _node_hand.getParam("collision_mask_z_2D", collision_mask.z);
50  else
51  _node_hand.getParam("collision_mask_z_3D", collision_mask.z);
52 
53  _node_hand.getParam("planning_depth_limit", planning_depth_limit);
54 
55  //=======================================================================
56  // Planner setup.
57  //=======================================================================
58 
59  planner.setPlanningDepth(planning_depth_limit);
60 
61  if (dimension == 2)
62  planner.setPlanningBounds(planning_bounds_x_, planning_bounds_y_);
63  else
64  planner.setPlanningBounds(planning_bounds_x_, planning_bounds_y_, planning_bounds_z_);
65 
66  planner.setPlanner(planner_name_);
67 
68  updateMap();
69  planner.setStateChecker(octree_, collision_mask.x, collision_mask.y, collision_mask.z);
70 
71  ROS_INFO("%s: %dD Planner Init done !", ros::this_node::getName().c_str(), dimension);
72 }
73 
75 {
76  if(octree_ !=nullptr)
77  {
78  delete octree_;
79  octree_ = nullptr;
80  }
81 
82  ROS_INFO("%s: requesting the map to %s...", ros::this_node::getName().c_str(), _node_hand.resolveName(serv_name).c_str());
83 
84  while((_node_hand.ok() && !ros::service::call(serv_name, req, resp)) || resp.map.data.size()==0)
85  {
86  ROS_WARN("Request to %s failed; trying again...", _node_hand.resolveName(serv_name).c_str());
87  usleep(1000000);
88  }
89  if (_node_hand.ok()){ // skip when CTRL-C
90  abs_octree_ = octomap_msgs::msgToMap(resp.map);
91 
92  if (abs_octree_)
93  {
94  octree_ = dynamic_cast<octomap::OcTree*>(abs_octree_);
95  }
96 
97  if (octree_){
98  ROS_INFO("Octomap received (%zu nodes)", octree_->size());
99  return true;
100  }
101  else{
102  ROS_ERROR("Error reading OcTree from stream");
103  return false;
104  }
105  }
106 }
107 
108 void ROS_OMPL_Planner::odomCallback(const nav_msgs::OdometryPtr &odom_msg)
109 {
110  current_position_[0] = odom_msg->pose.pose.position.x;
111  current_position_[1] = odom_msg->pose.pose.position.y;
112  current_position_[2] = odom_msg->pose.pose.position.z;
113 }
114 
115 bool ROS_OMPL_Planner::findPathToGoal(control_turtlebot::FindPathToGoal::Request& request, control_turtlebot::FindPathToGoal::Response& response)
116 {
117  goal_state[0] = request.goal_state_x;
118  goal_state[1] = request.goal_state_y;
119  goal_state[2] = request.goal_state_z;
120 
121  //Prevent invalid states because of too low altitude (Octomap issue)
122  if(goal_state[2]<planning_depth_limit) goal_state[2] = planning_depth_limit;
123  if(current_position_[2]<planning_depth_limit) current_position_[2] = planning_depth_limit;
124 
125  updateMap();
126  planner.updateStateChecker(octree_);
127 
128  // Start and Goal checking
129  if(! planner.isValid(current_position_))
130  ROS_WARN("%s: Start Position %lf/%lf/%lf is invalid !\n",ros::this_node::getName().c_str(), current_position_[0],current_position_[1],current_position_[2]);
131  if(! planner.isValid(goal_state))
132  ROS_WARN("%s: Goal Position %lf/%lf/%lf is invalid !\n",ros::this_node::getName().c_str(), goal_state[0],goal_state[1],goal_state[2]);
133 
134  planner.setStartGoal(current_position_, goal_state);
135 
136  ROS_INFO("%s: Searching path from %lf/%lf/%lf to %lf/%lf/%lf ...\n",ros::this_node::getName().c_str(), current_position_[0],current_position_[1],current_position_[2], goal_state[0],goal_state[1],goal_state[2]);
137 
138  if(planner.findPath(solving_time_))
139  {
140  _obstacle_flag = false;
141  ROS_INFO("%s: path has been found with simple_setup", ros::this_node::getName().c_str());
142  PublishPath(response);
143  }
144  else
145  ROS_WARN("%s: path has not been found", ros::this_node::getName().c_str());
146 
147  return true;
148 }
149 
150 //REALLY Ugly function
151 void ROS_OMPL_Planner::visualizeRRT(const og::SimpleSetup simple_setup)
152 {
153  og::PathGeometric geopath = simple_setup.getSolutionPath();
154  // %Tag(MARKER_INIT)%
155  visualization_msgs::Marker q_init_goal, visual_rrt, result_path, visual_result_path;
156  visual_result_path.header.frame_id = result_path.header.frame_id = q_init_goal.header.frame_id = visual_rrt.header.frame_id = "/odom";
157  visual_result_path.header.stamp = result_path.header.stamp = q_init_goal.header.stamp = visual_rrt.header.stamp = ros::Time::now();
158  q_init_goal.ns = "online_planner_points";
159  visual_rrt.ns = "online_planner_rrt";
160  result_path.ns = "online_planner_result";
161  visual_result_path.ns = "online_planner_result_path";
162  visual_result_path.action = result_path.action = q_init_goal.action = visual_rrt.action = visualization_msgs::Marker::ADD;
163 
164  visual_result_path.pose.orientation.w = result_path.pose.orientation.w = q_init_goal.pose.orientation.w = visual_rrt.pose.orientation.w = 1.0;
165  // %EndTag(MARKER_INIT)%
166 
167  // %Tag(ID)%
168  q_init_goal.id = 0;
169  visual_rrt.id = 1;
170  result_path.id = 2;
171  visual_result_path.id = 3;
172  // %EndTag(ID)%
173 
174  // %Tag(TYPE)%
175  result_path.type = q_init_goal.type = visualization_msgs::Marker::POINTS;
176  visual_rrt.type = visual_result_path.type = visualization_msgs::Marker::LINE_LIST;
177  // %EndTag(TYPE)%
178 
179  // %Tag(SCALE)%
180  // POINTS markers use x and y scale for width/height respectively
181  result_path.scale.x = q_init_goal.scale.x = 0.5;
182  result_path.scale.y = q_init_goal.scale.y = 0.5;
183  result_path.scale.z = q_init_goal.scale.z = 0.5;
184 
185  // LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width
186  visual_rrt.scale.x = 0.01;
187  visual_result_path.scale.x = 0.02;
188  // %EndTag(SCALE)%
189 
190  // %Tag(COLOR)%
191  // Points are green
192  visual_result_path.color.g = 1.0;
193  result_path.color.g = q_init_goal.color.g = 1.0;
194  visual_result_path.color.a = result_path.color.a = q_init_goal.color.a = 1.0;
195 
196  // Line strip is blue
197  visual_rrt.color.b = 1.0;
198  visual_rrt.color.a = 1.0;
199 
200 
201  ob::PlannerData planner_data(simple_setup.getSpaceInformation());
202  simple_setup.getPlannerData(planner_data);
203  std::vector< unsigned int > edgeList;
204  int num_parents;
205  //const ob::SE2StateSpace::StateType *state_se2;
206  const ob::RealVectorStateSpace::StateType *state_r2;
207 
208  const ob::RealVectorStateSpace::StateType *state;
209  geometry_msgs::Point p;
210 
211  if(planningDimension==3)
212  {
213  for (unsigned int i = 1 ; i < planner_data.numVertices() ; ++i)
214  {
215  if (planner_data.getVertex(i).getState() && planner_data.getIncomingEdges(i,edgeList) > 0)
216  {
217  state_r2 = planner_data.getVertex(i).getState()->as<ob::RealVectorStateSpace::StateType>();
218  p.x = state_r2->values[0];
219  p.y = state_r2->values[1];
220  p.z = state_r2->values[2];//3D change
221  visual_rrt.points.push_back(p);
222 
223  state_r2 = planner_data.getVertex(edgeList[0]).getState()->as<ob::RealVectorStateSpace::StateType>();
224  p.x = state_r2->values[0];
225  p.y = state_r2->values[1];
226  p.z = state_r2->values[2];//3D change
227  visual_rrt.points.push_back(p);
228  }
229  }
230  std::vector< ob::State * > states = geopath.getStates();
231 
232  for (uint32_t i = 0; i < geopath.getStateCount(); ++i)
233  {
234  // extract the component of the state and cast it to what we expect
235  state = states[i]->as<ob::RealVectorStateSpace::StateType>();
236 
237  p.x = state->values[0];
238  p.y = state->values[1];
239  p.z = state->values[2];//3D change
240 
241  result_path.points.push_back(p);
242 
243  if(i>0)
244  {
245  visual_result_path.points.push_back(p);
246  state = states[i-1]->as<ob::RealVectorStateSpace::StateType>();
247 
248  p.x = state->values[0];
249  p.y = state->values[1];
250  p.z = state->values[2];//3D change
251  visual_result_path.points.push_back(p);
252  }
253  }
254  }
255  else
256  {
257  for (unsigned int i = 1 ; i < planner_data.numVertices() ; ++i)
258  {
259  if (planner_data.getVertex(i).getState() && planner_data.getIncomingEdges(i,edgeList) > 0)
260  {
261  state_r2 = planner_data.getVertex(i).getState()->as<ob::RealVectorStateSpace::StateType>();
262  p.x = state_r2->values[0];
263  p.y = state_r2->values[1];
264  p.z = planning_depth_limit; //2D change
265  visual_rrt.points.push_back(p);
266 
267  state_r2 = planner_data.getVertex(edgeList[0]).getState()->as<ob::RealVectorStateSpace::StateType>();
268  p.x = state_r2->values[0];
269  p.y = state_r2->values[1];
270  p.z = planning_depth_limit; //2D change
271  visual_rrt.points.push_back(p);
272  }
273  }
274  std::vector< ob::State * > states = geopath.getStates();
275 
276  for (uint32_t i = 0; i < geopath.getStateCount(); ++i)
277  {
278  // extract the component of the state and cast it to what we expect
279  state = states[i]->as<ob::RealVectorStateSpace::StateType>();
280 
281  p.x = state->values[0];
282  p.y = state->values[1];
283  p.z = planning_depth_limit; //2D change
284 
285  result_path.points.push_back(p);
286 
287  if(i>0)
288  {
289  visual_result_path.points.push_back(p);
290  state = states[i-1]->as<ob::RealVectorStateSpace::StateType>();
291 
292  p.x = state->values[0];
293  p.y = state->values[1];
294  p.z = planning_depth_limit; //2D change
295  visual_result_path.points.push_back(p);
296  }
297  }
298  }
299  //visual_traj_pub_.publish(q_init_goal);
300  visual_traj_pub_.publish(visual_rrt);
301  visual_traj_pub_.publish(visual_result_path);
302  //visual_traj_pub_.publish(result_path);
303 }
304 
305 //Ugly function
306 void ROS_OMPL_Planner::PublishPath(control_turtlebot::FindPathToGoal::Response& response)
307 {
308  og::PathGeometric path = planner.getPath();
309  //Display path
310  visualizeRRT(planner.getSimpleSetup());
311 
312  std::vector< ob::State * > states = path.getStates();
313  const ob::RealVectorStateSpace::StateType *state;
314 
315  uint32_t state_nb = path.getStateCount();
316 
317  geometry_msgs::PoseArray path_msg;
318 
319  //A CHANGER : passage du nombre de state
320  //Passer par stamp dans le header ?
321  geometry_msgs::Pose state_nb_msg;
322  state_nb_msg.position.x=state_nb;
323  path_msg.poses.push_back(state_nb_msg);
324 
325  //ROS_INFO("State nb : %d",state_nb);
326  if(planningDimension==3)
327  {
328  for (uint32_t i = 0; i < state_nb; ++i)
329  {
330  geometry_msgs::Pose pose;
331  geometry_msgs::Point point;
332  // extract the component of the state and cast it to what we expect
333  state = states[i]->as<ob::RealVectorStateSpace::StateType>();
334 
335  point.x = state->values[0];
336  point.y = state->values[1];
337  point.z = state->values[2];
338 
339  pose.position = point;
340 
341  path_msg.poses.push_back(pose);
342  //Send path as response of the planning request
343  response.poses.push_back(pose);
344  }
345  }
346  else
347  {
348  for (uint32_t i = 0; i < state_nb; ++i)
349  {
350  geometry_msgs::Pose pose;
351  geometry_msgs::Point point;
352  // extract the component of the state and cast it to what we expect
353  state = states[i]->as<ob::RealVectorStateSpace::StateType>();
354 
355  point.x = state->values[0];
356  point.y = state->values[1];
357  point.z = planning_depth_limit;
358 
359  pose.position = point;
360 
361  path_msg.poses.push_back(pose);
362  //Send path as response of the planning request
363  response.poses.push_back(pose);
364  }
365  }
366  //Publish the path on a topic
367  path_pub_.publish(path_msg);
368 }
369 
371 {
372  og::PathGeometric path = planner.getPath();
373  //Display path
374  visualizeRRT(planner.getSimpleSetup());
375 
376  std::vector< ob::State * > states = path.getStates();
377  const ob::RealVectorStateSpace::StateType *state;
378 
379  uint32_t state_nb = path.getStateCount();
380 
381  geometry_msgs::PoseArray path_msg;
382 
383  //A CHANGER : passage du nombre de state
384  //Passer par stamp dans le header ?
385  geometry_msgs::Pose state_nb_msg;
386  state_nb_msg.position.x=state_nb;
387  path_msg.poses.push_back(state_nb_msg);
388 
389  //ROS_INFO("State nb : %d",state_nb);
390  if(planningDimension==3)
391  {
392  for (uint32_t i = 0; i < state_nb; ++i)
393  {
394  geometry_msgs::Pose pose;
395  geometry_msgs::Point point;
396  // extract the component of the state and cast it to what we expect
397  state = states[i]->as<ob::RealVectorStateSpace::StateType>();
398 
399  point.x = state->values[0];
400  point.y = state->values[1];
401  point.z = state->values[2];
402 
403  pose.position = point;
404 
405  path_msg.poses.push_back(pose);
406  }
407  }
408  else
409  {
410  for (uint32_t i = 0; i < state_nb; ++i)
411  {
412  geometry_msgs::Pose pose;
413  geometry_msgs::Point point;
414  // extract the component of the state and cast it to what we expect
415  state = states[i]->as<ob::RealVectorStateSpace::StateType>();
416 
417  point.x = state->values[0];
418  point.y = state->values[1];
419  point.z = planning_depth_limit;
420 
421  pose.position = point;
422 
423  path_msg.poses.push_back(pose);
424  }
425  }
426  //Publish the path on a topic
427  path_pub_.publish(path_msg);
428 }
429 
431 {
432  return _obstacle_flag;
433 }
434 
436 {
437  ROS_INFO("%s: Updating path ...\n",ros::this_node::getName().c_str());
438 
439  if(current_position_[2]<planning_depth_limit) current_position_[2] = planning_depth_limit;
440 
441  updateMap();
442 
443  // Start and Goal checking
444  if(! planner.isValid(current_position_))
445  ROS_WARN("%s: Start Position %lf/%lf/%lf is invalid !\n",ros::this_node::getName().c_str(), current_position_[0],current_position_[1],current_position_[2]);
446  if(! planner.isValid(goal_state))
447  ROS_WARN("%s: Goal Position %lf/%lf/%lf is invalid !\n",ros::this_node::getName().c_str(), goal_state[0],goal_state[1],goal_state[2]);
448 
449  if(planner.updatePath(octree_, solving_time_, current_position_, goal_state))
450  {
451  _obstacle_flag=false;
452  PublishPath();
453  ROS_INFO("%s: Path update done !\n",ros::this_node::getName().c_str());
454  return true;
455  }
456  else
457  {
458  _obstacle_flag = true;
459  ROS_WARN("%s: Path update failed !\n",ros::this_node::getName().c_str());
460  return false;
461  }
462 }
463 
465 {
466 
467 }
bool updatePath(octomap::OcTree *octree, double solving_time, const std::vector< double > start_state, const std::vector< double > goal_state)
Update the path.
void setStateChecker(const ob::StateValidityCheckerPtr state_checker)
Set the state checker.
void setStartGoal(const std::vector< double > start_state, const std::vector< double > goal_state)
Set the planning objectives.
bool findPath(double solving_time)
Solve the planning problem.
void visualizeRRT(const og::SimpleSetup simple_setup)
Procedure to visualize the resulting path.
og::PathGeometric getPath() const
Return the path found by the planner.
void setPlanningDepth(double depth_limit=1)
Set the planning depth.
bool updatePath()
Update the path.
void setPlanningBounds(const std::vector< double > planning_bounds_x, const std::vector< double > planning_bounds_y, const std::vector< double > planning_bounds_z=std::vector< double >())
Set the bounds of the planner workspace.
~ROS_OMPL_Planner()
Destructor.
og::SimpleSetup getSimpleSetup() const
Return the OMPL object containing most of the information of the planning problem.
bool updateMap()
Request and load the actual map from the Octomap server.
void PublishPath()
Publish the resulting path.
void setPlanner(const std::string plannerName="RRTstar")
Set the planning algorithm used to solve the problem.
void updateStateChecker(octomap::OcTree *octree)
Update the state checker.
ROS_OMPL_Planner(int dimension)
Constructor.
bool getObstacleFlag() const
Return the flag representing the validity of the current path.
void odomCallback(const nav_msgs::OdometryPtr &odom_msg)
Callback for getting current vehicle position.
bool findPathToGoal(control_turtlebot::FindPathToGoal::Request &request, control_turtlebot::FindPathToGoal::Response &response)
Find a path.
bool isValid(std::vector< double > position) const
Check if position is collision-free.