机器人操作系统(ROS2)入门与实践 课件 第6章 IMU在ROS2中的使用_第1页
机器人操作系统(ROS2)入门与实践 课件 第6章 IMU在ROS2中的使用_第2页
机器人操作系统(ROS2)入门与实践 课件 第6章 IMU在ROS2中的使用_第3页
机器人操作系统(ROS2)入门与实践 课件 第6章 IMU在ROS2中的使用_第4页
机器人操作系统(ROS2)入门与实践 课件 第6章 IMU在ROS2中的使用_第5页
已阅读5页,还剩14页未读 继续免费阅读

下载本文档

版权说明:本文档由用户提供并上传,收益归属内容提供方,若内容存在侵权,请进行举报或认领

文档简介

机器人操作系统(ROS2)入门与实践机器人操作系统(ROS2)入门与实践第1章LinuxUbuntu入门基础第2章ROS2安装与系统架构第3章ROS2编程基础第4章ROS2机器人运动控制第5章激光雷达在ROS2中的使用第6章IMU在ROS2中的使用第7章ROS2中的SLAM环境建图第8章ROS2中的NAV2自主导航第9章ROS2中的图像视觉应用第10章ROS2的三维视觉应用第11章ROS2的机械臂应用第12章基于ROS2的综合应用第6章6.3基于IMU的航向锁定实现

第6章IMU在ROS2中的使用6.2IMU数据获取6.1IMU的话题名称6.4本章小结

与激光雷达一样,ROS2中的IMU节点会在特定的话题里发布数据消息。话题名称在REP-145中建议为“/imu/data_raw”、“/imu/data”和“/imu/mag”三个。6.1ROS2里的IMU数据话题内容/imu/data_raw加速度计输出的矢量加速度和陀螺仪输出的旋转加速度。/imu/data/imu/data_raw的数据再加上融合后的四元数姿态描述/imu/mag磁强计输出的磁强数据IMU数据的获取是通过订阅IMU节点发布的话题,从话题中获取IMU节点发出的消息包来实现的。如图所示:6.2IMU数据获取详细操作步骤见教材P155-P166页cd~/ros2_ws/srcros2pkgcreateimu_pkg1、编写节点代码新建文件,命名为“imu_data.cpp”。6.2IMU数据获取6.2.1编写IMU数据获取程序#include<rclcpp/rclcpp.hpp>#include<sensor_msgs/msg/imu.hpp>#include<tf2/LinearMath/Quaternion.h>#include<tf2/LinearMath/Matrix3x3.h>

std::shared_ptr<rclcpp::Node>node;

voidIMUCallback(constsensor_msgs::msg::Imu::SharedPtrmsg){tf2::Quaterniontf2_quaternion;tf2_quaternion.setX(msg->orientation.x);tf2_quaternion.setY(msg->orientation.y);tf2_quaternion.setZ(msg->orientation.z);tf2_quaternion.setW(msg->orientation.w);

tf2::Matrix3x3matrix(tf2_quaternion);

doubleroll,pitch,yaw;matrix.getRPY(roll,pitch,yaw);roll=roll*180/M_PI;pitch=pitch*180/M_PI;yaw=yaw*180/M_PI;RCLCPP_INFO(node->get_logger(),"roll=%.0fpitch=%.0fyaw=%.0f",roll,pitch,yaw);}6.2IMU数据获取intmain(intargc,char**argv){rclcpp::init(argc,argv);

node=std::make_shared<rclcpp::Node>("imu_data_node");

autosub=node->create_subscription<sensor_msgs::msg::Imu>("/imu",10,IMUCallback);

rclcpp::spin(node);

rclcpp::shutdown();

return0;}6.2IMU数据获取2、设置编译规则3、修改软件包信息4、编译软件包find_package(rclcppREQUIRED)find_package(sensor_msgsREQUIRED)find_package(tf2REQUIRED)add_executable(imu_datasrc/imu_data.cpp)ament_target_dependencies(imu_data"rclcpp""sensor_msgs""tf2")install(TARGETSimu_dataDESTINATIONlib/${PROJECT_NAME})<depend>rclcpp</depend><depend>sensor_msgs</depend><depend>tf2</depend>cd~/ros2_wscolconbuild6.2IMU数据获取6.2.2仿真运行雷达数据获取程序sourceinstall/setup.bashros2launchwpr_simulation2wpb_simple.launch.pysourceinstall/setup.bashros2runimu_pkgimu_data6.2IMU数据获取打开第2个子窗口。6.2IMU数据获取

