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

terminal commit

Browse Source
This commit is contained in:
admin1
2022-08-30 18:44:20 +03:00
parent 979c7a2250
commit 89f25ad747
8 changed files with 444036 additions and 154 deletions
Download Patch File Download Diff File Expand all files Collapse all files

66
global_fusion/src/globalOptNode.cpp
View File

@@ -16,6 +16,7 @@
#include "ros/ros.h"
#include "globalOpt.h"
#include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/Image.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <tf/transform_datatypes.h>
@@ -99,6 +100,39 @@ void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
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;
// auto cov = pose_msg->pose.covariance;
// auto rCovArr = pose_msg->twist.covariance;
// auto tCovArr = pose_msg->pose.covariance;
// convert boost::array to std::array
// std::vector<double> transCov(9);
// std::vector<double> rotCov(9);
// std::vector<double> tCov(tCovArr.size());
// Presuming that the first 3 block shows the translation cov, and the rest shows rotation cov
// for (int r = 0; r < 3; r++){
// for (int c = 0; c < 3; c++){
// transCov[r*3 + c] = cov[r*6 + c];
// }
// }
//
// for (int r = 3; r < 6; r++){
// for (int c = 3; c < 6; c++){
// rotCov[(r-3)*3 + (c-3)] = cov[r*6 + c];
// }
// }
// std::cout << "The obtained below matrix from cov" << std::endl;
// // debugging. Output the below matrix
// for (int r = 0; r < 3; r++){
// for (int c = 0; c < 3; c++){
// std::cout << rotCov[r*3 + c] << " ";
// }
// std::cout << std::endl;
// }
// ROS_INFO_STREAM("the filled vector size" << rCov.size() << " the filled vector first element: " << rCov[0]);
globalEstimator.inputOdom(t, vio_t, vio_q);
@@ -144,7 +178,6 @@ void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
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();
@@ -156,18 +189,6 @@ void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
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);
@@ -184,6 +205,24 @@ void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
foutC.close();
}
void gps_img_callback(sensor_msgs::ImageConstPtr msg){
Eigen::Vector3d global_t;
Eigen:: Quaterniond global_q;
globalEstimator.getGlobalOdom(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_ = "imu_gps";
trans.stamp_ = 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);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "globalEstimator");
@@ -194,6 +233,7 @@ int main(int argc, char **argv)
ros::Subscriber sub_GPS = n.subscribe("/gps", 100, GPS_callback);
ros::Subscriber sub_vio = n.subscribe("/vins_estimator/odometry", 100, vio_callback);
ros::Subscriber sub_img = n.subscribe("/global_fusion_node/sensor_left", 100, gps_img_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);