feat: add control_performance_analysis (autowarefoundation#95)

* Feature/porting control performance analysis (autowarefoundation#1671) * Feature/control performance analysis (autowarefoundation#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 <border_goldenmarket@yahoo.co.jp> * Porting control performance analysis Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Apply lint Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Add boost dependency for optional Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Remove confusing abbreviation Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Fix dependency in packages.xml Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Add missing new line Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Add comment for eigen macro Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * pre-commit fixes Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> Co-authored-by: Ali BOYALI <boyali@users.noreply.github.com> * Fix package.xml (autowarefoundation#2056) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix typo for control_performance_analysis (autowarefoundation#2328) * fix typo * fix Contro -> Control * fix for spellcheck * fix * Change formatter to clang-format and black (autowarefoundation#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 <kenji.miyake@tier4.jp> * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply Black Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Add COLCON_IGNORE (autowarefoundation#500) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * adapt to actuation cmd/status as control msg (autowarefoundation#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 * port control_performance_analysis (autowarefoundation#698) * port control_performance_analysis Signed-off-by: kosuke murakami <kosuke.murakami@tier4.jp> * rename Signed-off-by: kosuke murakami <kosuke.murakami@tier4.jp> * fix topic name Signed-off-by: kosuke murakami <kosuke.murakami@tier4.jp> * remove unnecessary depedency Signed-off-by: kosuke murakami <kosuke.murakami@tier4.jp> * change name of odom topic Signed-off-by: kosuke murakami <kosuke.murakami@tier4.jp> * add readme in control_performance_analysis (autowarefoundation#716) * add readme Signed-off-by: kosuke murakami <kosuke.murakami@tier4.jp> * update readme * update readme * Update control/control_performance_analysis/README.md * Update twist topic name (autowarefoundation#736) Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Apply suggestions from code review Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Ali BOYALI <boyali@users.noreply.github.com> Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp> Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Co-authored-by: Kosuke Murakami <kosuke.murakami@tier4.jp> Co-authored-by: Tomoya Kimura <tomoya.kimura@tier4.jp>
badai-nguyen · Dec 7, 2021 · f9307e3 · f9307e3
1 parent 36bd70e
commit f9307e3
Show file tree

Hide file tree

Showing 14 changed files with 2,162 additions and 0 deletions.
diff --git a/control/control_performance_analysis/CMakeLists.txt b/control/control_performance_analysis/CMakeLists.txt
@@ -0,0 +1,59 @@
+cmake_minimum_required(VERSION 3.5)
+project(control_performance_analysis)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+find_package(Boost REQUIRED)
+
+find_package(eigen3_cmake_module REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+rosidl_generate_interfaces(control_performance_analysis_msgs
+  msg/Error.msg
+  msg/ErrorStamped.msg
+  DEPENDENCIES
+    std_msgs
+)
+
+ament_auto_add_library(control_performance_analysis_core SHARED
+  src/control_performance_analysis_utils.cpp
+  src/control_performance_analysis_core.cpp
+)
+
+ament_auto_add_library(control_performance_analysis_node SHARED
+  src/control_performance_analysis_node.cpp
+)
+
+rosidl_target_interfaces(control_performance_analysis_node
+  control_performance_analysis_msgs "rosidl_typesupport_cpp")
+
+target_link_libraries(control_performance_analysis_node
+  control_performance_analysis_core
+)
+
+rclcpp_components_register_node(control_performance_analysis_node
+  PLUGIN "control_performance_analysis::ControlPerformanceAnalysisNode"
+  EXECUTABLE control_performance_analysis
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    launch
+    config
+)
diff --git a/control/control_performance_analysis/README.md b/control/control_performance_analysis/README.md
@@ -0,0 +1,28 @@
+# control_performance_analysis
+
+## Purpose
+
+`control_performance_analysis` is the package to analyze the tracking performance of a control module.
+
+This package is used as a tool to quantify the results of the control module.
+That's why it doesn't interfere with the core logic of autonomous driving.
+
+Based on the various input from planning, control, and vehicle, it publishes the result of analysis as `control_performance_analysis::msg::ErrorStamped` defined in this package.
+
+## Input / Output
+
+### Input topics
+
+| Name                                               | Type                                                     | Description                                         |
+| -------------------------------------------------- | -------------------------------------------------------- | --------------------------------------------------- |
+| `/planning/scenario_planning/trajectory`           | autoware_auto_planning_msgs::msg::Trajectory             | Output trajectory from planning module.             |
+| `/control/trajectory_follower/lateral/control_cmd` | autoware_auto_control_msgs::msg::AckermannLateralCommand | Output lateral control command from control module. |
+| `/vehicle/status/steering_status`                  | autoware_auto_vehicle_msgs::msg::SteeringReport          | Steering information from vehicle.                  |
+| `/localization/kinematic_state`                    | nav_msgs::msg::Odometry                                  | Use twist from odometry.                            |
+| `/tf`                                              | tf2_msgs::msg::TFMessage                                 | Extract ego pose from tf.                           |
+
+### Output topics
+
+| Name                                    | Type                                            | Description                             |
+| --------------------------------------- | ----------------------------------------------- | --------------------------------------- |
+| `/control_performance/performance_vars` | control_performance_analysis::msg::ErrorStamped | The result of the performance analysis. |
diff --git a/control/control_performance_analysis/config/control_performance_analysis.param.yaml b/control/control_performance_analysis/config/control_performance_analysis.param.yaml
@@ -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
diff --git a/...mance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp b/...mance_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_
diff --git a/...mance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp b/...mance_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_