From dc93ca4ae3634bfbb06613e1f5b4dd983a590141 Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Thu, 21 Oct 2021 19:27:49 +0900 Subject: [PATCH 01/10] Feature/porting control performance analysis (#1671) * Feature/control performance analysis (#1212) * First commit of kinematic_controller * First commit. * second commit * Just setup updated Autoware. * changed package name. * Messages variables are created. * Writing subscribers and publishers. * Writing subscribers. Traj, pose and control_values are read into the node. * Computing control performance variables. * Computing control performance variables. * Current velocity subscribed. * Acceleration performance is computed. * Publishing completed. Will start rqt_multiplot * Publishing completed. Decay rate fixed. Will start rqt_multiplot * rqt_multiplot first configuration. * Update pure_pursuit.launch * Update pure_pursuit.launch * Update .gitignore * Update Error.msg * Update control_performance_utils.cpp * Update ErrorStamped.msg * Update package.xml * rqt_multiplot first configuration. * Update controller_performance_core.cpp * Update controller_performance_core.cpp * Update CMakeLists.txt * Update control_performance_analysis_param.yaml * EPS is added for value_decay_rate. * There is a bug. * Bug removed. * Bug removed. * lateral_acceleration is published. * Interpolated pose is added. * Update controller_performance_node.cpp * find Curve index bug is removed. * dot product on projection is updated. * Vehicle measured steering is included in the node and rqt_graph. * Review will be requested. * After the test: Three point curvature module is added. Std:vector will be fixed. * After the test: Curvature plot is added. * After the test: Fine tuned. * rqt curvature is modified. * Pure pursuit curvature is implemented and tested. Results are fine. * addressed some code review issues. Will replace get_pose. * GetPose is removed. * All the core review issues have been addressed. * Rename files Signed-off-by: wep21 * Porting control performance analysis Signed-off-by: wep21 * Apply lint Signed-off-by: wep21 * Add boost dependency for optional Signed-off-by: wep21 * Remove confusing abbreviation Signed-off-by: wep21 * Fix dependency in packages.xml Signed-off-by: wep21 * Add missing new line Signed-off-by: wep21 * Add comment for eigen macro Signed-off-by: wep21 * pre-commit fixes Signed-off-by: wep21 Co-authored-by: Ali BOYALI --- .../CMakeLists.txt | 59 ++ .../control_performance_analysis.param.yaml | 5 + .../config/error_rqt_multiplot.xml | 853 ++++++++++++++++++ .../control_performance_analysis_core.hpp | 104 +++ .../control_performance_analysis_node.hpp | 104 +++ .../control_performance_analysis_utils.hpp | 105 +++ ...controller_performance_analysis.launch.xml | 23 + .../msg/Error.msg | 9 + .../msg/ErrorStamped.msg | 2 + .../control_performance_analysis/package.xml | 38 + .../src/control_performance_analysis_core.cpp | 533 +++++++++++ .../src/control_performance_analysis_node.cpp | 275 ++++++ .../control_performance_analysis_utils.cpp | 53 ++ 13 files changed, 2163 insertions(+) create mode 100644 control/control_performance_analysis/CMakeLists.txt create mode 100644 control/control_performance_analysis/config/control_performance_analysis.param.yaml create mode 100644 control/control_performance_analysis/config/error_rqt_multiplot.xml create mode 100644 control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp create mode 100644 control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp create mode 100644 control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_utils.hpp create mode 100644 control/control_performance_analysis/launch/controller_performance_analysis.launch.xml create mode 100644 control/control_performance_analysis/msg/Error.msg create mode 100644 control/control_performance_analysis/msg/ErrorStamped.msg create mode 100644 control/control_performance_analysis/package.xml create mode 100644 control/control_performance_analysis/src/control_performance_analysis_core.cpp create mode 100644 control/control_performance_analysis/src/control_performance_analysis_node.cpp create mode 100644 control/control_performance_analysis/src/control_performance_analysis_utils.cpp 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/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..c427d308f02cf --- /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 + autoware_vehicle_msgs/Steering + + + data + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /vehicle/status/steering + 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/twist + geometry_msgs/TwistStamped + + + twist/linear/x + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /vehicle/status/twist + 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..7d4136c23cd8d --- /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 +#include +#include + +#include "eigen3/Eigen/Core" + +#include "autoware_control_msgs/msg/control_command_stamped.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/pose_array.hpp" +#include "geometry_msgs/msg/twist.hpp" + +#include "control_performance_analysis/control_performance_analysis_utils.hpp" + +namespace control_performance_analysis +{ +using autoware_control_msgs::msg::ControlCommandStamped; +using autoware_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 setCurrentControValue(const ControlCommandStamped & 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..fd852bf4f60ab --- /dev/null +++ b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.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_NODE_HPP_ +#define CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_NODE_HPP_ + +#include + +#include "boost/optional.hpp" + +#include "autoware_control_msgs/msg/control_command_stamped.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_utils/ros/self_pose_listener.hpp" +#include "autoware_vehicle_msgs/msg/steering.hpp" +#include "control_performance_analysis/msg/error_stamped.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "control_performance_analysis/control_performance_analysis_core.hpp" + +namespace control_performance_analysis +{ +using autoware_control_msgs::msg::ControlCommandStamped; +using autoware_planning_msgs::msg::Trajectory; +using autoware_vehicle_msgs::msg::Steering; +using control_performance_analysis::msg::ErrorStamped; +using geometry_msgs::msg::PoseStamped; +using geometry_msgs::msg::TwistStamped; + +// 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 ControlCommandStamped::ConstSharedPtr control_msg); + void onVecSteeringMeasured(const Steering::ConstSharedPtr meas_steer_msg); + void onVelocity(const TwistStamped::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. + ControlCommandStamped::ConstSharedPtr current_control_msg_ptr_; + Steering::ConstSharedPtr current_vec_steering_msg_ptr_; + TwistStamped::ConstSharedPtr current_velocity_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..575c53e9c3dfc --- /dev/null +++ b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_utils.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_UTILS_HPP_ +#define CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_UTILS_HPP_ + +#include +#include + +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/Geometry" + +#include "geometry_msgs/msg/quaternion.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2/utils.h" + +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..ccb374d4b0058 --- /dev/null +++ b/control/control_performance_analysis/launch/controller_performance_analysis.launch.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + 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..b6a320d051ea2 --- /dev/null +++ b/control/control_performance_analysis/package.xml @@ -0,0 +1,38 @@ + + + + control_performance_analysis + 0.1.0 + Controller Performance Evaluation + Ali Boyali + Apache License 2.0 + + ament_cmake_auto + rosidl_default_generators + + autoware_planning_msgs + autoware_control_msgs + autoware_vehicle_msgs + autoware_utils + geometry_msgs + libboost-dev + rclcpp + sensor_msgs + std_msgs + tf2 + tf2_eigen + tf2_ros + vehicle_info_util + + autoware_global_parameter_loader + rosidl_default_runtime + + ament_lint_auto + ament_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..c16d0653cda03 --- /dev/null +++ b/control/control_performance_analysis/src/control_performance_analysis_core.cpp @@ -0,0 +1,533 @@ +// 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 +#include +#include +#include +#include + +#include "control_performance_analysis/control_performance_analysis_core.hpp" + +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::setCurrentControValue( + const ControlCommandStamped & 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 vehcile 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_->control.steering_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 dpsi_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 * dpsi_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 current interpoint arc-length ds. + 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_purepursuit = vec_to_target[0] * normal_vec[0] + vec_to_target[1] * normal_vec[1]; + + // Pure pursuit curvature. + double curvature_purepursuit = + 2 * x_purepursuit / (look_ahead_distance_pp * look_ahead_distance_pp); + + return curvature_purepursuit; +} +} // 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..31ff5a5b6fbaa --- /dev/null +++ b/control/control_performance_analysis/src/control_performance_analysis_node.cpp @@ -0,0 +1,275 @@ +// 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 +#include + +#include "control_performance_analysis/msg/error_stamped.hpp" +#include "vehicle_info_util/vehicle_info_util.hpp" + +#include "control_performance_analysis/control_performance_analysis_node.hpp" + +namespace +{ +using control_performance_analysis::msg::ErrorStamped; +using control_performance_analysis::TargetPerformanceMsgVars; + +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/current_velocity", 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 ControlCommandStamped::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 Steering::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 TwistStamped::ConstSharedPtr msg) +{ + current_velocity_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_velocity_ptr_) { + RCLCPP_WARN_THROTTLE( + get_logger(), clock, 1000, + "waiting for current_velocity ..."); + 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_velocity_ptr_->twist); + control_performance_core_ptr_->setCurrentControValue(*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.twist.linear; + const auto & a = point.accel.linear; + + if ( + !isfinite(p.x) || !isfinite(p.y) || !isfinite(p.z) || !isfinite(o.x) || !isfinite(o.y) || + !isfinite(o.z) || !isfinite(o.w) || !isfinite(t.x) || !isfinite(t.y) || !isfinite(t.z) || + !isfinite(a.x) || !isfinite(a.y) || !isfinite(a.z)) + { + return false; + } else { + return true; + } + }); + + return check_condition; +} +} // namespace control_performance_analysis + +#include "rclcpp_components/register_node_macro.hpp" +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..80356fe0e8c6b --- /dev/null +++ b/control/control_performance_analysis/src/control_performance_analysis_utils.cpp @@ -0,0 +1,53 @@ +// 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 + +#include "control_performance_analysis/control_performance_analysis_utils.hpp" + +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 From 2b48b43e34653fe97a1212dde078895fa6b81e03 Mon Sep 17 00:00:00 2001 From: Kenji Miyake Date: Fri, 10 Sep 2021 14:07:00 +0900 Subject: [PATCH 02/10] Fix package.xml (#2056) Signed-off-by: Kenji Miyake --- control/control_performance_analysis/package.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/control/control_performance_analysis/package.xml b/control/control_performance_analysis/package.xml index b6a320d051ea2..090ae57f0a114 100644 --- a/control/control_performance_analysis/package.xml +++ b/control/control_performance_analysis/package.xml @@ -10,10 +10,10 @@ ament_cmake_auto rosidl_default_generators - autoware_planning_msgs autoware_control_msgs - autoware_vehicle_msgs + autoware_planning_msgs autoware_utils + autoware_vehicle_msgs geometry_msgs libboost-dev rclcpp From ce50ee25b3c9a89adeef3f62ee7132ca068a05c3 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 1 Nov 2021 21:36:25 +0900 Subject: [PATCH 03/10] Fix typo for control_performance_analysis (#2328) * fix typo * fix Contro -> Control * fix for spellcheck * fix --- .../control_performance_analysis_core.hpp | 2 +- .../control_performance_analysis_utils.hpp | 2 +- .../src/control_performance_analysis_core.cpp | 14 +++++++------- .../src/control_performance_analysis_node.cpp | 2 +- 4 files changed, 10 insertions(+), 10 deletions(-) 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 index 7d4136c23cd8d..e637edd503a3d 100644 --- 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 @@ -62,7 +62,7 @@ class ControlPerformanceAnalysisCore void setCurrentPose(const Pose & msg); void setCurrentWaypoints(const Trajectory & trajectory); void setCurrentVelocities(const Twist & twist_msg); - void setCurrentControValue(const ControlCommandStamped & msg); + void setCurrentControlValue(const ControlCommandStamped & msg); void setInterpolatedPose(Pose & interpolated_pose); void findCurveRefIdx(); 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 index 575c53e9c3dfc..98a964885712a 100644 --- 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 @@ -71,7 +71,7 @@ inline double angleDistance(double & target_angle, double const & reference_angl return -1.0 * diff_signed_correction; } -inline geometry_msgs::msg::Quaternion createOrientationMsgfromYaw(double yaw_angle) +inline geometry_msgs::msg::Quaternion createOrientationMsgFromYaw(double yaw_angle) { geometry_msgs::msg::Quaternion orientation_msg; double roll_angle = 0.0; diff --git a/control/control_performance_analysis/src/control_performance_analysis_core.cpp b/control/control_performance_analysis/src/control_performance_analysis_core.cpp index c16d0653cda03..3bd2ea7c66386 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_core.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_core.cpp @@ -55,7 +55,7 @@ void ControlPerformanceAnalysisCore::setCurrentPose(const Pose & msg) current_vec_pose_ptr_ = std::make_shared(msg); } -void ControlPerformanceAnalysisCore::setCurrentControValue( +void ControlPerformanceAnalysisCore::setCurrentControlValue( const ControlCommandStamped & msg) { current_control_ptr_ = std::make_shared(msg); @@ -73,7 +73,7 @@ std::pair ControlPerformanceAnalysisCore::findClosestPrevWayPoint * interval_vector_xy = {waypoint_1 - waypoint_0}_xy * */ - // Prepare vector of projection distance values; projection of vehcile vectors onto the intervals + // 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) { @@ -383,7 +383,7 @@ std::pair ControlPerformanceAnalysisCore::calculateClosestPose() double interp_yaw_angle = prev_yaw + ratio_t * dpsi_prev2next; Quaternion orient_msg = - utils::createOrientationMsgfromYaw(interp_yaw_angle); + utils::createOrientationMsgFromYaw(interp_yaw_angle); interpolated_pose.orientation = orient_msg; setInterpolatedPose(interpolated_pose); @@ -522,12 +522,12 @@ double ControlPerformanceAnalysisCore::estimatePurePursuitCurvature() utils::getNormalVector(current_vec_yaw); // ClockWise // Project this vector on the vehicle normal vector. - double x_purepursuit = vec_to_target[0] * normal_vec[0] + vec_to_target[1] * normal_vec[1]; + double x_pure_pursuit = vec_to_target[0] * normal_vec[0] + vec_to_target[1] * normal_vec[1]; // Pure pursuit curvature. - double curvature_purepursuit = - 2 * x_purepursuit / (look_ahead_distance_pp * look_ahead_distance_pp); + double curvature_pure_pursuit = + 2 * x_pure_pursuit / (look_ahead_distance_pp * look_ahead_distance_pp); - return curvature_purepursuit; + 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 index 31ff5a5b6fbaa..0e69bf7bf4e5e 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_node.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_node.cpp @@ -224,7 +224,7 @@ const control_performance_core_ptr_->setCurrentWaypoints(*current_trajectory_ptr_); control_performance_core_ptr_->setCurrentPose(current_pose_->pose); control_performance_core_ptr_->setCurrentVelocities(current_velocity_ptr_->twist); - control_performance_core_ptr_->setCurrentControValue(*current_control_msg_ptr_); + control_performance_core_ptr_->setCurrentControlValue(*current_control_msg_ptr_); // Find the index of the next waypoint. std::pair prev_closest_wp_pose_idx = From 733acec85d257eb769f09e5918d969e322fb4ae7 Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Tue, 2 Nov 2021 21:49:19 +0900 Subject: [PATCH 04/10] Change formatter to clang-format and black (#2332) * Revert "Temporarily comment out pre-commit hooks" This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3. * Replace ament_lint_common with autoware_lint_common Signed-off-by: Kenji Miyake * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake * Apply Black Signed-off-by: Kenji Miyake * Apply clang-format Signed-off-by: Kenji Miyake * Fix build errors Signed-off-by: Kenji Miyake * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake * Apply clang-format Signed-off-by: Kenji Miyake * Fix build errors Signed-off-by: Kenji Miyake --- .../control_performance_analysis_core.hpp | 20 +-- .../control_performance_analysis_node.hpp | 27 ++-- .../control_performance_analysis_utils.hpp | 23 +-- .../control_performance_analysis/package.xml | 2 +- .../src/control_performance_analysis_core.cpp | 136 ++++++++---------- .../src/control_performance_analysis_node.cpp | 90 +++++------- .../control_performance_analysis_utils.cpp | 7 +- 7 files changed, 139 insertions(+), 166 deletions(-) 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 index e637edd503a3d..859c19283949d 100644 --- 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 @@ -15,19 +15,19 @@ #ifndef CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_CORE_HPP_ #define CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_CORE_HPP_ -#include -#include -#include +#include "control_performance_analysis/control_performance_analysis_utils.hpp" -#include "eigen3/Eigen/Core" +#include -#include "autoware_control_msgs/msg/control_command_stamped.hpp" -#include "autoware_planning_msgs/msg/trajectory.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "geometry_msgs/msg/pose_array.hpp" -#include "geometry_msgs/msg/twist.hpp" +#include +#include +#include +#include +#include -#include "control_performance_analysis/control_performance_analysis_utils.hpp" +#include +#include +#include namespace control_performance_analysis { 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 index fd852bf4f60ab..a598f45c4b9a3 100644 --- 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 @@ -15,20 +15,21 @@ #ifndef CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_NODE_HPP_ #define CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_NODE_HPP_ -#include +#include "control_performance_analysis/control_performance_analysis_core.hpp" +#include "control_performance_analysis/msg/error_stamped.hpp" -#include "boost/optional.hpp" +#include +#include -#include "autoware_control_msgs/msg/control_command_stamped.hpp" -#include "autoware_planning_msgs/msg/trajectory.hpp" -#include "autoware_utils/ros/self_pose_listener.hpp" -#include "autoware_vehicle_msgs/msg/steering.hpp" -#include "control_performance_analysis/msg/error_stamped.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "rclcpp/rclcpp.hpp" +#include +#include +#include +#include +#include -#include "control_performance_analysis/control_performance_analysis_core.hpp" +#include + +#include namespace control_performance_analysis { @@ -57,10 +58,10 @@ class ControlPerformanceAnalysisNode : public rclcpp::Node private: // Subscribers and Local Variable Assignment - rclcpp::Subscription::SharedPtr sub_trajectory_; // subscribe to trajectory + 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_velocity_; // subscribe to velocity rclcpp::Subscription::SharedPtr sub_vehicle_steering_; // Self Pose listener. 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 index 98a964885712a..de05ea3da63f3 100644 --- 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 @@ -15,15 +15,16 @@ #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 "eigen3/Eigen/Core" -#include "eigen3/Eigen/Geometry" +#include -#include "geometry_msgs/msg/quaternion.hpp" -#include "rclcpp/rclcpp.hpp" -#include "tf2/utils.h" +#include +#include namespace control_performance_analysis { @@ -59,10 +60,10 @@ inline double computeLateralError( } /* - * 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. - * */ + * 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; diff --git a/control/control_performance_analysis/package.xml b/control/control_performance_analysis/package.xml index 090ae57f0a114..342d48ec92887 100644 --- a/control/control_performance_analysis/package.xml +++ b/control/control_performance_analysis/package.xml @@ -28,7 +28,7 @@ rosidl_default_runtime ament_lint_auto - ament_lint_common + autoware_lint_common rosidl_interface_packages diff --git a/control/control_performance_analysis/src/control_performance_analysis_core.cpp b/control/control_performance_analysis/src/control_performance_analysis_core.cpp index 3bd2ea7c66386..413c25039f939 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_core.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_core.cpp @@ -12,28 +12,26 @@ // 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 -#include "control_performance_analysis/control_performance_analysis_core.hpp" - namespace control_performance_analysis { using geometry_msgs::msg::Quaternion; -ControlPerformanceAnalysisCore::ControlPerformanceAnalysisCore() -: wheelbase_{2.74} +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) + double wheelbase, double curvature_interval_length) : wheelbase_{wheelbase}, curvature_interval_length_{curvature_interval_length} { // prepare control performance struct @@ -55,46 +53,47 @@ void ControlPerformanceAnalysisCore::setCurrentPose(const Pose & msg) current_vec_pose_ptr_ = std::make_shared(msg); } -void ControlPerformanceAnalysisCore::setCurrentControlValue( - const ControlCommandStamped & msg) +void ControlPerformanceAnalysisCore::setCurrentControlValue(const ControlCommandStamped & 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());} + 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 - * */ + * 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}; + // 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]); + // 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}; + // 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; + 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; - }; + return projection_distance_onto_interval; + }; // Fill the projection_distances vector. std::transform( @@ -104,8 +103,8 @@ std::pair ControlPerformanceAnalysisCore::findClosestPrevWayPoint // 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; - }; + return x < 0 ? std::numeric_limits::max() : x; + }; std::vector projections_distances_all_positive; std::transform( @@ -129,9 +128,9 @@ std::pair ControlPerformanceAnalysisCore::findClosestPrevWayPoint 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()); + (*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 @@ -139,29 +138,22 @@ 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"); + 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 ..."); + 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 ..."); + 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 ..."); + RCLCPP_WARN_THROTTLE(logger_, clock, 1000, "waiting for current_steering ..."); return false; } @@ -184,8 +176,7 @@ std::pair ControlPerformanceAnalysisCore::getPer if (!pair_pose_interp_wp_.first || !interpolated_pose_ptr_) { RCLCPP_WARN_THROTTLE( - logger_, clock_, 1000, - "Cannot get interpolated pose into control_performance algorithm"); + logger_, clock_, 1000, "Cannot get interpolated pose into control_performance algorithm"); return std::make_pair(false, TargetPerformanceMsgVars{}); } @@ -215,12 +206,11 @@ std::pair ControlPerformanceAnalysisCore::getPer 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); + 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); + double && heading_yaw_error = utils::angleDistance(vehicle_yaw_angle, target_yaw); // Set the values of TargetPerformanceMsgVars. target_vars.lateral_error = lateral_error; @@ -265,19 +255,18 @@ 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 ..."); + 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); + 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_; - }; + return dist > wheelbase_; + }; auto it = std::find_if( current_waypoints_ptr_->poses.cbegin() + *idx_prev_wp_, current_waypoints_ptr_->poses.cend(), @@ -316,10 +305,10 @@ std::pair ControlPerformanceAnalysisCore::calculateClosestPose() 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, - * - * */ + * 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); @@ -327,10 +316,10 @@ std::pair ControlPerformanceAnalysisCore::calculateClosestPose() // 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; + 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; + current_waypoints_ptr_->poses.at(*idx_prev_wp_).position.y; double dpsi_prev2next = utils::angleDistance(next_yaw, prev_yaw); @@ -382,8 +371,7 @@ std::pair ControlPerformanceAnalysisCore::calculateClosestPose() // Interpolate the yaw angle of pi : interpolated waypoint double interp_yaw_angle = prev_yaw + ratio_t * dpsi_prev2next; - Quaternion orient_msg = - utils::createOrientationMsgFromYaw(interp_yaw_angle); + Quaternion orient_msg = utils::createOrientationMsgFromYaw(interp_yaw_angle); interpolated_pose.orientation = orient_msg; setInterpolatedPose(interpolated_pose); @@ -427,9 +415,9 @@ double ControlPerformanceAnalysisCore::estimateCurvature() 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; + 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. @@ -446,8 +434,7 @@ double ControlPerformanceAnalysisCore::estimateCurvature() 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); + double estimated_curvature = utils::curvatureFromThreePoints(a_coord, b_coord, c_coord); return estimated_curvature; } @@ -466,12 +453,12 @@ double ControlPerformanceAnalysisCore::estimatePurePursuitCurvature() 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); + 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; - }; + return dist > look_ahead_distance_pp; + }; auto it = std::find_if( current_waypoints_ptr_->poses.cbegin() + *idx_prev_wp_, current_waypoints_ptr_->poses.cend(), @@ -518,8 +505,7 @@ double ControlPerformanceAnalysisCore::estimatePurePursuitCurvature() 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 + 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]; diff --git a/control/control_performance_analysis/src/control_performance_analysis_node.cpp b/control/control_performance_analysis/src/control_performance_analysis_node.cpp index 0e69bf7bf4e5e..20c367d367189 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_node.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_node.cpp @@ -12,18 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include "control_performance_analysis/control_performance_analysis_node.hpp" #include "control_performance_analysis/msg/error_stamped.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" -#include "control_performance_analysis/control_performance_analysis_node.hpp" +#include + +#include +#include namespace { -using control_performance_analysis::msg::ErrorStamped; using control_performance_analysis::TargetPerformanceMsgVars; +using control_performance_analysis::msg::ErrorStamped; ErrorStamped createPerformanceMsgVars(const TargetPerformanceMsgVars & target_performance_vars) { @@ -62,10 +63,8 @@ ControlPerformanceAnalysisNode::ControlPerformanceAnalysisNode( 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); + control_performance_core_ptr_ = std::make_unique( + param_.wheel_base, param_.curvature_interval_length); // Subscribers. sub_trajectory_ = create_subscription( @@ -73,8 +72,7 @@ ControlPerformanceAnalysisNode::ControlPerformanceAnalysisNode( std::bind(&ControlPerformanceAnalysisNode::onTrajectory, this, _1)); sub_control_steering_ = create_subscription( - "~/input/control_raw", 1, - std::bind(&ControlPerformanceAnalysisNode::onControlRaw, this, _1)); + "~/input/control_raw", 1, std::bind(&ControlPerformanceAnalysisNode::onControlRaw, this, _1)); sub_vehicle_steering_ = create_subscription( "~/input/measured_steering", 1, @@ -132,8 +130,7 @@ void ControlPerformanceAnalysisNode::onVecSteeringMeasured( { if (!meas_steer_msg) { RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), 1000, - "waiting for vehicle measured steering message ..."); + get_logger(), *get_clock(), 1000, "waiting for vehicle measured steering message ..."); return; } current_vec_steering_msg_ptr_ = meas_steer_msg; @@ -179,30 +176,22 @@ bool ControlPerformanceAnalysisNode::isDataReady() const { rclcpp::Clock clock{RCL_ROS_TIME}; if (!current_pose_) { - RCLCPP_WARN_THROTTLE( - get_logger(), clock, 1000, - "waiting for 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 ... "); + RCLCPP_WARN_THROTTLE(get_logger(), clock, 1000, "waiting for trajectory ... "); return false; } if (!current_velocity_ptr_) { - RCLCPP_WARN_THROTTLE( - get_logger(), clock, 1000, - "waiting for current_velocity ..."); + RCLCPP_WARN_THROTTLE(get_logger(), clock, 1000, "waiting for current_velocity ..."); return false; } if (!current_control_msg_ptr_) { - RCLCPP_WARN_THROTTLE( - get_logger(), clock, 1000, - "waiting for current_control_steering_val ..."); + RCLCPP_WARN_THROTTLE(get_logger(), clock, 1000, "waiting for current_control_steering_val ..."); return false; } @@ -210,15 +199,14 @@ bool ControlPerformanceAnalysisNode::isDataReady() const } /* - * - Pass trajectory and current pose to control_performance_analysis -> setCurrentPose() - * -> setWayPoints() - * -> findClosestPoint - * -> computePerformanceVars - * */ - -boost::optional ControlPerformanceAnalysisNode:: -computeTargetPerformanceMsgVars() -const + * - 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_); @@ -249,27 +237,25 @@ const 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.twist.linear; - const auto & a = point.accel.linear; - - if ( - !isfinite(p.x) || !isfinite(p.y) || !isfinite(p.z) || !isfinite(o.x) || !isfinite(o.y) || - !isfinite(o.z) || !isfinite(o.w) || !isfinite(t.x) || !isfinite(t.y) || !isfinite(t.z) || - !isfinite(a.x) || !isfinite(a.y) || !isfinite(a.z)) - { - return false; - } else { - return true; - } - }); + 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.twist.linear; + const auto & a = point.accel.linear; + + if ( + !isfinite(p.x) || !isfinite(p.y) || !isfinite(p.z) || !isfinite(o.x) || !isfinite(o.y) || + !isfinite(o.z) || !isfinite(o.w) || !isfinite(t.x) || !isfinite(t.y) || !isfinite(t.z) || + !isfinite(a.x) || !isfinite(a.y) || !isfinite(a.z)) { + return false; + } else { + return true; + } + }); return check_condition; } } // namespace control_performance_analysis -#include "rclcpp_components/register_node_macro.hpp" +#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 index 80356fe0e8c6b..ba4ae2d194701 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_utils.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_utils.cpp @@ -12,16 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #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) +double determinant(std::array const & a, std::array const & b) { return a[0] * b[1] - b[0] * a[1]; } From 4d30d620b441a92bcaa4455b68b6bab60f5962f1 Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Thu, 4 Nov 2021 19:32:37 +0900 Subject: [PATCH 05/10] Add COLCON_IGNORE (#500) Signed-off-by: Kenji Miyake --- control/control_performance_analysis/COLCON_IGNORE | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 control/control_performance_analysis/COLCON_IGNORE diff --git a/control/control_performance_analysis/COLCON_IGNORE b/control/control_performance_analysis/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d From 304192f9d98de9f24474389d98770bdc8ded32af Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Tue, 16 Nov 2021 11:49:27 +0900 Subject: [PATCH 06/10] adapt to actuation cmd/status as control msg (#646) * adapt to actuation cmd/status as control msg * fix readme * fix topics * fix remaing topics * as to pacmod interface * fix vehicle status * add header to twist * revert gyro_odometer_change * revert twist topic change * revert unchanged package --- .../config/error_rqt_multiplot.xml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/control/control_performance_analysis/config/error_rqt_multiplot.xml b/control/control_performance_analysis/config/error_rqt_multiplot.xml index c427d308f02cf..ceb9f2cb308e3 100644 --- a/control/control_performance_analysis/config/error_rqt_multiplot.xml +++ b/control/control_performance_analysis/config/error_rqt_multiplot.xml @@ -682,7 +682,7 @@ -1000 0 - /vehicle/status/steering + /vehicle/status/steering_status autoware_vehicle_msgs/Steering @@ -695,7 +695,7 @@ -1000 0 - /vehicle/status/steering + /vehicle/status/steering_status autoware_vehicle_msgs/Steering @@ -758,7 +758,7 @@ -1000 0 - /vehicle/status/twist + /vehicle/status/velocity_status geometry_msgs/TwistStamped @@ -771,7 +771,7 @@ -1000 0 - /vehicle/status/twist + /vehicle/status/velocity_status geometry_msgs/TwistStamped From 54e044077cccbaf99d60062a33b36150c63dae8a Mon Sep 17 00:00:00 2001 From: Kosuke Murakami Date: Fri, 19 Nov 2021 16:38:47 +0900 Subject: [PATCH 07/10] port control_performance_analysis (#698) * port control_performance_analysis Signed-off-by: kosuke murakami * rename Signed-off-by: kosuke murakami * fix topic name Signed-off-by: kosuke murakami * remove unnecessary depedency Signed-off-by: kosuke murakami * change name of odom topic Signed-off-by: kosuke murakami --- .../COLCON_IGNORE | 0 .../control_performance_analysis_core.hpp | 12 +++---- .../control_performance_analysis_node.hpp | 36 +++++++++---------- ...controller_performance_analysis.launch.xml | 21 ++++++----- .../control_performance_analysis/package.xml | 7 ++-- .../src/control_performance_analysis_core.cpp | 12 +++---- .../src/control_performance_analysis_node.cpp | 30 ++++++++-------- 7 files changed, 58 insertions(+), 60 deletions(-) delete mode 100644 control/control_performance_analysis/COLCON_IGNORE diff --git a/control/control_performance_analysis/COLCON_IGNORE b/control/control_performance_analysis/COLCON_IGNORE deleted file mode 100644 index e69de29bb2d1d..0000000000000 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 index 859c19283949d..4ffcc63f81473 100644 --- 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 @@ -19,8 +19,8 @@ #include -#include -#include +#include +#include #include #include #include @@ -31,8 +31,8 @@ namespace control_performance_analysis { -using autoware_control_msgs::msg::ControlCommandStamped; -using autoware_planning_msgs::msg::Trajectory; +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; @@ -62,7 +62,7 @@ class ControlPerformanceAnalysisCore void setCurrentPose(const Pose & msg); void setCurrentWaypoints(const Trajectory & trajectory); void setCurrentVelocities(const Twist & twist_msg); - void setCurrentControlValue(const ControlCommandStamped & msg); + void setCurrentControlValue(const AckermannLateralCommand & msg); void setInterpolatedPose(Pose & interpolated_pose); void findCurveRefIdx(); @@ -84,7 +84,7 @@ class ControlPerformanceAnalysisCore 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_; + std::shared_ptr current_control_ptr_; // Variables computed std::unique_ptr idx_prev_wp_; // the waypoint index, vehicle 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 index a598f45c4b9a3..201d7b2c89b4b 100644 --- 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 @@ -21,11 +21,11 @@ #include #include -#include -#include -#include +#include +#include +#include #include -#include +#include #include @@ -33,12 +33,12 @@ namespace control_performance_analysis { -using autoware_control_msgs::msg::ControlCommandStamped; -using autoware_planning_msgs::msg::Trajectory; -using autoware_vehicle_msgs::msg::Steering; +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 geometry_msgs::msg::TwistStamped; +using nav_msgs::msg::Odometry; // Parameters Struct struct Param @@ -59,10 +59,10 @@ class ControlPerformanceAnalysisNode : public rclcpp::Node 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_; + 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. @@ -78,9 +78,9 @@ class ControlPerformanceAnalysisNode : public rclcpp::Node // Callback Methods void onTrajectory(const Trajectory::ConstSharedPtr msg); void publishErrorMsg(const TargetPerformanceMsgVars & control_performance_vars); - void onControlRaw(const ControlCommandStamped::ConstSharedPtr control_msg); - void onVecSteeringMeasured(const Steering::ConstSharedPtr meas_steer_msg); - void onVelocity(const TwistStamped::ConstSharedPtr msg); + 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_; @@ -92,9 +92,9 @@ class ControlPerformanceAnalysisNode : public rclcpp::Node // Subscriber Parameters Trajectory::ConstSharedPtr current_trajectory_ptr_; // ConstPtr to local traj. - ControlCommandStamped::ConstSharedPtr current_control_msg_ptr_; - Steering::ConstSharedPtr current_vec_steering_msg_ptr_; - TwistStamped::ConstSharedPtr current_velocity_ptr_; + 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 diff --git a/control/control_performance_analysis/launch/controller_performance_analysis.launch.xml b/control/control_performance_analysis/launch/controller_performance_analysis.launch.xml index ccb374d4b0058..8a248c9e8745a 100644 --- a/control/control_performance_analysis/launch/controller_performance_analysis.launch.xml +++ b/control/control_performance_analysis/launch/controller_performance_analysis.launch.xml @@ -1,23 +1,22 @@ - + - - + + - - + + - - - - + + + + + diff --git a/control/control_performance_analysis/package.xml b/control/control_performance_analysis/package.xml index 342d48ec92887..bda3b7cce680b 100644 --- a/control/control_performance_analysis/package.xml +++ b/control/control_performance_analysis/package.xml @@ -10,12 +10,13 @@ ament_cmake_auto rosidl_default_generators - autoware_control_msgs - autoware_planning_msgs + autoware_auto_control_msgs + autoware_auto_planning_msgs + autoware_auto_vehicle_msgs autoware_utils - autoware_vehicle_msgs geometry_msgs libboost-dev + nav_msgs rclcpp sensor_msgs std_msgs diff --git a/control/control_performance_analysis/src/control_performance_analysis_core.cpp b/control/control_performance_analysis/src/control_performance_analysis_core.cpp index 413c25039f939..9847b874c8cb2 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_core.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_core.cpp @@ -53,9 +53,9 @@ void ControlPerformanceAnalysisCore::setCurrentPose(const Pose & msg) current_vec_pose_ptr_ = std::make_shared(msg); } -void ControlPerformanceAnalysisCore::setCurrentControlValue(const ControlCommandStamped & msg) +void ControlPerformanceAnalysisCore::setCurrentControlValue(const AckermannLateralCommand & msg) { - current_control_ptr_ = std::make_shared(msg); + current_control_ptr_ = std::make_shared(msg); } std::pair ControlPerformanceAnalysisCore::findClosestPrevWayPointIdx_path_direction() @@ -216,7 +216,7 @@ std::pair ControlPerformanceAnalysisCore::getPer target_vars.lateral_error = lateral_error; target_vars.heading_error = heading_yaw_error; - double steering_val = current_control_ptr_->control.steering_angle; + 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; @@ -321,7 +321,7 @@ std::pair ControlPerformanceAnalysisCore::calculateClosestPose() double dy_prev2next = current_waypoints_ptr_->poses.at(*idx_next_wp_).position.y - current_waypoints_ptr_->poses.at(*idx_prev_wp_).position.y; - double dpsi_prev2next = utils::angleDistance(next_yaw, prev_yaw); + 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}; @@ -369,7 +369,7 @@ std::pair ControlPerformanceAnalysisCore::calculateClosestPose() interpolated_pose.position.z = 0.0; // Interpolate the yaw angle of pi : interpolated waypoint - double interp_yaw_angle = prev_yaw + ratio_t * dpsi_prev2next; + double interp_yaw_angle = prev_yaw + ratio_t * delta_psi_prev2next; Quaternion orient_msg = utils::createOrientationMsgFromYaw(interp_yaw_angle); interpolated_pose.orientation = orient_msg; @@ -396,7 +396,7 @@ double ControlPerformanceAnalysisCore::estimateCurvature() Pose front_axleWP_pose_prev = current_waypoints_ptr_->poses.at(idx_prev_waypoint); - // Compute current interpoint arc-length ds. + // 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); diff --git a/control/control_performance_analysis/src/control_performance_analysis_node.cpp b/control/control_performance_analysis/src/control_performance_analysis_node.cpp index 20c367d367189..2a48ca4971ce9 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_node.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_node.cpp @@ -71,16 +71,15 @@ ControlPerformanceAnalysisNode::ControlPerformanceAnalysisNode( "~/input/reference_trajectory", 1, std::bind(&ControlPerformanceAnalysisNode::onTrajectory, this, _1)); - sub_control_steering_ = create_subscription( + sub_control_steering_ = create_subscription( "~/input/control_raw", 1, std::bind(&ControlPerformanceAnalysisNode::onControlRaw, this, _1)); - sub_vehicle_steering_ = create_subscription( + sub_vehicle_steering_ = create_subscription( "~/input/measured_steering", 1, std::bind(&ControlPerformanceAnalysisNode::onVecSteeringMeasured, this, _1)); - sub_velocity_ = create_subscription( - "~/input/current_velocity", 1, - std::bind(&ControlPerformanceAnalysisNode::onVelocity, 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); @@ -116,7 +115,7 @@ void ControlPerformanceAnalysisNode::onTrajectory(const Trajectory::ConstSharedP } void ControlPerformanceAnalysisNode::onControlRaw( - const ControlCommandStamped::ConstSharedPtr control_msg) + const AckermannLateralCommand::ConstSharedPtr control_msg) { if (!control_msg) { RCLCPP_ERROR(get_logger(), "steering signal has not been received yet ..."); @@ -126,7 +125,7 @@ void ControlPerformanceAnalysisNode::onControlRaw( } void ControlPerformanceAnalysisNode::onVecSteeringMeasured( - const Steering::ConstSharedPtr meas_steer_msg) + const SteeringReport::ConstSharedPtr meas_steer_msg) { if (!meas_steer_msg) { RCLCPP_WARN_THROTTLE( @@ -136,9 +135,9 @@ void ControlPerformanceAnalysisNode::onVecSteeringMeasured( current_vec_steering_msg_ptr_ = meas_steer_msg; } -void ControlPerformanceAnalysisNode::onVelocity(const TwistStamped::ConstSharedPtr msg) +void ControlPerformanceAnalysisNode::onVelocity(const Odometry::ConstSharedPtr msg) { - current_velocity_ptr_ = msg; + current_odom_ptr_ = msg; } void ControlPerformanceAnalysisNode::onTimer() @@ -185,8 +184,8 @@ bool ControlPerformanceAnalysisNode::isDataReady() const return false; } - if (!current_velocity_ptr_) { - RCLCPP_WARN_THROTTLE(get_logger(), clock, 1000, "waiting for current_velocity ..."); + if (!current_odom_ptr_) { + RCLCPP_WARN_THROTTLE(get_logger(), clock, 1000, "waiting for current_odom ..."); return false; } @@ -211,7 +210,7 @@ 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_velocity_ptr_->twist); + 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. @@ -240,13 +239,12 @@ 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.twist.linear; - const auto & a = point.accel.linear; + 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.x) || !isfinite(t.y) || !isfinite(t.z) || - !isfinite(a.x) || !isfinite(a.y) || !isfinite(a.z)) { + !isfinite(o.z) || !isfinite(o.w) || !isfinite(t) || !isfinite(a)) { return false; } else { return true; From 108ef43e0bfb2b376104d39508286f78485e27ae Mon Sep 17 00:00:00 2001 From: Kosuke Murakami Date: Mon, 22 Nov 2021 13:18:13 +0900 Subject: [PATCH 08/10] add readme in control_performance_analysis (#716) * add readme Signed-off-by: kosuke murakami * update readme * update readme * Update control/control_performance_analysis/README.md --- .../control_performance_analysis/README.md | 28 +++++++++++++++++++ 1 file changed, 28 insertions(+) create mode 100644 control/control_performance_analysis/README.md diff --git a/control/control_performance_analysis/README.md b/control/control_performance_analysis/README.md new file mode 100644 index 0000000000000..144b242fa4224 --- /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/odometry` | 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. | From 300ae60b2e6899d4172254555f38dc1358815332 Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Fri, 26 Nov 2021 12:39:34 +0900 Subject: [PATCH 09/10] Update twist topic name (#736) Signed-off-by: wep21 --- control/control_performance_analysis/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/control_performance_analysis/README.md b/control/control_performance_analysis/README.md index 144b242fa4224..d6631e8af8be3 100644 --- a/control/control_performance_analysis/README.md +++ b/control/control_performance_analysis/README.md @@ -18,7 +18,7 @@ Based on the various input from planning, control, and vehicle, it publishes the | `/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/odometry` | nav_msgs::msg::Odometry | Use twist from odometry. | +| `/localization/kinematic_state` | nav_msgs::msg::Odometry | Use twist from odometry. | | `/tf` | `tf2_msgs::msg::TFMessage | Extract ego pose from tf. | ### Output topics From 4d9e4c82b66c022a3b0f6d85979d7775deedc90a Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Fri, 3 Dec 2021 17:47:22 +0900 Subject: [PATCH 10/10] Apply suggestions from code review --- control/control_performance_analysis/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/control_performance_analysis/README.md b/control/control_performance_analysis/README.md index d6631e8af8be3..dbb0405590b50 100644 --- a/control/control_performance_analysis/README.md +++ b/control/control_performance_analysis/README.md @@ -19,7 +19,7 @@ Based on the various input from planning, control, and vehicle, it publishes the | `/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. | +| `/tf` | tf2_msgs::msg::TFMessage | Extract ego pose from tf. | ### Output topics