移动机器人激光SLAM导航（二）：运动控制与传感器篇

参考引用

  • 机器人工匠阿杰
  • wpr_simulation

1. 机器人运动控制

1.1 测试环境安装

  • wpr_simulation 安装
    $ mkdir -p catkin_ws/src
$ cd catkin_ws/src
$ git clone https://github.com/6-robot/wpr_simulation.git
$ cd wpr_simulation/scripts/
$ ./install_for_melodic.sh  # 自动下载和安装编译需要的依赖项
$ cd ~/catkin_ws
$ catkin_make

1.2 控制量纲与消息包

  • 矢量运动速度量纲为 m/s，旋转运动速度量纲为 rad/s，如：3.14 代表 1s 转半圈 180°
  • x、y、z 方向通过右手定则确定：食指指向正前方（+x），中指指向正左方（+y），大拇指指向正上方（+z）
    在这里插入图片描述

在这里插入图片描述

  • 速度控制消息包：geometry_msgs/Twist
    在这里插入图片描述

  • 消息包具体定义：linear & angular
    在这里插入图片描述

1.3 控制 x 方向速度 /cmd_vel

  • 构建一个新的功能包 vel_pkg，并在 vel_pkg 功能包的 src 下新建 vel_node.cpp 文件
    $ cd catkin_ws/src
$ catkin_create_pkg vel_pkg roscpp rospy geometry_msgs

    #include <ros/ros.h>
#include <geometry_msgs/Twist.h>

int main(int argc, char *argv[]) {
    ros::init(argc, argv, "vel_node");

    ros::NodeHandle n;
    // 向 ROS 大管家 NodeHandle 申请发布话题 /cmd_vel 并拿到发布对象 vel_pub
    ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
    // 构建一个 geometry_msgs/Twist 类型的消息包 vel_msg，用来承载要发送的速度值
    geometry_msgs::Twist vel_msg;
    vel_msg.linear.x = 0.1;
    vel_msg.linear.y = 0.0;
    vel_msg.linear.z = 0.0;
    vel_msg.angular.x = 0;
    vel_msg.angular.y = 0;
    vel_msg.angular.z = 0;

    // 开启一个 while 循环，不停的使用 vel_pub 对象发送速度消息包 vel_msg
    ros::Rate r(30);
    while (ros::ok()) {
        vel_pub.publish(vel_msg);
        r.sleep();
    }
    
    return 0;
}
// 修改 CMakeLists.txt 文件此处略
// 最后到 catkin_ws 目录下编译
  • 测试运行
    $ echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
$ roslaunch wpr_simulation wpb_simple.launch
$ rosrun vel_pkg vel_node

2. 使用 Rviz 观测传感器数据

  • Gazebo 是模拟真实机器人发出传感器数据的工具（虚拟的环境），现实中不存在并被真实的实体机器人和真实环境所代替
  • Rviz 是接收传感器数据并进行显示的工具（真实的环境），只有需要观察某些数据实时变化的时候才用到
    $ cd ~/catkin_ws/
$ roslaunch wpr_simulation wpb_simple.launch
$ roslaunch wpr_simulation wpb_rviz.launch

在这里插入图片描述

3. 激光雷达 /scan

3.1 消息包类型：sensor_msgs/LaserScan

在这里插入图片描述

# 1 / scan_time = LiDAR 扫描频率 
# --noarr 防止数组消息刷屏
$ rostopic echo /scan --noarr

在这里插入图片描述

在这里插入图片描述

在这里插入图片描述

上图中超过雷达测距范围的点数值变成 INF

3.2 获取激光雷达数据节点

  • 构建一个新的功能包，包名叫做 lidar_pkg，并在软件包中新建一个节点，节点名叫做 lidar_node
    $ cd catkin_ws/src
$ catkin_create_pkg lidar_pkg roscpp rospy sensor_msgs

    #include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>

