Viobot定位用于导航

news2024/11/22 15:16:48

注:此教程以轮式机器人作为一个样例,具体的应用还需要用户自己去做更深入的开发。当然,着并不是唯一的方法,有更好的方法也欢迎大家讲一下自己的思路,有什么说错的地方也欢迎大家批评指正。

路径规划部分我们选用轮式机器人比较常用的move_base。

整个架构就是Viobot使用stereo2算法提供机器人的当前位姿,输入到move_base作为规划起点,再给定目标终点,move_base会输出一个/cmd_vel的话题使机器人运动起来。至于/cmd_vel到控制机器人底盘运动(也就是base controller这部分)的实现就要用户自己去实现了,我们先默认都已经可以实现base controller了。

下面是我整理的一张整体的框架图

接下来就是例程的粗略讲解

1.viobot输出信息处理

接收stereo2的位姿和点云,使用TF转换,构建TF树。

1)接收消息,注册回调函数

std::string point_clound_topic;
//点云话题配置接口,可以通过launch文件配置点云输入,默认为TOF
nh_private.param<std::string>("point_clound_topic", point_clound_topic, "/pr_loop/tof_points");

sub_odom = nh.subscribe("/pr_loop/odometry_rect", 50, &VioOdomNodelet::loop_pose_callback, this);
sub_car_odom = nh.subscribe("/odom", 50, &VioOdomNodelet::car_odom_callback, this);
sub_pointcloud = nh.subscribe(point_clound_topic, 50, &VioOdomNodelet::loop_pointclound_callback, this);

2)回调函数处理位姿信息

自己去维护了一个从base_linkvio_odom的动态TF变换

