Merge branch 'main' into multi-object-tracker-branch

Author: technolojin

.cppcheck_suppressions

@@ -1,14 +1,11 @@
 *:*/test/*
 
 checkersReport
-constParameterReference
-funcArgNamesDifferent
 functionConst
 functionStatic
 missingInclude
 missingIncludeSystem
 noConstructor
-passedByValue
 // cspell: ignore uninit
 uninitMemberVar
 unknownMacro

.github/CODEOWNERS

@@ -83,17 +83,17 @@ localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/*
 localization/autoware_landmark_based_localizer/autoware_landmark_manager/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 localization/autoware_pose_covariance_modifier/** melike@leodrive.ai
 localization/autoware_stop_filter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
+localization/autoware_twist2accel/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 localization/ekf_localizer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp
 localization/geo_pose_projector/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 localization/gyro_odometer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 localization/localization_error_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 localization/localization_util/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 localization/ndt_scan_matcher/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
-localization/pose2twist/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
+localization/autoware_pose2twist/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 localization/pose_estimator_arbiter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 localization/pose_initializer/** anh.nguyen.2@tier4.jp isamu.takagi@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 localization/pose_instability_detector/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
-localization/autoware_twist2accel/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 localization/yabloc/yabloc_common/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 localization/yabloc/yabloc_image_processing/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 localization/yabloc/yabloc_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
@@ -131,7 +131,7 @@ perception/autoware_radar_object_clustering/** satoshi.tanaka@tier4.jp shunsuke.
 perception/autoware_radar_object_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
 perception/autoware_radar_tracks_msgs_converter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp
 perception/autoware_raindrop_cluster_filter/** dai.nguyen@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
-perception/autoware_shape_estimation/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
+perception/autoware_shape_estimation/** kcolak@leodrive.ai yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
 perception/autoware_simple_object_merger/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp
 perception/autoware_tensorrt_classifier/** kotaro.uetake@tier4.jp shunsuke.miura@tier4.jp
 perception/autoware_tensorrt_yolox/** dan.umeda@tier4.jp manato.hirabayashi@tier4.jp

common/autoware_universe_utils/src/geometry/random_convex_polygon.cpp

@@ -31,8 +31,11 @@ struct VectorsWithMin
 };
 
 VectorsWithMin prepare_coordinate_vectors(
-  const size_t nb_vertices, std::uniform_real_distribution<double> & random_double,
-  std::uniform_int_distribution<int> & random_bool, std::default_random_engine & random_engine)
+  const size_t nb_vertices,
+  std::uniform_real_distribution<double> &
+    random_double,                                   // cppcheck-suppress constParameterReference
+  std::uniform_int_distribution<int> & random_bool,  // cppcheck-suppress constParameterReference
+  std::default_random_engine & random_engine)
 {
   std::vector<double> v;
   for (auto i = 0UL; i < nb_vertices; ++i) {

common/tensorrt_common/src/tensorrt_common.cpp

@@ -137,6 +137,7 @@ void TrtCommon::setup()
     is_initialized_ = false;
     return;
   }
+  // cppcheck-suppress unreadVariable
   std::string engine_path = model_file_path_;
   if (model_file_path_.extension() == ".engine") {
     std::cout << "Load ... " << model_file_path_ << std::endl;
@@ -193,6 +194,7 @@ void TrtCommon::setup()
       logger_.stop_throttle(log_thread);
       logger_.log(nvinfer1::ILogger::Severity::kINFO, "End build engine");
     }
+    // cppcheck-suppress unreadVariable
     engine_path = cache_engine_path;
   } else {
     is_initialized_ = false;

control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/src/proxima_calc.cpp

@@ -178,15 +178,15 @@ class transform_model_to_eigen
   Eigen::VectorXd bias_linear_finalize_;
   Eigen::MatrixXd A_linear_reg_;
   Eigen::VectorXd b_linear_reg_;
-  int deg_;
-  int acc_delay_step_;
-  int steer_delay_step_;
-  int acc_ctrl_queue_size_;
-  int steer_ctrl_queue_size_;
-  int steer_ctrl_queue_size_core_;
-  double vel_normalize_;
-  double acc_normalize_;
-  double steer_normalize_;
+  int deg_{};
+  int acc_delay_step_{};
+  int steer_delay_step_{};
+  int acc_ctrl_queue_size_{};
+  int steer_ctrl_queue_size_{};
+  int steer_ctrl_queue_size_core_{};
+  double vel_normalize_{};
+  double acc_normalize_{};
+  double steer_normalize_{};
   static constexpr double max_acc_error_ = 20.0;
   static constexpr double max_steer_error_ = 20.0;
 
@@ -611,15 +611,15 @@ class transform_model_with_memory_to_eigen
   Eigen::VectorXd bias_linear_finalize_;
   Eigen::MatrixXd A_linear_reg_;
   Eigen::VectorXd b_linear_reg_;
-  int deg_;
-  int acc_delay_step_;
-  int steer_delay_step_;
-  int acc_ctrl_queue_size_;
-  int steer_ctrl_queue_size_;
-  int steer_ctrl_queue_size_core_;
-  double vel_normalize_;
-  double acc_normalize_;
-  double steer_normalize_;
+  int deg_{};
+  int acc_delay_step_{};
+  int steer_delay_step_{};
+  int acc_ctrl_queue_size_{};
+  int steer_ctrl_queue_size_{};
+  int steer_ctrl_queue_size_core_{};
+  double vel_normalize_{};
+  double acc_normalize_{};
+  double steer_normalize_{};
 
   static constexpr double max_acc_error_ = 20.0;
   static constexpr double max_steer_error_ = 20.0;

control/autoware_vehicle_cmd_gate/src/adapi_pause_interface.cpp

@@ -29,7 +29,7 @@ AdapiPauseInterface::AdapiPauseInterface(rclcpp::Node * node) : node_(node)
   publish();
 }
 
-bool AdapiPauseInterface::is_paused()
+bool AdapiPauseInterface::is_paused() const
 {
   return is_paused_;
 }

control/autoware_vehicle_cmd_gate/src/adapi_pause_interface.hpp

@@ -35,7 +35,7 @@ class AdapiPauseInterface
 
 public:
   explicit AdapiPauseInterface(rclcpp::Node * node);
-  bool is_paused();
+  bool is_paused() const;
   void publish();
   void update(const Control & control);
 

control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.cpp

@@ -252,8 +252,7 @@ double VehicleCmdFilter::calcLatAcc(const Control & cmd, const double v) const
   return v * v * std::tan(cmd.lateral.steering_tire_angle) / param_.wheel_base;
 }
 
-double VehicleCmdFilter::limitDiff(
-  const double curr, const double prev, const double diff_lim) const
+double VehicleCmdFilter::limitDiff(const double curr, const double prev, const double diff_lim)
 {
   double diff = std::max(std::min(curr - prev, diff_lim), -diff_lim);
   return prev + diff;

control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.hpp

@@ -75,7 +75,7 @@ class VehicleCmdFilter
   static IsFilterActivated checkIsActivated(
     const Control & c1, const Control & c2, const double tol = 1.0e-3);
 
-  Control getPrevCmd() { return prev_cmd_; }
+  Control getPrevCmd() const { return prev_cmd_; }
 
 private:
   VehicleCmdFilterParam param_;
@@ -87,7 +87,7 @@ class VehicleCmdFilter
   double calcLatAcc(const Control & cmd) const;
   double calcLatAcc(const Control & cmd, const double v) const;
   double calcSteerFromLatacc(const double v, const double latacc) const;
-  double limitDiff(const double curr, const double prev, const double diff_lim) const;
+  static double limitDiff(const double curr, const double prev, const double diff_lim);
 
   double interpolateFromSpeed(const LimitArray & limits) const;
   double getLonAccLim() const;

evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp

@@ -30,9 +30,8 @@ size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, cons
   const TrajectoryPoint & curr_p = traj.points.at(curr_id);
 
   size_t target_id = curr_id;
-  double current_distance = 0.0;
   for (size_t traj_id = curr_id + 1; traj_id < traj.points.size(); ++traj_id) {
-    current_distance = calcDistance2d(traj.points.at(traj_id), curr_p);
+    double current_distance = calcDistance2d(traj.points.at(traj_id), curr_p);
     if (current_distance >= distance) {
       target_id = traj_id;
       break;

localization/pose2twist/CMakeLists.txt localization/autoware_pose2twist/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.14)
-project(pose2twist)
+project(autoware_pose2twist)
 
 find_package(autoware_cmake REQUIRED)
 autoware_package()
@@ -9,7 +9,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
 )
 
 rclcpp_components_register_node(${PROJECT_NAME}
-  PLUGIN "Pose2Twist"
+  PLUGIN "autoware::pose2twist::Pose2Twist"
   EXECUTABLE ${PROJECT_NAME}_node
   EXECUTOR SingleThreadedExecutor
 )
@@ -19,6 +19,9 @@ if(BUILD_TESTING)
   ament_auto_add_gtest(test_angular_velocity
     test/test_angular_velocity.cpp
   )
+  target_include_directories(test_angular_velocity PRIVATE
+    src
+  )
 endif()
 
 ament_auto_package(

localization/pose2twist/README.md localization/autoware_pose2twist/README.md

@@ -1,8 +1,8 @@
-# pose2twist
+# autoware_pose2twist
 
 ## Purpose
 
-This `pose2twist` calculates the velocity from the input pose history. In addition to the computed twist, this node outputs the linear-x and angular-z components as a float message to simplify debugging.
+This `autoware_pose2twist` calculates the velocity from the input pose history. In addition to the computed twist, this node outputs the linear-x and angular-z components as a float message to simplify debugging.
 
 The `twist.linear.x` is calculated as `sqrt(dx * dx + dy * dy + dz * dz) / dt`, and the values in the `y` and `z` fields are zero.
 The `twist.angular` is calculated as `relative_rotation_vector / dt` for each field.

localization/pose2twist/launch/pose2twist.launch.xml localization/autoware_pose2twist/launch/pose2twist.launch.xml

@@ -2,7 +2,7 @@
   <arg name="input_pose_topic" default="/localization/pose_estimator/pose" description=""/>
   <arg name="output_twist_topic" default="/estimate_twist" description=""/>
 
-  <node pkg="pose2twist" exec="pose2twist_node" output="both">
+  <node pkg="autoware_pose2twist" exec="autoware_pose2twist_node" output="both">
     <remap from="pose" to="$(var input_pose_topic)"/>
     <remap from="twist" to="$(var output_twist_topic)"/>
   </node>

localization/pose2twist/package.xml localization/autoware_pose2twist/package.xml

@@ -1,9 +1,9 @@
 <?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>pose2twist</name>
+  <name>autoware_pose2twist</name>
   <version>0.1.0</version>
-  <description>The pose2twist package</description>
+  <description>The autoware_pose2twist package</description>
   <maintainer email="yamato.ando@tier4.jp">Yamato Ando</maintainer>
   <maintainer email="masahiro.sakamoto@tier4.jp">Masahiro Sakamoto</maintainer>
   <maintainer email="kento.yabuuchi.2@tier4.jp">Kento Yabuuchi</maintainer>

localization/pose2twist/src/pose2twist_core.cpp localization/autoware_pose2twist/src/pose2twist_core.cpp

@@ -12,12 +12,14 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "pose2twist/pose2twist_core.hpp"
+#include "pose2twist_core.hpp"
 
 #include <cmath>
 #include <cstddef>
 #include <functional>
 
+namespace autoware::pose2twist
+{
 Pose2Twist::Pose2Twist(const rclcpp::NodeOptions & options) : rclcpp::Node("pose2twist", options)
 {
   using std::placeholders::_1;
@@ -113,6 +115,7 @@ void Pose2Twist::callback_pose(geometry_msgs::msg::PoseStamped::SharedPtr pose_m
   angular_z_msg.data = static_cast<float>(twist_msg.twist.angular.z);
   angular_z_pub_->publish(angular_z_msg);
 }
+}  // namespace autoware::pose2twist
 
 #include <rclcpp_components/register_node_macro.hpp>
-RCLCPP_COMPONENTS_REGISTER_NODE(Pose2Twist)
+RCLCPP_COMPONENTS_REGISTER_NODE(autoware::pose2twist::Pose2Twist)

localization/pose2twist/include/pose2twist/pose2twist_core.hpp localization/autoware_pose2twist/src/pose2twist_core.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef POSE2TWIST__POSE2TWIST_CORE_HPP_
-#define POSE2TWIST__POSE2TWIST_CORE_HPP_
+#ifndef POSE2TWIST_CORE_HPP_
+#define POSE2TWIST_CORE_HPP_
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -27,6 +27,8 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #endif
 
+namespace autoware::pose2twist
+{
 // Compute the relative rotation of q2 from q1 as a rotation vector
 geometry_msgs::msg::Vector3 compute_relative_rotation_vector(
   const tf2::Quaternion & q1, const tf2::Quaternion & q2);
@@ -45,5 +47,6 @@ class Pose2Twist : public rclcpp::Node
   rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr linear_x_pub_;
   rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr angular_z_pub_;
 };
+}  // namespace autoware::pose2twist
 
-#endif  // POSE2TWIST__POSE2TWIST_CORE_HPP_
+#endif  // POSE2TWIST_CORE_HPP_

localization/pose2twist/test/test_angular_velocity.cpp localization/autoware_pose2twist/test/test_angular_velocity.cpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "pose2twist/pose2twist_core.hpp"
+#include "pose2twist_core.hpp"
 
 #include <gtest/gtest.h>
 
@@ -102,7 +102,7 @@ TEST(AngularVelocityFromQuaternion, CheckNumericalValidity)
 
     // Calculate the relative rotation between the initial and final quaternion
     const geometry_msgs::msg::Vector3 rotation_vector =
-      compute_relative_rotation_vector(initial_q, final_q);
+      autoware::pose2twist::compute_relative_rotation_vector(initial_q, final_q);
 
     EXPECT_NEAR(rotation_vector.x, expected_axis.x() * expected_angle, acceptable_error);
     EXPECT_NEAR(rotation_vector.y, expected_axis.y() * expected_angle, acceptable_error);

localization/autoware_twist2accel/src/twist2accel.hpp

@@ -42,7 +42,7 @@ namespace autoware::twist2accel
 class Twist2Accel : public rclcpp::Node
 {
 public:
-  explicit Twist2Accel(const rclcpp::NodeOptions & options);
+  explicit Twist2Accel(const rclcpp::NodeOptions & node_options);
 
 private:
   rclcpp::Publisher<geometry_msgs::msg::AccelWithCovarianceStamped>::SharedPtr

localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp

@@ -239,6 +239,11 @@ class EKFLocalizer : public rclcpp::Node
 
   autoware::universe_utils::StopWatch<std::chrono::milliseconds> stop_watch_;
 
+  /**
+   * @brief last angular velocity for compensating rph with delay
+   */
+  tf2::Vector3 last_angular_velocity_;
+
   friend class EKFLocalizerTestSuite;  // for test code
 };
 #endif  // EKF_LOCALIZER__EKF_LOCALIZER_HPP_

localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp

@@ -29,6 +29,8 @@
 #include <geometry_msgs/msg/twist_stamped.hpp>
 #include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
 
+#include <tf2/utils.h>
+
 #include <memory>
 #include <vector>
 
@@ -77,8 +79,8 @@ class EKFModule
   bool measurement_update_twist(
     const TwistWithCovariance & twist, const rclcpp::Time & t_curr,
     EKFDiagnosticInfo & twist_diag_info);
-  geometry_msgs::msg::PoseWithCovarianceStamped compensate_pose_with_z_delay(
-    const PoseWithCovariance & pose, const double delay_time);
+  geometry_msgs::msg::PoseWithCovarianceStamped compensate_rph_with_delay(
+    const PoseWithCovariance & pose, tf2::Vector3 last_angular_velocity, const double delay_time);
 
 private:
   TimeDelayKalmanFilter kalman_filter_;

localization/ekf_localizer/src/ekf_localizer.cpp

@@ -47,7 +47,8 @@ EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options)
   params_(this),
   ekf_dt_(params_.ekf_dt),
   pose_queue_(params_.pose_smoothing_steps),
-  twist_queue_(params_.twist_smoothing_steps)
+  twist_queue_(params_.twist_smoothing_steps),
+  last_angular_velocity_(0.0, 0.0, 0.0)
 {
   /* convert to continuous to discrete */
   proc_cov_vx_d_ = std::pow(params_.proc_stddev_vx_c * ekf_dt_, 2.0);
@@ -187,11 +188,13 @@ void EKFLocalizer::timer_callback()
       if (is_updated) {
         pose_is_updated = true;
 
-        // Update Simple 1D filter with considering change of z value due to measurement pose delay
+        // Update Simple 1D filter with considering change of roll, pitch and height (position z)
+        // values due to measurement pose delay
         const double delay_time =
           (current_time - pose->header.stamp).seconds() + params_.pose_additional_delay;
-        const auto pose_with_z_delay = ekf_module_->compensate_pose_with_z_delay(*pose, delay_time);
-        update_simple_1d_filters(pose_with_z_delay, params_.pose_smoothing_steps);
+        auto pose_with_rph_delay_compensation =
+          ekf_module_->compensate_rph_with_delay(*pose, last_angular_velocity_, delay_time);
+        update_simple_1d_filters(pose_with_rph_delay_compensation, params_.pose_smoothing_steps);
       }
     }
     DEBUG_INFO(
@@ -222,6 +225,10 @@ void EKFLocalizer::timer_callback()
         ekf_module_->measurement_update_twist(*twist, current_time, twist_diag_info_);
       if (is_updated) {
         twist_is_updated = true;
+        last_angular_velocity_ = tf2::Vector3(
+          twist->twist.twist.angular.x, twist->twist.twist.angular.y, twist->twist.twist.angular.z);
+      } else {
+        last_angular_velocity_ = tf2::Vector3(0.0, 0.0, 0.0);
       }
     }
     DEBUG_INFO(