Unifiy subscriber

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

control/trajectory_follower/CMakeLists.txt

@@ -25,6 +25,7 @@ set(LATERAL_CONTROLLER_LIB_HEADERS
   include/trajectory_follower/lateral_controller_base.hpp
   include/trajectory_follower/mpc_lateral_controller.hpp
   include/trajectory_follower/sync_data.hpp
+  include/trajectory_follower/input_data.hpp
   include/trajectory_follower/visibility_control.hpp
   include/trajectory_follower/interpolate.hpp
   include/trajectory_follower/lowpass_filter.hpp
@@ -59,6 +60,7 @@ set(LONGITUDINAL_CONTROLLER_LIB_HEADERS
   include/trajectory_follower/longitudinal_controller_base.hpp
   include/trajectory_follower/pid_longitudinal_controller.hpp
   include/trajectory_follower/sync_data.hpp
+  include/trajectory_follower/input_data.hpp
   include/trajectory_follower/debug_values.hpp
   include/trajectory_follower/pid.hpp
   include/trajectory_follower/smooth_stop.hpp

control/trajectory_follower/include/trajectory_follower/input_data.hpp

@@ -0,0 +1,41 @@
+// Copyright 2022 The Autoware Foundation
+//
+// 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 TRAJECTORY_FOLLOWER__INPUT_DATA_HPP_
+#define TRAJECTORY_FOLLOWER__INPUT_DATA_HPP_
+
+#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
+#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+
+namespace autoware
+{
+namespace motion
+{
+namespace control
+{
+namespace trajectory_follower
+{
+struct InputData
+{
+  autoware_auto_planning_msgs::msg::Trajectory::SharedPtr current_trajectory_ptr;
+  nav_msgs::msg::Odometry::SharedPtr current_odometry_ptr;
+  autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr current_steering_ptr;
+};
+}  // namespace trajectory_follower
+}  // namespace control
+}  // namespace motion
+}  // namespace autoware
+
+#endif  // TRAJECTORY_FOLLOWER__INPUT_DATA_HPP_

control/trajectory_follower/include/trajectory_follower/lateral_controller_base.hpp

@@ -16,6 +16,7 @@
 #define TRAJECTORY_FOLLOWER__LATERAL_CONTROLLER_BASE_HPP_
 
 #include "rclcpp/rclcpp.hpp"
+#include "trajectory_follower/input_data.hpp"
 #include "trajectory_follower/sync_data.hpp"
 
 #include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp"
@@ -38,6 +39,7 @@ class LateralControllerBase
 {
 public:
   virtual LateralOutput run() = 0;
+  virtual void setInputData(InputData const & input_data) = 0;
   void sync(LongitudinalSyncData const & longitudinal_sync_data)
   {
     longitudinal_sync_data_ = longitudinal_sync_data;

control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp

@@ -16,6 +16,7 @@
 #define TRAJECTORY_FOLLOWER__LONGITUDINAL_CONTROLLER_BASE_HPP_
 
 #include "rclcpp/rclcpp.hpp"
+#include "trajectory_follower/input_data.hpp"
 #include "trajectory_follower/sync_data.hpp"
 
 #include "autoware_auto_control_msgs/msg/longitudinal_command.hpp"
@@ -37,6 +38,7 @@ class LongitudinalControllerBase
 {
 public:
   virtual LongitudinalOutput run() = 0;
+  virtual void setInputData(InputData const & input_data) = 0;
   void sync(LateralSyncData const & lateral_sync_data) { lateral_sync_data_ = lateral_sync_data; };
 
 protected:

control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp

@@ -29,7 +29,6 @@
 #include "trajectory_follower/mpc_utils.hpp"
 #include "trajectory_follower/qp_solver/qp_solver_osqp.hpp"
 #include "trajectory_follower/qp_solver/qp_solver_unconstr_fast.hpp"
-#include "trajectory_follower/sync_data.hpp"
 #include "trajectory_follower/vehicle_model/vehicle_model_bicycle_dynamics.hpp"
 #include "trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics.hpp"
 #include "trajectory_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp"
@@ -87,12 +86,6 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController
   //!< @brief topic publisher for control diagnostic
   rclcpp::Publisher<autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic>::SharedPtr
     m_pub_diagnostic;
-  //!< @brief topic subscription for reference waypoints
-  rclcpp::Subscription<autoware_auto_planning_msgs::msg::Trajectory>::SharedPtr m_sub_ref_path;
-  //!< @brief subscription for current velocity
-  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr m_sub_odometry;
-  //!< @brief subscription for current steering
-  rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::SteeringReport>::SharedPtr m_sub_steering;
   //!< @brief timer to update after a given interval
   rclcpp::TimerBase::SharedPtr m_timer;
   //!< @brief subscription for transform messages
@@ -142,19 +135,22 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController
   tf2::BufferCore m_tf_buffer{tf2::BUFFER_CORE_DEFAULT_CACHE_TIME};
   tf2_ros::TransformListener m_tf_listener{m_tf_buffer};
 
-  LateralSyncData lateral_sync_data_;
-
   //!< initialize timer to work in real, simulation, and replay
   void initTimer(float64_t period_s);
   /**
    * @brief compute and publish control command for path follow with a constant control period
    */
   LateralOutput run() override;
 
+  /**
+   * @brief set input data like current odometry, trajectory and steering.
+   */
+  void setInputData(InputData const & input_data) override;
+
   /**
    * @brief set m_current_trajectory with received message
    */
-  void onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr);
+  void setTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr);
 
   /**
    * @brief update current_pose from tf
@@ -167,16 +163,6 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController
    */
   bool8_t checkData() const;
 
-  /**
-   * @brief set current_velocity with received message
-   */
-  void onOdometry(const nav_msgs::msg::Odometry::SharedPtr msg);
-
-  /**
-   * @brief set current_steering with received message
-   */
-  void onSteering(const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg);
-
   /**
    * @brief create control command
    * @param [in] ctrl_cmd published control command

control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp

@@ -87,8 +87,6 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal
   rclcpp::Node::SharedPtr node_;
 
   // ros variables
-  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr m_sub_current_velocity;
-  rclcpp::Subscription<autoware_auto_planning_msgs::msg::Trajectory>::SharedPtr m_sub_trajectory;
   rclcpp::Publisher<autoware_auto_control_msgs::msg::LongitudinalCommand>::SharedPtr
     m_pub_control_cmd;
   rclcpp::Publisher<autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic>::SharedPtr
@@ -222,20 +220,24 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal
    * @brief set current and previous velocity with received message
    * @param [in] msg current state message
    */
-  void callbackCurrentVelocity(const nav_msgs::msg::Odometry::ConstSharedPtr msg);
+  void setCurrentVelocity(const nav_msgs::msg::Odometry::ConstSharedPtr msg);
 
   /**
    * @brief set reference trajectory with received message
    * @param [in] msg trajectory message
    */
-  void callbackTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg);
+  void setTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg);
 
   /**
    * @brief compute control command, and publish periodically
    */
-  // void callbackTimerControl();
   LongitudinalOutput run() override;
 
+  /**
+   * @brief set input data like current odometry and trajectory.
+   */
+  void setInputData(InputData const & input_data) override;
+
   /**
    * @brief calculate data for controllers whose type is ControlData
    * @param [in] current_pose current ego pose

control/trajectory_follower/src/mpc_lateral_controller.cpp

@@ -150,15 +150,6 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node}
   m_pub_diagnostic =
     node_->create_publisher<autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic>(
       "~/output/lateral_diagnostic", 1);
-  m_sub_ref_path = node_->create_subscription<autoware_auto_planning_msgs::msg::Trajectory>(
-    "~/input/current_trajectory", rclcpp::QoS{1},
-    std::bind(&MpcLateralController::onTrajectory, this, _1));
-  m_sub_steering = node_->create_subscription<autoware_auto_vehicle_msgs::msg::SteeringReport>(
-    "~/input/current_steering", rclcpp::QoS{1},
-    std::bind(&MpcLateralController::onSteering, this, _1));
-  m_sub_odometry = node_->create_subscription<nav_msgs::msg::Odometry>(
-    "~/input/current_odometry", rclcpp::QoS{1},
-    std::bind(&MpcLateralController::onOdometry, this, _1));
 
   // TODO(Frederik.Beaujean) ctor is too long, should factor out parameter declarations
   declareMPCparameters();
@@ -235,9 +226,18 @@ LateralOutput MpcLateralController::run()
   output.sync_data.is_steer_converged =
     std::abs(cmd_msg.steering_tire_angle - m_current_steering_ptr->steering_tire_angle) <
     static_cast<float>(m_converged_steer_rad);
+
+
   return output;
 }
 
+void MpcLateralController::setInputData(InputData const & input_data)
+{
+  setTrajectory(input_data.current_trajectory_ptr);
+  m_current_odometry_ptr = input_data.current_odometry_ptr;
+  m_current_steering_ptr = input_data.current_steering_ptr;
+}
+
 bool8_t MpcLateralController::checkData() const
 {
   if (!m_mpc.hasVehicleModel()) {
@@ -271,9 +271,11 @@ bool8_t MpcLateralController::checkData() const
   return true;
 }
 
-void MpcLateralController::onTrajectory(
+void MpcLateralController::setTrajectory(
   const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg)
 {
+  if (!msg) return;
+
   m_current_trajectory_ptr = msg;
 
   if (!m_current_pose_ptr && !updateCurrentPose()) {
@@ -321,17 +323,6 @@ bool8_t MpcLateralController::updateCurrentPose()
   return true;
 }
 
-void MpcLateralController::onOdometry(const nav_msgs::msg::Odometry::SharedPtr msg)
-{
-  m_current_odometry_ptr = msg;
-}
-
-void MpcLateralController::onSteering(
-  const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg)
-{
-  m_current_steering_ptr = msg;
-}
-
 autoware_auto_control_msgs::msg::AckermannLateralCommand
 MpcLateralController::getStopControlCommand() const
 {

control/trajectory_follower/src/pid_longitudinal_controller.cpp

@@ -181,12 +181,6 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) : node
   m_min_pitch_rad = node_->declare_parameter<float64_t>("min_pitch_rad");  // [rad]
 
   // subscriber, publisher
-  m_sub_current_velocity = node_->create_subscription<nav_msgs::msg::Odometry>(
-    "~/input/current_odometry", rclcpp::QoS{1},
-    std::bind(&PidLongitudinalController::callbackCurrentVelocity, this, _1));
-  m_sub_trajectory = node_->create_subscription<autoware_auto_planning_msgs::msg::Trajectory>(
-    "~/input/current_trajectory", rclcpp::QoS{1},
-    std::bind(&PidLongitudinalController::callbackTrajectory, this, _1));
   m_pub_control_cmd = node_->create_publisher<autoware_auto_control_msgs::msg::LongitudinalCommand>(
     "~/output/longitudinal_control_cmd", rclcpp::QoS{1});
   m_pub_slope =
@@ -203,19 +197,28 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) : node
   // set lowpass filter for acc
   m_lpf_acc = std::make_shared<trajectory_follower::LowpassFilter1d>(0.0, 0.2);
 }
+void PidLongitudinalController::setInputData(InputData const & input_data)
+{
+  setTrajectory(input_data.current_trajectory_ptr);
+  setCurrentVelocity(input_data.current_odometry_ptr);
+}
 
-void PidLongitudinalController::callbackCurrentVelocity(
+void PidLongitudinalController::setCurrentVelocity(
   const nav_msgs::msg::Odometry::ConstSharedPtr msg)
 {
+  if (!msg) return;
+
   if (m_current_velocity_ptr) {
     m_prev_velocity_ptr = m_current_velocity_ptr;
   }
   m_current_velocity_ptr = std::make_shared<nav_msgs::msg::Odometry>(*msg);
 }
 
-void PidLongitudinalController::callbackTrajectory(
+void PidLongitudinalController::setTrajectory(
   const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg)
 {
+  if (!msg) return;
+
   if (!trajectory_follower::longitudinal_utils::isValidTrajectory(*msg)) {
     RCLCPP_ERROR_THROTTLE(
       node_->get_logger(), *node_->get_clock(), 3000, "received invalid trajectory. ignore.");

control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp

@@ -68,18 +68,26 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node
 
 private:
   // ros variables
-  rclcpp::TimerBase::SharedPtr m_timer_control_;
+  rclcpp::TimerBase::SharedPtr timer_control_;
+
+  trajectory_follower::InputData input_data_;
 
   std::shared_ptr<trajectory_follower::LongitudinalControllerBase> longitudinal_controller_;
   std::shared_ptr<trajectory_follower::LateralControllerBase> lateral_controller_;
 
+  rclcpp::Subscription<autoware_auto_planning_msgs::msg::Trajectory>::SharedPtr sub_ref_path_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odometry_;
+  rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::SteeringReport>::SharedPtr sub_steering_;
   rclcpp::Publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr
-    m_control_cmd_pub_;
+    control_cmd_pub_;
 
   /**
    * @brief compute control command, and publish periodically
    */
   void callbackTimerControl();
+  void onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr);
+  void onOdometry(const nav_msgs::msg::Odometry::SharedPtr msg);
+  void onSteering(const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg);
 };
 }  // namespace trajectory_follower_nodes
 }  // namespace control