下面实现一个航向锁定的功能。编写一个节点,先订阅“/imu/data”话题。从此话题中接收IMU节点发来的sensor_msgs::Imu类型消息包,解析出机器人的姿态角度。设置一个目标航向角度,让机器人朝着这个航向运动。若运行过程中机器人的姿态被改变了,导致航向角与目标角度不一致。则根据航向角偏差值,计算出纠偏速度值,打包成geometry_msgs::Twist类型的消息包,发布到“/cmd_vel”话题中。机器人底盘节点会从“/cmd_vel”话题获取速度值并执行,以此来实现航向锁定的行为。如图所示:6.3基于IMU的航向锁定实现详细操作步骤见教材P167-P179页1、编写节点代码新建文件,命名为“imu_behavior.cpp”。6.3基于IMU的航向锁定实现6.3.1编写航向锁定程序按照上一个实验,在工作空间中创建一个imu_pkg软件包。如果已经创建好了,可以直接使用,不用再重复创建。具体操作步骤如下:#include<rclcpp/rclcpp.hpp>#include<sensor_msgs/msg/imu.hpp>#include<geometry_msgs/msg/twist.hpp>#include<tf2/LinearMath/Quaternion.h>#include<tf2/LinearMath/Matrix3x3.h>

std::shared_ptr<rclcpp::Node>node;rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtrvel_pub;

voidIMUCallback(constsensor_msgs::msg::Imu::SharedPtrmsg){tf2::Quaterniontf2_quaternion;tf2_quaternion.setX(msg->orientation.x);tf2_quaternion.setY(msg->orientation.y);tf2_quaternion.setZ(msg->orientation.z);tf2_quaternion.setW(msg->orientation.w);

tf2::Matrix3x3matrix(tf2_quaternion);

doubleroll,pitch,yaw;matrix.getRPY(roll,pitch,yaw);roll=roll*180/M_PI;pitch=pitch*180/M_PI;yaw=yaw*180/M_PI;RCLCPP_INFO(node->get_logger(),"roll=%.0fpitch=%.0fyaw=%.0f",roll,pitch,yaw);6.3基于IMU的航向锁定实现doubletarget_yaw=90;

geometry_msgs::msg::Twistvel_msg;doublediff_angle=target_yaw-yaw;vel_msg.angular.z=diff_angle*0.01;vel_msg.linear.x=0.1;vel_pub->publish(vel_msg);}

intmain(intargc,char**argv){rclcpp::init(argc,argv);

node=rclcpp::Node::make_shared("imu_behavior_node");

autosub=node->create_subscription<sensor_msgs::msg::Imu>("imu",10,IMUCallback);

vel_pub=node->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel",10);

rclcpp::spin(node);

rclcpp::shutdown();

return0;}6.3基于IMU的航向锁定实现2、设置编译规则3、修改软件包信息4、编译软件包find_package(rclcppREQUIRED)find_package(sensor_msgsREQUIRED)find_package(tf2REQUIRED)find_package(geometry_msgsREQUIRED)add_executable(imu_behaviorsrc/imu_behavior.cpp)ament_target_dependencies(imu_behavior"rclcpp""sensor_msgs""tf2""geometry_msgs")install(TARGETSimu_dataimu_behaviorDESTINATIONlib/${PROJECT_NAME})<depend>rclcpp</depend><depend>sensor_msgs</depend><depend>tf2</depend><depend>geometry_msgs</depend>cd~/ros2_wscolconbuild6.3基于IMU的航向锁定实现sourceinstall/setup.bashros2launchwpr_simulation2wpb_simple.launch.pysourceinstall/setup.bashros2runimu_pkgimu_behavior6.3基于IMU的航向锁定实现6.3.2仿真运行航向锁定程序打开第2个子窗口。6

温馨提示

  • 1. 本站所有资源如无特殊说明,都需要本地电脑安装OFFICE2007和PDF阅读器。图纸软件为CAD,CAXA,PROE,UG,SolidWorks等.压缩文件请下载最新的WinRAR软件解压。
  • 2. 本站的文档不包含任何第三方提供的附件图纸等,如果需要附件,请联系上传者。文件的所有权益归上传用户所有。
  • 3. 本站RAR压缩包中若带图纸,网页内容里面会有图纸预览,若没有图纸预览就没有图纸。
  • 4. 未经权益所有人同意不得将文件中的内容挪作商业或盈利用途。
  • 5. 人人文库网仅提供信息存储空间,仅对用户上传内容的表现方式做保护处理,对用户上传分享的文档内容本身不做任何修改或编辑,并不能对任何下载内容负责。
  • 6. 下载文件中如有侵权或不适当内容,请与我们联系,我们立即纠正。
  • 7. 本站不保证下载资源的准确性、安全性和完整性, 同时也不承担用户因使用这些下载资源对自己和他人造成任何形式的伤害或损失。

评论

0/150

提交评论