1717#include " autoware/motion_utils/trajectory/trajectory.hpp"
1818#include " autoware/universe_utils/geometry/geometry.hpp"
1919
20+ #include < autoware/lanelet2_utils/conversion.hpp>
2021#include < autoware_lanelet2_extension/regulatory_elements/Forward.hpp>
21- #include < autoware_lanelet2_extension/utility/message_conversion.hpp>
22- #include < autoware_lanelet2_extension/utility/utilities.hpp>
2322#include < magic_enum.hpp>
2423
2524#include < lanelet2_core/geometry/Lanelet.h>
@@ -74,7 +73,7 @@ double calcElevationAngle(const lanelet::ConstLanelet & lane, const Pose & pose)
7473 const auto to_ros_msg = [](const auto & line) {
7574 std::vector<geometry_msgs::msg::Point> points;
7675 std::for_each (line.begin (), line.end (), [&points](const auto & p) {
77- points.push_back (lanelet::utils::conversion::toGeomMsgPt (p));
76+ points.push_back (autoware::experimental::lanelet2_utils::to_ros (p));
7877 });
7978 return points;
8079 };
@@ -97,7 +96,7 @@ double getLaneWidth(const lanelet::ConstLanelet & lane)
9796 const auto to_ros_msg = [](const auto & line) {
9897 std::vector<geometry_msgs::msg::Point> points;
9998 std::for_each (line.begin (), line.end (), [&points](const auto & p) {
100- points.push_back (lanelet::utils::conversion::toGeomMsgPt (p));
99+ points.push_back (autoware::experimental::lanelet2_utils::to_ros (p));
101100 });
102101 return points;
103102 };
@@ -116,7 +115,7 @@ double getMaxCurvature(const lanelet::ConstLanelets & lanes)
116115 const auto to_ros_msg = [](const auto & line) {
117116 std::vector<geometry_msgs::msg::Point> points;
118117 std::for_each (line.begin (), line.end (), [&points](const auto & p) {
119- points.push_back (lanelet::utils::conversion::toGeomMsgPt (p));
118+ points.push_back (autoware::experimental::lanelet2_utils::to_ros (p));
120119 });
121120 return points;
122121 };
@@ -143,7 +142,7 @@ std::pair<double, double> getLaneWidth(const lanelet::ConstLanelets & lanes)
143142 const auto to_ros_msg = [](const auto & line) {
144143 std::vector<geometry_msgs::msg::Point> points;
145144 std::for_each (line.begin (), line.end (), [&points](const auto & p) {
146- points.push_back (lanelet::utils::conversion::toGeomMsgPt (p));
145+ points.push_back (autoware::experimental::lanelet2_utils::to_ros (p));
147146 });
148147 return points;
149148 };
@@ -172,7 +171,7 @@ std::pair<double, double> getElevation(const lanelet::ConstLanelets & lanes)
172171 const auto to_ros_msg = [](const auto & line) {
173172 std::vector<geometry_msgs::msg::Point> points;
174173 std::for_each (line.begin (), line.end (), [&points](const auto & p) {
175- points.push_back (lanelet::utils::conversion::toGeomMsgPt (p));
174+ points.push_back (autoware::experimental::lanelet2_utils::to_ros (p));
176175 });
177176 return points;
178177 };
0 commit comments