【cartographer_ros】五: 发布和订阅陀螺仪Imu信息

人工智能108

上一节介绍了里程计Odometry传感数据的订阅和发布。

本节会介绍陀螺仪Imu数据的发布和订阅。陀螺仪在cartographer中主要用于前端位置预估和后端优化。

查看终端上的消息数据结构:

[En]

View the message data structure on the terminal:

rosmsg show sensor_msgs/Imu

Odometry消息类型数据结构如下:

Header header
geometry_msgs/Quaternion orientation
float64[9] orientation_covariance // Row major about x, y, z axes
geometry_msgs/Vector3 angular_velocity
float64[9] angular_velocity_covariance // Row major about x, y, z axes
geometry_msgs/Vector3 linear_acceleration
float64[9] linear_acceleration_covariance // Row major x, y z

其中linear_acceleration表示线加速度,angular_velocity表示角速度,orientation表示姿态,使用四元数表示。covariance表示对应协方差,体现各个数据的误差

陀螺仪用的是LPMS-IG1 RS232,这个陀螺仪同时能提供角速度 ,线加速度,和欧拉角。

#include <ros ros.h>
#include <tf tf.h>
#include <sensor_msgs imu.h>

using namespace std;

unsigned int step = 0;
unsigned int data_i = 0;
unsigned int data_len = 0;
unsigned char handle_buf[2048];

float acc[3];
float gyo[3];
float eular[3];

