feat(planning_api_interface): add publish steering factor (#1706)

* add basic planning api interface

Signed-off-by: tkhmy <tkh.my.p@gmail.com>

* add clean function

Signed-off-by: tkhmy <tkh.my.p@gmail.com>

* add publish empty steering factor

Signed-off-by: tkhmy <tkh.my.p@gmail.com>

* add publish lanechange steering factor

Signed-off-by: tkhmy <tkh.my.p@gmail.com>

* add pull out

Signed-off-by: khtan <tkh.my.p@gmail.com>

* add pull over

Signed-off-by: khtan <tkh.my.p@gmail.com>

* add intersection

Signed-off-by: khtan <tkh.my.p@gmail.com>

* manually add steering factor to prevent conflict

Signed-off-by: khtan <tkh.my.p@gmail.com>

* ci(pre-commit): autofix

* feat(rtc_interface): add_finish_distance

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* use updated distance

Signed-off-by: khtan <tkh.my.p@gmail.com>

* ci(pre-commit): autofix

* fix cpplint

Signed-off-by: khtan <tkh.my.p@gmail.com>

* add avoidance

Signed-off-by: khtan <tkh.my.p@gmail.com>

* ci(pre-commit): autofix

* fix avoidance

Signed-off-by: khtan <tkh.my.p@gmail.com>

* fix avoidance pubish

Signed-off-by: khtan <tkh.my.p@gmail.com>

* clean up lane change

Signed-off-by: khtan <tkh.my.p@gmail.com>

* fix pull out

Signed-off-by: khtan <tkh.my.p@gmail.com>

* clean up

Signed-off-by: khtan <tkh.my.p@gmail.com>

* remove over commit

Signed-off-by: khtan <tkh.my.p@gmail.com>

* add back pull out

Signed-off-by: khtan <tkh.my.p@gmail.com>

* ci(pre-commit): autofix

* add sterring api for intersect

Signed-off-by: khtan <tkh.my.p@gmail.com>

* fix pull out

Signed-off-by: khtan <tkh.my.p@gmail.com>

* add commend

Signed-off-by: khtan <tkh.my.p@gmail.com>

* get intersection distance

Signed-off-by: khtan <tkh.my.p@gmail.com>

* add trying status for intersection

Signed-off-by: khtan <tkh.my.p@gmail.com>

* change to steering_factor_interface

Signed-off-by: khtan <tkh.my.p@gmail.com>

* change to unique_ptr

Signed-off-by: khtan <tkh.my.p@gmail.com>

* fix pose and distance array to 2

Signed-off-by: khtan <tkh.my.p@gmail.com>

* change to array

Signed-off-by: khtan <tkh.my.p@gmail.com>

* make member variable private and add public function to get the variable

Signed-off-by: khtan <tkh.my.p@gmail.com>

* ci(pre-commit): autofix

* change to use modified_goal_pose_ instead of getRefinedGoal()

Signed-off-by: khtan <tkh.my.p@gmail.com>

* add straight direction, fix pull over and pull out error

Signed-off-by: khtan <tkh.my.p@gmail.com>

* Update planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp

change to get the signal from getTurnInfo()

Signed-off-by: tkhmy <tkh.my.p@gmail.com>

Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>

* Update planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp

encapsulated direction variable in lane change

Signed-off-by: tkhmy <tkh.my.p@gmail.com>

Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com>

* Update planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp

encapsulated direction variable in lane change

Signed-off-by: tkhmy <tkh.my.p@gmail.com>

Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com>

* encapsulated direction variable

Signed-off-by: khtan <tkh.my.p@gmail.com>

* functionalize steering factor for intersection

Signed-off-by: khtan <tkh.my.p@gmail.com>

* change to use autoware_adapi_v1_msgs

Signed-off-by: khtan <tkh.my.p@gmail.com>

* change topic naming

Signed-off-by: khtan <tkh.my.p@gmail.com>

Signed-off-by: tkhmy <tkh.my.p@gmail.com>
Signed-off-by: khtan <tkh.my.p@gmail.com>
Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>
Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>
Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com>

Author: 5 people

planning/behavior_path_planner/CMakeLists.txt

@@ -12,6 +12,7 @@ ament_auto_add_library(behavior_path_planner_node SHARED
   src/utilities.cpp
   src/path_utilities.cpp
   src/debug_utilities.cpp
+  src/steering_factor_interface.cpp
   src/turn_signal_decider.cpp
   src/scene_module/scene_module_bt_node_interface.cpp
   src/scene_module/side_shift/side_shift_module.cpp

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -23,6 +23,7 @@
 #include "behavior_path_planner/scene_module/pull_out/pull_out_module.hpp"
 #include "behavior_path_planner/scene_module/pull_over/pull_over_module.hpp"
 #include "behavior_path_planner/scene_module/side_shift/side_shift_module.hpp"
+#include "behavior_path_planner/steering_factor_interface.hpp"
 #include "behavior_path_planner/turn_signal_decider.hpp"
 
 #include <tier4_autoware_utils/ros/self_pose_listener.hpp>
@@ -72,6 +73,7 @@ using geometry_msgs::msg::TwistStamped;
 using nav_msgs::msg::OccupancyGrid;
 using nav_msgs::msg::Odometry;
 using rcl_interfaces::msg::SetParametersResult;
+using steering_factor_interface::SteeringFactorInterface;
 using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
 using tier4_planning_msgs::msg::PathChangeModule;
 using tier4_planning_msgs::msg::Scenario;
@@ -98,6 +100,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
 
   std::shared_ptr<PlannerData> planner_data_;
   std::shared_ptr<BehaviorTreeManager> bt_manager_;
+  std::unique_ptr<SteeringFactorInterface> steering_factor_interface_ptr_;
   tier4_autoware_utils::SelfPoseListener self_pose_listener_{this};
   Scenario::SharedPtr current_scenario_{nullptr};
 
@@ -171,6 +174,11 @@ class BehaviorPathPlannerNode : public rclcpp::Node
    */
   bool isForcedCandidatePath() const;
 
+  /**
+   * @brief publish the steering factor from intersection
+   */
+  void publish_steering_factor(const TurnIndicatorsCommand & turn_signal);
+
   template <class T>
   size_t findEgoIndex(const std::vector<T> & points) const
   {

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp

@@ -124,6 +124,11 @@ class AvoidanceModule : public SceneModuleInterface
         path.points, ego_position, left_shift.finish_pose.position);
       rtc_interface_left_.updateCooperateStatus(
         left_shift.uuid, true, start_distance, finish_distance, clock_->now());
+      if (finish_distance > -1.0e-03) {
+        steering_factor_interface_ptr_->updateSteeringFactor(
+          {left_shift.start_pose, left_shift.finish_pose}, {start_distance, finish_distance},
+          SteeringFactor::AVOIDANCE_PATH_CHANGE, SteeringFactor::LEFT, SteeringFactor::TURNING, "");
+      }
     }
 
     for (const auto & right_shift : right_shift_array_) {
@@ -133,6 +138,12 @@ class AvoidanceModule : public SceneModuleInterface
         path.points, ego_position, right_shift.finish_pose.position);
       rtc_interface_right_.updateCooperateStatus(
         right_shift.uuid, true, start_distance, finish_distance, clock_->now());
+      if (finish_distance > -1.0e-03) {
+        steering_factor_interface_ptr_->updateSteeringFactor(
+          {right_shift.start_pose, right_shift.finish_pose}, {start_distance, finish_distance},
+          SteeringFactor::AVOIDANCE_PATH_CHANGE, SteeringFactor::RIGHT, SteeringFactor::TURNING,
+          "");
+      }
     }
   }
 

planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp

@@ -18,10 +18,12 @@
 #include "behavior_path_planner/data_manager.hpp"
 #include "behavior_path_planner/utilities.hpp"
 
+#include <behavior_path_planner/steering_factor_interface.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <route_handler/route_handler.hpp>
 #include <rtc_interface/rtc_interface.hpp>
 
+#include <autoware_adapi_v1_msgs/msg/steering_factor_array.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
 #include <autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp>
@@ -40,10 +42,12 @@
 
 namespace behavior_path_planner
 {
+using autoware_adapi_v1_msgs::msg::SteeringFactor;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
 using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand;
 using rtc_interface::RTCInterface;
+using steering_factor_interface::SteeringFactorInterface;
 using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
 using unique_identifier_msgs::msg::UUID;
 using visualization_msgs::msg::MarkerArray;
@@ -216,6 +220,14 @@ class SceneModuleInterface
     return false;
   }
 
+  virtual void publishSteeringFactor()
+  {
+    if (!steering_factor_interface_ptr_) {
+      return;
+    }
+    steering_factor_interface_ptr_->publishSteeringFactor(clock_->now());
+  }
+
   /**
    * @brief set planner data
    */
@@ -249,6 +261,7 @@ class SceneModuleInterface
   mutable MarkerArray debug_marker_;
 
   std::shared_ptr<RTCInterface> rtc_interface_ptr_;
