每次打开终端都显示not found: "/opt/ros/foxy/share/slam_toolbox/local_setup.bash",虽然对程序运行没有影响,但是很烦人,有人知道在哪里设置吗?翻遍网页也没有找到哪个文件影响了终端
重要提示
提问前必看的发帖注意事项—— 提问前必看!不符合要求的问题拒绝回答!!
社区使用指南—如何添加标签修改密码
毛哥成山轮胎机油保养 发布的帖子
-
打开终端显示not found: "/opt/ros/foxy/share/slam_toolbox/local_setup.bash"
-
串口设备断电提示代码怎么写呢,有知道的吗?
我的速控板有个电源开关,用的是usb转串口连接,启动速控板节点launch文件,显示启动正常,即使给速控板关闭电源,也没有错误提示,怎样加个提示电源已关闭的提示代码呢?
bool JtbotFocDriver::ReadFormSerial() // 读串口 { if (Robot_Serial.available()) { rxdata = Robot_Serial.read(Robot_Serial.available()); if (rxdata.size() == sizeof(JtbotFeedback)) { memcpy(&Feedback, rxdata.c_str(), sizeof(JtbotFeedback)); uint16_t checksum = (uint16_t)(Feedback.start ^ Feedback.rpmR ^ Feedback.rpmL ^ Feedback.batVoltage ^ Feedback.boardTemp ^ Feedback.curL_DC ^ Feedback.curR_DC); if ((Feedback.start == START_FRAME) && (checksum == Feedback.checksum)) return true; else return false; } else { RCLCPP_INFO(this->get_logger(), "速控板读取数据失败22222"); return false; } } else { RCLCPP_INFO(this->get_logger(), "速控板读取数据失败11111"); return false; } }
我加的: RCLCPP_INFO(this->get_logger(), "速控板读取数据失败11111"); 这段代码只要开机就一直刷屏。
RCLCPP_INFO(this->get_logger(), "速控板读取数据失败22222"); 这段代码加的位置不管速控板是否开机根本就不提示,说明加的位置不对。
串口初始化的位置我也加了提示,只是没有插usb口有错误提示,如果usb口插好了速控板开不开电源都提示初始化成功。/**打开串口设备**/ try { Robot_Serial.setPort(port_name_); Robot_Serial.setBaudrate(baud_rate_); serial::Timeout to = serial::Timeout::simpleTimeout(2000); Robot_Serial.setTimeout(to); Robot_Serial.open(); } catch (serial::IOException &e) { RCLCPP_INFO(this->get_logger(), "foc速控板打开失败,请检查串口是否连接"); } if (Robot_Serial.isOpen()) { RCLCPP_INFO(this->get_logger(), "速控板串口打开成功"); } else { } }
懂c++的朋友给指点一下
速控板完整代码#include "jtbot_foc_driver/jtbot_foc_driver.h" using namespace std::chrono_literals; JtbotFocDriver::JtbotFocDriver() : Node("jtbot_foc_driver"), odom_theta_(0), x_pos_(0), y_pos_(0) // 构造函数,初始化成员变量 { wheel_diameter_ = WHEEL_DIAMETER; // 轮子直径 wheel_circumference_ = PI * wheel_diameter_; // 轮子周长 max_rpm_ = MAX_RPM; // 轮子最大转速 wheels_x_distance_ = FR_WHEELS_DISTANCE; // 2轮差速 前后轮子距离 0 wheels_y_distance_ = LR_WHEELS_DISTANCE; // 左右轮子距离 now_ = now(); declare_parameter("port_name", std::string("/dev/ttyTHS1")); // 根据实际端口修改 get_parameter("port_name", port_name_); declare_parameter("baud_rate", 115200); get_parameter("baud_rate", baud_rate_); // 发布电池电量 battery_pub_ = this->create_publisher<std_msgs::msg::Float32>("battery", 10); // 定时10毫秒发布电池和速度 odom 和 tf2 topic_timer_ = this->create_wall_timer(10ms, std::bind(&JtbotFocDriver::RosNodeTopicCallback, this)); // 接收速度话题 回调函数发布控制命令 velocity_sub_ = this->create_subscription<geometry_msgs::msg::Twist>("cmd_vel", 10, std::bind(&JtbotFocDriver::cmd_vel_callback, this, std::placeholders::_1)); // odom_publisher_ = this->create_publisher<nav_msgs::msg::Odometry>("odom_diff", 20); //创建odom发布者 odom_publisher_ = this->create_publisher<nav_msgs::msg::Odometry>("odom", 20); // 创建odom发布者 odom_transform_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this); // 创建tf2发布者 last_odom_time_ = now(); odom_.header.frame_id = "odom"; // odom_.child_frame_id = "base_footprint"; odom_.child_frame_id = "base_link"; odom_transform_.header.frame_id = "odom"; // odom_transform_.child_frame_id = "base_footprint"; odom_transform_.child_frame_id = "base_link"; /**打开串口设备**/ try { Robot_Serial.setPort(port_name_); Robot_Serial.setBaudrate(baud_rate_); serial::Timeout to = serial::Timeout::simpleTimeout(2000); Robot_Serial.setTimeout(to); Robot_Serial.open(); } catch (serial::IOException &e) { RCLCPP_INFO(this->get_logger(), "foc速控板打开失败,请检查串口是否连接"); } if (Robot_Serial.isOpen()) { RCLCPP_INFO(this->get_logger(), "速控板串口打开成功"); } else { } } JtbotFocDriver::~JtbotFocDriver() { Robot_Serial.close(); } void JtbotFocDriver::RosNodeTopicCallback() // ros节点话题回调 { if (true == ReadFormSerial()) { publisherBattery(); // 发布电压 低于33伏发布充电警告 PublisherOdom(); // 发布odom } } bool JtbotFocDriver::ReadFormSerial() // 读串口 { if (Robot_Serial.available()) { rxdata = Robot_Serial.read(Robot_Serial.available()); if (rxdata.size() == sizeof(JtbotFeedback)) { memcpy(&Feedback, rxdata.c_str(), sizeof(JtbotFeedback)); // printf("Feedbac左轮转速 %hd 右轮转速 %hd 电压 %lf 主板温度 %lf\n",Feedback.rpmL,Feedback.rpmR,double(Feedback.batVoltage)/100,double(Feedback.boardTemp)/10); uint16_t checksum = (uint16_t)(Feedback.start ^ Feedback.rpmR ^ Feedback.rpmL ^ Feedback.batVoltage ^ Feedback.boardTemp ^ Feedback.curL_DC ^ Feedback.curR_DC); if ((Feedback.start == START_FRAME) && (checksum == Feedback.checksum)) return true; else return false; } else { RCLCPP_INFO(this->get_logger(), "速控板读取数据失败22222"); return false; } } else { RCLCPP_INFO(this->get_logger(), "速控板读取数据失败11111"); return false; } } /*cmd_vel Subscriber的回调函数*/ void JtbotFocDriver::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr twist_msg) { SetVelocity(twist_msg->linear.x, twist_msg->linear.y, twist_msg->angular.z); } /*底盘速度发送函数*/ void JtbotFocDriver::SetVelocity(double x, double y, double angular_z) { calculateRPM(x, y, angular_z); // 把速度映射为foc波形 int16_t mSpeedL = map(req_rpm.motor1, -max_rpm_, max_rpm_, -FOC_MAX_PWM, FOC_MAX_PWM); int16_t mSpeedR = map(req_rpm.motor2, -max_rpm_, max_rpm_, -FOC_MAX_PWM, FOC_MAX_PWM); // 根据收到的cmd_vel计算左右轮的速度,装填数据,写人串口 Command.start = (uint16_t)START_FRAME; Command.mSpeedR = (uint16_t)mSpeedR; Command.mSpeedL = (uint16_t)mSpeedL; Command.checksum = (uint16_t)(Command.start ^ Command.mSpeedR ^ Command.mSpeedL); if (Robot_Serial.write((uint8_t *)&Command, sizeof(Command)) <= 0) { RCLCPP_INFO(this->get_logger(), "速控板写入速度控制数据失败,请检查"); } } void JtbotFocDriver::publisherBattery() // 发布显示电压 { if ((now() - now_).seconds() > 1 / BATTERY_RATE) { raw_battery_msg.data = (float)Feedback.batVoltage / 100; if (raw_battery_msg.data < 35) { RCLCPP_WARN(this->get_logger(), "当前电压: %f ,电压偏低,请充电!!", raw_battery_msg.data); battery_pub_->publish(raw_battery_msg); now_ = now(); } } } void JtbotFocDriver::PublisherOdom() // 发布odom { getVelocities(Feedback.rpmL, Feedback.rpmR); // 获取速度,数据填充到vel结构体 rclcpp::Time current_time = now(); odom_.header.stamp = current_time; // x线速度 auto linear_velocity_x_ = vel.linear_x; // y线速度 auto linear_velocity_y_ = vel.linear_y; // 角速度 auto angular_velocity_z_ = vel.angular_z; rclcpp::Duration vel_dt_ = current_time - last_odom_time_; last_odom_time_ = current_time; double delta_heading = angular_velocity_z_ * vel_dt_.nanoseconds() / 1e9; // radians double delta_x = (linear_velocity_x_ * cos(odom_theta_)) * vel_dt_.nanoseconds() / 1e9; // m double delta_y = (linear_velocity_x_ * sin(odom_theta_)) * vel_dt_.nanoseconds() / 1e9; // m x_pos_ += delta_x; y_pos_ += delta_y; odom_theta_ += delta_heading; tf2::Quaternion odom_q; // 声明四元数 odom_q.setRPY(0, 0, odom_theta_); // 欧拉角换算四元数 // 相对于父坐标系位置 odom_.pose.pose.position.x = x_pos_; odom_.pose.pose.position.y = y_pos_; odom_.pose.pose.position.z = 0.0; // 相对于父坐标系姿态四元数 odom_.pose.pose.orientation.x = odom_q.x(); odom_.pose.pose.orientation.y = odom_q.y(); odom_.pose.pose.orientation.z = odom_q.z(); odom_.pose.pose.orientation.w = odom_q.w(); // 姿态协方差 // odom_.pose.covariance[0] = 0.001; // odom_.pose.covariance[7] = 0.001; // odom_.pose.covariance[35] = 0.001; // 相对于子坐标系线速度和角速度 odom_.twist.twist.linear.x = linear_velocity_x_; odom_.twist.twist.linear.y = linear_velocity_y_; odom_.twist.twist.angular.z = angular_velocity_z_; // 协方差分为静止和运动: // pose: // if(linear_velocity_x_==0){ // //pose静止: // odom_.pose.covariance[0] = 1e-9; // odom_.pose.covariance[7] = 1e-3; // odom_.pose.covariance[8] = 1e-9; // odom_.pose.covariance[14] = 1e6; // odom_.pose.covariance[21] = 1e6; // odom_.pose.covariance[28] = 1e6; // odom_.pose.covariance[35] = 1e-9; // //twist静止: // odom_.twist.covariance[0] = 1e-9; // odom_.twist.covariance[7] = 1e-3; // odom_.twist.covariance[8] = 1e-9; // odom_.twist.covariance[14] = 1e6; // odom_.twist.covariance[21] = 1e6; // odom_.twist.covariance[28] = 1e6; // odom_.twist.covariance[35] = 1e-9; // } // else{ // //pose运动: // odom_.pose.covariance[0] = 1e-3; // odom_.pose.covariance[7] = 1e-3; // odom_.pose.covariance[8] = 0.0; // odom_.pose.covariance[14] = 1e6; // odom_.pose.covariance[21] = 1e6; // odom_.pose.covariance[28] = 1e6; // odom_.pose.covariance[35] = 1e3; // //twist运动: // odom_.twist.covariance[0] = 1e-3; // odom_.twist.covariance[7] = 1e-3; // odom_.twist.covariance[8] = 0.0; // odom_.twist.covariance[14] = 1e6; // odom_.twist.covariance[21] = 1e6; // odom_.twist.covariance[28] = 1e6; // odom_.twist.covariance[35] = 1e3; // } odom_.header.stamp = now(); odom_publisher_->publish(odom_); // printf("odom_.pose x y z %f %f %f \n",x_pos_,y_pos_,odom_theta_); // printf("odom_.twist x y z %f %f %f \n",linear_velocity_x_,linear_velocity_y_,angular_velocity_z_); odom_transform_.header.stamp = last_odom_time_; odom_transform_.transform.translation.x = x_pos_; odom_transform_.transform.translation.y = y_pos_; odom_transform_.transform.rotation.x = odom_q.x(); odom_transform_.transform.rotation.y = odom_q.y(); odom_transform_.transform.rotation.z = odom_q.z(); odom_transform_.transform.rotation.w = odom_q.w(); // odom_transform_broadcaster_->sendTransform(odom_transform_); //开odom imu融合需要关闭/tf话题 // printf("now last_odom_time_%ld %ld %ld\n",now(),current_time,last_odom_time_); } // 计算转速 void JtbotFocDriver::calculateRPM(float linear_x, float linear_y, float angular_z) { float linear_vel_x_mins; float linear_vel_y_mins; float angular_vel_z_mins; float tangential_vel; float x_rpm; float y_rpm; float tan_rpm; // 转换米/秒 至 米/分 linear_vel_x_mins = linear_x * 60; linear_vel_y_mins = linear_y * 60; // 转换 rad/秒 to rad/分 angular_vel_z_mins = angular_z * 60; tangential_vel = angular_vel_z_mins * ((wheels_x_distance_ / 2) + (wheels_y_distance_ / 2)); x_rpm = linear_vel_x_mins / wheel_circumference_; y_rpm = linear_vel_y_mins / wheel_circumference_; tan_rpm = tangential_vel / wheel_circumference_; req_rpm.motor1 = int16_t(x_rpm - y_rpm - tan_rpm); req_rpm.motor1 = constrain(req_rpm.motor1, -max_rpm_, max_rpm_); // front-right motor req_rpm.motor2 = -int16_t(x_rpm + y_rpm + tan_rpm); req_rpm.motor2 = constrain(req_rpm.motor2, -max_rpm_, max_rpm_); } void JtbotFocDriver::getVelocities(int rpm1, int rpm2) // 获取速度,数据填充到vel结构体 { float average_rps_x; // float average_rps_y; float average_rps_a; // 将平均每分钟转数转换为每秒转数,小车向前走左轮为正,右轮为负 average_rps_x = ((float)(rpm1 - rpm2) / 2) / 60; // RPM vel.linear_x = average_rps_x * wheel_circumference_; // m/s // 将y轴上的平均每分钟转数转换为每秒转数 // average_rps_y = ((float)(-rpm1 + rpm2) / 2) / 60; // RPM vel.linear_y = 0; // 将平均每分钟转数转换为每秒转数 average_rps_a = ((float)(rpm1 + rpm2) / 2) / 60; vel.angular_z = -(average_rps_a * wheel_circumference_) / ((wheels_x_distance_ / 2) + (wheels_y_distance_ / 2)); // rad/s } // 速度映射到foc波形 int16_t JtbotFocDriver::map(int16_t x, int16_t in_min, int16_t in_max, int16_t out_min, int16_t out_max) { return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; } int main(int argc, char *argv[]) { rclcpp::init(argc, argv); auto driver = std::make_shared<JtbotFocDriver>(); rclcpp::spin(driver); rclcpp::shutdown(); return 0; }
-
RE: 导航实物部署
@3568485143 如果没有imu 最好先不要用融合定位,融合的话底盘发出的/odom需要修改,假设修改成/odom_diff, 融合生成的话题也需要映射
remappings=[('/odometry/filtered','odom'),],
还要配置ekf.yaml文件,难度会增大很多,导致你很难判断哪里有问题 -
RE: 请问ros2 run和ros2 launch启动同一个节点的启动效果不同是什么原因呢?
@Lorry 可以了
KERNELS=="3-2.3:1.0", MODE:="0777", GROUP:="dialout", SYMLINK+="ttyimu",
以前弄了好多次,不行,刚刚重新试了一下,可以了。 -
RE: 请问ros2 run和ros2 launch启动同一个节点的启动效果不同是什么原因呢?
@Lorry 我试遍了网上给usb端口改别名的方法,就是不生效,只能把imu第一个插上选/dev/ttyUSB0,你有经验吗?
-
RE: 请问ros2 run和ros2 launch启动同一个节点的启动效果不同是什么原因呢?
@小鱼 c++还是有点驾驭不了,源码在这
链接文本
鱼总有时间帮我分析一下哪里有问题,现在这个程序用launch启动逻辑是不正常的,只能用ros2 run 启动,还有这个imu我用尽了各种办法都不能固定usb端口别名,只能是先插这个imu 它的端口号用ttyUSB0
usb.rulesKERNELS=="1-1.2:1.0", MODE:="0777", GROUP:="dialout",SYMLINK+="wheeltec_laser" KERNELS=="3-1:1.0", MODE:="0777", GROUP:="dialout", SYMLINK+="ttyTHS1" SUBSYSTEM=="usb", ATTR{idProduct}=="0403", ATTR{idVendor}=="2bc5", MODE:="0666", OWNER:="root", GROUP:="video", SYMLINK+="astra_pro" SUBSYSTEM=="usb", ATTR{idProduct}=="0501", ATTR{idVendor}=="2bc5", MODE:="0666", OWNER:="root", GROUP:="video", SYMLINK+="astrauvc" KERNEL=="ttyUSB*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="7523", MODE:="0777",GROUP:="dialout", SYMLINK+="ttyimu"
其他像雷达 摄像头 底盘驱动端口都生效了,这个ttyimu就是死活不生效,现在只能用/dev/ttyUSB0
-
RE: 导航实物部署
@3568485143 我这段时间也一直在研究实体机器人导航这一块,你的机器人开启imu了吗,如果没有开启先不用Node(
package='robot_localization',
executable='ekf_node',
这个节点,这个节点是融合里程计和imu的,跑通了在把这个节点加上,先用最简单的模型跑通导航,你可以参考一下我的学习记录
链接文本,
我也遇到了很多问题,我们可以一起探讨 -
请问ros2 run和ros2 launch启动同一个节点的启动效果不同是什么原因呢?
我参考网上代码写了一个JY_95T imu的获取程序,这款imu需要在接收数据前发送一串字符串命令,但是我发现我用ros2 run imu_get_cpp publisher_imu_node 启动能正确发送这段代码,用 ros2 launch imu_get_cpp imu_cpp.launch.py 启动这个imu节点这段命令不能正常发出,而是在按ctrl+c退出程序的时候才发出这段命令,按理说这两种启动节点的方法应该是一样的。有人知道什么原因吗?下面是2种启动方法的日志文件
m@ubun:~/ros2_ws$ ros2 run imu_get_cpp publisher_imu_node
[INFO] [1693883056.342819936] [publisher_imu_node]: imu串口初始化完成!
imu发出命令: a4 3 8 23 d2
imu发布线程工作中
^C[INFO] [1693883060.571197391] [rclcpp]: signal_handler(signal_value=2)
imu线程退出m@ubun:~/ros2_ws$ ros2 launch imu_get_cpp imu_cpp.launch.py
[INFO] [launch]: All log files can be found below /home/m/.ros/log/2023-09-05-11-05-57-304442-ubun-16558
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [publisher_imu_node-1]: process started with pid [16563]
[publisher_imu_node-1] [INFO] [1693883157.461656186] [publisher_imu_node]: imu串口初始化完成!
^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
[publisher_imu_node-1] [INFO] [1693883161.738837124] [rclcpp]: signal_handler(signal_value=2)
[publisher_imu_node-1] imu发出命令: a4 3 8 23 d2
[publisher_imu_node-1] imu发布线程工作中
[publisher_imu_node-1] imu线程退出
[INFO] [publisher_imu_node-1]: process has finished cleanly [pid 16563]imu_cpp.launch.py
#!/usr/bin/python3 from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node def generate_launch_description(): imu_node = Node(package='imu_get_cpp', executable='publisher_imu_node', name='publisher_imu_node', output='screen', ) return LaunchDescription([ imu_node, ])
-
RE: ros2 c++实现JY_95T IMU解算三轴 加速度 角速度 欧拉角 磁力计 四元数
@毛哥成山轮胎机油保养 我现在有一个疑问,2种运行方式,为什么程序运行的不一样呢,终端返回的信息也不一样 ?
ros2 launch imu_get_cpp imu_cpp.launch.py[INFO] [launch]: All log files can be found below /home/m/.ros/log/2023-08-31-18-56-23-189690-ubun-13754
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [publisher_imu_node-1]: process started with pid [13756]
[publisher_imu_node-1] [INFO] [1693479383.314753546] [publisher_imu_node]: imu串口初始化完成!
^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
[publisher_imu_node-1] [INFO] [1693479445.030912184] [rclcpp]: signal_handler(signal_value=2)
[publisher_imu_node-1] imu发出命令: a4 3 8 23 d2
[publisher_imu_node-1] imu发布线程工作中
[publisher_imu_node-1] imu线程退出/home/m/ros2_ws/install/imu_get_cpp/lib/imu_get_cpp/publisher_imu_node
[INFO] [1693479448.843189189] [publisher_imu_node]: imu串口初始化完成!
imu发出命令: a4 3 8 23 d2
imu发布线程工作中
^C^C[INFO] [1693479476.254140522] [rclcpp]: signal_handler(signal_value=2)
imu线程退出按理说这2种运行方式都是一样的,第一种是ros2的标准运行命令,第二种是按c++的运行命令,为什么程序运行的方式不一样呢,有没有懂的大佬给解释一下,非常感谢
-
ros2 c++实现JY_95T IMU解算三轴 加速度 角速度 欧拉角 磁力计 四元数
imu传感器数据解算原理:
此款imu内置44个寄存器,每个寄存器存储一个字节的数据,对应于不同的功能,具体功能参考上面的使用说明,其中8-42号寄存器储存的是imu的3个轴的实时数据,我们的目的就是通过c++读取这35个寄存器数据并加以解算。从串口读取到的一个完整的数据帧包含40位数据,0-3位4个Byte是imu的配置信息,4-38位35个Byte是数据信息,39位是校验和低八位。下面的程序就围绕这个原理展开。运行环境:ubuntu 20.04 ros2 foxy
目录结构:
imu_cpp.launch.py#!/usr/bin/python3 from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node def generate_launch_description(): imu_node = Node(package='imu_get_cpp', executable='publisher_imu_node', name='publisher_imu_node', output='screen', ) return LaunchDescription([ imu_node, ])
publisher_imu.cpp
#include <chrono> #include <math.h> #include "serial/serial.h" //导入串口库,ros2不带串口库,需要单独安装 #include <memory.h> #include "std_msgs/msg/string.hpp" #include "sensor_msgs/msg/magnetic_field.hpp" #include "sensor_msgs/msg/imu.hpp" #include "tf2/LinearMath/Quaternion.h" #include "tf2_ros/transform_broadcaster.h" #include "tf2_ros/static_transform_broadcaster.h" #include "nav_msgs/msg/odometry.hpp" #include <string> #include "rclcpp/rclcpp.hpp" #include "transform.hpp" serial::Serial ser; using namespace std::chrono_literals; class publisher_imu_node : public rclcpp::Node// 创建imu发布类继承自rclcpp::Node { private: std::string port; //端口名 int baudrate; //波特率查看硬件说明 transform_imu imu_fetch; //创建imu获取对象 rclcpp::TimerBase::SharedPtr timer_; //创建定时器 rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub_imu; // 创建发布者 public: publisher_imu_node() : Node("publisher_imu_node") //构造函数 { port = "/dev/ttyUSB0"; // 根据自己的主机修改串口号 baudrate = 115200; // 波特率,imu在波特率115200返回数据时间大概是1ms,9600下大概是10ms try { ser.setPort(port); //设置端口 ser.setBaudrate(baudrate); //设置波特率 serial::Timeout to = serial::Timeout::simpleTimeout(1); //设置超时 ser.setTimeout(to); ser.open(); //打开串口 } catch (serial::IOException &e) { RCLCPP_INFO(this->get_logger(), "不能打开端口 "); return; } if (ser.isOpen()) { RCLCPP_INFO(this->get_logger(), "imu串口初始化完成!"); unsigned char cmd_buffer[5] ={0xA4,0x03,0x08,0x23,0xD2}; //JY-95T启用35个寄存器 需要发送的串口包命令 ser.write(cmd_buffer,5); //JY-95T可以根据命令选择工作方式 printf("imu发出命令: %x %x %x %x %x\n",cmd_buffer[0],cmd_buffer[1],cmd_buffer[2],cmd_buffer[3],cmd_buffer[4]); } else { RCLCPP_INFO(this->get_logger(), "Serial Port ???"); return; } pub_imu = this->create_publisher<sensor_msgs::msg::Imu>("/imu_data", 20);//创建了话题 /imu_data // 创建定时器,100ms为周期,定时发布,这个时间设置超过200毫秒发布的数据卡顿 timer_ = this->create_wall_timer(100ms, std::bind(&publisher_imu_node::timer_callback, this)); printf("imu发布线程工作中\n"); } public: void timer_callback() //定时器的回调函数,每20毫秒回调一次 { int count = ser.available(); // 读取到缓存区数据的字节数 if (count > 0) { int num; rclcpp::Time now = this->get_clock()->now(); // 获取时间戳 std::vector<unsigned char> read_buf(count);//这里定义无符号,是因为read函数的形参是无符号 num = ser.read(&read_buf[0], count); // 读出缓存区缓存的数据,返回值为读到的数据字节数 imu_fetch.FetchData(read_buf, num); sensor_msgs::msg::Imu imu_data; //----------------imu data填充数据---------------- imu_data.header.stamp = now; imu_data.header.frame_id = "imu_link"; //imu_data.header.frame_id = "map"; //加速度 imu_data.linear_acceleration.x = imu_fetch.acc_x; imu_data.linear_acceleration.y = imu_fetch.acc_y; imu_data.linear_acceleration.z = imu_fetch.acc_z; //角速度 imu_data.angular_velocity.x = imu_fetch.gyro_x ; imu_data.angular_velocity.y = imu_fetch.gyro_y ; imu_data.angular_velocity.z = imu_fetch.gyro_z ; //四元数 imu_data.orientation.x = imu_fetch.quat_Q0; imu_data.orientation.y = imu_fetch.quat_Q1; imu_data.orientation.z = imu_fetch.quat_Q2; imu_data.orientation.w = imu_fetch.quat_Q3; pub_imu->publish(imu_data); //向话题放数据 /* Author: Mao Haiqing Time: 2023.7.6 description: 读取IMU数据 */ } } }; int main(int argc, char *argv[]) { rclcpp::init(argc, argv); auto node = std::make_shared<publisher_imu_node>();//创建对应节点的共享指针对象 rclcpp::spin(node); //运行节点,并检测退出信号 printf("imu线程退出\n"); rclcpp::shutdown(); return 0; }
transform.hpp
#include <string> #include <ctype.h> #include <float.h> #include <math.h> class transform_imu { public: double acc_x=0;//加速度 double acc_y=0; double acc_z=0; double gyro_x=0; //陀螺仪角速度 double gyro_y=0; double gyro_z=0; double angle_r=0; //欧拉角 double angle_p=0; double angle_y=0; double temp=0; //imu温度计 double mag_x=0; //磁力计 double mag_y=0; double mag_z=0; double quat_Q0=0; //四元数 double quat_Q1=0; double quat_Q2=0; double quat_Q3=0; public: transform_imu(){}; //默认构造函数 ~transform_imu(){}; //析构函数 // void FetchData(auto &data, int usLength) //获取数据 void FetchData(std::vector<unsigned char> &data, int usLength) { int index = 0; unsigned char sum=0; //for(int i = 0;i < usLength;i++){printf("%x ",data[i]);} //打印数据,查看收到的数据是否正确 while (usLength >= 40)//一个完整数据帧40字节[0xA4,0x03,0x08,0x23,数据35字节,校验位] { // 收到的数据开头4位:0xA4 帧头,0x03 读取,0x08 从8号寄存器开始,0x23 35个寄存器 if (data[index] != 0xA4)//0xA4是协议头 { index++;//指针(/索引)后移,继续找协议帧头 usLength--; continue; } //printf("找到数据包帧头,index是%d,uslength=%d\n",index,usLength);//每一次读取的数据长度 if(data[index + 1] != 0x03) //判断第二个数据是否正确 { // printf("第2数据包内容不对,进入下个循环"); usLength = usLength - 40; index += 40; continue; } // printf("读imu数据\n"); if(data[index + 2] != 0x08) //判断第3个数据是否正确 { printf("第三数据包内容不对,进入下个循环"); usLength = usLength - 40; index += 40; continue; } // printf("第08寄存器是数据\n"); if(data[index + 3] != 0x23) //判断第4个数据是否正确 { //printf("第四数据包内容不对,进入下个循环\n"); usLength = usLength - 40; index += 40; continue; } //printf("35个数据包\n"); //0-39字节相加的值取低8位与第40位校验,参考GY-95T 九轴模块使用说明 for(int i=index;i<index+40;i++) //前39为求和 { sum=0+data[i]; } if(sum != data[index+39]) //判断前39个数据的和是否与第40位相等 { //printf("校验和不对,进入下个循环\n"); usLength = usLength - 40; index += 40; continue; } // printf("校验和通过,数据没有问题\n"); /* 根据模块使用说明可知,一个完整数据帧40字节[0xA4,0x03,0x08,0x23,数据35字节,校验位] 1.开头4位:0xA4 帧头,0x03 读取,0x08 数据位从8号寄存器开始,0x23 35个寄存器 2.加速度位:4-9 3.陀螺仪位:10-15 4.欧拉角:16-21 5.磁场校准:22 6.器件温度:23-24 5.磁场:25-30 6.四元数:31-38 数据解析一定注意2个字节拼在一起用short类型,然后在转换成double类型,总体加小括号在作除法, 否则short类型作除法会被截断降低精度,表现在图形显示界面会卡,数据不流畅。拼好的数据单位参考 使用手册。 */ // 加速度 acc_x = ((double)((short)(data[index + 5]<<8 | data[index + 4])))/2048 * 9.8; //单位m/s^2 acc_y = ((double)((short)(data[index + 7]<<8 | data[index + 6])))/2048 * 9.8; acc_z = ((double)((short)(data[index + 9]<<8 | data[index + 8])))/2048 * 9.8; // 陀螺仪角速度 gyro_x = ((double)((short)(data[index + 11]<<8 | data[index + 10])))/16.4*3.1415926 / 180.0; //单位rad/sec gyro_y = ((double)((short)(data[index + 13]<<8 | data[index + 12])))/16.4*3.1415926 / 180.0; gyro_z = ((double)((short)(data[index + 15]<<8 | data[index + 14])))/16.4*3.1415926 / 180.0; //欧拉角 // angle_r = ((double)((short)(data[index + 17]<<8 | data[index + 16])))/100 ; // angle_p = ((double)((short)(data[index + 19]<<8 | data[index + 18])))/100 ; // angle_y = ((double)((short)(data[index + 21]<<8 | data[index + 20])))/100 ; //imu温度 temp = ((double)((short)(data[index + 24]<<8 | data[index + 23])))/100 ; //单位摄氏度 //磁力计 // mag_x = ((double)((short)(data[index + 26]<<8 | data[index + 25]))); // mag_y = ((double)((short)(data[index + 28]<<8 | data[index + 27]))); // mag_z = ((double)((short)(data[index + 30]<<8 | data[index + 29]))); //四元数 quat_Q0 = ((double)((short)(data[index + 32]<<8 | data[index + 31])))/10000 ; //无单位 quat_Q1 = ((double)((short)(data[index + 34]<<8 | data[index + 33])))/10000 ; quat_Q2 = ((double)((short)(data[index + 36]<<8 | data[index + 35])))/10000 ; quat_Q3 = ((double)((short)(data[index + 38]<<8 | data[index + 37])))/10000 ; //JY-95T imu传感器9轴,数据解析完成,具体用什么数据可以根据实际需要选用 //调试结束后可以屏蔽以下printf //printf("加速度x y z: %lf %lf %lf\n",acc_x,acc_y,acc_z); //printf("角速度x y z: %lf %lf %lf\n",gyro_x,gyro_y,gyro_z); //printf("欧拉角r p y: %lf %lf %lf %lf\n",angle_r,angle_p,angle_y); //printf("imu温度计 %lf\n",temp); //printf("磁力计x y z: %lf %lf %lf\n",mag_x, mag_y,mag_z); //printf("四元数Q0 Q1 Q2 Q3: %lf %lf %lf %lf\n",quat_Q0,quat_Q1,quat_Q2,quat_Q3); /* Author: Mao Haiqing Time: 2023.7.6 description: 读取IMU数据 */ usLength = usLength - 40; index += 40; } } };
CMakeLists.txt
cmake_minimum_required(VERSION 3.5) project(imu_get_cpp) # Default to C99 if(NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 99) endif() # Default to C++14 if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(serial REQUIRED) find_package(sensor_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) #find_package(geometry_msgs) find_package(nav_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_msgs REQUIRED) find_package(tf2_geometry_msgs REQUIRED) # uncomment the following section in order to fill in # further dependencies manually. # find_package(<dependency> REQUIRED) add_executable(publisher_imu_node src/publisher_imu.cpp src/transform.hpp) ament_target_dependencies(publisher_imu_node rclcpp std_msgs) ament_target_dependencies(publisher_imu_node rclcpp serial) ament_target_dependencies(publisher_imu_node rclcpp sensor_msgs) ament_target_dependencies(publisher_imu_node rclcpp rosidl_default_generators) ament_target_dependencies(publisher_imu_node rclcpp nav_msgs) ament_target_dependencies(publisher_imu_node rclcpp tf2_ros) ament_target_dependencies(publisher_imu_node rclcpp tf2_msgs) ament_target_dependencies(publisher_imu_node rclcpp tf2_geometry_msgs) install(TARGETS publisher_imu_node DESTINATION lib/${PROJECT_NAME}) install( DIRECTORY launch DESTINATION share/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights # uncomment the line when a copyright and license is not present in all source files #set(ament_cmake_copyright_FOUND TRUE) # the following line skips cpplint (only works in a git repo) # uncomment the line when this package is not in a git repo #set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() ament_package()
package.xml
<?xml version="1.0"?> <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> <package format="3"> <name>imu_get_cpp</name> <version>0.0.0</version> <description>TODO: Package description</description> <maintainer email="330406269@qq.com">m</maintainer> <license>TODO: License declaration</license> <buildtool_depend>ament_cmake</buildtool_depend> <depend>rclcpp</depend> <depend>std_msgs</depend> <depend>serial</depend> <depend>sensor_msgs</depend> <depend>nav_msgs</depend> <depend>tf2_ros</depend> <depend>tf2_msgs</depend> <depend>tf2_geometry_msgs</depend> <depend>geometry_msgs</depend> <test_depend>ament_lint_auto</test_depend> <test_depend>ament_lint_common</test_depend> <export> <build_type>ament_cmake</build_type> </export> </package>
编译
colcon build --packages-select imu_get_cpp
运行
ros2 launch imu_get_cpp imu_cpp.launch.py
查看话题
ros2 topic list
/clicked_point
/goal_pose
/imu_data
/initialpose
/parameter_events
/rosout
/tf
/tf_staticros2 topic echo /imu_data
header:
stamp:
sec: 1693478860
nanosec: 800160028
frame_id: imu_link
orientation:
x: 0.0866
y: -0.0067
z: 0.0078
w: -1.636
orientation_covariance:- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
angular_velocity:
x: 0.0010642251355013552
y: 0.0010642251355013552
z: 0.0010642251355013552
angular_velocity_covariance: - 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
linear_acceleration:
x: 0.08134765625000001
y: -0.09091796875000001
z: 9.866992187500001
linear_acceleration_covariance: - 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
数据正常,运行成功。
-
RE: ros2机器人foxy版用笔记本摄像头跑单目orb_slam3
@tianrunyang19 如果要想向前走下去,cmake 编译是一道绕不过的坎,花一个多星期百度找教程系统学一下吧,要不然后面没法玩,每个文件都需要编译。其实编译无非就是一个找头文件,找库文件的过程,
要根据提示看看缺什么文件,然后用
sudo find / -name 文件名 //在整个电脑内查找文件
如果有这个文件,头文件放在/usr/include,库文件放在/usr/lib(简单粗暴,只适合新手),如果没有这个文件要想办法得到这个文件。 -
RE: ros2机器人foxy版用笔记本摄像头跑单目orb_slam3
@2543298600 是用的robot_localization融合的吗?我用实体机器人跑的rviz2里面机器人模型是一直在闪,你试着改一下robot_localization 配置文件:
Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
publish_tf: false
机器人差速模型发布tf robot_localization 也发布tf 我觉得是同一个topic 2个发布者会有冲突,我把 publish_tf: false 就不闪了,不知道和你遇到的是不是同一个问题
-
RE: ros2机器人foxy版用笔记本摄像头跑单目orb_slam3
@3532206791 用笔记本的摄像头和外接的奥比中光摄像头我都试过,挺流畅的,只不过开源程序都是不成熟的程序,会有其他方面的问题,距离应用还有十万八千里。
-
RE: ros2机器人foxy版用笔记本摄像头跑单目orb_slam3
@3532206791 你让仿真的小车移动一下,也许就会出来图像,我没有跑过仿真的,但是用实体的摄像头跑单目跑到你这个地方也卡了我很久,有一次无意中移动了一下摄像头图像就出来了。他计算第一帧和第二帧图像,如果摄像头没动,第一帧和第二帧图像是同一个图像,初始化不了,不知道你碰到的问题和我曾经碰到的问题是否一样。
-
RE: ros2机器人foxy版用笔记本摄像头跑单目orb_slam3
@1157354382 m@ubun:~$ ros2 run orbslam3 mono ~/ORB_SLAM3/Vocabulary/ORBvoc.txt ~/ORB_SLAM3/Examples/Monocular/RealSense_D435i.yaml
ORB-SLAM3 Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
ORB-SLAM2 Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
This program comes with ABSOLUTELY NO WARRANTY;
This is free software, and you are welcome to redistribute it
under certain conditions. See LICENSE.txt.Input sensor was set to: Monocular
Loading settings from /home/m/ORB_SLAM3/Examples/Monocular/RealSense_D435i.yaml
Camera1.k3 optional parameter does not exist...
-Loaded camera 1
Camera.newHeight optional parameter does not exist...
Camera.newWidth optional parameter does not exist...
-Loaded image info
-Loaded ORB settings
Viewer.imageViewScale optional parameter does not exist...
-Loaded viewer settings
System.LoadAtlasFromFile optional parameter does not exist...
System.SaveAtlasToFile optional parameter does not exist...
-Loaded Atlas settings
System.thFarPoints optional parameter does not exist...
-Loaded misc parametersSLAM settings:
-Camera 1 parameters (Pinhole): [ 382.613 382.613 320.183 236.455 ]
-Camera 1 distortion parameters: [ 0 0 0 0 ]
-Original image size: [ 640 , 480 ]
-Current image size: [ 640 , 480 ]
-Sequence FPS: 30
-Features per image: 1250
-ORB scale factor: 1.2
-ORB number of scales: 8
-Initial FAST threshold: 20
-Min FAST threshold: 7Loading ORB Vocabulary. This could take a while...
Vocabulary loaded!Initialization of Atlas from scratch
Creation of new map with id: 0
Creation of new map with last KF id: 0
Seq. Name:
There are 1 cameras in the atlas
Camera 0 is pinhole
slam changedStarting the Viewer
你的下一步该加载摄像头配置了,看看你的摄像头配置文件路径对吗? -
RE: ros2机器人foxy版用笔记本摄像头跑单目orb_slam3
@1157354382 你这是跑的单目的程序,需要启动摄像头的,你的主程序已经启动,正在等你的摄像头数据,如果你的摄像头已经启动了,那要检查摄像头发布的话题和你的主程序接收的话题是否一致
ros2 topic list 查看话题