void DataReceivedCallback(std::vector<unsigned char> &data)
{
    unsigned char datasingle1;
    for (size_t k = 0; k < data.size(); k++)
    {
        datasingle1 = data[k];
        switch (step)
        {
        case 0:
        {
            if (datasingle1 == 0x3A)
            {
                step = 1;
                data_i = 0;
                memset(handle_buf, 0, 2048);
            }
            break;
        }
        case 1: // sensor id low
        {
            handle_buf[0] = datasingle1;
            step = 2;
            break;
        }
        case 2: // sensor id high
        {
            handle_buf[1] = datasingle1;
            step = 3;
            break;
        }
        case 3: //&#x6307;&#x4EE4;&#x53F7; low
        {
            handle_buf[2] = datasingle1;
            step = 4;
            break;
        }
        case 4: //&#x6307;&#x4EE4;&#x53F7; high
        {
            handle_buf[3] = datasingle1;
            step = 5;
            break;
        }
        case 5: //&#x6570;&#x636E;&#x957F;&#x5EA6; low
        {
            handle_buf[4] = datasingle1;
            data_len = datasingle1;
            step = 6;
            break;
        }
        case 6: //&#x6570;&#x636E;&#x957F;&#x5EA6; high
        {
            handle_buf[5] = datasingle1;
            data_len += (uint16_t)handle_buf[5] * 256;
            if (data_len > 512)
            {
                step = 0;
                cout << " data_len error : " << hex << datasingle1 << ",  " << data_len << std::endl;
            }
            else
            {
                if (data_len > 0)
                {
                    data_i = 0;
                    step = 7;
                }
                else
                {
                    step = 0;
                    cout << " data_len error : " << hex << datasingle1 << ",  " << data_len << std::endl;
                }
            }
            break;
        }
        case 7:
        {
            handle_buf[data_i + 6] = datasingle1;
            data_i++;
            if (data_i >= data_len + 4) //&#x5B8C;&#x6574;&#x4E00;&#x5E27;
            {
                //&#x5224;&#x65AD;&#x5305;&#x5C3E;
                if ((handle_buf[data_len + 8] != 0x0D) && (handle_buf[data_len + 9] != 0x0A))
                {
                    step = 0;
                    cout << " tail error : " << hex << handle_buf[data_len + 8] << ",  " << hex << handle_buf[data_len + 9] << std::endl;
                    break;
                }

                uint16_t lrc = ((uint16_t)handle_buf[data_len + 7] * 256) + (uint16_t)handle_buf[data_len + 6];

                //&#x5224;&#x65AD;lrc
                uint16_t sum_lrc = 0;
                for (unsigned int i = 0; i < (6 + data_len); i++)
                {
                    sum_lrc += handle_buf[i];
                }

                if (lrc != sum_lrc)
                {
                    step = 0;
                    cout << " crc error : " << lrc << ",  " << sum_lrc << std::endl;
                    break;
                }

                //&#x7EBF;&#x52A0;&#x901F;&#x5EA6;(&#x542B;&#x91CD;&#x529B;)
                acc[0] = *((float *)&handle_buf[22]);
                acc[1] = *((float *)&handle_buf[26]);
                acc[2] = *((float *)&handle_buf[30]);

                //&#x89D2;&#x901F;&#x5EA6;(&#x9640;&#x87BA;&#x4EEA;I&#x7684;&#x8F93;&#x51FA;)
                gyo[0] = *((float *)&handle_buf[82]);
                gyo[1] = *((float *)&handle_buf[86]);
                gyo[2] = *((float *)&handle_buf[90]);

                //&#x6B27;&#x62C9;&#x89D2;
                eular[0] = *((float *)&handle_buf[146]);
                eular[1] = *((float *)&handle_buf[150]);
                eular[2] = *((float *)&handle_buf[154]);

                step = 0;
            }
            break;
        }
        default:
            break;
        }
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "Imu_publisher");

    ros::NodeHandle n;
    ros::Publisher imu_pub = n.advertise<sensor_msgs::imu>("imu", 50);

    string device = "/dev/ttyUSB0";
    int baud_rate = 921600;
    int data_bits = 8;
    int stop_bits = 0;
    string parity = "n";

    boost::shared_ptr<serialport> serialPort;
    serialPort.reset(new SerialPort(device, baud_rate, data_bits, stop_bits, parity));
    auto binding = bind(&DataReceivedCallback, this, std::placeholders::_1);
    serialPort->setcallback(binding);
    if (serialPort->Open())
    {
        serialPort->LoadConfig();
        cout << "Init serial open success";
    }
    else
        cout << "Init serial open false";

    ros::Rate r(1.0);
    while (n.ok())
    {

        ros::spinOnce();

        sensor_msgs::Imu imu;
        imu.header.stamp = ros::Time::now();
        imu.header.frame_id = "base_link";
        imu.linear_acceleration.x = acc[0] * (-9.8);
        imu.linear_acceleration.y = acc[1] * (-9.8);
        imu.linear_acceleration.z = acc[2] * (-9.8);
        imu.angular_velocity.x = gyo[0] * 3.1415926 / 180.0;
        imu.angular_velocity.y = gyo[1] * 3.1415926 / 180.0;
        imu.angular_velocity.z = gyo[2] * 3.1415926 / 180.0;
        imu.orientation = tf::createQuaternionMsgFromRollPitchYaw(eular[0], eular[1], eular[2]);

        //&#x53D1;&#x5E03;Imu&#x6D88;&#x606F;
        imu_pub.publish(imu);

        last_time = current_time;
        r.sleep();
    }
}
</serialport></sensor_msgs::imu></unsigned></sensor_msgs></tf></ros>

SerialPort是自定义的串口通信类,附上代码:

(1) 通过rosbag订阅

rostopic echo /imu

(2) 通过rviz查看
打开rviz

rosrun rviz rviz

Fixed Frame修改为base_link,添加Imu并将Topic设为/imu

(3) 编写程序打印

#include "ros/ros.h"
#include "sensor_msgs/Imu.h"

void imuCallback(const sensor_msgs::Imu::ConstPtr &msg)
{
    ROS_INFO("imu: %f, %f, %f, %f, %f, %f, %f, %f, %f, %f", msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z,
             msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z,
             msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "listener");
    ros::NodeHandle node;
    ros::Subscriber subimu = node.subscribe("imu", 1000, imuCallback);
    ros::spin();
    return 0;
}