void VioOdomNodelet::loop_pose_callback(const nav_msgs::OdometryPtr &msg){
    static tf::TransformBroadcaster odom_broadcaster;//定义tf 对象
    geometry_msgs::TransformStamped odom_trans;//定义tf发布时需要的类型消息
    nav_msgs::Odometry odom;

    odom.header.stamp = msg->header.stamp;
    odom.header.frame_id = "vio_odom";
    odom.child_frame_id = "base_link";
    Eigen::Vector3d position_in_vio(msg->pose.pose.position.x, msg->pose.pose.position.y,msg->pose.pose.position.z);
    
    tf::Quaternion original_quat(msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w);
    
    //选转到跟base_link方向一致
    tf::Quaternion quat_rotate;
    quat_rotate.setRPY(0, -M_PI/2, M_PI/2);
    // quat_rotate.setRPY(0,M_PI/2, -M_PI/2);

    // Apply the rotations
    tf::Quaternion final_quat = original_quat * quat_rotate;
    
    // Convert the final quaternion back to a geometry_msgs Quaternion
    geometry_msgs::Quaternion geo_q;
    tf::quaternionTFToMsg(final_quat, geo_q);

    if(height_charge){//一个将Z轴归零的策略,可根据自己的实际情况配置
        frame_z = msg->pose.pose.position.z;
        position_in_vio.z() = 0;
        odom.pose.pose.position.z = 0;//z轴归零
        //这里为了把位姿朝向规整到水平面
        vio_quat = geo_q;
        // printf("--------------------\npose:\nx:%lf\ny:%lf\nz:%lf\nquat:\nx:%lf\ny:%lf\nz:%lf\nw:%lf\n",msg->pose.pose.position.x,msg->pose.pose.position.y,
        //                                                             msg->pose.pose.position.z,msg->pose.pose.orientation.x,msg->pose.pose.orientation.y,
        //                                                             msg->pose.pose.orientation.z,msg->pose.pose.orientation.w);
        double yaw = tf::getYaw(vio_quat);
        // geometry_msgs::Quaternion geo_q1 = tf::createQuaternionMsgFromYaw(yaw + M_PI / 2);
        geometry_msgs::Quaternion geo_q1 = tf::createQuaternionMsgFromYaw(yaw);

        odom.pose.pose.orientation = geo_q1;
    }
    else{
        odom.pose.pose.position.z = msg->pose.pose.position.z;
        odom.pose.pose.orientation = geo_q; 
    } 

    position_in_vio = position_in_vio + t_vio_base_link;

    // Coordinate transformation from VIO to base_link
    // odom.pose.pose.position.x = position_in_vio.x();
    // odom.pose.pose.position.y = position_in_vio.y();
    // odom.pose.pose.position.z = position_in_vio.z();
    odom.pose.pose.position.x = msg->pose.pose.position.x;
    odom.pose.pose.position.y = msg->pose.pose.position.y;

    //这里odom的速度可选stereo2输出的速度,也可以选择底盘论速计的速度
    // odom.twist.twist.linear.x = msg->twist.twist.linear.x;
    // odom.twist.twist.linear.y = msg->twist.twist.linear.y;
    odom.twist.twist.linear.x = linear_vx;
    odom.twist.twist.linear.y = linear_vy;
    odom.twist.twist.linear.z = 0.0;
    odom.twist.twist.angular.x = 0.0;
    odom.twist.twist.angular.y = 0.0;
    odom.twist.twist.angular.z = angular_vz;
    // odom.twist.twist.angular.z = msg->twist.twist.angular.z;

    odom_trans.header.stamp = msg->header.stamp;
    odom_trans.header.frame_id = "vio_odom";
    odom_trans.child_frame_id = "base_link";
    odom_trans.transform.translation.x = position_in_vio.x();//x坐标
    odom_trans.transform.translation.y = position_in_vio.y();//y坐标
    odom_trans.transform.translation.z = position_in_vio.z();//z坐标        
    odom_trans.transform.rotation = odom.pose.pose.orientation;//偏航角
    odom_broadcaster.sendTransform(odom_trans);

    //区分静止和运动时的协方差
    if(msg->twist.twist.linear.y == 0 && msg->twist.twist.linear.x == 0 && msg->twist.twist.angular.z == 0){
        memcpy(&odom.pose.covariance, odom_pose_covariance2, sizeof(odom_pose_covariance2));
        memcpy(&odom.twist.covariance, odom_twist_covariance2, sizeof(odom_twist_covariance2));
    }
    else{
        memcpy(&odom.pose.covariance, odom_pose_covariance, sizeof(odom_pose_covariance));
        memcpy(&odom.twist.covariance, odom_twist_covariance, sizeof(odom_twist_covariance));
    }
    pub_odom.publish(odom);
}

3)回调函数处理点云信息

把点云划到vio_odom上面,它会根据上面的base_linkvio_odom的动态TF变换变换

