pi/openvins_linux
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

gps fusion implemented

Browse Source
This commit is contained in:
admin1
2022-08-28 22:25:47 +03:00
parent b8ee6672d1
commit 979c7a2250
27 changed files with 132151 additions and 110 deletions
Download Patch File Download Diff File Expand all files Collapse all files

203
global_fusion/src/globalOptNode.cpp Normal file
View File

@@ -0,0 +1,203 @@
//
// Created by admin1 on 25.08.2022.
//
/*******************************************************
* Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology
*
* This file is part of VINS.
*
* Licensed under the GNU General Public License v3.0;
* you may not use this file except in compliance with the License.
*
* Author: Qin Tong (qintonguav@gmail.com)
*******************************************************/
#include "ros/ros.h"
#include "globalOpt.h"
#include <sensor_msgs/NavSatFix.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>
#include <iostream>
#include <stdio.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <fstream>
#include <queue>
#include <mutex>
#include <math.h>
GlobalOptimization globalEstimator;
ros::Publisher pub_global_odometry, pub_global_path, pub_car, pub_imu_tf_global;
nav_msgs::Path *global_path;
// we need to use pointer becuase otherwise TransformBroadcaster will be initializing here, before
// ros::init() called
std::shared_ptr<tf::TransformBroadcaster> TfBr;
double last_vio_t = -1;
std::queue<sensor_msgs::NavSatFixConstPtr> gpsQueue;
std::mutex m_buf;
void publish_car_model(double t, Eigen::Vector3d t_w_car, Eigen::Quaterniond q_w_car)
{
visualization_msgs::MarkerArray markerArray_msg;
visualization_msgs::Marker car_mesh;
car_mesh.header.stamp = ros::Time(t);
car_mesh.header.frame_id = "gps_global";
car_mesh.type = visualization_msgs::Marker::MESH_RESOURCE;
car_mesh.action = visualization_msgs::Marker::ADD;
car_mesh.id = 0;
car_mesh.mesh_resource = "package://global_fusion/models/car.dae";
Eigen::Matrix3d rot;
rot << 0, 0, -1, 0, -1, 0, -1, 0, 0;
Eigen::Quaterniond Q;
Q = q_w_car * rot;
car_mesh.pose.position.x = t_w_car.x();
car_mesh.pose.position.y = t_w_car.y();
car_mesh.pose.position.z = t_w_car.z();
car_mesh.pose.orientation.w = Q.w();
car_mesh.pose.orientation.x = Q.x();
car_mesh.pose.orientation.y = Q.y();
car_mesh.pose.orientation.z = Q.z();
car_mesh.color.a = 1.0;
car_mesh.color.r = 1.0;
car_mesh.color.g = 0.0;
car_mesh.color.b = 0.0;
float major_scale = 2.0;
car_mesh.scale.x = major_scale;
car_mesh.scale.y = major_scale;
car_mesh.scale.z = major_scale;
markerArray_msg.markers.push_back(car_mesh);
pub_car.publish(markerArray_msg);
}
void GPS_callback(const sensor_msgs::NavSatFixConstPtr &GPS_msg)
{
//printf("gps_callback! \n");
m_buf.lock();
gpsQueue.push(GPS_msg);
m_buf.unlock();
}
void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
{
//printf("vio_callback! \n");
double t = pose_msg->header.stamp.toSec();
last_vio_t = t;
Eigen::Vector3d vio_t(pose_msg->pose.pose.position.x, pose_msg->pose.pose.position.y, pose_msg->pose.pose.position.z);
Eigen::Quaterniond vio_q;
vio_q.w() = pose_msg->pose.pose.orientation.w;
vio_q.x() = pose_msg->pose.pose.orientation.x;
vio_q.y() = pose_msg->pose.pose.orientation.y;
vio_q.z() = pose_msg->pose.pose.orientation.z;
globalEstimator.inputOdom(t, vio_t, vio_q);
m_buf.lock();
while(!gpsQueue.empty())
{
sensor_msgs::NavSatFixConstPtr GPS_msg = gpsQueue.front();
double gps_t = GPS_msg->header.stamp.toSec();
printf("vio t: %f, gps t: %f \n", t, gps_t);
// 10ms sync tolerance
// PI: Let's change the sync tolerance from 10 ms to 100 ms
// if(gps_t >= t - 0.01 && gps_t <= t + 0.01)
if(gps_t >= t - 0.1 && gps_t <= t + 0.1)
{
//printf("receive GPS with timestamp %f\n", GPS_msg->header.stamp.toSec());
double latitude = GPS_msg->latitude;
double longitude = GPS_msg->longitude;
double altitude = GPS_msg->altitude;
//int numSats = GPS_msg->status.service;
// it was made for KITTI dataset before, which only gives the one parameter. But here we have 3 variances
// (the other values in the covariance matrix are 0).
double pos_accuracy;
pos_accuracy = std::sqrt( pow(GPS_msg->position_covariance[0], 2.0) + pow(GPS_msg->position_covariance[4], 2) + pow(GPS_msg->position_covariance[8], 2) );
if(pos_accuracy <= 0)
pos_accuracy = 1;
//printf("receive covariance %lf \n", pos_accuracy);
//if(GPS_msg->status.status > 8)
globalEstimator.inputGPS(t, latitude, longitude, altitude, pos_accuracy);
gpsQueue.pop();
break;
}
else if(gps_t < t - 0.01)
gpsQueue.pop();
else if(gps_t > t + 0.01)
break;
}
m_buf.unlock();
Eigen::Vector3d global_t;
Eigen:: Quaterniond global_q;
globalEstimator.getGlobalOdom(global_t, global_q);
nav_msgs::Odometry odometry;
odometry.header = pose_msg->header;
odometry.header.frame_id = "gps_global";
odometry.child_frame_id = "cam0_gps";
odometry.pose.pose.position.x = global_t.x();
odometry.pose.pose.position.y = global_t.y();
odometry.pose.pose.position.z = global_t.z();
odometry.pose.pose.orientation.x = global_q.x();
odometry.pose.pose.orientation.y = global_q.y();
odometry.pose.pose.orientation.z = global_q.z();
odometry.pose.pose.orientation.w = global_q.w();
pub_global_odometry.publish(odometry);
pub_global_path.publish(*global_path);
publish_car_model(t, global_t, global_q);
// Publish the tf so that Camera can bind to it.
tf::StampedTransform trans;
trans.frame_id_ = "gps_global";
trans.child_frame_id_ = "cam0_gps";
trans.stamp_ = pose_msg->header.stamp;
tf::Quaternion quat(global_q.x(), global_q.y(), global_q.z(), global_q.w());
trans.setRotation(quat);
tf::Vector3 orig(global_t.x(), global_t.y(), global_t.z());
trans.setOrigin(orig);
TfBr->sendTransform(trans);
// pub_imu_tf_global.publish(trans);
// write result to file
std::ofstream foutC("/home/admin1/workspace/catkin_ws_ov/src/open_vins/global_fusion/data/vio_global.csv", ios::app);
foutC.setf(ios::fixed, ios::floatfield);
foutC.precision(0);
foutC << pose_msg->header.stamp.toSec() * 1e9 << ",";
foutC.precision(5);
foutC << global_t.x() << ","
<< global_t.y() << ","
<< global_t.z() << ","
<< global_q.w() << ","
<< global_q.x() << ","
<< global_q.y() << ","
<< global_q.z() << endl;
foutC.close();
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "globalEstimator");
ros::NodeHandle n("~");
global_path = &globalEstimator.global_path;
TfBr = std::make_shared<tf::TransformBroadcaster>();
ros::Subscriber sub_GPS = n.subscribe("/gps", 100, GPS_callback);
ros::Subscriber sub_vio = n.subscribe("/vins_estimator/odometry", 100, vio_callback);
pub_global_path = n.advertise<nav_msgs::Path>("global_path", 100);
pub_global_odometry = n.advertise<nav_msgs::Odometry>("global_odometry", 100);
// pub_imu_tf_global = n.advertise<tf::StampedTransform>("tf_imu_global", 100);
pub_car = n.advertise<visualization_msgs::MarkerArray>("car_model", 1000);
ros::spin();
return 0;
}