接收实时速度话题,发布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 话题有了,激光雷达的话题也有了,接下来要把他们组合在一起了,有没有志同道合的一起玩呀