+  std::unique_ptr<SteeringFactorInterface> steering_factor_interface_ptr_;
   UUID uuid_;
   bool is_waiting_approval_;
 

planning/behavior_path_planner/include/behavior_path_planner/steering_factor_interface.hpp

@@ -0,0 +1,56 @@
+// 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 BEHAVIOR_PATH_PLANNER__STEERING_FACTOR_INTERFACE_HPP_
+#define BEHAVIOR_PATH_PLANNER__STEERING_FACTOR_INTERFACE_HPP_
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "autoware_adapi_v1_msgs/msg/steering_factor_array.hpp"
+#include "geometry_msgs/msg/pose.hpp"
+#include "unique_identifier_msgs/msg/uuid.hpp"
+
+#include <mutex>
+#include <string>
+#include <vector>
+
+namespace steering_factor_interface
+{
+using autoware_adapi_v1_msgs::msg::SteeringFactor;
+using autoware_adapi_v1_msgs::msg::SteeringFactorArray;
+using geometry_msgs::msg::Pose;
+
+class SteeringFactorInterface
+{
+public:
+  SteeringFactorInterface(rclcpp::Node * node, const std::string & name);
+  void publishSteeringFactor(const rclcpp::Time & stamp);
+  void updateSteeringFactor(
+    const std::array<Pose, 2> & pose, const std::array<double, 2> distance, const uint16_t type,
+    const uint16_t direction, const uint16_t status, const std::string detail);
+  void clearSteeringFactors();
+
+private:
+  rclcpp::Logger getLogger() const;
+
+  rclcpp::Publisher<SteeringFactorArray>::SharedPtr pub_steering_factors_;
+
+  std::mutex mutex_;
+  rclcpp::Logger logger_;
+  SteeringFactorArray registered_steering_factors_;
+};
+
+}  // namespace steering_factor_interface
+
+#endif  // BEHAVIOR_PATH_PLANNER__STEERING_FACTOR_INTERFACE_HPP_

planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp

@@ -22,6 +22,7 @@
 
 #include <lanelet2_core/LaneletMap.h>
 
+#include <limits>
 #include <memory>
 #include <utility>
 
@@ -45,6 +46,9 @@ class TurnSignalDecider
     intersection_search_distance_ = intersection_search_distance;
   }
 
+  std::pair<bool, bool> getIntersectionTurnSignalFlag();
+  std::pair<Pose, double> getIntersectionPoseAndDistance();
+
 private:
   std::pair<TurnIndicatorsCommand, double> getIntersectionTurnSignal(
     const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx,
@@ -56,6 +60,10 @@ class TurnSignalDecider
   // data
   double intersection_search_distance_{0.0};
   double base_link2front_{0.0};
+  mutable bool intersection_turn_signal_ = false;
+  mutable bool approaching_intersection_turn_signal_ = false;
+  mutable double intersection_distance_ = std::numeric_limits<double>::max();
+  mutable Pose intersection_pose_point_ = Pose();
 };
 }  // namespace behavior_path_planner
 

