Add parameter for applying offset to yaw angle

tier4 · Sep 18, 2024 · 2914909 · 2914909
1 parent ec44a6c
commit 2914909
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 0 deletions.
diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp
@@ -128,6 +128,9 @@ class EKFLocalizer : public rclcpp::Node
   double proc_cov_vx_d_;        //!< @brief  discrete process noise in d_vx=0
   double proc_cov_wz_d_;        //!< @brief  discrete process noise in d_wz=0
 
+  /* yaw_offset */
+  double yaw_offset;        //!< @brief  wz process noise
+
   enum IDX {
     X = 0,
     Y = 1,

diff --git a/localization/ekf_localizer/launch/ekf_localizer.launch.xml b/localization/ekf_localizer/launch/ekf_localizer.launch.xml
@@ -26,6 +26,9 @@
   <arg name="proc_stddev_vx_c" default="5.0"/>
   <arg name="proc_stddev_wz_c" default="1.0"/>
 
+  <!-- yaw offset -->
+  <arg name="yaw_offset" default="0.0"/>
+
   <!-- output topic name -->
   <arg name="output_odom_name" default="ekf_odom"/>
   <arg name="output_pose_name" default="ekf_pose"/>

diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp
@@ -63,6 +63,8 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti
     proc_stddev_yaw_bias_c_ = 0.0;
   }
 
+  yaw_offset_ = declare_parameter("yaw_offset", 0.0);
+
   /* convert to continuous to discrete */
   proc_cov_vx_d_ = std::pow(proc_stddev_vx_c_ * ekf_dt_, 2.0);
   proc_cov_wz_d_ = std::pow(proc_stddev_wz_c_ * ekf_dt_, 2.0);
@@ -332,6 +334,7 @@ void EKFLocalizer::callbackTwistWithCovariance(
   geometry_msgs::msg::TwistStamped twist;
   twist.header = msg->header;
   twist.twist = msg->twist.twist;
+  twist.twist.angular.z = twist.twist.angular.z + yaw_offset_; // add yaw_offset_
   current_twist_ptr_ = std::make_shared<geometry_msgs::msg::TwistStamped>(twist);
   current_twist_covariance_ = msg->twist.covariance;
 }