ROS目标跟随(路径规划、slam、定位、雷达)——接上文,改善跟随的位置

确保进行跟随的小车始终在身后

    • 最终效果
    • 代码改进

最终效果

ROS目标跟随改进版

代码改进

这里给出上一篇博客的链接:https://blog.csdn.net/m0_71523511/article/details/135610191

使用上一篇的launch文件创建机器人时,ros会自动创建一个坐标系相对关系发布节点:/tf,通过观察此节点发布的信息,我发现可以通过监听里程计坐标系:/robot2/odom和小车底盘坐标系/robot2/base_footprint的坐标变换关系来实现想要的效果。直接上代码:

#include"ros/ros.h"
#include"tf2_ros/transform_listener.h"
#include"tf2_ros/buffer.h"
#include"nav_msgs/Odometry.h"
#include"geometry_msgs/PoseStamped.h"
#include"tf2_geometry_msgs/tf2_geometry_msgs.h"
#include"geometry_msgs/TransformStamped.h"
#include"geometry_msgs/Twist.h"struct EulerAngles {double roll, pitch, yaw;
};int main(int argc, char *argv[])
{ros::init(argc,argv,"test03_control_turtle2");ros::NodeHandle nh;tf2_ros::Buffer buffer;tf2_ros::TransformListener sub(buffer);ros::Publisher pub = nh.advertise<geometry_msgs::PoseStamped>("/robot1/move_base_simple/goal",100);ros::Rate rate(0.5);while (ros::ok()){try{geometry_msgs::TransformStamped son1toson2 = buffer.lookupTransform("map","base_link2",ros::Time(0));geometry_msgs::TransformStamped son1toson3 = buffer.lookupTransform("robot2/odom","robot2/base_footprint",ros::Time(0));ROS_INFO("son1xiangduiyuson2 : pianyiliang(%.2f,%.2f)",son1toson2.transform.translation.x,son1toson2.transform.translation.y);geometry_msgs::PoseStamped odometry;odometry.header.frame_id = "robot1/odom";//下面这四行就是四元数转为偏航角的公式EulerAngles angles;double sinr_cosp = 2 * (son1toson3.transform.rotation.w * son1toson3.transform.rotation.z + son1toson3.transform.rotation.y * son1toson3.transform.rotation.x);double cosr_cosp = 1 - 2 * (son1toson3.transform.rotation.z * son1toson3.transform.rotation.z + son1toson3.transform.rotation.y * son1toson3.transform.rotation.y);angles.yaw = std::atan2(sinr_cosp, cosr_cosp);if((angles.yaw >0) && (angles.yaw < 1.5)){odometry.pose.position.x = (son1toson2.transform.translation.x-0.3);odometry.pose.position.y = (son1toson2.transform.translation.y-0.3);}else if((angles.yaw >1.5) && (angles.yaw < 3)){odometry.pose.position.x = (son1toson2.transform.translation.x+0.3);odometry.pose.position.y = (son1toson2.transform.translation.y-0.3);}else if((angles.yaw >-1.5) && (angles.yaw < 0)){odometry.pose.position.x = (son1toson2.transform.translation.x-0.3);odometry.pose.position.y = (son1toson2.transform.translation.y+0.3);}else if((angles.yaw >-3) && (angles.yaw < -1.5)){odometry.pose.position.x = (son1toson2.transform.translation.x+0.3);odometry.pose.position.y = (son1toson2.transform.translation.y+0.3);}else{odometry.pose.position.x = (son1toson2.transform.translation.x-0.3);odometry.pose.position.y = (son1toson2.transform.translation.y-0.3);}odometry.pose.orientation.w = 1.0;ROS_INFO("stop_here = (%.2f,%.2f)",odometry.pose.position.x,odometry.pose.position.y);ROS_INFO("siyuanshu = (%.10f)",angles.yaw);pub.publish(odometry);}catch(const std::exception& e){ROS_ERROR("Exception: %s", e.what());}rate.sleep();ros::spinOnce();}return 0;
}

通过代码不难看出,这里的实现就是将检测到的坐标系变换中的四元数转为了偏航角,通过偏航角就可以知道小车此时的朝向,通过测试最终写成四段if多条件判断语句,小车朝向不同,那么第二辆进行跟随的小车它的落点也就不同,这样就可以实现进行跟随的小车始终跟随在前面一辆下车的后面。
如果想要更精细的落点可以增加判断的语句。

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

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

相关文章

阿里云服务器租用价格2024年新版活动报价和租用收费标准

2024年最新阿里云服务器租用费用优惠价格表&#xff0c;轻量2核2G3M带宽轻量服务器一年61元&#xff0c;折合5元1个月&#xff0c;新老用户同享99元一年服务器&#xff0c;2核4G5M服务器ECS优惠价199元一年&#xff0c;2核4G4M轻量服务器165元一年&#xff0c;2核4G服务器30元3…

《剑指Offer》笔记题解思路技巧优化 Java版本——新版leetcode_Part_3

《剑指Offer》笔记&题解&思路&技巧&优化_Part_3 &#x1f60d;&#x1f60d;&#x1f60d; 相知&#x1f64c;&#x1f64c;&#x1f64c; 相识&#x1f622;&#x1f622;&#x1f622; 开始刷题1. LCR 138. 有效数字——表示数值的字符串2. LCR 139. 训练计划…

MATLAB导出图程序

本文将以代码的形式快速介绍MATLAB导出图到Paper 1 从simulation导出数 2 与simulation同源文件夹下创建导图m文件 代码如下&#xff1a; % 实验后的数据处理用 M-文件 % clear all % 清空工作空间 % close all      % 关闭所有图形窗口 % load adp.mat …

算法中关于数学的题目练习

算法中关于数学的题目练习 1、买不到的数目题目信息思路题解 2、蚂蚁感冒题目信息思路题解 3、饮料换购题目信息思路题解 1、买不到的数目 题目信息 思路 数学结论&#xff08;证明略&#xff09;&#xff1a; p、q为正整数且互质&#xff0c;不能由p、q凑出来的最大的数为(p…

java生态环境评价Myeclipse开发mysql数据库web结构java编程计算机网页项目

一、源码特点 java 生态环境评价管理系统是一套完善的java web信息管理系统&#xff0c;对理解JSP java编程开发语言有帮助&#xff0c;系统具有完整的源代码和数据库&#xff0c;系统主要采用B/S模式开发。开发环境为 TOMCAT7.0,Myeclipse8.5开发&#xff0c;数据库为Mysq…

RK3399平台开发系列讲解(USB篇)USB控制传输方式介绍

🚀返回专栏总目录 文章目录 一、控制传输详解二、Setup阶段和Data阶段三、Setup 事务格式沉淀、分享、成长,让自己和他人都能有所收获!😄 📢USB控制传输是USB通信中的一种基本传输类型,用于控制USB设备的配置和操作。它由 Setup 阶段和 Data 阶段组成,可用于发送命令…

SIFT 2D/3D检测原理

一、SIFT 2D 二、SIFT 3D SIFT 3D关键点检测以及SAC-IA粗配准-CSDN博客

轻资产上班族副业,steam搬砖项目新手1周出结果

作为一个80后社畜&#xff0c;我打从上大学就喜欢倒腾各种赚钱的事情&#xff0c;不管操作网络上还是现实中的任何项目我都会亲自去摸索其中的赚钱原理&#xff0c;实践才能出真知。steam搬砖项目是我实操了3年多的项目&#xff0c;这个steam搬砖项目长期稳定又老牌阳光&#x…

12.18 校招 实习 内推 面经

绿*泡*泡VX&#xff1a; neituijunsir 交流裙 &#xff0c;内推/实习/校招汇总表格 1、实习&校招 | 寒序科技 2024实习/校招/合伙人招募启动 实习&校招 | 寒序科技 2024实习/校招/合伙人招募启动 2、校招 | 北京海纳川汽车部件2024届校园招聘火热进行中 校招 | 北…

突破编程_C++_面试(高级特性(2))

面试题8&#xff1a;什么是线程局部存储的技术 线程局部存储&#xff08;Thread Local Storage&#xff0c;TLS&#xff09;是一种存储变量的方法&#xff0c;这些变量在其所在的线程内是全局可访问的&#xff0c;但不能被其他线程访问&#xff0c;从而实现了变量的线程独立性…

深入理解lambda表达式

深入理解ASP.NET Core中的中间件和Lambda表达式 var builder WebApplication.CreateBuilder(args); var app builder.Build(); app.Use(async (context, next) > { // Add code before request. await next(context);// Add code after request.}); 这段C#代码是用于设…

固定表结构与可自定义表结构

整个平台的表结构分为两种&#xff1a;固定的和可自定义的。 固定表结构适合于比较固定的信息对象&#xff0c;例如在平台的客户关系管理模块中&#xff0c;尽管各个行业有所差异&#xff0c;但是大同小异&#xff0c;可以固化表结构&#xff0c;使用实体来映射表。对象的编辑…

模型 IPO(输入、处理、输出)学习模型

系列文章 分享 模型&#xff0c;了解更多&#x1f449; 模型_总纲目录。重在提升认知。信息转化与传递。 1 模型 IPO(输入、处理、输出)学习模型的应用 1.1 项目管理知识体系 PMBOK 中的IPO应用 在项目管理领域&#xff0c;PMBOK&#xff08;Project Management Body of Know…

防火墙 iptables(二)--------------------SNAT与DNAT

一、SNAT ①SNAT 应用环境: 局域网主机共享单个公网IP地址接入Internet (私有IP不能在Internet中正常路由) ②SNAT原理: 源地址转换&#xff0c;根据指定条件修改数据包的源IP地址&#xff0c;通常被叫做源映射 数据包从内网发送到公网时&#xff0c;SNAT会把数据包的源IP由…

下一代模型:Gemini 1.5,正如它的名字一样闪亮登场

每周跟踪AI热点新闻动向和震撼发展 想要探索生成式人工智能的前沿进展吗&#xff1f;订阅我们的简报&#xff0c;深入解析最新的技术突破、实际应用案例和未来的趋势。与全球数同行一同&#xff0c;从行业内部的深度分析和实用指南中受益。不要错过这个机会&#xff0c;成为AI领…

[uniapp的页面传参]详细讲解uniapp中页面传参的传递方式和接受方式 使用案例 代码注释

目录 一、传递方式1. URL传参2. Storage传参3. Vuex传参4.api传参eventChannel 二、接受方式1. URL传参2. Storage传参3. Vuex传参4.api传参eventChannel 三、使用案例四.提醒 在uniapp中&#xff0c;页面传参是非常常见的需求。本文将详细讲解uniapp中页面传参的传递方式和接受…

003 - Hugo, 创建文章

003 - Hugo, 创建文章创建文章单个md文件md文件图片总结 文章内容Front Matter文章目录数学公式的显示KaTeXMathJax 图片 003 - Hugo, 创建文章 创建文章 单个md文件 创建文章的方式&#xff1a; 手动创建&#xff1a;在post目录下&#xff0c;手动创建md文件。命令创建&am…

QObject 的拷贝构造和赋值操作

QObject中没有提供一个拷贝构造函数和赋值操作符给外界使用&#xff0c;其实拷贝构造和赋值的操作都是已经声明了的&#xff0c;但是它们被使用了Q_DISABLE_COPY () 宏放在了private区域。因此所有继承自QObject的类都使用这个宏声明了他们的拷贝构造函数和赋值操作符为私有。 …

Java集合篇之深入解析LinkedList

写在开头 作为ArrayList的同门师兄弟&#xff0c;LinkedList的师门地位逊色不少&#xff0c;除了在做算法题的时候我们会用到它之外&#xff0c;在实际的开发工作中我们极少使用它&#xff0c;就连它的创造者都说&#xff1a;“I wrote it&#xff0c;and I never use it”&am…

[java基础揉碎]二维数组

目录 什么是二维数组&#xff1a; 二维数组在内存中的布局: 动态初始化: 静态初始化: 杨辉三角: 使用细节和注意事项: 什么是二维数组&#xff1a; 1.从定义形式上看 int[][] 2.可以这样理解&#xff0c;原来的一维数组的每个元素是一维数组&#xff0c;就构成二维数…