planning/behavior_path_planner/package.xml

@@ -39,6 +39,7 @@
 
   <build_depend>autoware_cmake</build_depend>
 
+  <depend>autoware_adapi_v1_msgs</depend>
   <depend>autoware_auto_perception_msgs</depend>
   <depend>autoware_auto_planning_msgs</depend>
   <depend>autoware_auto_vehicle_msgs</depend>

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -149,6 +149,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
       planner_data_->parameters.base_link2front, intersection_search_distance);
   }
 
+  steering_factor_interface_ptr_ = std::make_unique<SteeringFactorInterface>(this, "intersection");
+
   // Start timer
   {
     const auto planning_hz = declare_parameter("planning_hz", 10.0);
@@ -614,6 +616,8 @@ void BehaviorPathPlannerNode::run()
     hazard_signal.stamp = get_clock()->now();
     turn_signal_publisher_->publish(turn_signal);
     hazard_signal_publisher_->publish(hazard_signal);
+
+    publish_steering_factor(turn_signal);
   }
 
   // for debug
@@ -629,6 +633,36 @@ void BehaviorPathPlannerNode::run()
   RCLCPP_DEBUG(get_logger(), "----- behavior path planner end -----\n\n");
 }
 
+void BehaviorPathPlannerNode::publish_steering_factor(const TurnIndicatorsCommand & turn_signal)
+{
+  const auto [intersection_flag, approaching_intersection_flag] =
+    turn_signal_decider_.getIntersectionTurnSignalFlag();
+  if (intersection_flag || approaching_intersection_flag) {
+    const uint16_t steering_factor_direction = std::invoke([&turn_signal]() {
+      if (turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) {
+        return SteeringFactor::LEFT;
+      }
+      return SteeringFactor::RIGHT;
+    });
+
+    const auto [intersection_pose, intersection_distance] =
+      turn_signal_decider_.getIntersectionPoseAndDistance();
+    const uint16_t steering_factor_state = std::invoke([&intersection_flag]() {
+      if (intersection_flag) {
+        return SteeringFactor::TURNING;
+      }
+      return SteeringFactor::TRYING;
+    });
+
+    steering_factor_interface_ptr_->updateSteeringFactor(
+      {intersection_pose, intersection_pose}, {intersection_distance, intersection_distance},
+      SteeringFactor::INTERSECTION, steering_factor_direction, steering_factor_state, "");
+  } else {
+    steering_factor_interface_ptr_->clearSteeringFactors();
+  }
+  steering_factor_interface_ptr_->publishSteeringFactor(get_clock()->now());
+}
+
 PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath(
   const BehaviorModuleOutput & bt_output, const std::shared_ptr<PlannerData> planner_data)
 {

planning/behavior_path_planner/src/behavior_tree_manager.cpp

@@ -103,6 +103,7 @@ BehaviorModuleOutput BehaviorTreeManager::run(const std::shared_ptr<PlannerData>
       m->onExit();
     }
     m->publishRTCStatus();
+    m->publishSteeringFactor();
   });
   return output;
 }

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -56,6 +56,7 @@ AvoidanceModule::AvoidanceModule(
   uuid_right_{generateUUID()}
 {
   using std::placeholders::_1;
+  steering_factor_interface_ptr_ = std::make_unique<SteeringFactorInterface>(&node, "avoidance");
 }
 
 bool AvoidanceModule::isExecutionRequested() const
@@ -2082,6 +2083,18 @@ CandidateOutput AvoidanceModule::planCandidate() const
     output.lateral_shift = new_shift_points->at(i).getRelativeLength();
     output.start_distance_to_path_change = new_shift_points->front().start_longitudinal;
     output.finish_distance_to_path_change = new_shift_points->back().end_longitudinal;
+
+    const uint16_t steering_factor_direction = std::invoke([&output]() {
+      if (output.lateral_shift > 0.0) {
+        return SteeringFactor::LEFT;
+      }
+      return SteeringFactor::RIGHT;
+    });
+    steering_factor_interface_ptr_->updateSteeringFactor(
+      {new_shift_points->front().start, new_shift_points->back().end},
+      {output.start_distance_to_path_change, output.finish_distance_to_path_change},
+      SteeringFactor::AVOIDANCE_PATH_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING,
+      "");
   }
 
   const size_t ego_idx = findEgoIndex(shifted_path.path.points);
