8 #include "ROS_OMPL_Planner.hpp" 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);
29 planning_bounds_x_.resize(2);
30 planning_bounds_y_.resize(2);
31 planning_bounds_z_.resize(2);
32 current_position_.resize(3);
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_);
42 _node_hand.getParam(
"solving_time", solving_time_);
43 _node_hand.getParam(
"planner_name", planner_name_);
45 _node_hand.getParam(
"collision_mask_x", collision_mask.x);
46 _node_hand.getParam(
"collision_mask_y", collision_mask.y);
49 _node_hand.getParam(
"collision_mask_z_2D", collision_mask.z);
51 _node_hand.getParam(
"collision_mask_z_3D", collision_mask.z);
53 _node_hand.getParam(
"planning_depth_limit", planning_depth_limit);
64 planner.
setPlanningBounds(planning_bounds_x_, planning_bounds_y_, planning_bounds_z_);
69 planner.
setStateChecker(octree_, collision_mask.x, collision_mask.y, collision_mask.z);
71 ROS_INFO(
"%s: %dD Planner Init done !", ros::this_node::getName().c_str(), dimension);
82 ROS_INFO(
"%s: requesting the map to %s...", ros::this_node::getName().c_str(), _node_hand.resolveName(serv_name).c_str());
84 while((_node_hand.ok() && !ros::service::call(serv_name, req, resp)) || resp.map.data.size()==0)
86 ROS_WARN(
"Request to %s failed; trying again...", _node_hand.resolveName(serv_name).c_str());
90 abs_octree_ = octomap_msgs::msgToMap(resp.map);
94 octree_ =
dynamic_cast<octomap::OcTree*
>(abs_octree_);
98 ROS_INFO(
"Octomap received (%zu nodes)", octree_->size());
102 ROS_ERROR(
"Error reading OcTree from stream");
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;
117 goal_state[0] = request.goal_state_x;
118 goal_state[1] = request.goal_state_y;
119 goal_state[2] = request.goal_state_z;
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;
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]);
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]);
140 _obstacle_flag =
false;
141 ROS_INFO(
"%s: path has been found with simple_setup", ros::this_node::getName().c_str());
145 ROS_WARN(
"%s: path has not been found", ros::this_node::getName().c_str());
153 og::PathGeometric geopath = simple_setup.getSolutionPath();
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;
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;
171 visual_result_path.id = 3;
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;
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;
186 visual_rrt.scale.x = 0.01;
187 visual_result_path.scale.x = 0.02;
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;
197 visual_rrt.color.b = 1.0;
198 visual_rrt.color.a = 1.0;
201 ob::PlannerData planner_data(simple_setup.getSpaceInformation());
202 simple_setup.getPlannerData(planner_data);
203 std::vector< unsigned int > edgeList;
206 const ob::RealVectorStateSpace::StateType *state_r2;
208 const ob::RealVectorStateSpace::StateType *state;
209 geometry_msgs::Point p;
211 if(planningDimension==3)
213 for (
unsigned int i = 1 ; i < planner_data.numVertices() ; ++i)
215 if (planner_data.getVertex(i).getState() && planner_data.getIncomingEdges(i,edgeList) > 0)
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];
221 visual_rrt.points.push_back(p);
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];
227 visual_rrt.points.push_back(p);
230 std::vector< ob::State * > states = geopath.getStates();
232 for (uint32_t i = 0; i < geopath.getStateCount(); ++i)
235 state = states[i]->as<ob::RealVectorStateSpace::StateType>();
237 p.x = state->values[0];
238 p.y = state->values[1];
239 p.z = state->values[2];
241 result_path.points.push_back(p);
245 visual_result_path.points.push_back(p);
246 state = states[i-1]->as<ob::RealVectorStateSpace::StateType>();
248 p.x = state->values[0];
249 p.y = state->values[1];
250 p.z = state->values[2];
251 visual_result_path.points.push_back(p);
257 for (
unsigned int i = 1 ; i < planner_data.numVertices() ; ++i)
259 if (planner_data.getVertex(i).getState() && planner_data.getIncomingEdges(i,edgeList) > 0)
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;
265 visual_rrt.points.push_back(p);
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;
271 visual_rrt.points.push_back(p);
274 std::vector< ob::State * > states = geopath.getStates();
276 for (uint32_t i = 0; i < geopath.getStateCount(); ++i)
279 state = states[i]->as<ob::RealVectorStateSpace::StateType>();
281 p.x = state->values[0];
282 p.y = state->values[1];
283 p.z = planning_depth_limit;
285 result_path.points.push_back(p);
289 visual_result_path.points.push_back(p);
290 state = states[i-1]->as<ob::RealVectorStateSpace::StateType>();
292 p.x = state->values[0];
293 p.y = state->values[1];
294 p.z = planning_depth_limit;
295 visual_result_path.points.push_back(p);
300 visual_traj_pub_.publish(visual_rrt);
301 visual_traj_pub_.publish(visual_result_path);
308 og::PathGeometric path = planner.
getPath();
312 std::vector< ob::State * > states = path.getStates();
313 const ob::RealVectorStateSpace::StateType *state;
315 uint32_t state_nb = path.getStateCount();
317 geometry_msgs::PoseArray path_msg;
321 geometry_msgs::Pose state_nb_msg;
322 state_nb_msg.position.x=state_nb;
323 path_msg.poses.push_back(state_nb_msg);
326 if(planningDimension==3)
328 for (uint32_t i = 0; i < state_nb; ++i)
330 geometry_msgs::Pose pose;
331 geometry_msgs::Point point;
333 state = states[i]->as<ob::RealVectorStateSpace::StateType>();
335 point.x = state->values[0];
336 point.y = state->values[1];
337 point.z = state->values[2];
339 pose.position = point;
341 path_msg.poses.push_back(pose);
343 response.poses.push_back(pose);
348 for (uint32_t i = 0; i < state_nb; ++i)
350 geometry_msgs::Pose pose;
351 geometry_msgs::Point point;
353 state = states[i]->as<ob::RealVectorStateSpace::StateType>();
355 point.x = state->values[0];
356 point.y = state->values[1];
357 point.z = planning_depth_limit;
359 pose.position = point;
361 path_msg.poses.push_back(pose);
363 response.poses.push_back(pose);
367 path_pub_.publish(path_msg);
372 og::PathGeometric path = planner.
getPath();
376 std::vector< ob::State * > states = path.getStates();
377 const ob::RealVectorStateSpace::StateType *state;
379 uint32_t state_nb = path.getStateCount();
381 geometry_msgs::PoseArray path_msg;
385 geometry_msgs::Pose state_nb_msg;
386 state_nb_msg.position.x=state_nb;
387 path_msg.poses.push_back(state_nb_msg);
390 if(planningDimension==3)
392 for (uint32_t i = 0; i < state_nb; ++i)
394 geometry_msgs::Pose pose;
395 geometry_msgs::Point point;
397 state = states[i]->as<ob::RealVectorStateSpace::StateType>();
399 point.x = state->values[0];
400 point.y = state->values[1];
401 point.z = state->values[2];
403 pose.position = point;
405 path_msg.poses.push_back(pose);
410 for (uint32_t i = 0; i < state_nb; ++i)
412 geometry_msgs::Pose pose;
413 geometry_msgs::Point point;
415 state = states[i]->as<ob::RealVectorStateSpace::StateType>();
417 point.x = state->values[0];
418 point.y = state->values[1];
419 point.z = planning_depth_limit;
421 pose.position = point;
423 path_msg.poses.push_back(pose);
427 path_pub_.publish(path_msg);
432 return _obstacle_flag;
437 ROS_INFO(
"%s: Updating path ...\n",ros::this_node::getName().c_str());
439 if(current_position_[2]<planning_depth_limit) current_position_[2] = planning_depth_limit;
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]);
449 if(planner.
updatePath(octree_, solving_time_, current_position_, goal_state))
451 _obstacle_flag=
false;
453 ROS_INFO(
"%s: Path update done !\n",ros::this_node::getName().c_str());
458 _obstacle_flag =
true;
459 ROS_WARN(
"%s: Path update failed !\n",ros::this_node::getName().c_str());
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.