TARE-PLANNER学习记录

参考：

CMU-TARE 探索算法官方社区问答汇总_cmu localplanner 部署-CSDN博客

Tare_planner学习笔记_tare planner-CSDN博客

Tare_planner 学习教程(二)_tareplanner-CSDN博客

（学习笔记）机器人自主导航从零开始第七步——TARE Planner自主探索算法-CSDN博客

11.7

配置：

首先是github地址：caochao39/tare_planner: TARE Exploration Planner for Ground Vehicles

然后需要下载CMU仿真环境（以及仿真对应场景的mesh）GitHub - HongbiaoZ/autonomous_exploration_development_environment: Leveraging system development and robot deployment for ground-based autonomous navigation and exploration.

中途有libusb问题报错，修改了sixpair.c部分代码（本机是ubuntu20.04 noetic）

具体位置如下：

autonomous_exploration_development_environment-noetic/src/joystick_drivers/ps3joy/src/sixpair.c

下面是我修改的sixpair.c全部代码

#include <string.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h> // 添加 <stdlib.h> 以支持 exit
#include <libusb-1.0/libusb.h>

#define VENDOR 0x054c
#define PRODUCT 0x0268

#define USB_DIR_IN 0x80
#define USB_DIR_OUT 0

void fatal(char *msg) { perror(msg); exit(1); }

void show_master(libusb_device_handle *devh, int itfnum) {
    printf("Current Bluetooth master: ");
    unsigned char msg[8];
    int res = libusb_control_transfer
        (devh, USB_DIR_IN | LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE,
         0x01, 0x03f5, itfnum, msg, sizeof(msg), 5000);
    if (res < 0) { perror("USB_REQ_GET_CONFIGURATION"); return; }
    printf("%02x:%02x:%02x:%02x:%02x:%02x\n",
           msg[2], msg[3], msg[4], msg[5], msg[6], msg[7]);
}