@@ -2488,6 +2501,7 @@ void AvoidanceModule::onExit()
   current_state_ = BT::NodeStatus::IDLE;
   clearWaitingApproval();
   removeRTCStatus();
+  steering_factor_interface_ptr_->clearSteeringFactors();
 }
 
 void AvoidanceModule::initVariables()

planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp

@@ -46,6 +46,7 @@ LaneChangeModule::LaneChangeModule(
   uuid_left_{generateUUID()},
   uuid_right_{generateUUID()}
 {
+  steering_factor_interface_ptr_ = std::make_unique<SteeringFactorInterface>(&node, "lane_change");
 }
 
 BehaviorModuleOutput LaneChangeModule::run()
@@ -61,11 +62,24 @@ BehaviorModuleOutput LaneChangeModule::run()
     status_.lane_change_path.shift_point.start.position);
   const double finish_distance = motion_utils::calcSignedArcLength(
     output.path->points, current_pose.position, status_.lane_change_path.shift_point.end.position);
-  if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) {
-    waitApprovalLeft(start_distance, finish_distance);
-  } else if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) {
-    waitApprovalRight(start_distance, finish_distance);
-  }
+
+  const uint16_t steering_factor_direction =
+    std::invoke([this, &start_distance, &finish_distance, &turn_signal_info]() {
+      if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) {
+        waitApprovalLeft(start_distance, finish_distance);
+        return SteeringFactor::LEFT;
+      }
+      if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) {
+        waitApprovalRight(start_distance, finish_distance);
+        return SteeringFactor::RIGHT;
+      }
+      return SteeringFactor::UNKNOWN;
+    });
+  // TODO(tkhmy) add handle status TRYING
+  steering_factor_interface_ptr_->updateSteeringFactor(
+    {status_.lane_change_path.shift_point.start, status_.lane_change_path.shift_point.end},
+    {start_distance, finish_distance}, SteeringFactor::LANE_CHANGE, steering_factor_direction,
+    SteeringFactor::TURNING, "");
   return output;
 }
 
@@ -85,6 +99,7 @@ void LaneChangeModule::onExit()
 {
   clearWaitingApproval();
   removeRTCStatus();
+  steering_factor_interface_ptr_->clearSteeringFactors();
   debug_marker_.markers.clear();
   current_state_ = BT::NodeStatus::IDLE;
   RCLCPP_DEBUG(getLogger(), "LANE_CHANGE onExit");
@@ -209,6 +224,18 @@ CandidateOutput LaneChangeModule::planCandidate() const
     selected_path.path.points, planner_data_->self_pose->pose.position,
     selected_path.shift_point.end.position);
 
+  const uint16_t steering_factor_direction = std::invoke([&output]() {
+    if (output.lateral_shift > 0.0) {
+      return SteeringFactor::LEFT;
+    }
+    return SteeringFactor::RIGHT;
+  });
+
+  steering_factor_interface_ptr_->updateSteeringFactor(
+    {selected_path.shift_point.start, selected_path.shift_point.end},
+    {output.start_distance_to_path_change, output.finish_distance_to_path_change},
+    SteeringFactor::LANE_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING, "");
+
   return output;
 }