feat: add vehicle, control and localization calibration tools (#22)

* add vehicle control localization tools

* style(pre-commit): autofix

* fix pre commit

* Fix to pass cspell about localization

Signed-off-by: Shintaro Sakoda <[email protected]>

* ignore words from spellcheck

* ROS2 -> ROS 2

* feat: convert autoware_auto_msg to autoware_msg (#42)

* merge

* delete autoware_auto

* style(pre-commit): autofix

---------

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

* add prefix to vehicle_info_utils

Signed-off-by: tomoya.kimura <[email protected]>

* style(pre-commit): autofix

* fix: add s to VehicleInfoUtil

Signed-off-by: tomoya.kimura <[email protected]>

* fix

---------

Signed-off-by: Shintaro Sakoda <[email protected]>
Signed-off-by: tomoya.kimura <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Shintaro Sakoda <[email protected]>
Co-authored-by: SakodaShintaro <[email protected]>
Co-authored-by: tomoya.kimura <[email protected]>

common/tier4_calibration_msgs/CMakeLists.txt

@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 3.5)
+project(tier4_calibration_msgs)
+
+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 -Werror)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/BoolStamped.msg"
+  "msg/Float32Stamped.msg"
+  "msg/EstimationResult.msg"
+  "msg/TimeDelay.msg"
+  DEPENDENCIES
+    std_msgs
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package()

common/tier4_calibration_msgs/msg/BoolStamped.msg

@@ -0,0 +1,2 @@
+std_msgs/Header header
+bool data

common/tier4_calibration_msgs/msg/EstimationResult.msg

@@ -0,0 +1,7 @@
+float64[] result
+float64[] result_mean
+float64[] result_stddev
+float64[] absolute_error
+float64[] mean_absolute_error
+float64[] stddev_absolute_error
+float64[] covariance

common/tier4_calibration_msgs/msg/Float32Stamped.msg

@@ -0,0 +1,2 @@
+std_msgs/Header header
+float32 data

common/tier4_calibration_msgs/msg/TimeDelay.msg

@@ -0,0 +1,8 @@
+float64 time_delay
+float64 correlation_peak
+float64 mean
+float64 stddev
+bool is_valid_data
+float64[] time_delay_by_cross_correlation
+float64[] first_order_model_coefficients
+float64[] second_order_model_coefficients

common/tier4_calibration_msgs/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>tier4_calibration_msgs</name>
+  <version>0.1.0</version>
+  <description>The tier4_calibration_msgs package</description>
+  <maintainer email="[email protected]">Tomoya Kimura</maintainer>
+  <maintainer email="[email protected]">Taiki Tanaka</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+  <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

control/stop_accel_evaluator/CMakeLists.txt

@@ -0,0 +1,20 @@
+cmake_minimum_required(VERSION 3.5)
+project(stop_accel_evaluator)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(stop_accel_evaluator_node SHARED
+  src/stop_accel_evaluator_node.cpp
+)
+
+rclcpp_components_register_node(stop_accel_evaluator_node
+  PLUGIN "stop_accel_evaluator::StopAccelEvaluatorNode"
+  EXECUTABLE stop_accel_evaluator
+)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    launch
+    config
+)

control/stop_accel_evaluator/README.md

@@ -0,0 +1,12 @@
+# Stop Accel Evaluator
+
+The role of this node is to evaluate how smooth it is when a vehicle stops by calculating vehicle acceleration just before stopping.
+
+## How to use
+
+```sh
+ros2 launch stop_accel_evaluator stop_accel_evaluator.launch.xml
+```
+
+Then you can see `stop_accel_evaluator/stop_accel` topic.
+This topic is published only when a vehicle stops.

control/stop_accel_evaluator/config/stop_accel_evaluator.param.yaml

