1414
1515#include " map_height_fitter/map_height_fitter.hpp"
1616
17+ #include < lanelet2_extension/utility/message_conversion.hpp>
18+ #include < lanelet2_extension/utility/query.hpp>
19+
20+ #include < autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
1721#include < autoware_map_msgs/srv/get_partial_point_cloud_map.hpp>
1822#include < sensor_msgs/msg/point_cloud2.hpp>
1923#include < tf2_geometry_msgs/tf2_geometry_msgs.hpp>
2024
25+ #include < lanelet2_core/Forward.h>
26+ #include < lanelet2_core/LaneletMap.h>
2127#include < pcl/point_cloud.h>
2228#include < pcl/point_types.h>
2329#include < pcl_conversions/pcl_conversions.h>
@@ -31,52 +37,74 @@ struct MapHeightFitter::Impl
3137 static constexpr char enable_partial_load[] = " enable_partial_load"
3238
3339 explicit Impl (rclcpp::Node * node);
34- void on_map (const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg);
40+ void on_pcd_map (const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg);
41+ void on_vector_map (const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg);
3542 bool get_partial_point_cloud_map (const Point & point);
36- double get_ground_height (const tf2::Vector3 & point) const ;
43+ double get_ground_height (const Point & point) const ;
3744 std::optional<Point > fit (const Point & position, const std::string & frame);
3845
3946 tf2::BufferCore tf2_buffer_;
4047 tf2_ros::TransformListener tf2_listener_;
4148 std::string map_frame_;
42- pcl::PointCloud<pcl::PointXYZ>::Ptr map_cloud_;
4349 rclcpp::Node * node_;
4450
51+ std::string fit_target_;
52+
53+ // for fitting by pointcloud_map_loader
4554 rclcpp::CallbackGroup::SharedPtr group_;
46- rclcpp::Client<autoware_map_msgs::srv::GetPartialPointCloudMap>::SharedPtr cli_map_;
47- rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_map_;
48- rclcpp::AsyncParametersClient::SharedPtr params_map_loader_;
55+ pcl::PointCloud<pcl::PointXYZ>::Ptr map_cloud_;
56+ rclcpp::Client<autoware_map_msgs::srv::GetPartialPointCloudMap>::SharedPtr cli_pcd_map_;
57+ rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_pcd_map_;
58+ rclcpp::AsyncParametersClient::SharedPtr params_pcd_map_loader_;
59+
60+ // for fitting by vector_map_loader
61+ lanelet::LaneletMapPtr vector_map_;
62+ rclcpp::Subscription<autoware_auto_mapping_msgs::msg::HADMapBin>::SharedPtr sub_vector_map_;
4963};
5064
5165MapHeightFitter::Impl::Impl (rclcpp::Node * node) : tf2_listener_(tf2_buffer_), node_(node)
5266{
53- const auto callback = [this ](const std::shared_future<std::vector<rclcpp::Parameter>> & future) {
54- bool partial_load = false ;
55- for (const auto & param : future.get ()) {
56- if (param.get_name () == enable_partial_load) {
57- partial_load = param.as_bool ();
58- }
59- }
60-
61- if (partial_load) {
62- group_ = node_->create_callback_group (rclcpp::CallbackGroupType::MutuallyExclusive);
63- cli_map_ = node_->
create_client <autoware_map_msgs::srv::GetPartialPointCloudMap>(
64- " ~/partial_map_load"
65- } else {
66- const auto durable_qos = rclcpp::QoS (1 ).transient_local ();
67- sub_map_ = node_->create_subscription <sensor_msgs::msg::PointCloud2>(
68- " ~/pointcloud_map"
69- std::bind (&MapHeightFitter::Impl::on_map, this , std::placeholders::_1));
70- }
71- };
72-
73- const auto map_loader_name = node->declare_parameter <std::string>(" map_loader_name"
74- params_map_loader_ = rclcpp::AsyncParametersClient::make_shared (node, map_loader_name);
75- params_map_loader_->wait_for_service ();
76- params_map_loader_->get_parameters ({enable_partial_load}, callback);
67+ fit_target_ = node->declare_parameter <std::string>(" map_height_fitter.target"
68+ if (fit_target_ == " pointcloud_map"
69+ const auto callback =
70+ [this ](const std::shared_future<std::vector<rclcpp::Parameter>> & future) {
71+ bool partial_load = false ;
72+ for (const auto & param : future.get ()) {
73+ if (param.get_name () == enable_partial_load) {
74+ partial_load = param.as_bool ();
75+ }
76+ }
77+
78+ if (partial_load) {
79+ group_ = node_->create_callback_group (rclcpp::CallbackGroupType::MutuallyExclusive);
80+ cli_pcd_map_ = node_->
create_client <autoware_map_msgs::srv::GetPartialPointCloudMap>(
81+ " ~/partial_map_load"
82+ } else {
83+ const auto durable_qos = rclcpp::QoS (1 ).transient_local ();
84+ sub_pcd_map_ = node_->create_subscription <sensor_msgs::msg::PointCloud2>(
85+ " ~/pointcloud_map"
86+ std::bind (&MapHeightFitter::Impl::on_pcd_map, this , std::placeholders::_1));
87+ }
88+ };
89+
90+ const auto map_loader_name =
91+ node->declare_parameter <std::string>(" map_height_fitter.map_loader_name"
92+ params_pcd_map_loader_ = rclcpp::AsyncParametersClient::make_shared (node, map_loader_name);
93+ params_pcd_map_loader_->wait_for_service ();
94+ params_pcd_map_loader_->get_parameters ({enable_partial_load}, callback);
95+
96+ } else if (fit_target_ == " vector_map"
97+ const auto durable_qos = rclcpp::QoS (1 ).transient_local ();
98+ sub_vector_map_ = node_->
create_subscription <autoware_auto_mapping_msgs::msg::HADMapBin>(
99+ " ~/vector_map"
100+ std::bind (&MapHeightFitter::Impl::on_vector_map, this , std::placeholders::_1));
101+
102+ } else {
103+ throw std::runtime_error (" invalid fit_target"
104+ }
77105}
78106
79- void MapHeightFitter::Impl::on_map (const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)
107+ void MapHeightFitter::Impl::on_pcd_map (const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)
80108{
81109 map_frame_ = msg->header .frame_id ;
82110 map_cloud_ = pcl::PointCloud<pcl::PointXYZ>::
Ptr (
new pcl::PointCloud<pcl::PointXYZ>);
@@ -87,11 +115,11 @@ bool MapHeightFitter::Impl::get_partial_point_cloud_map(const Point & point)
87115{
88116 const auto logger = node_->get_logger ();
89117
90- if (!cli_map_ ) {
118+ if (!cli_pcd_map_ ) {
91119 RCLCPP_WARN_STREAM (logger, " Partial map loading in pointcloud_map_loader is not enabled"
92120 return false ;
93121 }
94- if (!cli_map_ ->service_is_ready ()) {
122+ if (!cli_pcd_map_ ->service_is_ready ()) {
95123 RCLCPP_WARN_STREAM (logger, " Partial map loading in pointcloud_map_loader is not ready"
96124 return false ;
97125 }
@@ -102,7 +130,7 @@ bool MapHeightFitter::Impl::get_partial_point_cloud_map(const Point & point)
102130 req->area .radius = 50 ;
103131
104132 RCLCPP_DEBUG (logger, " Send request to map_loader"
105- auto future = cli_map_ ->async_send_request (req);
133+ auto future = cli_pcd_map_ ->async_send_request (req);
106134 auto status = future.wait_for (std::chrono::seconds (1 ));
107135 while (status != std::future_status::ready) {
108136 RCLCPP_DEBUG (logger, " waiting response"
@@ -134,72 +162,121 @@ bool MapHeightFitter::Impl::get_partial_point_cloud_map(const Point & point)
134162 return true ;
135163}
136164
137- double MapHeightFitter::Impl::get_ground_height (const tf2::Vector3 & point) const
165+ void MapHeightFitter::Impl::on_vector_map (
166+ const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg)
138167{
139- const double x = point. getX ();
140- const double y = point. getY ( );
141-
142- // find distance d to closest point
143- double min_dist2 = INFINITY;
144- for (const auto & p : map_cloud_-> points ) {
145- const double dx = x - p. x ;
146- const double dy = y - p. y ;
147- const double sd = (dx * dx) + (dy * dy);
148- min_dist2 = std::min (min_dist2, sd) ;
149- }
168+ vector_map_ = std::make_shared<lanelet::LaneletMap> ();
169+ lanelet::utils::conversion::fromBinMsg (*msg, vector_map_ );
170+ map_frame_ = msg-> header . frame_id ;
171+ }
172+
173+ double MapHeightFitter::Impl::get_ground_height (const Point & point) const
174+ {
175+ const auto logger = node_-> get_logger () ;
176+
177+ const double x = point. x ;
178+ const double y = point. y ;
150179
151- // find lowest height within radius (d+1.0)
152- const double radius2 = std::pow (std::sqrt (min_dist2) + 1.0 , 2.0 );
153180 double height = INFINITY;
154- for (const auto & p : map_cloud_->points ) {
155- const double dx = x - p.x ;
156- const double dy = y - p.y ;
157- const double sd = (dx * dx) + (dy * dy);
158- if (sd < radius2) {
159- height = std::min (height, static_cast <double >(p.z ));
181+ if (fit_target_ == " pointcloud_map"
182+ // find distance d to closest point
183+ double min_dist2 = INFINITY;
184+ for (const auto & p : map_cloud_->points ) {
185+ const double dx = x - p.x ;
186+ const double dy = y - p.y ;
187+ const double sd = (dx * dx) + (dy * dy);
188+ min_dist2 = std::min (min_dist2, sd);
189+ }
190+
191+ // find lowest height within radius (d+1.0)
192+ const double radius2 = std::pow (std::sqrt (min_dist2) + 1.0 , 2.0 );
193+
194+ for (const auto & p : map_cloud_->points ) {
195+ const double dx = x - p.x ;
196+ const double dy = y - p.y ;
197+ const double sd = (dx * dx) + (dy * dy);
198+ if (sd < radius2) {
199+ height = std::min (height, static_cast <double >(p.z ));
200+ }
201+ }
202+ } else if (fit_target_ == " vector_map"
203+ lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer (vector_map_);
204+
205+ geometry_msgs::msg::Pose pose;
206+ pose.position .x = x;
207+ pose.position .y = y;
208+ pose.position .z = 0.0 ;
209+ lanelet::ConstLanelet closest_lanelet;
210+ const bool result =
211+ lanelet::utils::query::getClosestLanelet (all_lanelets, pose, &closest_lanelet);
212+ if (!result) {
213+ RCLCPP_WARN_STREAM (logger, " failed to get closest lanelet"
214+ return point.z ;
160215 }
216+ height = closest_lanelet.centerline ().back ().z ();
161217 }
162218
163- return std::isfinite (height) ? height : point.getZ () ;
219+ return std::isfinite (height) ? height : point.z ;
164220}
165221
166222std::optional<Point > MapHeightFitter::Impl::fit (const Point & position, const std::string & frame)
167223{
168224 const auto logger = node_->get_logger ();
169- tf2::Vector3 point (position. x , position. y , position. z );
225+ RCLCPP_INFO_STREAM (logger, " fit_target: " << fit_target_ << " , frame: " << frame );
170226
171- RCLCPP_DEBUG (logger, " original point: %.3f %.3f %.3f" getX (), point.getY (), point.getZ ());
227+ Point point;
228+ point.x = position.x ;
229+ point.y = position.y ;
230+ point.z = position.z ;
172231
173- if (cli_map_) {
174- if (!get_partial_point_cloud_map (position)) {
232+ RCLCPP_DEBUG (logger, " original point: %.3f %.3f %.3f" x , point.y , point.z );
233+
234+ // prepare data
235+ if (fit_target_ == " pointcloud_map"
236+ if (cli_pcd_map_) { // if cli_pcd_map_ is available, prepare pointcloud map by partial loading
237+ if (!get_partial_point_cloud_map (position)) {
238+ RCLCPP_WARN_STREAM (logger, " failed to get partial point cloud map"
239+ return std::nullopt;
240+ }
241+ } // otherwise, pointcloud map should be already prepared by on_pcd_map
242+ if (!map_cloud_) {
243+ RCLCPP_WARN_STREAM (logger, " point cloud map is not ready"
244+ return std::nullopt;
245+ }
246+ } else if (fit_target_ == " vector_map"
247+ // vector_map_ should be already prepared by on_vector_map
248+ if (!vector_map_) {
249+ RCLCPP_WARN_STREAM (logger, " vector map is not ready"
175250 return std::nullopt;
176251 }
252+ } else {
253+ throw std::runtime_error (" invalid fit_target"
177254 }
178255
179- if (!map_cloud_) {
180- RCLCPP_WARN_STREAM (logger, " point cloud map is not ready"
256+ // transform frame to map_frame_
257+ try {
258+ const auto stamped = tf2_buffer_.lookupTransform (frame, map_frame_, tf2::TimePointZero);
259+ tf2::doTransform (point, point, stamped);
260+ } catch (tf2::TransformException & exception ) {
261+ RCLCPP_WARN_STREAM (logger, " failed to lookup transform: " exception .what ());
181262 return std::nullopt;
182263 }
183264
265+ // fit height on map_frame_
266+ point.z = get_ground_height (point);
267+
268+ // transform map_frame_ to frame
184269 try {
185270 const auto stamped = tf2_buffer_.lookupTransform (map_frame_, frame, tf2::TimePointZero);
186- tf2::Transform transform{tf2::Quaternion{}, tf2::Vector3{}};
187- tf2::fromMsg (stamped.transform , transform);
188- point = transform * point;
189- point.setZ (get_ground_height (point));
190- point = transform.inverse () * point;
271+ tf2::doTransform (point, point, stamped);
191272 } catch (tf2::TransformException & exception ) {
192273 RCLCPP_WARN_STREAM (logger, " failed to lookup transform: " exception .what ());
193274 return std::nullopt;
194275 }
195276
196- RCLCPP_DEBUG (logger, " modified point: %.3f %.3f %.3f" getX () , point.getY () , point.getZ () );
277+ RCLCPP_DEBUG (logger, " modified point: %.3f %.3f %.3f" x , point.y , point.z );
197278
198- Point result;
199- result.x = point.getX ();
200- result.y = point.getY ();
201- result.z = point.getZ ();
202- return result;
279+ return point;
203280}
204281
205282MapHeightFitter::MapHeightFitter (rclcpp::Node * node)
![pre-commit-ci[bot]](https://avatars.githubusercontent.com/in/68672?v=4&size=40){kind=avatar}
0 commit comments