diff --git a/control/control_performance_analysis/CMakeLists.txt b/control/control_performance_analysis/CMakeLists.txt new file mode 100644 index 0000000000000..7adf34bea4eb5 --- /dev/null +++ b/control/control_performance_analysis/CMakeLists.txt @@ -0,0 +1,59 @@ +cmake_minimum_required(VERSION 3.5) +project(control_performance_analysis) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(Boost REQUIRED) + +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) + +rosidl_generate_interfaces(control_performance_analysis_msgs + msg/Error.msg + msg/ErrorStamped.msg + DEPENDENCIES + std_msgs +) + +ament_auto_add_library(control_performance_analysis_core SHARED + src/control_performance_analysis_utils.cpp + src/control_performance_analysis_core.cpp +) + +ament_auto_add_library(control_performance_analysis_node SHARED + src/control_performance_analysis_node.cpp +) + +rosidl_target_interfaces(control_performance_analysis_node + control_performance_analysis_msgs "rosidl_typesupport_cpp") + +target_link_libraries(control_performance_analysis_node + control_performance_analysis_core +) + +rclcpp_components_register_node(control_performance_analysis_node + PLUGIN "control_performance_analysis::ControlPerformanceAnalysisNode" + EXECUTABLE control_performance_analysis +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/control/control_performance_analysis/README.md b/control/control_performance_analysis/README.md new file mode 100644 index 0000000000000..dbb0405590b50 --- /dev/null +++ b/control/control_performance_analysis/README.md @@ -0,0 +1,28 @@ +# control_performance_analysis + +## Purpose + +`control_performance_analysis` is the package to analyze the tracking performance of a control module. + +This package is used as a tool to quantify the results of the control module. +That's why it doesn't interfere with the core logic of autonomous driving. + +Based on the various input from planning, control, and vehicle, it publishes the result of analysis as `control_performance_analysis::msg::ErrorStamped` defined in this package. + +## Input / Output + +### Input topics + +| Name | Type | Description | +| -------------------------------------------------- | -------------------------------------------------------- | --------------------------------------------------- | +| `/planning/scenario_planning/trajectory` | autoware_auto_planning_msgs::msg::Trajectory | Output trajectory from planning module. | +| `/control/trajectory_follower/lateral/control_cmd` | autoware_auto_control_msgs::msg::AckermannLateralCommand | Output lateral control command from control module. | +| `/vehicle/status/steering_status` | autoware_auto_vehicle_msgs::msg::SteeringReport | Steering information from vehicle. | +| `/localization/kinematic_state` | nav_msgs::msg::Odometry | Use twist from odometry. | +| `/tf` | tf2_msgs::msg::TFMessage | Extract ego pose from tf. | + +### Output topics + +| Name | Type | Description | +| --------------------------------------- | ----------------------------------------------- | --------------------------------------- | +| `/control_performance/performance_vars` | control_performance_analysis::msg::ErrorStamped | The result of the performance analysis. | diff --git a/control/control_performance_analysis/config/control_performance_analysis.param.yaml b/control/control_performance_analysis/config/control_performance_analysis.param.yaml new file mode 100644 index 0000000000000..9867e170e9adb --- /dev/null +++ b/control/control_performance_analysis/config/control_performance_analysis.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + # -- publishing period -- + control_period: 0.033 + double curvature_interval_length_: 5.0 diff --git a/control/control_performance_analysis/config/error_rqt_multiplot.xml b/control/control_performance_analysis/config/error_rqt_multiplot.xml new file mode 100644 index 0000000000000..ceb9f2cb308e3 --- /dev/null +++ b/control/control_performance_analysis/config/error_rqt_multiplot.xml @@ -0,0 +1,853 @@ + + + + #000000 + #fce94f + false + false + + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + header/seq + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /control_performance/performance_vars + control_performance_analysis/ErrorStamped + + + error/lateral_error + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /control_performance/performance_vars + control_performance_analysis/ErrorStamped + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Lateral Error + + + + true + + 30 + Lateral Error Plot + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /control_performance/performance_vars + control_performance_analysis/ErrorStamped + + + error/error_energy + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /control_performance/performance_vars + control_performance_analysis/ErrorStamped + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Error Energy + + + + true + + 30 + Error Energy Plot + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /control_performance/performance_vars + control_performance_analysis/ErrorStamped + + + error/lateral_error_velocity + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /control_performance/performance_vars + control_performance_analysis/ErrorStamped + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Lateral Error Speed + + + + true + + 30 + Lateral Error Velocity Plot + + + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + error/heading_error + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /control_performance/performance_vars + control_performance_analysis/ErrorStamped + + + error/heading_error + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /control_performance/performance_vars + control_performance_analysis/ErrorStamped + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Heading Angle Tracking Error + + + + true + + 30 + Heading Error Plot + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /control_performance/performance_vars + control_performance_analysis/ErrorStamped + + + error/control_effort_energy + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /control_performance/performance_vars + control_performance_analysis/ErrorStamped + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Steering Energy + + + + true + + 30 + Control Energy Plot + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /control_performance/performance_vars + control_performance_analysis/ErrorStamped + + + error/lateral_error_acceleration + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /control_performance/performance_vars + control_performance_analysis/ErrorStamped + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Lateral Error Acceleration Approximation + + + + true + + 30 + Lateral Acceleration Plot + + + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /control_performance/performance_vars + control_performance_analysis/ErrorStamped + + + error/value_approximation + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /control_performance/performance_vars + control_performance_analysis/ErrorStamped + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Value Function + + + + true + + 30 + Approximate Value + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /control_performance/performance_vars + control_performance_analysis/ErrorStamped + + + error/curvature_estimate + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /control_performance/performance_vars + control_performance_analysis/ErrorStamped + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Curvature + + + + true + + 30 + Value Decay Rate Plot + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /control/trajectory_follower/control_cmd + autoware_control_msgs/ControlCommandStamped + + + control/steering_angle + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /control/trajectory_follower/control_cmd + autoware_control_msgs/ControlCommandStamped + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Computed Steering + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /vehicle/status/steering_status + autoware_vehicle_msgs/Steering + + + data + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /vehicle/status/steering_status + autoware_vehicle_msgs/Steering + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Measured Vehicle Steering + + + + true + + 30 + Measured and Computed Steerings Plot + + + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /vehicle/status/velocity_status + geometry_msgs/TwistStamped + + + twist/linear/x + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /vehicle/status/velocity_status + geometry_msgs/TwistStamped + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Velocity Curve + + + + true + + 30 + Longitudinal Velocity Vx Plot + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + true + + 30 + Untitled Plot + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + true + + 30 + Untitled Plot + + + + false +
+ diff --git a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp new file mode 100644 index 0000000000000..4ffcc63f81473 --- /dev/null +++ b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp @@ -0,0 +1,104 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_CORE_HPP_ +#define CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_CORE_HPP_ + +#include "control_performance_analysis/control_performance_analysis_utils.hpp" + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace control_performance_analysis +{ +using autoware_auto_control_msgs::msg::AckermannLateralCommand; +using autoware_auto_planning_msgs::msg::Trajectory; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::PoseArray; +using geometry_msgs::msg::Twist; + +struct TargetPerformanceMsgVars +{ + double lateral_error; + double heading_error; + double control_effort_energy; + double error_energy; + double value_approximation; + double curvature_estimate; + double curvature_estimate_pp; + double lateral_error_velocity; + double lateral_error_acceleration; +}; + +class ControlPerformanceAnalysisCore +{ +public: + // See https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + ControlPerformanceAnalysisCore(); + ControlPerformanceAnalysisCore(double wheelbase, double curvature_interval_length); + + // Setters + void setCurrentPose(const Pose & msg); + void setCurrentWaypoints(const Trajectory & trajectory); + void setCurrentVelocities(const Twist & twist_msg); + void setCurrentControlValue(const AckermannLateralCommand & msg); + void setInterpolatedPose(Pose & interpolated_pose); + + void findCurveRefIdx(); + std::pair findClosestPrevWayPointIdx_path_direction(); + double estimateCurvature(); + double estimatePurePursuitCurvature(); + + // Getters + bool isDataReady() const; + std::pair getPerformanceVars(); + Pose getPrevWPPose() const; + std::pair calculateClosestPose(); + +private: + double wheelbase_; + double curvature_interval_length_; + + // Variables Received Outside + std::shared_ptr current_waypoints_ptr_; + std::shared_ptr current_vec_pose_ptr_; + std::shared_ptr> current_velocities_ptr_; // [Vx, Heading rate] + std::shared_ptr current_control_ptr_; + + // Variables computed + std::unique_ptr idx_prev_wp_; // the waypoint index, vehicle + std::unique_ptr idx_curve_ref_wp_; // index of waypoint corresponds to front axle center + std::unique_ptr idx_next_wp_; // the next waypoint index, vehicle heading to + std::unique_ptr prev_target_vars_{}; + std::shared_ptr interpolated_pose_ptr_; + // V = xPx' ; Value function from DARE Lyap matrix P + Eigen::Matrix2d const lyap_P_ = (Eigen::MatrixXd(2, 2) << 2.342, 8.60, 8.60, 64.29).finished(); + double const contR{10.0}; // Control weight in LQR + + rclcpp::Logger logger_{rclcpp::get_logger("control_performance_analysis")}; + rclcpp::Clock clock_{RCL_ROS_TIME}; +}; +} // namespace control_performance_analysis + +#endif // CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_CORE_HPP_ diff --git a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp new file mode 100644 index 0000000000000..201d7b2c89b4b --- /dev/null +++ b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp @@ -0,0 +1,105 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_NODE_HPP_ +#define CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_NODE_HPP_ + +#include "control_performance_analysis/control_performance_analysis_core.hpp" +#include "control_performance_analysis/msg/error_stamped.hpp" + +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include + +namespace control_performance_analysis +{ +using autoware_auto_control_msgs::msg::AckermannLateralCommand; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_vehicle_msgs::msg::SteeringReport; +using control_performance_analysis::msg::ErrorStamped; +using geometry_msgs::msg::PoseStamped; +using nav_msgs::msg::Odometry; + +// Parameters Struct +struct Param +{ + // Global parameters + double wheel_base; + double curvature_interval_length; + + // Control Method Parameters + double control_period; +}; + +class ControlPerformanceAnalysisNode : public rclcpp::Node +{ +public: + explicit ControlPerformanceAnalysisNode(const rclcpp::NodeOptions & node_options); + +private: + // Subscribers and Local Variable Assignment + rclcpp::Subscription::SharedPtr sub_trajectory_; // subscribe to trajectory + rclcpp::Subscription::SharedPtr + sub_control_steering_; // subscribe to steering control value + rclcpp::Subscription::SharedPtr sub_velocity_; // subscribe to velocity + rclcpp::Subscription::SharedPtr sub_vehicle_steering_; + + // Self Pose listener. + autoware_utils::SelfPoseListener self_pose_listener_{this}; // subscribe to pose listener. + + // Publishers + rclcpp::Publisher::SharedPtr pub_error_msg_; // publish error message + + // Node Methods + bool isDataReady() const; // check if data arrive + static bool isValidTrajectory(const Trajectory & traj); + boost::optional computeTargetPerformanceMsgVars() const; + + // Callback Methods + void onTrajectory(const Trajectory::ConstSharedPtr msg); + void publishErrorMsg(const TargetPerformanceMsgVars & control_performance_vars); + void onControlRaw(const AckermannLateralCommand::ConstSharedPtr control_msg); + void onVecSteeringMeasured(const SteeringReport::ConstSharedPtr meas_steer_msg); + void onVelocity(const Odometry::ConstSharedPtr msg); + + // Timer - To Publish In Control Period + rclcpp::TimerBase::SharedPtr timer_publish_; + void onTimer(); + + // Parameters + Param param_{}; // wheelbase, control period and feedback coefficients. + TargetPerformanceMsgVars target_error_vars_{}; + + // Subscriber Parameters + Trajectory::ConstSharedPtr current_trajectory_ptr_; // ConstPtr to local traj. + AckermannLateralCommand::ConstSharedPtr current_control_msg_ptr_; + SteeringReport::ConstSharedPtr current_vec_steering_msg_ptr_; + Odometry::ConstSharedPtr current_odom_ptr_; + PoseStamped::ConstSharedPtr current_pose_; // pose of the vehicle, x, y, heading + + // Algorithm + std::unique_ptr control_performance_core_ptr_; +}; +} // namespace control_performance_analysis + +#endif // CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_NODE_HPP_ diff --git a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_utils.hpp b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_utils.hpp new file mode 100644 index 0000000000000..de05ea3da63f3 --- /dev/null +++ b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_utils.hpp @@ -0,0 +1,106 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_UTILS_HPP_ +#define CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_UTILS_HPP_ + +#include +#include +#include + +#include + +#include + +#include +#include + +namespace control_performance_analysis +{ +namespace utils +{ +// Right hand sided tangent and normal vectors +inline std::vector getTangentVector(double yaw_angle) +{ + return std::vector{cos(yaw_angle), sin(yaw_angle)}; +} + +inline std::vector getNormalVector(double yaw_angle) +{ + return std::vector{-sin(yaw_angle), cos(yaw_angle)}; +} + +inline double computeLateralError( + std::vector & closest_point_position, std::vector & vehicle_position, + double & yaw_angle) +{ + // Normal vector of vehicle direction + std::vector normal_vector = getNormalVector(yaw_angle); + + // Vector to path point originating from the vehicle + std::vector vector_to_path_point{ + closest_point_position[0] - vehicle_position[0], + closest_point_position[1] - vehicle_position[1]}; + + double lateral_error = + normal_vector[0] * vector_to_path_point[0] + normal_vector[1] * vector_to_path_point[1]; + + return lateral_error; +} + +/* + * Shortest distance between two angles. As the angles are cyclic, interpolation between to + * angles must be carried out using the distance value instead of using the end values of + * two points. + * */ +inline double angleDistance(double & target_angle, double const & reference_angle) +{ + double diff = std::fmod(target_angle - reference_angle + M_PI_2, 2 * M_PI) - M_PI_2; + double diff_signed_correction = diff < -M_PI_2 ? diff + 2 * M_PI : diff; // Fix sign + + return -1.0 * diff_signed_correction; +} + +inline geometry_msgs::msg::Quaternion createOrientationMsgFromYaw(double yaw_angle) +{ + geometry_msgs::msg::Quaternion orientation_msg; + double roll_angle = 0.0; + double pitch_angle = 0.0; + + tf2::Quaternion quaternion; + quaternion.setRPY(roll_angle, pitch_angle, yaw_angle); + + orientation_msg.w = quaternion.getW(); + orientation_msg.x = quaternion.getX(); + orientation_msg.y = quaternion.getY(); + orientation_msg.z = quaternion.getZ(); + + return orientation_msg; +} + +// p-points a, b contains [x, y] coordinates. +double determinant(std::array const & a, std::array const & b); + +double triangleArea( + std::array const & a, std::array const & b, + std::array const & c); + +double curvatureFromThreePoints( + std::array const & a, std::array const & b, + std::array const & c); + +} // namespace utils +} // namespace control_performance_analysis + +#endif // CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_UTILS_HPP_ diff --git a/control/control_performance_analysis/launch/controller_performance_analysis.launch.xml b/control/control_performance_analysis/launch/controller_performance_analysis.launch.xml new file mode 100644 index 0000000000000..8a248c9e8745a --- /dev/null +++ b/control/control_performance_analysis/launch/controller_performance_analysis.launch.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/control/control_performance_analysis/msg/Error.msg b/control/control_performance_analysis/msg/Error.msg new file mode 100644 index 0000000000000..f63ed2117ddce --- /dev/null +++ b/control/control_performance_analysis/msg/Error.msg @@ -0,0 +1,9 @@ +float64 lateral_error +float64 heading_error +float64 control_effort_energy +float64 error_energy +float64 value_approximation +float64 curvature_estimate +float64 curvature_estimate_pp +float64 lateral_error_velocity +float64 lateral_error_acceleration diff --git a/control/control_performance_analysis/msg/ErrorStamped.msg b/control/control_performance_analysis/msg/ErrorStamped.msg new file mode 100644 index 0000000000000..8b0b37f653b40 --- /dev/null +++ b/control/control_performance_analysis/msg/ErrorStamped.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +control_performance_analysis/Error error diff --git a/control/control_performance_analysis/package.xml b/control/control_performance_analysis/package.xml new file mode 100644 index 0000000000000..bda3b7cce680b --- /dev/null +++ b/control/control_performance_analysis/package.xml @@ -0,0 +1,39 @@ + + + + control_performance_analysis + 0.1.0 + Controller Performance Evaluation + Ali Boyali + Apache License 2.0 + + ament_cmake_auto + rosidl_default_generators + + autoware_auto_control_msgs + autoware_auto_planning_msgs + autoware_auto_vehicle_msgs + autoware_utils + geometry_msgs + libboost-dev + nav_msgs + rclcpp + sensor_msgs + std_msgs + tf2 + tf2_eigen + tf2_ros + vehicle_info_util + + autoware_global_parameter_loader + rosidl_default_runtime + + ament_lint_auto + autoware_lint_common + + rosidl_interface_packages + + + ament_cmake + + diff --git a/control/control_performance_analysis/src/control_performance_analysis_core.cpp b/control/control_performance_analysis/src/control_performance_analysis_core.cpp new file mode 100644 index 0000000000000..9847b874c8cb2 --- /dev/null +++ b/control/control_performance_analysis/src/control_performance_analysis_core.cpp @@ -0,0 +1,519 @@ +// Copyright 2021 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. + +#include "control_performance_analysis/control_performance_analysis_core.hpp" + +#include +#include +#include +#include +#include + +namespace control_performance_analysis +{ +using geometry_msgs::msg::Quaternion; + +ControlPerformanceAnalysisCore::ControlPerformanceAnalysisCore() : wheelbase_{2.74} +{ + prev_target_vars_ = std::make_unique(TargetPerformanceMsgVars{}); + current_velocities_ptr_ = std::make_shared>(2, 0.0); +} + +ControlPerformanceAnalysisCore::ControlPerformanceAnalysisCore( + double wheelbase, double curvature_interval_length) +: wheelbase_{wheelbase}, curvature_interval_length_{curvature_interval_length} +{ + // prepare control performance struct + prev_target_vars_ = std::make_unique(TargetPerformanceMsgVars{}); + current_velocities_ptr_ = std::make_shared>(2, 0.0); +} + +void ControlPerformanceAnalysisCore::setCurrentWaypoints(const Trajectory & trajectory) +{ + current_waypoints_ptr_ = std::make_shared(); + + for (const auto & point : trajectory.points) { + current_waypoints_ptr_->poses.emplace_back(point.pose); + } +} + +void ControlPerformanceAnalysisCore::setCurrentPose(const Pose & msg) +{ + current_vec_pose_ptr_ = std::make_shared(msg); +} + +void ControlPerformanceAnalysisCore::setCurrentControlValue(const AckermannLateralCommand & msg) +{ + current_control_ptr_ = std::make_shared(msg); +} + +std::pair ControlPerformanceAnalysisCore::findClosestPrevWayPointIdx_path_direction() +{ + if (!isDataReady()) { + return std::make_pair(false, std::numeric_limits::quiet_NaN()); + } + + // How far the next waypoint can be ahead of the vehicle direction. + double acceptable_min_distance = 2.0; + + /* + * Create Vectors of Path Directions for each interval + * interval_vector_xy = {waypoint_1 - waypoint_0}_xy + * */ + + // Prepare vector of projection distance values; projection of vehicle vectors onto the intervals + std::vector projection_distances_ds; // + + auto f_projection_dist = [this](auto pose_1, auto pose_0) { + // Vector of intervals. + std::vector int_vec{ + pose_1.position.x - pose_0.position.x, pose_1.position.y - pose_0.position.y}; + + // Compute the magnitude of path interval vector + double ds_mag = std::hypot(int_vec[0], int_vec[1]); + + // Vector to vehicle from the origin waypoints. + std::vector vehicle_vec{ + this->current_vec_pose_ptr_->position.x - pose_0.position.x, + this->current_vec_pose_ptr_->position.y - pose_0.position.y}; + + double projection_distance_onto_interval; + projection_distance_onto_interval = + (int_vec[0] * vehicle_vec[0] + int_vec[1] * vehicle_vec[1]) / ds_mag; + + return projection_distance_onto_interval; + }; + + // Fill the projection_distances vector. + std::transform( + current_waypoints_ptr_->poses.cbegin() + 1, current_waypoints_ptr_->poses.cend(), + current_waypoints_ptr_->poses.cbegin(), std::back_inserter(projection_distances_ds), + f_projection_dist); + + // Lambda function to replace negative numbers with a large number. + auto fnc_check_if_negative = [](auto x) { + return x < 0 ? std::numeric_limits::max() : x; + }; + + std::vector projections_distances_all_positive; + std::transform( + projection_distances_ds.cbegin(), projection_distances_ds.cend(), + std::back_inserter(projections_distances_all_positive), fnc_check_if_negative); + + // Minimum of all positive distances and the index of the next waypoint. + auto it = std::min_element( + projections_distances_all_positive.cbegin(), projections_distances_all_positive.cend()); + + // Extract the location of iterator idx and store in the class. + int32_t && temp_idx_prev_wp_ = std::distance(projections_distances_all_positive.cbegin(), it); + idx_prev_wp_ = std::make_unique(temp_idx_prev_wp_); + + // Distance of next waypoint to the vehicle, for anomaly detection. + double min_distance_ds = projections_distances_all_positive[*idx_prev_wp_]; + int32_t length_of_trajectory = + std::distance(current_waypoints_ptr_->poses.cbegin(), current_waypoints_ptr_->poses.cend()); + + // Find and set the waypoint L-wheelbase meters ahead of the current waypoint. + findCurveRefIdx(); + + return ((min_distance_ds <= acceptable_min_distance) & (*idx_prev_wp_ >= 0) & + (*idx_prev_wp_ <= length_of_trajectory)) + ? std::make_pair(true, *idx_prev_wp_) + : std::make_pair(false, std::numeric_limits::quiet_NaN()); +} + +bool ControlPerformanceAnalysisCore::isDataReady() const +{ + rclcpp::Clock clock{RCL_ROS_TIME}; + if (!current_vec_pose_ptr_) { + RCLCPP_WARN_THROTTLE( + logger_, clock, 1000, "cannot get current pose into control_performance algorithm"); + return false; + } + + if (!current_waypoints_ptr_) { + RCLCPP_WARN_THROTTLE(logger_, clock, 1000, "cannot get current trajectory waypoints ..."); + return false; + } + + if (!current_velocities_ptr_) { + RCLCPP_WARN_THROTTLE(logger_, clock, 1000, "waiting for current_velocity ..."); + return false; + } + + if (!current_control_ptr_) { + RCLCPP_WARN_THROTTLE(logger_, clock, 1000, "waiting for current_steering ..."); + return false; + } + + return true; +} + +std::pair ControlPerformanceAnalysisCore::getPerformanceVars() +{ + constexpr double EPS = std::numeric_limits::epsilon(); + + // Check if data is ready. + if (!isDataReady() || !idx_prev_wp_) { + return std::make_pair(false, TargetPerformanceMsgVars{}); + } + + TargetPerformanceMsgVars target_vars{}; + + // Get the interpolated pose + std::pair pair_pose_interp_wp_ = calculateClosestPose(); + + if (!pair_pose_interp_wp_.first || !interpolated_pose_ptr_) { + RCLCPP_WARN_THROTTLE( + logger_, clock_, 1000, "Cannot get interpolated pose into control_performance algorithm"); + + return std::make_pair(false, TargetPerformanceMsgVars{}); + } + + auto && pose_interp_wp_ = pair_pose_interp_wp_.second; + + // Create interpolated waypoint vector + std::vector interp_waypoint_xy{pose_interp_wp_.position.x, pose_interp_wp_.position.y}; + + // Create vehicle position vector + std::vector vehicle_position_xy{ + current_vec_pose_ptr_->position.x, current_vec_pose_ptr_->position.y}; + + // Get Yaw angles of the reference waypoint and the vehicle + double const target_yaw = tf2::getYaw(pose_interp_wp_.orientation); + double vehicle_yaw_angle = tf2::getYaw(current_vec_pose_ptr_->orientation); + + // Compute Curvature at the point where the front axle might follow + // get the waypoint corresponds to the front_axle center + + if (!idx_curve_ref_wp_) { + RCLCPP_ERROR(logger_, "Cannot find index of curvature reference waypoint "); + return std::make_pair(false, TargetPerformanceMsgVars{}); + } + + double curvature_est = estimateCurvature(); // three point curvature + double curvature_est_pp = estimatePurePursuitCurvature(); // pure pursuit curvature + + // Compute lateral error - projection of vector to waypoint from vehicle onto the vehicle normal. + double && lateral_error = + utils::computeLateralError(interp_waypoint_xy, vehicle_position_xy, vehicle_yaw_angle); + + // Compute the yaw angle error. + double && heading_yaw_error = utils::angleDistance(vehicle_yaw_angle, target_yaw); + + // Set the values of TargetPerformanceMsgVars. + target_vars.lateral_error = lateral_error; + target_vars.heading_error = heading_yaw_error; + + double steering_val = current_control_ptr_->steering_tire_angle; + target_vars.control_effort_energy = contR * steering_val * steering_val; // u*R*u'; + + Eigen::Vector2d error_vec; + error_vec << lateral_error, heading_yaw_error; + + target_vars.error_energy = error_vec.dot(error_vec); + target_vars.value_approximation = error_vec.transpose() * lyap_P_ * error_vec; // x'Px + + double prev_value_approximation = prev_target_vars_->value_approximation; + double && value_decay = target_vars.value_approximation - prev_value_approximation; + + target_vars.curvature_estimate = curvature_est; + target_vars.curvature_estimate_pp = curvature_est_pp; + + double Vx = current_velocities_ptr_->at(0); + target_vars.lateral_error_velocity = Vx * sin(heading_yaw_error); + target_vars.lateral_error_acceleration = Vx * tan(steering_val) / wheelbase_ - curvature_est * Vx; + + prev_target_vars_ = std::move(std::make_unique(target_vars)); + + return std::make_pair(true, target_vars); +} + +Pose ControlPerformanceAnalysisCore::getPrevWPPose() const +{ + Pose pose_ref_waypoint_ = current_waypoints_ptr_->poses.at(*idx_prev_wp_); + return pose_ref_waypoint_; +} +void ControlPerformanceAnalysisCore::setCurrentVelocities(const Twist & twist_msg) +{ + current_velocities_ptr_->at(0) = twist_msg.linear.x; + current_velocities_ptr_->at(1) = twist_msg.angular.z; +} +void ControlPerformanceAnalysisCore::findCurveRefIdx() +{ + // Get the previous waypoint as the reference + if (!interpolated_pose_ptr_) { + RCLCPP_WARN_THROTTLE( + logger_, clock_, 1000, "Cannot set the curvature_idx, no valid interpolated pose ..."); + + return; + } + + auto fun_distance_cond = [this](auto pose_t) { + double dist = std::hypot( + pose_t.position.x - this->interpolated_pose_ptr_->position.x, + pose_t.position.y - this->interpolated_pose_ptr_->position.y); + + return dist > wheelbase_; + }; + + auto it = std::find_if( + current_waypoints_ptr_->poses.cbegin() + *idx_prev_wp_, current_waypoints_ptr_->poses.cend(), + fun_distance_cond); + + if (it == current_waypoints_ptr_->poses.cend()) { + it = std::prev(it); + } + + int32_t && temp_idx_curve_ref_wp = std::distance(current_waypoints_ptr_->poses.cbegin(), it); + idx_curve_ref_wp_ = std::make_unique(temp_idx_curve_ref_wp); +} + +std::pair ControlPerformanceAnalysisCore::calculateClosestPose() +{ + Pose interpolated_pose; + + // Get index of prev waypoint and sanity check. + if (!idx_prev_wp_) { + RCLCPP_ERROR(logger_, "Cannot find the next waypoint."); + return std::make_pair(false, interpolated_pose); + } + + // Define the next waypoint - so that we can define an interval in which the car follow a line. + int32_t idx_next_wp_temp; + int32_t total_num_of_waypoints_in_traj = + std::distance(current_waypoints_ptr_->poses.cbegin(), current_waypoints_ptr_->poses.cend()); + + if (*idx_prev_wp_ < total_num_of_waypoints_in_traj - 1) { + idx_next_wp_temp = *idx_prev_wp_ + 1; + + } else { + idx_next_wp_temp = total_num_of_waypoints_in_traj - 1; + } + + idx_next_wp_ = std::make_unique(idx_next_wp_temp); + + /* + * Create two vectors originating from the previous waypoints to the next waypoint and the + * vehicle position and find projection of vehicle vector on the the trajectory section, + * + * */ + + // First get te yaw angles of all three poses. + double prev_yaw = tf2::getYaw(current_waypoints_ptr_->poses.at(*idx_prev_wp_).orientation); + double next_yaw = tf2::getYaw(current_waypoints_ptr_->poses.at(*idx_next_wp_).orientation); + + // Previous waypoint to next waypoint. + double dx_prev2next = current_waypoints_ptr_->poses.at(*idx_next_wp_).position.x - + current_waypoints_ptr_->poses.at(*idx_prev_wp_).position.x; + + double dy_prev2next = current_waypoints_ptr_->poses.at(*idx_next_wp_).position.y - + current_waypoints_ptr_->poses.at(*idx_prev_wp_).position.y; + + double delta_psi_prev2next = utils::angleDistance(next_yaw, prev_yaw); + + // Create a vector from p0 (prev) --> p1 (to next wp) + std::vector v_prev2next_wp{dx_prev2next, dy_prev2next}; + + // Previous waypoint to the vehicle pose + /* + * p0:previous waypoint ----> p1 next waypoint + * vector = p1 - p0. We project vehicle vector on this interval to + * + * */ + + double dx_prev2vehicle = + current_vec_pose_ptr_->position.x - current_waypoints_ptr_->poses.at(*idx_prev_wp_).position.x; + + double dy_prev2vehicle = + current_vec_pose_ptr_->position.y - current_waypoints_ptr_->poses.at(*idx_prev_wp_).position.y; + + // Vector from p0 to p_vehicle + std::vector v_prev2vehicle{dx_prev2vehicle, dy_prev2vehicle}; + + // Compute the length of v_prev2next_wp : vector from p0 --> p1 + double distance_p02p1 = std::hypot(dx_prev2next, dy_prev2next); + + // Compute how far the car is away from p0 in p1 direction. p_interp is the location of the + // interpolated waypoint. This is the dot product normalized by the length of the interval. + // a.b = |a|.|b|.cos(alpha) -- > |a|.cos(alpha) = a.b / |b| where b is the path interval, + + double distance_p02p_interp = + (dx_prev2next * dx_prev2vehicle + dy_prev2next * dy_prev2vehicle) / distance_p02p1; + + /* + * We use the following linear interpolation + * pi = p0 + ratio_t * (p1 - p0) + * */ + + double ratio_t = distance_p02p_interp / distance_p02p1; + + // Interpolate pose.position and pose.orientation + interpolated_pose.position.x = + current_waypoints_ptr_->poses.at(*idx_prev_wp_).position.x + ratio_t * dx_prev2next; + + interpolated_pose.position.y = + current_waypoints_ptr_->poses.at(*idx_prev_wp_).position.y + ratio_t * dy_prev2next; + + interpolated_pose.position.z = 0.0; + + // Interpolate the yaw angle of pi : interpolated waypoint + double interp_yaw_angle = prev_yaw + ratio_t * delta_psi_prev2next; + + Quaternion orient_msg = utils::createOrientationMsgFromYaw(interp_yaw_angle); + interpolated_pose.orientation = orient_msg; + + setInterpolatedPose(interpolated_pose); + + return std::make_pair(true, interpolated_pose); +} + +// Sets interpolated waypoint_ptr_. +void ControlPerformanceAnalysisCore::setInterpolatedPose(Pose & interpolated_pose) +{ + interpolated_pose_ptr_ = std::make_shared(interpolated_pose); +} + +double ControlPerformanceAnalysisCore::estimateCurvature() +{ + // Get idx of front-axle center reference point on the trajectory. + // get the waypoint corresponds to the front_axle center. + Pose front_axleWP_pose = current_waypoints_ptr_->poses.at(*idx_curve_ref_wp_); + + // for guarding -1 in finding previous waypoint for the front axle + int32_t idx_prev_waypoint = *idx_curve_ref_wp_ >= 1 ? *idx_curve_ref_wp_ - 1 : *idx_curve_ref_wp_; + + Pose front_axleWP_pose_prev = current_waypoints_ptr_->poses.at(idx_prev_waypoint); + + // Compute arc-length ds between 2 points. + double ds_arc_length = std::hypot( + front_axleWP_pose_prev.position.x - front_axleWP_pose.position.x, + front_axleWP_pose_prev.position.y - front_axleWP_pose.position.y); + + auto distance_to_traj0 = idx_prev_waypoint * ds_arc_length; + + // Define waypoints 10 meters behind the rear axle if exist. + // If not exist, we will take the first point of the + // curvature triangle as the start point of the trajectory. + auto && num_of_back_indices = std::round(curvature_interval_length_ / ds_arc_length); + int32_t loc_of_back_idx = + (*idx_curve_ref_wp_ - num_of_back_indices < 0) ? 0 : *idx_curve_ref_wp_ - num_of_back_indices; + + // Define location of forward point 10 meters ahead of the front axle on curve. + uint32_t max_idx = + std::distance(current_waypoints_ptr_->poses.cbegin(), current_waypoints_ptr_->poses.cend()); + + auto num_of_forward_indices = num_of_back_indices; + int32_t loc_of_forward_idx = (*idx_curve_ref_wp_ + num_of_forward_indices > max_idx) + ? max_idx - 1 + : *idx_curve_ref_wp_ + num_of_forward_indices - 1; + + // We have three indices of the three trajectory poses. + // We compute a curvature estimate from these points. + + std::array a_coord{ + current_waypoints_ptr_->poses.at(loc_of_back_idx).position.x, + current_waypoints_ptr_->poses.at(loc_of_back_idx).position.y}; + + std::array b_coord{ + current_waypoints_ptr_->poses.at(*idx_curve_ref_wp_).position.x, + current_waypoints_ptr_->poses.at(*idx_curve_ref_wp_).position.y}; + + std::array c_coord{ + current_waypoints_ptr_->poses.at(loc_of_forward_idx).position.x, + current_waypoints_ptr_->poses.at(loc_of_forward_idx).position.y}; + + double estimated_curvature = utils::curvatureFromThreePoints(a_coord, b_coord, c_coord); + + return estimated_curvature; +} + +double ControlPerformanceAnalysisCore::estimatePurePursuitCurvature() +{ + if (!interpolated_pose_ptr_) { + RCLCPP_WARN_THROTTLE( + logger_, clock_, 1000, + "Cannot set pure pursuit_target_point_idx, no valid interpolated pose ..."); + + return 0; + } + + double Vx = current_velocities_ptr_->at(0); + double look_ahead_distance_pp = std::max(wheelbase_, 2 * Vx); + + auto fun_distance_cond = [this, &look_ahead_distance_pp](auto pose_t) { + double dist = std::hypot( + pose_t.position.x - this->interpolated_pose_ptr_->position.x, + pose_t.position.y - this->interpolated_pose_ptr_->position.y); + + return dist > look_ahead_distance_pp; + }; + + auto it = std::find_if( + current_waypoints_ptr_->poses.cbegin() + *idx_prev_wp_, current_waypoints_ptr_->poses.cend(), + fun_distance_cond); + + Pose target_pose_pp; + + // If there is no waypoint left on the trajectory, interpolate one. + if (it == current_waypoints_ptr_->poses.cend()) { + // Interpolate a waypoint. + it = std::prev(it); + int32_t && temp_idx_pp = std::distance(current_waypoints_ptr_->poses.cbegin(), it); + Pose const & last_pose_on_traj = current_waypoints_ptr_->poses.at(temp_idx_pp); + + // get the yaw angle of the last traj point. + double const && yaw_pp = tf2::getYaw(last_pose_on_traj.orientation); + + // get unit tangent in this direction. + std::vector unit_tangent = utils::getTangentVector(yaw_pp); + + target_pose_pp.position.z = 0; + target_pose_pp.position.x = + last_pose_on_traj.position.x + look_ahead_distance_pp * unit_tangent[0]; + target_pose_pp.position.y = + last_pose_on_traj.position.y + look_ahead_distance_pp * unit_tangent[1]; + + target_pose_pp.orientation = last_pose_on_traj.orientation; + } else { + // idx of the last waypoint on the trajectory is + int32_t && temp_idx_pp = std::distance(current_waypoints_ptr_->poses.cbegin(), it); + Pose const & last_pose_on_traj = current_waypoints_ptr_->poses.at(temp_idx_pp); + + target_pose_pp.position.z = last_pose_on_traj.position.z; + target_pose_pp.position.x = last_pose_on_traj.position.x; + target_pose_pp.position.y = last_pose_on_traj.position.y; + target_pose_pp.orientation = last_pose_on_traj.orientation; + } + + // We have target pose for the pure pursuit. + // Find projection of target vector from vehicle. + + std::vector vec_to_target{ + target_pose_pp.position.x - current_vec_pose_ptr_->position.x, + target_pose_pp.position.y - current_vec_pose_ptr_->position.y}; + + double const && current_vec_yaw = tf2::getYaw(current_vec_pose_ptr_->orientation); + std::vector normal_vec = utils::getNormalVector(current_vec_yaw); // ClockWise + + // Project this vector on the vehicle normal vector. + double x_pure_pursuit = vec_to_target[0] * normal_vec[0] + vec_to_target[1] * normal_vec[1]; + + // Pure pursuit curvature. + double curvature_pure_pursuit = + 2 * x_pure_pursuit / (look_ahead_distance_pp * look_ahead_distance_pp); + + return curvature_pure_pursuit; +} +} // namespace control_performance_analysis diff --git a/control/control_performance_analysis/src/control_performance_analysis_node.cpp b/control/control_performance_analysis/src/control_performance_analysis_node.cpp new file mode 100644 index 0000000000000..2a48ca4971ce9 --- /dev/null +++ b/control/control_performance_analysis/src/control_performance_analysis_node.cpp @@ -0,0 +1,259 @@ +// Copyright 2021 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. + +#include "control_performance_analysis/control_performance_analysis_node.hpp" + +#include "control_performance_analysis/msg/error_stamped.hpp" + +#include + +#include +#include + +namespace +{ +using control_performance_analysis::TargetPerformanceMsgVars; +using control_performance_analysis::msg::ErrorStamped; + +ErrorStamped createPerformanceMsgVars(const TargetPerformanceMsgVars & target_performance_vars) +{ + ErrorStamped error_msgs{}; + + error_msgs.error.lateral_error = target_performance_vars.lateral_error; + error_msgs.error.heading_error = target_performance_vars.heading_error; + error_msgs.error.control_effort_energy = target_performance_vars.control_effort_energy; + error_msgs.error.error_energy = target_performance_vars.error_energy; + error_msgs.error.value_approximation = target_performance_vars.value_approximation; + error_msgs.error.curvature_estimate = target_performance_vars.curvature_estimate; + error_msgs.error.curvature_estimate_pp = target_performance_vars.curvature_estimate_pp; + error_msgs.error.lateral_error_velocity = target_performance_vars.lateral_error_velocity; + error_msgs.error.lateral_error_acceleration = target_performance_vars.lateral_error_acceleration; + + return error_msgs; +} +} // namespace + +namespace control_performance_analysis +{ +using vehicle_info_util::VehicleInfoUtil; + +ControlPerformanceAnalysisNode::ControlPerformanceAnalysisNode( + const rclcpp::NodeOptions & node_options) +: Node("control_performance_analysis", node_options) +{ + using std::placeholders::_1; + + // Implement Reading Global and Local Variables. + const auto vehicle_info = VehicleInfoUtil(*this).getVehicleInfo(); + param_.wheel_base = vehicle_info.wheel_base_m; + + // Node Parameters. + param_.control_period = declare_parameter("control_period", 0.033); + param_.curvature_interval_length = declare_parameter("curvature_interval_length", 10.0); + + // Prepare error computation class with the wheelbase parameter. + control_performance_core_ptr_ = std::make_unique( + param_.wheel_base, param_.curvature_interval_length); + + // Subscribers. + sub_trajectory_ = create_subscription( + "~/input/reference_trajectory", 1, + std::bind(&ControlPerformanceAnalysisNode::onTrajectory, this, _1)); + + sub_control_steering_ = create_subscription( + "~/input/control_raw", 1, std::bind(&ControlPerformanceAnalysisNode::onControlRaw, this, _1)); + + sub_vehicle_steering_ = create_subscription( + "~/input/measured_steering", 1, + std::bind(&ControlPerformanceAnalysisNode::onVecSteeringMeasured, this, _1)); + + sub_velocity_ = create_subscription( + "~/input/odometry", 1, std::bind(&ControlPerformanceAnalysisNode::onVelocity, this, _1)); + + // Publishers + pub_error_msg_ = create_publisher("~/output/error_stamped", 1); + + // Timer + { + auto on_timer = std::bind(&ControlPerformanceAnalysisNode::onTimer, this); + auto period = std::chrono::duration_cast( + std::chrono::duration(param_.control_period)); + timer_publish_ = std::make_shared>( + this->get_clock(), period, std::move(on_timer), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(timer_publish_, nullptr); + } + + // Wait for first self pose + self_pose_listener_.waitForFirstPose(); +} + +void ControlPerformanceAnalysisNode::onTrajectory(const Trajectory::ConstSharedPtr msg) +{ + if (msg->points.size() < 3) { + RCLCPP_DEBUG(get_logger(), "received path size < 3, is not sufficient."); + return; + } + + if (!isValidTrajectory(*msg)) { + RCLCPP_ERROR(get_logger(), "Trajectory is invalid!, stop computing."); + return; + } + + current_trajectory_ptr_ = msg; +} + +void ControlPerformanceAnalysisNode::onControlRaw( + const AckermannLateralCommand::ConstSharedPtr control_msg) +{ + if (!control_msg) { + RCLCPP_ERROR(get_logger(), "steering signal has not been received yet ..."); + return; + } + current_control_msg_ptr_ = control_msg; +} + +void ControlPerformanceAnalysisNode::onVecSteeringMeasured( + const SteeringReport::ConstSharedPtr meas_steer_msg) +{ + if (!meas_steer_msg) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 1000, "waiting for vehicle measured steering message ..."); + return; + } + current_vec_steering_msg_ptr_ = meas_steer_msg; +} + +void ControlPerformanceAnalysisNode::onVelocity(const Odometry::ConstSharedPtr msg) +{ + current_odom_ptr_ = msg; +} + +void ControlPerformanceAnalysisNode::onTimer() +{ + // Read and Update Current Pose updating var:current_pose_. + current_pose_ = self_pose_listener_.getCurrentPose(); + + // Check Data Stream + if (!isDataReady()) { + // Publish Here + return; + } + + // Compute Control Performance Variables. + auto performanceVars = computeTargetPerformanceMsgVars(); + if (!performanceVars) { + RCLCPP_ERROR(get_logger(), "steering signal has not been received yet ..."); + return; + } + + // If successful publish. + publishErrorMsg(*performanceVars); +} + +void ControlPerformanceAnalysisNode::publishErrorMsg( + const TargetPerformanceMsgVars & control_performance_vars) +{ + control_performance_analysis::ErrorStamped error_msgs = + createPerformanceMsgVars(control_performance_vars); + + pub_error_msg_->publish(error_msgs); +} + +bool ControlPerformanceAnalysisNode::isDataReady() const +{ + rclcpp::Clock clock{RCL_ROS_TIME}; + if (!current_pose_) { + RCLCPP_WARN_THROTTLE(get_logger(), clock, 1000, "waiting for current_pose ..."); + return false; + } + + if (!current_trajectory_ptr_) { + RCLCPP_WARN_THROTTLE(get_logger(), clock, 1000, "waiting for trajectory ... "); + return false; + } + + if (!current_odom_ptr_) { + RCLCPP_WARN_THROTTLE(get_logger(), clock, 1000, "waiting for current_odom ..."); + return false; + } + + if (!current_control_msg_ptr_) { + RCLCPP_WARN_THROTTLE(get_logger(), clock, 1000, "waiting for current_control_steering_val ..."); + return false; + } + + return true; +} + +/* + * - Pass trajectory and current pose to control_performance_analysis -> setCurrentPose() + * -> setWayPoints() + * -> findClosestPoint + * -> computePerformanceVars + * */ + +boost::optional +ControlPerformanceAnalysisNode::computeTargetPerformanceMsgVars() const +{ + // Set trajectory and current pose of controller_performance_core. + control_performance_core_ptr_->setCurrentWaypoints(*current_trajectory_ptr_); + control_performance_core_ptr_->setCurrentPose(current_pose_->pose); + control_performance_core_ptr_->setCurrentVelocities(current_odom_ptr_->twist.twist); + control_performance_core_ptr_->setCurrentControlValue(*current_control_msg_ptr_); + + // Find the index of the next waypoint. + std::pair prev_closest_wp_pose_idx = + control_performance_core_ptr_->findClosestPrevWayPointIdx_path_direction(); + + if (!prev_closest_wp_pose_idx.first) { + RCLCPP_ERROR(get_logger(), "Cannot find closest waypoint"); + return {}; + } + + // Compute control performance values. + const std::pair target_performance_vars = + control_performance_core_ptr_->getPerformanceVars(); + + if (!target_performance_vars.first) { + RCLCPP_ERROR(get_logger(), "Cannot compute control performance vars ..."); + return {}; + } + + return target_performance_vars.second; +} + +bool ControlPerformanceAnalysisNode::isValidTrajectory(const Trajectory & traj) +{ + bool check_condition = std::all_of(traj.points.cbegin(), traj.points.cend(), [](auto point) { + const auto & p = point.pose.position; + const auto & o = point.pose.orientation; + const auto & t = point.longitudinal_velocity_mps; + const auto & a = point.acceleration_mps2; + + if ( + !isfinite(p.x) || !isfinite(p.y) || !isfinite(p.z) || !isfinite(o.x) || !isfinite(o.y) || + !isfinite(o.z) || !isfinite(o.w) || !isfinite(t) || !isfinite(a)) { + return false; + } else { + return true; + } + }); + + return check_condition; +} +} // namespace control_performance_analysis + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(control_performance_analysis::ControlPerformanceAnalysisNode) diff --git a/control/control_performance_analysis/src/control_performance_analysis_utils.cpp b/control/control_performance_analysis/src/control_performance_analysis_utils.cpp new file mode 100644 index 0000000000000..ba4ae2d194701 --- /dev/null +++ b/control/control_performance_analysis/src/control_performance_analysis_utils.cpp @@ -0,0 +1,52 @@ +// Copyright 2021 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. + +#include "control_performance_analysis/control_performance_analysis_utils.hpp" + +#include + +namespace control_performance_analysis +{ +namespace utils +{ +double determinant(std::array const & a, std::array const & b) +{ + return a[0] * b[1] - b[0] * a[1]; +} + +double triangleArea( + std::array const & a, std::array const & b, std::array const & c) +{ + double m1 = determinant(a, b); + double m2 = determinant(b, c); + double m3 = determinant(c, a); + + return 0.5 * (m1 + m2 + m3); +} + +double curvatureFromThreePoints( + std::array const & a, std::array const & b, std::array const & c) +{ + double area = triangleArea(a, b, c); + + double amag = std::hypot(a[0] - b[0], a[1] - b[1]); // magnitude of triangle edges + double bmag = std::hypot(b[0] - c[0], b[1] - c[1]); + double cmag = std::hypot(c[0] - a[0], c[1] - a[1]); + + double curvature = 4 * area / std::max(amag * bmag * cmag, 1e-4); + + return curvature; +} +} // namespace utils +} // namespace control_performance_analysis