diff --git a/beacon_ros2/config/rviz.rviz b/beacon_ros2/config/rviz.rviz index b7ca700906519aa46074cc6589371edd6d3d381f..3a3487e336d195b7cda57f4fdb6ecbab463a1d5e 100644 --- a/beacon_ros2/config/rviz.rviz +++ b/beacon_ros2/config/rviz.rviz @@ -7,7 +7,7 @@ Panels: - /Global Options1 - /Status1 Splitter Ratio: 0.5 - Tree Height: 380 + Tree Height: 364 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -62,7 +62,7 @@ Visualization Manager: Value: /left_image Value: true - Class: rviz_default_plugins/Image - Enabled: true + Enabled: false Max Value: 1 Median window: 5 Min Value: 0 @@ -74,6 +74,34 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /right_image + Value: false + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /stereo_match + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /mono_match Value: true Enabled: true Global Options: @@ -144,12 +172,12 @@ Visualization Manager: Window Geometry: Displays: collapsed: false - Height: 846 + Height: 926 Hide Left Dock: false Hide Right Dock: false Image: collapsed: false - QMainWindow State: 000000ff00000000fd000000040000000000000292000002b4fc020000000afb0000000a0049006d006100670065010000003b000001690000002800fffffffb0000000a0049006d00610067006501000001aa000001450000002800fffffffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000001a100000205000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000036bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000036b000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000780000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d0065010000000000000450000000000000000000000218000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000029200000304fc020000000cfb0000000a0049006d006100670065010000003b0000010e0000002800fffffffb0000000a0049006d006100670065000000012f000001040000002800fffffffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007302000002b2000001b100000340000001f7fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000014f000001020000002800fffffffb0000000a0049006d0061006700650100000257000000e80000002800ffffff000000010000010f0000036bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000036b000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000780000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000002180000030400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -159,5 +187,5 @@ Window Geometry: Views: collapsed: false Width: 1200 - X: 79 - Y: 97 + X: 71 + Y: 38 diff --git a/beacon_ros2/include/beacon_ros2/offline_process.h b/beacon_ros2/include/beacon_ros2/offline_process.h index 337db1c03e5d2e393621d214e4157ad9bd6c2186..7a36a25d0b0aac47abd669613aca487f1b722361 100644 --- a/beacon_ros2/include/beacon_ros2/offline_process.h +++ b/beacon_ros2/include/beacon_ros2/offline_process.h @@ -35,6 +35,10 @@ class PohangStereoImageLoader : public rclcpp::Node { std::placeholders::_2)); } + ~PohangStereoImageLoader() { + m_reader.close(); + } + protected: void loadData() { constexpr int batch_size{100}; diff --git a/beacon_ros2/include/beacon_ros2/stereo_odometry_offline.h b/beacon_ros2/include/beacon_ros2/stereo_odometry_offline.h index 07c387f6dbe6d8e0b39241022372cd37afd9f654..3a0c007760b47532b14e12260681bd3ca9488131 100644 --- a/beacon_ros2/include/beacon_ros2/stereo_odometry_offline.h +++ b/beacon_ros2/include/beacon_ros2/stereo_odometry_offline.h @@ -26,6 +26,7 @@ class StereoOdometryOffline protected: rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr m_left_img_pub{}; rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr m_right_img_pub{}; + rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr m_mono_img_pub{}; rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr m_stereo_img_pub{}; protected: diff --git a/beacon_ros2/src/stereo_odometry_offline.cpp b/beacon_ros2/src/stereo_odometry_offline.cpp index e3cbaa046cb9d96b17d865832f67a9c9fdc8a472..a2c2d552df7936cdd6a2f8b150a283c6a6774b01 100644 --- a/beacon_ros2/src/stereo_odometry_offline.cpp +++ b/beacon_ros2/src/stereo_odometry_offline.cpp @@ -14,6 +14,8 @@ StereoOdometryOffline::StereoOdometryOffline(const std::string& path_to_bag, this->create_publisher<sensor_msgs::msg::Image>("left_image", 10000); m_right_img_pub = this->create_publisher<sensor_msgs::msg::Image>("right_image", 10000); + m_mono_img_pub = + this->create_publisher<sensor_msgs::msg::Image>("mono_match", 10000); m_stereo_img_pub = this->create_publisher<sensor_msgs::msg::Image>("stereo_match", 10000); } @@ -35,7 +37,6 @@ void StereoOdometryOffline::runImpl(StereoImageMsg msg) { display_left.header = left->header; display_left.encoding = left->encoding; m_left_img_pub->publish(*display_left.toImageMsg()); - // m_left_img_pub->publish(*left->toImageMsg()); cv_bridge::CvImage display_right{}; display_right.image = display_imgs.right_image; @@ -43,10 +44,16 @@ void StereoOdometryOffline::runImpl(StereoImageMsg msg) { display_right.encoding = right->encoding; m_right_img_pub->publish(*display_right.toImageMsg()); + cv_bridge::CvImage display_mono{}; + display_mono.image = display_imgs.monocular_match; + display_mono.header = left->header; + display_mono.encoding = left->encoding; + m_mono_img_pub->publish(*display_mono.toImageMsg()); + cv_bridge::CvImage display_stereo{}; display_stereo.image = display_imgs.stereo_match; - display_stereo.header = left->header; - display_stereo.encoding = left->encoding; + display_stereo.header = right->header; + display_stereo.encoding = right->encoding; m_stereo_img_pub->publish(*display_stereo.toImageMsg()); }