// 构建回调函数 LidarCallback()，用来接收和处理雷达数据
void LidarCallback(const sensor_msgs::LaserScan msg) {
    float fMidDist = msg.ranges[180];
    // 调用 ROS_INFO() 打印雷达检测到的前方障碍物距离
    ROS_INFO("qianfangceju ranges[180] = %f m", fMidDist);
}

int main(int argc, char *argv[]) {
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "lidar_node");

    // 向 ROS 大管家 NodeHandle 申请订阅话题 /scan，并设置回调函数为 LidarCallback()
    ros::NodeHandle n;
    ros::Subscriber lidar_sub = n.subscribe("/scan", 10, &LidarCallback);

    ros::spin();

    return 0;
}
// 修改 CMakeLists.txt 文件此处略
// 最后到 catkin_ws 目录下编译

3.3 激光雷达避障节点

在这里插入图片描述

// 在 3.2 小节基础上修改
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/Twist.h>

ros::Publisher vel_pub;  // 全局变量，保证回调函数也能使用
int nCount = 0;

void LidarCallback(const sensor_msgs::LaserScan msg) {
    float fMidDist = msg.ranges[180];
    ROS_INFO("qianfangceju ranges[180] = %f m", fMidDist);

    if(nCount > 0) {
        nCount--;
        return;
    }

    // 构建速度控制消息包 vel_cmd
    geometry_msgs::Twist vel_cmd; 
    // 根据激光雷达的测距数值，实时调整机器人运动速度，从而避开障碍物
    if (fMidDist < 1.5) {
        vel_cmd.angular.z = 0.3;
        nCount = 40;  // 确保机器人旋转的角度足够避开障碍物
    } else {
        vel_cmd.linear.x = 0.1;
    }
    vel_pub.publish(vel_cmd);
}

int main(int argc, char *argv[]) {
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "lidar_node");

    // 让大管家 NodeHandle 发布速度控制话题 /cmd_vel
    ros::NodeHandle n;
    ros::Subscriber lidar_sub = n.subscribe("/scan", 10, &LidarCallback);
    vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 10); // 发布速度控制话题 /cmd_vel

    ros::spin();

    return 0;
}

4. IMU

4.1 消息包类型 sensor_msgs/Imu

在这里插入图片描述

4.2 获取 IMU 数据节点

  • ROS 官方 IMU 话题种类标准
    在这里插入图片描述

  • 构建一个新的软件包叫做 imu_pkg，在软件包中新建一个节点叫做 imu_node.cpp

    $ cd catkin_ws/src
$ catkin_create_pkg imu_pkg roscpp rospy sensor_msgs

    #include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <tf/tf.h>

void IMUCallback(sensor_msgs::Imu msg) {
    // 先对四元数 orientation 的协方差矩阵第一个数值进行判断，如果小于0
    // 说明四元数的值不存在，具体查看sensor_msgs/Imu Message说明
    if(msg.orientation_covariance[0] < 0) {
        return;
    }
    // 使用 tf 工具将四元数转换成 tf 四元数对象
    tf::Quaternion quaternion ( 
        msg.orientation.x,
        msg.orientation.y,
        msg.orientation.z,
        msg.orientation.w
    );
    double roll, pitch, yaw; // 用来装载转换后的欧拉角结果
    // 先将 quaternion 转换成一个 tf 的 3×3 矩阵对象，然后调用 getRPY 转换成欧拉角
    tf::Matrix3x3(quaternion).getRPY(roll, pitch, yaw);
    // 将弧度值转换为角度值
    roll = roll * 180 / M_PI;
    pitch = pitch * 180 / M_PI;
    yaw = yaw * 180 / M_PI;

    // 调用 ROS_INFO() 显示转换后的欧拉角数值
    ROS_INFO("roll = %.0f pitch = %.0f yaw = %.0f", roll, pitch, yaw);
}

