RRT的障碍物检测模块代码逻辑有问题

kun1224 commented 1 year ago

判断两点都没有与障碍物相交，并不能推出它们的连线不与障碍物相交

通过判断投影点是否与障碍物相交，来判断连线是否与障碍物相交？改动如下

/**
* 判断是否有障碍物
* @param node 节点坐标
* @return
*/
bool RRT::obstacleFree(RRT::Node *node) //??
{
double x1 = node->path_x[0];
double y1 = node->path_y[0];
double x2 = node->path_x[1];
double y2 = node->path_y[1];

for (vector<double> obs : obstacle_list)
{
    double a = (obs[0] - x1) * (x2 - x1) + (obs[1] - y1) * (y2 - y1); // 计算当前障碍物与连线的投影长度
    double b = (x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1);         // 计算连线长度的平方
    double t = a / b;                                                 // 计算投影长度与连线长度的比值
    if (t < 0.0)
    { // 如果比值小于0，则将比值设为0
        t = 0.0;
    }
    else if (t > 1.0)
    { // 如果比值大于1，则将比值设为1
        t = 1.0;
    }
    double x = x1 + t * (x2 - x1); // 计算投影点的x坐标
    double y = y1 + t * (y2 - y1); // 计算投影点的y坐标
    // 判断连线是否与当前障碍物相交
    if (pow(obs[0] - x, 2) + pow(obs[1] - y, 2) <= pow(obs[2] + robot_radius, 2))
    {
        return false; // 与障碍物相交，返回false
    }
}
return true; // safe
}

CHH3213 commented 1 year ago

判断两点都没有与障碍物相交，并不能推出它们的连线不与障碍物相交
1. 通过判断投影点是否与障碍物相交，来判断连线是否与障碍物相交？改动如下

/**
 * 判断是否有障碍物
 * @param node 节点坐标
 * @return
 */
bool RRT::obstacleFree(RRT::Node *node) //??
{
    double x1 = node->path_x[0];
    double y1 = node->path_y[0];
    double x2 = node->path_x[1];
    double y2 = node->path_y[1];

    for (vector<double> obs : obstacle_list)
    {
        double a = (obs[0] - x1) * (x2 - x1) + (obs[1] - y1) * (y2 - y1); // 计算当前障碍物与连线的投影长度
        double b = (x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1);         // 计算连线长度的平方
        double t = a / b;                                                 // 计算投影长度与连线长度的比值
        if (t < 0.0)
        { // 如果比值小于0，则将比值设为0
            t = 0.0;
        }
        else if (t > 1.0)
        { // 如果比值大于1，则将比值设为1
            t = 1.0;
        }
        double x = x1 + t * (x2 - x1); // 计算投影点的x坐标
        double y = y1 + t * (y2 - y1); // 计算投影点的y坐标
        // 判断连线是否与当前障碍物相交
        if (pow(obs[0] - x, 2) + pow(obs[1] - y, 2) <= pow(obs[2] + robot_radius, 2))
        {
            return false; // 与障碍物相交，返回false
        }
    }
    return true; // safe
}

yep, make sense. In my origin code, I just use a simple way to determine whether the agent collisions obstacle, since I didnot think too much. I have updated a new version to judge the relation between the agent and obstacle, The way I used is simlar to your way.

kun1224 commented 1 year ago

ok,good ! its my first issue I address to an open source item. your reply let me kown my isssue is meaningful,thank you!

CHH3213 commented 1 year ago

ok,good ! its my first issue I address to an open source item. your reply let me kown my isssue is meaningful,thank you!

if you find something related to code is wrong, you can also use "pull requests", I will review your requests. This is directive way to solve the wrong code.

CHH3213 / chhRobotics_CPP

RRT的障碍物检测模块代码逻辑有问题 #3