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 0void 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.launchsource 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	管理和操作由多个节点组成的路径，支持路径的添加、转换、反转和可视化。