下面这一句:
std::pair<double, double> p_n;
完整代码:
//调用函数七:判断两节点之间是否有障碍物
//obstacleFree
bool RRTstarPlannerROS::obstacleFree(Node node_nearest, double px, double py)
{
std::pair<double, double> p_n;
p_n.first = 0.0;
p_n.second = 0.0;
double dist = distance(node_nearest.x, node_nearest.y, px, py);
if (dist < resolution_)
{
if (collision(px, py))
return false;
else
return true;
}
else
{
int value = int(floor(dist/resolution_));
double theta = atan2(py - node_nearest.y, px - node_nearest.x);
int n = 1;
for (int i = 0;i < value; i++)
{
p_n.first = node_nearest.x + n*resolution_*cos(theta);
p_n.second = node_nearest.y + n*resolution_*sin(theta);
if (collision(p_n.first, p_n.second))
return false;
n++;
}
return true;
}
}