int main(int argc, char *argv[]) {
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "imu_node");

    // 在节点中向 ROS 大管家 NodeHandle 申请订阅话题 /imu/data，并设置回调函数为 IMUCallback()
    ros::NodeHandle n;
    ros::Subscriber imu_sub = n.subscribe("/imu/data", 10, IMUCallback);

    ros::spin();

    return 0;
}
// 修改 CMakeLists.txt 文件此处略
// 到 catkin_ws 目录下编译

4.3 IMU 航向锁定节点

在这里插入图片描述

// 在 4.2 小节基础上修改
#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include "tf/tf.h"
#include "geometry_msgs/Twist.h"

// 速度消息发布对象（全局变量）
ros::Publisher vel_pub;

// IMU 回调函数
void IMUCallback(const sensor_msgs::Imu msg) {
    // 检测消息包中四元数数据是否存在
    if(msg.orientation_covariance[0] < 0)
        return;
    // 四元数转成欧拉角
    tf::Quaternion quaternion(
        msg.orientation.x,
        msg.orientation.y,
        msg.orientation.z,
        msg.orientation.w
    );
    double roll, pitch, yaw;
    tf::Matrix3x3(quaternion).getRPY(roll, pitch, yaw);
    // 弧度换算成角度
    roll = roll*180/M_PI;
    pitch = pitch*180/M_PI;
    yaw = yaw*180/M_PI;
    ROS_INFO("滚转 = %.0f 俯仰 = %.0f 朝向 = %.0f", roll, pitch, yaw);
    // 速度消息包
    geometry_msgs::Twist vel_cmd;

    // 设定一个目标朝向角，当姿态信息中的朝向角和目标朝向角不一致时，控制机器人转向目标朝向角
    double target_yaw = 90;  // 目标朝向角
    // 计算速度
    double diff_angle = target_yaw - yaw;
    vel_cmd.angular.z = diff_angle * 0.01;
    vel_cmd.linear.x = 0.1;
    vel_pub.publish(vel_cmd);
}

int main(int argc, char **argv) {
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "imu_node"); 

    ros::NodeHandle n;
    // 订阅 IMU 的数据话题
    ros::Subscriber sub = n.subscribe("imu/data", 100, IMUCallback);
    // 让 ROS 大管家 NodeHandle 发布速度控制话题 /cmd_vel
    vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",10);
    
    ros::spin();
    return 0;
}

5. ROS 常用消息包

在这里插入图片描述

5.1 标准消息包 std_msgs

在这里插入图片描述

5.2 几何消息包 geometry_msgs

在这里插入图片描述

其中一些消息包带了 stamped 关键词，这些消息包都是多了一个 Header，也就是多了时间戳信息和坐标系（frame） ID，将空间量和时间量进行了绑定，多用于一些预测和滤波算法中

5.3 传感器消息包 sensor_msgs

在这里插入图片描述

6. 占据栅格地图

导航所使用的地图数据，就是 ROS 导航软件包里的 map_server 节点在话题 /map 中发布的消息数据，消息类型是 nav_msgs 消息包的 OccupancyGrid（占据栅格）地图，就是一种正方形小格子组成的地图，每个格子里填入一个数值，表示障碍物占据情况
在这里插入图片描述

6.1 占据栅格地图形象解释

  • 在地面上划分出一个个大小一样的正方形栅格，然后在栅格中填充不同的颜色表示占据情况，没有障碍物的栅格为白色，被占据的栅格为黑色（除去中间的墙面，剩下的黑白方格便是栅格地图）
    在这里插入图片描述

  • 栅格尺寸大小可变，越小则黑色的区域越精细，也就越接近障碍物的轮廓，但同时地图的数据量就越大，处理的时候计算量就越大，所以一般会给栅格设置一个适当的尺寸
    在这里插入图片描述

  • 栅格尺寸：指的是栅格的单边长度，体现了地图的精细程度，常被用来表示栅格地图的分辨率，ROS 中栅格地图的默认分辨率为 0.05m，也就是每个栅格的边长为5cm
    在这里插入图片描述

  • 数字表示栅格
    在这里插入图片描述

  • 将栅格一行一行拼接起来变成一个数组，有了这个数组，再加上栅格的行、列数等信息，就能通过具体的数值将这个栅格地图描述清楚了
    在这里插入图片描述

