From e174fd64bdc0f5401e7a32c64ce6dd5ef5a037d3 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Thu, 12 May 2022 22:23:07 +0900 Subject: [PATCH 1/5] feat(trajectory_follower): integrate latlon controller Signed-off-by: kosuke55 --- control/pure_pursuit/CMakeLists.txt | 12 +- .../config/pure_pursuit.param.yaml | 4 +- .../pure_pursuit_lateral_controller.hpp | 130 +++ control/pure_pursuit/package.xml | 1 + .../pure_pursuit_lateral_controller.cpp | 216 +++++ control/trajectory_follower/CMakeLists.txt | 10 + .../trajectory_follower/input_data.hpp | 41 + .../lateral_controller_base.hpp | 59 ++ .../longitudinal_controller_base.hpp | 55 ++ .../mpc_lateral_controller.hpp} | 65 +- .../pid_longitudinal_controller.hpp} | 51 +- .../include/trajectory_follower/sync_data.hpp | 41 + control/trajectory_follower/package.xml | 1 + .../src/mpc_lateral_controller.cpp} | 281 +++--- .../src/pid_longitudinal_controller.cpp} | 364 ++++---- .../trajectory_follower_nodes/CMakeLists.txt | 63 +- .../controller_node.hpp | 115 +++ .../latlon_muxer_node.hpp | 76 -- control/trajectory_follower_nodes/package.xml | 1 + .../lateral_controller_defaults.param.yaml | 7 +- ...ongitudinal_controller_defaults.param.yaml | 4 +- .../src/controller_node.cpp | 172 ++++ .../src/latlon_muxer_node.cpp | 97 -- .../test/test_controller_node.cpp | 881 ++++++++++++++++++ .../lateral_controller.param.yaml | 3 +- .../longitudinal_controller.param.yaml | 4 +- .../launch/control.launch.py | 114 +-- 27 files changed, 2169 insertions(+), 699 deletions(-) create mode 100644 control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp create mode 100644 control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp create mode 100644 control/trajectory_follower/include/trajectory_follower/input_data.hpp create mode 100644 control/trajectory_follower/include/trajectory_follower/lateral_controller_base.hpp create mode 100644 control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp rename control/{trajectory_follower_nodes/include/trajectory_follower_nodes/lateral_controller_node.hpp => trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp} (76%) rename control/{trajectory_follower_nodes/include/trajectory_follower_nodes/longitudinal_controller_node.hpp => trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp} (88%) create mode 100644 control/trajectory_follower/include/trajectory_follower/sync_data.hpp rename control/{trajectory_follower_nodes/src/lateral_controller_node.cpp => trajectory_follower/src/mpc_lateral_controller.cpp} (59%) rename control/{trajectory_follower_nodes/src/longitudinal_controller_node.cpp => trajectory_follower/src/pid_longitudinal_controller.cpp} (68%) create mode 100644 control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp delete mode 100644 control/trajectory_follower_nodes/include/trajectory_follower_nodes/latlon_muxer_node.hpp create mode 100644 control/trajectory_follower_nodes/src/controller_node.cpp delete mode 100644 control/trajectory_follower_nodes/src/latlon_muxer_node.cpp create mode 100644 control/trajectory_follower_nodes/test/test_controller_node.cpp diff --git a/control/pure_pursuit/CMakeLists.txt b/control/pure_pursuit/CMakeLists.txt index fca2fe8d5ba6a..b1c73d5397f07 100644 --- a/control/pure_pursuit/CMakeLists.txt +++ b/control/pure_pursuit/CMakeLists.txt @@ -19,18 +19,18 @@ ament_auto_add_library(pure_pursuit_core SHARED ) # pure_pursuit -ament_auto_add_library(pure_pursuit_node SHARED - src/pure_pursuit/pure_pursuit_node.cpp +ament_auto_add_library(pure_pursuit_lateral_controller SHARED + src/pure_pursuit/pure_pursuit_lateral_controller.cpp src/pure_pursuit/pure_pursuit_viz.cpp ) -target_link_libraries(pure_pursuit_node +target_link_libraries(pure_pursuit_lateral_controller pure_pursuit_core ) -rclcpp_components_register_node(pure_pursuit_node - PLUGIN "pure_pursuit::PurePursuitNode" - EXECUTABLE pure_pursuit_node_exe +rclcpp_components_register_node(pure_pursuit_lateral_controller + PLUGIN "pure_pursuit::PurePursuitLateralController" + EXECUTABLE pure_pursuit_lateral_controller_exe ) ament_auto_package(INSTALL_TO_SHARE diff --git a/control/pure_pursuit/config/pure_pursuit.param.yaml b/control/pure_pursuit/config/pure_pursuit.param.yaml index b52e369b4ca6f..ee3e6b742ff44 100644 --- a/control/pure_pursuit/config/pure_pursuit.param.yaml +++ b/control/pure_pursuit/config/pure_pursuit.param.yaml @@ -1,9 +1,7 @@ /**: ros__parameters: - # -- system -- - control_period: 0.033 - # -- algorithm -- lookahead_distance_ratio: 2.2 min_lookahead_distance: 2.5 reverse_min_lookahead_distance: 7.0 + converged_steer_rad: 0.1 diff --git a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp new file mode 100644 index 0000000000000..3b1aaba0af370 --- /dev/null +++ b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp @@ -0,0 +1,130 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +/* + * Copyright 2015-2019 Autoware Foundation. 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 PURE_PURSUIT__PURE_PURSUIT_LATERAL_CONTROLLER_HPP_ +#define PURE_PURSUIT__PURE_PURSUIT_LATERAL_CONTROLLER_HPP_ + +#include "pure_pursuit/pure_pursuit.hpp" +#include "pure_pursuit/pure_pursuit_viz.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "tier4_autoware_utils/ros/self_pose_listener.hpp" +#include "trajectory_follower/lateral_controller_base.hpp" + +#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" + +#include // To be replaced by std::optional in C++17 + +#include +#include + +using autoware::motion::control::trajectory_follower::InputData; +using autoware::motion::control::trajectory_follower::LateralControllerBase; +using autoware::motion::control::trajectory_follower::LateralOutput; +using autoware_auto_control_msgs::msg::AckermannLateralCommand; + +namespace pure_pursuit +{ +struct Param +{ + // Global Parameters + double wheel_base; + + // Algorithm Parameters + double lookahead_distance_ratio; + double min_lookahead_distance; + double reverse_min_lookahead_distance; // min_lookahead_distance in reverse gear + double converged_steer_rad_; +}; + +struct DebugData +{ + geometry_msgs::msg::Point next_target; +}; + +class PurePursuitLateralController : public LateralControllerBase +{ +public: + explicit PurePursuitLateralController(rclcpp::Node & node); + +private: + rclcpp::Node::SharedPtr node_; + tier4_autoware_utils::SelfPoseListener self_pose_listener_; + + autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr trajectory_; + nav_msgs::msg::Odometry::ConstSharedPtr current_odometry_; + autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr current_steering_; + + bool isDataReady(); + + void onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg); + void onCurrentOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + + // TF + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; + + // Debug Publisher + rclcpp::Publisher::SharedPtr pub_debug_marker_; + + void publishDebugMarker() const; + + /** + * @brief compute control command for path follow with a constant control period + */ + boost::optional run() override; + AckermannLateralCommand generateCtrlCmdMsg(const double target_curvature); + + /** + * @brief set input data + */ + void setInputData(InputData const & input_data) override; + + // Parameter + Param param_; + + // Algorithm + std::unique_ptr pure_pursuit_; + + boost::optional calcTargetCurvature(); + boost::optional calcTargetPoint() const; + + // Debug + mutable DebugData debug_data_; +}; + +} // namespace pure_pursuit + +#endif // PURE_PURSUIT__PURE_PURSUIT_LATERAL_CONTROLLER_HPP_ diff --git a/control/pure_pursuit/package.xml b/control/pure_pursuit/package.xml index fb97ea262004d..b780835bff3df 100644 --- a/control/pure_pursuit/package.xml +++ b/control/pure_pursuit/package.xml @@ -24,6 +24,7 @@ tf2_geometry_msgs tf2_ros tier4_autoware_utils + trajectory_follower vehicle_info_util visualization_msgs diff --git a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp new file mode 100644 index 0000000000000..ced2819aa123c --- /dev/null +++ b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp @@ -0,0 +1,216 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Copyright 2015-2019 Autoware Foundation. 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 "pure_pursuit/pure_pursuit_lateral_controller.hpp" + +#include "pure_pursuit/pure_pursuit_viz.hpp" +#include "pure_pursuit/util/planning_utils.hpp" +#include "pure_pursuit/util/tf_utils.hpp" + +#include + +#include +#include +#include + +namespace +{ +double calcLookaheadDistance( + const double velocity, const double lookahead_distance_ratio, const double min_lookahead_distance) +{ + const double lookahead_distance = lookahead_distance_ratio * std::abs(velocity); + return std::max(lookahead_distance, min_lookahead_distance); +} + +} // namespace + +namespace pure_pursuit +{ +PurePursuitLateralController::PurePursuitLateralController(rclcpp::Node & node) +: node_{&node}, self_pose_listener_(&node), tf_buffer_(node_->get_clock()), tf_listener_(tf_buffer_) +{ + pure_pursuit_ = std::make_unique(); + + // Vehicle Parameters + const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*node_).getVehicleInfo(); + param_.wheel_base = vehicle_info.wheel_base_m; + + // Algorithm Parameters + param_.lookahead_distance_ratio = + node_->declare_parameter("lookahead_distance_ratio", 2.2); + param_.min_lookahead_distance = node_->declare_parameter("min_lookahead_distance", 2.5); + param_.reverse_min_lookahead_distance = + node_->declare_parameter("reverse_min_lookahead_distance", 7.0); + param_.converged_steer_rad_ = node_->declare_parameter("converged_steer_rad", 0.1); + + // Debug Publishers + pub_debug_marker_ = + node_->create_publisher("~/debug/markers", 0); + + // Wait for first current pose + tf_utils::waitForTransform(tf_buffer_, "map", "base_link"); +} + +bool PurePursuitLateralController::isDataReady() +{ + if (!current_odometry_) { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 5000, "waiting for current_odometry..."); + return false; + } + + if (!trajectory_) { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 5000, "waiting for trajectory..."); + return false; + } + + if (!current_pose_) { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 5000, "waiting for current_pose..."); + return false; + } + + return true; +} + +void PurePursuitLateralController::setInputData(InputData const & input_data) +{ + trajectory_ = input_data.current_trajectory_ptr; + current_odometry_ = input_data.current_odometry_ptr; + current_steering_ = input_data.current_steering_ptr; +} + +boost::optional PurePursuitLateralController::run() +{ + current_pose_ = self_pose_listener_.getCurrentPose(); + if (!isDataReady()) { + return boost::none; + } + + const auto target_curvature = calcTargetCurvature(); + AckermannLateralCommand cmd_msg; + if (target_curvature) { + cmd_msg = generateCtrlCmdMsg(*target_curvature); + publishDebugMarker(); + } else { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 5000, "failed to solve pure_pursuit"); + cmd_msg = generateCtrlCmdMsg({0.0}); + } + + LateralOutput output; + output.control_cmd = cmd_msg; + output.sync_data.is_steer_converged = + std::abs(cmd_msg.steering_tire_angle - current_steering_->steering_tire_angle) < + static_cast(param_.converged_steer_rad_); + return output; +} + +AckermannLateralCommand PurePursuitLateralController::generateCtrlCmdMsg( + const double target_curvature) +{ + AckermannLateralCommand cmd; + cmd.stamp = node_->get_clock()->now(); + cmd.steering_tire_angle = + planning_utils::convertCurvatureToSteeringAngle(param_.wheel_base, target_curvature); + // pub_ctrl_cmd_->publish(cmd); + return cmd; +} + +void PurePursuitLateralController::publishDebugMarker() const +{ + visualization_msgs::msg::MarkerArray marker_array; + + marker_array.markers.push_back(createNextTargetMarker(debug_data_.next_target)); + marker_array.markers.push_back( + createTrajectoryCircleMarker(debug_data_.next_target, current_pose_->pose)); + + pub_debug_marker_->publish(marker_array); +} + +boost::optional PurePursuitLateralController::calcTargetCurvature() +{ + // Ignore invalid trajectory + if (trajectory_->points.size() < 3) { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 5000, "received path size is < 3, ignored"); + return {}; + } + + // Calculate target point for velocity/acceleration + const auto target_point = calcTargetPoint(); + if (!target_point) { + return {}; + } + + const double target_vel = target_point->longitudinal_velocity_mps; + + // Calculate lookahead distance + const bool is_reverse = (target_vel < 0); + const double min_lookahead_distance = + is_reverse ? param_.reverse_min_lookahead_distance : param_.min_lookahead_distance; + const double lookahead_distance = calcLookaheadDistance( + current_odometry_->twist.twist.linear.x, param_.lookahead_distance_ratio, + min_lookahead_distance); + + // Set PurePursuit data + pure_pursuit_->setCurrentPose(current_pose_->pose); + pure_pursuit_->setWaypoints(planning_utils::extractPoses(*trajectory_)); + pure_pursuit_->setLookaheadDistance(lookahead_distance); + + // Run PurePursuit + const auto pure_pursuit_result = pure_pursuit_->run(); + if (!pure_pursuit_result.first) { + return {}; + } + + const auto kappa = pure_pursuit_result.second; + + // Set debug data + debug_data_.next_target = pure_pursuit_->getLocationOfNextTarget(); + + return kappa; +} + +boost::optional +PurePursuitLateralController::calcTargetPoint() const +{ + const auto closest_idx_result = planning_utils::findClosestIdxWithDistAngThr( + planning_utils::extractPoses(*trajectory_), current_pose_->pose, 3.0, M_PI_4); + + if (!closest_idx_result.first) { + RCLCPP_ERROR(node_->get_logger(), "cannot find closest waypoint"); + return {}; + } + + return trajectory_->points.at(closest_idx_result.second); +} +} // namespace pure_pursuit diff --git a/control/trajectory_follower/CMakeLists.txt b/control/trajectory_follower/CMakeLists.txt index 67d157df835de..6dd7c7cedc4be 100644 --- a/control/trajectory_follower/CMakeLists.txt +++ b/control/trajectory_follower/CMakeLists.txt @@ -7,6 +7,7 @@ autoware_package() # lateral_controller set(LATERAL_CONTROLLER_LIB lateral_controller_lib) set(LATERAL_CONTROLLER_LIB_SRC + src/mpc_lateral_controller.cpp src/interpolate.cpp src/lowpass_filter.cpp src/mpc.cpp @@ -21,6 +22,10 @@ set(LATERAL_CONTROLLER_LIB_SRC ) set(LATERAL_CONTROLLER_LIB_HEADERS + include/trajectory_follower/lateral_controller_base.hpp + include/trajectory_follower/mpc_lateral_controller.hpp + include/trajectory_follower/sync_data.hpp + include/trajectory_follower/input_data.hpp include/trajectory_follower/visibility_control.hpp include/trajectory_follower/interpolate.hpp include/trajectory_follower/lowpass_filter.hpp @@ -45,12 +50,17 @@ target_compile_options(${LATERAL_CONTROLLER_LIB} PRIVATE -Wno-error=old-style-ca # longitudinal_controller set(LONGITUDINAL_CONTROLLER_LIB longitudinal_controller_lib) set(LONGITUDINAL_CONTROLLER_LIB_SRC + src/pid_longitudinal_controller.cpp src/pid.cpp src/smooth_stop.cpp src/longitudinal_controller_utils.cpp ) set(LONGITUDINAL_CONTROLLER_LIB_HEADERS + include/trajectory_follower/longitudinal_controller_base.hpp + include/trajectory_follower/pid_longitudinal_controller.hpp + include/trajectory_follower/sync_data.hpp + include/trajectory_follower/input_data.hpp include/trajectory_follower/debug_values.hpp include/trajectory_follower/pid.hpp include/trajectory_follower/smooth_stop.hpp diff --git a/control/trajectory_follower/include/trajectory_follower/input_data.hpp b/control/trajectory_follower/include/trajectory_follower/input_data.hpp new file mode 100644 index 0000000000000..505e5e0de2545 --- /dev/null +++ b/control/trajectory_follower/include/trajectory_follower/input_data.hpp @@ -0,0 +1,41 @@ +// Copyright 2022 The Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRAJECTORY_FOLLOWER__INPUT_DATA_HPP_ +#define TRAJECTORY_FOLLOWER__INPUT_DATA_HPP_ + +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" +#include "nav_msgs/msg/odometry.hpp" + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +struct InputData +{ + autoware_auto_planning_msgs::msg::Trajectory::SharedPtr current_trajectory_ptr; + nav_msgs::msg::Odometry::SharedPtr current_odometry_ptr; + autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr current_steering_ptr; +}; +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware + +#endif // TRAJECTORY_FOLLOWER__INPUT_DATA_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/lateral_controller_base.hpp b/control/trajectory_follower/include/trajectory_follower/lateral_controller_base.hpp new file mode 100644 index 0000000000000..0a234fa0b42b1 --- /dev/null +++ b/control/trajectory_follower/include/trajectory_follower/lateral_controller_base.hpp @@ -0,0 +1,59 @@ +// Copyright 2022 The Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRAJECTORY_FOLLOWER__LATERAL_CONTROLLER_BASE_HPP_ +#define TRAJECTORY_FOLLOWER__LATERAL_CONTROLLER_BASE_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "trajectory_follower/input_data.hpp" +#include "trajectory_follower/sync_data.hpp" + +#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" + +#include + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +struct LateralOutput +{ + autoware_auto_control_msgs::msg::AckermannLateralCommand control_cmd; + LateralSyncData sync_data; +}; + +class LateralControllerBase +{ +public: + virtual boost::optional run() = 0; + virtual void setInputData(InputData const & input_data) = 0; + void sync(LongitudinalSyncData const & longitudinal_sync_data) + { + longitudinal_sync_data_ = longitudinal_sync_data; + }; + +protected: + LongitudinalSyncData longitudinal_sync_data_; +}; + +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware + +#endif // TRAJECTORY_FOLLOWER__LATERAL_CONTROLLER_BASE_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp b/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp new file mode 100644 index 0000000000000..6eb4d13119275 --- /dev/null +++ b/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp @@ -0,0 +1,55 @@ +// Copyright 2022 The Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRAJECTORY_FOLLOWER__LONGITUDINAL_CONTROLLER_BASE_HPP_ +#define TRAJECTORY_FOLLOWER__LONGITUDINAL_CONTROLLER_BASE_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "trajectory_follower/input_data.hpp" +#include "trajectory_follower/sync_data.hpp" + +#include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" + +#include + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +struct LongitudinalOutput +{ + autoware_auto_control_msgs::msg::LongitudinalCommand control_cmd; + LongitudinalSyncData sync_data; +}; +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; } + +protected: + LateralSyncData lateral_sync_data_; +}; + +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware + +#endif // TRAJECTORY_FOLLOWER__LONGITUDINAL_CONTROLLER_BASE_HPP_ diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/lateral_controller_node.hpp b/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp similarity index 76% rename from control/trajectory_follower_nodes/include/trajectory_follower_nodes/lateral_controller_node.hpp rename to control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp index 089292d97021a..e8d1c0bbf4bd2 100644 --- a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/lateral_controller_node.hpp +++ b/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp @@ -12,17 +12,17 @@ // 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_ +#ifndef TRAJECTORY_FOLLOWER__MPC_LATERAL_CONTROLLER_HPP_ +#define TRAJECTORY_FOLLOWER__MPC_LATERAL_CONTROLLER_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/lateral_controller_base.hpp" #include "trajectory_follower/lowpass_filter.hpp" #include "trajectory_follower/mpc.hpp" #include "trajectory_follower/mpc_trajectory.hpp" @@ -32,7 +32,7 @@ #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 "trajectory_follower/visibility_control.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" @@ -56,42 +56,33 @@ namespace motion { namespace control { -namespace trajectory_follower_nodes +namespace trajectory_follower { 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 +class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralControllerBase { public: /** * @brief constructor */ - explicit LateralController(const rclcpp::NodeOptions & node_options); + explicit MpcLateralController(rclcpp::Node & node); /** * @brief destructor */ - virtual ~LateralController(); + virtual ~MpcLateralController(); private: - //!< @brief topic publisher for control command - rclcpp::Publisher::SharedPtr - m_pub_ctrl_cmd; + rclcpp::Node * node_; + //!< @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; @@ -112,6 +103,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC LateralController : public rclcpp::Node /* parameters for stop state */ float64_t m_stop_state_entry_ego_speed; float64_t m_stop_state_entry_target_speed; + float64_t m_converged_steer_rad; + bool m_check_steer_converged_for_stopped_state; // MPC object trajectory_follower::MPC m_mpc; @@ -140,14 +133,19 @@ class TRAJECTORY_FOLLOWER_PUBLIC LateralController : public rclcpp::Node //!< 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 + * @brief compute control command for path follow with a constant control period */ - void onTimer(); + boost::optional run() override; + + /** + * @brief set input data like current odometry, trajectory and steering. + */ + void setInputData(InputData const & input_data) override; /** * @brief set m_current_trajectory with received message */ - void onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr); + void setTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr); /** * @brief update current_pose from tf @@ -161,20 +159,11 @@ class TRAJECTORY_FOLLOWER_PUBLIC LateralController : public rclcpp::Node 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 + * @brief create control command + * @param [in] ctrl_cmd published control command */ - void publishCtrlCmd(autoware_auto_control_msgs::msg::AckermannLateralCommand cmd); + autoware_auto_control_msgs::msg::AckermannLateralCommand createCtrlCmdMsg( + autoware_auto_control_msgs::msg::AckermannLateralCommand ctrl_cmd); /** * @brief publish predicted future trajectory @@ -209,7 +198,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC LateralController : public rclcpp::Node */ bool8_t isValidTrajectory(const autoware_auto_planning_msgs::msg::Trajectory & traj) const; - OnSetParametersCallbackHandle::SharedPtr m_set_param_res; + rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr m_set_param_res; /** * @brief Declare MPC parameters as ROS parameters to allow tuning on the fly @@ -222,9 +211,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC LateralController : public rclcpp::Node rcl_interfaces::msg::SetParametersResult paramCallback( const std::vector & parameters); }; -} // namespace trajectory_follower_nodes +} // namespace trajectory_follower } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER_NODES__LATERAL_CONTROLLER_NODE_HPP_ +#endif // TRAJECTORY_FOLLOWER__MPC_LATERAL_CONTROLLER_HPP_ diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/longitudinal_controller_node.hpp b/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp similarity index 88% rename from control/trajectory_follower_nodes/include/trajectory_follower_nodes/longitudinal_controller_node.hpp rename to control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp index 4ee28a192b028..2c9723fb7f2c9 100644 --- a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/longitudinal_controller_node.hpp +++ b/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp @@ -12,8 +12,8 @@ // 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_ +#ifndef TRAJECTORY_FOLLOWER__PID_LONGITUDINAL_CONTROLLER_HPP_ +#define TRAJECTORY_FOLLOWER__PID_LONGITUDINAL_CONTROLLER_HPP_ #include "eigen3/Eigen/Core" #include "eigen3/Eigen/Geometry" @@ -24,6 +24,7 @@ #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" #include "trajectory_follower/debug_values.hpp" +#include "trajectory_follower/longitudinal_controller_base.hpp" #include "trajectory_follower/longitudinal_controller_utils.hpp" #include "trajectory_follower/lowpass_filter.hpp" #include "trajectory_follower/pid.hpp" @@ -38,6 +39,7 @@ #include "nav_msgs/msg/odometry.hpp" #include "tf2_msgs/msg/tf_message.hpp" +#include #include #include #include @@ -49,19 +51,19 @@ namespace motion { namespace control { -namespace trajectory_follower_nodes +namespace trajectory_follower { 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 +/// \class PidLongitudinalController /// \brief The node class used for generating longitudinal control commands (velocity/acceleration) -class TRAJECTORY_FOLLOWER_PUBLIC LongitudinalController : public rclcpp::Node +class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public LongitudinalControllerBase { public: - explicit LongitudinalController(const rclcpp::NodeOptions & node_options); + explicit PidLongitudinalController(rclcpp::Node & node); private: struct Motion @@ -82,24 +84,19 @@ class TRAJECTORY_FOLLOWER_PUBLIC LongitudinalController : public rclcpp::Node float64_t slope_angle{0.0}; float64_t dt{0.0}; }; - + rclcpp::Node * node_; // ros variables - rclcpp::Subscription::SharedPtr m_sub_current_velocity; - rclcpp::Subscription::SharedPtr m_sub_trajectory; - rclcpp::Publisher::SharedPtr - m_pub_control_cmd; rclcpp::Publisher::SharedPtr 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; + rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr m_set_param_res; rcl_interfaces::msg::SetParametersResult paramCallback( const std::vector & parameters); @@ -126,6 +123,10 @@ class TRAJECTORY_FOLLOWER_PUBLIC LongitudinalController : public rclcpp::Node bool8_t m_enable_overshoot_emergency; bool8_t m_enable_slope_compensation; bool8_t m_enable_large_tracking_error_emergency; + bool8_t m_enable_keep_stopped_until_steer_convergence; + + // trajectory beffer for detecting new trajectory + std::deque m_trajectory_buffer; // smooth stop transition struct StateTransitionParams @@ -139,6 +140,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC LongitudinalController : public rclcpp::Node float64_t stopped_state_entry_duration_time; float64_t stopped_state_entry_vel; float64_t stopped_state_entry_acc; + float64_t stopped_state_new_traj_duration_time; + float64_t stopped_state_new_traj_end_dist; // emergency float64_t emergency_state_overshoot_stop_dist; float64_t emergency_state_traj_trans_dev; @@ -208,24 +211,29 @@ class TRAJECTORY_FOLLOWER_PUBLIC LongitudinalController : public rclcpp::Node // debug values trajectory_follower::DebugValues m_debug_values; - std::shared_ptr m_last_running_time{std::make_shared(this->now())}; + std::shared_ptr m_last_running_time{std::make_shared(node_->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); + void setCurrentVelocity(const nav_msgs::msg::Odometry::ConstSharedPtr msg); /** * @brief set reference trajectory with received message * @param [in] msg trajectory message */ - void callbackTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg); + void setTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg); /** * @brief compute control command, and publish periodically */ - void callbackTimerControl(); + boost::optional run() override; + + /** + * @brief set input data like current odometry and trajectory. + */ + void setInputData(InputData const & input_data) override; /** * @brief calculate data for controllers whose type is ControlData @@ -239,6 +247,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC LongitudinalController : public rclcpp::Node */ Motion calcEmergencyCtrlCmd(const float64_t dt) const; + bool isNewTrajectory(); + /** * @brief update control state according to the current situation * @param [in] current_control_state current control state @@ -262,7 +272,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC LongitudinalController : public rclcpp::Node * @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); + autoware_auto_control_msgs::msg::LongitudinalCommand createCtrlCmdMsg( + const Motion & ctrl_cmd, const float64_t & current_vel); /** * @brief publish debug data @@ -366,9 +377,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC LongitudinalController : public rclcpp::Node const Motion & ctrl_cmd, const geometry_msgs::msg::Pose & current_pose, const ControlData & control_data); }; -} // namespace trajectory_follower_nodes +} // namespace trajectory_follower } // namespace control } // namespace motion } // namespace autoware -#endif // TRAJECTORY_FOLLOWER_NODES__LONGITUDINAL_CONTROLLER_NODE_HPP_ +#endif // TRAJECTORY_FOLLOWER__PID_LONGITUDINAL_CONTROLLER_HPP_ diff --git a/control/trajectory_follower/include/trajectory_follower/sync_data.hpp b/control/trajectory_follower/include/trajectory_follower/sync_data.hpp new file mode 100644 index 0000000000000..48224a5498a9c --- /dev/null +++ b/control/trajectory_follower/include/trajectory_follower/sync_data.hpp @@ -0,0 +1,41 @@ +// Copyright 2022 The Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRAJECTORY_FOLLOWER__SYNC_DATA_HPP_ +#define TRAJECTORY_FOLLOWER__SYNC_DATA_HPP_ + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower +{ +struct LateralSyncData +{ + bool is_steer_converged{false}; +}; + +struct LongitudinalSyncData +{ + bool is_velocity_converged{false}; +}; + +} // namespace trajectory_follower +} // namespace control +} // namespace motion +} // namespace autoware + +#endif // TRAJECTORY_FOLLOWER__SYNC_DATA_HPP_ diff --git a/control/trajectory_follower/package.xml b/control/trajectory_follower/package.xml index f43ffdb07ed20..974c066d7b81c 100644 --- a/control/trajectory_follower/package.xml +++ b/control/trajectory_follower/package.xml @@ -28,6 +28,7 @@ tf2 tf2_ros tier4_autoware_utils + vehicle_info_util ament_cmake_ros ament_lint_auto diff --git a/control/trajectory_follower_nodes/src/lateral_controller_node.cpp b/control/trajectory_follower/src/mpc_lateral_controller.cpp similarity index 59% rename from control/trajectory_follower_nodes/src/lateral_controller_node.cpp rename to control/trajectory_follower/src/mpc_lateral_controller.cpp index 4b8e609429da9..059024ff047ae 100644 --- a/control/trajectory_follower_nodes/src/lateral_controller_node.cpp +++ b/control/trajectory_follower/src/mpc_lateral_controller.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower_nodes/lateral_controller_node.hpp" +#include "trajectory_follower/mpc_lateral_controller.hpp" #include "tf2_ros/create_timer_ros.h" @@ -30,7 +30,7 @@ namespace motion { namespace control { -namespace trajectory_follower_nodes +namespace trajectory_follower { namespace { @@ -49,38 +49,45 @@ void update_param( } } // namespace -LateralController::LateralController(const rclcpp::NodeOptions & node_options) -: Node("lateral_controller", node_options) +MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} { 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_mpc.m_ctrl_period = node_->get_parameter("ctrl_period").as_double(); + m_enable_path_smoothing = node_->declare_parameter("enable_path_smoothing"); + m_path_filter_moving_ave_num = node_->declare_parameter("path_filter_moving_ave_num"); + m_curvature_smoothing_num_traj = + node_->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"); + node_->declare_parameter("curvature_smoothing_num_ref_steer"); + m_traj_resample_dist = node_->declare_parameter("traj_resample_dist"); + m_mpc.m_admissible_position_error = + node_->declare_parameter("admissible_position_error"); + m_mpc.m_admissible_yaw_error_rad = + node_->declare_parameter("admissible_yaw_error_rad"); + m_mpc.m_use_steer_prediction = node_->declare_parameter("use_steer_prediction"); + m_mpc.m_param.steer_tau = node_->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"); + m_stop_state_entry_ego_speed = node_->declare_parameter("stop_state_entry_ego_speed"); + m_stop_state_entry_target_speed = + node_->declare_parameter("stop_state_entry_target_speed"); + m_converged_steer_rad = node_->declare_parameter("converged_steer_rad"); + m_check_steer_converged_for_stopped_state = + node_->declare_parameter("check_steer_converged_for_stopped_state"); /* 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"); + const float64_t steer_lim_deg = node_->declare_parameter("steer_lim_deg"); + const float64_t steer_rate_lim_dps = node_->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_info_util::VehicleInfoUtil(*node_).getVehicleInfo().wheel_base_m; /* vehicle model setup */ - const std::string vehicle_model_type = declare_parameter("vehicle_model_type"); + const std::string vehicle_model_type = + node_->declare_parameter("vehicle_model_type"); std::shared_ptr vehicle_model_ptr; if (vehicle_model_type == "kinematics") { vehicle_model_ptr = std::make_shared( @@ -89,35 +96,35 @@ LateralController::LateralController(const rclcpp::NodeOptions & node_options) 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"); + const float64_t mass_fl = node_->declare_parameter("vehicle.mass_fl"); + const float64_t mass_fr = node_->declare_parameter("vehicle.mass_fr"); + const float64_t mass_rl = node_->declare_parameter("vehicle.mass_rl"); + const float64_t mass_rr = node_->declare_parameter("vehicle.mass_rr"); + const float64_t cf = node_->declare_parameter("vehicle.cf"); + const float64_t cr = node_->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"); + RCLCPP_ERROR(node_->get_logger(), "vehicle_model_type is undefined"); } /* QP solver setup */ - const std::string qp_solver_type = declare_parameter("qp_solver_type"); + const std::string qp_solver_type = node_->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()); + qpsolver_ptr = std::make_shared(node_->get_logger()); } else { - RCLCPP_ERROR(get_logger(), "qp_solver_type is undefined"); + RCLCPP_ERROR(node_->get_logger(), "qp_solver_type is undefined"); } /* delay compensation */ { - const float64_t delay_tmp = declare_parameter("input_delay"); + const float64_t delay_tmp = node_->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); @@ -125,55 +132,39 @@ LateralController::LateralController(const rclcpp::NodeOptions & node_options) /* initialize lowpass filter */ { - const float64_t steering_lpf_cutoff_hz = declare_parameter("steering_lpf_cutoff_hz"); + const float64_t steering_lpf_cutoff_hz = + node_->declare_parameter("steering_lpf_cutoff_hz"); const float64_t error_deriv_lpf_cutoff_hz = - declare_parameter("error_deriv_lpf_cutoff_hz"); + node_->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( + m_pub_predicted_traj = node_->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)); + m_pub_diagnostic = + node_->create_publisher( + "~/output/lateral_diagnostic", 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_set_param_res = node_->add_on_set_parameters_callback( + std::bind(&MpcLateralController::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()); + m_mpc.setLogger(node_->get_logger()); + m_mpc.setClock(node_->get_clock()); } -LateralController::~LateralController() -{ - autoware_auto_control_msgs::msg::AckermannLateralCommand stop_cmd = getStopControlCommand(); - publishCtrlCmd(stop_cmd); -} +MpcLateralController::~MpcLateralController() {} -void LateralController::onTimer() +boost::optional MpcLateralController::run() { if (!checkData() || !updateCurrentPose()) { - return; + return boost::none; } autoware_auto_control_msgs::msg::AckermannLateralCommand ctrl_cmd; @@ -197,72 +188,96 @@ void LateralController::onTimer() // 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); + const auto cmd_msg = createCtrlCmdMsg(m_ctrl_cmd_prev); publishPredictedTraj(predicted_traj); publishDiagnostic(diagnostic); - return; + LateralOutput output; + output.control_cmd = cmd_msg; + output.sync_data.is_steer_converged = + std::abs(cmd_msg.steering_tire_angle - m_current_steering_ptr->steering_tire_angle) < + static_cast(m_converged_steer_rad); + return boost::optional(output); } if (!is_mpc_solved) { RCLCPP_WARN_SKIPFIRST_THROTTLE( - get_logger(), *get_clock(), 5000 /*ms*/, "MPC is not solved. publish 0 velocity."); + node_->get_logger(), *node_->get_clock(), 5000 /*ms*/, + "MPC is not solved. publish 0 velocity."); ctrl_cmd = getStopControlCommand(); } m_ctrl_cmd_prev = ctrl_cmd; - publishCtrlCmd(ctrl_cmd); + const auto cmd_msg = createCtrlCmdMsg(ctrl_cmd); publishPredictedTraj(predicted_traj); publishDiagnostic(diagnostic); + LateralOutput output; + output.control_cmd = cmd_msg; + output.sync_data.is_steer_converged = + std::abs(cmd_msg.steering_tire_angle - m_current_steering_ptr->steering_tire_angle) < + static_cast(m_converged_steer_rad); + + return boost::optional(output); } -bool8_t LateralController::checkData() const +void MpcLateralController::setInputData(InputData const & input_data) +{ + setTrajectory(input_data.current_trajectory_ptr); + m_current_odometry_ptr = input_data.current_odometry_ptr; + m_current_steering_ptr = input_data.current_steering_ptr; +} + +bool8_t MpcLateralController::checkData() const { if (!m_mpc.hasVehicleModel()) { - RCLCPP_DEBUG(get_logger(), "MPC does not have a vehicle model"); + RCLCPP_DEBUG(node_->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"); + RCLCPP_DEBUG(node_->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); + node_->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); + node_->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."); + RCLCPP_DEBUG(node_->get_logger(), "trajectory size is zero."); return false; } return true; } -void LateralController::onTrajectory( +void MpcLateralController::setTrajectory( const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) { + if (!msg) return; + m_current_trajectory_ptr = msg; if (!m_current_pose_ptr && !updateCurrentPose()) { - RCLCPP_DEBUG(get_logger(), "Current pose is not received yet."); + RCLCPP_DEBUG(node_->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."); + RCLCPP_DEBUG(node_->get_logger(), "received path size is < 3, not enough."); return; } if (!isValidTrajectory(*msg)) { - RCLCPP_ERROR(get_logger(), "Trajectory is invalid!! stop computing."); + RCLCPP_ERROR(node_->get_logger(), "Trajectory is invalid!! stop computing."); return; } @@ -271,16 +286,18 @@ void LateralController::onTrajectory( m_curvature_smoothing_num_traj, m_curvature_smoothing_num_ref_steer, m_current_pose_ptr); } -bool8_t LateralController::updateCurrentPose() +bool8_t MpcLateralController::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()); + node_->get_logger(), *node_->get_clock(), 5000 /*ms*/, ex.what()); + RCLCPP_WARN_SKIPFIRST_THROTTLE( + node_->get_logger(), *node_->get_clock(), 5000 /*ms*/, + m_tf_buffer.allFramesAsString().c_str()); return false; } @@ -294,19 +311,8 @@ bool8_t LateralController::updateCurrentPose() 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 +MpcLateralController::getStopControlCommand() const { autoware_auto_control_msgs::msg::AckermannLateralCommand cmd; cmd.steering_tire_angle = static_cast(m_steer_cmd_prev); @@ -315,7 +321,7 @@ autoware_auto_control_msgs::msg::AckermannLateralCommand LateralController::getS } autoware_auto_control_msgs::msg::AckermannLateralCommand -LateralController::getInitialControlCommand() const +MpcLateralController::getInitialControlCommand() const { autoware_auto_control_msgs::msg::AckermannLateralCommand cmd; cmd.steering_tire_angle = m_current_steering_ptr->steering_tire_angle; @@ -323,7 +329,7 @@ LateralController::getInitialControlCommand() const return cmd; } -bool8_t LateralController::isStoppedState() const +bool8_t MpcLateralController::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 @@ -342,87 +348,85 @@ bool8_t LateralController::isStoppedState() const 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) { + std::fabs(target_vel) < m_stop_state_entry_target_speed && + (!m_check_steer_converged_for_stopped_state || + std::abs(m_ctrl_cmd_prev.steering_tire_angle - m_current_steering_ptr->steering_tire_angle) < + static_cast(m_converged_steer_rad))) { return true; } else { return false; } } -void LateralController::publishCtrlCmd( +autoware_auto_control_msgs::msg::AckermannLateralCommand MpcLateralController::createCtrlCmdMsg( autoware_auto_control_msgs::msg::AckermannLateralCommand ctrl_cmd) { - ctrl_cmd.stamp = this->now(); - m_pub_ctrl_cmd->publish(ctrl_cmd); + ctrl_cmd.stamp = node_->now(); m_steer_cmd_prev = ctrl_cmd.steering_tire_angle; + return ctrl_cmd; } -void LateralController::publishPredictedTraj( +void MpcLateralController::publishPredictedTraj( autoware_auto_planning_msgs::msg::Trajectory & predicted_traj) const { - predicted_traj.header.stamp = this->now(); + predicted_traj.header.stamp = node_->now(); predicted_traj.header.frame_id = m_current_trajectory_ptr->header.frame_id; m_pub_predicted_traj->publish(predicted_traj); } -void LateralController::publishDiagnostic( +void MpcLateralController::publishDiagnostic( autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic & diagnostic) const { - diagnostic.diag_header.data_stamp = this->now(); + diagnostic.diag_header.data_stamp = node_->now(); diagnostic.diag_header.name = std::string("linear-MPC lateral controller"); m_pub_diagnostic->publish(diagnostic); } -void LateralController::initTimer(float64_t period_s) +void MpcLateralController::declareMPCparameters() { - 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.prediction_horizon = node_->declare_parameter("mpc_prediction_horizon"); + m_mpc.m_param.prediction_dt = node_->declare_parameter("mpc_prediction_dt"); + m_mpc.m_param.weight_lat_error = node_->declare_parameter("mpc_weight_lat_error"); + m_mpc.m_param.weight_heading_error = + node_->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"); + node_->declare_parameter("mpc_weight_heading_error_squared_vel"); + m_mpc.m_param.weight_steering_input = + node_->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"); + node_->declare_parameter("mpc_weight_steering_input_squared_vel"); + m_mpc.m_param.weight_lat_jerk = node_->declare_parameter("mpc_weight_lat_jerk"); + m_mpc.m_param.weight_steer_rate = node_->declare_parameter("mpc_weight_steer_rate"); + m_mpc.m_param.weight_steer_acc = node_->declare_parameter("mpc_weight_steer_acc"); m_mpc.m_param.low_curvature_weight_lat_error = - declare_parameter("mpc_low_curvature_weight_lat_error"); + node_->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"); + node_->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"); + node_->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"); + node_->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"); + node_->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"); + node_->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"); + node_->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"); + node_->declare_parameter("mpc_low_curvature_weight_steer_acc"); m_mpc.m_param.low_curvature_thresh_curvature = - declare_parameter("mpc_low_curvature_thresh_curvature"); + node_->declare_parameter("mpc_low_curvature_thresh_curvature"); m_mpc.m_param.weight_terminal_lat_error = - declare_parameter("mpc_weight_terminal_lat_error"); + node_->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"); + node_->declare_parameter("mpc_weight_terminal_heading_error"); + m_mpc.m_param.zero_ff_steer_deg = node_->declare_parameter("mpc_zero_ff_steer_deg"); + m_mpc.m_param.acceleration_limit = node_->declare_parameter("mpc_acceleration_limit"); + m_mpc.m_param.velocity_time_constant = + node_->declare_parameter("mpc_velocity_time_constant"); } -rcl_interfaces::msg::SetParametersResult LateralController::paramCallback( +rcl_interfaces::msg::SetParametersResult MpcLateralController::paramCallback( const std::vector & parameters) { rcl_interfaces::msg::SetParametersResult result; @@ -492,7 +496,7 @@ rcl_interfaces::msg::SetParametersResult LateralController::paramCallback( return result; } -bool8_t LateralController::isValidTrajectory( +bool8_t MpcLateralController::isValidTrajectory( const autoware_auto_planning_msgs::msg::Trajectory & traj) const { for (const auto & p : traj.points) { @@ -509,10 +513,7 @@ bool8_t LateralController::isValidTrajectory( return true; } -} // namespace trajectory_follower_nodes +} // namespace trajectory_follower } // 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/longitudinal_controller_node.cpp b/control/trajectory_follower/src/pid_longitudinal_controller.cpp similarity index 68% rename from control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp rename to control/trajectory_follower/src/pid_longitudinal_controller.cpp index 525f9b8c79c76..7bc301cc166a4 100644 --- a/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp +++ b/control/trajectory_follower/src/pid_longitudinal_controller.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower_nodes/longitudinal_controller_node.hpp" +#include "trajectory_follower/pid_longitudinal_controller.hpp" #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 @@ -31,108 +32,118 @@ namespace motion { namespace control { -namespace trajectory_follower_nodes +namespace trajectory_follower { -LongitudinalController::LongitudinalController(const rclcpp::NodeOptions & node_options) -: Node("longitudinal_controller", node_options) +PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) : node_{&node} { using std::placeholders::_1; // parameters timer - m_longitudinal_ctrl_period = declare_parameter("longitudinal_ctrl_period"); + m_longitudinal_ctrl_period = node_->get_parameter("ctrl_period").as_double(); - m_wheel_base = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo().wheel_base_m; + m_wheel_base = vehicle_info_util::VehicleInfoUtil(*node_).getVehicleInfo().wheel_base_m; // parameters for delay compensation - m_delay_compensation_time = declare_parameter("delay_compensation_time"); // [s] + m_delay_compensation_time = + node_->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_smooth_stop = node_->declare_parameter("enable_smooth_stop"); + m_enable_overshoot_emergency = node_->declare_parameter("enable_overshoot_emergency"); m_enable_large_tracking_error_emergency = - declare_parameter("enable_large_tracking_error_emergency"); - m_enable_slope_compensation = declare_parameter("enable_slope_compensation"); + node_->declare_parameter("enable_large_tracking_error_emergency"); + m_enable_slope_compensation = node_->declare_parameter("enable_slope_compensation"); + m_enable_keep_stopped_until_steer_convergence = + node_->declare_parameter("enable_keep_stopped_until_steer_convergence"); // 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_stop_dist = node_->declare_parameter("drive_state_stop_dist"); // [m] p.drive_state_offset_stop_dist = - declare_parameter("drive_state_offset_stop_dist"); // [m] + node_->declare_parameter("drive_state_offset_stop_dist"); // [m] // stopping - p.stopping_state_stop_dist = declare_parameter("stopping_state_stop_dist"); // [m] + p.stopping_state_stop_dist = + node_->declare_parameter("stopping_state_stop_dist"); // [m] p.stopped_state_entry_duration_time = - declare_parameter("stopped_state_entry_duration_time"); // [s] + node_->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²] + p.stopped_state_entry_vel = + node_->declare_parameter("stopped_state_entry_vel"); // [m/s] + p.stopped_state_entry_acc = + node_->declare_parameter("stopped_state_entry_acc"); // [m/s²] + p.stopped_state_new_traj_duration_time = + node_->declare_parameter("stopped_state_new_traj_duration_time"); // [s] + p.stopped_state_new_traj_end_dist = + node_->declare_parameter("stopped_state_new_traj_end_dist"); // [m] // emergency p.emergency_state_overshoot_stop_dist = - declare_parameter("emergency_state_overshoot_stop_dist"); // [m] + node_->declare_parameter("emergency_state_overshoot_stop_dist"); // [m] p.emergency_state_traj_trans_dev = - declare_parameter("emergency_state_traj_trans_dev"); // [m] + node_->declare_parameter("emergency_state_traj_trans_dev"); // [m] p.emergency_state_traj_rot_dev = - declare_parameter("emergency_state_traj_rot_dev"); // [m] + node_->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")}; + const float64_t kp{node_->declare_parameter("kp")}; + const float64_t ki{node_->declare_parameter("ki")}; + const float64_t kd{node_->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] + const float64_t max_pid{node_->declare_parameter("max_out")}; // [m/s^2] + const float64_t min_pid{node_->declare_parameter("min_out")}; // [m/s^2] + const float64_t max_p{node_->declare_parameter("max_p_effort")}; // [m/s^2] + const float64_t min_p{node_->declare_parameter("min_p_effort")}; // [m/s^2] + const float64_t max_i{node_->declare_parameter("max_i_effort")}; // [m/s^2] + const float64_t min_i{node_->declare_parameter("min_i_effort")}; // [m/s^2] + const float64_t max_d{node_->declare_parameter("max_d_effort")}; // [m/s^2] + const float64_t min_d{node_->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")}; + const float64_t lpf_vel_error_gain{node_->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] + node_->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] + node_->declare_parameter("enable_brake_keeping_before_stop"); // [-] + m_brake_keeping_acc = node_->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] + node_->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] + node_->declare_parameter("smooth_stop_min_strong_acc")}; // [m/s^2] + const float64_t weak_acc{ + node_->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] + node_->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] + node_->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] + node_->declare_parameter("smooth_stop_max_fast_vel")}; // [m/s] const float64_t min_running_vel{ - declare_parameter("smooth_stop_min_running_vel")}; // [m/s] + node_->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] + node_->declare_parameter("smooth_stop_min_running_acc")}; // [m/s^2] const float64_t weak_stop_time{ - declare_parameter("smooth_stop_weak_stop_time")}; // [s] + node_->declare_parameter("smooth_stop_weak_stop_time")}; // [s] const float64_t weak_stop_dist{ - declare_parameter("smooth_stop_weak_stop_dist")}; // [m] + node_->declare_parameter("smooth_stop_weak_stop_dist")}; // [m] const float64_t strong_stop_dist{ - declare_parameter("smooth_stop_strong_stop_dist")}; // [m] + node_->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, @@ -142,91 +153,87 @@ LongitudinalController::LongitudinalController(const rclcpp::NodeOptions & node_ // 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] + p.vel = node_->declare_parameter("stopped_vel"); // [m/s] + p.acc = node_->declare_parameter("stopped_acc"); // [m/s^2] + p.jerk = node_->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] + p.vel = node_->declare_parameter("emergency_vel"); // [m/s] + p.acc = node_->declare_parameter("emergency_acc"); // [m/s^2] + p.jerk = node_->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] + m_max_acc = node_->declare_parameter("max_acc"); // [m/s^2] + m_min_acc = node_->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] + m_max_jerk = node_->declare_parameter("max_jerk"); // [m/s^3] + m_min_jerk = node_->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_use_traj_for_pitch = node_->declare_parameter("use_trajectory_for_pitch_calculation"); + const float64_t lpf_pitch_gain{node_->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] + m_max_pitch_rad = node_->declare_parameter("max_pitch_rad"); // [rad] + m_min_pitch_rad = node_->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)); - } + m_pub_slope = + node_->create_publisher( + "~/output/slope_angle", rclcpp::QoS{1}); + m_pub_debug = + node_->create_publisher( + "~/output/longitudinal_diagnostic", rclcpp::QoS{1}); // set parameter callback - m_set_param_res = this->add_on_set_parameters_callback( - std::bind(&LongitudinalController::paramCallback, this, _1)); + m_set_param_res = node_->add_on_set_parameters_callback( + std::bind(&PidLongitudinalController::paramCallback, this, _1)); // set lowpass filter for acc m_lpf_acc = std::make_shared(0.0, 0.2); } +void PidLongitudinalController::setInputData(InputData const & input_data) +{ + setTrajectory(input_data.current_trajectory_ptr); + setCurrentVelocity(input_data.current_odometry_ptr); +} -void LongitudinalController::callbackCurrentVelocity( +void PidLongitudinalController::setCurrentVelocity( const nav_msgs::msg::Odometry::ConstSharedPtr msg) { + if (!msg) return; + if (m_current_velocity_ptr) { m_prev_velocity_ptr = m_current_velocity_ptr; } m_current_velocity_ptr = std::make_shared(*msg); } -void LongitudinalController::callbackTrajectory( +void PidLongitudinalController::setTrajectory( const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg) { + if (!msg) return; + if (!trajectory_follower::longitudinal_utils::isValidTrajectory(*msg)) { - RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 3000, "received invalid trajectory. ignore."); + RCLCPP_ERROR_THROTTLE( + node_->get_logger(), *node_->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."); + node_->get_logger(), *node_->get_clock(), 3000, "Unexpected trajectory size < 2. Ignored."); return; } m_trajectory_ptr = std::make_shared(*msg); } -rcl_interfaces::msg::SetParametersResult LongitudinalController::paramCallback( +rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallback( const std::vector & parameters) { auto update_param = [&](const std::string & name, float64_t & v) { @@ -251,6 +258,8 @@ rcl_interfaces::msg::SetParametersResult LongitudinalController::paramCallback( 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("stopped_state_new_traj_duration_time", p.stopped_state_new_traj_duration_time); + update_param("stopped_state_new_traj_end_dist", p.stopped_state_new_traj_end_dist); 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); @@ -258,22 +267,22 @@ rcl_interfaces::msg::SetParametersResult LongitudinalController::paramCallback( // 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()}; + float64_t kp{node_->get_parameter("kp").as_double()}; + float64_t ki{node_->get_parameter("ki").as_double()}; + float64_t kd{node_->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()}; + float64_t max_pid{node_->get_parameter("max_out").as_double()}; + float64_t min_pid{node_->get_parameter("min_out").as_double()}; + float64_t max_p{node_->get_parameter("max_p_effort").as_double()}; + float64_t min_p{node_->get_parameter("min_p_effort").as_double()}; + float64_t max_i{node_->get_parameter("max_i_effort").as_double()}; + float64_t min_i{node_->get_parameter("min_i_effort").as_double()}; + float64_t max_d{node_->get_parameter("max_d_effort").as_double()}; + float64_t min_d{node_->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); @@ -289,17 +298,17 @@ rcl_interfaces::msg::SetParametersResult LongitudinalController::paramCallback( // 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()}; + float64_t max_strong_acc{node_->get_parameter("smooth_stop_max_strong_acc").as_double()}; + float64_t min_strong_acc{node_->get_parameter("smooth_stop_min_strong_acc").as_double()}; + float64_t weak_acc{node_->get_parameter("smooth_stop_weak_acc").as_double()}; + float64_t weak_stop_acc{node_->get_parameter("smooth_stop_weak_stop_acc").as_double()}; + float64_t strong_stop_acc{node_->get_parameter("smooth_stop_strong_stop_acc").as_double()}; + float64_t max_fast_vel{node_->get_parameter("smooth_stop_max_fast_vel").as_double()}; + float64_t min_running_vel{node_->get_parameter("smooth_stop_min_running_vel").as_double()}; + float64_t min_running_acc{node_->get_parameter("smooth_stop_min_running_acc").as_double()}; + float64_t weak_stop_time{node_->get_parameter("smooth_stop_weak_stop_time").as_double()}; + float64_t weak_stop_dist{node_->get_parameter("smooth_stop_weak_stop_dist").as_double()}; + float64_t strong_stop_dist{node_->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); @@ -349,13 +358,13 @@ rcl_interfaces::msg::SetParametersResult LongitudinalController::paramCallback( return result; } -void LongitudinalController::callbackTimerControl() +boost::optional PidLongitudinalController::run() { // 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; + return boost::none; } // get current ego pose @@ -378,9 +387,12 @@ void LongitudinalController::callbackTimerControl() } 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; + const auto cmd_msg = + createCtrlCmdMsg(raw_ctrl_cmd, control_data.current_motion.vel); // create control command + publishDebugData(raw_ctrl_cmd, control_data); // publish debug data + LongitudinalOutput output; + output.control_cmd = cmd_msg; + return output; } // update control state @@ -390,13 +402,17 @@ void LongitudinalController::callbackTimerControl() const Motion ctrl_cmd = calcCtrlCmd(m_control_state, current_pose, control_data); // publish control command - publishCtrlCmd(ctrl_cmd, control_data.current_motion.vel); + const auto cmd_msg = createCtrlCmdMsg(ctrl_cmd, control_data.current_motion.vel); + LongitudinalOutput output; + output.control_cmd = cmd_msg; // publish debug data publishDebugData(ctrl_cmd, control_data); + + return output; } -LongitudinalController::ControlData LongitudinalController::getControlData( +PidLongitudinalController::ControlData PidLongitudinalController::getControlData( const geometry_msgs::msg::Pose & current_pose) { ControlData control_data{}; @@ -442,7 +458,7 @@ LongitudinalController::ControlData LongitudinalController::getControlData( return control_data; } -LongitudinalController::Motion LongitudinalController::calcEmergencyCtrlCmd( +PidLongitudinalController::Motion PidLongitudinalController::calcEmergencyCtrlCmd( const float64_t dt) const { // These accelerations are without slope compensation @@ -452,14 +468,40 @@ LongitudinalController::Motion LongitudinalController::calcEmergencyCtrlCmd( 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); + node_->get_logger(), *node_->get_clock(), 3000, "[Emergency stop] vel: %3.3f, acc: %3.3f", vel, + acc); return Motion{vel, acc}; } -LongitudinalController::ControlState LongitudinalController::updateControlState( +bool PidLongitudinalController::isNewTrajectory() +{ + // flags for state transition + const auto & p = m_state_transition_params; + + m_trajectory_buffer.push_back(*m_trajectory_ptr); + while (true) { + const auto time_diff = rclcpp::Time(m_trajectory_buffer.back().header.stamp) - + rclcpp::Time(m_trajectory_buffer.front().header.stamp); + if (time_diff.seconds() < p.stopped_state_new_traj_duration_time) { + break; + } + m_trajectory_buffer.pop_front(); + } + + for (const auto & trajectory : m_trajectory_buffer) { + if ( + tier4_autoware_utils::calcDistance2d( + trajectory.points.back().pose, m_trajectory_ptr->points.back().pose) > + p.stopped_state_new_traj_end_dist) { + return true; + } + } + return false; +} + +PidLongitudinalController::ControlState PidLongitudinalController::updateControlState( const ControlState current_control_state, const ControlData & control_data) { const float64_t current_vel = control_data.current_motion.vel; @@ -472,16 +514,18 @@ LongitudinalController::ControlState LongitudinalController::updateControlState( 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 keep_stopped_condition = + !lateral_sync_data_.is_steer_converged || isNewTrajectory(); 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()); + m_last_running_time = std::make_shared(node_->now()); } const bool8_t stopped_condition = m_last_running_time - ? (this->now() - *m_last_running_time).seconds() > p.stopped_state_entry_duration_time + ? (node_->now() - *m_last_running_time).seconds() > p.stopped_state_entry_duration_time : false; const bool8_t emergency_condition = @@ -526,7 +570,9 @@ LongitudinalController::ControlState LongitudinalController::updateControlState( return ControlState::DRIVE; } } else if (current_control_state == ControlState::STOPPED) { - if (departure_condition_from_stopped) { + if ( + departure_condition_from_stopped && + (!m_enable_keep_stopped_until_steer_convergence || !keep_stopped_condition)) { m_pid_vel.reset(); m_lpf_vel_error->reset(0.0); // prevent the car from taking a long time to start to move @@ -542,7 +588,7 @@ LongitudinalController::ControlState LongitudinalController::updateControlState( return current_control_state; } -LongitudinalController::Motion LongitudinalController::calcCtrlCmd( +PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( const ControlState & current_control_state, const geometry_msgs::msg::Pose & current_pose, const ControlData & control_data) { @@ -572,7 +618,7 @@ LongitudinalController::Motion LongitudinalController::calcCtrlCmd( 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(), + node_->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, @@ -583,8 +629,8 @@ LongitudinalController::Motion LongitudinalController::calcCtrlCmd( 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); + node_->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; @@ -593,7 +639,7 @@ LongitudinalController::Motion LongitudinalController::calcCtrlCmd( 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); + node_->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); } @@ -612,25 +658,27 @@ LongitudinalController::Motion LongitudinalController::calcCtrlCmd( } // Do not use nearest_idx here -void LongitudinalController::publishCtrlCmd(const Motion & ctrl_cmd, float64_t current_vel) +autoware_auto_control_msgs::msg::LongitudinalCommand PidLongitudinalController::createCtrlCmdMsg( + const Motion & ctrl_cmd, const float64_t & current_vel) { // publish control command autoware_auto_control_msgs::msg::LongitudinalCommand cmd{}; - cmd.stamp = this->now(); + 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({this->now(), current_vel}); + m_vel_hist.push_back({node_->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; + + return cmd; } -void LongitudinalController::publishDebugData( +void PidLongitudinalController::publishDebugData( const Motion & ctrl_cmd, const ControlData & control_data) { using trajectory_follower::DebugValues; @@ -645,7 +693,7 @@ void LongitudinalController::publishDebugData( // publish debug values autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic debug_msg{}; - debug_msg.diag_header.data_stamp = this->now(); + debug_msg.diag_header.data_stamp = node_->now(); for (const auto & v : m_debug_values.getValues()) { debug_msg.diag_array.data.push_back( static_cast(v)); @@ -654,28 +702,28 @@ void LongitudinalController::publishDebugData( // slope angle autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic slope_msg{}; - slope_msg.diag_header.data_stamp = this->now(); + slope_msg.diag_header.data_stamp = node_->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 PidLongitudinalController::getDt() { float64_t dt; if (!m_prev_control_time) { dt = m_longitudinal_ctrl_period; - m_prev_control_time = std::make_shared(this->now()); + m_prev_control_time = std::make_shared(node_->now()); } else { - dt = (this->now() - *m_prev_control_time).seconds(); - *m_prev_control_time = this->now(); + dt = (node_->now() - *m_prev_control_time).seconds(); + *m_prev_control_time = node_->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 +PidLongitudinalController::Motion PidLongitudinalController::getCurrentMotion() const { const float64_t dv = m_current_velocity_ptr->twist.twist.linear.x - m_prev_velocity_ptr->twist.twist.linear.x; @@ -692,7 +740,7 @@ LongitudinalController::Motion LongitudinalController::getCurrentMotion() const return Motion{current_vel, current_acc}; } -enum LongitudinalController::Shift LongitudinalController::getCurrentShift( +enum PidLongitudinalController::Shift PidLongitudinalController::getCurrentShift( const size_t nearest_idx) const { constexpr float64_t epsilon = 1e-5; @@ -708,7 +756,7 @@ enum LongitudinalController::Shift LongitudinalController::getCurrentShift( return m_prev_shift; } -float64_t LongitudinalController::calcFilteredAcc( +float64_t PidLongitudinalController::calcFilteredAcc( const float64_t raw_acc, const ControlData & control_data) { using trajectory_follower::DebugValues; @@ -730,12 +778,12 @@ float64_t LongitudinalController::calcFilteredAcc( return acc_jerk_filtered; } -void LongitudinalController::storeAccelCmd(const float64_t accel) +void PidLongitudinalController::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.stamp = node_->now(); cmd.acceleration = static_cast(accel); // store published ctrl cmd @@ -749,12 +797,12 @@ void LongitudinalController::storeAccelCmd(const float64_t accel) if (m_ctrl_cmd_vec.size() <= 2) { return; } - if ((this->now() - m_ctrl_cmd_vec.at(1).stamp).seconds() > m_delay_compensation_time) { + if ((node_->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( +float64_t PidLongitudinalController::applySlopeCompensation( const float64_t input_acc, const float64_t pitch, const Shift shift) const { if (!m_enable_slope_compensation) { @@ -768,7 +816,7 @@ float64_t LongitudinalController::applySlopeCompensation( return compensated_acc; } -LongitudinalController::Motion LongitudinalController::keepBrakeBeforeStop( +PidLongitudinalController::Motion PidLongitudinalController::keepBrakeBeforeStop( const autoware_auto_planning_msgs::msg::Trajectory & traj, const Motion & target_motion, const size_t nearest_idx) const { @@ -803,7 +851,7 @@ LongitudinalController::Motion LongitudinalController::keepBrakeBeforeStop( } autoware_auto_planning_msgs::msg::TrajectoryPoint -LongitudinalController::calcInterpolatedTargetValue( +PidLongitudinalController::calcInterpolatedTargetValue( const autoware_auto_planning_msgs::msg::Trajectory & traj, const geometry_msgs::msg::Pose & pose, const size_t nearest_idx) const { @@ -831,7 +879,7 @@ LongitudinalController::calcInterpolatedTargetValue( m_state_transition_params.emergency_state_traj_rot_dev); } -float64_t LongitudinalController::predictedVelocityInTargetPoint( +float64_t PidLongitudinalController::predictedVelocityInTargetPoint( const Motion current_motion, const float64_t delay_compensation_time) const { const float64_t current_vel = current_motion.vel; @@ -851,9 +899,9 @@ float64_t LongitudinalController::predictedVelocityInTargetPoint( float64_t pred_vel = current_vel_abs; const auto past_delay_time = - this->now() - rclcpp::Duration::from_seconds(delay_compensation_time); + node_->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 ((node_->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) * @@ -872,14 +920,14 @@ float64_t LongitudinalController::predictedVelocityInTargetPoint( 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(); + (node_->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( +float64_t PidLongitudinalController::applyVelocityFeedback( const Motion target_motion, const float64_t dt, const float64_t current_vel) { using trajectory_follower::DebugValues; @@ -905,7 +953,7 @@ float64_t LongitudinalController::applyVelocityFeedback( return feedback_acc; } -void LongitudinalController::updatePitchDebugValues( +void PidLongitudinalController::updatePitchDebugValues( const float64_t pitch, const float64_t traj_pitch, const float64_t raw_pitch) { using trajectory_follower::DebugValues; @@ -918,7 +966,7 @@ void LongitudinalController::updatePitchDebugValues( m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_TRAJ_DEG, traj_pitch * to_degrees); } -void LongitudinalController::updateDebugVelAcc( +void PidLongitudinalController::updateDebugVelAcc( const Motion & target_motion, const geometry_msgs::msg::Pose & current_pose, const ControlData & control_data) { @@ -938,11 +986,7 @@ void LongitudinalController::updateDebugVelAcc( m_debug_values.setValues(DebugValues::TYPE::ERROR_VEL, target_motion.vel - current_vel); } -} // namespace trajectory_follower_nodes +} // namespace trajectory_follower } // namespace control } // namespace motion } // namespace autoware - -#include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE( - autoware::motion::control::trajectory_follower_nodes::LongitudinalController) diff --git a/control/trajectory_follower_nodes/CMakeLists.txt b/control/trajectory_follower_nodes/CMakeLists.txt index fa005c1c0bfda..c6431f1d2504c 100644 --- a/control/trajectory_follower_nodes/CMakeLists.txt +++ b/control/trajectory_follower_nodes/CMakeLists.txt @@ -4,68 +4,31 @@ 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 +set(CONTROLLER_NODE controller_node) +ament_auto_add_library(${CONTROLLER_NODE} SHARED + include/trajectory_follower_nodes/controller_node.hpp + src/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 +target_compile_options(${CONTROLLER_NODE} PRIVATE -Wno-error=old-style-cast) +rclcpp_components_register_node(${CONTROLLER_NODE} + PLUGIN "autoware::motion::control::trajectory_follower_nodes::Controller" + EXECUTABLE ${CONTROLLER_NODE}_exe ) 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_lateral_controller_node.cpp - test/test_longitudinal_controller_node.cpp + test/test_controller_node.cpp ) ament_target_dependencies(${TRAJECTORY_FOLLOWER_NODES_TEST} fake_test_node) - target_link_libraries(${TRAJECTORY_FOLLOWER_NODES_TEST} ${LATLON_MUXER_NODE} ${LATERAL_CONTROLLER_NODE} ${LONGITUDINAL_CONTROLLER_NODE}) + target_link_libraries( + ${TRAJECTORY_FOLLOWER_NODES_TEST} ${CONTROLLER_NODE}) find_package(autoware_testing REQUIRED) - 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" + 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" ) endif() 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 new file mode 100644 index 0000000000000..91b99fc7837c9 --- /dev/null +++ b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp @@ -0,0 +1,115 @@ +// 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__CONTROLLER_NODE_HPP_ +#define TRAJECTORY_FOLLOWER_NODES__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/lateral_controller_base.hpp" +#include "trajectory_follower/longitudinal_controller_base.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/ackermann_control_command.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 +{ +using trajectory_follower::LateralOutput; +using trajectory_follower::LongitudinalOutput; +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; + +/// \classController +/// \brief The node class used for generating longitudinal control commands (velocity/acceleration) +class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node +{ +public: + explicit Controller(const rclcpp::NodeOptions & node_options); + virtual ~Controller() {} + +private: + rclcpp::TimerBase::SharedPtr timer_control_; + trajectory_follower::InputData input_data_; + double timeout_thr_sec_; + boost::optional longitudinal_output_{boost::none}; + boost::optional lateral_output_{boost::none}; + + std::shared_ptr longitudinal_controller_; + std::shared_ptr lateral_controller_; + + rclcpp::Subscription::SharedPtr sub_ref_path_; + rclcpp::Subscription::SharedPtr sub_odometry_; + rclcpp::Subscription::SharedPtr sub_steering_; + rclcpp::Publisher::SharedPtr + control_cmd_pub_; + + enum class LateralControllerMode { + INVALID = 0, + MPC = 1, + PURE_PURSUIT = 2, + }; + enum class LongitudinalControllerMode { + INVALID = 0, + PID = 1, + }; + + /** + * @brief compute control command, and publish periodically + */ + void callbackTimerControl(); + void onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr); + void onOdometry(const nav_msgs::msg::Odometry::SharedPtr msg); + void onSteering(const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg); + bool isTimeOut(); + LateralControllerMode getLateralControllerMode(const std::string & algorithm_name) const; + LongitudinalControllerMode getLongitudinalControllerMode( + const std::string & algorithm_name) const; +}; +} // namespace trajectory_follower_nodes +} // namespace control +} // namespace motion +} // namespace autoware + +#endif // TRAJECTORY_FOLLOWER_NODES__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/package.xml b/control/trajectory_follower_nodes/package.xml index 0db906f84049c..7c9ffde6b898c 100644 --- a/control/trajectory_follower_nodes/package.xml +++ b/control/trajectory_follower_nodes/package.xml @@ -15,6 +15,7 @@ autoware_auto_planning_msgs autoware_auto_system_msgs autoware_auto_vehicle_msgs + pure_pursuit rclcpp rclcpp_components trajectory_follower diff --git a/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml b/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml index e692fa8e14811..0a5279faf7054 100644 --- a/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml +++ b/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml @@ -1,7 +1,6 @@ /**: ros__parameters: # -- system -- - ctrl_period: 0.03 # control period [s] traj_resample_dist: 0.1 # path resampling interval [m] use_steer_prediction: false # flag for using steer prediction (do not use steer measurement) admissible_position_error: 5.0 # stop mpc calculation when error is larger than the following value @@ -43,8 +42,8 @@ # -- vehicle model -- vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics input_delay: 0.24 # steering input delay time for delay compensation - vehicle_model_steer_tau: 0.1 # steering dynamics time constant (1d approximation) [s] - steer_lim_deg: 20.0 # steering angle limit [deg] + vehicle_model_steer_tau: 0.3 # steering dynamics time constant (1d approximation) [s] + steer_lim_deg: 40.0 # steering angle limit [deg] steer_rate_lim_dps: 600.0 # steering angle rate limit [deg/s] acceleration_limit: 2.0 # acceleration limit for trajectory velocity modification [m/ss] velocity_time_constant: 0.3 # velocity dynamics time constant for trajectory velocity modification [s] @@ -56,6 +55,8 @@ # stop state: steering command is kept in the previous value in the stop state. stop_state_entry_ego_speed: 0.001 stop_state_entry_target_speed: 0.001 + converged_steer_rad: 0.1 + check_steer_converged_for_stopped_state: true # vehicle parameters mass_kg: 2400.0 diff --git a/control/trajectory_follower_nodes/param/longitudinal_controller_defaults.param.yaml b/control/trajectory_follower_nodes/param/longitudinal_controller_defaults.param.yaml index eb1b78224774f..79da58c9b1f35 100644 --- a/control/trajectory_follower_nodes/param/longitudinal_controller_defaults.param.yaml +++ b/control/trajectory_follower_nodes/param/longitudinal_controller_defaults.param.yaml @@ -1,12 +1,12 @@ /**: ros__parameters: - longitudinal_ctrl_period: 0.03 # control period [s] delay_compensation_time: 0.17 enable_smooth_stop: true enable_overshoot_emergency: true enable_large_tracking_error_emergency: true enable_slope_compensation: false + enable_keep_stopped_until_steer_convergence: true # state transition drive_state_stop_dist: 0.5 @@ -15,6 +15,8 @@ stopped_state_entry_duration_time: 0.1 stopped_state_entry_vel: 0.1 stopped_state_entry_acc: 0.1 + stopped_state_new_traj_duration_time: 1.0 + stopped_state_new_traj_end_dist: 0.3 emergency_state_overshoot_stop_dist: 1.5 emergency_state_traj_trans_dev: 3.0 emergency_state_traj_rot_dev: 0.7 diff --git a/control/trajectory_follower_nodes/src/controller_node.cpp b/control/trajectory_follower_nodes/src/controller_node.cpp new file mode 100644 index 0000000000000..94c05c04b1a78 --- /dev/null +++ b/control/trajectory_follower_nodes/src/controller_node.cpp @@ -0,0 +1,172 @@ +// 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/controller_node.hpp" + +#include "motion_common/motion_common.hpp" +#include "motion_common/trajectory_common.hpp" +#include "pure_pursuit/pure_pursuit_lateral_controller.hpp" +#include "time_utils/time_utils.hpp" +#include "trajectory_follower/mpc_lateral_controller.hpp" +#include "trajectory_follower/pid_longitudinal_controller.hpp" + +#include +#include +#include +#include +#include +#include + +namespace autoware +{ +namespace motion +{ +namespace control +{ +namespace trajectory_follower_nodes +{ +Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("controller", node_options) +{ + using std::placeholders::_1; + + const double ctrl_period = declare_parameter("ctrl_period", 0.03); + timeout_thr_sec_ = declare_parameter("timeout_thr_sec", 0.5); + + const auto lateral_controller_mode = + getLateralControllerMode(declare_parameter("lateral_controller_mode", "mpc_follower")); + switch (lateral_controller_mode) { + case LateralControllerMode::MPC: { + lateral_controller_ = std::make_shared(*this); + break; + } + case LateralControllerMode::PURE_PURSUIT: { + lateral_controller_ = std::make_shared(*this); + break; + } + default: + throw std::domain_error("[LateralController] invalid algorithm"); + } + + const auto longitudinal_controller_mode = + getLongitudinalControllerMode(declare_parameter("longitudinal_controller_mode", "pid")); + switch (longitudinal_controller_mode) { + case LongitudinalControllerMode::PID: { + longitudinal_controller_ = + std::make_shared(*this); + break; + } + default: + throw std::domain_error("[LongitudinalController] invalid algorithm"); + } + + sub_ref_path_ = create_subscription( + "~/input/reference_trajectory", rclcpp::QoS{1}, std::bind(&Controller::onTrajectory, this, _1)); + sub_steering_ = create_subscription( + "~/input/current_steering", rclcpp::QoS{1}, std::bind(&Controller::onSteering, this, _1)); + sub_odometry_ = create_subscription( + "~/input/current_odometry", rclcpp::QoS{1}, std::bind(&Controller::onOdometry, this, _1)); + control_cmd_pub_ = create_publisher( + "~/output/control_cmd", rclcpp::QoS{1}.transient_local()); + + // Timer + { + const auto period_ns = std::chrono::duration_cast( + std::chrono::duration(ctrl_period)); + timer_control_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&Controller::callbackTimerControl, this)); + } +} + +Controller::LateralControllerMode Controller::getLateralControllerMode( + const std::string & controller_mode) const +{ + if (controller_mode == "mpc_follower") return LateralControllerMode::MPC; + if (controller_mode == "pure_pursuit") return LateralControllerMode::PURE_PURSUIT; + + throw std::domain_error("[LateralController] undesired algorithm is selected."); + return LateralControllerMode::INVALID; +} + +Controller::LongitudinalControllerMode Controller::getLongitudinalControllerMode( + const std::string & controller_mode) const +{ + if (controller_mode == "pid") return LongitudinalControllerMode::PID; + + throw std::domain_error("[LongitudinalController] undesired algorithm is selected."); + return LongitudinalControllerMode::INVALID; +} + +void Controller::onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) +{ + input_data_.current_trajectory_ptr = msg; +} + +void Controller::onOdometry(const nav_msgs::msg::Odometry::SharedPtr msg) +{ + input_data_.current_odometry_ptr = msg; +} + +void Controller::onSteering(const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg) +{ + input_data_.current_steering_ptr = msg; +} + +bool Controller::isTimeOut() +{ + if (!longitudinal_output_ || !lateral_output_) return true; + + const auto now = this->now(); + if ((now - lateral_output_->control_cmd.stamp).seconds() > timeout_thr_sec_) { + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 1000 /*ms*/, + "Lateral control command too old, control_cmd will not be published."); + return true; + } + if ((now - longitudinal_output_->control_cmd.stamp).seconds() > timeout_thr_sec_) { + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 1000 /*ms*/, + "Longitudinal control command too old, control_cmd will not be published."); + return true; + } + return false; +} + +void Controller::callbackTimerControl() +{ + longitudinal_controller_->setInputData(input_data_); // trajectory, odometry + lateral_controller_->setInputData(input_data_); // trajectory, odometry, steering + + const auto lon_out = longitudinal_controller_->run(); + longitudinal_output_ = lon_out ? lon_out : longitudinal_output_; + const auto lat_out = lateral_controller_->run(); + lateral_output_ = lat_out ? lat_out : lateral_output_; + + if (lateral_output_) longitudinal_controller_->sync(lateral_output_->sync_data); + if (longitudinal_output_) lateral_controller_->sync(longitudinal_output_->sync_data); + if (isTimeOut()) return; + + autoware_auto_control_msgs::msg::AckermannControlCommand out; + out.stamp = this->now(); + out.lateral = lateral_output_->control_cmd; + out.longitudinal = longitudinal_output_->control_cmd; + control_cmd_pub_->publish(out); +} + +} // 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::Controller) 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/test/test_controller_node.cpp b/control/trajectory_follower_nodes/test/test_controller_node.cpp new file mode 100644 index 0000000000000..ddc4fb1fe817f --- /dev/null +++ b/control/trajectory_follower_nodes/test/test_controller_node.cpp @@ -0,0 +1,881 @@ +// 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 "ament_index_cpp/get_package_share_directory.hpp" +#include "fake_test_node/fake_test_node.hpp" +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/time.hpp" +#include "trajectory_follower_nodes/controller_node.hpp" +#include "trajectory_follower_test_utils.hpp" + +#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.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 "geometry_msgs/msg/transform_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" + +#include +#include + +using Controller = autoware::motion::control::trajectory_follower_nodes::Controller; +using AckermannControlCommand = autoware_auto_control_msgs::msg::AckermannControlCommand; +using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; +using TrajectoryPoint = autoware_auto_planning_msgs::msg::TrajectoryPoint; +using VehicleOdometry = nav_msgs::msg::Odometry; +using SteeringReport = autoware_auto_vehicle_msgs::msg::SteeringReport; + +using FakeNodeFixture = autoware::tools::testing::FakeTestNode; + +const rclcpp::Duration one_second(1, 0); + +std::shared_ptr makeNode() +{ + // Pass default parameter file to the node + const auto share_dir = ament_index_cpp::get_package_share_directory("trajectory_follower_nodes"); + rclcpp::NodeOptions node_options; + node_options.append_parameter_override("ctrl_period", 0.03); + node_options.append_parameter_override("timeout_thr_sec", 0.5); + node_options.append_parameter_override( + "enable_keep_stopped_until_steer_convergence", false); // longitudinal + node_options.arguments( + {"--ros-args", "--params-file", share_dir + "/param/lateral_controller_defaults.param.yaml", + "--params-file", share_dir + "/param/longitudinal_controller_defaults.param.yaml", + "--params-file", share_dir + "/param/test_vehicle_info.param.yaml"}); + std::shared_ptr node = std::make_shared(node_options); + + // Enable all logging in the node + auto ret = + rcutils_logging_set_logger_level(node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); + if (ret != RCUTILS_RET_OK) { + std::cout << "Failed to set logging severerity to DEBUG\n"; + } + return node; +} + +TEST_F(FakeNodeFixture, no_input) +{ + // Data to test + AckermannControlCommand::SharedPtr cmd_msg; + bool received_control_command = false; + // Node + std::shared_ptr node = makeNode(); + // Publisher/Subscribers + rclcpp::Publisher::SharedPtr traj_pub = + this->create_publisher("controller/input/reference_trajectory"); + rclcpp::Publisher::SharedPtr odom_pub = + this->create_publisher("controller/input/current_odometry"); + rclcpp::Publisher::SharedPtr steer_pub = + this->create_publisher("controller/input/current_steering"); + rclcpp::Subscription::SharedPtr cmd_sub = + this->create_subscription( + "controller/output/control_cmd", *this->get_fake_node(), + [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { + cmd_msg = msg; + received_control_command = true; + }); + std::shared_ptr br = + std::make_shared(this->get_fake_node()); + + // No published data: expect a stopped command + test_utils::waitForMessage( + node, this, received_control_command, std::chrono::seconds{1LL}, false); + ASSERT_FALSE(received_control_command); +} + +TEST_F(FakeNodeFixture, empty_trajectory) +{ + // Data to test + AckermannControlCommand::SharedPtr cmd_msg; + bool received_control_command = false; + // Node + std::shared_ptr node = makeNode(); + // Publisher/Subscribers + rclcpp::Publisher::SharedPtr traj_pub = + this->create_publisher("controller/input/reference_trajectory"); + rclcpp::Publisher::SharedPtr odom_pub = + this->create_publisher("controller/input/current_odometry"); + rclcpp::Publisher::SharedPtr steer_pub = + this->create_publisher("controller/input/current_steering"); + rclcpp::Subscription::SharedPtr cmd_sub = + this->create_subscription( + "controller/output/control_cmd", *this->get_fake_node(), + [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { + cmd_msg = msg; + received_control_command = true; + }); + std::shared_ptr br = + std::make_shared(this->get_fake_node()); + + // Dummy transform: ego is at (0.0, 0.0) in map frame + geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); + transform.header.stamp = node->now(); + br->sendTransform(transform); + + // Spin for transform to be published + test_utils::spinWhile(node); + + // Empty trajectory: expect a stopped command + Trajectory traj_msg; + traj_msg.header.stamp = node->now(); + traj_msg.header.frame_id = "map"; + VehicleOdometry odom_msg; + SteeringReport steer_msg; + traj_msg.header.stamp = node->now(); + odom_msg.header.stamp = node->now(); + odom_msg.twist.twist.linear.x = 0.0; + steer_msg.stamp = node->now(); + steer_msg.steering_tire_angle = 0.0; + traj_pub->publish(traj_msg); + odom_pub->publish(odom_msg); + steer_pub->publish(steer_msg); + + test_utils::waitForMessage( + node, this, received_control_command, std::chrono::seconds{1LL}, false); + ASSERT_FALSE(received_control_command); +} + +// lateral +TEST_F(FakeNodeFixture, straight_trajectory) +{ + // Data to test + AckermannControlCommand::SharedPtr cmd_msg; + bool received_control_command = false; + // Node + std::shared_ptr node = makeNode(); + // Publisher/Subscribers + rclcpp::Publisher::SharedPtr traj_pub = + this->create_publisher("controller/input/reference_trajectory"); + rclcpp::Publisher::SharedPtr odom_pub = + this->create_publisher("controller/input/current_odometry"); + rclcpp::Publisher::SharedPtr steer_pub = + this->create_publisher("controller/input/current_steering"); + rclcpp::Subscription::SharedPtr cmd_sub = + this->create_subscription( + "controller/output/control_cmd", *this->get_fake_node(), + [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { + cmd_msg = msg; + received_control_command = true; + }); + std::shared_ptr br = + std::make_shared(this->get_fake_node()); + + // Dummy transform: ego is at (0.0, 0.0) in map frame + geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); + transform.header.stamp = node->now(); + br->sendTransform(transform); + + // Spin for transform to be published + test_utils::spinWhile(node); + + // Straight trajectory: expect no steering + received_control_command = false; + Trajectory traj_msg; + traj_msg.header.stamp = node->now(); + traj_msg.header.frame_id = "map"; + VehicleOdometry odom_msg; + SteeringReport steer_msg; + TrajectoryPoint p; + traj_msg.header.stamp = node->now(); + p.pose.position.x = -1.0; + p.pose.position.y = 0.0; + p.longitudinal_velocity_mps = 1.0f; + traj_msg.points.push_back(p); + p.pose.position.x = 0.0; + p.pose.position.y = 0.0; + p.longitudinal_velocity_mps = 1.0f; + traj_msg.points.push_back(p); + p.pose.position.x = 1.0; + p.pose.position.y = 0.0; + p.longitudinal_velocity_mps = 1.0f; + traj_msg.points.push_back(p); + p.pose.position.x = 2.0; + p.pose.position.y = 0.0; + p.longitudinal_velocity_mps = 1.0f; + traj_msg.points.push_back(p); + traj_pub->publish(traj_msg); + odom_msg.header.stamp = node->now(); + odom_msg.twist.twist.linear.x = 1.0; + steer_msg.stamp = node->now(); + steer_msg.steering_tire_angle = 0.0; + odom_pub->publish(odom_msg); + steer_pub->publish(steer_msg); + + test_utils::waitForMessage(node, this, received_control_command); + ASSERT_TRUE(received_control_command); + EXPECT_EQ(cmd_msg->lateral.steering_tire_angle, 0.0f); + EXPECT_EQ(cmd_msg->lateral.steering_tire_rotation_rate, 0.0f); + EXPECT_GT(rclcpp::Time(cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); +} + +TEST_F(FakeNodeFixture, right_turn) +{ + // Data to test + AckermannControlCommand::SharedPtr cmd_msg; + bool received_control_command = false; + // Node + std::shared_ptr node = makeNode(); + // Publisher/Subscribers + rclcpp::Publisher::SharedPtr traj_pub = + this->create_publisher("controller/input/reference_trajectory"); + rclcpp::Publisher::SharedPtr odom_pub = + this->create_publisher("controller/input/current_odometry"); + rclcpp::Publisher::SharedPtr steer_pub = + this->create_publisher("controller/input/current_steering"); + rclcpp::Subscription::SharedPtr cmd_sub = + this->create_subscription( + "controller/output/control_cmd", *this->get_fake_node(), + [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { + cmd_msg = msg; + received_control_command = true; + }); + std::shared_ptr br = + std::make_shared(this->get_fake_node()); + + // Dummy transform: ego is at (0.0, 0.0) in map frame + geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); + transform.header.stamp = node->now(); + br->sendTransform(transform); + + // Spin for transform to be published + test_utils::spinWhile(node); + + // Right turning trajectory: expect right steering + received_control_command = false; + Trajectory traj_msg; + traj_msg.header.stamp = node->now(); + traj_msg.header.frame_id = "map"; + VehicleOdometry odom_msg; + SteeringReport steer_msg; + TrajectoryPoint p; + traj_msg.points.clear(); + p.pose.position.x = -1.0; + p.pose.position.y = -1.0; + p.longitudinal_velocity_mps = 1.0f; + traj_msg.points.push_back(p); + p.pose.position.x = 0.0; + p.pose.position.y = 0.0; + p.longitudinal_velocity_mps = 1.0f; + traj_msg.points.push_back(p); + p.pose.position.x = 1.0; + p.pose.position.y = -1.0; + p.longitudinal_velocity_mps = 1.0f; + traj_msg.points.push_back(p); + p.pose.position.x = 2.0; + p.pose.position.y = -2.0; + p.longitudinal_velocity_mps = 1.0f; + traj_msg.points.push_back(p); + traj_pub->publish(traj_msg); + odom_msg.header.stamp = node->now(); + odom_msg.twist.twist.linear.x = 1.0; + steer_msg.stamp = node->now(); + steer_msg.steering_tire_angle = 0.0; + odom_pub->publish(odom_msg); + steer_pub->publish(steer_msg); + + test_utils::waitForMessage(node, this, received_control_command); + ASSERT_TRUE(received_control_command); + EXPECT_LT(cmd_msg->lateral.steering_tire_angle, 0.0f); + EXPECT_LT(cmd_msg->lateral.steering_tire_rotation_rate, 0.0f); + EXPECT_GT(rclcpp::Time(cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); +} + +TEST_F(FakeNodeFixture, left_turn) +{ + // Data to test + AckermannControlCommand::SharedPtr cmd_msg; + bool received_control_command = false; + // Node + std::shared_ptr node = makeNode(); + // Publisher/Subscribers + rclcpp::Publisher::SharedPtr traj_pub = + this->create_publisher("controller/input/reference_trajectory"); + rclcpp::Publisher::SharedPtr odom_pub = + this->create_publisher("controller/input/current_odometry"); + rclcpp::Publisher::SharedPtr steer_pub = + this->create_publisher("controller/input/current_steering"); + rclcpp::Subscription::SharedPtr cmd_sub = + this->create_subscription( + "controller/output/control_cmd", *this->get_fake_node(), + [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { + cmd_msg = msg; + received_control_command = true; + }); + std::shared_ptr br = + std::make_shared(this->get_fake_node()); + + // Dummy transform: ego is at (0.0, 0.0) in map frame + geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); + transform.header.stamp = node->now(); + br->sendTransform(transform); + + // Spin for transform to be published + test_utils::spinWhile(node); + + // Left turning trajectory: expect left steering + received_control_command = false; + Trajectory traj_msg; + traj_msg.header.stamp = node->now(); + traj_msg.header.frame_id = "map"; + VehicleOdometry odom_msg; + SteeringReport steer_msg; + TrajectoryPoint p; + traj_msg.points.clear(); + p.pose.position.x = -1.0; + p.pose.position.y = 1.0; + p.longitudinal_velocity_mps = 1.0f; + traj_msg.points.push_back(p); + p.pose.position.x = 0.0; + p.pose.position.y = 0.0; + p.longitudinal_velocity_mps = 1.0f; + traj_msg.points.push_back(p); + p.pose.position.x = 1.0; + p.pose.position.y = 1.0; + p.longitudinal_velocity_mps = 1.0f; + traj_msg.points.push_back(p); + p.pose.position.x = 2.0; + p.pose.position.y = 2.0; + p.longitudinal_velocity_mps = 1.0f; + traj_msg.points.push_back(p); + traj_pub->publish(traj_msg); + odom_msg.header.stamp = node->now(); + odom_msg.twist.twist.linear.x = 1.0; + steer_msg.stamp = node->now(); + steer_msg.steering_tire_angle = 0.0; + odom_pub->publish(odom_msg); + steer_pub->publish(steer_msg); + + test_utils::waitForMessage(node, this, received_control_command); + ASSERT_TRUE(received_control_command); + EXPECT_GT(cmd_msg->lateral.steering_tire_angle, 0.0f); + EXPECT_GT(cmd_msg->lateral.steering_tire_rotation_rate, 0.0f); + EXPECT_GT(rclcpp::Time(cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); +} + +TEST_F(FakeNodeFixture, stopped) +{ + // Data to test + AckermannControlCommand::SharedPtr cmd_msg; + bool received_control_command = false; + // Node + std::shared_ptr node = makeNode(); + // Publisher/Subscribers + rclcpp::Publisher::SharedPtr traj_pub = + this->create_publisher("controller/input/reference_trajectory"); + rclcpp::Publisher::SharedPtr odom_pub = + this->create_publisher("controller/input/current_odometry"); + rclcpp::Publisher::SharedPtr steer_pub = + this->create_publisher("controller/input/current_steering"); + rclcpp::Subscription::SharedPtr cmd_sub = + this->create_subscription( + "controller/output/control_cmd", *this->get_fake_node(), + [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { + cmd_msg = msg; + received_control_command = true; + }); + std::shared_ptr br = + std::make_shared(this->get_fake_node()); + + // Dummy transform: ego is at (0.0, 0.0) in map frame + geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); + transform.header.stamp = node->now(); + br->sendTransform(transform); + + // Spin for transform to be published + test_utils::spinWhile(node); + + // Straight trajectory: expect no steering + received_control_command = false; + Trajectory traj_msg; + traj_msg.header.stamp = node->now(); + traj_msg.header.frame_id = "map"; + VehicleOdometry odom_msg; + SteeringReport steer_msg; + TrajectoryPoint p; + traj_msg.header.stamp = node->now(); + p.pose.position.x = -1.0; + p.pose.position.y = 0.0; + // Set a 0 current velocity and 0 target velocity -> stopped state + p.longitudinal_velocity_mps = 0.0f; + traj_msg.points.push_back(p); + p.pose.position.x = 0.0; + p.pose.position.y = 0.0; + p.longitudinal_velocity_mps = 0.0f; + traj_msg.points.push_back(p); + p.pose.position.x = 1.0; + p.pose.position.y = 0.0; + p.longitudinal_velocity_mps = 0.0f; + traj_msg.points.push_back(p); + p.pose.position.x = 2.0; + p.pose.position.y = 0.0; + p.longitudinal_velocity_mps = 0.0f; + traj_msg.points.push_back(p); + traj_pub->publish(traj_msg); + odom_msg.header.stamp = node->now(); + odom_msg.twist.twist.linear.x = 0.0; + steer_msg.stamp = node->now(); + steer_msg.steering_tire_angle = -0.5; + odom_pub->publish(odom_msg); + steer_pub->publish(steer_msg); + + test_utils::waitForMessage(node, this, received_control_command); + ASSERT_TRUE(received_control_command); + EXPECT_EQ(cmd_msg->lateral.steering_tire_angle, steer_msg.steering_tire_angle); + EXPECT_EQ(cmd_msg->lateral.steering_tire_rotation_rate, 0.0f); + EXPECT_GT(rclcpp::Time(cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); +} + +// longitudinal +TEST_F(FakeNodeFixture, longitudinal_keep_velocity) +{ + // Data to test + AckermannControlCommand::SharedPtr cmd_msg; + bool received_control_command = false; + // Node + std::shared_ptr node = makeNode(); + // Publisher/Subscribers + rclcpp::Publisher::SharedPtr traj_pub = + this->create_publisher("controller/input/reference_trajectory"); + rclcpp::Publisher::SharedPtr odom_pub = + this->create_publisher("controller/input/current_odometry"); + rclcpp::Publisher::SharedPtr steer_pub = + this->create_publisher("controller/input/current_steering"); + rclcpp::Subscription::SharedPtr cmd_sub = + this->create_subscription( + "controller/output/control_cmd", *this->get_fake_node(), + [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { + cmd_msg = msg; + received_control_command = true; + }); + std::shared_ptr br = + std::make_shared(this->get_fake_node()); + + // Dummy transform: ego is at (0.0, 0.0) in map frame + geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); + transform.header.stamp = node->now(); + br->sendTransform(transform); + /// Already running at target vel + Non stopping trajectory -> no change in velocity + // Publish velocity + VehicleOdometry odom_msg; + odom_msg.header.stamp = node->now(); + odom_msg.twist.twist.linear.x = 1.0; + odom_pub->publish(odom_msg); + // the node needs to receive two velocity msg + rclcpp::spin_some(node); + rclcpp::spin_some(this->get_fake_node()); + odom_msg.header.stamp = node->now(); + odom_pub->publish(odom_msg); + // Publish steering + SteeringReport steer_msg; + steer_msg.stamp = node->now(); + steer_msg.steering_tire_angle = 0.0; + steer_pub->publish(steer_msg); + // Publish non stopping trajectory + Trajectory traj; + traj.header.stamp = node->now(); + traj.header.frame_id = "map"; + TrajectoryPoint point; + point.pose.position.x = 0.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = 1.0; + traj.points.push_back(point); + point.pose.position.x = 50.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = 1.0; + traj.points.push_back(point); + point.pose.position.x = 100.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = 1.0; + traj.points.push_back(point); + traj_pub->publish(traj); + test_utils::waitForMessage(node, this, received_control_command); + + ASSERT_TRUE(received_control_command); + EXPECT_DOUBLE_EQ(cmd_msg->longitudinal.speed, 1.0); + EXPECT_DOUBLE_EQ(cmd_msg->longitudinal.acceleration, 0.0); + + // Generate another control message + received_control_command = false; + traj_pub->publish(traj); + test_utils::waitForMessage(node, this, received_control_command); + ASSERT_TRUE(received_control_command); + EXPECT_DOUBLE_EQ(cmd_msg->longitudinal.speed, 1.0); + EXPECT_DOUBLE_EQ(cmd_msg->longitudinal.acceleration, 0.0); +} + +TEST_F(FakeNodeFixture, longitudinal_slow_down) +{ + // Data to test + AckermannControlCommand::SharedPtr cmd_msg; + bool received_control_command = false; + // Node + std::shared_ptr node = makeNode(); + // Publisher/Subscribers + rclcpp::Publisher::SharedPtr traj_pub = + this->create_publisher("controller/input/reference_trajectory"); + rclcpp::Publisher::SharedPtr odom_pub = + this->create_publisher("controller/input/current_odometry"); + rclcpp::Publisher::SharedPtr steer_pub = + this->create_publisher("controller/input/current_steering"); + rclcpp::Subscription::SharedPtr cmd_sub = + this->create_subscription( + "controller/output/control_cmd", *this->get_fake_node(), + [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { + cmd_msg = msg; + received_control_command = true; + }); + std::shared_ptr br = + std::make_shared(this->get_fake_node()); + + // Dummy transform: ego is at (0.0, 0.0) in map frame + geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); + transform.header.stamp = node->now(); + br->sendTransform(transform); + /// Already running at target vel + Non stopping trajectory -> no change in velocity + // Publish velocity + VehicleOdometry odom_msg; + odom_msg.header.stamp = node->now(); + odom_msg.twist.twist.linear.x = 1.0; + odom_pub->publish(odom_msg); + // the node needs to receive two velocity msg + rclcpp::spin_some(node); + rclcpp::spin_some(this->get_fake_node()); + odom_msg.header.stamp = node->now(); + odom_pub->publish(odom_msg); + // Publish steering + SteeringReport steer_msg; + steer_msg.stamp = node->now(); + steer_msg.steering_tire_angle = 0.0; + steer_pub->publish(steer_msg); + // Publish non stopping trajectory + Trajectory traj; + traj.header.stamp = node->now(); + traj.header.frame_id = "map"; + TrajectoryPoint point; + point.pose.position.x = 0.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = 0.5; + traj.points.push_back(point); + point.pose.position.x = 50.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = 0.5; + traj.points.push_back(point); + point.pose.position.x = 100.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = 0.5; + traj.points.push_back(point); + traj_pub->publish(traj); + test_utils::waitForMessage(node, this, received_control_command); + + ASSERT_TRUE(received_control_command); + EXPECT_LT(cmd_msg->longitudinal.speed, static_cast(odom_msg.twist.twist.linear.x)); + EXPECT_LT(cmd_msg->longitudinal.acceleration, 0.0f); + + // Generate another control message + received_control_command = false; + traj_pub->publish(traj); + test_utils::waitForMessage(node, this, received_control_command); + ASSERT_TRUE(received_control_command); + EXPECT_LT(cmd_msg->longitudinal.speed, static_cast(odom_msg.twist.twist.linear.x)); + EXPECT_LT(cmd_msg->longitudinal.acceleration, 0.0f); +} + +TEST_F(FakeNodeFixture, longitudinal_accelerate) +{ + // Data to test + AckermannControlCommand::SharedPtr cmd_msg; + bool received_control_command = false; + // Node + std::shared_ptr node = makeNode(); + // Publisher/Subscribers + rclcpp::Publisher::SharedPtr traj_pub = + this->create_publisher("controller/input/reference_trajectory"); + rclcpp::Publisher::SharedPtr odom_pub = + this->create_publisher("controller/input/current_odometry"); + rclcpp::Publisher::SharedPtr steer_pub = + this->create_publisher("controller/input/current_steering"); + rclcpp::Subscription::SharedPtr cmd_sub = + this->create_subscription( + "controller/output/control_cmd", *this->get_fake_node(), + [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { + cmd_msg = msg; + received_control_command = true; + }); + std::shared_ptr br = + std::make_shared(this->get_fake_node()); + + // Dummy transform: ego is at (0.0, 0.0) in map frame + geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); + transform.header.stamp = node->now(); + br->sendTransform(transform); + /// Below target vel + Non stopping trajectory -> accelerate + // Publish velocity + VehicleOdometry odom_msg; + odom_msg.header.stamp = node->now(); + odom_msg.twist.twist.linear.x = 0.5; + odom_pub->publish(odom_msg); + // the node needs to receive two velocity msg + rclcpp::spin_some(node); + rclcpp::spin_some(this->get_fake_node()); + odom_msg.header.stamp = node->now(); + odom_pub->publish(odom_msg); + // Publish steering + SteeringReport steer_msg; + steer_msg.stamp = node->now(); + steer_msg.steering_tire_angle = 0.0; + steer_pub->publish(steer_msg); + // Publish non stopping trajectory + Trajectory traj; + traj.header.stamp = node->now(); + traj.header.frame_id = "map"; + TrajectoryPoint point; + point.pose.position.x = 0.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = 1.0; + traj.points.push_back(point); + point.pose.position.x = 50.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = 1.0; + traj.points.push_back(point); + point.pose.position.x = 100.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = 1.0; + traj.points.push_back(point); + traj_pub->publish(traj); + test_utils::waitForMessage(node, this, received_control_command); + + ASSERT_TRUE(received_control_command); + EXPECT_GT(cmd_msg->longitudinal.speed, static_cast(odom_msg.twist.twist.linear.x)); + EXPECT_GT(cmd_msg->longitudinal.acceleration, 0.0f); + + // Generate another control message + received_control_command = false; + traj_pub->publish(traj); + test_utils::waitForMessage(node, this, received_control_command); + ASSERT_TRUE(received_control_command); + EXPECT_GT(cmd_msg->longitudinal.speed, static_cast(odom_msg.twist.twist.linear.x)); + EXPECT_GT(cmd_msg->longitudinal.acceleration, 0.0f); +} + +TEST_F(FakeNodeFixture, longitudinal_stopped) +{ + // Data to test + AckermannControlCommand::SharedPtr cmd_msg; + bool received_control_command = false; + // Node + std::shared_ptr node = makeNode(); + // Publisher/Subscribers + rclcpp::Publisher::SharedPtr traj_pub = + this->create_publisher("controller/input/reference_trajectory"); + rclcpp::Publisher::SharedPtr odom_pub = + this->create_publisher("controller/input/current_odometry"); + rclcpp::Publisher::SharedPtr steer_pub = + this->create_publisher("controller/input/current_steering"); + rclcpp::Subscription::SharedPtr cmd_sub = + this->create_subscription( + "controller/output/control_cmd", *this->get_fake_node(), + [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { + cmd_msg = msg; + received_control_command = true; + }); + std::shared_ptr br = + std::make_shared(this->get_fake_node()); + + // Dummy transform: ego is at (0.0, 0.0) in map frame + geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); + transform.header.stamp = node->now(); + br->sendTransform(transform); + /// Below target vel + Non stopping trajectory -> accelerate + // Publish velocity + VehicleOdometry odom_msg; + odom_msg.header.stamp = node->now(); + odom_msg.twist.twist.linear.x = 0.0; + odom_pub->publish(odom_msg); + // the node needs to receive two velocity msg + rclcpp::spin_some(node); + rclcpp::spin_some(this->get_fake_node()); + odom_msg.header.stamp = node->now(); + odom_pub->publish(odom_msg); + // Publish steering + SteeringReport steer_msg; + steer_msg.stamp = node->now(); + steer_msg.steering_tire_angle = 0.0; + steer_pub->publish(steer_msg); + // Publish stopping trajectory + Trajectory traj; + traj.header.stamp = node->now(); + traj.header.frame_id = "map"; + TrajectoryPoint point; + point.pose.position.x = 0.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = 0.0; + traj.points.push_back(point); + point.pose.position.x = 50.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = 0.0; + traj.points.push_back(point); + point.pose.position.x = 100.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = 0.0; + traj.points.push_back(point); + traj_pub->publish(traj); + test_utils::waitForMessage(node, this, received_control_command); + + ASSERT_TRUE(received_control_command); + EXPECT_DOUBLE_EQ(cmd_msg->longitudinal.speed, 0.0f); + EXPECT_LT( + cmd_msg->longitudinal.acceleration, 0.0f); // when stopped negative acceleration to brake +} + +TEST_F(FakeNodeFixture, longitudinal_reverse) +{ + // Data to test + AckermannControlCommand::SharedPtr cmd_msg; + bool received_control_command = false; + // Node + std::shared_ptr node = makeNode(); + // Publisher/Subscribers + rclcpp::Publisher::SharedPtr traj_pub = + this->create_publisher("controller/input/reference_trajectory"); + rclcpp::Publisher::SharedPtr odom_pub = + this->create_publisher("controller/input/current_odometry"); + rclcpp::Publisher::SharedPtr steer_pub = + this->create_publisher("controller/input/current_steering"); + rclcpp::Subscription::SharedPtr cmd_sub = + this->create_subscription( + "controller/output/control_cmd", *this->get_fake_node(), + [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { + cmd_msg = msg; + received_control_command = true; + }); + std::shared_ptr br = + std::make_shared(this->get_fake_node()); + + // Dummy transform: ego is at (0.0, 0.0) in map frame + geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); + transform.header.stamp = node->now(); + br->sendTransform(transform); + /// Below target vel + Non stopping trajectory -> accelerate + // Publish velocity + VehicleOdometry odom_msg; + odom_msg.header.stamp = node->now(); + odom_msg.twist.twist.linear.x = 0.0; + odom_pub->publish(odom_msg); + // the node needs to receive two velocity msg + rclcpp::spin_some(node); + rclcpp::spin_some(this->get_fake_node()); + odom_msg.header.stamp = node->now(); + odom_pub->publish(odom_msg); + // Publish steering + SteeringReport steer_msg; + steer_msg.stamp = node->now(); + steer_msg.steering_tire_angle = 0.0; + steer_pub->publish(steer_msg); + // Publish reverse + Trajectory traj; + traj.header.stamp = node->now(); + traj.header.frame_id = "map"; + TrajectoryPoint point; + point.pose.position.x = 0.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = -1.0; + traj.points.push_back(point); + point.pose.position.x = 50.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = -1.0; + traj.points.push_back(point); + point.pose.position.x = 100.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = -1.0; + traj.points.push_back(point); + traj_pub->publish(traj); + test_utils::waitForMessage(node, this, received_control_command); + + ASSERT_TRUE(received_control_command); + EXPECT_LT(cmd_msg->longitudinal.speed, 0.0f); + EXPECT_GT(cmd_msg->longitudinal.acceleration, 0.0f); +} + +TEST_F(FakeNodeFixture, longitudinal_emergency) +{ + // Data to test + AckermannControlCommand::SharedPtr cmd_msg; + bool received_control_command = false; + // Node + std::shared_ptr node = makeNode(); + // Publisher/Subscribers + rclcpp::Publisher::SharedPtr traj_pub = + this->create_publisher("controller/input/reference_trajectory"); + rclcpp::Publisher::SharedPtr odom_pub = + this->create_publisher("controller/input/current_odometry"); + rclcpp::Publisher::SharedPtr steer_pub = + this->create_publisher("controller/input/current_steering"); + rclcpp::Subscription::SharedPtr cmd_sub = + this->create_subscription( + "controller/output/control_cmd", *this->get_fake_node(), + [&cmd_msg, &received_control_command](const AckermannControlCommand::SharedPtr msg) { + cmd_msg = msg; + received_control_command = true; + }); + std::shared_ptr br = + std::make_shared(this->get_fake_node()); + + // Dummy transform: ego is at (0.0, 0.0) in map frame + geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); + transform.header.stamp = node->now(); + br->sendTransform(transform); + /// Below target vel + Non stopping trajectory -> accelerate + // Publish velocity + VehicleOdometry odom_msg; + odom_msg.header.stamp = node->now(); + odom_msg.twist.twist.linear.x = 0.0; + odom_pub->publish(odom_msg); + // the node needs to receive two velocity msg + rclcpp::spin_some(node); + rclcpp::spin_some(this->get_fake_node()); + odom_msg.header.stamp = node->now(); + odom_pub->publish(odom_msg); + // Publish steering + SteeringReport steer_msg; + steer_msg.stamp = node->now(); + steer_msg.steering_tire_angle = 0.0; + steer_pub->publish(steer_msg); + // Publish trajectory starting away from the current ego pose + Trajectory traj; + traj.header.stamp = node->now(); + traj.header.frame_id = "map"; + TrajectoryPoint point; + point.pose.position.x = 10.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = 1.0; + traj.points.push_back(point); + point.pose.position.x = 50.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = 1.0; + traj.points.push_back(point); + point.pose.position.x = 100.0; + point.pose.position.y = 0.0; + point.longitudinal_velocity_mps = 1.0; + traj.points.push_back(point); + traj_pub->publish(traj); + test_utils::waitForMessage(node, this, received_control_command); + + ASSERT_TRUE(received_control_command); + // Emergencies (e.g., far from trajectory) produces braking command (0 vel, negative accel) + EXPECT_DOUBLE_EQ(cmd_msg->longitudinal.speed, 0.0f); + EXPECT_LT(cmd_msg->longitudinal.acceleration, 0.0f); +} diff --git a/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml b/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml index f8a129af4d2a4..75bd287291f2a 100644 --- a/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml +++ b/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml @@ -2,7 +2,6 @@ ros__parameters: # -- system -- - ctrl_period: 0.03 # control period [s] traj_resample_dist: 0.1 # path resampling interval [m] use_steer_prediction: false # flag for using steer prediction (do not use steer measurement) admissible_position_error: 5.0 # stop mpc calculation when error is larger than the following value @@ -57,6 +56,8 @@ # stop state: steering command is kept in the previous value in the stop state. stop_state_entry_ego_speed: 0.001 stop_state_entry_target_speed: 0.001 + converged_steer_rad: 0.1 + check_steer_converged_for_stopped_state: true # vehicle parameters vehicle: diff --git a/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml b/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml index 7a5308fcc83df..d7fce048865c9 100644 --- a/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml +++ b/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml @@ -1,12 +1,12 @@ /**: ros__parameters: - longitudinal_ctrl_period: 0.03 # control period [s] delay_compensation_time: 0.17 enable_smooth_stop: true enable_overshoot_emergency: true enable_large_tracking_error_emergency: true enable_slope_compensation: false + enable_keep_stopped_until_steer_convergence: true # state transition drive_state_stop_dist: 0.5 @@ -15,6 +15,8 @@ stopped_state_entry_duration_time: 0.1 stopped_state_entry_vel: 0.1 stopped_state_entry_acc: 0.1 + stopped_state_new_traj_duration_time: 1.0 + stopped_state_new_traj_end_dist: 0.3 emergency_state_overshoot_stop_dist: 1.5 emergency_state_traj_trans_dev: 3.0 emergency_state_traj_rot_dev: 0.7 diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index b3d26d0927859..8b6200b33ca4b 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -23,7 +23,6 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import ComposableNodeContainer -from launch_ros.actions import LoadComposableNodes from launch_ros.actions import PushRosNamespace from launch_ros.descriptions import ComposableNode from launch_ros.substitutions import FindPackageShare @@ -31,7 +30,6 @@ def launch_setup(context, *args, **kwargs): - lateral_controller_mode = LaunchConfiguration("lateral_controller_mode").perform(context) vehicle_info_param_path = LaunchConfiguration("vehicle_info_param_file").perform(context) with open(vehicle_info_param_path, "r") as f: vehicle_info_param = yaml.safe_load(f)["/**"]["ros__parameters"] @@ -41,12 +39,6 @@ def launch_setup(context, *args, **kwargs): lon_controller_param_path = LaunchConfiguration("lon_controller_param_path").perform(context) with open(lon_controller_param_path, "r") as f: lon_controller_param = yaml.safe_load(f)["/**"]["ros__parameters"] - latlon_muxer_param_path = LaunchConfiguration("latlon_muxer_param_path").perform(context) - with open(latlon_muxer_param_path, "r") as f: - latlon_muxer_param = yaml.safe_load(f)["/**"]["ros__parameters"] - pure_pursuit_param_path = LaunchConfiguration("pure_pursuit_param_path").perform(context) - with open(pure_pursuit_param_path, "r") as f: - pure_pursuit_param = yaml.safe_load(f)["/**"]["ros__parameters"] vehicle_cmd_gate_param_path = LaunchConfiguration("vehicle_cmd_gate_param_path").perform( context ) @@ -58,80 +50,28 @@ def launch_setup(context, *args, **kwargs): with open(lane_departure_checker_param_path, "r") as f: lane_departure_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"] - # lateral controller - lat_controller_component = ComposableNode( + controller_component = ComposableNode( package="trajectory_follower_nodes", - plugin="autoware::motion::control::trajectory_follower_nodes::LateralController", - name="lateral_controller_node_exe", + plugin="autoware::motion::control::trajectory_follower_nodes::Controller", + name="controller_node_exe", namespace="trajectory_follower", remappings=[ ("~/input/reference_trajectory", "/planning/scenario_planning/trajectory"), ("~/input/current_odometry", "/localization/kinematic_state"), ("~/input/current_steering", "/vehicle/status/steering_status"), - ("~/output/control_cmd", "lateral/control_cmd"), ("~/output/predicted_trajectory", "lateral/predicted_trajectory"), - ("~/output/diagnostic", "lateral/diagnostic"), - ], - parameters=[ - lat_controller_param, - vehicle_info_param, - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - pure_pursuit_component = ComposableNode( - package="pure_pursuit", - plugin="pure_pursuit::PurePursuitNode", - name="pure_pursuit_node_exe", - namespace="trajectory_follower", - remappings=[ - ("input/reference_trajectory", "/planning/scenario_planning/trajectory"), - ("input/current_odometry", "/localization/kinematic_state"), - ("output/control_raw", "lateral/control_cmd"), - ], - parameters=[ - pure_pursuit_param, - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - # longitudinal controller - lon_controller_component = ComposableNode( - package="trajectory_follower_nodes", - plugin="autoware::motion::control::trajectory_follower_nodes::LongitudinalController", - name="longitudinal_controller_node_exe", - namespace="trajectory_follower", - remappings=[ - ("~/input/current_trajectory", "/planning/scenario_planning/trajectory"), - ("~/input/current_odometry", "/localization/kinematic_state"), - ("~/output/control_cmd", "longitudinal/control_cmd"), + ("~/output/lateral_diagnostic", "lateral/diagnostic"), ("~/output/slope_angle", "longitudinal/slope_angle"), - ("~/output/diagnostic", "longitudinal/diagnostic"), + ("~/output/longitudinal_diagnostic", "longitudinal/diagnostic"), + ("~/output/control_cmd", "control_cmd"), ], parameters=[ - lon_controller_param, - vehicle_info_param, { - "show_debug_info": LaunchConfiguration("show_debug_info"), - "enable_pub_debug": LaunchConfiguration("enable_pub_debug"), + "ctrl_period": 0.03, + "lateral_controller_mode": LaunchConfiguration("lateral_controller_mode"), }, - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - # latlon muxer - latlon_muxer_component = ComposableNode( - package="trajectory_follower_nodes", - plugin="autoware::motion::control::trajectory_follower_nodes::LatLonMuxer", - name="latlon_muxer_node_exe", - namespace="trajectory_follower", - remappings=[ - ("~/input/lateral/control_cmd", "lateral/control_cmd"), - ("~/input/longitudinal/control_cmd", "longitudinal/control_cmd"), - ("~/output/control_cmd", "control_cmd"), - ], - parameters=[ - latlon_muxer_param, + lon_controller_param, + lat_controller_param, ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) @@ -246,35 +186,19 @@ def launch_setup(context, *args, **kwargs): package="rclcpp_components", executable=LaunchConfiguration("container_executable"), composable_node_descriptions=[ - lon_controller_component, - latlon_muxer_component, + controller_component, lane_departure_component, shift_decider_component, vehicle_cmd_gate_component, ], ) - # lateral controller is separated since it may be another controller (e.g. pure pursuit) - if lateral_controller_mode == "mpc_follower": - lat_controller_loader = LoadComposableNodes( - composable_node_descriptions=[lat_controller_component], - target_container=container, - # condition=LaunchConfigurationEquals("lateral_controller_mode", "mpc"), - ) - elif lateral_controller_mode == "pure_pursuit": - lat_controller_loader = LoadComposableNodes( - composable_node_descriptions=[pure_pursuit_component], - target_container=container, - # condition=LaunchConfigurationEquals("lateral_controller_mode", "mpc"), - ) - group = GroupAction( [ PushRosNamespace("control"), container, external_cmd_selector_loader, external_cmd_converter_loader, - lat_controller_loader, ] ) @@ -314,14 +238,6 @@ def add_launch_arg(name: str, default_value=None, description=None): ], "path to the parameter file of lateral controller", ) - add_launch_arg( - "pure_pursuit_param_path", - [ - FindPackageShare("pure_pursuit"), - "/config/pure_pursuit.param.yaml", - ], - "path to the parameter file of lateral controller", - ) add_launch_arg( "lon_controller_param_path", [ @@ -330,14 +246,6 @@ def add_launch_arg(name: str, default_value=None, description=None): ], "path to the parameter file of longitudinal controller", ) - add_launch_arg( - "latlon_muxer_param_path", - [ - FindPackageShare("tier4_control_launch"), - "/config/trajectory_follower/latlon_muxer.param.yaml", - ], - "path to the parameter file of latlon muxer", - ) add_launch_arg( "vehicle_cmd_gate_param_path", [ From 1ed8e2fe536dde001579978fa889fe493e091154 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Wed, 29 Jun 2022 11:12:58 +0900 Subject: [PATCH 2/5] Remove unnecessary throw error Signed-off-by: kosuke55 --- control/trajectory_follower_nodes/src/controller_node.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/control/trajectory_follower_nodes/src/controller_node.cpp b/control/trajectory_follower_nodes/src/controller_node.cpp index 94c05c04b1a78..9bf12d424de2a 100644 --- a/control/trajectory_follower_nodes/src/controller_node.cpp +++ b/control/trajectory_follower_nodes/src/controller_node.cpp @@ -94,7 +94,6 @@ Controller::LateralControllerMode Controller::getLateralControllerMode( if (controller_mode == "mpc_follower") return LateralControllerMode::MPC; if (controller_mode == "pure_pursuit") return LateralControllerMode::PURE_PURSUIT; - throw std::domain_error("[LateralController] undesired algorithm is selected."); return LateralControllerMode::INVALID; } @@ -103,7 +102,6 @@ Controller::LongitudinalControllerMode Controller::getLongitudinalControllerMode { if (controller_mode == "pid") return LongitudinalControllerMode::PID; - throw std::domain_error("[LongitudinalController] undesired algorithm is selected."); return LongitudinalControllerMode::INVALID; } From bb97765d48eb80b31f18e72cd843702629446346 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Thu, 30 Jun 2022 21:14:37 +0900 Subject: [PATCH 3/5] update from review comment Signed-off-by: Takamasa Horibe --- .../mpc_lateral_controller.hpp | 11 ++- .../pid_longitudinal_controller.hpp | 7 -- .../src/mpc_lateral_controller.cpp | 91 +++++++++++++------ .../src/pid_longitudinal_controller.cpp | 41 +-------- .../lateral_controller_defaults.param.yaml | 4 +- .../param/latlon_muxer_defaults.param.yaml | 3 - ...ongitudinal_controller_defaults.param.yaml | 2 - .../src/controller_node.cpp | 14 +-- .../lateral_controller.param.yaml | 4 +- .../longitudinal_controller.param.yaml | 2 - 10 files changed, 94 insertions(+), 85 deletions(-) delete mode 100644 control/trajectory_follower_nodes/param/latlon_muxer_defaults.param.yaml diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp b/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp index e8d1c0bbf4bd2..f66d87c1f7f47 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp +++ b/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp @@ -104,7 +104,12 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController float64_t m_stop_state_entry_ego_speed; float64_t m_stop_state_entry_target_speed; float64_t m_converged_steer_rad; - bool m_check_steer_converged_for_stopped_state; + float64_t m_new_traj_duration_time; // check trajectory shape change + float64_t m_new_traj_end_dist; // check trajectory shape change + bool m_keep_steer_control_until_converged; + + // trajectory buffer for detecting new trajectory + std::deque m_trajectory_buffer; // MPC object trajectory_follower::MPC m_mpc; @@ -198,6 +203,10 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController */ bool8_t isValidTrajectory(const autoware_auto_planning_msgs::msg::Trajectory & traj) const; + bool8_t isTrajectoryShapeChanged() const; + + bool isSteerConverged(const autoware_auto_control_msgs::msg::AckermannLateralCommand & cmd) const; + rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr m_set_param_res; /** diff --git a/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp b/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp index 2c9723fb7f2c9..3bd3f38871a53 100644 --- a/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp +++ b/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp @@ -125,9 +125,6 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal bool8_t m_enable_large_tracking_error_emergency; bool8_t m_enable_keep_stopped_until_steer_convergence; - // trajectory beffer for detecting new trajectory - std::deque m_trajectory_buffer; - // smooth stop transition struct StateTransitionParams { @@ -140,8 +137,6 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal float64_t stopped_state_entry_duration_time; float64_t stopped_state_entry_vel; float64_t stopped_state_entry_acc; - float64_t stopped_state_new_traj_duration_time; - float64_t stopped_state_new_traj_end_dist; // emergency float64_t emergency_state_overshoot_stop_dist; float64_t emergency_state_traj_trans_dev; @@ -247,8 +242,6 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal */ Motion calcEmergencyCtrlCmd(const float64_t dt) const; - bool isNewTrajectory(); - /** * @brief update control state according to the current situation * @param [in] current_control_state current control state diff --git a/control/trajectory_follower/src/mpc_lateral_controller.cpp b/control/trajectory_follower/src/mpc_lateral_controller.cpp index 059024ff047ae..f66fafc8abf63 100644 --- a/control/trajectory_follower/src/mpc_lateral_controller.cpp +++ b/control/trajectory_follower/src/mpc_lateral_controller.cpp @@ -73,8 +73,10 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} m_stop_state_entry_target_speed = node_->declare_parameter("stop_state_entry_target_speed"); m_converged_steer_rad = node_->declare_parameter("converged_steer_rad"); - m_check_steer_converged_for_stopped_state = - node_->declare_parameter("check_steer_converged_for_stopped_state"); + m_keep_steer_control_until_converged = + node_->declare_parameter("keep_steer_control_until_converged"); + m_new_traj_duration_time = node_->declare_parameter("new_traj_duration_time"); // [s] + m_new_traj_end_dist = node_->declare_parameter("new_traj_end_dist"); // [m] /* mpc parameters */ const float64_t steer_lim_deg = node_->declare_parameter("steer_lim_deg"); @@ -180,6 +182,16 @@ boost::optional MpcLateralController::run() *m_current_steering_ptr, m_current_odometry_ptr->twist.twist.linear.x, m_current_pose_ptr->pose, ctrl_cmd, predicted_traj, diagnostic); + publishPredictedTraj(predicted_traj); + publishDiagnostic(diagnostic); + + const auto createLateralOutput = [this](const auto & cmd) { + LateralOutput output; + output.control_cmd = createCtrlCmdMsg(cmd); + output.sync_data.is_steer_converged = isSteerConverged(cmd); + return boost::optional(output); + }; + if (isStoppedState()) { // Reset input buffer for (auto & value : m_mpc.m_input_buffer) { @@ -187,16 +199,7 @@ boost::optional MpcLateralController::run() } // Use previous command value as previous raw steer command m_mpc.m_raw_steer_cmd_prev = m_ctrl_cmd_prev.steering_tire_angle; - - const auto cmd_msg = createCtrlCmdMsg(m_ctrl_cmd_prev); - publishPredictedTraj(predicted_traj); - publishDiagnostic(diagnostic); - LateralOutput output; - output.control_cmd = cmd_msg; - output.sync_data.is_steer_converged = - std::abs(cmd_msg.steering_tire_angle - m_current_steering_ptr->steering_tire_angle) < - static_cast(m_converged_steer_rad); - return boost::optional(output); + return createLateralOutput(m_ctrl_cmd_prev); } if (!is_mpc_solved) { @@ -207,16 +210,7 @@ boost::optional MpcLateralController::run() } m_ctrl_cmd_prev = ctrl_cmd; - const auto cmd_msg = createCtrlCmdMsg(ctrl_cmd); - publishPredictedTraj(predicted_traj); - publishDiagnostic(diagnostic); - LateralOutput output; - output.control_cmd = cmd_msg; - output.sync_data.is_steer_converged = - std::abs(cmd_msg.steering_tire_angle - m_current_steering_ptr->steering_tire_angle) < - static_cast(m_converged_steer_rad); - - return boost::optional(output); + return createLateralOutput(ctrl_cmd); } void MpcLateralController::setInputData(InputData const & input_data) @@ -226,6 +220,22 @@ void MpcLateralController::setInputData(InputData const & input_data) m_current_steering_ptr = input_data.current_steering_ptr; } +bool MpcLateralController::isSteerConverged( + const autoware_auto_control_msgs::msg::AckermannLateralCommand & cmd) const +{ + // wait for a while to propagate the trajectory shape to the output command when the trajectory + // shape is changed. + if (isTrajectoryShapeChanged()) { + return false; + } + + const bool is_converged = + std::abs(cmd.steering_tire_angle - m_current_steering_ptr->steering_tire_angle) < + static_cast(m_converged_steer_rad); + + return is_converged; +} + bool8_t MpcLateralController::checkData() const { if (!m_mpc.hasVehicleModel()) { @@ -284,6 +294,17 @@ void MpcLateralController::setTrajectory( 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); + + // update trajectory buffer to check the trajectory shape change. + m_trajectory_buffer.push_back(*m_current_trajectory_ptr); + while (rclcpp::ok()) { + const auto time_diff = rclcpp::Time(m_trajectory_buffer.back().header.stamp) - + rclcpp::Time(m_trajectory_buffer.front().header.stamp); + if (time_diff.seconds() < m_new_traj_duration_time) { + break; + } + m_trajectory_buffer.pop_front(); + } } bool8_t MpcLateralController::updateCurrentPose() @@ -346,12 +367,15 @@ bool8_t MpcLateralController::isStoppedState() const 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; + + const auto latest_published_cmd = m_ctrl_cmd_prev; // use prev_cmd as a latest published command + if (m_keep_steer_control_until_converged && !isSteerConverged(latest_published_cmd)) { + return false; // not stopState: keep control + } + if ( std::fabs(current_vel) < m_stop_state_entry_ego_speed && - std::fabs(target_vel) < m_stop_state_entry_target_speed && - (!m_check_steer_converged_for_stopped_state || - std::abs(m_ctrl_cmd_prev.steering_tire_angle - m_current_steering_ptr->steering_tire_angle) < - static_cast(m_converged_steer_rad))) { + std::fabs(target_vel) < m_stop_state_entry_target_speed) { return true; } else { return false; @@ -496,6 +520,21 @@ rcl_interfaces::msg::SetParametersResult MpcLateralController::paramCallback( return result; } +bool MpcLateralController::isTrajectoryShapeChanged() const +{ + // TODO(Horibe): update implementation to check trajectory shape around ego vehicle. + // Now temporally check the goal position. + for (const auto & trajectory : m_trajectory_buffer) { + if ( + tier4_autoware_utils::calcDistance2d( + trajectory.points.back().pose, m_current_trajectory_ptr->points.back().pose) > + m_new_traj_end_dist) { + return true; + } + } + return false; +} + bool8_t MpcLateralController::isValidTrajectory( const autoware_auto_planning_msgs::msg::Trajectory & traj) const { diff --git a/control/trajectory_follower/src/pid_longitudinal_controller.cpp b/control/trajectory_follower/src/pid_longitudinal_controller.cpp index 7bc301cc166a4..2d7e2550ebfee 100644 --- a/control/trajectory_follower/src/pid_longitudinal_controller.cpp +++ b/control/trajectory_follower/src/pid_longitudinal_controller.cpp @@ -73,10 +73,7 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) : node node_->declare_parameter("stopped_state_entry_vel"); // [m/s] p.stopped_state_entry_acc = node_->declare_parameter("stopped_state_entry_acc"); // [m/s²] - p.stopped_state_new_traj_duration_time = - node_->declare_parameter("stopped_state_new_traj_duration_time"); // [s] - p.stopped_state_new_traj_end_dist = - node_->declare_parameter("stopped_state_new_traj_end_dist"); // [m] + // emergency p.emergency_state_overshoot_stop_dist = node_->declare_parameter("emergency_state_overshoot_stop_dist"); // [m] @@ -258,8 +255,6 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac 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("stopped_state_new_traj_duration_time", p.stopped_state_new_traj_duration_time); - update_param("stopped_state_new_traj_end_dist", p.stopped_state_new_traj_end_dist); 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); @@ -475,32 +470,6 @@ PidLongitudinalController::Motion PidLongitudinalController::calcEmergencyCtrlCm return Motion{vel, acc}; } -bool PidLongitudinalController::isNewTrajectory() -{ - // flags for state transition - const auto & p = m_state_transition_params; - - m_trajectory_buffer.push_back(*m_trajectory_ptr); - while (true) { - const auto time_diff = rclcpp::Time(m_trajectory_buffer.back().header.stamp) - - rclcpp::Time(m_trajectory_buffer.front().header.stamp); - if (time_diff.seconds() < p.stopped_state_new_traj_duration_time) { - break; - } - m_trajectory_buffer.pop_front(); - } - - for (const auto & trajectory : m_trajectory_buffer) { - if ( - tier4_autoware_utils::calcDistance2d( - trajectory.points.back().pose, m_trajectory_ptr->points.back().pose) > - p.stopped_state_new_traj_end_dist) { - return true; - } - } - return false; -} - PidLongitudinalController::ControlState PidLongitudinalController::updateControlState( const ControlState current_control_state, const ControlData & control_data) { @@ -514,8 +483,9 @@ PidLongitudinalController::ControlState PidLongitudinalController::updateControl 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 keep_stopped_condition = - !lateral_sync_data_.is_steer_converged || isNewTrajectory(); + m_enable_keep_stopped_until_steer_convergence && !lateral_sync_data_.is_steer_converged; const bool8_t stopping_condition = stop_dist < p.stopping_state_stop_dist; if ( @@ -570,9 +540,8 @@ PidLongitudinalController::ControlState PidLongitudinalController::updateControl return ControlState::DRIVE; } } else if (current_control_state == ControlState::STOPPED) { - if ( - departure_condition_from_stopped && - (!m_enable_keep_stopped_until_steer_convergence || !keep_stopped_condition)) { + if (keep_stopped_condition) return 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 diff --git a/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml b/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml index 0a5279faf7054..78086b846ebca 100644 --- a/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml +++ b/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml @@ -56,7 +56,9 @@ stop_state_entry_ego_speed: 0.001 stop_state_entry_target_speed: 0.001 converged_steer_rad: 0.1 - check_steer_converged_for_stopped_state: true + keep_steer_control_until_converged: true + new_traj_duration_time: 1.0 + new_traj_end_dist: 0.3 # vehicle parameters mass_kg: 2400.0 diff --git a/control/trajectory_follower_nodes/param/latlon_muxer_defaults.param.yaml b/control/trajectory_follower_nodes/param/latlon_muxer_defaults.param.yaml deleted file mode 100644 index 371bb99787559..0000000000000 --- a/control/trajectory_follower_nodes/param/latlon_muxer_defaults.param.yaml +++ /dev/null @@ -1,3 +0,0 @@ -/**: - ros__parameters: - timeout_thr_sec: 0.5 # [s] Time limit after which input messages are discarded. diff --git a/control/trajectory_follower_nodes/param/longitudinal_controller_defaults.param.yaml b/control/trajectory_follower_nodes/param/longitudinal_controller_defaults.param.yaml index 79da58c9b1f35..eb2ef443c4576 100644 --- a/control/trajectory_follower_nodes/param/longitudinal_controller_defaults.param.yaml +++ b/control/trajectory_follower_nodes/param/longitudinal_controller_defaults.param.yaml @@ -15,8 +15,6 @@ stopped_state_entry_duration_time: 0.1 stopped_state_entry_vel: 0.1 stopped_state_entry_acc: 0.1 - stopped_state_new_traj_duration_time: 1.0 - stopped_state_new_traj_end_dist: 0.3 emergency_state_overshoot_stop_dist: 1.5 emergency_state_traj_trans_dev: 3.0 emergency_state_traj_rot_dev: 0.7 diff --git a/control/trajectory_follower_nodes/src/controller_node.cpp b/control/trajectory_follower_nodes/src/controller_node.cpp index 9bf12d424de2a..f56a26721e4da 100644 --- a/control/trajectory_follower_nodes/src/controller_node.cpp +++ b/control/trajectory_follower_nodes/src/controller_node.cpp @@ -122,8 +122,6 @@ void Controller::onSteering(const autoware_auto_vehicle_msgs::msg::SteeringRepor bool Controller::isTimeOut() { - if (!longitudinal_output_ || !lateral_output_) return true; - const auto now = this->now(); if ((now - lateral_output_->control_cmd.stamp).seconds() > timeout_thr_sec_) { RCLCPP_ERROR_THROTTLE( @@ -146,12 +144,16 @@ void Controller::callbackTimerControl() lateral_controller_->setInputData(input_data_); // trajectory, odometry, steering const auto lon_out = longitudinal_controller_->run(); - longitudinal_output_ = lon_out ? lon_out : longitudinal_output_; + longitudinal_output_ = lon_out ? lon_out : longitudinal_output_; // use previous value if none. const auto lat_out = lateral_controller_->run(); - lateral_output_ = lat_out ? lat_out : lateral_output_; + lateral_output_ = lat_out ? lat_out : lateral_output_; // use previous value if none. + + if (!longitudinal_output_ || !lateral_output_) return; + + longitudinal_controller_->sync(lateral_output_->sync_data); + lateral_controller_->sync(longitudinal_output_->sync_data); - if (lateral_output_) longitudinal_controller_->sync(lateral_output_->sync_data); - if (longitudinal_output_) lateral_controller_->sync(longitudinal_output_->sync_data); + // TODO(Horibe): Think specification. This comes from the old implementation. if (isTimeOut()) return; autoware_auto_control_msgs::msg::AckermannControlCommand out; diff --git a/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml b/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml index 75bd287291f2a..63ca9d66cef1b 100644 --- a/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml +++ b/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml @@ -57,7 +57,9 @@ stop_state_entry_ego_speed: 0.001 stop_state_entry_target_speed: 0.001 converged_steer_rad: 0.1 - check_steer_converged_for_stopped_state: true + keep_steer_control_until_converged: true + new_traj_duration_time: 1.0 + new_traj_end_dist: 0.3 # vehicle parameters vehicle: diff --git a/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml b/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml index d7fce048865c9..b6e1c3a38c799 100644 --- a/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml +++ b/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml @@ -15,8 +15,6 @@ stopped_state_entry_duration_time: 0.1 stopped_state_entry_vel: 0.1 stopped_state_entry_acc: 0.1 - stopped_state_new_traj_duration_time: 1.0 - stopped_state_new_traj_end_dist: 0.3 emergency_state_overshoot_stop_dist: 1.5 emergency_state_traj_trans_dev: 3.0 emergency_state_traj_rot_dev: 0.7 From eedf63fb03865f06ccc54561191bd19b0a4d159b Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Fri, 1 Jul 2022 17:29:16 +0900 Subject: [PATCH 4/5] Set steer converged params false Signed-off-by: kosuke55 --- .../param/lateral_controller_defaults.param.yaml | 2 +- .../param/longitudinal_controller_defaults.param.yaml | 2 +- .../config/trajectory_follower/lateral_controller.param.yaml | 2 +- .../trajectory_follower/longitudinal_controller.param.yaml | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml b/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml index 78086b846ebca..f08e39cab9829 100644 --- a/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml +++ b/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml @@ -56,7 +56,7 @@ stop_state_entry_ego_speed: 0.001 stop_state_entry_target_speed: 0.001 converged_steer_rad: 0.1 - keep_steer_control_until_converged: true + keep_steer_control_until_converged: false new_traj_duration_time: 1.0 new_traj_end_dist: 0.3 diff --git a/control/trajectory_follower_nodes/param/longitudinal_controller_defaults.param.yaml b/control/trajectory_follower_nodes/param/longitudinal_controller_defaults.param.yaml index eb2ef443c4576..e5e82ec101be2 100644 --- a/control/trajectory_follower_nodes/param/longitudinal_controller_defaults.param.yaml +++ b/control/trajectory_follower_nodes/param/longitudinal_controller_defaults.param.yaml @@ -6,7 +6,7 @@ enable_overshoot_emergency: true enable_large_tracking_error_emergency: true enable_slope_compensation: false - enable_keep_stopped_until_steer_convergence: true + enable_keep_stopped_until_steer_convergence: false # state transition drive_state_stop_dist: 0.5 diff --git a/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml b/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml index 63ca9d66cef1b..79b131806a4d6 100644 --- a/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml +++ b/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml @@ -57,7 +57,7 @@ stop_state_entry_ego_speed: 0.001 stop_state_entry_target_speed: 0.001 converged_steer_rad: 0.1 - keep_steer_control_until_converged: true + keep_steer_control_until_converged: false new_traj_duration_time: 1.0 new_traj_end_dist: 0.3 diff --git a/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml b/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml index b6e1c3a38c799..28fa297afbecb 100644 --- a/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml +++ b/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml @@ -6,7 +6,7 @@ enable_overshoot_emergency: true enable_large_tracking_error_emergency: true enable_slope_compensation: false - enable_keep_stopped_until_steer_convergence: true + enable_keep_stopped_until_steer_convergence: false # state transition drive_state_stop_dist: 0.5 From 670b9407d52bf26d964caa815b870671fa2d84aa Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Sat, 2 Jul 2022 16:23:41 +0900 Subject: [PATCH 5/5] Update params of design.md Signed-off-by: kosuke55 --- .../design/lateral_controller-design.md | 5 ++- .../design/longitudinal_controller-design.md | 34 +++++++++---------- 2 files changed, 21 insertions(+), 18 deletions(-) diff --git a/control/trajectory_follower_nodes/design/lateral_controller-design.md b/control/trajectory_follower_nodes/design/lateral_controller-design.md index 83e6b58957af6..16dbedec54036 100644 --- a/control/trajectory_follower_nodes/design/lateral_controller-design.md +++ b/control/trajectory_follower_nodes/design/lateral_controller-design.md @@ -54,7 +54,6 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving. | Name | Type | Description | Default value | | :------------------------------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------ | :------------ | | show_debug_info | bool | display debug info | false | -| ctrl_period | double | control period [s] | 0.03 | | traj_resample_dist | double | distance of waypoints in resampling [m] | 0.1 | | enable_path_smoothing | bool | path smoothing flag. This should be true when uses path resampling to reduce resampling noise. | true | | path_filter_moving_ave_num | int | number of data points moving average filter for path smoothing | 35 | @@ -66,6 +65,10 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving. | admissible_yaw_error_rad | double | stop vehicle when following yaw angle error is larger than this value [rad]. | 1.57 | | stop_state_entry_ego_speed \*1 | double | threshold value of the ego vehicle speed used to the stop state entry condition | 0.0 | | stop_state_entry_target_speed \*1 | double | threshold value of the target speed used to the stop state entry condition | 0.0 | +| converged_steer_rad | double | threshold value of the steer convergence | 0.1 | +| keep_steer_control_until_converged | bool | keep steer control until steer is converged | false | +| new_traj_duration_time | double | threshold value of the time to be considered as new trajectory | 1.0 | +| new_traj_end_dist | double | threshold value of the distance between trajectory ends to be considered as new trajectory | 0.3 | (\*1) To prevent unnecessary steering movement, the steering command is fixed to the previous value in the stop state. diff --git a/control/trajectory_follower_nodes/design/longitudinal_controller-design.md b/control/trajectory_follower_nodes/design/longitudinal_controller-design.md index 21d893ee2f01c..207a9755d7117 100644 --- a/control/trajectory_follower_nodes/design/longitudinal_controller-design.md +++ b/control/trajectory_follower_nodes/design/longitudinal_controller-design.md @@ -146,23 +146,23 @@ In this controller, the predicted ego-velocity and the target velocity after the The default parameters defined in `param/lateral_controller_defaults.param.yaml` are adjusted to the AutonomouStuff Lexus RX 450h for under 40 km/h driving. -| Name | Type | Description | Default value | -| :------------------------------------ | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| longitudinal_ctrl_period | double | longitudinal control period [s] | 0.03 | -| delay_compensation_time | double | delay for longitudinal control [s] | 0.17 | -| enable_smooth_stop | bool | flag to enable transition to STOPPING | true | -| enable_overshoot_emergency | bool | flag to enable transition to EMERGENCY when the ego is over the stop line with a certain distance. See `emergency_state_overshoot_stop_dist`. | true | -| enable_large_tracking_error_emergency | bool | flag to enable transition to EMERGENCY when the closest trajectory point search is failed due to a large deviation between trajectory and ego pose. | true | -| enable_slope_compensation | bool | flag to modify output acceleration for slope compensation. The source of the slope angle can be selected from ego-pose or trajectory angle. See `use_trajectory_for_pitch_calculation`. | true | -| enable_brake_keeping_before_stop | bool | flag to keep a certain acceleration during DRIVE state before the ego stops. See [Brake keeping](#brake-keeping). | false | -| max_acc | double | max value of output acceleration [m/s^2] | 3.0 | -| min_acc | double | min value of output acceleration [m/s^2] | -5.0 | -| max_jerk | double | max value of jerk of output acceleration [m/s^3] | 2.0 | -| min_jerk | double | min value of jerk of output acceleration [m/s^3] | -5.0 | -| use_trajectory_for_pitch_calculation | bool | If true, the slope is estimated from trajectory z-level. Otherwise the pitch angle of the ego pose is used. | false | -| lpf_pitch_gain | double | gain of low-pass filter for pitch estimation | 0.95 | -| max_pitch_rad | double | max value of estimated pitch [rad] | 0.1 | -| min_pitch_rad | double | min value of estimated pitch [rad] | -0.1 | +| Name | Type | Description | Default value | +| :------------------------------------------ | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| delay_compensation_time | double | delay for longitudinal control [s] | 0.17 | +| enable_smooth_stop | bool | flag to enable transition to STOPPING | true | +| enable_overshoot_emergency | bool | flag to enable transition to EMERGENCY when the ego is over the stop line with a certain distance. See `emergency_state_overshoot_stop_dist`. | true | +| enable_large_tracking_error_emergency | bool | flag to enable transition to EMERGENCY when the closest trajectory point search is failed due to a large deviation between trajectory and ego pose. | true | +| enable_slope_compensation | bool | flag to modify output acceleration for slope compensation. The source of the slope angle can be selected from ego-pose or trajectory angle. See `use_trajectory_for_pitch_calculation`. | true | +| enable_brake_keeping_before_stop | bool | flag to keep a certain acceleration during DRIVE state before the ego stops. See [Brake keeping](#brake-keeping). | false | +| enable_keep_stopped_until_steer_convergence | bool | flag to keep stopped condition until until the steer converges. | false | +| max_acc | double | max value of output acceleration [m/s^2] | 3.0 | +| min_acc | double | min value of output acceleration [m/s^2] | -5.0 | +| max_jerk | double | max value of jerk of output acceleration [m/s^3] | 2.0 | +| min_jerk | double | min value of jerk of output acceleration [m/s^3] | -5.0 | +| use_trajectory_for_pitch_calculation | bool | If true, the slope is estimated from trajectory z-level. Otherwise the pitch angle of the ego pose is used. | false | +| lpf_pitch_gain | double | gain of low-pass filter for pitch estimation | 0.95 | +| max_pitch_rad | double | max value of estimated pitch [rad] | 0.1 | +| min_pitch_rad | double | min value of estimated pitch [rad] | -0.1 | #### State transition