【完】

下一节会介绍路标Landmark数据的发布和订阅。

Original: https://www.cnblogs.com/CloudFlame/p/16443733.html
Author: CloudFlame
Title: 【cartographer_ros】五: 发布和订阅陀螺仪Imu信息



相关阅读

Title: 使用opencv分割图像(python实现)

使用opencv分割图像(python实现)

  1. 概述
    本人在做无人农机的时候需要进行图像处理,寻找目标物并进行分割,于是总结网络各种小车寻迹等等demo,自己也终于把图像颜色分割做的七七八八,特来记录一下,留作以后参考。
  2. 过程

  3. 导包

import numpy as np
import cv2
  • 读取图片(此处可以换成视频流,具体可以百度一下,原理相同,都是对一帧图片进行处理)
image = cv2.imread("C:/Users/92803/Downloads/1.jpg")
  • 对想要的颜色进行二值化处理
lower_blue = np.array([100,43,46])
upper_blue = np.array([124,255,255])
imgHSV = cv2.cvtColor(image,cv2.COLOR_BGR2HSV)
cv_image = cv2.inRange(imgHSV,lower_blue,upper_blue)
#cv_image&#x5DF2;&#x7ECF;&#x662F;&#x4E8C;&#x503C;&#x77E9;&#x9635;&#x4E86;

首先,图片从BGR格式转为HSV格式,因为RGB格式的图片在显示上非常方便,但是对于图像处理HSV格式是比较常用的,具体可以查看HSV格式和RGB格式的区别。
其次,cv2.inRange就是将HSV格式的图像进行二值化,在lower_blue和upper_blue之间的填充255,其余的都填充0。
具体各种颜色的值参考以下图片:
【cartographer_ros】五: 发布和订阅陀螺仪Imu信息
我以上填写的是过滤蓝色,填写对应的值就可以了。

  • 过滤之后发现还有一些噪点,于是接着对图像进行腐蚀和膨胀:
#&#x5BF9;&#x56FE;&#x7247;&#x8FDB;&#x884C;&#x8150;&#x8680;&#x64CD;&#x4F5C;
kernel = cv2.getStructuringElement(cv2.MORPH_RECT,(10,10))
#&#x8FD9;&#x91CC;&#x7684;&#x53C2;&#x6570;&#x662F;&#x9009;&#x62E9;&#x5377;&#x79EF;&#x6838;&#x7684;&#x5927;&#x5C0F;
cv_image = cv2.erode(cv_image,kernel)
#&#x5BF9;&#x56FE;&#x7247;&#x8FDB;&#x884C;&#x81A8;&#x80C0;&#x64CD;&#x4F5C;
kernel = cv2.getStructuringElement(cv2.MORPH_RECT,(10,10))
cv_image = cv2.dilate(cv_image,kernel)
  • 如果对结果还不满意,还可以对连通区域的面积进行过滤
