视觉里程计的发布主函数

/**
 * @file ros_rgbd.cc
 * @author guoqing (1337841346@qq.com)
 * @brief ORB RGB-D 输入的ROS节点实现
 * @version 0.1
 * @date 2019-08-06
 *
 * @copyright Copyright (c) 2019
 *
 */

#include <iostream>
#include <algorithm>
#include <fstream>
#include <chrono>

#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <opencv2/core/core.hpp>

#include <System.h>
#include "Converter.h"

// geometry_msgs  环境配置  1:　(1)包含头文件  #include "geometry_msgs/PoseStamped.h";
//         (2)find_package(  INCLUDE_DIRS  CATKIN_DEPENDS roscpp geometry_msgs  DEPENDS);
// 这样我们才可以使用这个类型的消息
// 2: 　通过geometry_msgs::PoseStamped msg定义一个叫msg的对象，该对象可以使用上图中的header,pose两个数据成员．即你可以通过msg.header,msg.pose来调用数据类型为std_msgs/Header,geometry_msgs/Pose的两个数据成员．
#include "geometry_msgs/PoseStamped.h"
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>

using namespace std;
using namespace cv;

cv::Mat CurrentTcw;
cv::Mat LastTcw;

class ImageGrabber
{
public:
    ImageGrabber(ORB_SLAM2::System *pSLAM) : mpSLAM(pSLAM) {}

    void GrabRGBD(const sensor_msgs::ImageConstPtr &msgRGB, const sensor_msgs::ImageConstPtr &msgD);

