Is it possible to use RRT exploration on ROS noetic?

[SinanK94]

I would like to use the RRT exploration on ROS noetic, but my mobile robot is not moving anymore. Before I started with noetic I used ROS melodic and evertyhing was great. Is there another issue beside the difference between python 2.7 and python 3. What could create the problem by switching to python 3?

I did an investigation in the filter.py with the debugger in visual code and realized that the centroids array can not save the data from ms.cluster_centers_ (dtype = float64) in the array. Thats why the centroids array is empty and causes issues.

    while not rospy.is_shutdown():
        # -------------------------------------------------------------------------
        # Clustering frontier points
        centroids = []
        front = copy(frontiers)
        if len(front) > 1:
            ms = MeanShift(bandwidth=0.3)
            ms.fit(front)
            centroids = ms.cluster_centers_  # centroids array is the centers of each cluster

        # if there is only one frontier no need for clustering, i.e. centroids=frontiers
        if len(front) == 1:
            centroids = front
        frontiers = copy(centroids)

What could be the solution for that?

Thank you very much! I appreciate your help!

[SinanK94]

The robot is moving right now, I fixed the issue. Could somebody please verify my solution?
What I only did was to change the filter.py code.

old filter.py:

while z < len(centroids):
          cond = False
          temppoint.point.x = centroids[z][0]
          temppoint.point.y = centroids[z][1]

          for i in range(0, n_robots):

              transformedPoint = tfLisn.transformPoint(
                  globalmaps[i].header.frame_id, temppoint)
              x = array([transformedPoint.point.x, transformedPoint.point.y])
              cond = (gridValue(globalmaps[i], x) > threshold) or cond
          if (cond or (informationGain(mapData, [centroids[z][0], centroids[z][1]], info_radius*0.5)) < 0.2):
              centroids = delete(centroids, (z), axis=0)
              z = z-1
          z += 1

      arraypoints.points = []
      for i in centroids:
          tempPoint.x = i[0]
          tempPoint.y = i[1]
          arraypoints.points.append(copy(tempPoint))
      filterpub.publish(arraypoints)
      pp = []
      for q in range(0, len(frontiers)):
          p.x = frontiers[q][0]
          p.y = frontiers[q][1]
          pp.append(copy(p))
      points.points = pp
      pp = []
      for q in range(0, len(centroids)):
          p.x = centroids[q][0]
          p.y = centroids[q][1]
          pp.append(copy(p))
      points_clust.points = pp
      pub.publish(points)
      pub2.publish(points_clust)
      rate.sleep()

new filter.py:

  while z < len(centroids):
            cond = False
            temppoint.point.x = centroids[z][0]
            temppoint.point.y = centroids[z][1]

            for i in range(0, n_robots):

                transformedPoint = tfLisn.transformPoint(
                    globalmaps[i].header.frame_id, temppoint)
                x = array([transformedPoint.point.x, transformedPoint.point.y])
                cond = (gridValue(globalmaps[i], x) > threshold) or cond
            if (cond or (informationGain(mapData, [centroids[z][0], centroids[z][1]], info_radius*0.5)) < 0.2):
                centroids = delete(centroids, (z), axis=0)
                z = z-1
            z += 1

          arraypoints.points = []
          for i in centroids:
              tempPoint.x = i[0]
              tempPoint.y = i[1]
              arraypoints.points.append(copy(tempPoint))
              filterpub.publish(arraypoints)
              pp = []
          for q in range(0, len(frontiers)):
              p.x = frontiers[q][0]
              p.y = frontiers[q][1]
              pp.append(copy(p))
              points.points = pp
              pp = []
         for q in range(0, len(centroids)):
              p.x = centroids[q][0]
              p.y = centroids[q][1]
              pp.append(copy(p))
              points_clust.points = pp
              pub.publish(points)
              pub2.publish(points_clust)
         rate.sleep()

Finally the problem was that the publishing code do not get any centroid data after the while loop. Therefore the assigner could not send any move action to the mobile robot.

[tomkimsour]

Could you make it a pull request ?

[phoenixrider12]

My robot is not moving even after this change also, nothing gets printed on the terminal after "Waiting for the robot transform". Can you help?
@SinanK94

[Liu404mob]

i

My robot is not moving even after this change also, nothing gets printed on the terminal after "Waiting for the robot transform". Can you help? @SinanK94

