AMCL定位融合UWB

2021/12/26，实验效果不好，发现多传感器融合好像不像我想得那么简单，还不太懂…

2021/12/27，一个大问题ekf并没有融合odom，odom_combined发布的数据和uwb(gps)基本是重合的，就算把数据改得很夸张，也和uwb(gps)重合，而ACML订阅的是什么呢？因为还能导航，ACML输出的位置数据和odom是差不多的。看了一些博客发现ekf有两种融合模式，一种是增加/gps话题，uwb以/gps话题发布，一种是uwb以/vo话题发布，两种形式的融合算法是不同的（具体我也没看），前者以/gps为主导，后者根据协方差加权？？？

AMCL定位融合UWB

ros串口通信

1.安装串口助手

sudo apt-get install cutecom
sudo cutecom

2.查看电脑链接的串口信息（名称）：

dmesg | grep ttyS*

然后在串口助手里打开串口没输出，而且dmesg | grep ttyS*报错了
3.设置串口权限（为了解决2的问题）

创建文件/etc/udev/rules.d/70-ttyusb.rules

sudo gedit /etc/udev/rules.d/70-ttyusb.rules

在文件内增加一行

KERNEL=="ttyUSB[0-9]*", MODE="0666"

增加访问权限

 sudo chmod 666 /dev/ttyUSB0

一顿操作下来，依然报2错，而且串口工具收不到数据

4.USB转串口驱动（PL2303）

参考ubuntu装pl2303USB转串口驱动
依然报错

5.可以用cutecom收发数据了

虽然能收数据，但是输入：dmesg | grep ttyS*依然报pl2303 converter now disconnected from ttyUSB0，也就是第2步的错，难道是传感器传输消息不稳定？我甚至错都没找到，诶，难道我只能当个复制粘贴怪？

6.下载ROS对应版本的工具包

sudo apt-get install ros-melodic-serial

进入下载的软件包的位置

roscd serial

若是安装成功会看到

$:/opt/ros/melodic/share/serial

7.将uwb数据融合进robot_pose_ekf定位的一点点理论

Adding a GPS sensor to the Robot Pose EKF Filter：官网通过robot_pose_ekf三个基本输入中odom,imu,vo中的三维视觉里程计VO来实现融合的，就是把GPS数据经过UTM转换再输入VO中
ROS中加入GPS的robot_pose_ekf定位：robot_pose_ekf可以接收4路输入分别是odom/imu/vo/gps
官网中gps的数据格式为：

msg.header.stamp = gps_time                   // time of gps measurement
 msg.header.frame_id = base_footprint          // the tracked robot frame
 msg.pose.pose.position.x = gps_x              // x measurement GPS.
 msg.pose.pose.position.y = gps_y              // y measurement GPS.
 msg.pose.pose.position.z = gps_z              // z measurement GPS.
 msg.pose.pose.orientation.x = 1               // identity quaternion
 msg.pose.pose.orientation.y = 0               // identity quaternion
 msg.pose.pose.orientation.z = 0               // identity quaternion
 msg.pose.pose.orientation.w = 0               // identity quaternion
 msg.pose.covariance = {cox_x, 0, 0, 0, 0, 0,  // covariance on gps_x
                        0, cov_y, 0, 0, 0, 0,  // covariance on gps_y
                        0, 0, cov_z, 0, 0, 0,  // covariance on gps_z
                        0, 0, 0, 99999, 0, 0,  // large covariance on rot x
                        0, 0, 0, 0, 99999, 0,  // large covariance on rot y
                        0, 0, 0, 0, 0, 99999}  // large covariance on rot z

对角线就是测量数据的方差，这里请教了朋友，协方差矩阵根据经验给定，可靠的数据就把协方差设置小点比如10的负六次方，不可靠的数据协方差设置大点比如10的六次方（毕竟方差越大的数据越不可信），也可以自己写算法计算可以动态调整的方差。

8:创建一个工作空间uwb_ws,并建立相应的pkg：uwbwork
参考：ROS下搭建 UWB 下行数据解析驱动

#include  
#include   //ROS已经内置了的串口包 
#include  
#include  
#include 
#include 
#include
#include 