@@ -0,0 +1,8 @@
+/**:
+  ros__parameters:
+    imu_deque_size: 30
+    not_running_acc: 0.005
+    not_running_vel: 0.1
+    stop_valid_imu_accel_num: 4
+    lpf_pitch_gain: 0.95
+    lpf_acc_gain: 0.2

control/stop_accel_evaluator/include/stop_accel_evaluator/stop_accel_evaluator_node.hpp

@@ -0,0 +1,80 @@
+// 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 STOP_ACCEL_EVALUATOR__STOP_ACCEL_EVALUATOR_NODE_HPP_
+#define STOP_ACCEL_EVALUATOR__STOP_ACCEL_EVALUATOR_NODE_HPP_
+
+#include "rclcpp/rclcpp.hpp"
+#include "signal_processing/lowpass_filter_1d.hpp"
+#include "tier4_autoware_utils/ros/self_pose_listener.hpp"
+
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "sensor_msgs/msg/imu.hpp"
+#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp"
+#include "tier4_debug_msgs/msg/float32_stamped.hpp"
+
+#include <deque>
+#include <memory>
+
+namespace stop_accel_evaluator
+{
+using geometry_msgs::msg::TwistStamped;
+using nav_msgs::msg::Odometry;
+using sensor_msgs::msg::Imu;
+using tier4_autoware_utils::SelfPoseListener;
+using tier4_debug_msgs::msg::Float32MultiArrayStamped;
+using tier4_debug_msgs::msg::Float32Stamped;
+
+class StopAccelEvaluatorNode : public rclcpp::Node
+{
+public:
+  explicit StopAccelEvaluatorNode(const rclcpp::NodeOptions & node_options);
+
+private:
+  std::unique_ptr<TwistStamped> current_vel_ptr_{nullptr};
+  std::unique_ptr<TwistStamped> prev_vel_ptr_{nullptr};
+
+  double stop_accel_{0.0};
+  double stop_accel_with_gravity_{0.0};
+
+  rclcpp::Subscription<Odometry>::SharedPtr sub_twist_;
+  rclcpp::Subscription<Imu>::SharedPtr sub_imu_;
+  rclcpp::Subscription<Float32MultiArrayStamped>::SharedPtr sub_debug_values_;
+
+  rclcpp::Publisher<Float32Stamped>::SharedPtr pub_stop_accel_;
+  rclcpp::Publisher<Float32Stamped>::SharedPtr pub_stop_accel_with_gravity_;
+
+  std::deque<double> imu_deque_;
+
+  size_t imu_deque_size_;
+  double not_running_acc_;
+  double not_running_vel_;
+  int stop_valid_imu_accel_num_;
+
+  double current_acc_{0.0};
+  std::shared_ptr<LowpassFilter1d> lpf_acc_{nullptr};
+  std::shared_ptr<LowpassFilter1d> lpf_pitch_{nullptr};
+
+  SelfPoseListener self_pose_listener_{this};
+
+  void onTwist(const Odometry::ConstSharedPtr msg);
+  void onImu(const Imu::ConstSharedPtr msg);
+
+  void calculateStopAccel();
+  void publishStopAccel() const;
+};
+}  // namespace stop_accel_evaluator
+
+#endif  // STOP_ACCEL_EVALUATOR__STOP_ACCEL_EVALUATOR_NODE_HPP_

control/stop_accel_evaluator/launch/stop_accel_evaluator.launch.xml

@@ -0,0 +1,10 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="stop_accel_evaluator_param_path" default="$(find-pkg-share stop_accel_evaluator)/config/stop_accel_evaluator.param.yaml"/>
+
+  <node pkg="stop_accel_evaluator" exec="stop_accel_evaluator" name="stop_accel_evaluator" output="screen">
+    <param from="$(var stop_accel_evaluator_param_path)"/>
+    <remap from="~/twist" to="/localization/kinematic_state"/>
+    <remap from="~/imu" to="/sensing/imu/imu_data"/>
+  </node>
+</launch>

control/stop_accel_evaluator/package.xml

@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>stop_accel_evaluator</name>
+  <version>0.1.0</version>
+  <description>The stop_accel_evaluator</description>
+  <maintainer email="[email protected]">Takayuki Murooka</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <build_depend>autoware_cmake</build_depend>
+
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+  <depend>signal_processing</depend>
+  <depend>std_msgs</depend>
+  <depend>tier4_autoware_utils</depend>
+  <depend>tier4_debug_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>