Skip to content

Commit

Permalink
added baseling loading to player
Browse files Browse the repository at this point in the history
  • Loading branch information
@XiaotaoGuo
XiaotaoGuo committed Dec 6, 2021
1 parent 301816c commit 368c602
Show file tree
Hide file tree
Showing 2 changed files with 112 additions and 1 deletion.
104 changes: 103 additions & 1 deletion src/ROSThread.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,6 +47,7 @@ ROSThread::~ROSThread()
sick_middle_thread_.active_ = false;
stereo_thread_.active_ = false;
omni_thread_.active_ = false;
baseline_odom_thread_.active_ = false;
usleep(100000);

data_stamp_thread_.cv_.notify_all();
Expand Down Expand Up @@ -87,6 +88,9 @@ ROSThread::~ROSThread()

omni_thread_.cv_.notify_all();
if(omni_thread_.thread_.joinable()) omni_thread_.thread_.join();

baseline_odom_thread_.cv_.notify_all();
if(baseline_odom_thread_.thread_.joinable()) baseline_odom_thread_.thread_.join();
}

void ROSThread::ros_initialize(ros::NodeHandle &n)
Expand Down Expand Up @@ -130,6 +134,8 @@ void ROSThread::ros_initialize(ros::NodeHandle &n)
// omni3_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>("/occam_node/image3/camera_info", 10);
// omni4_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>("/occam_node/image4/camera_info", 10);

baseline_odom_pub_ = nh_.advertise<nav_msgs::Odometry>("/baseline/odom", 1000);

clock_pub_ = nh_.advertise<rosgraph_msgs::Clock>("/clock", 1);

}
Expand Down Expand Up @@ -184,6 +190,10 @@ void ROSThread::Ready()
omni_thread_.cv_.notify_all();
if(omni_thread_.thread_.joinable()) omni_thread_.thread_.join();

baseline_odom_thread_.active_ = false;
baseline_odom_thread_.cv_.notify_all();
if(baseline_odom_thread_.thread_.joinable()) baseline_odom_thread_.thread_.join();


//check path is right or not

Expand All @@ -206,9 +216,23 @@ void ROSThread::Ready()
// data_stamp_[stamp] = data_name;
data_stamp_.insert( multimap<int64_t, string>::value_type(stamp, data_name));
}
cout << "Stamp data are loaded" << endl;
fclose(fp);

fp = fopen((data_folder_path_+"/global_pose.csv").c_str(), "r");
double temp_data[12];
int baseline_count = 0;
while(fscanf(fp,"%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf\n",&stamp,&temp_data[0],&temp_data[1],&temp_data[2],\
&temp_data[3],&temp_data[4],&temp_data[5],\
&temp_data[6],&temp_data[7],&temp_data[8],\
&temp_data[9],&temp_data[10],&temp_data[11]) == 13){
// data_stamp_[stamp] = data_name;
data_stamp_.insert( multimap<int64_t, string>::value_type(stamp, "baseline"));
baseline_count++;
}
fclose(fp);
cout << baseline_count << " baseline data loaded" << endl;
cout << "Stamp data are loaded" << endl;

initial_data_stamp_ = data_stamp_.begin()->first - 1;
last_data_stamp_ = prev(data_stamp_.end(),1)->first - 1;

Expand Down Expand Up @@ -377,6 +401,56 @@ void ROSThread::Ready()
cout << "Encoder data are loaded" << endl;
fclose(fp);

//Read baseline data
fp = fopen((data_folder_path_+"/global_pose.csv").c_str(),"r");
double baseline_mat[12];
nav_msgs::Odometry baseline_data;
baseline_odom_data_.clear();
Eigen::Vector3f init_pos = Eigen::Vector3f::Zero();
while(fscanf(fp,"%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf\n",&stamp, &baseline_mat[0],&baseline_mat[1],&baseline_mat[2],\
&baseline_mat[3],&baseline_mat[4],&baseline_mat[5],\
&baseline_mat[6],&baseline_mat[7],&baseline_mat[8],\
&baseline_mat[9],&baseline_mat[10],&baseline_mat[11]) == 13){

Eigen::Matrix3d rot_eigen;
rot_eigen << baseline_mat[0], baseline_mat[1], baseline_mat[2],
baseline_mat[4], baseline_mat[5], baseline_mat[6],
baseline_mat[8], baseline_mat[9], baseline_mat[10];
Eigen::Quaterniond quat(rot_eigen);
quat.normalize();
geometry_msgs::Quaternion baseline_odom_quat;
baseline_odom_quat.w = quat.w();
baseline_odom_quat.x = quat.x();
baseline_odom_quat.y = quat.y();
baseline_odom_quat.z = quat.z();

baseline_data.header.stamp.fromNSec(stamp);
baseline_data.header.frame_id = "world";
baseline_data.child_frame_id = "baseline";
baseline_data.pose.pose.position.x = baseline_mat[3];
baseline_data.pose.pose.position.y = baseline_mat[7];
baseline_data.pose.pose.position.z = baseline_mat[11];
baseline_data.pose.pose.orientation = baseline_odom_quat;

if (init_pos.isZero())
{
init_pos.x() = baseline_data.pose.pose.position.x;
init_pos.y() = baseline_data.pose.pose.position.y;
init_pos.z() = baseline_data.pose.pose.position.z;
}
else
{
baseline_data.pose.pose.position.x -= init_pos.x();
baseline_data.pose.pose.position.y -= init_pos.y();
baseline_data.pose.pose.position.z -= init_pos.z();
baseline_odom_data_[stamp] = baseline_data;
}


}
cout << "Baseline data are loaded" << endl;
fclose(fp);