void VioOdomNodelet::loop_pointclound_callback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg_in){
    sensor_msgs::PointCloud2 cloud_msg_out = *cloud_msg_in;

    // Set the frame_id of the output pointcloud to base_link
    cloud_msg_out.header.frame_id = "vio_odom";

    if(height_charge){
        Eigen::Quaterniond q(vio_quat.w, vio_quat.x, vio_quat.y, vio_quat.z);
        Eigen::Matrix3d rotation_matrix = q.normalized().toRotationMatrix();

        //将Eigen::Matrix3d 手动转换为 tf::Matrix3x3
        tf::Matrix3x3 tf_rotation_matrix(rotation_matrix(0, 0), rotation_matrix(0, 1), rotation_matrix(0, 2),
                                        rotation_matrix(1, 0), rotation_matrix(1, 1), rotation_matrix(1, 2),
                                        rotation_matrix(2, 0), rotation_matrix(2, 1), rotation_matrix(2, 2));

        double yaw, pitch, roll;
        tf_rotation_matrix.getRPY(roll, pitch, yaw);

        //创建pitch和roll的逆旋转
        Eigen::Matrix3d pitch_matrix_inv = Eigen::AngleAxisd(-pitch, Eigen::Vector3d::UnitY()).toRotationMatrix();
        Eigen::Matrix3d roll_matrix_inv = Eigen::AngleAxisd(-roll, Eigen::Vector3d::UnitX()).toRotationMatrix();

        Eigen::Matrix3d final_rotation_matrix;
        // 应用逆旋转到原始四元数上
        if(inv){
            final_rotation_matrix = pitch_matrix_inv * roll_matrix_inv;
        }
        else{
            final_rotation_matrix << 1,0,0,0,1,0,0,0,1;
        }

        sensor_msgs::PointCloud pointcl1;
        sensor_msgs::convertPointCloud2ToPointCloud(cloud_msg_out,pointcl1);
        for(int i = 0;i < pointcl1.points.size();i++){
            Eigen::Vector3d point_eigen(pointcl1.points[i].x, pointcl1.points[i].y, pointcl1.points[i].z);
            point_eigen = final_rotation_matrix * point_eigen;
            pointcl1.points[i].x = point_eigen.x();
            pointcl1.points[i].y = point_eigen.y();
            pointcl1.points[i].z = point_eigen.z() - frame_z;
            // printf("frame_z = %lf\n",frame_z);
        }
        sensor_msgs::convertPointCloudToPointCloud2(pointcl1,cloud_msg_out);  
    }
    pub_pointcloud.publish(cloud_msg_out);
}

2.pointcloud转laserscan

其实就是根据原有的开源代码做了一下修改,原有的代码配置了use_inf为false时,当点云的距离大于range_max就不显示了,这样会造成一些刷新上面的困难,所以再输出/scan话题之前加了1.5m的距离,让/scan话题初始就是一个range_max+1.5的扇形,根据障碍物来刷新。

3.move_base配置

1)launch文件

主要是启动map_server加载地图文件和启动move_base并加载了配置文件。
<launch>
    <!-- Run the map server -->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find nav_robot)/maps/map_office.yaml"/>
    <!-- <node name="map_server" pkg="map_server" type="map_server" args="$(find nav_robot)/maps/blank_map.yaml"/> -->


    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
        <rosparam file="$(find nav_robot)/config/mark_1/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find nav_robot)/config/mark_1/costmap_common_params.yaml" command="load" ns="local_costmap" />
        <rosparam file="$(find nav_robot)/config/mark_1/local_costmap_params.yaml" command="load" />
        <rosparam file="$(find nav_robot)/config/mark_1/global_costmap_params.yaml" command="load" />
        <rosparam file="$(find nav_robot)/config/mark_1/move_base_teb_params.yaml" command="load" />
        <rosparam file="$(find nav_robot)/config/mark_1/teb_local_planner_params.yaml" command="load" />
    </node>
    <!-- <node name="rviz" pkg="rviz" type="rviz" args="-d $(find nav_robot)/rviz_cfg/nav_test.rviz" /> -->

</launch>

2)地图文件

这个需要用户把自己的场景先建里一个地图先验,可以是使用雷达等设备,也可以使用viobot(这个建图要单独开一篇来讲)。

3)move_base配置文件

每个文件的参数都有详细注释,用户可以自行查看参数的意义和选择配置。

在文件costmap_common_params.yaml有一part需要重点说明的:

障碍物层输入是/scan ,不使用pointcloud是因为move_base底层代码逻辑,避障是使用世界系的点云的,其实避障使用body系点云应该是更合理的,所以我们把一定高度范围的pointcloud转成了/scan;其次是obstacle_range和raytrace_range两个参数,它会跟踪raytrace_range范围内的障碍物,但是只有在obstacle_range范围内的障碍物点才会被加到代价地图,所以我们前面在转/scan的出话题的时候加了那个1.5m就是为了把大部分点定到obstacle_range和raytrace_range中间,使得障碍物能够快速被刷新。

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

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈,一经查实,立即删除!

