Merge pull request #35 from wep21/autoware-core

feat: add autoware core

Author: Tobias-Fischer

patch/ros-kilted-autoware-adapi-adaptors.patch

@@ -0,0 +1,32 @@
+diff --git a/src/initial_pose_adaptor.cpp b/src/initial_pose_adaptor.cpp
+index 3b9aaab..044e314 100644
+--- a/src/initial_pose_adaptor.cpp
++++ b/src/initial_pose_adaptor.cpp
+@@ -43,7 +43,7 @@ InitialPoseAdaptor::InitialPoseAdaptor(const rclcpp::NodeOptions & options)
+     std::bind(&InitialPoseAdaptor::on_initial_pose, this, std::placeholders::_1));
+ 
+   cli_initialize_ =
+-    create_client<Initialize::Service>(Initialize::name, rmw_qos_profile_services_default);
++    create_client<Initialize::Service>(Initialize::name, rclcpp::ServicesQoS());
+ }
+ 
+ void InitialPoseAdaptor::on_initial_pose(const PoseWithCovarianceStamped::ConstSharedPtr msg)
+diff --git a/src/routing_adaptor.cpp b/src/routing_adaptor.cpp
+index 9c89e2b..415f570 100644
+--- a/src/routing_adaptor.cpp
++++ b/src/routing_adaptor.cpp
+@@ -34,11 +34,11 @@ RoutingAdaptor::RoutingAdaptor(const rclcpp::NodeOptions & options)
+     "~/input/waypoint", 10, std::bind(&RoutingAdaptor::on_waypoint, this, _1));
+ 
+   cli_reroute_ = create_client<ChangeRoutePoints::Service>(
+-    ChangeRoutePoints::name, rmw_qos_profile_services_default);
++    ChangeRoutePoints::name, rclcpp::ServicesQoS());
+   cli_route_ =
+-    create_client<SetRoutePoints::Service>(SetRoutePoints::name, rmw_qos_profile_services_default);
++    create_client<SetRoutePoints::Service>(SetRoutePoints::name, rclcpp::ServicesQoS());
+   cli_clear_ =
+-    create_client<ClearRoute::Service>(ClearRoute::name, rmw_qos_profile_services_default);
++    create_client<ClearRoute::Service>(ClearRoute::name, rclcpp::ServicesQoS());
+ 
+   const auto state_qos = rclcpp::QoS{RouteState::depth}
+                            .reliability(RouteState::reliability)

patch/ros-kilted-autoware-default-adapi.patch

@@ -0,0 +1,58 @@
+diff --git a/src/interface.cpp b/src/interface.cpp
+index 16e2031..26108bc 100644
+--- a/src/interface.cpp
++++ b/src/interface.cpp
+@@ -21,7 +21,7 @@ InterfaceNode::InterfaceNode(const rclcpp::NodeOptions & options)
+ : Node("interface", options),
+   srv_(create_service<Version::Service>(
+     Version::name,
+-    [this](
++    [](
+       const Version::Service::Request::SharedPtr, const Version::Service::Response::SharedPtr res) {
+       res->major = 1;
+       res->minor = 9;
+diff --git a/src/localization.cpp b/src/localization.cpp
+index e4eca71..ce8f46a 100644
+--- a/src/localization.cpp
++++ b/src/localization.cpp
+@@ -37,7 +37,7 @@ LocalizationNode::LocalizationNode(const rclcpp::NodeOptions & options)
+   srv_initialize_ = create_service<autoware::adapi_specs::localization::Initialize::Service>(
+     autoware::adapi_specs::localization::Initialize::name,
+     std::bind(&LocalizationNode::on_initialize, this, std::placeholders::_1, std::placeholders::_2),
+-    rmw_qos_profile_services_default, group_cli_);
++    rclcpp::ServicesQoS(), group_cli_);
+ 
+   // Component Interface
+   sub_state_ = create_subscription<
+diff --git a/src/routing.cpp b/src/routing.cpp
+index 1528db6..15580d0 100644
+--- a/src/routing.cpp
++++ b/src/routing.cpp
+@@ -100,15 +100,15 @@ RoutingNode::RoutingNode(const rclcpp::NodeOptions & options)
+   cli_clear_route_ =
+     create_client<autoware::component_interface_specs::planning::ClearRoute::Service>(
+       autoware::component_interface_specs::planning::ClearRoute::name,
+-      rmw_qos_profile_services_default, group_cli_);
++      rclcpp::ServicesQoS(), group_cli_);
+   cli_set_waypoint_route_ =
+     create_client<autoware::component_interface_specs::planning::SetWaypointRoute::Service>(
+       autoware::component_interface_specs::planning::SetWaypointRoute::name,
+-      rmw_qos_profile_services_default, group_cli_);
++      rclcpp::ServicesQoS(), group_cli_);
+   cli_set_lanelet_route_ =
+     create_client<autoware::component_interface_specs::planning::SetLaneletRoute::Service>(
+       autoware::component_interface_specs::planning::SetLaneletRoute::name,
+-      rmw_qos_profile_services_default, group_cli_);
++      rclcpp::ServicesQoS(), group_cli_);
+   sub_operation_mode_ =
+     create_subscription<autoware::component_interface_specs::system::OperationModeState::Message>(
+       autoware::component_interface_specs::system::OperationModeState::name,
+@@ -119,7 +119,7 @@ RoutingNode::RoutingNode(const rclcpp::NodeOptions & options)
+   cli_operation_mode_ =
+     create_client<autoware::component_interface_specs::system::ChangeOperationMode::Service>(
+       autoware::component_interface_specs::system::ChangeOperationMode::name,
+-      rmw_qos_profile_services_default, group_cli_);
++      rclcpp::ServicesQoS(), group_cli_);
+ 
+   is_autoware_control_ = false;
+   is_auto_mode_ = false;