contours,hie = cv2.findContours(cv_image,cv2.RETR_LIST,cv2.CHAIN_APPROX_NONE)
'''
&#x8C03;&#x7528;&#x51FD;&#x6570;&#x53D1;&#x73B0;&#x56FE;&#x50CF;&#x4E2D;&#x7684;&#x8FDE;&#x901A;&#x533A;&#x57DF;&#xFF0C;&#x6CE8;&#x610F;&#x8FD9;&#x91CC;opencv3&#x8FD4;&#x56DE;&#x7684;&#x662F;&#x4E09;&#x4E2A;&#x53C2;&#x6570;&#x5373;image,contours,hie
contours&#x4E0E;hie&#x4E00;&#x4E00;&#x5BF9;&#x5E94;&#xFF0C;&#x6839;&#x636E;findContours&#x5BFB;&#x627E;&#x7684;&#x6A21;&#x5F0F;&#xFF0C;contours&#x53EF;&#x4EE5;&#x627E;&#x5230;&#x8FDE;&#x901A;&#x533A;&#x57DF;&#x7684;&#x6240;&#x6709;&#x70B9;&#x8F6E;&#x5ED3;&#xFF0C;&#x8F6C;&#x6298;&#x70B9;&#x8F6E;&#x5ED3;&#x7B49;&#xFF0C;&#x6B64;&#x5904;&#x662F;&#x627E;&#x6240;&#x6709;&#x70B9;&#x8F6E;&#x5ED3;&#x3002;
len(contours)&#x8FD4;&#x56DE;&#x7684;&#x662F;&#x8FDE;&#x901A;&#x533A;&#x57DF;&#x7684;&#x4E2A;&#x6570;
'''
contours = np.array(contours,dtype=object)
#&#x4EE5;&#x4E0A;contours&#x8FD4;&#x56DE;&#x7684;&#x662F;&#x4E00;&#x4E2A;&#x5143;&#x7EC4;&#x5F62;&#x5F0F;&#xFF0C;&#x8F6C;&#x4E3A;ndarray&#x683C;&#x5F0F;&#xFF0C;&#x65B9;&#x4FBF;&#x5904;&#x7406;
index = 0
for contour in contours:
    area = cv2.contourArea(contour)
    #&#x8C03;&#x7528;&#x51FD;&#x6570;&#xFF0C;&#x6C42;&#x53D6;&#x8FDE;&#x901A;&#x533A;&#x57DF;&#x9762;&#x79EF;&#xFF08;&#x50CF;&#x7D20;&#x70B9;&#x4E2A;&#x6570;&#xFF09;
    if area < 10000:
        #&#x8BBE;&#x7F6E;&#x9608;&#x503C;
        cv2.drawContours(cv_image,contour[np.newaxis,:],-1,(0,0,0),cv2.FILLED)
        #&#x5BF9;&#x4E8E;&#x9762;&#x79EF;&#x5C0F;&#x4E8E;10000&#x50CF;&#x7D20;&#x7684;&#x8FDE;&#x901A;&#x533A;&#x57DF;&#xFF0C;&#x8FDB;&#x884C;&#x586B;&#x5145;&#x9ED1;&#x8272;&#xFF08;&#x672C;&#x6765;&#x662F;&#x767D;&#x8272;&#xFF09;
        contours = np.delete(contours,index)
        #&#x4ECE;contours &#x4E2D;&#x5220;&#x9664;
        index = index-1
        #&#x540C;&#x65F6;&#x7D22;&#x5F15;&#x51CF;1
    index = index + 1
    #&#x81EA;&#x589E;1

for index,contour in enumerate(contours):
    x,y,w,h = cv2.boundingRect(contour)
    cv2.rectangle(cv_image,(x,y),(x+w,y+h),(139,35,35),5)
    #&#x5BF9;&#x4E8E;&#x9762;&#x79EF;&#x5927;&#x4E8E;&#x7B49;&#x4E8E;10000&#x8FDE;&#x901A;&#x533A;&#x57DF;&#x5728;&#x56FE;&#x50CF;&#x4E2D;&#x753B;&#x51FA;&#x77E9;&#x5F62;

对于为什么不放在一个for循环中,是因为有的连通预期画出矩形之后,然后填充面积不符合的区域,可能会把画的矩形框给擦除,所以分两步进行处理。

  • 显示图片
cv2.imshow("image",cv_image)

#&#x8FD9;&#x4E00;&#x6B65;&#x662F;&#x5FC5;&#x987B;&#x7684;&#xFF0C;&#x4E0D;&#x52A0;&#x4F1A;&#x62A5;&#x9519;
cv2.waitKey(0)

参考资料:
1: cv2.findContours()函数
2:OpenCV-清除小面积连通域
3:HSV基本颜色分量范围
4:三分钟带你快速学习RGB、HSV和HSL颜色空间
5:cv2.inRange函数

Original: https://blog.csdn.net/weixin_41181442/article/details/125826001
Author: Ment_peo
Title: 使用opencv分割图像(python实现)