bool SBPLPlannerManager::
select(std::string const & name, bool recycle, std::ostream * opt_err_os)
{
if (recycle && (name == name_))
return true;
SBPLPlanner * new_planner(createSBPLPlanner(name, environment_, bforwardsearch_, mdpCfg_, opt_err_os));
if ( ! new_planner)
return false;
delete planner_;
planner_ = new_planner;
name_ = name;
return true;
}
//--------------------------------------------------------------------------------
//questa callback riceve in ingresso l'array di waypoints e per ogni coppia di
//waypoints adiacenti invoca il planner
//--------------------------------------------------------------------------------
void goalSelectionCallback(geometry_msgs::PoseArray waypoints){
//dimensione dell'array di waypoints
size_t n = waypoints.poses.size();
for( size_t i = 0; i < n; i++){
//istanzio un planner per ogni coppia di waypoints
PathPlanning new_planner(nh);
planner = &new_planner;
nav_msgs::Path path;
//al primo step il punto iniziale è la posa del robot
if( i == 0 ) {
planner->set_input(traversability_pcl,wall_pcl,wall_kdTree,traversability_kdtree,robot_idx);
}
//agli step successivi il punto iniziale è il goal dello step precedente
else {
pcl::PointXYZI in_point = convert(waypoints.poses.at(i-1));
//faccio il KNearestNeighbor search giusto per utilizzare la planner->set_input(..) scritta dagli altri
//poi dobbiamo scriverci la nostra set_input(..) e tutto questo non sarà più necessario
pcl::KdTreeFLANN<pcl::PointXYZI> kdtree;
kdtree.setInputCloud(traversability_pcl.makeShared());
std::vector<int> pointIdxNKNSearch(1);
std::vector<float> pointNKNSquaredDistance(1);
kdtree.nearestKSearch(in_point, 1, pointIdxNKNSearch, pointNKNSquaredDistance);
size_t input_idx = pointIdxNKNSearch[0];
planner->set_input(traversability_pcl,wall_pcl,wall_kdTree,traversability_kdtree,input_idx);
}
//qui setto il goal
pcl::PointXYZI goal_point = convert(waypoints.poses.at(i));
planner->set_goal(goal_point);
goal_selection = true;
ROS_INFO("goal selection");
//qui lancio il planner
if(planner->planning(path)){//<--questa funzione va riscritta!!!
path_pub.publish(path);
}
else{
ROS_INFO("no path exist for desired goal, please choose another goal");
goal_selection = true;
}
ROS_INFO("path_computed");
}
}