    ORB_SLAM2::System *mpSLAM;
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "RGBD");
    ros::start();

    if (argc != 3)
    {
        cerr << endl
             << "Usage: rosrun ORB_SLAM2 RGBD path_to_vocabulary path_to_settings" << endl;
        ros::shutdown();
        return 1;
    }

    // Create SLAM system. It initializes all system threads and gets ready to process frames.
    ORB_SLAM2::System SLAM(argv[1], argv[2], true);

    ImageGrabber igb(&SLAM);

    ros::NodeHandle nh;

    message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/color/image_raw", 1);
    message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "/camera/aligned_depth_to_color/image_raw", 1);
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> sync_pol;
    message_filters::Synchronizer<sync_pol> sync(sync_pol(10), rgb_sub, depth_sub);
    sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD, &igb, _1, _2));

    //*********计算并且发布 Pub_Tcw*********
    // cout << "left and right 方向的平移  Twc.at<float>( 0,3 ) ==  " << mCurrentFrame.mTcw.at<float>(0, 3) << endl;
    //             cout << " up and down 方向的平移 Twc.at<float> ( 1,3 )==  " << mCurrentFrame.mTcw.at<float>(1, 3) << endl;
    //             cout << " ZZZZZZZZZZZZZZZZZ方向的平移 Twc.at<float> ( 2,3 ) ZZ==  " << mCurrentFrame.mTcw.at<float>(2, 3)*5 <<"米"<< endl;

    ///******************************************
    ros::Publisher odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 50);
    tf::TransformBroadcaster odom_broadcaster;

    double scale = 5;                  // ORb中计算数值到米制单位的尺度
    LastTcw = CurrentTcw;              //计算vx,vy,v_yaw
    ros::Time current_time, last_time; //#转为s：ros::Time t_s = t.toSec();
    current_time = ros::Time::now();
    last_time = ros::Time::now();

    ros::Rate r(14.0); //用于固定频率循环的类，作用是使用1Hz的频率发布odom消息，当然，在实际系统中，往往需要更快的速度进行发布。
                       // Rate类对象的工作原理是，从上一次sleep结束后到下一次sleep开始时，如果经过的时间没有超过指定的频率时间，就通过sleep到指定频率时间。
                       // 比如指定Rate为10Hz（100ms），程序一次循环只用了10ms，那么sleep就会休眠90ms，然后进入下一次循环。
                       // 因此如果循环程序运行的时间大于指定Rate，那么这个Rate就失效了。
    const double vo_pose_covariance[36] = {1e-3, 0, 0, 0, 0, 0,
                                           0, 1e-3, 0, 0, 0, 0,
                                           0, 0, 1e6, 0, 0, 0,
                                           0, 0, 0, 1e6, 0, 0,
                                           0, 0, 0, 0, 1e6, 0,
                                           0, 0, 0, 0, 0, 1e3};
    const double vo_pose_covariance2[36] = {1e-9, 0, 0, 0, 0, 0,
                                            0, 1e-3, 1e-9, 0, 0, 0,
                                            0, 0, 1e6, 0, 0, 0,
                                            0, 0, 0, 1e6, 0, 0,
                                            0, 0, 0, 0, 1e6, 0,
                                            0, 0, 0, 0, 0, 1e-9};

    const double vo_twist_covariance[36] = {1e-3, 0, 0, 0, 0, 0,
                                            0, 1e-3, 0, 0, 0, 0,
                                            0, 0, 1e6, 0, 0, 0,
                                            0, 0, 0, 1e6, 0, 0,
                                            0, 0, 0, 0, 1e6, 0,
                                            0, 0, 0, 0, 0, 1e3};
    const double vo_twist_covariance2[36] = {1e-9, 0, 0, 0, 0, 0,
                                             0, 1e-3, 1e-9, 0, 0, 0,
                                             0, 0, 1e6, 0, 0, 0,
                                             0, 0, 0, 1e6, 0, 0,
                                             0, 0, 0, 0, 1e6, 0,
                                             0, 0, 0, 0, 0, 1e-9};
    // 我们设置机器人的默认前进速度，这三个参数在循环中更新发布出去
    double vx = 0.0;
    double vy = -0.0;
    double v_yaw = 0.0;
    double dx = 0.0;
    double dy = -0.0;
    double d_yaw = 0.0;
    double dt = 0.0;
    double current_yaw = 0.0;
    double last_yaw = 0.0;
    double current_x = 0.0;
    double last_x = 0.0;
    double current_y = 0.0;
    double last_y = 0.0;
    while (ros::ok()) //消息发布器在一个while里面一直循环发送
    {
        if (CurrentTcw.size().height > 0)
        {
            cv::Mat Pub_Rwc = CurrentTcw.rowRange(0, 3).colRange(0, 3).t();
            //以及平移向量
            cv::Mat Pub_twc = -Pub_Rwc * CurrentTcw.rowRange(0, 3).col(3);
            //用四元数表示旋转： vector<float> q = Converter::toQuaternion(Rwc);  //ORB中写成函数了，这里不用函数，更直白一些
            // Rotation_toMatrix3d
            Eigen::Matrix<double, 3, 3> eigMat;

            eigMat << Pub_Rwc.at<float>(0, 0), Pub_Rwc.at<float>(0, 1), Pub_Rwc.at<float>(0, 2),
                Pub_Rwc.at<float>(1, 0), Pub_Rwc.at<float>(1, 1), Pub_Rwc.at<float>(1, 2),
                Pub_Rwc.at<float>(2, 0), Pub_Rwc.at<float>(2, 1), Pub_Rwc.at<float>(2, 2);

            Eigen::Quaterniond q(eigMat);
            float q0 = q.w(); // ORB中的四元数计算结束
            float q1 = q.x();
            float q2 = q.y();
            float q3 = q.z();

            //欧拉角计算：原文链接：https://blog.csdn.net/xiaoma_bk/article/details/79082629
            double sinr_cosp = 2 * (q0 * q1 + q2 * q3);
            double cosr_cosp = 1 - 2 * (q0 * q0 + q1 * q1);

            float Roll_x_forward = atan2(sinr_cosp, cosr_cosp) * 57.3; // roll

            // pitch (y-axis rotation)
            double sinp = 2 * (q0 * q2 - q1 * q3);
            if (std::abs(sinp) >= 1)
                float Pitch_y_left = std::copysign(M_PI / 2, sinp) * 57.3; // use 90 degrees if out of range
            else
                float Pitch_y_left = std::asin(sinp) * 57.3;

            // yaw (z-axis rotation)
            double siny_cosp = 2 * (q1 * q2 + q0 * q3);
            double cosy_cosp = 1 - 2 * (q2 * q2 + q3 * q3);
            float Yaw_z_up = std::atan2(siny_cosp, cosy_cosp) * 57.3;
            current_yaw = (Yaw_z_up / 57.3); //弧度制
            current_time = ros::Time::now();
            current_x = CurrentTcw.at<float>(2, 3) * scale;
            last_x = LastTcw.at<float>(2, 3) * scale;

            current_y = CurrentTcw.at<float>(0, 3) * scale;
            last_y = LastTcw.at<float>(0, 3) * scale;
            cout << "main()   YYYYYYYYYYYYY，向左为正方向的平移  Twc.at<float>( 0,3 ) ==  " << CurrentTcw.at<float>(0, 3) * scale << "米" << endl;
            cout << " main() ZZZZZZZZZZZZZZZ，向上为正 方向，Z的平移 Twc.at<float> ( 1,3 )ZZ==  " << CurrentTcw.at<float>(1, 3) * scale << "米" << endl;
            cout << "main()  XXXXXXXXXXXXXXX,向后为正方向，X的平移 Twc.at<float> ( 2,3 ) XX==  " << current_x << "米" << endl;
            cout << "main()  偏航角（围绕Z_up旋转），yaw==  " << Yaw_z_up << "度" << endl;

            // 3.使用速度信息，来计算并更新里程计的信息，包括单位时间内机器人在x轴、y轴的坐标变化和角度的变化。在实际系统中，需要更具里程计的实际信息进行更新。
            //  nav_msgs  环境配置  1:　(1)包含头文件  #include <tf/transform_broadcaster.h>;
            //                                                                                    #include <nav_msgs/Odometry.h>;
            //                                             2.            find_package(   nav_msgs  );

            //  参考教程：https://blog.csdn.net/datase/article/details/80535710    写nav_msgs的发布者
            //  1.  首先需要包含相关的头文件。   #include <tf/transform_broadcaster.h>
            // #include <nav_msgs/Odometry.h>
            // 2.定义一个消息发布者来发布“odom”消息，在定义一个tf广播，来发布tf变换信息。
            // since all odometry is 6DOF we'll need a quaternion created from yaw
            //为了兼容二维和三维的功能包，让消息结构更加通用，里程计的偏航角需要转换成四元数才能发布，使用ROS为我们提供了偏航角与四元数相互转换的功能。
            geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(current_yaw); //         //在odom参考系下角度是弧度制
            //创建一个tf发布需要使用的TransformStamped类型消息，然后根据消息结构填充当前的时间戳、参考系id、子参考系id，注意两个参考系的id必须要是“odom”和“base_link”。
            geometry_msgs::TransformStamped odom_trans;
            odom_trans.header.stamp = current_time;
            odom_trans.header.frame_id = "odom";
            odom_trans.child_frame_id = "camera";

            // NOTE: tf中的坐标系是右手坐标系，x_forward , y_left , z_up
            //在odom参考系下以x,y的单位都是m。
            odom_trans.transform.translation.x = current_x;
            odom_trans.transform.translation.y = current_y;
            odom_trans.transform.translation.z = 0.0;
            odom_trans.transform.rotation = odom_quat;

            // send the transform
            odom_broadcaster.sendTransform(odom_trans);

            //我们还要发布nav_msgs/Odometry消息，让导航包获取机器人的速度。创建消息变量，然后填充时间戳
            nav_msgs::Odometry odom;
            odom.header.stamp = current_time;
            odom.header.frame_id = "odom";
            //  填充机器人的位置、速度，然后发布消息。注意，我们发布的是机器人本体的信息，所以参考系需要填"base_link"。
            dt = (current_time - last_time).toSec();
            // cout << "Track()   YYYYYYYYYYYYY，向左为正方向的平移  Twc.at<float>( 0,3 ) ==  " << CurrentTcw.at<float>(0, 3) << endl;
            //     cout << " Track() ZZZZZZZZZZZZZZZ，向上为正 方向，Z的平移 Twc.at<float> ( 1,3 )ZZ==  " << CurrentTcw.at<float>(1, 3) << endl;
            //     cout << "Track()  XXXXXXXXXXXXXXX,向后为正方向，X的平移 Twc.at<float> ( 2,3 ) XX==  " << CurrentTcw.at<float>(2, 3)*5 <<"米"<< endl;

            dx = (CurrentTcw.at<float>(2, 3) - LastTcw.at<float>(2, 3)) * scale;
            dy = (CurrentTcw.at<float>(0, 3) - LastTcw.at<float>(0, 3)) * scale;
            d_yaw = current_yaw - last_yaw;

            vx = dx / dt;                          //     米/s
            vy = dy / dt;                          //     米/s
            v_yaw = d_yaw / dt;                    //     rad/s
            odom.pose.pose.position.x = current_x; //需要
            odom.pose.pose.position.y = current_y; //需要
            odom.pose.pose.position.z = 0.0;
            odom.pose.pose.orientation = odom_quat;

            odom.child_frame_id = "base_link";
            odom.twist.twist.linear.x = vx; //需要
            odom.twist.twist.linear.y = vy; //需要
            odom.twist.twist.linear.z = 0.0;
            odom.twist.twist.angular.z = v_yaw; //需要
            odom.twist.twist.angular.y = 0.0;
            odom.twist.twist.angular.x = 0.0;
            // //给里程计加上协方差 //参考：https://blog.csdn.net/buffei58/article/details/105617693
            if (vx != 0 && vy != 0 && v_yaw != 0) //如果小车velocity非零，考虑到运动中编码器可能带来的滑动误差，认为IMu的数据更可靠
            {

                memcpy(&odom.pose.covariance, vo_pose_covariance, sizeof(vo_pose_covariance)),
                    memcpy(&odom.twist.covariance, vo_twist_covariance, sizeof(vo_twist_covariance));
            }
            else 如果velocity是零，说明编码器的误差会比较小，认为编码器数据更可靠
            {
                memcpy(&odom.pose.covariance, vo_pose_covariance2, sizeof(vo_pose_covariance2)),
                    memcpy(&odom.twist.covariance, vo_twist_covariance2, sizeof(vo_twist_covariance2));
            }
            // publish the message
            odom_pub.publish(odom);
        } // if

        ros::spinOnce(); // 当spinOnce函数被调用时，spinOnce就会调用回调函数队列中第一个callback函数，此时callback函数被执行。
                         // spinOnce函数执行一次后，接着执行下面的语句。 等到下次spinOnce函数又被调用时，回调函数队列中第二个callback函数就会被调用，以此类推。

        last_time = current_time;
        LastTcw = CurrentTcw;
        last_yaw = current_yaw;

        typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> sync_pol; //时间戳相近
        message_filters::Synchronizer<sync_pol> sync(sync_pol(10), rgb_sub, depth_sub);
        sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD, &igb, _1, _2));

        r.sleep();
    }
    ///***********************

    // ros::spin();  //原来的，现在换成    while + ros::spinOnce()

    // Stop all threads
    SLAM.Shutdown();

    // Save camera trajectory
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");

    ros::shutdown();

    return 0;
}

void ImageGrabber::GrabRGBD(const sensor_msgs::ImageConstPtr &msgRGB, const sensor_msgs::ImageConstPtr &msgD)
{
    // Copy the ros image message to cv::Mat.
    cv_bridge::CvImageConstPtr cv_ptrRGB;
    try
    {
        cv_ptrRGB = cv_bridge::toCvShare(msgRGB);
    }
    catch (cv_bridge::Exception &e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    cv_bridge::CvImageConstPtr cv_ptrD;
    try
    {
        cv_ptrD = cv_bridge::toCvShare(msgD);
    }
    catch (cv_bridge::Exception &e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    // mpSLAM->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec());
    // Pass the image to the SLAM system
    CurrentTcw = mpSLAM->Track(cv_ptrRGB->image, cv_ptrD->image, cv_ptrRGB->header.stamp.toSec());
}