接收实时速度话题,发布odom话题
-
我买的机器人底盘轮毂电机控制板输出的是实时的速度话题,导航需要odom话题,经过2个多月的c++学习,终于大概看懂了底层代码的意思,用别的版本的代码拼凑了一个速度转odom的代码,让我写我还是写不出来(本人菜鸟一个,哈哈),只能抄别人的代码修改一下.
pub_odom.cpp
#include "rclcpp/rclcpp.hpp" #include <rclcpp/time.hpp> #include <math.h> #include <riki_msgs/msg/velocities.hpp> #include <nav_msgs/msg/odometry.hpp> #include <geometry_msgs/msg/twist.hpp> #include <tf2_ros/transform_broadcaster.h> #include <tf2/LinearMath/Quaternion.h> #include <riki_msgs/msg/battery.hpp> #include <sensor_msgs/msg/imu.hpp> using std::placeholders::_1; class Pub_odom : public rclcpp::Node { public: Pub_odom(std::string name) : Node(name), imu_angular_z_(0.0), odom_theta_(0) { //创建一个发布者用于发布odom odom_publisher_ = this->create_publisher<nav_msgs::msg::Odometry>("odom", 20); //创建一个订阅者用于订阅raw_vel话题 velocity_sub_ = this->create_subscription<riki_msgs::msg::Velocities>("raw_vel", 20, std::bind(&Pub_odom::velCallback, this, _1)); //创建一个订阅者用于订阅电池电量话题 battery_sub_ = this->create_subscription<riki_msgs::msg::Battery>("battery", 1, std::bind(&Pub_odom::batteryCallback, this, _1)); //创建一个订阅者用于订阅imu话题 imu_sub_ = this->create_subscription<sensor_msgs::msg::Imu>("imu/data", 1, std::bind(&Pub_odom::ImuCallback, this, _1)); odom_transform_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this); declare_parameter("linear_scale", 1.0); get_parameter("linear_scale", linear_scale_); declare_parameter("low_battery", 34.0); get_parameter("low_battery", low_battery_); last_odom_time_ = now(); odom_.header.frame_id = "odom"; odom_.child_frame_id = "base_footprint"; odom_transform_.header.frame_id = "odom"; odom_transform_.child_frame_id = "base_footprint"; RCLCPP_INFO(this->get_logger(), "linear_scale_: %f", linear_scale_); } ~Pub_odom() { odom_transform_broadcaster_.reset(); velocity_sub_.reset(); odom_publisher_.reset(); battery_sub_.reset(); } private: //声明一个发布者(成员变量),用于发布话题 rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_publisher_; //声明一个订阅者(成员变量),用于订阅话题 rclcpp::Subscription<riki_msgs::msg::Velocities>::SharedPtr velocity_sub_; //声明一个订阅者(成员变量),用于订阅电池电量话题 rclcpp::Subscription<riki_msgs::msg::Battery>::SharedPtr battery_sub_; //声明一个imu订阅者(成员变量),订阅imu话题 rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_; std::shared_ptr<tf2_ros::TransformBroadcaster> odom_transform_broadcaster_; nav_msgs::msg::Odometry odom_; geometry_msgs::msg::TransformStamped odom_transform_; void euler_to_quat(float roll, float pitch, float yaw, float *q) { float cy = cos(yaw * 0.5); float sy = sin(yaw * 0.5); float cp = cos(pitch * 0.5); float sp = sin(pitch * 0.5); float cr = cos(roll * 0.5); float sr = sin(roll * 0.5); q[0] = cy * cp * cr + sy * sp * sr; q[1] = cy * cp * sr - sy * sp * cr; q[2] = sy * cp * sr + cy * sp * cr; q[3] = sy * cp * cr - cy * sp * sr; } void ImuCallback(const sensor_msgs::msg::Imu::SharedPtr imu) { imu_angular_z_ = imu->angular_velocity.z; } void batteryCallback(const riki_msgs::msg::Battery::SharedPtr bat) { float cur_battery = bat->battery; if (cur_battery < low_battery_) { RCLCPP_WARN(this->get_logger(), "Current Voltage: %f ,Low Battery, Low Battery Please Recharge!!", cur_battery); } } //收到话题数据的回调函数 void velCallback(const riki_msgs::msg::Velocities::SharedPtr vel) { rclcpp::Time current_time = now(); //x线速度 linear_velocity_x_ = vel->linear_x * linear_scale_; //y线速度 linear_velocity_y_ = vel->linear_y * linear_scale_; //角速度 angular_velocity_z_ = vel->angular_z; auto 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_) - linear_velocity_y_ * sin(odom_theta_)) * vel_dt_.nanoseconds() / 1e9; //m double delta_y = (linear_velocity_x_ * sin(odom_theta_) + linear_velocity_y_ * cos(odom_theta_)) * vel_dt_.nanoseconds() / 1e9; //m //calculate current position of the robot x_pos_ += delta_x; y_pos_ += delta_y; odom_theta_ += delta_heading; //以四元数计算机器人航向 //ROS具有以四元数角计算偏航的功能 float q[4]; euler_to_quat(0, 0, odom_theta_, q); 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 = (double)q[1]; odom_.pose.pose.orientation.y = (double)q[2]; odom_.pose.pose.orientation.z = (double)q[3]; odom_.pose.pose.orientation.w = (double)q[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_.twist.twist.linear.x = linear_velocity_x_; odom_.twist.twist.linear.y = linear_velocity_y_; //odom_.twist.twist.angular.z = angular_velocity_z_; //odom_.twist.twist.angular.z = imu_angular_z_; odom_.pose.covariance[0] = 0.001; odom_.pose.covariance[7] = 0.001; odom_.pose.covariance[35] = 0.001; odom_.header.stamp = now(); odom_publisher_->publish(odom_); 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_); } float linear_scale_; float low_battery_; float steering_angle_; float linear_velocity_x_; float linear_velocity_y_; float angular_velocity_z_; float imu_angular_z_; rclcpp::Time last_odom_time_; float vel_dt_; float x_pos_; float y_pos_; float odom_theta_; }; int main(int argc, char const *argv[]) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared<Pub_odom>("pub_odom")); rclcpp::shutdown(); return 0; } -
odom 话题有了,激光雷达的话题也有了,接下来要把他们组合在一起了,有没有志同道合的一起玩呀