相关文章

什么是量化交易接口?(股票下单接口)特点(一)

股市领域里的量化交易接口是一种用于与金融市场进行交互的编程接口&#xff0c;它允许开发者通过计算机程序自动执行交易策略。量化交易接口通常提供以下功能&#xff1a; 1. 实时市场数据获取&#xff1a;量化交易接口通常可以提供实时的市场行情数据&#xff0c;包括股票、期…

el-upload 上传附件

案例: <template><div><el-drawertitle"附件信息"v-model"drawer"direction"rtl":before-close"handleClose"size"35%":close-on-click-modal"false":close-on-press-escape"false">…

2. 配置版本

2.1 安装 Python 版本 ① 首先打开 Anaconda 的 Prompt 终端&#xff0c;输入 jupyter kernelspec list&#xff0c;查看已经安装的 python 版本。如下图所示&#xff0c;只有 Anaconda 自带的 python3。 ② 输入 conda create --name py3.6.3 python3.6.3 命令&#xff0c;输…

rman备份数据文件和归档指定不同的filesperset

客户说带库上抓小文件会导致带库紧张。 大部分的备份软件都会用filesperset参数来做备份&#xff0c;这样恢复速度快&#xff0c;而且也有利于带库重复删除。 客户用的plus archivelog的形式&#xff0c;一条命令的做全备&#xff0c;现在就有filesperset不区分dbf和arc的情况…

Java之API详解之Objects类的详细解析

5 Objects类 5.1 概述 tips&#xff1a;了解内容 查看API文档&#xff0c;我们可以看到API文档中关于Objects类的定义如下&#xff1a; Objects类所在包是在java.util包下&#xff0c;因此在使用的时候需要进行导包。并且Objects类是被final修饰的&#xff0c;因此该类不能被继…

跨境电商独立站如何实现全球开店,获得更多流量?

对于独立站卖家来说&#xff0c;针对一个国家搭建一个站点、运营&#xff0c;就已经要花上不少力气了。更别说想要在多个市场售卖了&#xff0c;每个国家不同的货币、语言、定价、付款方式等等就已经够让人头大。 研究显示&#xff0c;40%的人不会从其他语言的网站上购买产品。…

Windows系统配置jar包自启动

目录 一、下载 WinSW.NET4.exe二、创建windows的目录结构三、编写xml配置文件四、检查环境4.1 是否安装jdk4.2 是否存在.net framework4的服务 五、安装 一、下载 WinSW.NET4.exe https://github.com/winsw/winsw/releases 下载完毕改名为jar名&#xff0c;也就是 项目.exe64位…

QT使用QImage做图片切割

#include "mainwindow.h" #include "ui_mainwindow.h" #include <QFileDialog> #include <QDebug>MainWindow::MainWindow(QWidget *parent) :QMainWindow(parent),ui(new Ui::MainWindow) {ui->setupUi(this);// 选择本地图片文件QString …

了解java中的通配符“?“

目录 通配符的作用 先看一段代码 用通配符"?"后,代码变化 结论 通配符上界 通配符下界 对通配符上下界的注释理解及其练习代码 简记: ? 用于在泛型的使用&#xff0c;即为通配符. 在Java中&#xff0c;通配符&#xff08;wildcard&#xff09;主要用于泛型…

11.物联网lwip,网卡原理

一。LWIP协议栈内存管理 1.LWIP内存管理方案 &#xff08;1&#xff09;堆heap 1.灰色为已使用内存 2.黑色为未使用内存 3.紫色为使用后内存 按照某种算法&#xff0c;把数据放在内存块中 &#xff08;2&#xff09;池pool 设置内存池&#xff0c;设置成大小相同的内存块。 2…

想报考pmp,一定得经过培训机构吗?

