Skip to content

Commit ae49b1f

Browse files
kminodaKYabuuchi
kminoda
and
KYabuuchi
committed
feat(segment_filter): publish images with lines and refactor (autowarefoundation#29)
* feat(segment_filter): publish images with lines Signed-off-by: kminoda <koji.minoda@tier4.jp> * update validation Signed-off-by: kminoda <koji.minoda@tier4.jp> * update imgproc (reverted) Signed-off-by: kminoda <koji.minoda@tier4.jp> * large change inclding refactoring Signed-off-by: kminoda <koji.minoda@tier4.jp> * major update Signed-off-by: kminoda <koji.minoda@tier4.jp> * revert rviz config Signed-off-by: kminoda <koji.minoda@tier4.jp> * minor fix in name Signed-off-by: kminoda <koji.minoda@tier4.jp> * add validation option Signed-off-by: kminoda <koji.minoda@tier4.jp> * update architecture svg Signed-off-by: kminoda <koji.minoda@tier4.jp> * rename validation.launch to overlay.launch Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> * no throw runtime_error (unintentionaly applying format) Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> --------- Signed-off-by: kminoda <koji.minoda@tier4.jp> Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp> Co-authored-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp>
1 parent 6051642 commit ae49b1f

21 files changed

+372
-91
lines changed

‎localization/yabloc/docs/yabloc_architecture.drawio.svg

+1-1
Loading

‎localization/yabloc/imgproc/segment_filter/include/segment_filter/segment_filter.hpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -49,8 +49,8 @@ class SegmentFilter : public rclcpp::Node
4949
common::SynchroSubscriber<PointCloud2, Image> synchro_subscriber_;
5050
common::StaticTfSubscriber tf_subscriber_;
5151

52-
rclcpp::Publisher<PointCloud2>::SharedPtr pub_cloud_;
53-
rclcpp::Publisher<PointCloud2>::SharedPtr pub_middle_cloud_;
52+
rclcpp::Publisher<PointCloud2>::SharedPtr pub_projected_cloud_;
53+
rclcpp::Publisher<PointCloud2>::SharedPtr pub_debug_cloud_;
5454
rclcpp::Publisher<Image>::SharedPtr pub_image_;
5555

5656
ProjectFunc project_func_ = nullptr;

‎localization/yabloc/imgproc/segment_filter/src/segment_filter_core.cpp

+19-3
Original file line numberDiff line numberDiff line change
@@ -39,7 +39,8 @@ SegmentFilter::SegmentFilter()
3939
auto cb = std::bind(&SegmentFilter::execute, this, _1, _2);
4040
synchro_subscriber_.set_callback(std::move(cb));
4141

42-
pub_cloud_ = create_publisher<PointCloud2>("projected_line_segments_cloud", 10);
42+
pub_projected_cloud_ = create_publisher<PointCloud2>("projected_line_segments_cloud", 10);
43+
pub_debug_cloud_ = create_publisher<PointCloud2>("debug/line_segments_cloud", 10);
4344
pub_image_ = create_publisher<Image>("projected_image", 10);
4445
}
4546

@@ -100,7 +101,7 @@ void SegmentFilter::execute(const PointCloud2 & line_segments_msg, const Image &
100101
pcl::PointCloud<pcl::PointNormal> valid_edges = project_lines(*line_segments_cloud, indices);
101102
pcl::PointCloud<pcl::PointNormal> invalid_edges = project_lines(*line_segments_cloud, indices, true);
102103

103-
// Line segments
104+
// Projected line segments
104105
{
105106
pcl::PointCloud<pcl::PointXYZLNormal> combined_edges;
106107
for (const auto & pn : valid_edges) {
@@ -117,7 +118,7 @@ void SegmentFilter::execute(const PointCloud2 & line_segments_msg, const Image &
117118
pln.label = 0;
118119
combined_edges.push_back(pln);
119120
}
120-
common::publish_cloud(*pub_cloud_, combined_edges, stamp);
121+
common::publish_cloud(*pub_projected_cloud_, combined_edges, stamp);
121122
}
122123

123124
// Image
@@ -135,6 +136,21 @@ void SegmentFilter::execute(const PointCloud2 & line_segments_msg, const Image &
135136
}
136137
common::publish_image(*pub_image_, projected_image, stamp);
137138
}
139+
140+
// Line segments for debug
141+
{
142+
pcl::PointCloud<pcl::PointXYZLNormal> combined_debug_edges;
143+
for (size_t index = 0; index < line_segments_cloud->size(); ++index) {
144+
const pcl::PointNormal& pn = line_segments_cloud->at(index);
145+
pcl::PointXYZLNormal pln;
146+
pln.getVector3fMap() = pn.getVector3fMap();
147+
pln.getNormalVector3fMap() = pn.getNormalVector3fMap();
148+
if (indices.count(index) > 0) pln.label = 255;
149+
else pln.label = 0;
150+
combined_debug_edges.push_back(pln);
151+
}
152+
common::publish_cloud(*pub_debug_cloud_, combined_debug_edges, stamp);
153+
}
138154
}
139155