void set_master(libusb_device_handle *devh, int itfnum, int mac[6]) {
    printf("Setting master bd_addr to %02x:%02x:%02x:%02x:%02x:%02x\n",
           mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
    unsigned char msg[8]= { 0x01, 0x00, mac[0], mac[1], mac[2], mac[3], mac[4], mac[5] };
    int res = libusb_control_transfer
        (devh,
         USB_DIR_OUT | LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE,
         0x09,
         0x03f5, itfnum, msg, sizeof(msg),
         5000);
    if (res < 0) fatal("USB_REQ_SET_CONFIGURATION");
}

void process_device(int argc, char **argv, libusb_device *dev,
                    struct libusb_config_descriptor *cfg, int itfnum) { // 修正类型声明
    int mac[6];

    libusb_device_handle *devh;
    int res = libusb_open(dev, &devh);
    if (res != 0 || !devh) fatal("usb_open");

    libusb_detach_kernel_driver(devh, itfnum);

    res = libusb_claim_interface(devh, itfnum);
    if (res < 0) fatal("usb_claim_interface");

    show_master(devh, itfnum);

    if (argc >= 2) {
        if (sscanf(argv[1], "%x:%x:%x:%x:%x:%x",
                   &mac[0],&mac[1],&mac[2],&mac[3],&mac[4],&mac[5]) != 6) {

            printf("usage: %s [<bd_addr of master>]\n", argv[0]);
            exit(1);
        }
    } else {
        FILE *f = popen("hcitool dev", "r");
        if (!f ||
            fscanf(f, "%*s\n%*s %x:%x:%x:%x:%x:%x",
                   &mac[0],&mac[1],&mac[2],&mac[3],&mac[4],&mac[5]) != 6) {
            printf("Unable to retrieve local bd_addr from `hcitool dev`.\n");
            printf("Please enable Bluetooth or specify an address manually.\n");
            exit(1);
        }
        pclose(f);
    }

    set_master(devh, itfnum, mac);

    libusb_release_interface(devh, itfnum);
    libusb_close(devh);
}

int main(int argc, char *argv[]) {  
    libusb_context *ctx = NULL;
    int r = libusb_init(&ctx);
    if (r < 0) {
        fprintf(stderr, "Failed to initialize libusb: %s\n", libusb_error_name(r));
        return 1;
    }

    libusb_device **devs;
    ssize_t cnt = libusb_get_device_list(ctx, &devs);
    if (cnt < 0) {
        fprintf(stderr, "Failed to get device list\n");
        libusb_exit(ctx);
        return 1;
    }

    int found = 0;
    for (ssize_t i = 0; i < cnt; i++) {
        libusb_device *dev = devs[i];
        struct libusb_device_descriptor desc; // 使用struct修正类型声明
        int res = libusb_get_device_descriptor(dev, &desc);
        if (res < 0) {
            fprintf(stderr, "Failed to get device descriptor\n");
            continue;
        }

        if (desc.idVendor == VENDOR && desc.idProduct == PRODUCT) {
            process_device(argc, argv, dev, NULL, 0);
            found++;
        }
    }

    libusb_free_device_list(devs, 1);
    libusb_exit(ctx);

    if (!found) {
        printf("No controller found on USB busses. Please connect your joystick via USB.\n");
        return 1;
    }

    return 0;
}

然后是protobuf版本问题，降版本到protobuf3.6.1编译就ok了

运行仿真：

选取仿真的环境(替换enviroment)

source devel/setup.sh
roslaunch vehicle_simulator system_<environment>.launch

source devel/setup.sh
roslaunch tare_planner explore_<environment>.launch

比如要在garage中进行：

source devel/setup.sh
roslaunch vehicle_simulator system_garage.launch

source devel/setup.sh
roslaunch tare_planner explore_garage.launch

下面在campus环境运行，记录在仿真环节的问题：

1.or-tools解决tsp问题（局部路径规划），如何进行路径平滑？

2.随着机器人局部路径规划进行，以及机器人进行运动，发现绿色方块深浅发生变化，这代表什么变化了？

3.发现随着局部路径规划进行，外部绿色方块减少的时候，浅蓝色路径也会更新，是否和全局路径相关？

思考：局部路径算法和全局路径算法分开，局部路径负责动态更新绿色方框内各个view_point之间的路径规划，而全局路径是在绿色方块进行更新时，进行重新规划淡蓝色路径的。

ps：如何去确定哪几个viewpoint去和外面的绿色方块连接呢？

后续校园环境仿真方案：

1.纯lidar方案

采集纯激光雷达点云地图导入CloudCompare进行downsample和计算每个点的normal。然后将其保存成文件（比如ply格式），再用meshlab打开并重建成mesh。最后用meshlab把它存成dae格式的文件，加载进gazebo进行仿真即可。

2.rgb方案

对图像渲染有要求的话则可以用图像生成3D模型(campus环境制作方案)，先用google earth对其从不同的角度采集图片，最后用这些图片进行3D reconstruction

用谷歌地球采集图片之后，3D重建再保存为dae 使用meshroom

meshroom：https://github.com/alicevision/meshroom

FAST-LIO部署：

使用 fastlio 部署 system_real_robot.launch

问：使用fastlio部署system_real_robot.launch loam接口按照要求已经调整好，发现tf树没连接上，不知道怎么修改，还有fastlio的frame坐标在地图上混乱的应该怎么调整?
答：你好，我们的系统实际需要的就是两个话题，一个是包含sensor在map下的odometry(integrated_to_init)，另一个就是当前帧点云在map下的投影(velodyne_cloud_registered)，只要提供这两个信息，loam_interface会自己发布一套tf以及点云出来。需要注意的就是在fastlio里不能发布重复的map到sensor的关系，最简单的做法就是将fastlio里的map和sensor(如果有的话)都换成别的名字，防止重复发布有冲突的tf

调试问题：

https://bbs.csdn.net/topics/614500720

阅读源码：

1.exploration_path

主要是实现了一个用于探索和路径规划的基础数据结构

类名	作用
Node	表示探索路径中的一个节点，包含节点类型、位置、状态信息等。
ExplorationPath	管理和操作由多个节点组成的路径，支持路径的添加、转换、反转和可视化。