6.2 占据栅格地图消息类型

在这里插入图片描述

在这里插入图片描述

6.3 占据栅格地图发布节点

在这里插入图片描述

  • 构建一个软件包 map_pkg，依赖项里加上 nav_msgs
    $ cd ~/catkin_ws/src
$ catkin_create_pkg map_pkg roscpp rospy nav_msgs
$ cd ..
$ code .
  • 在 map_pkg 里创建一个节点 map_pub_node
    #include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>

int main(int argc, char *argv[]) {
    ros::init(argc, argv, "map_pub_node");
    // 通过大管家发布话题 /map，消息类型为 nav_msgs::OccupancyGrid
    ros::NodeHandle n;
    ros::Publisher pub = n.advertise<nav_msgs::OccupancyGrid>("/map", 10);

    ros::Rate r(1);

    while(ros::ok()) {
        // 构建一个 nav_msgs::OccupancyGrid 地图消息包，并对其进行赋值
        nav_msgs::OccupancyGrid msg;

        msg.header.frame_id = "map";
        msg.header.stamp = ros::Time::now();

        msg.info.origin.position.x = 0;
        msg.info.origin.position.y = 0;
        msg.info.resolution = 1.0;
        msg.info.width = 4;
        msg.info.height = 2;

        msg.data.resize(4 * 2);
        msg.data[0] = 100;
        msg.data[1] = 100;
        msg.data[2] = 0;
        msg.data[3] = -1;

        pub.publish(msg);
        r.sleep();
    }
 
    return 0;
}
// 修改 CMakeLists.txt 文件此处略
// 到 catkin_ws 目录下编译
  • 启动 Rviz，订阅话题 /map，显示地图
    $ roscore
$ rosrun map_pkg map_pub_node
$ rviz

在这里插入图片描述

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：/a/364876.html

如若内容造成侵权/违法违规/事实不符，请联系我们进行投诉反馈qq邮箱809451989@qq.com，一经查实，立即删除！

相关文章

Elasticsearch-内存结构

Elasticsearch-内存结构

ElasticSearch的内存从大的结构可以分堆内存&#xff08;On Heap&#xff09;和堆外内存&#xff08;Off Heap&#xff09;。Off Heap部分由Lucene进行管理。On Heap部分存在可GC部分和不可GC部分&#xff0c;可GC部分通过GC回收垃圾对象&#xff0c;从而释放内存。不可GC部分不…
阅读更多...
【项目日记(七)】第三层: 页缓存的具体实现(上)

【项目日记(七)】第三层: 页缓存的具体实现(上)

&#x1f493;博主CSDN主页:杭电码农-NEO&#x1f493;   ⏩专栏分类:项目日记-高并发内存池⏪   &#x1f69a;代码仓库:NEO的学习日记&#x1f69a;   &#x1f339;关注我&#x1faf5;带你做项目   &#x1f51d;&#x1f51d; 开发环境: Visual Studio 2022 项目日…
阅读更多...
three.js CSS3DRenderer、CSS3DObject渲染HTML标签

three.js CSS3DRenderer、CSS3DObject渲染HTML标签

有空的老铁关注一下我的抖音&#xff1a; 效果&#xff1a; <template><div><el-container><el-main><div class"box-card-left"><div id"threejs" style"border: 1px solid red;position: relative;"><…
阅读更多...
react 之 useInperativeHandle

react 之 useInperativeHandle