140156
bool SegmentFilter::is_near_element(

‎localization/yabloc/validation/overlay_monitor/CMakeLists.txt renamed to ‎localization/yabloc/validation/lanelet2_overlay_monitor/CMakeLists.txt

+3-3
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
cmake_minimum_required(VERSION 3.5)
2-
project(overlay_monitor)
2+
project(lanelet2_overlay_monitor)
33

44
if(NOT CMAKE_CXX_STANDARD)
55
set(CMAKE_CXX_STANDARD 17)
@@ -27,8 +27,8 @@ find_package(Sophus REQUIRED)
2727

2828
# ===================================================
2929
# Executable
30-
set(TARGET overlay_node)
31-
ament_auto_add_executable(${TARGET} src/overlay_core.cpp src/overlay_node.cpp)
30+
set(TARGET lanelet2_overlay_monitor_node)
31+
ament_auto_add_executable(${TARGET} src/lanelet2_overlay_monitor_core.cpp src/lanelet2_overlay_monitor_node.cpp)
3232
target_include_directories(${TARGET} PUBLIC include)
3333
target_include_directories(${TARGET} SYSTEM PUBLIC ${EIGEN_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS})
3434
target_link_libraries(${TARGET} Sophus::Sophus ${PCL_LIBRARIES})

‎localization/yabloc/validation/overlay_monitor/include/overlay_monitor/overlay_monitor.hpp renamed to ‎localization/yabloc/validation/lanelet2_overlay_monitor/include/lanelet2_overlay_monitor/lanelet2_overlay_monitor.hpp

+4-4
Original file line numberDiff line numberDiff line change
@@ -32,9 +32,9 @@
3232
#include <pcl/point_cloud.h>
3333
#include <pcl/point_types.h>
3434

35-
namespace yabloc::overlay
35+
namespace yabloc::lanelet2_overlay
3636
{
37-
class Overlay : public rclcpp::Node
37+
class Lanelet2Overlay : public rclcpp::Node
3838
{
3939
public:
4040
using Pose = geometry_msgs::msg::Pose;
@@ -46,7 +46,7 @@ class Overlay : public rclcpp::Node
4646
using Image = sensor_msgs::msg::Image;
4747
using Float32Array = std_msgs::msg::Float32MultiArray;
4848

49-
Overlay();
49+
Lanelet2Overlay();
5050

5151
private:
5252
common::StaticTfSubscriber tf_subscriber_;
@@ -79,4 +79,4 @@ class Overlay : public rclcpp::Node
7979

8080
void make_vis_marker(const LineSegments & ls, const Pose & pose, const rclcpp::Time & stamp);
8181
};
82-
} // namespace yabloc::overlay
82+
} // namespace yabloc::lanelet2_overlay

‎localization/yabloc/validation/overlay_monitor/package.xml renamed to ‎localization/yabloc/validation/lanelet2_overlay_monitor/package.xml

+1-1
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
<?xml version="1.0"?>
22
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
33
<package format="3">
4-
<name>overlay_monitor</name>
4+
<name>lanelet2_overlay_monitor</name>
55
<version>0.0.0</version>
66
<description>overlay lanelet2 onto camera image</description>
77
<maintainer email="kento.yabuuchi.2@tier4.jp">Kento Yabuuchi</maintainer>

‎localization/yabloc/validation/overlay_monitor/src/overlay_core.cpp renamed to ‎localization/yabloc/validation/lanelet2_overlay_monitor/src/lanelet2_overlay_monitor_core.cpp

+13-13
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,7 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#include "overlay_monitor/overlay_monitor.hpp"
15+
#include "lanelet2_overlay_monitor/lanelet2_overlay_monitor.hpp"
1616

1717
#include <eigen3/Eigen/StdVector>
1818
#include <opencv4/opencv2/calib3d.hpp>
@@ -26,16 +26,16 @@
2626

2727
#include <pcl_conversions/pcl_conversions.h>
2828

