feat(default_ad_api): add planning api

common/autoware_ad_api_specs/include/autoware_ad_api_specs/planning.hpp

@@ -0,0 +1,46 @@
+// Copyright 2022 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE_AD_API_SPECS__PLANNING_HPP_
+#define AUTOWARE_AD_API_SPECS__PLANNING_HPP_
+
+#include <rclcpp/qos.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/steering_factor_array.hpp>
+#include <autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp>
+
+namespace autoware_ad_api::planning
+{
+
+struct VelocityFactors
+{
+  using Message = autoware_adapi_v1_msgs::msg::VelocityFactorArray;
+  static constexpr char name[] = "/api/planning/velocity_factors";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+};
+
+struct SteeringFactors
+{
+  using Message = autoware_adapi_v1_msgs::msg::SteeringFactorArray;
+  static constexpr char name[] = "/api/planning/steering_factors";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+};
+
+}  // namespace autoware_ad_api::planning
+
+#endif  // AUTOWARE_AD_API_SPECS__PLANNING_HPP_

common/component_interface_specs/include/component_interface_specs/localization.hpp

@@ -19,6 +19,7 @@
 
 #include <autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>
 #include <autoware_adapi_v1_msgs/srv/initialize_localization.hpp>
+#include <nav_msgs/msg/odometry.hpp>
 
 namespace localization_interface
 {
@@ -38,6 +39,15 @@ struct InitializationState
   static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
 };
 
+struct KinematicState
+{
+  using Message = nav_msgs::msg::Odometry;
+  static constexpr char name[] = "/localization/kinematic_state";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+};
+
 }  // namespace localization_interface
 
 #endif  // COMPONENT_INTERFACE_SPECS__LOCALIZATION_HPP_

common/component_interface_specs/include/component_interface_specs/planning.hpp

@@ -20,6 +20,7 @@
 #include <autoware_adapi_v1_msgs/msg/route_state.hpp>
 #include <autoware_adapi_v1_msgs/srv/clear_route.hpp>
 #include <autoware_adapi_v1_msgs/srv/set_route_points.hpp>
+#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
 #include <autoware_planning_msgs/msg/lanelet_route.hpp>
 #include <autoware_planning_msgs/srv/set_route.hpp>
 
@@ -62,6 +63,15 @@ struct Route
   static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
 };
 
+struct Trajectory
+{
+  using Message = autoware_auto_planning_msgs::msg::Trajectory;
+  static constexpr char name[] = "/planning/scenario_planning/trajectory";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+};
+
 }  // namespace planning_interface
 
 #endif  // COMPONENT_INTERFACE_SPECS__PLANNING_HPP_

system/default_ad_api/CMakeLists.txt

@@ -10,6 +10,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/localization.cpp
   src/motion.cpp
   src/operation_mode.cpp
+  src/planning.cpp
   src/routing.cpp
   src/utils/route_conversion.cpp
   src/compatibility/autoware_state.cpp
@@ -22,6 +23,7 @@ rclcpp_components_register_nodes(${PROJECT_NAME}
   "default_ad_api::LocalizationNode"
   "default_ad_api::MotionNode"
   "default_ad_api::OperationModeNode"
+  "default_ad_api::PlanningNode"
   "default_ad_api::RoutingNode"
 )
 

system/default_ad_api/launch/default_ad_api.launch.py

@@ -36,6 +36,7 @@ def generate_launch_description():
         create_api_node("localization", "LocalizationNode"),
         create_api_node("motion", "MotionNode", parameters=[{"require_accept_start": False}]),
         create_api_node("operation_mode", "OperationModeNode"),