想要申请PMP考试&#xff0c;必须具备35个学时的PMBOK项目管理学习或培训经验。这是报考的首要条件。 所有符合条件的机构必须具备“PMI”和“中国国际基金会人才交流中心”的授权资质之一即可 首先&#xff0c;在进行英文报名时&#xff0c;需要提供并填写35学时的培训经历。…

WordPress(2)主题安装-菜单-菜单图标

提示:文章写完后,目录可以自动生成,如何生成可参考右边的帮助文档 文章目录 前言一、上传你的主题文件启用我的是 LoliMeow二、主题设置1.小工具中设置即可在网站右侧显示2.在文章中新建分类用做菜单3.也可在文字的前面添加图标[图标库](http://fa.fangguokeji.cn/)前言 提…

Visual Studio 2022的MFC框架——AfxWinMain全局对象和InitInstance函数

我是荔园微风&#xff0c;作为一名在IT界整整25年的老兵&#xff0c;今天我们来重新审视一下Visual Studio 2022下开发工具的MFC框架知识。 在看这篇帖子前&#xff0c;请先看我的另一篇帖子《Visual Studio 2022的MFC框架——应用程序向导》。 当程序调用了CWinApp类的构造…

shell常用脚本

1、Java项目重启脚本 #!/bin/bash# 定义一个函数来结束进程和启动新进程 function restart(){JAR_NAME$1 # jar包LOG_NAME$2 # 日志JVM_ARGS$3 # jvm 参数PID$(ps -ef | grep java | grep ${JAR_NAME} | awk { print $2 })if [[ ! -z "$PID" ]]; thenecho "…

LeetCode(力扣)700. 二叉搜索树中的搜索Python

LeetCode20. 有效的括号 题目链接代码 题目链接 https://leetcode.cn/problems/search-in-a-binary-search-tree/ 代码 递归法 # Definition for a binary tree node. # class TreeNode: # def __init__(self, val0, leftNone, rightNone): # self.val val #…

进度猫:如何做好项目进度管理?有几点建议

项目目进度管理是在项目实施过程中&#xff0c;对各阶段的进展和项目最终完成的期限进行的管理。目的是保证项目能在满足其时间的约束条件的前提下实现项目的总体目标。 因此为了保证项目能够按时完成&#xff0c;需要制订详尽的可操作性的项目进度管理计划&#xff0c;才可以…

MySQL数据库备份及恢复

数据备份的重要性 1、备份的主要目的是灾难恢复 2、在生产环境中&#xff0c;数据的安全性至关重要 3、任何数据的丢失都可能产生严重的后果 4、造成数据丢失的原因 备份类型(重点) 1、物理备份 数据库备份可以分为物理备份和逻辑备份。物理备份是对数据库操作系统的物…

【App端】uni-app使用百度地图api和echarts省市地图下钻

目录 前言方案一&#xff1a;echarts百度地图获取百度地图AK安装echarts和引入百度地图api完整使用代码 方案二&#xff1a;echarts地图和柱状图变形动画实现思路完整使用代码 方案三&#xff1a;中国地图和各省市地图下钻实现思路完整使用代码 前言 近期的app项目中想加一个功…

QGIS-计算几何内部点(一定在几何内)

在提取几何图像的中心点相关的X Y时&#xff0c;我们往往希望提取的点在几何内部&#xff0c;因为对于不规则图形而言&#xff0c;特别是凹几何&#xff0c;提取的点可能在图形外&#xff0c;QGIS中提供了相关的函数用于提取点中心点&#xff1a; 打开图形的属性列表&#xff…

Springboot2.0 上传图片 jar包导出启动(第二章)

目录 一&#xff0c;目录文件结构讲解二&#xff0c;文件上传实战三&#xff0c;jar包方式运行web项目的文件上传和访问处理&#xff08;核心知识&#xff09;最后 一&#xff0c;目录文件结构讲解 简介&#xff1a;讲解SpringBoot目录文件结构和官方推荐的目录规范 1、目录讲解…