From d8f78e253c7ccc461612b7887c6386d58bfd270b Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Sat, 25 Jun 2022 22:39:41 +0900 Subject: [PATCH] Clean up Signed-off-by: kosuke55 --- .../longitudinal_controller_base.hpp | 2 +- .../src/mpc_lateral_controller.cpp | 12 - .../src/pid_longitudinal_controller.cpp | 4 +- .../trajectory_follower_nodes/CMakeLists.txt | 56 -- .../controller_node.hpp | 2 +- .../lateral_controller_node.hpp | 230 ----- .../latlon_muxer_node.hpp | 76 -- .../longitudinal_controller_node.hpp | 373 ------- .../src/lateral_controller_node.cpp | 518 ---------- .../src/latlon_muxer_node.cpp | 97 -- .../src/longitudinal_controller_node.cpp | 944 ------------------ 11 files changed, 3 insertions(+), 2311 deletions(-) delete mode 100644 control/trajectory_follower_nodes/include/trajectory_follower_nodes/lateral_controller_node.hpp delete mode 100644 control/trajectory_follower_nodes/include/trajectory_follower_nodes/latlon_muxer_node.hpp delete mode 100644 control/trajectory_follower_nodes/include/trajectory_follower_nodes/longitudinal_controller_node.hpp delete mode 100644 control/trajectory_follower_nodes/src/lateral_controller_node.cpp delete mode 100644 control/trajectory_follower_nodes/src/latlon_muxer_node.cpp delete mode 100644 control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp 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 bd7e51a809b1a..6eb4d13119275 100644 --- a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp +++ b/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp @@ -41,7 +41,7 @@ class LongitudinalControllerBase public: virtual boost::optional run() = 0; virtual void setInputData(InputData const & input_data) = 0; - void sync(LateralSyncData const & lateral_sync_data) { lateral_sync_data_ = lateral_sync_data; }; + void sync(LateralSyncData const & lateral_sync_data) { lateral_sync_data_ = lateral_sync_data; } protected: LateralSyncData lateral_sync_data_; diff --git a/control/trajectory_follower/src/mpc_lateral_controller.cpp b/control/trajectory_follower/src/mpc_lateral_controller.cpp index c0a7a6438e93d..7ae59c27387b5 100644 --- a/control/trajectory_follower/src/mpc_lateral_controller.cpp +++ b/control/trajectory_follower/src/mpc_lateral_controller.cpp @@ -139,9 +139,6 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} m_mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); } - /* set up ros system */ - // initTimer(m_mpc.m_ctrl_period); - m_pub_ctrl_cmd = node_->create_publisher( "~/output/lateral_control_cmd", 1); @@ -368,7 +365,6 @@ autoware_auto_control_msgs::msg::AckermannLateralCommand MpcLateralController::c autoware_auto_control_msgs::msg::AckermannLateralCommand ctrl_cmd) { ctrl_cmd.stamp = node_->now(); - // m_pub_ctrl_cmd->publish(ctrl_cmd); m_steer_cmd_prev = ctrl_cmd.steering_tire_angle; return ctrl_cmd; } @@ -389,14 +385,6 @@ void MpcLateralController::publishDiagnostic( m_pub_diagnostic->publish(diagnostic); } -// void MpcLateralController::initTimer(float64_t period_s) -// { -// const auto period_ns = std::chrono::duration_cast( -// std::chrono::duration(period_s)); -// m_timer = rclcpp::create_timer( -// this, node_->get_clock(), period_ns, std::bind(&MpcLateralController::onTimer, this)); -// } - void MpcLateralController::declareMPCparameters() { m_mpc.m_param.prediction_horizon = node_->declare_parameter("mpc_prediction_horizon"); diff --git a/control/trajectory_follower/src/pid_longitudinal_controller.cpp b/control/trajectory_follower/src/pid_longitudinal_controller.cpp index 63f43a30568b4..e66a8ce95fc36 100644 --- a/control/trajectory_follower/src/pid_longitudinal_controller.cpp +++ b/control/trajectory_follower/src/pid_longitudinal_controller.cpp @@ -16,10 +16,9 @@ #include "motion_common/motion_common.hpp" #include "motion_common/trajectory_common.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include "time_utils/time_utils.hpp" -#include - #include #include #include @@ -665,7 +664,6 @@ autoware_auto_control_msgs::msg::LongitudinalCommand PidLongitudinalController:: cmd.stamp = node_->now(); cmd.speed = static_cast(ctrl_cmd.vel); cmd.acceleration = static_cast(ctrl_cmd.acc); - // m_pub_control_cmd->publish(cmd); // store current velocity history m_vel_hist.push_back({node_->now(), current_vel}); diff --git a/control/trajectory_follower_nodes/CMakeLists.txt b/control/trajectory_follower_nodes/CMakeLists.txt index d3b74bc722573..c6431f1d2504c 100644 --- a/control/trajectory_follower_nodes/CMakeLists.txt +++ b/control/trajectory_follower_nodes/CMakeLists.txt @@ -4,48 +4,6 @@ project(trajectory_follower_nodes) find_package(autoware_cmake REQUIRED) autoware_package() -set(LATERAL_CONTROLLER_NODE lateral_controller_node) -ament_auto_add_library(${LATERAL_CONTROLLER_NODE} SHARED - include/trajectory_follower_nodes/lateral_controller_node.hpp - src/lateral_controller_node.cpp -) - -# TODO(lateral_controller) : RCLCPP_ERROR_THROTTLE() has built-in old-style casts. -# TODO(lateral_controller) : Temporary workaround until this is fixed. -target_compile_options(${LATERAL_CONTROLLER_NODE} PRIVATE -Wno-error=old-style-cast) -rclcpp_components_register_node(${LATERAL_CONTROLLER_NODE} - PLUGIN "autoware::motion::control::trajectory_follower_nodes::LateralController" - EXECUTABLE ${LATERAL_CONTROLLER_NODE}_exe -) - -set(LONGITUDINAL_CONTROLLER_NODE longitudinal_controller_node) -ament_auto_add_library(${LONGITUDINAL_CONTROLLER_NODE} SHARED - include/trajectory_follower_nodes/longitudinal_controller_node.hpp - src/longitudinal_controller_node.cpp -) - -# TODO(longitudinal_controller) : RCLCPP_ERROR_THROTTLE() has built-in old-style casts. -# TODO(longitudinal_controller) : Temporary workaround until this is fixed. -target_compile_options(${LONGITUDINAL_CONTROLLER_NODE} PRIVATE -Wno-error=old-style-cast) -rclcpp_components_register_node(${LONGITUDINAL_CONTROLLER_NODE} - PLUGIN "autoware::motion::control::trajectory_follower_nodes::LongitudinalController" - EXECUTABLE ${LONGITUDINAL_CONTROLLER_NODE}_exe -) - -set(LATLON_MUXER_NODE latlon_muxer_node) -ament_auto_add_library(${LATLON_MUXER_NODE} SHARED - include/trajectory_follower_nodes/latlon_muxer_node.hpp - src/latlon_muxer_node.cpp -) - -# TODO(latlon_muxer) : RCLCPP_ERROR_THROTTLE() has built-in old-style casts. -# TODO(latlon_muxer) : Temporary workaround until this is fixed. -target_compile_options(${LATLON_MUXER_NODE} PRIVATE -Wno-error=old-style-cast) -rclcpp_components_register_node(${LATLON_MUXER_NODE} - PLUGIN "autoware::motion::control::trajectory_follower_nodes::LatLonMuxer" - EXECUTABLE ${LATLON_MUXER_NODE}_exe -) - set(CONTROLLER_NODE controller_node) ament_auto_add_library(${CONTROLLER_NODE} SHARED include/trajectory_follower_nodes/controller_node.hpp @@ -62,10 +20,7 @@ if(BUILD_TESTING) set(TRAJECTORY_FOLLOWER_NODES_TEST test_trajectory_follower_nodes) ament_add_ros_isolated_gtest(${TRAJECTORY_FOLLOWER_NODES_TEST} test/trajectory_follower_test_utils.hpp - # test/test_latlon_muxer_node.cpp test/test_controller_node.cpp - # test/test_lateral_controller_node.cpp - # test/test_longitudinal_controller_node.cpp ) ament_target_dependencies(${TRAJECTORY_FOLLOWER_NODES_TEST} fake_test_node) target_link_libraries( @@ -75,17 +30,6 @@ if(BUILD_TESTING) add_smoke_test(${PROJECT_NAME} ${CONTROLLER_NODE}_exe PARAM_FILENAMES "lateral_controller_defaults.param.yaml longitudinal_controller_defaults.param.yaml test_vehicle_info.param.yaml" ) - - - # add_smoke_test(${PROJECT_NAME} ${LATLON_MUXER_NODE}_exe - # PARAM_FILENAMES "latlon_muxer_defaults.param.yaml" - # ) - # add_smoke_test(${PROJECT_NAME} ${LATERAL_CONTROLLER_NODE}_exe - # PARAM_FILENAMES "lateral_controller_defaults.param.yaml test_vehicle_info.param.yaml" - # ) - # add_smoke_test(${PROJECT_NAME} ${LONGITUDINAL_CONTROLLER_NODE}_exe - # PARAM_FILENAMES "longitudinal_controller_defaults.param.yaml test_vehicle_info.param.yaml" - # ) endif() ament_auto_package( 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 d349bb676251b..1d2f841a4bba9 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 @@ -67,7 +67,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node { public: explicit Controller(const rclcpp::NodeOptions & node_options); - virtual ~Controller(){}; + virtual ~Controller() {} private: rclcpp::TimerBase::SharedPtr timer_control_; diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/lateral_controller_node.hpp b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/lateral_controller_node.hpp deleted file mode 100644 index 089292d97021a..0000000000000 --- a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/lateral_controller_node.hpp +++ /dev/null @@ -1,230 +0,0 @@ -// Copyright 2021 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_NODES__LATERAL_CONTROLLER_NODE_HPP_ -#define TRAJECTORY_FOLLOWER_NODES__LATERAL_CONTROLLER_NODE_HPP_ - -#include "common/types.hpp" -#include "osqp_interface/osqp_interface.hpp" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_components/register_node_macro.hpp" -#include "tf2/utils.h" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" -#include "trajectory_follower/interpolate.hpp" -#include "trajectory_follower/lowpass_filter.hpp" -#include "trajectory_follower/mpc.hpp" -#include "trajectory_follower/mpc_trajectory.hpp" -#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/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" -#include "trajectory_follower_nodes/visibility_control.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" - -#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_system_msgs/msg/float32_multi_array_diagnostic.hpp" -#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" -#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include "tf2_msgs/msg/tf_message.hpp" - -#include -#include -#include -#include - -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower_nodes -{ -using autoware::common::types::bool8_t; -using autoware::common::types::float64_t; -namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; - -class TRAJECTORY_FOLLOWER_PUBLIC LateralController : public rclcpp::Node -{ -public: - /** - * @brief constructor - */ - explicit LateralController(const rclcpp::NodeOptions & node_options); - - /** - * @brief destructor - */ - virtual ~LateralController(); - -private: - //!< @brief topic publisher for control command - rclcpp::Publisher::SharedPtr - m_pub_ctrl_cmd; - //!< @brief topic publisher for predicted trajectory - rclcpp::Publisher::SharedPtr m_pub_predicted_traj; - //!< @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 - rclcpp::Subscription::SharedPtr m_tf_sub; - rclcpp::Subscription::SharedPtr m_tf_static_sub; - - /* parameters for path smoothing */ - //!< @brief flag for path smoothing - bool8_t m_enable_path_smoothing; - //!< @brief param of moving average filter for path smoothing - int64_t m_path_filter_moving_ave_num; - //!< @brief point-to-point index distance for curvature calculation for trajectory //NOLINT - int64_t m_curvature_smoothing_num_traj; - //!< @brief point-to-point index distance for curvature calculation for reference steer command - //!< //NOLINT - int64_t m_curvature_smoothing_num_ref_steer; - //!< @brief path resampling interval [m] - float64_t m_traj_resample_dist; - - /* parameters for stop state */ - float64_t m_stop_state_entry_ego_speed; - float64_t m_stop_state_entry_target_speed; - - // MPC object - trajectory_follower::MPC m_mpc; - - //!< @brief measured pose - geometry_msgs::msg::PoseStamped::SharedPtr m_current_pose_ptr; - //!< @brief measured velocity - nav_msgs::msg::Odometry::SharedPtr m_current_odometry_ptr; - //!< @brief measured steering - autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr m_current_steering_ptr; - //!< @brief reference trajectory - autoware_auto_planning_msgs::msg::Trajectory::SharedPtr m_current_trajectory_ptr; - - //!< @brief mpc filtered output in previous period - float64_t m_steer_cmd_prev = 0.0; - - //!< @brief flag of m_ctrl_cmd_prev initialization - bool8_t m_is_ctrl_cmd_prev_initialized = false; - //!< @brief previous control command - autoware_auto_control_msgs::msg::AckermannLateralCommand m_ctrl_cmd_prev; - - //!< @brief buffer for transforms - tf2::BufferCore m_tf_buffer{tf2::BUFFER_CORE_DEFAULT_CACHE_TIME}; - tf2_ros::TransformListener m_tf_listener{m_tf_buffer}; - - //!< 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 - */ - void onTimer(); - - /** - * @brief set m_current_trajectory with received message - */ - void onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr); - - /** - * @brief update current_pose from tf - * @return true if the current pose was updated, false otherwise - */ - bool8_t updateCurrentPose(); - - /** - * @brief check if the received data is valid. - */ - 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 publish control command - * @param [in] cmd published control command - */ - void publishCtrlCmd(autoware_auto_control_msgs::msg::AckermannLateralCommand cmd); - - /** - * @brief publish predicted future trajectory - * @param [in] predicted_traj published predicted trajectory - */ - void publishPredictedTraj(autoware_auto_planning_msgs::msg::Trajectory & predicted_traj) const; - - /** - * @brief publish diagnostic message - * @param [in] diagnostic published diagnostic - */ - void publishDiagnostic( - autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic & diagnostic) const; - - /** - * @brief get stop command - */ - autoware_auto_control_msgs::msg::AckermannLateralCommand getStopControlCommand() const; - - /** - * @brief get initial command - */ - autoware_auto_control_msgs::msg::AckermannLateralCommand getInitialControlCommand() const; - - /** - * @brief check ego car is in stopped state - */ - bool8_t isStoppedState() const; - - /** - * @brief check if the trajectory has valid value - */ - bool8_t isValidTrajectory(const autoware_auto_planning_msgs::msg::Trajectory & traj) const; - - OnSetParametersCallbackHandle::SharedPtr m_set_param_res; - - /** - * @brief Declare MPC parameters as ROS parameters to allow tuning on the fly - */ - void declareMPCparameters(); - - /** - * @brief Called when parameters are changed outside of node - */ - rcl_interfaces::msg::SetParametersResult paramCallback( - const std::vector & parameters); -}; -} // namespace trajectory_follower_nodes -} // namespace control -} // namespace motion -} // namespace autoware - -#endif // TRAJECTORY_FOLLOWER_NODES__LATERAL_CONTROLLER_NODE_HPP_ diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/latlon_muxer_node.hpp b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/latlon_muxer_node.hpp deleted file mode 100644 index 59a23a0966090..0000000000000 --- a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/latlon_muxer_node.hpp +++ /dev/null @@ -1,76 +0,0 @@ -// Copyright 2021 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_NODES__LATLON_MUXER_NODE_HPP_ -#define TRAJECTORY_FOLLOWER_NODES__LATLON_MUXER_NODE_HPP_ - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp/time.hpp" -#include "rclcpp_components/register_node_macro.hpp" -#include "trajectory_follower_nodes/visibility_control.hpp" - -#include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp" -#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" -#include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" - -#include -#include - -namespace autoware -{ -namespace motion -{ -namespace control -{ -/// \brief Resources relating to the trajectory_follower_nodes package -namespace trajectory_follower_nodes -{ -/// \class LatLonMuxer -/// \brief The node class used for muxing lateral and longitudinal messages -class LatLonMuxer : public rclcpp::Node -{ -public: - explicit TRAJECTORY_FOLLOWER_PUBLIC LatLonMuxer(const rclcpp::NodeOptions & node_options); - -private: - // \brief Callback for the lateral control command - void latCtrlCmdCallback( - const autoware_auto_control_msgs::msg::AckermannLateralCommand::SharedPtr msg); - // \brief Callback for the longitudinal control command - void lonCtrlCmdCallback( - const autoware_auto_control_msgs::msg::LongitudinalCommand::SharedPtr msg); - // \brief Publish the combined control command message - void publishCmd(); - // \brief Check that the received messages are not too old - // \return bool True if the stored messages timed out - bool checkTimeout(); - - rclcpp::Publisher::SharedPtr - m_control_cmd_pub; - rclcpp::Subscription::SharedPtr - m_lat_control_cmd_sub; - rclcpp::Subscription::SharedPtr - m_lon_control_cmd_sub; - - std::shared_ptr m_lat_cmd; - std::shared_ptr m_lon_cmd; - // \brief Timeout duration in seconds - double m_timeout_thr_sec; -}; // class LatLonMuxer -} // namespace trajectory_follower_nodes -} // namespace control -} // namespace motion -} // namespace autoware - -#endif // TRAJECTORY_FOLLOWER_NODES__LATLON_MUXER_NODE_HPP_ diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/longitudinal_controller_node.hpp b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/longitudinal_controller_node.hpp deleted file mode 100644 index f9825902954f4..0000000000000 --- a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/longitudinal_controller_node.hpp +++ /dev/null @@ -1,373 +0,0 @@ -// Copyright 2021 Tier IV, Inc. All rights reserved. -// -// 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_NODES__LONGITUDINAL_CONTROLLER_NODE_HPP_ -#define TRAJECTORY_FOLLOWER_NODES__LONGITUDINAL_CONTROLLER_NODE_HPP_ - -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/Geometry" -#include "motion_common/motion_common.hpp" -#include "motion_common/trajectory_common.hpp" -#include "rclcpp/rclcpp.hpp" -#include "tf2/utils.h" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" -#include "trajectory_follower/debug_values.hpp" -#include "trajectory_follower/longitudinal_controller_utils.hpp" -#include "trajectory_follower/lowpass_filter.hpp" -#include "trajectory_follower/pid.hpp" -#include "trajectory_follower/smooth_stop.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" - -#include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_system_msgs/msg/float32_multi_array_diagnostic.hpp" -#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include "tf2_msgs/msg/tf_message.hpp" - -#include -#include -#include -#include - -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower_nodes -{ -using autoware::common::types::bool8_t; -using autoware::common::types::float64_t; -namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; -namespace motion_common = ::autoware::motion::motion_common; - -/// \class LongitudinalController -/// \brief The node class used for generating longitudinal control commands (velocity/acceleration) -class TRAJECTORY_FOLLOWER_PUBLIC LongitudinalController : public rclcpp::Node -{ -public: - explicit LongitudinalController(const rclcpp::NodeOptions & node_options); - -private: - struct Motion - { - float64_t vel{0.0}; - float64_t acc{0.0}; - }; - - enum class Shift { Forward = 0, Reverse }; - - struct ControlData - { - bool8_t is_far_from_trajectory{false}; - size_t nearest_idx{0}; // nearest_idx = 0 when nearest_idx is not found with findNearestIdx - Motion current_motion{}; - Shift shift{Shift::Forward}; // shift is used only to calculate the sign of pitch compensation - float64_t stop_dist{0.0}; // signed distance that is positive when car is before the stopline - float64_t slope_angle{0.0}; - float64_t dt{0.0}; - }; - - // 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 - m_pub_slope; - rclcpp::Publisher::SharedPtr - m_pub_debug; - rclcpp::TimerBase::SharedPtr m_timer_control; - - rclcpp::Subscription::SharedPtr m_tf_sub; - rclcpp::Subscription::SharedPtr m_tf_static_sub; - tf2::BufferCore m_tf_buffer{tf2::BUFFER_CORE_DEFAULT_CACHE_TIME}; - tf2_ros::TransformListener m_tf_listener{m_tf_buffer}; - - OnSetParametersCallbackHandle::SharedPtr m_set_param_res; - rcl_interfaces::msg::SetParametersResult paramCallback( - const std::vector & parameters); - - // pointers for ros topic - std::shared_ptr m_current_velocity_ptr{nullptr}; - std::shared_ptr m_prev_velocity_ptr{nullptr}; - std::shared_ptr m_trajectory_ptr{nullptr}; - - // vehicle info - float64_t m_wheel_base; - - // control state - enum class ControlState { DRIVE = 0, STOPPING, STOPPED, EMERGENCY }; - ControlState m_control_state{ControlState::STOPPED}; - - // control period - float64_t m_longitudinal_ctrl_period; - - // delay compensation - float64_t m_delay_compensation_time; - - // enable flags - bool8_t m_enable_smooth_stop; - bool8_t m_enable_overshoot_emergency; - bool8_t m_enable_slope_compensation; - - // smooth stop transition - struct StateTransitionParams - { - // drive - float64_t drive_state_stop_dist; - float64_t drive_state_offset_stop_dist; - // stopping - float64_t stopping_state_stop_dist; - // stop - float64_t stopped_state_entry_duration_time; - float64_t stopped_state_entry_vel; - float64_t stopped_state_entry_acc; - // emergency - float64_t emergency_state_overshoot_stop_dist; - float64_t emergency_state_traj_trans_dev; - float64_t emergency_state_traj_rot_dev; - }; - StateTransitionParams m_state_transition_params; - - // drive - trajectory_follower::PIDController m_pid_vel; - std::shared_ptr m_lpf_vel_error{nullptr}; - float64_t m_current_vel_threshold_pid_integrate; - bool8_t m_enable_brake_keeping_before_stop; - float64_t m_brake_keeping_acc; - - // smooth stop - trajectory_follower::SmoothStop m_smooth_stop; - - // stop - struct StoppedStateParams - { - float64_t vel; - float64_t acc; - float64_t jerk; - }; - StoppedStateParams m_stopped_state_params; - - // emergency - struct EmergencyStateParams - { - float64_t vel; - float64_t acc; - float64_t jerk; - }; - EmergencyStateParams m_emergency_state_params; - - // acceleration limit - float64_t m_max_acc; - float64_t m_min_acc; - - // jerk limit - float64_t m_max_jerk; - float64_t m_min_jerk; - - // slope compensation - bool8_t m_use_traj_for_pitch; - std::shared_ptr m_lpf_pitch{nullptr}; - float64_t m_max_pitch_rad; - float64_t m_min_pitch_rad; - - // 1st order lowpass filter for acceleration - std::shared_ptr m_lpf_acc{nullptr}; - - // buffer of send command - std::vector m_ctrl_cmd_vec; - - // for calculating dt - std::shared_ptr m_prev_control_time{nullptr}; - - // shift mode - Shift m_prev_shift{Shift::Forward}; - - // diff limit - Motion m_prev_ctrl_cmd{}; // with slope compensation - Motion m_prev_raw_ctrl_cmd{}; // without slope compensation - std::vector> m_vel_hist; - - // debug values - trajectory_follower::DebugValues m_debug_values; - - std::shared_ptr m_last_running_time{std::make_shared(this->now())}; - - /** - * @brief set current and previous velocity with received message - * @param [in] msg current state message - */ - void callbackCurrentVelocity(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); - - /** - * @brief compute control command, and publish periodically - */ - void callbackTimerControl(); - - /** - * @brief calculate data for controllers whose type is ControlData - * @param [in] current_pose current ego pose - */ - ControlData getControlData(const geometry_msgs::msg::Pose & current_pose); - - /** - * @brief calculate control command in emergency state - * @param [in] dt time between previous and current one - */ - Motion calcEmergencyCtrlCmd(const float64_t dt) const; - - /** - * @brief update control state according to the current situation - * @param [in] current_control_state current control state - * @param [in] control_data control data - */ - ControlState updateControlState( - const ControlState current_control_state, const ControlData & control_data); - - /** - * @brief calculate control command based on the current control state - * @param [in] current_control_state current control state - * @param [in] current_pose current ego pose - * @param [in] control_data control data - */ - Motion calcCtrlCmd( - const ControlState & current_control_state, const geometry_msgs::msg::Pose & current_pose, - const ControlData & control_data); - - /** - * @brief publish control command - * @param [in] ctrl_cmd calculated control command to control velocity - * @param [in] current_vel current velocity of the vehicle - */ - void publishCtrlCmd(const Motion & ctrl_cmd, const float64_t current_vel); - - /** - * @brief publish debug data - * @param [in] ctrl_cmd calculated control command to control velocity - * @param [in] control_data data for control calculation - */ - void publishDebugData(const Motion & ctrl_cmd, const ControlData & control_data); - - /** - * @brief calculate time between current and previous one - */ - float64_t getDt(); - - /** - * @brief calculate current velocity and acceleration - */ - Motion getCurrentMotion() const; - - /** - * @brief calculate direction (forward or backward) that vehicle moves - * @param [in] nearest_idx nearest index on trajectory to vehicle - */ - enum Shift getCurrentShift(const size_t nearest_idx) const; - - /** - * @brief filter acceleration command with limitation of acceleration and jerk, and slope - * compensation - * @param [in] raw_acc acceleration before filtered - * @param [in] control_data data for control calculation - */ - float64_t calcFilteredAcc(const float64_t raw_acc, const ControlData & control_data); - - /** - * @brief store acceleration command before slope compensation - * @param [in] accel command before slope compensation - */ - void storeAccelCmd(const float64_t accel); - - /** - * @brief add acceleration to compensate for slope - * @param [in] acc acceleration before slope compensation - * @param [in] pitch pitch angle (upward is negative) - * @param [in] shift direction that vehicle move (forward or backward) - */ - float64_t applySlopeCompensation( - const float64_t acc, const float64_t pitch, const Shift shift) const; - - /** - * @brief keep target motion acceleration negative before stop - * @param [in] traj reference trajectory - * @param [in] motion delay compensated target motion - */ - Motion keepBrakeBeforeStop( - const autoware_auto_planning_msgs::msg::Trajectory & traj, const Motion & target_motion, - const size_t nearest_idx) const; - - /** - * @brief interpolate trajectory point that is nearest to vehicle - * @param [in] traj reference trajectory - * @param [in] point vehicle position - * @param [in] nearest_idx index of the trajectory point nearest to the vehicle position - */ - autoware_auto_planning_msgs::msg::TrajectoryPoint calcInterpolatedTargetValue( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const geometry_msgs::msg::Pose & pose, const size_t nearest_idx) const; - - /** - * @brief calculate predicted velocity after time delay based on past control commands - * @param [in] current_motion current velocity and acceleration of the vehicle - * @param [in] delay_compensation_time predicted time delay - */ - float64_t predictedVelocityInTargetPoint( - const Motion current_motion, const float64_t delay_compensation_time) const; - - /** - * @brief calculate velocity feedback with feed forward and pid controller - * @param [in] target_motion reference velocity and acceleration. This acceleration will be used - * as feed forward. - * @param [in] dt time step to use - * @param [in] current_vel current velocity of the vehicle - */ - float64_t applyVelocityFeedback( - const Motion target_motion, const float64_t dt, const float64_t current_vel); - - /** - * @brief update variables for debugging about pitch - * @param [in] pitch current pitch of the vehicle (filtered) - * @param [in] traj_pitch current trajectory pitch - * @param [in] raw_pitch current raw pitch of the vehicle (unfiltered) - */ - void updatePitchDebugValues( - const float64_t pitch, const float64_t traj_pitch, const float64_t raw_pitch); - - /** - * @brief update variables for velocity and acceleration - * @param [in] ctrl_cmd latest calculated control command - * @param [in] current_pose current pose of the vehicle - * @param [in] control_data data for control calculation - */ - void updateDebugVelAcc( - const Motion & ctrl_cmd, const geometry_msgs::msg::Pose & current_pose, - const ControlData & control_data); -}; -} // namespace trajectory_follower_nodes -} // namespace control -} // namespace motion -} // namespace autoware - -#endif // TRAJECTORY_FOLLOWER_NODES__LONGITUDINAL_CONTROLLER_NODE_HPP_ diff --git a/control/trajectory_follower_nodes/src/lateral_controller_node.cpp b/control/trajectory_follower_nodes/src/lateral_controller_node.cpp deleted file mode 100644 index 4b8e609429da9..0000000000000 --- a/control/trajectory_follower_nodes/src/lateral_controller_node.cpp +++ /dev/null @@ -1,518 +0,0 @@ -// Copyright 2021 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. - -#include "trajectory_follower_nodes/lateral_controller_node.hpp" - -#include "tf2_ros/create_timer_ros.h" - -#include -#include -#include -#include -#include -#include -#include - -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower_nodes -{ -namespace -{ -using namespace std::literals::chrono_literals; - -template -void update_param( - const std::vector & parameters, const std::string & name, T & value) -{ - auto it = std::find_if( - parameters.cbegin(), parameters.cend(), - [&name](const rclcpp::Parameter & parameter) { return parameter.get_name() == name; }); - if (it != parameters.cend()) { - value = static_cast(it->template get_value()); - } -} -} // namespace - -LateralController::LateralController(const rclcpp::NodeOptions & node_options) -: Node("lateral_controller", node_options) -{ - using std::placeholders::_1; - - m_mpc.m_ctrl_period = declare_parameter("ctrl_period"); - m_enable_path_smoothing = declare_parameter("enable_path_smoothing"); - m_path_filter_moving_ave_num = declare_parameter("path_filter_moving_ave_num"); - m_curvature_smoothing_num_traj = declare_parameter("curvature_smoothing_num_traj"); - m_curvature_smoothing_num_ref_steer = - declare_parameter("curvature_smoothing_num_ref_steer"); - m_traj_resample_dist = declare_parameter("traj_resample_dist"); - m_mpc.m_admissible_position_error = declare_parameter("admissible_position_error"); - m_mpc.m_admissible_yaw_error_rad = declare_parameter("admissible_yaw_error_rad"); - m_mpc.m_use_steer_prediction = declare_parameter("use_steer_prediction"); - m_mpc.m_param.steer_tau = declare_parameter("vehicle_model_steer_tau"); - - /* stop state parameters */ - m_stop_state_entry_ego_speed = declare_parameter("stop_state_entry_ego_speed"); - m_stop_state_entry_target_speed = declare_parameter("stop_state_entry_target_speed"); - - /* mpc parameters */ - const float64_t steer_lim_deg = declare_parameter("steer_lim_deg"); - const float64_t steer_rate_lim_dps = declare_parameter("steer_rate_lim_dps"); - constexpr float64_t deg2rad = static_cast(autoware::common::types::PI) / 180.0; - m_mpc.m_steer_lim = steer_lim_deg * deg2rad; - m_mpc.m_steer_rate_lim = steer_rate_lim_dps * deg2rad; - const float64_t wheelbase = - vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo().wheel_base_m; - - /* vehicle model setup */ - const std::string vehicle_model_type = declare_parameter("vehicle_model_type"); - std::shared_ptr vehicle_model_ptr; - if (vehicle_model_type == "kinematics") { - vehicle_model_ptr = std::make_shared( - wheelbase, m_mpc.m_steer_lim, m_mpc.m_param.steer_tau); - } else if (vehicle_model_type == "kinematics_no_delay") { - vehicle_model_ptr = std::make_shared( - wheelbase, m_mpc.m_steer_lim); - } else if (vehicle_model_type == "dynamics") { - const float64_t mass_fl = declare_parameter("vehicle.mass_fl"); - const float64_t mass_fr = declare_parameter("vehicle.mass_fr"); - const float64_t mass_rl = declare_parameter("vehicle.mass_rl"); - const float64_t mass_rr = declare_parameter("vehicle.mass_rr"); - const float64_t cf = declare_parameter("vehicle.cf"); - const float64_t cr = declare_parameter("vehicle.cr"); - - // vehicle_model_ptr is only assigned in ctor, so parameter value have to be passed at init time - // // NOLINT - vehicle_model_ptr = std::make_shared( - wheelbase, mass_fl, mass_fr, mass_rl, mass_rr, cf, cr); - } else { - RCLCPP_ERROR(get_logger(), "vehicle_model_type is undefined"); - } - - /* QP solver setup */ - const std::string qp_solver_type = declare_parameter("qp_solver_type"); - std::shared_ptr qpsolver_ptr; - if (qp_solver_type == "unconstraint_fast") { - qpsolver_ptr = std::make_shared(); - } else if (qp_solver_type == "osqp") { - qpsolver_ptr = std::make_shared(get_logger()); - } else { - RCLCPP_ERROR(get_logger(), "qp_solver_type is undefined"); - } - - /* delay compensation */ - { - const float64_t delay_tmp = declare_parameter("input_delay"); - const float64_t delay_step = std::round(delay_tmp / m_mpc.m_ctrl_period); - m_mpc.m_param.input_delay = delay_step * m_mpc.m_ctrl_period; - m_mpc.m_input_buffer = std::deque(static_cast(delay_step), 0.0); - } - - /* initialize lowpass filter */ - { - const float64_t steering_lpf_cutoff_hz = declare_parameter("steering_lpf_cutoff_hz"); - const float64_t error_deriv_lpf_cutoff_hz = - declare_parameter("error_deriv_lpf_cutoff_hz"); - m_mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); - } - - /* set up ros system */ - initTimer(m_mpc.m_ctrl_period); - - m_pub_ctrl_cmd = create_publisher( - "~/output/control_cmd", 1); - m_pub_predicted_traj = create_publisher( - "~/output/predicted_trajectory", 1); - m_pub_diagnostic = create_publisher( - "~/output/diagnostic", 1); - m_sub_ref_path = create_subscription( - "~/input/reference_trajectory", rclcpp::QoS{1}, - std::bind(&LateralController::onTrajectory, this, _1)); - m_sub_steering = create_subscription( - "~/input/current_steering", rclcpp::QoS{1}, - std::bind(&LateralController::onSteering, this, _1)); - m_sub_odometry = create_subscription( - "~/input/current_odometry", rclcpp::QoS{1}, - std::bind(&LateralController::onOdometry, this, _1)); - - // TODO(Frederik.Beaujean) ctor is too long, should factor out parameter declarations - declareMPCparameters(); - - /* get parameter updates */ - m_set_param_res = - this->add_on_set_parameters_callback(std::bind(&LateralController::paramCallback, this, _1)); - - m_mpc.setQPSolver(qpsolver_ptr); - m_mpc.setVehicleModel(vehicle_model_ptr, vehicle_model_type); - - m_mpc.setLogger(get_logger()); - m_mpc.setClock(get_clock()); -} - -LateralController::~LateralController() -{ - autoware_auto_control_msgs::msg::AckermannLateralCommand stop_cmd = getStopControlCommand(); - publishCtrlCmd(stop_cmd); -} - -void LateralController::onTimer() -{ - if (!checkData() || !updateCurrentPose()) { - return; - } - - autoware_auto_control_msgs::msg::AckermannLateralCommand ctrl_cmd; - autoware_auto_planning_msgs::msg::Trajectory predicted_traj; - autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic diagnostic; - - if (!m_is_ctrl_cmd_prev_initialized) { - m_ctrl_cmd_prev = getInitialControlCommand(); - m_is_ctrl_cmd_prev_initialized = true; - } - - const bool8_t is_mpc_solved = m_mpc.calculateMPC( - *m_current_steering_ptr, m_current_odometry_ptr->twist.twist.linear.x, m_current_pose_ptr->pose, - ctrl_cmd, predicted_traj, diagnostic); - - if (isStoppedState()) { - // Reset input buffer - for (auto & value : m_mpc.m_input_buffer) { - value = m_ctrl_cmd_prev.steering_tire_angle; - } - // Use previous command value as previous raw steer command - m_mpc.m_raw_steer_cmd_prev = m_ctrl_cmd_prev.steering_tire_angle; - - publishCtrlCmd(m_ctrl_cmd_prev); - publishPredictedTraj(predicted_traj); - publishDiagnostic(diagnostic); - return; - } - - if (!is_mpc_solved) { - RCLCPP_WARN_SKIPFIRST_THROTTLE( - get_logger(), *get_clock(), 5000 /*ms*/, "MPC is not solved. publish 0 velocity."); - ctrl_cmd = getStopControlCommand(); - } - - m_ctrl_cmd_prev = ctrl_cmd; - publishCtrlCmd(ctrl_cmd); - publishPredictedTraj(predicted_traj); - publishDiagnostic(diagnostic); -} - -bool8_t LateralController::checkData() const -{ - if (!m_mpc.hasVehicleModel()) { - RCLCPP_DEBUG(get_logger(), "MPC does not have a vehicle model"); - return false; - } - if (!m_mpc.hasQPSolver()) { - RCLCPP_DEBUG(get_logger(), "MPC does not have a QP solver"); - return false; - } - - if (!m_current_odometry_ptr) { - RCLCPP_DEBUG( - get_logger(), "waiting data. current_velocity = %d", m_current_odometry_ptr != nullptr); - return false; - } - - if (!m_current_steering_ptr) { - RCLCPP_DEBUG( - get_logger(), "waiting data. current_steering = %d", m_current_steering_ptr != nullptr); - return false; - } - - if (m_mpc.m_ref_traj.size() == 0) { - RCLCPP_DEBUG(get_logger(), "trajectory size is zero."); - return false; - } - - return true; -} - -void LateralController::onTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) -{ - m_current_trajectory_ptr = msg; - - if (!m_current_pose_ptr && !updateCurrentPose()) { - RCLCPP_DEBUG(get_logger(), "Current pose is not received yet."); - return; - } - - if (msg->points.size() < 3) { - RCLCPP_DEBUG(get_logger(), "received path size is < 3, not enough."); - return; - } - - if (!isValidTrajectory(*msg)) { - RCLCPP_ERROR(get_logger(), "Trajectory is invalid!! stop computing."); - return; - } - - m_mpc.setReferenceTrajectory( - *msg, m_traj_resample_dist, m_enable_path_smoothing, m_path_filter_moving_ave_num, - m_curvature_smoothing_num_traj, m_curvature_smoothing_num_ref_steer, m_current_pose_ptr); -} - -bool8_t LateralController::updateCurrentPose() -{ - geometry_msgs::msg::TransformStamped transform; - try { - transform = m_tf_buffer.lookupTransform( - m_current_trajectory_ptr->header.frame_id, "base_link", tf2::TimePointZero); - } catch (tf2::TransformException & ex) { - RCLCPP_WARN_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000 /*ms*/, ex.what()); - RCLCPP_WARN_SKIPFIRST_THROTTLE( - get_logger(), *get_clock(), 5000 /*ms*/, m_tf_buffer.allFramesAsString().c_str()); - return false; - } - - geometry_msgs::msg::PoseStamped ps; - ps.header = transform.header; - ps.pose.position.x = transform.transform.translation.x; - ps.pose.position.y = transform.transform.translation.y; - ps.pose.position.z = transform.transform.translation.z; - ps.pose.orientation = transform.transform.rotation; - m_current_pose_ptr = std::make_shared(ps); - return true; -} - -void LateralController::onOdometry(const nav_msgs::msg::Odometry::SharedPtr msg) -{ - m_current_odometry_ptr = msg; -} - -void LateralController::onSteering( - const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg) -{ - m_current_steering_ptr = msg; -} - -autoware_auto_control_msgs::msg::AckermannLateralCommand LateralController::getStopControlCommand() - const -{ - autoware_auto_control_msgs::msg::AckermannLateralCommand cmd; - cmd.steering_tire_angle = static_cast(m_steer_cmd_prev); - cmd.steering_tire_rotation_rate = 0.0; - return cmd; -} - -autoware_auto_control_msgs::msg::AckermannLateralCommand -LateralController::getInitialControlCommand() const -{ - autoware_auto_control_msgs::msg::AckermannLateralCommand cmd; - cmd.steering_tire_angle = m_current_steering_ptr->steering_tire_angle; - cmd.steering_tire_rotation_rate = 0.0; - return cmd; -} - -bool8_t LateralController::isStoppedState() const -{ - // Note: This function used to take into account the distance to the stop line - // for the stop state judgement. However, it has been removed since the steering - // control was turned off when approaching/exceeding the stop line on a curve or - // emergency stop situation and it caused large tracking error. - const int64_t nearest = trajectory_follower::MPCUtils::calcNearestIndex( - *m_current_trajectory_ptr, m_current_pose_ptr->pose); - - // If the nearest index is not found, return false - if (nearest < 0) { - return false; - } - - const float64_t current_vel = m_current_odometry_ptr->twist.twist.linear.x; - const float64_t target_vel = - m_current_trajectory_ptr->points.at(static_cast(nearest)).longitudinal_velocity_mps; - if ( - std::fabs(current_vel) < m_stop_state_entry_ego_speed && - std::fabs(target_vel) < m_stop_state_entry_target_speed) { - return true; - } else { - return false; - } -} - -void LateralController::publishCtrlCmd( - autoware_auto_control_msgs::msg::AckermannLateralCommand ctrl_cmd) -{ - ctrl_cmd.stamp = this->now(); - m_pub_ctrl_cmd->publish(ctrl_cmd); - m_steer_cmd_prev = ctrl_cmd.steering_tire_angle; -} - -void LateralController::publishPredictedTraj( - autoware_auto_planning_msgs::msg::Trajectory & predicted_traj) const -{ - predicted_traj.header.stamp = this->now(); - predicted_traj.header.frame_id = m_current_trajectory_ptr->header.frame_id; - m_pub_predicted_traj->publish(predicted_traj); -} - -void LateralController::publishDiagnostic( - autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic & diagnostic) const -{ - diagnostic.diag_header.data_stamp = this->now(); - diagnostic.diag_header.name = std::string("linear-MPC lateral controller"); - m_pub_diagnostic->publish(diagnostic); -} - -void LateralController::initTimer(float64_t period_s) -{ - const auto period_ns = std::chrono::duration_cast( - std::chrono::duration(period_s)); - m_timer = rclcpp::create_timer( - this, get_clock(), period_ns, std::bind(&LateralController::onTimer, this)); -} - -void LateralController::declareMPCparameters() -{ - m_mpc.m_param.prediction_horizon = declare_parameter("mpc_prediction_horizon"); - m_mpc.m_param.prediction_dt = declare_parameter("mpc_prediction_dt"); - m_mpc.m_param.weight_lat_error = declare_parameter("mpc_weight_lat_error"); - m_mpc.m_param.weight_heading_error = declare_parameter("mpc_weight_heading_error"); - m_mpc.m_param.weight_heading_error_squared_vel = - declare_parameter("mpc_weight_heading_error_squared_vel"); - m_mpc.m_param.weight_steering_input = declare_parameter("mpc_weight_steering_input"); - m_mpc.m_param.weight_steering_input_squared_vel = - declare_parameter("mpc_weight_steering_input_squared_vel"); - m_mpc.m_param.weight_lat_jerk = declare_parameter("mpc_weight_lat_jerk"); - m_mpc.m_param.weight_steer_rate = declare_parameter("mpc_weight_steer_rate"); - m_mpc.m_param.weight_steer_acc = declare_parameter("mpc_weight_steer_acc"); - m_mpc.m_param.low_curvature_weight_lat_error = - declare_parameter("mpc_low_curvature_weight_lat_error"); - m_mpc.m_param.low_curvature_weight_heading_error = - declare_parameter("mpc_low_curvature_weight_heading_error"); - m_mpc.m_param.low_curvature_weight_heading_error_squared_vel = - declare_parameter("mpc_low_curvature_weight_heading_error_squared_vel"); - m_mpc.m_param.low_curvature_weight_steering_input = - declare_parameter("mpc_low_curvature_weight_steering_input"); - m_mpc.m_param.low_curvature_weight_steering_input_squared_vel = - declare_parameter("mpc_low_curvature_weight_steering_input_squared_vel"); - m_mpc.m_param.low_curvature_weight_lat_jerk = - declare_parameter("mpc_low_curvature_weight_lat_jerk"); - m_mpc.m_param.low_curvature_weight_steer_rate = - declare_parameter("mpc_low_curvature_weight_steer_rate"); - m_mpc.m_param.low_curvature_weight_steer_acc = - declare_parameter("mpc_low_curvature_weight_steer_acc"); - m_mpc.m_param.low_curvature_thresh_curvature = - declare_parameter("mpc_low_curvature_thresh_curvature"); - m_mpc.m_param.weight_terminal_lat_error = - declare_parameter("mpc_weight_terminal_lat_error"); - m_mpc.m_param.weight_terminal_heading_error = - declare_parameter("mpc_weight_terminal_heading_error"); - m_mpc.m_param.zero_ff_steer_deg = declare_parameter("mpc_zero_ff_steer_deg"); - m_mpc.m_param.acceleration_limit = declare_parameter("mpc_acceleration_limit"); - m_mpc.m_param.velocity_time_constant = declare_parameter("mpc_velocity_time_constant"); -} - -rcl_interfaces::msg::SetParametersResult LateralController::paramCallback( - const std::vector & parameters) -{ - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - result.reason = "success"; - - // strong exception safety wrt MPCParam - trajectory_follower::MPCParam param = m_mpc.m_param; - try { - update_param(parameters, "mpc_prediction_horizon", param.prediction_horizon); - update_param(parameters, "mpc_prediction_dt", param.prediction_dt); - update_param(parameters, "mpc_weight_lat_error", param.weight_lat_error); - update_param(parameters, "mpc_weight_heading_error", param.weight_heading_error); - update_param( - parameters, "mpc_weight_heading_error_squared_vel", param.weight_heading_error_squared_vel); - update_param(parameters, "mpc_weight_steering_input", param.weight_steering_input); - update_param( - parameters, "mpc_weight_steering_input_squared_vel", param.weight_steering_input_squared_vel); - update_param(parameters, "mpc_weight_lat_jerk", param.weight_lat_jerk); - update_param(parameters, "mpc_weight_steer_rate", param.weight_steer_rate); - update_param(parameters, "mpc_weight_steer_acc", param.weight_steer_acc); - update_param( - parameters, "mpc_low_curvature_weight_lat_error", param.low_curvature_weight_lat_error); - update_param( - parameters, "mpc_low_curvature_weight_heading_error", - param.low_curvature_weight_heading_error); - update_param( - parameters, "mpc_low_curvature_weight_heading_error_squared_vel", - param.low_curvature_weight_heading_error_squared_vel); - update_param( - parameters, "mpc_low_curvature_weight_steering_input", - param.low_curvature_weight_steering_input); - update_param( - parameters, "mpc_low_curvature_weight_steering_input_squared_vel", - param.low_curvature_weight_steering_input_squared_vel); - update_param( - parameters, "mpc_low_curvature_weight_lat_jerk", param.low_curvature_weight_lat_jerk); - update_param( - parameters, "mpc_low_curvature_weight_steer_rate", param.low_curvature_weight_steer_rate); - update_param( - parameters, "mpc_low_curvature_weight_steer_acc", param.low_curvature_weight_steer_acc); - update_param( - parameters, "mpc_low_curvature_thresh_curvature", param.low_curvature_thresh_curvature); - update_param(parameters, "mpc_weight_terminal_lat_error", param.weight_terminal_lat_error); - update_param( - parameters, "mpc_weight_terminal_heading_error", param.weight_terminal_heading_error); - update_param(parameters, "mpc_zero_ff_steer_deg", param.zero_ff_steer_deg); - update_param(parameters, "mpc_acceleration_limit", param.acceleration_limit); - update_param(parameters, "mpc_velocity_time_constant", param.velocity_time_constant); - - // initialize input buffer - update_param(parameters, "input_delay", param.input_delay); - const float64_t delay_step = std::round(param.input_delay / m_mpc.m_ctrl_period); - const float64_t delay = delay_step * m_mpc.m_ctrl_period; - if (param.input_delay != delay) { - param.input_delay = delay; - m_mpc.m_input_buffer = std::deque(static_cast(delay_step), 0.0); - } - - // transaction succeeds, now assign values - m_mpc.m_param = param; - } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { - result.successful = false; - result.reason = e.what(); - } - - return result; -} - -bool8_t LateralController::isValidTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & traj) const -{ - for (const auto & p : traj.points) { - if ( - !isfinite(p.pose.position.x) || !isfinite(p.pose.position.y) || - !isfinite(p.pose.orientation.w) || !isfinite(p.pose.orientation.x) || - !isfinite(p.pose.orientation.y) || !isfinite(p.pose.orientation.z) || - !isfinite(p.longitudinal_velocity_mps) || !isfinite(p.lateral_velocity_mps) || - !isfinite(p.lateral_velocity_mps) || !isfinite(p.heading_rate_rps) || - !isfinite(p.front_wheel_angle_rad) || !isfinite(p.rear_wheel_angle_rad)) { - return false; - } - } - return true; -} - -} // namespace trajectory_follower_nodes -} // namespace control -} // namespace motion -} // namespace autoware - -RCLCPP_COMPONENTS_REGISTER_NODE( - autoware::motion::control::trajectory_follower_nodes::LateralController) diff --git a/control/trajectory_follower_nodes/src/latlon_muxer_node.cpp b/control/trajectory_follower_nodes/src/latlon_muxer_node.cpp deleted file mode 100644 index d50033ecb3199..0000000000000 --- a/control/trajectory_follower_nodes/src/latlon_muxer_node.cpp +++ /dev/null @@ -1,97 +0,0 @@ -// Copyright 2021 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. - -#include "trajectory_follower_nodes/latlon_muxer_node.hpp" - -#include -#include - -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower_nodes -{ - -LatLonMuxer::LatLonMuxer(const rclcpp::NodeOptions & node_options) -: rclcpp::Node("latlon_muxer", node_options) -{ - m_control_cmd_pub = create_publisher( - "~/output/control_cmd", rclcpp::QoS{1}.transient_local()); - m_lat_control_cmd_sub = - create_subscription( - "~/input/lateral/control_cmd", rclcpp::QoS{1}, - std::bind(&LatLonMuxer::latCtrlCmdCallback, this, std::placeholders::_1)); - m_lon_control_cmd_sub = create_subscription( - "~/input/longitudinal/control_cmd", rclcpp::QoS{1}, - std::bind(&LatLonMuxer::lonCtrlCmdCallback, this, std::placeholders::_1)); - m_timeout_thr_sec = declare_parameter("timeout_thr_sec"); -} - -bool LatLonMuxer::checkTimeout() -{ - const auto now = this->now(); - if ((now - m_lat_cmd->stamp).seconds() > m_timeout_thr_sec) { - RCLCPP_ERROR_THROTTLE( - get_logger(), *get_clock(), 1000 /*ms*/, - "Lateral control command too old, muxed command will not be published."); - return false; - } - if ((now - m_lon_cmd->stamp).seconds() > m_timeout_thr_sec) { - RCLCPP_ERROR_THROTTLE( - get_logger(), *get_clock(), 1000 /*ms*/, - "Longitudinal control command too old, muxed command will not be published."); - return false; - } - return true; -} - -void LatLonMuxer::publishCmd() -{ - if (!m_lat_cmd || !m_lon_cmd) { - return; - } - if (!checkTimeout()) { - return; - } - - autoware_auto_control_msgs::msg::AckermannControlCommand out; - out.stamp = this->now(); - out.lateral = *m_lat_cmd; - out.longitudinal = *m_lon_cmd; - - m_control_cmd_pub->publish(out); -} - -void LatLonMuxer::latCtrlCmdCallback( - const autoware_auto_control_msgs::msg::AckermannLateralCommand::SharedPtr input_msg) -{ - m_lat_cmd = input_msg; - publishCmd(); -} - -void LatLonMuxer::lonCtrlCmdCallback( - const autoware_auto_control_msgs::msg::LongitudinalCommand::SharedPtr input_msg) -{ - m_lon_cmd = std::make_shared(*input_msg); - publishCmd(); -} -} // namespace trajectory_follower_nodes -} // namespace control -} // namespace motion -} // namespace autoware - -RCLCPP_COMPONENTS_REGISTER_NODE(autoware::motion::control::trajectory_follower_nodes::LatLonMuxer) diff --git a/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp b/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp deleted file mode 100644 index fb3cda35d8317..0000000000000 --- a/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp +++ /dev/null @@ -1,944 +0,0 @@ -// Copyright 2021 Tier IV, Inc. All rights reserved. -// -// 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 "trajectory_follower_nodes/longitudinal_controller_node.hpp" - -#include "motion_common/motion_common.hpp" -#include "motion_common/trajectory_common.hpp" -#include "time_utils/time_utils.hpp" - -#include -#include -#include -#include -#include -#include - -namespace autoware -{ -namespace motion -{ -namespace control -{ -namespace trajectory_follower_nodes -{ -LongitudinalController::LongitudinalController(const rclcpp::NodeOptions & node_options) -: Node("longitudinal_controller", node_options) -{ - using std::placeholders::_1; - - // parameters timer - m_longitudinal_ctrl_period = declare_parameter("longitudinal_ctrl_period"); - - m_wheel_base = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo().wheel_base_m; - - // parameters for delay compensation - m_delay_compensation_time = declare_parameter("delay_compensation_time"); // [s] - - // parameters to enable functions - m_enable_smooth_stop = declare_parameter("enable_smooth_stop"); - m_enable_overshoot_emergency = declare_parameter("enable_overshoot_emergency"); - m_enable_slope_compensation = declare_parameter("enable_slope_compensation"); - - // parameters for state transition - { - auto & p = m_state_transition_params; - // drive - p.drive_state_stop_dist = declare_parameter("drive_state_stop_dist"); // [m] - p.drive_state_offset_stop_dist = - declare_parameter("drive_state_offset_stop_dist"); // [m] - // stopping - p.stopping_state_stop_dist = declare_parameter("stopping_state_stop_dist"); // [m] - p.stopped_state_entry_duration_time = - declare_parameter("stopped_state_entry_duration_time"); // [s] - // stop - p.stopped_state_entry_vel = declare_parameter("stopped_state_entry_vel"); // [m/s] - p.stopped_state_entry_acc = declare_parameter("stopped_state_entry_acc"); // [m/s²] - // emergency - p.emergency_state_overshoot_stop_dist = - declare_parameter("emergency_state_overshoot_stop_dist"); // [m] - p.emergency_state_traj_trans_dev = - declare_parameter("emergency_state_traj_trans_dev"); // [m] - p.emergency_state_traj_rot_dev = - declare_parameter("emergency_state_traj_rot_dev"); // [m] - } - - // parameters for drive state - { - // initialize PID gain - const float64_t kp{declare_parameter("kp")}; - const float64_t ki{declare_parameter("ki")}; - const float64_t kd{declare_parameter("kd")}; - m_pid_vel.setGains(kp, ki, kd); - - // initialize PID limits - const float64_t max_pid{declare_parameter("max_out")}; // [m/s^2] - const float64_t min_pid{declare_parameter("min_out")}; // [m/s^2] - const float64_t max_p{declare_parameter("max_p_effort")}; // [m/s^2] - const float64_t min_p{declare_parameter("min_p_effort")}; // [m/s^2] - const float64_t max_i{declare_parameter("max_i_effort")}; // [m/s^2] - const float64_t min_i{declare_parameter("min_i_effort")}; // [m/s^2] - const float64_t max_d{declare_parameter("max_d_effort")}; // [m/s^2] - const float64_t min_d{declare_parameter("min_d_effort")}; // [m/s^2] - m_pid_vel.setLimits(max_pid, min_pid, max_p, min_p, max_i, min_i, max_d, min_d); - - // set lowpass filter for vel error and pitch - const float64_t lpf_vel_error_gain{declare_parameter("lpf_vel_error_gain")}; - m_lpf_vel_error = - std::make_shared(0.0, lpf_vel_error_gain); - - m_current_vel_threshold_pid_integrate = - declare_parameter("current_vel_threshold_pid_integration"); // [m/s] - - m_enable_brake_keeping_before_stop = - declare_parameter("enable_brake_keeping_before_stop"); // [-] - m_brake_keeping_acc = declare_parameter("brake_keeping_acc"); // [m/s^2] - } - - // parameters for smooth stop state - { - const float64_t max_strong_acc{ - declare_parameter("smooth_stop_max_strong_acc")}; // [m/s^2] - const float64_t min_strong_acc{ - declare_parameter("smooth_stop_min_strong_acc")}; // [m/s^2] - const float64_t weak_acc{declare_parameter("smooth_stop_weak_acc")}; // [m/s^2] - const float64_t weak_stop_acc{ - declare_parameter("smooth_stop_weak_stop_acc")}; // [m/s^2] - const float64_t strong_stop_acc{ - declare_parameter("smooth_stop_strong_stop_acc")}; // [m/s^2] - - const float64_t max_fast_vel{ - declare_parameter("smooth_stop_max_fast_vel")}; // [m/s] - const float64_t min_running_vel{ - declare_parameter("smooth_stop_min_running_vel")}; // [m/s] - const float64_t min_running_acc{ - declare_parameter("smooth_stop_min_running_acc")}; // [m/s^2] - const float64_t weak_stop_time{ - declare_parameter("smooth_stop_weak_stop_time")}; // [s] - - const float64_t weak_stop_dist{ - declare_parameter("smooth_stop_weak_stop_dist")}; // [m] - const float64_t strong_stop_dist{ - declare_parameter("smooth_stop_strong_stop_dist")}; // [m] - - m_smooth_stop.setParams( - max_strong_acc, min_strong_acc, weak_acc, weak_stop_acc, strong_stop_acc, max_fast_vel, - min_running_vel, min_running_acc, weak_stop_time, weak_stop_dist, strong_stop_dist); - } - - // parameters for stop state - { - auto & p = m_stopped_state_params; - p.vel = declare_parameter("stopped_vel"); // [m/s] - p.acc = declare_parameter("stopped_acc"); // [m/s^2] - p.jerk = declare_parameter("stopped_jerk"); // [m/s^3] - } - - // parameters for emergency state - { - auto & p = m_emergency_state_params; - p.vel = declare_parameter("emergency_vel"); // [m/s] - p.acc = declare_parameter("emergency_acc"); // [m/s^2] - p.jerk = declare_parameter("emergency_jerk"); // [m/s^3] - } - - // parameters for acceleration limit - m_max_acc = declare_parameter("max_acc"); // [m/s^2] - m_min_acc = declare_parameter("min_acc"); // [m/s^2] - - // parameters for jerk limit - m_max_jerk = declare_parameter("max_jerk"); // [m/s^3] - m_min_jerk = declare_parameter("min_jerk"); // [m/s^3] - - // parameters for slope compensation - m_use_traj_for_pitch = declare_parameter("use_trajectory_for_pitch_calculation"); - const float64_t lpf_pitch_gain{declare_parameter("lpf_pitch_gain")}; - m_lpf_pitch = std::make_shared(0.0, lpf_pitch_gain); - m_max_pitch_rad = declare_parameter("max_pitch_rad"); // [rad] - m_min_pitch_rad = declare_parameter("min_pitch_rad"); // [rad] - - // subscriber, publisher - m_sub_current_velocity = create_subscription( - "~/input/current_odometry", rclcpp::QoS{1}, - std::bind(&LongitudinalController::callbackCurrentVelocity, this, _1)); - m_sub_trajectory = create_subscription( - "~/input/current_trajectory", rclcpp::QoS{1}, - std::bind(&LongitudinalController::callbackTrajectory, this, _1)); - m_pub_control_cmd = create_publisher( - "~/output/control_cmd", rclcpp::QoS{1}); - m_pub_slope = create_publisher( - "~/output/slope_angle", rclcpp::QoS{1}); - m_pub_debug = create_publisher( - "~/output/diagnostic", rclcpp::QoS{1}); - - // Timer - { - const auto period_ns = std::chrono::duration_cast( - std::chrono::duration(m_longitudinal_ctrl_period)); - m_timer_control = rclcpp::create_timer( - this, get_clock(), period_ns, std::bind(&LongitudinalController::callbackTimerControl, this)); - } - - // set parameter callback - m_set_param_res = this->add_on_set_parameters_callback( - std::bind(&LongitudinalController::paramCallback, this, _1)); - - // set lowpass filter for acc - m_lpf_acc = std::make_shared(0.0, 0.2); -} - -void LongitudinalController::callbackCurrentVelocity( - const nav_msgs::msg::Odometry::ConstSharedPtr msg) -{ - if (m_current_velocity_ptr) { - m_prev_velocity_ptr = m_current_velocity_ptr; - } - m_current_velocity_ptr = std::make_shared(*msg); -} - -void LongitudinalController::callbackTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg) -{ - if (!trajectory_follower::longitudinal_utils::isValidTrajectory(*msg)) { - RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 3000, "received invalid trajectory. ignore."); - return; - } - - if (msg->points.size() < 2) { - RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), 3000, "Unexpected trajectory size < 2. Ignored."); - return; - } - - m_trajectory_ptr = std::make_shared(*msg); -} - -rcl_interfaces::msg::SetParametersResult LongitudinalController::paramCallback( - const std::vector & parameters) -{ - auto update_param = [&](const std::string & name, float64_t & v) { - auto it = std::find_if( - parameters.cbegin(), parameters.cend(), - [&name](const rclcpp::Parameter & parameter) { return parameter.get_name() == name; }); - if (it != parameters.cend()) { - v = it->as_double(); - return true; - } - return false; - }; - - // delay compensation - update_param("delay_compensation_time", m_delay_compensation_time); - - // state transition - { - auto & p = m_state_transition_params; - update_param("drive_state_stop_dist", p.drive_state_stop_dist); - update_param("stopping_state_stop_dist", p.stopping_state_stop_dist); - update_param("stopped_state_entry_duration_time", p.stopped_state_entry_duration_time); - update_param("stopped_state_entry_vel", p.stopped_state_entry_vel); - update_param("stopped_state_entry_acc", p.stopped_state_entry_acc); - update_param("emergency_state_overshoot_stop_dist", p.emergency_state_overshoot_stop_dist); - update_param("emergency_state_traj_trans_dev", p.emergency_state_traj_trans_dev); - update_param("emergency_state_traj_rot_dev", p.emergency_state_traj_rot_dev); - } - - // drive state - { - float64_t kp{get_parameter("kp").as_double()}; - float64_t ki{get_parameter("ki").as_double()}; - float64_t kd{get_parameter("kd").as_double()}; - update_param("kp", kp); - update_param("ki", ki); - update_param("kd", kd); - m_pid_vel.setGains(kp, ki, kd); - - float64_t max_pid{get_parameter("max_out").as_double()}; - float64_t min_pid{get_parameter("min_out").as_double()}; - float64_t max_p{get_parameter("max_p_effort").as_double()}; - float64_t min_p{get_parameter("min_p_effort").as_double()}; - float64_t max_i{get_parameter("max_i_effort").as_double()}; - float64_t min_i{get_parameter("min_i_effort").as_double()}; - float64_t max_d{get_parameter("max_d_effort").as_double()}; - float64_t min_d{get_parameter("min_d_effort").as_double()}; - update_param("max_out", max_pid); - update_param("min_out", min_pid); - update_param("max_p_effort", max_p); - update_param("min_p_effort", min_p); - update_param("max_i_effort", max_i); - update_param("min_i_effort", min_i); - update_param("max_d_effort", max_d); - update_param("min_d_effort", min_d); - m_pid_vel.setLimits(max_pid, min_pid, max_p, min_p, max_i, min_i, max_d, min_d); - - update_param("current_vel_threshold_pid_integration", m_current_vel_threshold_pid_integrate); - } - - // stopping state - { - float64_t max_strong_acc{get_parameter("smooth_stop_max_strong_acc").as_double()}; - float64_t min_strong_acc{get_parameter("smooth_stop_min_strong_acc").as_double()}; - float64_t weak_acc{get_parameter("smooth_stop_weak_acc").as_double()}; - float64_t weak_stop_acc{get_parameter("smooth_stop_weak_stop_acc").as_double()}; - float64_t strong_stop_acc{get_parameter("smooth_stop_strong_stop_acc").as_double()}; - float64_t max_fast_vel{get_parameter("smooth_stop_max_fast_vel").as_double()}; - float64_t min_running_vel{get_parameter("smooth_stop_min_running_vel").as_double()}; - float64_t min_running_acc{get_parameter("smooth_stop_min_running_acc").as_double()}; - float64_t weak_stop_time{get_parameter("smooth_stop_weak_stop_time").as_double()}; - float64_t weak_stop_dist{get_parameter("smooth_stop_weak_stop_dist").as_double()}; - float64_t strong_stop_dist{get_parameter("smooth_stop_strong_stop_dist").as_double()}; - update_param("smooth_stop_max_strong_acc", max_strong_acc); - update_param("smooth_stop_min_strong_acc", min_strong_acc); - update_param("smooth_stop_weak_acc", weak_acc); - update_param("smooth_stop_weak_stop_acc", weak_stop_acc); - update_param("smooth_stop_strong_stop_acc", strong_stop_acc); - update_param("smooth_stop_max_fast_vel", max_fast_vel); - update_param("smooth_stop_min_running_vel", min_running_vel); - update_param("smooth_stop_min_running_acc", min_running_acc); - update_param("smooth_stop_weak_stop_time", weak_stop_time); - update_param("smooth_stop_weak_stop_dist", weak_stop_dist); - update_param("smooth_stop_strong_stop_dist", strong_stop_dist); - m_smooth_stop.setParams( - max_strong_acc, min_strong_acc, weak_acc, weak_stop_acc, strong_stop_acc, max_fast_vel, - min_running_vel, min_running_acc, weak_stop_time, weak_stop_dist, strong_stop_dist); - } - - // stop state - { - auto & p = m_stopped_state_params; - update_param("stopped_vel", p.vel); - update_param("stopped_acc", p.acc); - update_param("stopped_jerk", p.jerk); - } - - // emergency state - { - auto & p = m_emergency_state_params; - update_param("emergency_vel", p.vel); - update_param("emergency_acc", p.acc); - update_param("emergency_jerk", p.jerk); - } - - // acceleration limit - update_param("min_acc", m_min_acc); - - // jerk limit - update_param("max_jerk", m_max_jerk); - update_param("min_jerk", m_min_jerk); - - // slope compensation - update_param("max_pitch_rad", m_max_pitch_rad); - update_param("min_pitch_rad", m_min_pitch_rad); - - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - result.reason = "success"; - return result; -} - -void LongitudinalController::callbackTimerControl() -{ - // wait for initial pointers - if ( - !m_current_velocity_ptr || !m_prev_velocity_ptr || !m_trajectory_ptr || - !m_tf_buffer.canTransform(m_trajectory_ptr->header.frame_id, "base_link", tf2::TimePointZero)) { - return; - } - - // get current ego pose - geometry_msgs::msg::TransformStamped tf = - m_tf_buffer.lookupTransform(m_trajectory_ptr->header.frame_id, "base_link", tf2::TimePointZero); - - // calculate current pose and control data - geometry_msgs::msg::Pose current_pose; - current_pose.position.x = tf.transform.translation.x; - current_pose.position.y = tf.transform.translation.y; - current_pose.position.z = tf.transform.translation.z; - current_pose.orientation = tf.transform.rotation; - - const auto control_data = getControlData(current_pose); - - // self pose is far from trajectory - if (control_data.is_far_from_trajectory) { - m_control_state = ControlState::EMERGENCY; // update control state - const Motion raw_ctrl_cmd = calcEmergencyCtrlCmd(control_data.dt); // calculate control command - m_prev_raw_ctrl_cmd = raw_ctrl_cmd; - publishCtrlCmd(raw_ctrl_cmd, control_data.current_motion.vel); // publish control command - publishDebugData(raw_ctrl_cmd, control_data); // publish debug data - return; - } - - // update control state - m_control_state = updateControlState(m_control_state, control_data); - - // calculate control command - const Motion ctrl_cmd = calcCtrlCmd(m_control_state, current_pose, control_data); - - // publish control command - publishCtrlCmd(ctrl_cmd, control_data.current_motion.vel); - - // publish debug data - publishDebugData(ctrl_cmd, control_data); -} - -LongitudinalController::ControlData LongitudinalController::getControlData( - const geometry_msgs::msg::Pose & current_pose) -{ - ControlData control_data{}; - - // dt - control_data.dt = getDt(); - - // current velocity and acceleration - control_data.current_motion = getCurrentMotion(); - - // nearest idx - const float64_t max_dist = m_state_transition_params.emergency_state_traj_trans_dev; - const float64_t max_yaw = m_state_transition_params.emergency_state_traj_rot_dev; - const auto nearest_idx_opt = - motion_common::findNearestIndex(m_trajectory_ptr->points, current_pose, max_dist, max_yaw); - - // return here if nearest index is not found - if (!nearest_idx_opt) { - control_data.is_far_from_trajectory = true; - return control_data; - } - control_data.nearest_idx = *nearest_idx_opt; - - // shift - control_data.shift = getCurrentShift(control_data.nearest_idx); - if (control_data.shift != m_prev_shift) { - m_pid_vel.reset(); - } - m_prev_shift = control_data.shift; - - // distance to stopline - control_data.stop_dist = trajectory_follower::longitudinal_utils::calcStopDistance( - current_pose, *m_trajectory_ptr, max_dist, max_yaw); - - // pitch - const float64_t raw_pitch = - trajectory_follower::longitudinal_utils::getPitchByPose(current_pose.orientation); - const float64_t traj_pitch = trajectory_follower::longitudinal_utils::getPitchByTraj( - *m_trajectory_ptr, control_data.nearest_idx, m_wheel_base); - control_data.slope_angle = m_use_traj_for_pitch ? traj_pitch : m_lpf_pitch->filter(raw_pitch); - updatePitchDebugValues(control_data.slope_angle, traj_pitch, raw_pitch); - - return control_data; -} - -LongitudinalController::Motion LongitudinalController::calcEmergencyCtrlCmd( - const float64_t dt) const -{ - // These accelerations are without slope compensation - const auto & p = m_emergency_state_params; - const float64_t vel = trajectory_follower::longitudinal_utils::applyDiffLimitFilter( - p.vel, m_prev_raw_ctrl_cmd.vel, dt, p.acc); - const float64_t acc = trajectory_follower::longitudinal_utils::applyDiffLimitFilter( - p.acc, m_prev_raw_ctrl_cmd.acc, dt, p.jerk); - - auto clock = rclcpp::Clock{RCL_ROS_TIME}; - RCLCPP_ERROR_THROTTLE( - get_logger(), clock, 3000, "[Emergency stop] vel: %3.3f, acc: %3.3f", vel, acc); - - return Motion{vel, acc}; -} - -LongitudinalController::ControlState LongitudinalController::updateControlState( - const ControlState current_control_state, const ControlData & control_data) -{ - const float64_t current_vel = control_data.current_motion.vel; - const float64_t current_acc = control_data.current_motion.acc; - const float64_t stop_dist = control_data.stop_dist; - - // flags for state transition - const auto & p = m_state_transition_params; - - const bool8_t departure_condition_from_stopping = - stop_dist > p.drive_state_stop_dist + p.drive_state_offset_stop_dist; - const bool8_t departure_condition_from_stopped = stop_dist > p.drive_state_stop_dist; - - const bool8_t stopping_condition = stop_dist < p.stopping_state_stop_dist; - if ( - std::fabs(current_vel) > p.stopped_state_entry_vel || - std::fabs(current_acc) > p.stopped_state_entry_acc) { - m_last_running_time = std::make_shared(this->now()); - } - const bool8_t stopped_condition = - m_last_running_time - ? (this->now() - *m_last_running_time).seconds() > p.stopped_state_entry_duration_time - : false; - - const bool8_t emergency_condition = - m_enable_overshoot_emergency && stop_dist < -p.emergency_state_overshoot_stop_dist; - - // transit state - if (current_control_state == ControlState::DRIVE) { - if (emergency_condition) { - return ControlState::EMERGENCY; - } - - if (m_enable_smooth_stop) { - if (stopping_condition) { - // predictions after input time delay - const float64_t pred_vel_in_target = - predictedVelocityInTargetPoint(control_data.current_motion, m_delay_compensation_time); - const float64_t pred_stop_dist = - control_data.stop_dist - - 0.5 * (pred_vel_in_target + current_vel) * m_delay_compensation_time; - m_smooth_stop.init(pred_vel_in_target, pred_stop_dist); - return ControlState::STOPPING; - } - } else { - if (stopped_condition && !departure_condition_from_stopped) { - return ControlState::STOPPED; - } - } - } else if (current_control_state == ControlState::STOPPING) { - if (emergency_condition) { - return ControlState::EMERGENCY; - } - - if (stopped_condition) { - return ControlState::STOPPED; - } - - if (departure_condition_from_stopping) { - m_pid_vel.reset(); - m_lpf_vel_error->reset(0.0); - // prevent the car from taking a long time to start to move - m_prev_ctrl_cmd.acc = std::max(0.0, m_prev_ctrl_cmd.acc); - return ControlState::DRIVE; - } - } else if (current_control_state == ControlState::STOPPED) { - if (departure_condition_from_stopped) { - m_pid_vel.reset(); - m_lpf_vel_error->reset(0.0); - // prevent the car from taking a long time to start to move - m_prev_ctrl_cmd.acc = std::max(0.0, m_prev_ctrl_cmd.acc); - return ControlState::DRIVE; - } - } else if (m_control_state == ControlState::EMERGENCY) { - if (stopped_condition && !emergency_condition) { - return ControlState::STOPPED; - } - } - - return current_control_state; -} - -LongitudinalController::Motion LongitudinalController::calcCtrlCmd( - const ControlState & current_control_state, const geometry_msgs::msg::Pose & current_pose, - const ControlData & control_data) -{ - const size_t nearest_idx = control_data.nearest_idx; - const float64_t current_vel = control_data.current_motion.vel; - const float64_t current_acc = control_data.current_motion.acc; - - // velocity and acceleration command - Motion raw_ctrl_cmd{}; - Motion target_motion{}; - if (current_control_state == ControlState::DRIVE) { - const auto target_pose = trajectory_follower::longitudinal_utils::calcPoseAfterTimeDelay( - current_pose, m_delay_compensation_time, current_vel); - const auto target_interpolated_point = - calcInterpolatedTargetValue(*m_trajectory_ptr, target_pose, nearest_idx); - target_motion = Motion{ - target_interpolated_point.longitudinal_velocity_mps, - target_interpolated_point.acceleration_mps2}; - - target_motion = keepBrakeBeforeStop(*m_trajectory_ptr, target_motion, nearest_idx); - - const float64_t pred_vel_in_target = - predictedVelocityInTargetPoint(control_data.current_motion, m_delay_compensation_time); - m_debug_values.setValues( - trajectory_follower::DebugValues::TYPE::PREDICTED_VEL, pred_vel_in_target); - - raw_ctrl_cmd.vel = target_motion.vel; - raw_ctrl_cmd.acc = applyVelocityFeedback(target_motion, control_data.dt, pred_vel_in_target); - RCLCPP_DEBUG( - get_logger(), - "[feedback control] vel: %3.3f, acc: %3.3f, dt: %3.3f, v_curr: %3.3f, v_ref: %3.3f " - "feedback_ctrl_cmd.ac: %3.3f", - raw_ctrl_cmd.vel, raw_ctrl_cmd.acc, control_data.dt, current_vel, target_motion.vel, - raw_ctrl_cmd.acc); - } else if (current_control_state == ControlState::STOPPING) { - raw_ctrl_cmd.acc = m_smooth_stop.calculate( - control_data.stop_dist, current_vel, current_acc, m_vel_hist, m_delay_compensation_time); - raw_ctrl_cmd.vel = m_stopped_state_params.vel; - - RCLCPP_DEBUG( - get_logger(), "[smooth stop]: Smooth stopping. vel: %3.3f, acc: %3.3f", raw_ctrl_cmd.vel, - raw_ctrl_cmd.acc); - } else if (current_control_state == ControlState::STOPPED) { - // This acceleration is without slope compensation - const auto & p = m_stopped_state_params; - raw_ctrl_cmd.vel = p.vel; - raw_ctrl_cmd.acc = trajectory_follower::longitudinal_utils::applyDiffLimitFilter( - p.acc, m_prev_raw_ctrl_cmd.acc, control_data.dt, p.jerk); - - RCLCPP_DEBUG( - get_logger(), "[Stopped]. vel: %3.3f, acc: %3.3f", raw_ctrl_cmd.vel, raw_ctrl_cmd.acc); - } else if (current_control_state == ControlState::EMERGENCY) { - raw_ctrl_cmd = calcEmergencyCtrlCmd(control_data.dt); - } - - // store acceleration without slope compensation - m_prev_raw_ctrl_cmd = raw_ctrl_cmd; - - // apply slope compensation and filter acceleration and jerk - const float64_t filtered_acc_cmd = calcFilteredAcc(raw_ctrl_cmd.acc, control_data); - const Motion filtered_ctrl_cmd{raw_ctrl_cmd.vel, filtered_acc_cmd}; - - // update debug visualization - updateDebugVelAcc(target_motion, current_pose, control_data); - - return filtered_ctrl_cmd; -} - -// Do not use nearest_idx here -void LongitudinalController::publishCtrlCmd(const Motion & ctrl_cmd, float64_t current_vel) -{ - // publish control command - autoware_auto_control_msgs::msg::LongitudinalCommand cmd{}; - cmd.stamp = this->now(); - cmd.speed = static_cast(ctrl_cmd.vel); - cmd.acceleration = static_cast(ctrl_cmd.acc); - m_pub_control_cmd->publish(cmd); - - // store current velocity history - m_vel_hist.push_back({this->now(), current_vel}); - while (m_vel_hist.size() > static_cast(0.5 / m_longitudinal_ctrl_period)) { - m_vel_hist.erase(m_vel_hist.begin()); - } - - m_prev_ctrl_cmd = ctrl_cmd; -} - -void LongitudinalController::publishDebugData( - const Motion & ctrl_cmd, const ControlData & control_data) -{ - using trajectory_follower::DebugValues; - // set debug values - m_debug_values.setValues(DebugValues::TYPE::DT, control_data.dt); - m_debug_values.setValues(DebugValues::TYPE::CALCULATED_ACC, control_data.current_motion.acc); - m_debug_values.setValues(DebugValues::TYPE::SHIFT, static_cast(control_data.shift)); - m_debug_values.setValues(DebugValues::TYPE::STOP_DIST, control_data.stop_dist); - m_debug_values.setValues( - DebugValues::TYPE::CONTROL_STATE, static_cast(m_control_state)); - m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_PUBLISHED, ctrl_cmd.acc); - - // publish debug values - autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic debug_msg{}; - debug_msg.diag_header.data_stamp = this->now(); - for (const auto & v : m_debug_values.getValues()) { - debug_msg.diag_array.data.push_back( - static_cast(v)); - } - m_pub_debug->publish(debug_msg); - - // slope angle - autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic slope_msg{}; - slope_msg.diag_header.data_stamp = this->now(); - slope_msg.diag_array.data.push_back( - static_cast(control_data.slope_angle)); - m_pub_slope->publish(slope_msg); -} - -float64_t LongitudinalController::getDt() -{ - float64_t dt; - if (!m_prev_control_time) { - dt = m_longitudinal_ctrl_period; - m_prev_control_time = std::make_shared(this->now()); - } else { - dt = (this->now() - *m_prev_control_time).seconds(); - *m_prev_control_time = this->now(); - } - const float64_t max_dt = m_longitudinal_ctrl_period * 2.0; - const float64_t min_dt = m_longitudinal_ctrl_period * 0.5; - return std::max(std::min(dt, max_dt), min_dt); -} - -LongitudinalController::Motion LongitudinalController::getCurrentMotion() const -{ - const float64_t dv = - m_current_velocity_ptr->twist.twist.linear.x - m_prev_velocity_ptr->twist.twist.linear.x; - const float64_t dt = std::max( - (rclcpp::Time(m_current_velocity_ptr->header.stamp) - - rclcpp::Time(m_prev_velocity_ptr->header.stamp)) - .seconds(), - 1e-03); - const float64_t accel = dv / dt; - - const float64_t current_vel = m_current_velocity_ptr->twist.twist.linear.x; - const float64_t current_acc = m_lpf_acc->filter(accel); - - return Motion{current_vel, current_acc}; -} - -enum LongitudinalController::Shift LongitudinalController::getCurrentShift( - const size_t nearest_idx) const -{ - constexpr float64_t epsilon = 1e-5; - - const float64_t target_vel = m_trajectory_ptr->points.at(nearest_idx).longitudinal_velocity_mps; - - if (target_vel > epsilon) { - return Shift::Forward; - } else if (target_vel < -epsilon) { - return Shift::Reverse; - } - - return m_prev_shift; -} - -float64_t LongitudinalController::calcFilteredAcc( - const float64_t raw_acc, const ControlData & control_data) -{ - using trajectory_follower::DebugValues; - const float64_t acc_max_filtered = ::motion::motion_common::clamp(raw_acc, m_min_acc, m_max_acc); - m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_ACC_LIMITED, acc_max_filtered); - - // store ctrl cmd without slope filter - storeAccelCmd(acc_max_filtered); - - const float64_t acc_slope_filtered = - applySlopeCompensation(acc_max_filtered, control_data.slope_angle, control_data.shift); - m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_SLOPE_APPLIED, acc_slope_filtered); - - // This jerk filter must be applied after slope compensation - const float64_t acc_jerk_filtered = trajectory_follower::longitudinal_utils::applyDiffLimitFilter( - acc_slope_filtered, m_prev_ctrl_cmd.acc, control_data.dt, m_max_jerk, m_min_jerk); - m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_JERK_LIMITED, acc_jerk_filtered); - - return acc_jerk_filtered; -} - -void LongitudinalController::storeAccelCmd(const float64_t accel) -{ - if (m_control_state == ControlState::DRIVE) { - // convert format - autoware_auto_control_msgs::msg::LongitudinalCommand cmd; - cmd.stamp = this->now(); - cmd.acceleration = static_cast(accel); - - // store published ctrl cmd - m_ctrl_cmd_vec.emplace_back(cmd); - } else { - // reset command - m_ctrl_cmd_vec.clear(); - } - - // remove unused ctrl cmd - if (m_ctrl_cmd_vec.size() <= 2) { - return; - } - if ((this->now() - m_ctrl_cmd_vec.at(1).stamp).seconds() > m_delay_compensation_time) { - m_ctrl_cmd_vec.erase(m_ctrl_cmd_vec.begin()); - } -} - -float64_t LongitudinalController::applySlopeCompensation( - const float64_t input_acc, const float64_t pitch, const Shift shift) const -{ - if (!m_enable_slope_compensation) { - return input_acc; - } - const float64_t pitch_limited = std::min(std::max(pitch, m_min_pitch_rad), m_max_pitch_rad); - - // Acceleration command is always positive independent of direction (= shift) when car is running - float64_t sign = (shift == Shift::Forward) ? -1 : (shift == Shift::Reverse ? 1 : 0); - float64_t compensated_acc = input_acc + sign * 9.81 * std::sin(pitch_limited); - return compensated_acc; -} - -LongitudinalController::Motion LongitudinalController::keepBrakeBeforeStop( - const autoware_auto_planning_msgs::msg::Trajectory & traj, const Motion & target_motion, - const size_t nearest_idx) const -{ - Motion output_motion = target_motion; - - if (m_enable_brake_keeping_before_stop == false) { - return output_motion; - } - // const auto stop_idx = tier4_autoware_utils::searchZeroVelocityIndex(traj.points); - const auto stop_idx = motion_common::searchZeroVelocityIndex(traj.points); - if (!stop_idx) { - return output_motion; - } - - float64_t min_acc_before_stop = std::numeric_limits::max(); - size_t min_acc_idx = std::numeric_limits::max(); - for (int i = static_cast(*stop_idx); i >= 0; --i) { - const auto ui = static_cast(i); - if (traj.points.at(ui).acceleration_mps2 > static_cast(min_acc_before_stop)) { - break; - } - min_acc_before_stop = traj.points.at(ui).acceleration_mps2; - min_acc_idx = ui; - } - - const float64_t brake_keeping_acc = std::max(m_brake_keeping_acc, min_acc_before_stop); - if (nearest_idx >= min_acc_idx && target_motion.acc > brake_keeping_acc) { - output_motion.acc = brake_keeping_acc; - } - - return output_motion; -} - -autoware_auto_planning_msgs::msg::TrajectoryPoint -LongitudinalController::calcInterpolatedTargetValue( - const autoware_auto_planning_msgs::msg::Trajectory & traj, const geometry_msgs::msg::Pose & pose, - const size_t nearest_idx) const -{ - if (traj.points.size() == 1) { - return traj.points.at(0); - } - - // If the current position is not within the reference trajectory, enable the edge value. - // Else, apply linear interpolation - if (nearest_idx == 0) { - if (motion_common::calcSignedArcLength(traj.points, pose.position, 0) > 0) { - return traj.points.at(0); - } - } - if (nearest_idx == traj.points.size() - 1) { - if ( - motion_common::calcSignedArcLength(traj.points, pose.position, traj.points.size() - 1) < 0) { - return traj.points.at(traj.points.size() - 1); - } - } - - // apply linear interpolation - return trajectory_follower::longitudinal_utils::lerpTrajectoryPoint( - traj.points, pose, m_state_transition_params.emergency_state_traj_trans_dev, - m_state_transition_params.emergency_state_traj_rot_dev); -} - -float64_t LongitudinalController::predictedVelocityInTargetPoint( - const Motion current_motion, const float64_t delay_compensation_time) const -{ - const float64_t current_vel = current_motion.vel; - const float64_t current_acc = current_motion.acc; - - if (std::fabs(current_vel) < 1e-01) { // when velocity is low, no prediction - return current_vel; - } - - const float64_t current_vel_abs = std::fabs(current_vel); - if (m_ctrl_cmd_vec.size() == 0) { - const float64_t pred_vel = current_vel + current_acc * delay_compensation_time; - // avoid to change sign of current_vel and pred_vel - return pred_vel > 0 ? std::copysign(pred_vel, current_vel) : 0.0; - } - - float64_t pred_vel = current_vel_abs; - - const auto past_delay_time = - this->now() - rclcpp::Duration::from_seconds(delay_compensation_time); - for (std::size_t i = 0; i < m_ctrl_cmd_vec.size(); ++i) { - if ((this->now() - m_ctrl_cmd_vec.at(i).stamp).seconds() < m_delay_compensation_time) { - if (i == 0) { - // size of m_ctrl_cmd_vec is less than m_delay_compensation_time - pred_vel = current_vel_abs + static_cast(m_ctrl_cmd_vec.at(i).acceleration) * - delay_compensation_time; - return pred_vel > 0 ? std::copysign(pred_vel, current_vel) : 0.0; - } - // add velocity to accel * dt - const float64_t acc = m_ctrl_cmd_vec.at(i - 1).acceleration; - const auto curr_time_i = rclcpp::Time(m_ctrl_cmd_vec.at(i).stamp); - const float64_t time_to_next_acc = std::min( - (curr_time_i - rclcpp::Time(m_ctrl_cmd_vec.at(i - 1).stamp)).seconds(), - (curr_time_i - past_delay_time).seconds()); - pred_vel += acc * time_to_next_acc; - } - } - - const float64_t last_acc = m_ctrl_cmd_vec.at(m_ctrl_cmd_vec.size() - 1).acceleration; - const float64_t time_to_current = - (this->now() - m_ctrl_cmd_vec.at(m_ctrl_cmd_vec.size() - 1).stamp).seconds(); - pred_vel += last_acc * time_to_current; - - // avoid to change sign of current_vel and pred_vel - return pred_vel > 0 ? std::copysign(pred_vel, current_vel) : 0.0; -} - -float64_t LongitudinalController::applyVelocityFeedback( - const Motion target_motion, const float64_t dt, const float64_t current_vel) -{ - using trajectory_follower::DebugValues; - const float64_t current_vel_abs = std::fabs(current_vel); - const float64_t target_vel_abs = std::fabs(target_motion.vel); - const bool8_t enable_integration = (current_vel_abs > m_current_vel_threshold_pid_integrate); - const float64_t error_vel_filtered = m_lpf_vel_error->filter(target_vel_abs - current_vel_abs); - - std::vector pid_contributions(3); - const float64_t pid_acc = - m_pid_vel.calculate(error_vel_filtered, dt, enable_integration, pid_contributions); - const float64_t feedback_acc = target_motion.acc + pid_acc; - - m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_PID_APPLIED, feedback_acc); - m_debug_values.setValues(DebugValues::TYPE::ERROR_VEL_FILTERED, error_vel_filtered); - m_debug_values.setValues( - DebugValues::TYPE::ACC_CMD_FB_P_CONTRIBUTION, pid_contributions.at(0)); // P - m_debug_values.setValues( - DebugValues::TYPE::ACC_CMD_FB_I_CONTRIBUTION, pid_contributions.at(1)); // I - m_debug_values.setValues( - DebugValues::TYPE::ACC_CMD_FB_D_CONTRIBUTION, pid_contributions.at(2)); // D - - return feedback_acc; -} - -void LongitudinalController::updatePitchDebugValues( - const float64_t pitch, const float64_t traj_pitch, const float64_t raw_pitch) -{ - using trajectory_follower::DebugValues; - const float64_t to_degrees = (180.0 / static_cast(autoware::common::types::PI)); - m_debug_values.setValues(DebugValues::TYPE::PITCH_LPF_RAD, pitch); - m_debug_values.setValues(DebugValues::TYPE::PITCH_LPF_DEG, pitch * to_degrees); - m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_RAD, raw_pitch); - m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_DEG, raw_pitch * to_degrees); - m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_TRAJ_RAD, traj_pitch); - m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_TRAJ_DEG, traj_pitch * to_degrees); -} - -void LongitudinalController::updateDebugVelAcc( - const Motion & target_motion, const geometry_msgs::msg::Pose & current_pose, - const ControlData & control_data) -{ - using trajectory_follower::DebugValues; - const float64_t current_vel = control_data.current_motion.vel; - const size_t nearest_idx = control_data.nearest_idx; - - const auto interpolated_point = - calcInterpolatedTargetValue(*m_trajectory_ptr, current_pose, nearest_idx); - - m_debug_values.setValues(DebugValues::TYPE::CURRENT_VEL, current_vel); - m_debug_values.setValues(DebugValues::TYPE::TARGET_VEL, target_motion.vel); - m_debug_values.setValues(DebugValues::TYPE::TARGET_ACC, target_motion.acc); - m_debug_values.setValues( - DebugValues::TYPE::NEAREST_VEL, interpolated_point.longitudinal_velocity_mps); - m_debug_values.setValues(DebugValues::TYPE::NEAREST_ACC, interpolated_point.acceleration_mps2); - m_debug_values.setValues(DebugValues::TYPE::ERROR_VEL, target_motion.vel - current_vel); -} - -} // namespace trajectory_follower_nodes -} // namespace control -} // namespace motion -} // namespace autoware - -#include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE( - autoware::motion::control::trajectory_follower_nodes::LongitudinalController)