+        create_api_node("planning", "PlanningNode"),
         create_api_node("routing", "RoutingNode"),
     ]
     container = ComposableNodeContainer(

system/default_ad_api/src/planning.cpp

@@ -0,0 +1,151 @@
+// Copyright 2022 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "planning.hpp"
+
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace default_ad_api
+{
+
+template <class T>
+void concat(std::vector<T> & v1, const std::vector<T> & v2)
+{
+  v1.insert(v1.end(), v2.begin(), v2.end());
+}
+
+template <class T>
+std::vector<typename rclcpp::Subscription<T>::SharedPtr> init_factors(
+  rclcpp::Node * node, std::vector<typename T::ConstSharedPtr> & factors,
+  const std::vector<std::string> & topics)
+{
+  const auto callback = [&factors](const int index) {
+    return [&factors, index](const typename T::ConstSharedPtr msg) { factors[index] = msg; };
+  };
+
+  std::vector<typename rclcpp::Subscription<T>::SharedPtr> subs;
+  for (size_t index = 0; index < topics.size(); ++index) {
+    subs.push_back(node->create_subscription<T>(topics[index], rclcpp::QoS(1), callback(index)));
+  }
+  factors.resize(topics.size());
+  return subs;
+}
+
+template <class T>
+T merge_factors(const rclcpp::Time stamp, const std::vector<typename T::ConstSharedPtr> & factors)
+{
+  T message;
+  message.header.stamp = stamp;
+  message.header.frame_id = "map";
+
+  for (const auto & factor : factors) {
+    if (factor) {
+      concat(message.factors, factor->factors);
+    }
+  }
+  return message;
+}
+
+PlanningNode::PlanningNode(const rclcpp::NodeOptions & options) : Node("planning", options)
+{
+  // TODO(Takagi, Isamu): remove default value
+  stop_distance_ = declare_parameter<double>("stop_distance", 1.0);
+  stop_duration_ = declare_parameter<double>("stop_duration", 1.0);
+  stop_checker_ = std::make_unique<VehicleStopChecker>(this, stop_duration_ + 1.0);
+
+  std::vector<std::string> velocity_factor_topics = {
+    "/planning/velocity_factors/blind_spot",
+    "/planning/velocity_factors/crosswalk",
+    "/planning/velocity_factors/detection_area",
+    "/planning/velocity_factors/intersection",
+    "/planning/velocity_factors/merge_from_private",
+    "/planning/velocity_factors/no_stopping_area",
+    "/planning/velocity_factors/obstacle_stop",
+    "/planning/velocity_factors/obstacle_cruise",
+    "/planning/velocity_factors/occlusion_spot",
+    "/planning/velocity_factors/stop_line",
+    "/planning/velocity_factors/surround_obstacle",
+    "/planning/velocity_factors/traffic_light",
+    "/planning/velocity_factors/virtual_traffic_light",
+    "/planning/velocity_factors/walkway"};
+
+  std::vector<std::string> steering_factor_topics = {
+    "/planning/steering_factor/avoidance", "/planning/steering_factor/intersection",
+    "/planning/steering_factor/lane_change", "/planning/steering_factor/pull_out",
+    "/planning/steering_factor/pull_over"};
+
+  sub_velocity_factors_ =
+    init_factors<VelocityFactorArray>(this, velocity_factors_, velocity_factor_topics);
+  sub_steering_factors_ =
+    init_factors<SteeringFactorArray>(this, steering_factors_, steering_factor_topics);
+
+  const auto adaptor = component_interface_utils::NodeAdaptor(this);
+  adaptor.init_pub(pub_velocity_factors_);
+  adaptor.init_pub(pub_steering_factors_);
+  adaptor.init_sub(sub_kinematic_state_, this, &PlanningNode::on_kinematic_state);
+  adaptor.init_sub(sub_trajectory_, this, &PlanningNode::on_trajectory);
+
+  const auto rate = rclcpp::Rate(5);
+  timer_ = rclcpp::create_timer(this, get_clock(), rate.period(), [this]() { on_timer(); });
+}
+
+void PlanningNode::on_trajectory(const Trajectory::ConstSharedPtr msg) { trajectory_ = msg; }
+
+void PlanningNode::on_kinematic_state(const KinematicState::ConstSharedPtr msg)
+{
+  kinematic_state_ = msg;
+
+  geometry_msgs::msg::TwistStamped twist;
+  twist.header = msg->header;
+  twist.twist = msg->twist.twist;
+  stop_checker_->addTwist(twist);
+}
+
+void PlanningNode::on_timer()
+{
+  using autoware_adapi_v1_msgs::msg::VelocityFactor;
+  auto velocity = merge_factors<VelocityFactorArray>(now(), velocity_factors_);
+  auto steering = merge_factors<SteeringFactorArray>(now(), steering_factors_);
+
+  // Set the distance if it is nan.
+  if (trajectory_ && kinematic_state_) {
+    for (auto & factor : velocity.factors) {
+      if (std::isnan(factor.distance)) {
+        const auto & curr_point = kinematic_state_->pose.pose.position;
+        const auto & stop_point = factor.pose.position;
+        const auto & points = trajectory_->points;
+        factor.distance = motion_utils::calcSignedArcLength(points, curr_point, stop_point);
+      }
+    }
+  }
+
+  // Set the status if it is unknown.
+  const auto is_vehicle_stopped = stop_checker_->isVehicleStopped(stop_duration_);
+  for (auto & factor : velocity.factors) {
+    if ((factor.status == VelocityFactor::UNKNOWN) && (!std::isnan(factor.distance))) {
+      const auto is_stopped = is_vehicle_stopped && (factor.distance < stop_distance_);
+      factor.status = is_stopped ? VelocityFactor::STOPPED : VelocityFactor::APPROACHING;
+    }
+  }
+
+  pub_velocity_factors_->publish(velocity);
+  pub_steering_factors_->publish(steering);
+}
+
+}  // namespace default_ad_api
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::PlanningNode)