useInperativeHandle是通过ref暴露子组件中的方法 1.场景说明-直接调用子组件内部的方法 import { forwardRef, useImperativeHandle, useRef } from "react"// 子组件const Son forwardRef((props, ref) > {// 实现聚焦逻辑const inputRef useRef(null)const …
阅读更多...
【知识点】Java常用

【知识点】Java常用

文章目录 基础基础数据类型内部类Java IOIO多路复用重要概念 Channel **通道**重要概念 Buffer **数据缓存区**重要概念 Selector **选择器** 关键字final 元注解常用接口异常处理ErrorException JVM与虚拟机JVM内存模型本地方法栈虚拟机栈 Stack堆 Heap方法区 Method Area (JD…
阅读更多...
npm安装报错，出现.staging

npm安装报错，出现.staging

问题场景&#xff1a;同事发了一个本地的安装包&#xff0c;拿到了没仔细看&#xff0c;直接npm install&#xff0c;没有发现根目录下的package-lock.json。然后就发现安装一直不成功&#xff0c;还会卡主。并且在node_modules文件夹下还会出现.staging文件夹&#xff0c;正常…
阅读更多...
计算机网络实验六

计算机网络实验六

目录 实验6 静态路由与RIP协议配置 1、实验目的 2、实验设备 &#xff08;1&#xff09;内容一&#xff1a;&#xff08;静态路由配置&#xff09; &#xff08;2&#xff09;内容二&#xff1a;&#xff08;RIP协议配置&#xff09; 3、网络拓扑及IP地址分配 &#xff…
阅读更多...
3593 蓝桥杯 查找最大元素 简单

3593 蓝桥杯 查找最大元素 简单

3593 蓝桥杯 查找最大元素 简单 // C风格解法1&#xff0c;通过率100%&#xff0c;多组数据处理样式//str "abcdefgfedcba" //abcdefg(max)fedcba//str "xxxxx" //x(max)x(max)x(max)x(max)x(max)#include<bits/stdc.h>const int N 1e2 10;char …
阅读更多...
无法在 word 中登录 Grammarly

无法在 word 中登录 Grammarly

目录 1. 情况描述 2. 解决方法 3. 原因分析 1. 情况描述 在浏览器中可以登录 Grammarly&#xff0c;但是在 word 中登录失败&#xff0c;大致如下图所示&#xff1a; 我自己没有截图&#xff0c;这是网上别人的图&#xff0c;但差不多都长这个样子。 2. 解决方法 我点击了…
阅读更多...
量化交易学习4（投资组合基本认识）

量化交易学习4（投资组合基本认识）

1 如何衡量投资组合的收益率 1.1 投资组合收益率的计算方法 1.2 投资组合的绝对收益率和相对收益率 2 如何衡量投资组合的风险 2.1 风险的定义 风险是指在未来可能发生的不确定性事件所带来的潜在损失。 在投资领域中&#xff0c;风险通常指投资所面临的不确定性和潜在的损失…
阅读更多...
react native错误记录

react native错误记录

第一次运行到安卓失败 Could not find implementation class com.facebook.react.ReactRootProjectPlugin for plugin com.facebook.react.rootproject specified in jar:file:/D:/Android_Studio_Data/.gradle/caches/jars-9/o_3a1fd35320f05989063e7069031b710f/react-nativ…
阅读更多...
钉钉群机器人-发送群消息

钉钉群机器人-发送群消息

1、钉钉群创建机器人 添加完成后&#xff0c;要记住 Webhook 路径&#xff1b; 2、机器人接入文档网址 自定义机器人接入 - 钉钉开放平台 3、JAVA代码 import com.dingtalk.api.DefaultDingTalkClient; import com.dingtalk.api.DingTalkClient; import com.dingtalk.api.re…
阅读更多...
JVM工作原理与实战(三十二)：GC调优

JVM工作原理与实战(三十二)：GC调优

专栏导航 JVM工作原理与实战 RabbitMQ入门指南 从零开始了解大数据 目录 专栏导航 前言 一、GC调优 二、GC调优的核心指标 总结 前言 JVM作为Java程序的运行环境&#xff0c;其负责解释和执行字节码&#xff0c;管理内存&#xff0c;确保安全&#xff0c;支持多线程和提供…
阅读更多...
idea 中 tomcat 乱码问题修复

idea 中 tomcat 乱码问题修复

之前是修改 Tomcat 目录下 conf/logging.properties 的配置&#xff0c;将 UTF-8 修改为 GBK&#xff0c;现在发现不用这样修改了。只需要修改 IDEA 中 Tomcat 的配置就可以了。 修改IDEA中Tomcat的配置&#xff1a;添加-Dfile.encodingUTF-8 本文结束
阅读更多...
网络协议与攻击模拟_13缓存DNS与DNS报文

网络协议与攻击模拟_13缓存DNS与DNS报文

一、缓存DNS服务器 1、引入缓存DNS 工作可能会遇到这样的场景&#xff0c;内网部署有一台缓存DNS服务器&#xff0c;这台服务器是由我们自己搭建的。为什么放在内网呢&#xff1f;因为我内网中的机器请求内网返回会比较快。第一次会慢点&#xff0c;因为有一个转发的过程&am…
阅读更多...
arcgis javascript api4.x加载非公开或者私有的arcgis地图服务

arcgis javascript api4.x加载非公开或者私有的arcgis地图服务

需求&#xff1a; 加载arcgis没有公开或者私有的地图服务&#xff0c;同时还想实现加载时不弹出登录窗口 提示&#xff1a;​ 下述是针对独立的arcgis server&#xff0c;没有portal的应用场景&#xff1b; 如果有portal可以参考链接&#xff1a;https://mp.weixin.qq.com/s/W…
阅读更多...
供应商规模成倍增长，医疗器械制造商如何让采购效率更进一步｜创新场景50...

供应商规模成倍增长，医疗器械制造商如何让采购效率更进一步｜创新场景50...

ITValue 随着企业的快速发展&#xff0c;采购供应链网络日益庞大&#xff0c;企业在供应商管理上面临着管理体系分散、风险难以管控&#xff0c;采购过程环节多等问题&#xff0c;供应商内外协同亟待解决。 作者&#xff5c;秦聪慧 专题&#xff5c;创新场景50 ITValue 制造企业…
阅读更多...
SpringMVC处理ajax请求之@ResponseBody注解，将后端数据响应到浏览器

SpringMVC处理ajax请求之@ResponseBody注解，将后端数据响应到浏览器

上一篇文章讲到SpringMVC处理ajax请求用到的RequestBody注解SpringMVC处理ajax请求&#xff08;RequestBody注解&#xff09;&#xff0c;ajax向后端传递的数据格式详解-CSDN博客&#xff0c;这个注解帮我们解决了如何将客户端的数据通过json数据传递到服务器&#xff0c;简单说…
阅读更多...
SpringCloud-搭建Eureka服务模块

SpringCloud-搭建Eureka服务模块

在构建分布式微服务体系中&#xff0c;搭建Eureka服务模块是实现服务注册与发现的关键一步。Spring Cloud作为领先的微服务框架&#xff0c;通过Eureka为我们提供了高效的服务治理能力。本文将深入探讨如何使用Spring Cloud&#xff0c;逐步引导读者完成Eureka服务模块的搭建。…
阅读更多...
docker手动迁移镜像

docker手动迁移镜像

1&#xff0c;将镜像保存在本地 docker save 镜像名称:版本号 > 镜像名称.tar 2&#xff0c;下载镜像 通过 ftp 工具或者命令&#xff0c;下载到本地 3&#xff0c;上传镜像到目标 docker 所在服务器 4&#xff0c;导入镜像 docker load < 镜像名称.tar
阅读更多...
最新文章

Copyright @ 2022~2023