From 3036491ec01f5260a2155c25ef9c4d36dd60ab69 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Thu, 26 May 2022 20:00:29 +0900 Subject: [PATCH] Unifiy subscriber Signed-off-by: kosuke55 --- control/trajectory_follower/CMakeLists.txt | 2 + .../trajectory_follower/input_data.hpp | 41 +++++++++++++++++++ .../lateral_controller_base.hpp | 2 + .../longitudinal_controller_base.hpp | 2 + .../mpc_lateral_controller.hpp | 26 +++--------- .../pid_longitudinal_controller.hpp | 12 +++--- .../src/mpc_lateral_controller.cpp | 33 ++++++--------- .../src/pid_longitudinal_controller.cpp | 19 +++++---- .../controller_node.hpp | 12 +++++- .../src/controller_node.cpp | 34 ++++++++++++--- 10 files changed, 122 insertions(+), 61 deletions(-) create mode 100644 control/trajectory_follower/include/trajectory_follower/input_data.hpp diff --git a/control/trajectory_follower/CMakeLists.txt b/control/trajectory_follower/CMakeLists.txt index 5d564ba33c62b..6dd7c7cedc4be 100644 --- a/control/trajectory_follower/CMakeLists.txt +++ b/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 diff --git a/control/trajectory_follower/include/trajectory_follower/input_data.hpp b/control/trajectory_follower/include/trajectory_follower/input_data.hpp new file mode 100644 index 0000000000000..505e5e0de2545 --- /dev/null +++ b/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_ diff --git a/control/trajectory_follower/include/trajectory_follower/lateral_controller_base.hpp b/control/trajectory_follower/include/trajectory_follower/lateral_controller_base.hpp index 11ba95722fb1e..dea01daaae9f7 100644 --- a/control/trajectory_follower/include/trajectory_follower/lateral_controller_base.hpp +++ b/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; diff --git a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp b/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp index d355b8e1fdebf..a2dad813980e3 100644 --- a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp +++ b/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: diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp b/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp index 47c38b6fecfbd..2f31ad645dae4 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp +++ b/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::SharedPtr m_pub_diagnostic; - //!< @brief topic subscription for reference waypoints - rclcpp::Subscription::SharedPtr m_sub_ref_path; - //!< @brief subscription for current velocity - rclcpp::Subscription::SharedPtr m_sub_odometry; - //!< @brief subscription for current steering - rclcpp::Subscription::SharedPtr m_sub_steering; //!< @brief timer to update after a given interval rclcpp::TimerBase::SharedPtr m_timer; //!< @brief subscription for transform messages @@ -142,8 +135,6 @@ 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); /** @@ -151,10 +142,15 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController */ 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 diff --git a/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp b/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp index 26f0ebbb36e18..582579d2376a2 100644 --- a/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp +++ b/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::SharedPtr m_sub_current_velocity; - rclcpp::Subscription::SharedPtr m_sub_trajectory; rclcpp::Publisher::SharedPtr m_pub_control_cmd; rclcpp::Publisher::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 diff --git a/control/trajectory_follower/src/mpc_lateral_controller.cpp b/control/trajectory_follower/src/mpc_lateral_controller.cpp index ce97a66b6579b..64466eded55ab 100644 --- a/control/trajectory_follower/src/mpc_lateral_controller.cpp +++ b/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( "~/output/lateral_diagnostic", 1); - m_sub_ref_path = node_->create_subscription( - "~/input/current_trajectory", rclcpp::QoS{1}, - std::bind(&MpcLateralController::onTrajectory, this, _1)); - m_sub_steering = node_->create_subscription( - "~/input/current_steering", rclcpp::QoS{1}, - std::bind(&MpcLateralController::onSteering, this, _1)); - m_sub_odometry = node_->create_subscription( - "~/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(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 { diff --git a/control/trajectory_follower/src/pid_longitudinal_controller.cpp b/control/trajectory_follower/src/pid_longitudinal_controller.cpp index fb26bbbc9b625..c6eb078c1e99b 100644 --- a/control/trajectory_follower/src/pid_longitudinal_controller.cpp +++ b/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("min_pitch_rad"); // [rad] // subscriber, publisher - m_sub_current_velocity = node_->create_subscription( - "~/input/current_odometry", rclcpp::QoS{1}, - std::bind(&PidLongitudinalController::callbackCurrentVelocity, this, _1)); - m_sub_trajectory = node_->create_subscription( - "~/input/current_trajectory", rclcpp::QoS{1}, - std::bind(&PidLongitudinalController::callbackTrajectory, this, _1)); m_pub_control_cmd = node_->create_publisher( "~/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(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(*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."); diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp index 73ead9b57266f..17a22f0f65022 100644 --- a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp +++ b/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 longitudinal_controller_; std::shared_ptr lateral_controller_; + rclcpp::Subscription::SharedPtr sub_ref_path_; + rclcpp::Subscription::SharedPtr sub_odometry_; + rclcpp::Subscription::SharedPtr sub_steering_; rclcpp::Publisher::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 diff --git a/control/trajectory_follower_nodes/src/controller_node.cpp b/control/trajectory_follower_nodes/src/controller_node.cpp index aa31736823d1e..8ecd44d028008 100644 --- a/control/trajectory_follower_nodes/src/controller_node.cpp +++ b/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("ctrl_period", 0.015); + const double ctrl_period = declare_parameter("ctrl_period", 0.015); lateral_controller_ = std::make_shared(*this); longitudinal_controller_ = std::make_shared(*this); - m_control_cmd_pub_ = create_publisher( + sub_ref_path_ = create_subscription( + "~/input/reference_trajectory", rclcpp::QoS{1}, std::bind(&Controller::onTrajectory, this, _1)); + sub_steering_ = create_subscription( + "~/input/current_steering", rclcpp::QoS{1}, std::bind(&Controller::onSteering, this, _1)); + sub_odometry_ = create_subscription( + "~/input/current_odometry", rclcpp::QoS{1}, std::bind(&Controller::onOdometry, this, _1)); + control_cmd_pub_ = create_publisher( "~/output/control_cmd", rclcpp::QoS{1}.transient_local()); // Timer { const auto period_ns = std::chrono::duration_cast( - std::chrono::duration(m_ctrl_period)); - m_timer_control_ = rclcpp::create_timer( + std::chrono::duration(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