int main (int argc,char** argv)
{ 

    float uwb_x;
    float uwb_y;
    char data_string[2][7];//字符串形式存串口读取的x,y 在用函数转为浮点型数据
    serial::Serial ser; //声明串口对象 
    //初始化节点 
    ros::init(argc, argv, "uwbsub"); 
    //声明节点句柄 
    ros::NodeHandle nh; 
    //发布UWB话题
    ros::Publisher uwb_publisher = nh.advertise<nav_msgs::Odometry>("gps", 100); 
    try 
    { 
        auto port_name =ros::param::param<std::string>("~port_name", "/dev/ttyUSB0");
        auto baud_rate = ros::param::param<int>("~baud_rate", 115200);
        auto timeout = serial::Timeout::simpleTimeout(1000);
        //设置串口属性，并打开串口 
        ser.setPort(port_name); 
        ser.setBaudrate(baud_rate); 
        ser.setTimeout(timeout); 
        ser.open(); 
    }
    catch (serial::IOException& e) 
    { 
       ROS_ERROR_STREAM("Unable to open port "); 
       return -1; 
    } 
    //检测串口是否已经打开，并给出提示信息 
    if(ser.isOpen()) 
    { 
       ROS_INFO_STREAM("Serial Port initialized"); 
    } 
    else 
    { 
        return -1; 
    }

    //指定循环的频率 
    ros::Rate loop_rate(200); 
    //处理从串口来的uwb数据
    while(ros::ok()) 
    { 
        //返回串口缓冲区中当前剩余的字符个数。一般用这个函数来判断串口的缓冲区有无数据
        //当Serial.available()>0时，说明串口接收到了数据，可以读取
		//串口缓存字符数
    	size_t n = ser.available();
        //ROS_INFO_STREAM(n);
        int32_t ex;
        int32_t ey;
        if(n>=22)
        { 
            uint8_t tmpdata[5000];
            ser.read(tmpdata,n);//tmpdata 参数缓存 data_size 数据长度
           if(tmpdata[0]==0X55)//效验是否正确
            {
             int32_t ax=tmpdata[8];
             int32_t bx=tmpdata[9];
             int32_t cx=tmpdata[10];
             int32_t dx=tmpdata[11];
             ex=ax+bx*16*16+cx*16*16*16*16+dx*16*16*16*16*16*16;
             int32_t ay=tmpdata[12];
             int32_t by=tmpdata[13];
             int32_t cy=tmpdata[14];
             int32_t dy=tmpdata[15];
             ey=ay+by*16*16+cy*16*16*16*16+dy*16*16*16*16*16*16;
           
        
             nav_msgs::Odometry uwb_data;
             float uwb_x=ex/100.0;
             float uwb_y=ey/100.0;
             ROS_INFO_STREAM("***************(x,y)***************");
             //ROS_INFO_STREAM(ex);
             //ROS_INFO_STREAM(ey);
             ROS_INFO_STREAM(uwb_x);
             ROS_INFO_STREAM(uwb_y);
             ROS_INFO_STREAM("***************(x,y)***************");
             uwb_data.pose.pose.position.x = uwb_x;
             uwb_data.pose.pose.position.y = uwb_y;
             uwb_data.pose.pose.position.z = 0;
             //返回当前时刻的Time对象
             uwb_data.header.stamp=ros::Time::now();
             uwb_data.header.frame_id="uwb";
             uwb_data.child_frame_id="base_footprint";
             uwb_data.pose.pose.orientation.x=1;
             uwb_data.pose.pose.orientation.y=0;
             uwb_data.pose.pose.orientation.z=0;
             uwb_data.pose.pose.orientation.w=0;
             uwb_data.pose.covariance={0.1, 0, 0, 0, 0, 0,  // covariance on gps_x
                                       0, 0.1, 0, 0, 0, 0,  // covariance on gps_y
                                       0, 0, 999999, 0, 0, 0,  // covariance on gps_z
                                       0, 0, 0, 999999, 0, 0,  // large covariance on rot x
                                       0, 0, 0, 0, 999999, 0,  // large covariance on rot y
                                       0, 0, 0, 0, 0, 999999} ;
             uwb_publisher.publish(uwb_data);
           //处理ROS的信息，比如订阅消息,并调用回调函数 
          }
        }
        ros::spinOnce(); 
        loop_rate.sleep(); 
    }
    //关闭串口
    ser.close();
    return 0;
}

8.录制bag文件
参考ROS下bag包的录制与回放数据

mkdir ~/savebag //创建新的文件夹
cd ~/savebag     //到这个文件夹下
rosbag record -a    //录制 -a表示录制了所有节点发布的话题
rosbag record -O setbagname 话题1 话题2  //录制部分话题

8.在rviz中显示轨迹

遇到的问题

串口工具cutecom可以打印信息，但是程序报错Unable to open port
串口添加权限，*是要打开的串口

sudo chmod 777 /dev/ttyUSB*

如何查看串口信息
查看所有串口信息（有时候是/drivers）：

cat /proc/tty/driver/serial

catkin_make不能生成可执行文件
指定功能包编译

catkin_make -DCATKIN_WHITELIST_PACKAGES="package_name"

运行串口读取，并做成topic发布程序时，报错Segmentation fault (core dumped)
Segmentation fault (core dumped)一般是指内存越界，将程序一点点注释排查后，应该是serial.read()引起的报错Segmentation fault (core dumped)
原程序为：

size_t n = ser.available();
uint8_t tmpdata[22];//串口返回的数据为22个字节
ser.read(tmpdata,n);

将22改为n出现过的最大数