control/trajectory_follower_nodes/src/controller_node.cpp

@@ -39,26 +39,50 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control
 {
   using std::placeholders::_1;
 
-  const double m_ctrl_period = declare_parameter<float64_t>("ctrl_period", 0.015);
+  const double ctrl_period = declare_parameter<float64_t>("ctrl_period", 0.015);
 
   lateral_controller_ = std::make_shared<trajectory_follower::MpcLateralController>(*this);
   longitudinal_controller_ =
     std::make_shared<trajectory_follower::PidLongitudinalController>(*this);
 
-  m_control_cmd_pub_ = create_publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>(
+  sub_ref_path_ = create_subscription<autoware_auto_planning_msgs::msg::Trajectory>(
+    "~/input/reference_trajectory", rclcpp::QoS{1}, std::bind(&Controller::onTrajectory, this, _1));
+  sub_steering_ = create_subscription<autoware_auto_vehicle_msgs::msg::SteeringReport>(
+    "~/input/current_steering", rclcpp::QoS{1}, std::bind(&Controller::onSteering, this, _1));
+  sub_odometry_ = create_subscription<nav_msgs::msg::Odometry>(
+    "~/input/current_odometry", rclcpp::QoS{1}, std::bind(&Controller::onOdometry, this, _1));
+  control_cmd_pub_ = create_publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>(
     "~/output/control_cmd", rclcpp::QoS{1}.transient_local());
 
   // Timer
   {
     const auto period_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
-      std::chrono::duration<float64_t>(m_ctrl_period));
-    m_timer_control_ = rclcpp::create_timer(
+      std::chrono::duration<float64_t>(ctrl_period));
+    timer_control_ = rclcpp::create_timer(
       this, get_clock(), period_ns, std::bind(&Controller::callbackTimerControl, this));
   }
 }
 
+void Controller::onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg)
+{
+  input_data_.current_trajectory_ptr = msg;
+}
+
+void Controller::onOdometry(const nav_msgs::msg::Odometry::SharedPtr msg)
+{
+  input_data_.current_odometry_ptr = msg;
+}
+
+void Controller::onSteering(const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg)
+{
+  input_data_.current_steering_ptr = msg;
+}
+
 void Controller::callbackTimerControl()
 {
+  longitudinal_controller_->setInputData(input_data_);  // trajectory, odometry
+  lateral_controller_->setInputData(input_data_);       // trajectory, odometry, steering
+
   const auto longitudinal_output = longitudinal_controller_->run();
   const auto lateral_output = lateral_controller_->run();
 
@@ -69,7 +93,7 @@ void Controller::callbackTimerControl()
   out.stamp = this->now();
   out.lateral = lateral_output.control_cmd;
   out.longitudinal = longitudinal_output.control_cmd;
-  m_control_cmd_pub_->publish(out);
+  control_cmd_pub_->publish(out);
 }
 
 }  // namespace trajectory_follower_nodes