Skip to content

Commit 49f78e2

Browse files
ToniRVGitHub Enterprise
ToniRV
authored and
GitHub Enterprise
committed
Merge pull request #40 from SPARK/fix/make_vio_work_on_tesse
Fix/make vio work on tesse
2 parents 1457bd6 + a6827da commit 49f78e2

File tree

72 files changed

+916
-2070
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

72 files changed

+916
-2070
lines changed

‎include/spark-vio-ros/base-data-source.h‎

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -70,11 +70,12 @@ class RosBaseDataProvider : public DataProvider {
7070
bool parseCameraData(StereoCalibration* stereo_calib);
7171

7272
// Parse IMU calibration info (for ros online)
73-
bool parseImuData(ImuData* imudata, ImuParams* imuparams);
73+
bool parseImuData(ImuData* imu_data, ImuParams* imu_params) const;
7474

7575
// Publish all outputs by calling individual functions below
7676
void publishOutput(const SpinOutputPacket& vio_output);
7777

78+
7879
protected:
7980
VioFrontEndParams frontend_params_;
8081
StereoCalibration stereo_calib_;
@@ -94,6 +95,10 @@ class RosBaseDataProvider : public DataProvider {
9495
// Queue to store and retrieve VIO output in a thread-safe way.
9596
ThreadsafeQueue<SpinOutputPacket> vio_output_queue_;
9697

98+
// Store IMU data from last frame
99+
// TODO(Toni) I don't see where this is used!?
100+
ImuData imu_data_;
101+
97102
private:
98103
// Define publisher for debug images.
99104
image_transport::Publisher debug_img_pub_;
@@ -123,6 +128,8 @@ class RosBaseDataProvider : public DataProvider {
123128
void publishDebugImage(const Timestamp& timestamp,
124129
const cv::Mat& debug_image) const;
125130

131+
void printParsedParams() const;
132+
126133
// Define tf broadcaster for world to base_link (IMU).
127134
tf::TransformBroadcaster tf_broadcaster_;
128135
};

‎include/spark-vio-ros/ros-data-source.h‎

Lines changed: 2 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -42,9 +42,8 @@ class RosDataProvider : public RosBaseDataProvider {
4242
// Define Node Handler for Cam Callback (and Queue)
4343
ros::NodeHandle nh_cam_;
4444

45-
ImuData imu_data_; // store IMU data from last frame
46-
Timestamp last_time_stamp_; // Timestamp correponding to last frame
47-
Timestamp last_imu_time_stamp_; // Timestamp corresponding to last imu meas
45+
Timestamp last_timestamp_; // Timestamp correponding to last frame
46+
Timestamp last_imu_timestamp_; // Timestamp corresponding to last imu meas
4847
int frame_count_; // Keep track of number of frames processed
4948

5049
StereoBuffer stereo_buffer_;
@@ -85,9 +84,6 @@ class RosDataProvider : public RosBaseDataProvider {
8584
// Define subscriber for Reinit data
8685
ros::Subscriber reinit_flag_subscriber_;
8786
ros::Subscriber reinit_pose_subscriber_;
88-
89-
// Print the parameters
90-
void print() const;
9187
};
9288

9389
} // namespace VIO

‎include/spark-vio-ros/rosbag-data-source.h‎

Lines changed: 20 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717
#include <rosbag/bag.h>
1818
#include <rosbag/view.h>
1919
#include <sensor_msgs/Image.h>
20+
#include <nav_msgs/Odometry.h>
2021

2122
#include "spark-vio-ros/base-data-source.h"
2223

@@ -30,8 +31,8 @@ struct RosbagData {
3031
std::vector<sensor_msgs::ImageConstPtr> right_imgs_;
3132
// Vector of timestamps see issue in .cpp file
3233
std::vector<Timestamp> timestamps_;
33-
// IMU data
34-
ImuData imu_data_;
34+
// Ground-truth Odometry (only if available).
35+
std::vector<nav_msgs::OdometryConstPtr> gt_odometry_;
3536
};
3637

3738
class RosbagDataProvider : public RosBaseDataProvider {
@@ -45,19 +46,31 @@ class RosbagDataProvider : public RosBaseDataProvider {
4546

4647
private:
4748
// Parse rosbag data
49+
// Optionally, send a ground-truth odometry topic if available in the rosbag.
50+
// If gt_odom_topic is empty (""), it will be ignored.
4851
bool parseRosbag(const std::string& bag_path,
4952
const std::string& left_imgs_topic,
5053
const std::string& right_imgs_topic,
51-
const std::string& imu_topic, RosbagData* data);
54+
const std::string& imu_topic,
55+
const std::string& gt_odom_topic,
56+
RosbagData* data);
5257

53-
// Parse IMU calibration info (for rosbag)
54-
bool parseImuData(RosbagData* rosbag_data, ImuParams* imuparams);
58+
// Get ground-truth nav state for VIO initialization.
59+
// It uses odometry messages inside of the rosbag as ground-truth (indexed
60+
// by the sequential order in the rosbag).
61+
VioNavState getGroundTruthVioNavState(const size_t& k_frame) const;
5562

56-
// Print the parameters
57-
void print() const;
63+
// Publish clock
64+
void publishClock(const Timestamp& timestamp) const;
65+
66+
// Publish ground-truth odometry
67+
void publishGroundTruthOdometry(
68+
const nav_msgs::OdometryConstPtr& gt_odom) const;
5869

5970
private:
6071
RosbagData rosbag_data_;
72+
ros::Publisher clock_pub_;
73+
ros::Publisher gt_odometry_pub_;
6174
};
6275

6376
} // namespace VIO

‎launch/gt_logger.launch‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,10 @@
1+
<launch>
2+
<arg name="gt_topic" default="/tesse/odom"/>
3+
<arg name="output_dir" default="$(find spark_vio_ros)/output_logs/TESSE/"/>
4+
5+
<node name="gt_logger_node" pkg="spark_vio_ros" type="gt_logger_node.py"
6+
output="screen">
7+
<param name="gt_topic" value="$(arg gt_topic)"/>
8+
<param name="output_dir" value="$(arg output_dir)"/>
9+
</node>
10+
</launch>

‎launch/spark_vio_ros.launch‎

Lines changed: 42 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -11,15 +11,30 @@
1111
<param name="use_sim_time" value="true"/>
1212
<arg name="rosbag_path" unless="$(arg online)"/>
1313

14+
<!-- If true, SparkVio will log output of all modules to the
15+
'log_output_path' location. -->
16+
<arg name="log_output" default="false"/>
17+
<arg name="log_output_path"
18+
default="$(find spark_vio_ros)/output_logs/"/>
19+
<!-- If true, log ground-truth poses to a csv file using the
20+
gt_logger_node, and processing data from the gt_topic rostopic -->
21+
<arg name="log_gt_data" default="false"/>
22+
<arg name="gt_topic" if="$(arg log_gt_data)"
23+
default="ground_truth_odometry_topic"/>
24+
1425
<!-- Parameters -->
1526
<!-- each dataset has its own set of parameter files -->
1627
<!-- the parameters are set in param folder, ordered by the dataset's name -->
17-
<arg name="params_folder" value="$(find spark_vio_ros)/param/$(arg dataset_name)"/>
28+
<arg name="params_folder"
29+
value="$(find spark_vio_ros)/param/$(arg dataset_name)"/>
1830

1931
<!-- Subscribed Topics -->
20-
<arg name="left_cam_topic" default="/cam0/image_raw"/>
21-
<arg name="right_cam_topic" default="/cam1/image_raw"/>
22-
<arg name="imu_topic" default="/imu0"/>
32+
<arg name="left_cam_topic" default="/cam0/image_raw"/>
33+
<arg name="right_cam_topic" default="/cam1/image_raw"/>
34+
<arg name="imu_topic" default="/imu0"/>
35+
<!-- Empty string ("") means no ground-truth available. Used for init if
36+
requested to do ground-truth initialization. -->
37+
<arg name="odometry_ground_truth_topic" default=""/>
2338

2439
<!-- Frame IDs -->
2540
<arg name="world_frame_id" default="world"/>
@@ -31,7 +46,7 @@
3146
<arg name="backend_type" default="1" />
3247
<!-- Visualize pipeline output in OpenCV. -->
3348
<!-- Note that this is duplicated from the flags file -->
34-
<arg name="visualize" default="false" />
49+
<arg name="visualize" default="true" />
3550

3651
<!-- Launch main node -->
3752
<node name="spark_vio_ros" pkg="spark_vio_ros" type="spark_vio_ros" output="screen"
@@ -47,8 +62,8 @@
4762
--log_prefix=1
4863
--v=$(arg verbosity)
4964
--backend_type=$(arg backend_type)
50-
--log_output=0
51-
--output_path='.'
65+
--log_output=$(arg log_output)
66+
--output_path=$(arg log_output_path)
5267
--visualize=$(arg visualize)
5368
--parallel_run=$(arg parallel)
5469
--online_run=$(arg online)">
@@ -66,15 +81,25 @@
6681
<param name="left_cam_rosbag_topic" value="$(arg left_cam_topic)" unless="$(arg online)"/>
6782
<param name="right_cam_rosbag_topic" value="$(arg right_cam_topic)" unless="$(arg online)"/>
6883
<param name="imu_rosbag_topic" value="$(arg imu_topic)" unless="$(arg online)"/>
84+
<param name="ground_truth_odometry_rosbag_topic"
85+
value="$(arg odometry_ground_truth_topic)" unless="$(arg online)"/>
86+
87+
ground_truth_odometry_rosbag_topic
6988

7089
<!-- Other subscription topics -->
7190
<remap from="reinit_flag" to="/sparkvio/reinit_flag"/>
7291
<remap from="reinit_pose" to="/sparkvio/reinit_pose"/>
7392

7493
<!-- Remap publisher topics -->
75-
<remap from="odometry" to="/sparkvio/odometry"/>
76-
<remap from="resiliency" to="/sparkvio/resiliency"/>
77-
<remap from="imu_bias" to="/sparkvio/imu_bias"/>
94+
<!-- TODO(Toni): we should use a group ns="sparkvio" instead -->
95+
<remap from="odometry" to="/sparkvio/odometry"/>
96+
<remap from="resiliency" to="/sparkvio/resiliency"/>
97+
<remap from="imu_bias" to="/sparkvio/imu_bias"/>
98+
<remap from="mesh" to="/sparkvio/mesh"/>
99+
<remap from="frontend_stats" to="/sparkvio/frontend_stats"/>
100+
<remap from="debug_mesh_img" to="/sparkvio/debug_mesh_img"/>
101+
<remap from="time_horizon_pointcloud"
102+
to="/sparkvio/time_horizon_pointcloud"/>
78103

79104
<!-- Resiliency Thresholds: TODO(Sandro) document -->
80105
<param name="velocity_det_threshold" value="0.1"/>
@@ -86,4 +111,11 @@
86111
<rosparam command="load" file="$(arg params_folder)/calibration.yaml"/>
87112

88113
</node>
114+
115+
<!-- Log ground-truth data only if requested-->
116+
<node if="$(arg log_gt_data)" name="gt_logger_node" pkg="spark_vio_ros"
117+
type="gt_logger_node.py" output="screen">
118+
<param name="gt_topic" value="$(arg gt_topic)"/>
119+
<param name="output_dir" value="$(arg log_output_path)"/>
120+
</node>
89121
</launch>

‎launch/spark_vio_ros_euroc.launch‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,16 @@
66
<arg name="online" default="true" />
77
<!-- Set use_sim_time to true if you use rosbag with clock argument -->
88
<param name="use_sim_time" value="true"/>
9+
<!-- Set log_output to true and SparkVio will log output of all modules to
10+
the log_output_path location. -->
11+
<arg name="log_output" default="true"/>
12+
<arg name="log_output_path"
13+
default="$(find spark_vio_ros)/output_logs/$(arg dataset_name)"
14+
if="$(arg log_output)"/>
15+
<!-- Set to true and specify gt_topic if you want to log ground-truth data -->
16+
<arg name="log_gt_data" default="false"/>
17+
<arg name="gt_topic" if="$(arg log_gt_data)"
18+
default="/vicon/firefly_sbx/firefly_sbx"/>
919

1020
<!-- Change rosbag path if online argument is false -->
1121
<arg name="rosbag_path" default="/home/tonirv/datasets/euroc/V1_01_easy/V1_01_easy.bag"

‎launch/spark_vio_ros_kitti.launch‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,13 @@
33
<arg name="parallel" default="True" />
44
<arg name="online" default="True" />
55

6+
<arg name="log_output" default="false"/>
7+
<arg name="log_output_path"
8+
default="$(find spark_vio_ros)/output_logs/"
9+
if="$(arg log_output)"/>
10+
<arg name="log_gt_data" default="false"/>
11+
<arg name="gt_topic" default=""
12+
if="$(arg log_gt_data)"/>
613
<!-- Set use_sim_time to true if you use rosbag with clock argument -->
714
<param name="use_sim_time" value="true" if="$(arg online)"/>
815

‎launch/spark_vio_ros_simulator.launch‎

Lines changed: 0 additions & 53 deletions
This file was deleted.

‎launch/spark_vio_ros_tesse.launch‎

Lines changed: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,38 @@
1+
<launch>
2+
<arg name="dataset_name" value="TESSE"/>
3+
<arg name="verbosity" default="0"/>
4+
<arg name="backend_type" default="1"/>
5+
<arg name="online" default="true"/>
6+
<param name="use_sim_time" value="true"/>
7+
8+
<arg name="parallel" default="true"/>
9+
10+
<arg name="log_output" default="true"/>
11+
<arg name="log_output_path"
12+
default="$(find spark_vio_ros)/output_logs/$(arg dataset_name)"
13+
if="$(arg log_output)"/>
14+
<arg name="log_gt_data" default="true"/>
15+
<arg name="gt_topic" default="/tesse/odom"
16+
if="$(arg log_gt_data)"/>
17+
18+
<!-- Change rosbag path if online argument is false -->
19+
<arg name="rosbag_path"
20+
default="/home/tonirv/Code/ROS/tess_ws/src/TESSE_interface/ROS/tesse_ros_bridge/scripts/2019-08-28-16-57-09.bag"
21+
unless="$(arg online)"/>
22+
23+
<!-- Frame IDs -->
24+
<arg name="base_link_frame_id" value="base_link_spark"/>
25+
26+
<!-- Subscriber Topics -->
27+
<arg name="left_cam_topic" value="/tesse/left_cam"/>
28+
<arg name="right_cam_topic" value="/tesse/right_cam"/>
29+
<arg name="imu_topic" value="/tesse/imu"/>
30+
<!-- Empty string ("") means no ground-truth available. Used for init if
31+
requested to do ground-truth initialization. -->
32+
<arg name="odometry_ground_truth_topic" value="$(arg gt_topic)"/>
33+
34+
<!-- Launch actual pipeline -->
35+
<include file="$(find spark_vio_ros)/launch/spark_vio_ros.launch"
36+
pass_all_args="true"/>
37+
38+
</launch>

0 commit comments

Comments
 (0)