29-
namespace yabloc::overlay
29+
namespace yabloc::lanelet2_overlay
3030
{
31-
Overlay::Overlay() : Node("overlay"), tf_subscriber_(get_clock()), pose_buffer_{40}
31+
Lanelet2Overlay::Lanelet2Overlay() : Node("lanelet2_overlay"), tf_subscriber_(get_clock()), pose_buffer_{40}
3232
{
3333
using std::placeholders::_1;
3434

3535
// Subscriber
36-
auto cb_info = std::bind(&Overlay::on_info, this, _1);
37-
auto cb_image = std::bind(&Overlay::on_image, this, _1);
38-
auto cb_line_segments = std::bind(&Overlay::on_line_segments, this, _1);
36+
auto cb_info = std::bind(&Lanelet2Overlay::on_info, this, _1);
37+
auto cb_image = std::bind(&Lanelet2Overlay::on_image, this, _1);
38+
auto cb_line_segments = std::bind(&Lanelet2Overlay::on_line_segments, this, _1);
3939
auto cb_pose = [this](const PoseStamped & msg) -> void { pose_buffer_.push_back(msg); };
4040
auto cb_ground = [this](const Float32Array & msg) -> void { ground_plane_.set(msg); };
4141

@@ -53,16 +53,16 @@ Overlay::Overlay() : Node("overlay"), tf_subscriber_(get_clock()), pose_buffer_{
5353

5454
// Publisher
5555
pub_vis_ = create_publisher<Marker>("projected_marker", 10);
56-
pub_image_ = create_publisher<sensor_msgs::msg::Image>("overlay_image", 10);
56+
pub_image_ = create_publisher<sensor_msgs::msg::Image>("lanelet2_overlay_image", 10);
5757
}
5858

59-
void Overlay::on_info(const CameraInfo & msg)
59+
void Lanelet2Overlay::on_info(const CameraInfo & msg)
6060
{
6161
info_ = msg;
6262
camera_extrinsic_ = tf_subscriber_(info_->header.frame_id, "base_link");
6363
}
6464

65-
void Overlay::on_image(const sensor_msgs::msg::Image & msg)
65+
void Lanelet2Overlay::on_image(const sensor_msgs::msg::Image & msg)
6666
{
6767
cv::Mat image = common::decompress_to_cv_mat(msg);
6868
const rclcpp::Time stamp = msg.header.stamp;
@@ -87,7 +87,7 @@ void Overlay::on_image(const sensor_msgs::msg::Image & msg)
8787
draw_overlay(image, synched_pose, stamp);
8888
}
8989

90-
void Overlay::on_line_segments(const PointCloud2 & msg)
90+
void Lanelet2Overlay::on_line_segments(const PointCloud2 & msg)
9191
{
9292
const rclcpp::Time stamp = msg.header.stamp;
9393

@@ -112,7 +112,7 @@ void Overlay::on_line_segments(const PointCloud2 & msg)
112112
make_vis_marker(line_segments_cloud, synched_pose.pose, stamp);
113113
}
114114

115-
void Overlay::draw_overlay(
115+
void Lanelet2Overlay::draw_overlay(
116116
const cv::Mat & image, const std::optional<Pose> & pose, const rclcpp::Time & stamp)
117117
{
118118
if (ll2_cloud_.empty()) return;
@@ -134,7 +134,7 @@ void Overlay::draw_overlay(
134134
common::publish_image(*pub_image_, show_image, stamp);
135135
}
136136

137-
void Overlay::draw_overlay_line_segments(
137+
void Lanelet2Overlay::draw_overlay_line_segments(
138138
cv::Mat & image, const Pose & pose, const LineSegments & near_segments)
139139
{
140140
if (!camera_extrinsic_.has_value()) return;
@@ -181,7 +181,7 @@ void Overlay::draw_overlay_line_segments(
181181
}
182182
}
183183

184-
void Overlay::make_vis_marker(
184+
void Lanelet2Overlay::make_vis_marker(
185185
const LineSegments & ls, const Pose & pose, const rclcpp::Time & stamp)
186186
{
187187
Marker marker;

‎localization/yabloc/validation/overlay_monitor/src/overlay_node.cpp renamed to ‎localization/yabloc/validation/lanelet2_overlay_monitor/src/lanelet2_overlay_monitor_node.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -12,12 +12,12 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#include "overlay_monitor/overlay_monitor.hpp"
15+
#include "lanelet2_overlay_monitor/lanelet2_overlay_monitor.hpp"
1616

1717
int main(int argc, char * argv[])
1818
{
1919
rclcpp::init(argc, argv);
20-
rclcpp::spin(std::make_shared<yabloc::overlay::Overlay>());
20+
rclcpp::spin(std::make_shared<yabloc::lanelet2_overlay::Lanelet2Overlay>());
2121
rclcpp::shutdown();
2222
return 0;
2323
}

‎localization/yabloc/validation/line_segments_overlay_monitor/CMakeLists.txt

+37
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,37 @@
1+
cmake_minimum_required(VERSION 3.5)
2+
project(line_segments_overlay_monitor)
3+
4+
if(NOT CMAKE_CXX_STANDARD)
5+
set(CMAKE_CXX_STANDARD 17)
6+
set(CMAKE_CXX_STANDARD_REQUIRED ON)
7+
set(CMAKE_CXX_EXTENSIONS OFF)
8+
endif()
9+
10+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
11+
add_compile_options(-Wall -Wextra -Wpedantic -Werror)
12+
endif()
13+
14+
# ===================================================
15+
find_package(ament_cmake_auto REQUIRED)
16+
ament_auto_find_build_dependencies()
17+
18+
# ===================================================
19+
# Eigen3
20+
find_package(Eigen3 REQUIRED)
21+
22+
# PCL
23+
find_package(PCL REQUIRED COMPONENTS common)
24+
25+
# Sophus
26+
find_package(Sophus REQUIRED)
27+
28+
# ===================================================
29+
# Executable
30+
set(TARGET line_segments_overlay_monitor_node)
31+
ament_auto_add_executable(${TARGET} src/line_segments_overlay_monitor_core.cpp src/line_segments_overlay_monitor_node.cpp)
32+
target_include_directories(${TARGET} PUBLIC include)
33+
target_include_directories(${TARGET} SYSTEM PUBLIC ${EIGEN_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS})
34+
target_link_libraries(${TARGET} Sophus::Sophus ${PCL_LIBRARIES})
35+
36+
# ===================================================
37+
ament_auto_package()

‎localization/yabloc/validation/line_segments_overlay_monitor/include/line_segments_overlay_monitor/line_segments_overlay_monitor.hpp

+45
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,45 @@
1+
// Copyright 2023 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#pragma once
16+
#include <rclcpp/rclcpp.hpp>
17+
18+
#include <sensor_msgs/msg/image.hpp>
19+
#include <sensor_msgs/msg/point_cloud2.hpp>
20+
21+
#include <pcl/point_cloud.h>
22+
#include <pcl/point_types.h>
23+
24+
namespace yabloc::line_segments_overlay
25+
{
26+
class LineSegmentsOverlay : public rclcpp::Node
27+
{
28+
public:
29+
using PointCloud2 = sensor_msgs::msg::PointCloud2;
30+
using Image = sensor_msgs::msg::Image;
31+
using LineSegment = pcl::PointXYZLNormal;
32+
using LineSegments = pcl::PointCloud<LineSegment>;
33+
LineSegmentsOverlay();
34+
35+
private:
36+
void on_image(const Image::ConstSharedPtr& img_msg);
37+
void on_line_segments(const PointCloud2::ConstSharedPtr& line_segments_msg);
38+
rclcpp::Subscription<Image>::SharedPtr sub_image_;
39+
rclcpp::Subscription<PointCloud2>::SharedPtr sub_line_segments_;
40+
rclcpp::Publisher<Image>::SharedPtr pub_debug_image_;
41+
42+
std::map<rclcpp::Time, Image::ConstSharedPtr> image_buffer_;
43+
size_t max_buffer_size_;
44+
};
45+
} // namespace yabloc::overlay

‎localization/yabloc/validation/line_segments_overlay_monitor/package.xml

+27
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,27 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>line_segments_overlay_monitor</name>
5+
<version>0.0.0</version>
6+
<description>overlay detected line segments onto camera image</description>
7+
<maintainer email="koji.minoda@tier4.jp">Kento Yabuuchi</maintainer>
8+
<license>Apache License 2.0</license>
9+
10+
<buildtool_depend>ament_cmake</buildtool_depend>
11+
12+
<depend>rclcpp</depend>
13+
<depend>std_msgs</depend>
14+
<depend>sensor_msgs</depend>
15+
<depend>nav_msgs</depend>
16+
<depend>visualization_msgs</depend>
17+
<depend>ublox_msgs</depend>
18+
19+
<depend>yabloc_common</depend>
20+
21+
<test_depend>ament_lint_auto</test_depend>
22+
<test_depend>ament_lint_common</test_depend>
23+
24+
<export>
25+
<build_type>ament_cmake</build_type>
26+
</export>
27+
</package>

0 commit comments

Comments
 (0)