system/default_ad_api/src/planning.hpp

@@ -0,0 +1,67 @@
+// Copyright 2022 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef PLANNING_HPP_
+#define PLANNING_HPP_
+
+#include <autoware_ad_api_specs/planning.hpp>
+#include <component_interface_specs/localization.hpp>
+#include <component_interface_specs/planning.hpp>
+#include <motion_utils/motion_utils.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <memory>
+#include <vector>
+
+// This file should be included after messages.
+#include "utils/types.hpp"
+
+namespace default_ad_api
+{
+
+class PlanningNode : public rclcpp::Node
+{
+public:
+  explicit PlanningNode(const rclcpp::NodeOptions & options);
+
+private:
+  using VelocityFactorArray = autoware_adapi_v1_msgs::msg::VelocityFactorArray;
+  using SteeringFactorArray = autoware_adapi_v1_msgs::msg::SteeringFactorArray;
+  Pub<autoware_ad_api::planning::VelocityFactors> pub_velocity_factors_;
+  Pub<autoware_ad_api::planning::SteeringFactors> pub_steering_factors_;
+  Sub<planning_interface::Trajectory> sub_trajectory_;
+  Sub<localization_interface::KinematicState> sub_kinematic_state_;
+  std::vector<rclcpp::Subscription<VelocityFactorArray>::SharedPtr> sub_velocity_factors_;
+  std::vector<rclcpp::Subscription<SteeringFactorArray>::SharedPtr> sub_steering_factors_;
+  std::vector<VelocityFactorArray::ConstSharedPtr> velocity_factors_;
+  std::vector<SteeringFactorArray::ConstSharedPtr> steering_factors_;
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  using VehicleStopChecker = motion_utils::VehicleStopCheckerBase;
+  using Trajectory = planning_interface::Trajectory::Message;
+  using KinematicState = localization_interface::KinematicState::Message;
+  void on_trajectory(const Trajectory::ConstSharedPtr msg);
+  void on_kinematic_state(const KinematicState::ConstSharedPtr msg);
+  void on_timer();
+
+  double stop_distance_;
+  double stop_duration_;
+  std::unique_ptr<VehicleStopChecker> stop_checker_;
+  Trajectory::ConstSharedPtr trajectory_;
+  KinematicState::ConstSharedPtr kinematic_state_;
+};
+
+}  // namespace default_ad_api
+
+#endif  // PLANNING_HPP_