//Read fog data
fp = fopen((data_folder_path_+"/sensor_data/fog.csv").c_str(),"r");
float d_roll, d_pitch, d_yaw;
Expand Down Expand Up @@ -839,6 +913,8 @@ void ROSThread::Ready()
stereo_thread_.active_ = true;
omni_thread_.active_ = true;

baseline_odom_thread_.active_ = true;

data_stamp_thread_.thread_ = std::thread(&ROSThread::DataStampThread,this);
altimter_thread_.thread_ = std::thread(&ROSThread::AltimeterThread,this);
encoder_thread_.thread_ = std::thread(&ROSThread::EncoderThread,this);
Expand All @@ -853,6 +929,8 @@ void ROSThread::Ready()
stereo_thread_.thread_ = std::thread(&ROSThread::StereoThread,this);
omni_thread_.thread_ = std::thread(&ROSThread::OmniThread,this);

baseline_odom_thread_.thread_ = std::thread(&ROSThread::BaselineThread, this);

}

void ROSThread::DataStampThread()
Expand Down Expand Up @@ -940,6 +1018,10 @@ void ROSThread::DataStampThread()
}else if(iter->second.compare("omni") == 0 && omni_active_ == true){
omni_thread_.push(stamp);
omni_thread_.cv_.notify_all();
}else if (iter->second.compare("baseline") == 0)
{
baseline_odom_thread_.push(stamp);
baseline_odom_thread_.cv_.notify_all();
}
stamp_show_count_++;
if(stamp_show_count_ > 100){
Expand Down Expand Up @@ -1037,6 +1119,26 @@ void ROSThread::FogThread()
}
}

void ROSThread::BaselineThread()
{
while(1){
std::unique_lock<std::mutex> ul(baseline_odom_thread_.mutex_);
baseline_odom_thread_.cv_.wait(ul);
if(baseline_odom_thread_.active_ == false) return;
ul.unlock();

while(!baseline_odom_thread_.data_queue_.empty()){
auto data = baseline_odom_thread_.pop();
//process
if(baseline_odom_data_.find(data) != baseline_odom_data_.end()){
baseline_odom_pub_.publish(baseline_odom_data_[data]);
}

}
if(baseline_odom_thread_.active_ == false) return;
}
}

void ROSThread::GpsThread()
{
while(1){
Expand Down
9 changes: 9 additions & 0 deletions src/ROSThread.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,7 @@
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Quaternion.h>
#include <tf/transform_datatypes.h>
#include <tf_conversions/tf_eigen.h>


#include <dynamic_reconfigure/server.h>
Expand Down Expand Up @@ -160,6 +161,8 @@ class ROSThread : public QThread
ros::Publisher sick_back_pub_;
ros::Publisher sick_middle_pub_;

ros::Publisher baseline_odom_pub_; // baseline odom publisher

ros::Publisher stereo_left_pub_;
ros::Publisher stereo_right_pub_;

Expand All @@ -183,6 +186,8 @@ class ROSThread : public QThread
map<int64_t, sensor_msgs::Imu> imu_data_;
map<int64_t, sensor_msgs::MagneticField> mag_data_;

map<int64_t, nav_msgs::Odometry> baseline_odom_data_;

DataThread<int64_t> data_stamp_thread_;
DataThread<int64_t> altimter_thread_;
DataThread<int64_t> encoder_thread_;
Expand All @@ -197,6 +202,8 @@ class ROSThread : public QThread
DataThread<int64_t> sick_back_thread_;
DataThread<int64_t> sick_middle_thread_;

DataThread<int64_t> baseline_odom_thread_;

map<int64_t, int64_t> stop_period_; //start and stop stamp

void DataStampThread();
Expand All @@ -213,6 +220,8 @@ class ROSThread : public QThread
void StereoThread();
void OmniThread();

void BaselineThread();

void FilePlayerStart(const std_msgs::BoolConstPtr& msg);
void FilePlayerStop(const std_msgs::BoolConstPtr& msg);

Expand Down

0 comments on commit 368c602

Please sign in to comment.