void my_parse_voir(char *str, int n, t_server *s_info)
{
int x;
int y;
int i;
i = 0;
if (strcmp(str, "voir") == 0)
act_voir(s_info, n);
else if (strncmp(str, "pose", 4) == 0)
my_pose(n, s_info, &str[5]);
else
my_parse_other(str, n , s_info);
}
/*
void map_callback(const nav_msgs::OccupancyGrid::ConstPtr &msg)
{
// threshold the map to find frontier
//SimpleCCL cclo;
//cclo.th = 255;
//cclo.setMap(*msg);
//cclo.print();
}
*/
bool find_next_frontier()
{
geometry_msgs::Pose pose;
if (!my_pose(&pose, listener, map_frame, base_frame))
{
//ROS_WARN("Cannot find my pose");
return false;
}
double dist = distance<geometry_msgs::Point, geometry_msgs::Point>(pose.position, frontier);
if (dist > goal_tolerance)
{
bool path_clear = false;
for (int i = 0; i < global_status.status_list.size(); i++)
{
int stat = global_status.status_list[i].status;
if ((stat == 1 && local_status) || stat == 0)
path_clear = true;
if (stat == 4 || stat == 5) // fail
{
reached_frontiers.push_back(frontier);
path_clear = false;
break;
}
}
if (path_clear)
return false;
}
else if (frontier.x != 0 || frontier.y != 0)
{
reached_frontiers.push_back(frontier);
}
local_status = true;
if (global_frontiers.size() == 0)
{
ROS_WARN("No frontier to allocate at this time");
reached_frontiers.clear();
return false;
}
if (random_frontier)
{
old_frontier = frontier;
frontier = global_frontiers[rand() % global_frontiers.size()];
return true;
}
// nearest frontier allocation using bird view
if (bird_view)
{
double mindist = 0, fr_dist;
bool allocated = false;
old_frontier = frontier;
for (int i = 0; i < global_frontiers.size(); i++)
{
dist = distance<geometry_msgs::Point, geometry_msgs::Point>(pose.position, global_frontiers[i]);
fr_dist = distance<geometry_msgs::Point, geometry_msgs::Point>(old_frontier, global_frontiers[i]);
if ((old_frontier.x == 0 && old_frontier.y == 0) || ((mindist == 0 || dist < mindist) && dist > goal_tolerance && fr_dist > frontier_tolerance))
{
frontier = global_frontiers[i];
mindist = dist;
allocated = true;
}
}
return allocated;
}
// nearest frontier allocation using plan from move_base
nav_msgs::GetPlan srv;
double mindist = 0, fr_dist;
bool allocated = false;
old_frontier = frontier;
for (int i = 0; i < global_frontiers.size(); i++)
{
srv.request.start.header.stamp = ros::Time::now();
srv.request.start.header.frame_id = map_frame;
srv.request.start.pose.position = pose.position;
srv.request.start.pose.orientation.x = 0.0;
srv.request.start.pose.orientation.y = 0.0;
srv.request.start.pose.orientation.z = 0.0;
srv.request.start.pose.orientation.x = 1.0;
srv.request.goal.header.stamp = ros::Time::now();
srv.request.goal.header.frame_id = map_frame;
srv.request.goal.pose.position = global_frontiers[i];
srv.request.goal.pose.orientation.x = 0.0;
srv.request.goal.pose.orientation.y = 0.0;
srv.request.goal.pose.orientation.z = 0.0;
srv.request.goal.pose.orientation.x = 1.0;
srv.request.tolerance = 0.5;
if (planner.call(srv))
{
dist = srv.response.plan.poses.size();
if ((old_frontier.x == 0 && old_frontier.y == 0) || ((mindist == 0 || dist < mindist) ))
{
frontier = global_frontiers[i];
mindist = dist;
allocated = true;
}
}
else
{
ROS_ERROR("Failed to call service %s", planner_srv.c_str());
continue;
}
}
return allocated;
}
