estimator debug && compatible modification with v3.1.3 (x1)

Signed-off-by: kminoda <koji.minoda@tier4.jp>

localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/deviation_estimator.hpp

@@ -30,6 +30,7 @@
 #include "sensor_msgs/msg/imu.hpp"
 #include "std_msgs/msg/float64.hpp"
 #include "tier4_debug_msgs/msg/float64_stamped.hpp"
+#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp"
 
 #include <iostream>
 #include <memory>
@@ -58,7 +59,7 @@ class DeviationEstimator : public rclcpp::Node
 
 private:
   rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr sub_pose_with_cov_;
-  rclcpp::Subscription<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr
+  rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::VelocityReport>::SharedPtr
     sub_wheel_odometry_;
   rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr sub_imu_;
   rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr pub_coef_vx_;
@@ -104,7 +105,7 @@ class DeviationEstimator : public rclcpp::Node
   void callback_pose_with_covariance(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
 
   void callback_wheel_odometry(
-    const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr wheel_odometry_msg_ptr);
+    const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr wheel_odometry_msg_ptr);
 
   void callback_imu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr);
 

localization/deviation_estimation_tools/deviation_estimator/include/deviation_estimator/logger.hpp

@@ -28,7 +28,7 @@
 class Logger
 {
 public:
-  Logger(const std::string & output_dir, const std::string & imu_topic);
+  Logger(const std::string & output_dir);
   void log_estimated_result_section(
     const double stddev_vx, const double coef_vx,
     const geometry_msgs::msg::Vector3 & angular_velocity_stddev,
@@ -39,6 +39,5 @@ class Logger
   const std::string output_log_path_;
   const std::string output_imu_param_path_;
   const std::string output_velocity_param_path_;
-  const std::string imu_topic_;
 };
 #endif  // DEVIATION_ESTIMATOR__LOGGER_HPP_

localization/deviation_estimation_tools/deviation_estimator/launch/deviation_estimator.launch.xml

@@ -15,14 +15,14 @@
   <arg name="in_pose_with_cov_name" default="/localization/pose_estimator/pose_with_covariance"/>
 
   <arg name="in_wheel_odometry" default="/vehicle/status/velocity_status"/>
-  <arg name="in_imu" default="/sensing/imu/tamagawa/imu_raw"/>
+  <arg name="in_imu" default="/sensing/lidar/front_center/livox/imu"/>
 
   <arg name="output_bias_angular_velocity" default="estimated_bias_angular_velocity"/>
   <arg name="output_coef_vx" default="estimated_coef_vx"/>
 
   <node pkg="deviation_estimator" exec="deviation_estimator" name="deviation_estimator" output="screen">
     <remap from="in_pose_with_covariance" to="$(var in_pose_with_cov_name)"/>
-
+    <remap from="in_imu" to="$(var in_imu)"/>
     <remap from="in_wheel_odometry" to="$(var in_wheel_odometry)"/>
 
     <param from="$(var param_path)"/>

localization/deviation_estimation_tools/deviation_estimator/package.xml

@@ -10,6 +10,7 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <build_depend>autoware_cmake</build_depend>
 
+  <depend>autoware_auto_vehicle_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>nav_msgs</depend>
   <depend>rclcpp</depend>

localization/deviation_estimation_tools/deviation_estimator/src/deviation_estimator.cpp

@@ -131,7 +131,7 @@ DeviationEstimator::DeviationEstimator(
   tf_listener_(tf_buffer_),
   output_frame_(declare_parameter<std::string>("base_link", "base_link")),
   results_dir_(declare_parameter<std::string>("results_dir")),
-  results_logger_(results_dir_, imu_topic_)
+  results_logger_(results_dir_)
 {
   show_debug_info_ = declare_parameter("show_debug_info", false);
   dt_design_ = declare_parameter("dt_design", 10.0);
@@ -164,7 +164,7 @@ DeviationEstimator::DeviationEstimator(
 
   sub_imu_ = create_subscription<sensor_msgs::msg::Imu>(
     "in_imu", 1, std::bind(&DeviationEstimator::callback_imu, this, _1));
-  sub_wheel_odometry_ = create_subscription<geometry_msgs::msg::TwistWithCovarianceStamped>(
+  sub_wheel_odometry_ = create_subscription<autoware_auto_vehicle_msgs::msg::VelocityReport>(
     "in_wheel_odometry", 1, std::bind(&DeviationEstimator::callback_wheel_odometry, this, _1));
   results_logger_.log_estimated_result_section(
     0.2, 0.0, geometry_msgs::msg::Vector3{}, geometry_msgs::msg::Vector3{});
@@ -192,11 +192,11 @@ void DeviationEstimator::callback_pose_with_covariance(
 }
 
 void DeviationEstimator::callback_wheel_odometry(
-  const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr wheel_odometry_msg_ptr)
+  const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr wheel_odometry_msg_ptr)
 {
   tier4_debug_msgs::msg::Float64Stamped vx;
   vx.stamp = wheel_odometry_msg_ptr->header.stamp;
-  vx.data = wheel_odometry_msg_ptr->twist.twist.linear.x;
+  vx.data = wheel_odometry_msg_ptr->longitudinal_velocity;
 
   if (use_predefined_coef_vx_) {
     vx.data *= predefined_coef_vx_;

localization/deviation_estimation_tools/deviation_estimator/src/logger.cpp

@@ -16,11 +16,10 @@
 
 #include <cmath>
 
-Logger::Logger(const std::string & output_dir, const std::string & imu_topic)
+Logger::Logger(const std::string & output_dir)
 : output_log_path_(output_dir + "/output.txt"),
   output_imu_param_path_(output_dir + "/imu_corrector.param.yaml"),
-  output_velocity_param_path_(output_dir + "/vehicle_velocity_converter.param.yaml"),
-  imu_topic_(imu_topic)
+  output_velocity_param_path_(output_dir + "/vehicle_velocity_converter.param.yaml")
 {
   std::ofstream file_log(output_log_path_);
   file_log.close();
@@ -43,7 +42,7 @@ void Logger::log_estimated_result_section(
   file_velocity_param << "  ros__parameters:\n";
   file_velocity_param << fmt::format("    speed_scale_factor: {:.5f}\n", coef_vx);
   file_velocity_param << fmt::format("    velocity_stddev_xx: {:.5f}\n", stddev_vx);
-  file_velocity_param << "    velocity_stddev_xx: 0.1 # Default value\n";
+  file_velocity_param << "    angular_velocity_stddev_zz: 0.1 # Default value\n";
   file_velocity_param << "    frame_id: base_link # Default value\n";
   file_velocity_param.close();
 

localization/deviation_estimation_tools/deviation_evaluator/include/deviation_evaluator/deviation_evaluator.hpp

@@ -29,6 +29,8 @@
 #include "std_msgs/msg/float64.hpp"
 #include "tier4_debug_msgs/msg/float64_stamped.hpp"
 
+#include "deviation_evaluator/utils.hpp"
+
 #include <deque>
 #include <fstream>
 #include <iostream>

localization/deviation_estimation_tools/deviation_evaluator/include/deviation_evaluator/utils.hpp

@@ -0,0 +1,123 @@
+// Copyright 2018-2019 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+
+#ifndef DEVIATION_EVALUATOR__UTILS_HPP_
+#define DEVIATION_EVALUATOR__UTILS_HPP_
+
+#include "tier4_autoware_utils/geometry/geometry.hpp"
+#include "tier4_autoware_utils/math/normalization.hpp"
+#include "tier4_autoware_utils/math/constants.hpp"
+
+// ToDo (kminoda): Replace these functions with the one from tier4_autoware_utils.
+// Currently these functions are declared here since this tool has to be compatible with older version of Autoware.
+
+template <class Pose1, class Pose2>
+bool isDrivingForward(const Pose1 & src_pose, const Pose2 & dst_pose)
+{
+  // check the first point direction
+  const double src_yaw = tf2::getYaw(tier4_autoware_utils::getPose(src_pose).orientation);
+  const double pose_direction_yaw = tier4_autoware_utils::calcAzimuthAngle(tier4_autoware_utils::getPoint(src_pose), tier4_autoware_utils::getPoint(dst_pose));
+  return std::fabs(tier4_autoware_utils::normalizeRadian(src_yaw - pose_direction_yaw)) < tier4_autoware_utils::pi / 2.0;
+}
+
+/**
+ * @brief Calculate a point by linear interpolation.
+ * @param src source point
+ * @param dst destination point
+ * @param ratio interpolation ratio, which should be [0.0, 1.0]
+ * @return interpolated point
+ */
+template <class Point1, class Point2>
+geometry_msgs::msg::Point calcInterpolatedPoint(
+  const Point1 & src, const Point2 & dst, const double ratio)
+{
+  const auto src_point = tier4_autoware_utils::getPoint(src);
+  const auto dst_point = tier4_autoware_utils::getPoint(dst);
+
+  tf2::Vector3 src_vec;
+  src_vec.setX(src_point.x);
+  src_vec.setY(src_point.y);
+  src_vec.setZ(src_point.z);
+
+  tf2::Vector3 dst_vec;
+  dst_vec.setX(dst_point.x);
+  dst_vec.setY(dst_point.y);
+  dst_vec.setZ(dst_point.z);
+
+  // Get pose by linear interpolation
+  const double clamped_ratio = std::clamp(ratio, 0.0, 1.0);
+  const auto & vec = tf2::lerp(src_vec, dst_vec, clamped_ratio);
+
+  geometry_msgs::msg::Point point;
+  point.x = vec.x();
+  point.y = vec.y();
+  point.z = vec.z();
+
+  return point;
+}
+
+/**
+ * @brief Calculate a pose by linear interpolation.
+ * Note that if dist(src_pose, dst_pose)<=0.01
+ * the orientation of the output pose is same as the orientation
+ * of the dst_pose
+ * @param src source point
+ * @param dst destination point
+ * @param ratio interpolation ratio, which should be [0.0, 1.0]
+ * @param set_orientation_from_position_direction set position by spherical interpolation if false
+ * @return interpolated point
+ */
+template <class Pose1, class Pose2>
+geometry_msgs::msg::Pose calcInterpolatedPose(
+  const Pose1 & src_pose, const Pose2 & dst_pose, const double ratio,
+  const bool set_orientation_from_position_direction = true)
+{
+  const double clamped_ratio = std::clamp(ratio, 0.0, 1.0);
+
+  geometry_msgs::msg::Pose output_pose;
+  output_pose.position =
+    calcInterpolatedPoint(tier4_autoware_utils::getPoint(src_pose), tier4_autoware_utils::getPoint(dst_pose), clamped_ratio);
+
+  if (set_orientation_from_position_direction) {
+    const double input_poses_dist = tier4_autoware_utils::calcDistance2d(tier4_autoware_utils::getPoint(src_pose), tier4_autoware_utils::getPoint(dst_pose));
+    const bool is_driving_forward = tier4_autoware_utils::isDrivingForward(src_pose, dst_pose);
+
+    // Get orientation from interpolated point and src_pose
+    if ((is_driving_forward && clamped_ratio > 1.0 - (1e-6)) || input_poses_dist < 1e-3) {
+      output_pose.orientation = tier4_autoware_utils::getPose(dst_pose).orientation;
+    } else if (!is_driving_forward && clamped_ratio < 1e-6) {
+      output_pose.orientation = tier4_autoware_utils::getPose(src_pose).orientation;
+    } else {
+      const auto & base_pose = is_driving_forward ? dst_pose : src_pose;
+      const double pitch = tier4_autoware_utils::calcElevationAngle(tier4_autoware_utils::getPoint(output_pose), tier4_autoware_utils::getPoint(base_pose));
+      const double yaw = tier4_autoware_utils::calcAzimuthAngle(tier4_autoware_utils::getPoint(output_pose), tier4_autoware_utils::getPoint(base_pose));
+      output_pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw);
+    }
+  } else {
+    // Get orientation by spherical linear interpolation
+    tf2::Transform src_tf;
+    tf2::Transform dst_tf;
+    tf2::fromMsg(tier4_autoware_utils::getPose(src_pose), src_tf);
+    tf2::fromMsg(tier4_autoware_utils::getPose(dst_pose), dst_tf);
+    const auto & quaternion = tf2::slerp(src_tf.getRotation(), dst_tf.getRotation(), clamped_ratio);
+    output_pose.orientation = tf2::toMsg(quaternion);
+  }
+
+  return output_pose;
+}
+
+
+
+#endif  // DEVIATION_EVALUATOR__UTILS_HPP_

localization/deviation_estimation_tools/deviation_evaluator/src/deviation_evaluator.cpp

@@ -203,6 +203,6 @@ geometry_msgs::msg::Pose DeviationEvaluator::interpolatePose(const double time)
   const double time_start = rclcpp::Time((*(iter_next - 1))->header.stamp).seconds();
   const double time_end = rclcpp::Time((*iter_next)->header.stamp).seconds();
   const double ratio = (time - time_start) / (time_end - time_start);
-  return tier4_autoware_utils::calcInterpolatedPose(
+  return calcInterpolatedPose(
     (*(iter_next - 1))->pose, (*iter_next)->pose, ratio);
 }