feat: add control_performance_analysis

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
+)

control/control_performance_analysis/README.md

@@ -0,0 +1,28 @@
+# control_performance_analysis
+
+## Purpose
+
+`control_performance_analysis` is the package to analyze the tracking performance of a control module.
+
+This package is used as a tool to quantify the results of the control module.
+That's why it doesn't interfere with the core logic of autonomous driving.
+
+Based on the various input from planning, control, and vehicle, it publishes the result of analysis as `control_performance_analysis::msg::ErrorStamped` defined in this package.
+
+## Input / Output
+
+### Input topics
+
+| Name                                               | Type                                                     | Description                                         |
+| -------------------------------------------------- | -------------------------------------------------------- | --------------------------------------------------- |
+| `/planning/scenario_planning/trajectory`           | autoware_auto_planning_msgs::msg::Trajectory             | Output trajectory from planning module.             |
+| `/control/trajectory_follower/lateral/control_cmd` | autoware_auto_control_msgs::msg::AckermannLateralCommand | Output lateral control command from control module. |
+| `/vehicle/status/steering_status`                  | autoware_auto_vehicle_msgs::msg::SteeringReport          | Steering information from vehicle.                  |
+| `/localization/kinematic_state`                    | nav_msgs::msg::Odometry                                  | Use twist from odometry.                            |
+| `/tf`                                              | `tf2_msgs::msg::TFMessage                                | Extract ego pose from tf.                           |
+
+### Output topics
+
+| Name                                    | Type                                            | Description                             |
+| --------------------------------------- | ----------------------------------------------- | --------------------------------------- |
+| `/control_performance/performance_vars` | control_performance_analysis::msg::ErrorStamped | The result of the performance analysis. |

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

control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp

@@ -0,0 +1,104 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_CORE_HPP_
+#define CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_CORE_HPP_
+
+#include "control_performance_analysis/control_performance_analysis_utils.hpp"
+
+#include <eigen3/Eigen/Core>
+
+#include <autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp>
+#include <autoware_auto_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 <memory>
+#include <utility>
+#include <vector>
+
+namespace control_performance_analysis
+{
+using autoware_auto_control_msgs::msg::AckermannLateralCommand;
+using autoware_auto_planning_msgs::msg::Trajectory;
+using geometry_msgs::msg::Pose;
+using geometry_msgs::msg::PoseArray;
+using geometry_msgs::msg::Twist;
+
+struct TargetPerformanceMsgVars
+{
+  double lateral_error;
+  double heading_error;
+  double control_effort_energy;
+  double error_energy;
+  double value_approximation;
+  double curvature_estimate;
+  double curvature_estimate_pp;
+  double lateral_error_velocity;
+  double lateral_error_acceleration;
+};
+
+class ControlPerformanceAnalysisCore
+{
+public:
+  // See https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  ControlPerformanceAnalysisCore();
+  ControlPerformanceAnalysisCore(double wheelbase, double curvature_interval_length);
+
+  // Setters
+  void setCurrentPose(const Pose & msg);
+  void setCurrentWaypoints(const Trajectory & trajectory);
+  void setCurrentVelocities(const Twist & twist_msg);
+  void setCurrentControlValue(const AckermannLateralCommand & msg);
+  void setInterpolatedPose(Pose & interpolated_pose);
+
+  void findCurveRefIdx();
+  std::pair<bool, int32_t> findClosestPrevWayPointIdx_path_direction();
+  double estimateCurvature();
+  double estimatePurePursuitCurvature();
+
+  // Getters
+  bool isDataReady() const;
+  std::pair<bool, TargetPerformanceMsgVars> getPerformanceVars();
+  Pose getPrevWPPose() const;
+  std::pair<bool, Pose> calculateClosestPose();
+
+private:
+  double wheelbase_;
+  double curvature_interval_length_;
+
+  // Variables Received Outside
+  std::shared_ptr<PoseArray> current_waypoints_ptr_;
+  std::shared_ptr<Pose> current_vec_pose_ptr_;
+  std::shared_ptr<std::vector<double>> current_velocities_ptr_;  // [Vx, Heading rate]
+  std::shared_ptr<AckermannLateralCommand> current_control_ptr_;
+
+  // Variables computed
+  std::unique_ptr<int32_t> idx_prev_wp_;       // the waypoint index, vehicle
+  std::unique_ptr<int32_t> idx_curve_ref_wp_;  // index of waypoint corresponds to front axle center
+  std::unique_ptr<int32_t> idx_next_wp_;       //  the next waypoint index, vehicle heading to
+  std::unique_ptr<TargetPerformanceMsgVars> prev_target_vars_{};
+  std::shared_ptr<Pose> 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_

control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp

@@ -0,0 +1,105 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_NODE_HPP_
+#define CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_NODE_HPP_
+
+#include "control_performance_analysis/control_performance_analysis_core.hpp"
+#include "control_performance_analysis/msg/error_stamped.hpp"
+
+#include <autoware_utils/ros/self_pose_listener.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp>
+#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
+#include <autoware_auto_vehicle_msgs/msg/steering_report.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+
+#include <boost/optional.hpp>
+
+#include <memory>
+
+namespace control_performance_analysis
+{
+using autoware_auto_control_msgs::msg::AckermannLateralCommand;
+using autoware_auto_planning_msgs::msg::Trajectory;
+using autoware_auto_vehicle_msgs::msg::SteeringReport;
+using control_performance_analysis::msg::ErrorStamped;
+using geometry_msgs::msg::PoseStamped;
+using nav_msgs::msg::Odometry;
+
+// Parameters Struct
+struct Param
+{
+  // Global parameters
+  double wheel_base;
+  double curvature_interval_length;
+
+  // Control Method Parameters
+  double control_period;
+};
+
+class ControlPerformanceAnalysisNode : public rclcpp::Node
+{
+public:
+  explicit ControlPerformanceAnalysisNode(const rclcpp::NodeOptions & node_options);
+
+private:
+  // Subscribers and Local Variable Assignment
+  rclcpp::Subscription<Trajectory>::SharedPtr sub_trajectory_;  // subscribe to trajectory
+  rclcpp::Subscription<AckermannLateralCommand>::SharedPtr
+    sub_control_steering_;                                  // subscribe to steering control value
+  rclcpp::Subscription<Odometry>::SharedPtr sub_velocity_;  // subscribe to velocity
+  rclcpp::Subscription<SteeringReport>::SharedPtr sub_vehicle_steering_;
+
+  // Self Pose listener.
+  autoware_utils::SelfPoseListener self_pose_listener_{this};  // subscribe to pose listener.
+
+  // Publishers
+  rclcpp::Publisher<ErrorStamped>::SharedPtr pub_error_msg_;  // publish error message
+
+  // Node Methods
+  bool isDataReady() const;  // check if data arrive
+  static bool isValidTrajectory(const Trajectory & traj);
+  boost::optional<TargetPerformanceMsgVars> computeTargetPerformanceMsgVars() const;
+
+  // Callback Methods
+  void onTrajectory(const Trajectory::ConstSharedPtr msg);
+  void publishErrorMsg(const TargetPerformanceMsgVars & control_performance_vars);
+  void onControlRaw(const AckermannLateralCommand::ConstSharedPtr control_msg);
+  void onVecSteeringMeasured(const SteeringReport::ConstSharedPtr meas_steer_msg);
+  void onVelocity(const Odometry::ConstSharedPtr msg);
+
+  // Timer - To Publish In Control Period
+  rclcpp::TimerBase::SharedPtr timer_publish_;
+  void onTimer();
+
+  // Parameters
+  Param param_{};  // wheelbase, control period and feedback coefficients.
+  TargetPerformanceMsgVars target_error_vars_{};
+
+  // Subscriber Parameters
+  Trajectory::ConstSharedPtr current_trajectory_ptr_;  // ConstPtr to local traj.
+  AckermannLateralCommand::ConstSharedPtr current_control_msg_ptr_;
+  SteeringReport::ConstSharedPtr current_vec_steering_msg_ptr_;
+  Odometry::ConstSharedPtr current_odom_ptr_;
+  PoseStamped::ConstSharedPtr current_pose_;  // pose of the vehicle, x, y, heading
+
+  // Algorithm
+  std::unique_ptr<ControlPerformanceAnalysisCore> control_performance_core_ptr_;
+};
+}  // namespace control_performance_analysis
+
+#endif  // CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_NODE_HPP_