i meet the same situation,it is also not moving

[phoenixrider12]

What is the last thing that gets printed on terminal

[Gwak-tata]

I solved this problem by referring to pull requests and issues.
Some of the code may be unrelated, but I will let you know about all the files I modified.

scripts/ filter.py

def globalMap(data):
    global global1, globalmaps, litraIndx, namespace_init_count, n_robots
    global1 = data
    if n_robots > 1:
        indx = int(data._connection_header['topic']
                   [litraIndx+1])-namespace_init_count
    elif n_robots == 1:
        indx = 0
    globalmaps[indx] = data

>>line204
while z < len(centroids):
            cond = False
            temppoint.point.x = centroids[z][0]
            temppoint.point.y = centroids[z][1]

            for i in range(0, n_robots):

                transformedPoint = tfLisn.transformPoint(
                    globalmaps[i].header.frame_id, temppoint)
                x = array([transformedPoint.point.x, transformedPoint.point.y])
                cond = (gridValue(globalmaps[i], x) > threshold) or cond
            if (cond or (informationGain(mapData, [centroids[z][0], centroids[z][1]], info_radius*0.5)) < 0.2):
                centroids = delete(centroids, (z), axis=0)
                z = z-1
            z += 1

# -------------------------------------------------------------------------
# publishing
            arraypoints.points = []
            for i in centroids:
                tempPoint.x = i[0]
                tempPoint.y = i[1]
                arraypoints.points.append(copy(tempPoint))
                filterpub.publish(arraypoints)
                pp = []
            for q in range(0, len(frontiers)):
                p.x = frontiers[q][0]
                p.y = frontiers[q][1]
                pp.append(copy(p))
                points.points = pp
                pp = []
            for q in range(0, len(centroids)):
                p.x = centroids[q][0]
                p.y = centroids[q][1]
                pp.append(copy(p))
                points_clust.points = pp
                pub.publish(points)
                pub2.publish(points_clust)
            rate.sleep()

scripts/ functions.py

def point_of_index(mapData, i):
    y = mapData.info.origin.position.y + \
        (i//mapData.info.width)*mapData.info.resolution
    x = mapData.info.origin.position.x + \
        (i-(i//mapData.info.width)*(mapData.info.width))*mapData.info.resolution
    return array([x, y])

def informationGain(mapData, point, r):
    gain = 0
    resolution = mapData.info.resolution
    width = mapData.info.width
    map_min_x = mapData.info.origin.position.x
    map_min_y = mapData.info.origin.position.y
    center_x = int(floor((point[0] - map_min_x) / resolution))
    center_y = int(floor((point[1] - map_min_y) / resolution))
    r_int = int(r / resolution)
    for idx_y in range(center_y - r_int, center_y + r_int + 1):
        for idx_x in range(center_x - r_int, center_x + r_int + 1):
            idx = idx_y * width + idx_x
            # skip grids out of map
            if idx < 0 or idx >= len(mapData.data):
                continue
            if mapData.data[idx] == -1:
                gain += 1
    return gain * resolution ** 2

src/function.cpp

int8_t ObstacleFree(std::vector<float> xnear, std::vector<float> &xnew, nav_msgs::OccupancyGrid mapsub) {
    float rez = float(mapsub.info.resolution) * 0.2;
    float stepz = int(ceil(Norm(xnew, xnear)) / rez); 
    std::vector<float> xi = xnear;
    int8_t obs = 0; 
    int8_t unk = 0;

    geometry_msgs::Point p;
    for (int c = 0; c < stepz; c++) {
        xi = Steer(xi, xnew, rez);

        if (gridValue(mapsub, xi) == 100) {     
            obs = 1; 
        }

        if (gridValue(mapsub, xi) == -1) {      
            unk = 1;    
            break;
        }
    }

    int8_t out = 0;
    xnew = xi;
    if (unk == 1) {  
        out = -1;
    } else if (obs == 1) {  
        out = 0;
    } else {  
        out = 1;
    }

    return out;
}

include/ functions.h

int8_t ObstacleFree(std::vector<float> , std::vector<float> & , nav_msgs::OccupancyGrid);

src/ global_rrt_detector.cpp & local_rrt_detector.cpp

int8_t   checking=ObstacleFree(x_nearest,x_new,mapData);