patch/ros-kilted-autoware-ground-filter.patch

@@ -0,0 +1,33 @@
+diff --git a/include/autoware/ground_filter/grid.hpp b/include/autoware/ground_filter/grid.hpp
+index b69426c..c57fe92 100644
+--- a/include/autoware/ground_filter/grid.hpp
++++ b/include/autoware/ground_filter/grid.hpp
+@@ -25,6 +25,12 @@
+ #include <utility>
+ #include <vector>
+ 
++#ifndef M_PIf
++#define M_PIf  3.14159265358979324f
++#define M_PI_2f 2 * M_PIf
++#define M_PI_4f 4 * M_PIf
++#endif
++
+ namespace
+ {
+ 
+diff --git a/src/node.cpp b/src/node.cpp
+index 1fd0f90..9305f75 100644
+--- a/src/node.cpp
++++ b/src/node.cpp
+@@ -204,9 +204,9 @@ void GroundFilterComponent::subscribe()
+   if (use_indices_) {
+     // Subscribe to the input using a filter
+     sub_input_filter_.subscribe(
+-      this, "input", rclcpp::SensorDataQoS().keep_last(max_queue_size_).get_rmw_qos_profile());
++      this, "input", rclcpp::SensorDataQoS().keep_last(max_queue_size_));
+     sub_indices_filter_.subscribe(
+-      this, "indices", rclcpp::SensorDataQoS().keep_last(max_queue_size_).get_rmw_qos_profile());
++      this, "indices", rclcpp::SensorDataQoS().keep_last(max_queue_size_));
+ 
+     if (approximate_sync_) {
+       sync_input_indices_a_ = std::make_shared<

patch/ros-kilted-autoware-lanelet2-extension.patch

@@ -17,16 +17,3 @@ index a3ec429..6a64b82 100644
  ament_auto_add_executable(${PROJECT_NAME}_sample src/sample_code.cpp)
  target_link_libraries(${PROJECT_NAME}_sample
    ${PROJECT_NAME}_lib
-diff --git a/lib/query.cpp b/lib/query.cpp
-index 4c61bba..11a9cca 100644
---- a/lib/query.cpp
-+++ b/lib/query.cpp
-@@ -32,7 +32,7 @@
- #include <lanelet2_core/geometry/Lanelet.h>
- #include <lanelet2_core/primitives/Lanelet.h>
- #include <lanelet2_routing/RoutingGraph.h>
--#include <tf2/utils.h>
-+#include <tf2/utils.hpp>
- 
- #include <iostream>
- #include <utility>

patch/ros-kilted-autoware-map-loader.osx.patch

@@ -0,0 +1,26 @@
+diff --git a/CMakeLists.txt b/CMakeLists.txt
+index 9f8f868..bfa9cde 100644
+--- a/CMakeLists.txt
++++ b/CMakeLists.txt
+@@ -15,7 +15,7 @@ ament_auto_add_library(pointcloud_map_loader_node SHARED
+   src/pointcloud_map_loader/utils.cpp
+ )
+ target_link_libraries(pointcloud_map_loader_node ${PCL_LIBRARIES})
+-target_link_libraries(pointcloud_map_loader_node yaml-cpp)
++target_link_libraries(pointcloud_map_loader_node yaml-cpp::yaml-cpp fmt::fmt)
+ 
+ target_include_directories(pointcloud_map_loader_node
+   SYSTEM PUBLIC
+diff --git a/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp
+index 54e9432..62ae5f6 100644
+--- a/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp
++++ b/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp
+@@ -104,7 +104,7 @@ void Lanelet2MapLoaderNode::on_map_projector_info(
+     if (map_major_ver > static_cast<uint64_t>(lanelet::autoware::version)) {
+       RCLCPP_WARN(
+         get_logger(),
+-        "format_version(%ld) of the provided map(%s) is larger than the supported version(%ld)",
++        "format_version(%llu) of the provided map(%s) is larger than the supported version(%llu)",
+         map_major_ver, lanelet2_filename.c_str(),
+         static_cast<uint64_t>(lanelet::autoware::version));
+       if (!allow_unsupported_version) {

patch/ros-kilted-autoware-path-generator.osx.patch

@@ -0,0 +1,22 @@
+diff --git a/src/node.cpp b/src/node.cpp
+index 3530c79..9527336 100644
+--- a/src/node.cpp
++++ b/src/node.cpp
+@@ -256,7 +256,7 @@ std::optional<PathWithLaneId> PathGenerator::generate_path(
+     utils::get_lanelets_within_route_up_to(*current_lanelet_, planner_data_, backward_length);
+   if (!backward_lanelets_within_route) {
+     RCLCPP_ERROR(
+-      get_logger(), "Failed to get backward lanelets within route for current lanelet (id: %ld)",
++      get_logger(), "Failed to get backward lanelets within route for current lanelet (id: %lld)",
+       current_lanelet_->id());
+     return std::nullopt;
+   }
+@@ -284,7 +284,7 @@ std::optional<PathWithLaneId> PathGenerator::generate_path(
+     utils::get_lanelets_within_route_after(*current_lanelet_, planner_data_, forward_length);
+   if (!forward_lanelets_within_route) {
+     RCLCPP_ERROR(
+-      get_logger(), "Failed to get forward lanelets within route for current lanelet (id: %ld)",
++      get_logger(), "Failed to get forward lanelets within route for current lanelet (id: %lld)",
+       current_lanelet_->id());
+     return std::nullopt;
+   }

patch/ros-kilted-autoware-pose-initializer.osx.patch

@@ -0,0 +1,22 @@
+diff --git a/src/route_handler.cpp b/src/route_handler.cpp
+index 878f1aa..d3604b4 100644
+--- a/src/route_handler.cpp
++++ b/src/route_handler.cpp
+@@ -2073,7 +2073,7 @@ bool RouteHandler::planPathLaneletsBetweenCheckpoints(
+           RCLCPP_WARN(
+             logger_,
+             "Failed to find reroute on previous preferred lanelets %s, but on previous route "
+-            "segment %ld still",
++            "segment %lld still",
+             preferred_lanelets_str.str().c_str(), closest_lanelet.id());
+           return closest_lanelet;
+         }
+@@ -2134,7 +2134,7 @@ bool RouteHandler::planPathLaneletsBetweenCheckpoints(
+     const double optional_route_length = optional_route->length2d();
+     const double optional_route_cost = optional_route_length + angle_diff_weight * angle_diff;
+     RCLCPP_DEBUG(
+-      logger_, "Lanelet ID %ld: Route length = %.1f, Angle Diff = %.4f rad, Route cost = %.2f",
++      logger_, "Lanelet ID %lld: Route length = %.1f, Angle Diff = %.4f rad, Route cost = %.2f",
+       st_llt.id(), optional_route_length, angle_diff, optional_route_cost);
+     if (optional_route_cost < min_route_cost) {
+       min_route_cost = optional_route_cost;

patch/ros-kilted-autoware-route-handler.osx.patch

@@ -0,0 +1,25 @@
+diff --git a/include/rosidl_runtime_cpp/traits.hpp b/include/rosidl_runtime_cpp/traits.hpp
+index 2d285b7b2..1f6ba834d 100644
+--- a/include/rosidl_runtime_cpp/traits.hpp
++++ b/include/rosidl_runtime_cpp/traits.hpp
+@@ -128,14 +128,20 @@ inline void value_to_yaml(const std::string & value, std::ostream & out)
+ inline void value_to_yaml(const std::u16string & value, std::ostream & out)
+ {
+   out << "\"";
++#if __cplusplus < 201703L
+   std::wstring_convert<std::codecvt_utf8_utf16<char16_t>, char16_t> convert;
++#endif
+   auto flags = out.flags();
+   size_t index = 0;
+   while (index < value.size()) {
+     uint_least16_t character = static_cast<uint_least16_t>(value[index]);
+     if (!(character & 0xff80)) {  // ASCII
++#if __cplusplus < 201703L
+       std::string character_as_string = convert.to_bytes(character);
+       out << std::hex << character_as_string.c_str();
++#else
++      out << static_cast<char>(character);
++#endif
+     } else if (!(character & 0xff00)) {  // only 1 byte set
+       out << "\\x" << std::hex << std::setw(2) << std::setfill('0') << \
+         character;

patch/ros-kilted-rosidl-runtime-cpp.osx.patch

@@ -146,3 +146,17 @@ proxsuite:
     dep_name: proxsuite
     max_pin: 'x.x'
     override_version: '0.7.2'
+autoware_internal_planning_msgs:
+  build_number: 13
+autoware_planning_msgs:
+  build_number: 13
+autoware_system_msgs:
+  build_number: 13
+autoware_lanelet2_extension:
+  build_number: 13
+autoware_lanelet2_extension_python:
+  build_number: 13
+rclcpp:
+  build_number: 13
+rosidl_runtime_cpp:
+  build_number: 13