diff --git a/aichallenge/pkill_all.sh b/aichallenge/pkill_all.sh
new file mode 100644
index 00000000..81320523
--- /dev/null
+++ b/aichallenge/pkill_all.sh
@@ -0,0 +1,17 @@
+pkill -9 robot_state_pub
+pkill -9 vehicle_velocit
+pkill -9 imu_gnss_poser_
+pkill -9 twist2accel
+pkill -9 empty_objects_p
+pkill -9 goal_pose_visua
+pkill -9 path_to_traject
+pkill -9 goal_pose_sette
+pkill -9 ros2
+pkill -9 python3
+pkill -9 rviz2
+pkill -9 ekf_localizer
+pkill -9 mission_planner
+pkill -9 component_conta
+pkill -9 simple_pure_pur
+pkill -9 initial_pose_ad
+pkill -9 routing_adaptor
diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml
index f0ccb859..8e2fe222 100644
--- a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml
+++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml
@@ -41,6 +41,15 @@
+
+
+
+
+
+
+
+
+
@@ -48,7 +57,7 @@
-
+
@@ -58,8 +67,8 @@
-
-
+
+
@@ -197,8 +206,8 @@
-
-
+
+
diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/map/lanelet2_map.osm b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/map/lanelet2_map.osm
index fc70d3ad..0c9222a4 100644
--- a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/map/lanelet2_map.osm
+++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/map/lanelet2_map.osm
@@ -1,6 +1,6 @@
-
+
@@ -4801,58 +4801,46 @@
-
+
-
+
-
+
-
-
+
+
-
+
-
-
+
+
-
+
-
-
+
+
-
+
-
+
-
-
-
-
-
-
-
-
-
-
-
-
-
+
-
+
-
+
-
-
+
+
@@ -5962,8 +5950,6 @@
-
-
diff --git a/aichallenge/workspace/src/aichallenge_submit/gyro_odometer/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/gyro_odometer/CMakeLists.txt
new file mode 100644
index 00000000..59a5e112
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_submit/gyro_odometer/CMakeLists.txt
@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 3.14)
+project(gyro_odometer)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_executable(${PROJECT_NAME}
+ src/gyro_odometer_node.cpp
+ src/gyro_odometer_core.cpp
+)
+
+target_link_libraries(${PROJECT_NAME} fmt)
+
+ament_auto_add_library(gyro_odometer_node SHARED
+ src/gyro_odometer_core.cpp
+)
+
+if(BUILD_TESTING)
+ ament_add_ros_isolated_gtest(test_gyro_odometer
+ test/test_main.cpp
+ test/test_gyro_odometer_pubsub.cpp
+ test/test_gyro_odometer_helper.cpp
+ )
+ ament_target_dependencies(test_gyro_odometer
+ rclcpp
+ )
+ target_link_libraries(test_gyro_odometer
+ gyro_odometer_node
+ )
+endif()
+
+
+ament_auto_package(INSTALL_TO_SHARE
+ launch
+)
diff --git a/aichallenge/workspace/src/aichallenge_submit/gyro_odometer/README.md b/aichallenge/workspace/src/aichallenge_submit/gyro_odometer/README.md
new file mode 100644
index 00000000..ec50a113
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_submit/gyro_odometer/README.md
@@ -0,0 +1,39 @@
+# gyro_odometer
+
+## Purpose
+
+`gyro_odometer` is the package to estimate twist by combining imu and vehicle speed.
+
+## Inputs / Outputs
+
+### Input
+
+| Name | Type | Description |
+| ------------------------------- | ------------------------------------------------ | ---------------------------------- |
+| `vehicle/twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | twist with covariance from vehicle |
+| `imu` | `sensor_msgs::msg::Imu` | imu from sensor |
+
+### Output
+
+| Name | Type | Description |
+| ----------------------- | ------------------------------------------------ | ------------------------------- |
+| `twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | estimated twist with covariance |
+
+## Parameters
+
+| Parameter | Type | Description |
+| --------------------- | ------ | -------------------------------- |
+| `output_frame` | String | output's frame id |
+| `message_timeout_sec` | Double | delay tolerance time for message |
+
+## Assumptions / Known limits
+
+- [Assumption] The frame_id of input twist message must be set to base_link.
+
+- [Assumption] The covariance in the input messages must be properly assigned.
+
+- [Assumption] The angular velocity is set to zero if both the longitudinal vehicle velocity and the angular velocity around the yaw axis are sufficiently small. This is for suppression of the IMU angular velocity bias. Without this process, we misestimate the vehicle status when stationary.
+
+- [Limitation] The frequency of the output messages depends on the frequency of the input IMU message.
+
+- [Limitation] We cannot produce reliable values for the lateral and vertical velocities. Therefore we assign large values to the corresponding elements in the output covariance matrix.
diff --git a/aichallenge/workspace/src/aichallenge_submit/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp b/aichallenge/workspace/src/aichallenge_submit/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp
new file mode 100644
index 00000000..b0c7a5c4
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_submit/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp
@@ -0,0 +1,76 @@
+// Copyright 2015-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 GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_
+#define GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_
+
+#include "tier4_autoware_utils/ros/transform_listener.hpp"
+#include "tier4_autoware_utils/tier4_autoware_utils.hpp"
+
+#include
+
+#include
+#include
+#include
+
+#include
+#ifdef ROS_DISTRO_GALACTIC
+#include
+#else
+#include
+#endif
+
+#include
+#include
+#include
+
+class GyroOdometer : public rclcpp::Node
+{
+private:
+ using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX;
+
+public:
+ explicit GyroOdometer(const rclcpp::NodeOptions & options);
+ ~GyroOdometer();
+
+private:
+ void callbackVehicleTwist(
+ const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_msg_ptr);
+ void callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr);
+ void publishData(const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw);
+
+ rclcpp::Subscription::SharedPtr
+ vehicle_twist_sub_;
+ rclcpp::Subscription::SharedPtr imu_sub_;
+
+ rclcpp::Publisher::SharedPtr twist_raw_pub_;
+ rclcpp::Publisher::SharedPtr
+ twist_with_covariance_raw_pub_;
+
+ rclcpp::Publisher::SharedPtr twist_pub_;
+ rclcpp::Publisher::SharedPtr
+ twist_with_covariance_pub_;
+
+ std::shared_ptr transform_listener_;
+
+ std::string output_frame_;
+ double message_timeout_sec_;
+
+ bool vehicle_twist_arrived_;
+ bool imu_arrived_;
+ std::deque vehicle_twist_queue_;
+ std::deque gyro_queue_;
+};
+
+#endif // GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_
diff --git a/aichallenge/workspace/src/aichallenge_submit/gyro_odometer/launch/gyro_odometer.launch.xml b/aichallenge/workspace/src/aichallenge_submit/gyro_odometer/launch/gyro_odometer.launch.xml
new file mode 100644
index 00000000..aeb50527
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_submit/gyro_odometer/launch/gyro_odometer.launch.xml
@@ -0,0 +1,29 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/aichallenge/workspace/src/aichallenge_submit/gyro_odometer/package.xml b/aichallenge/workspace/src/aichallenge_submit/gyro_odometer/package.xml
new file mode 100644
index 00000000..5aa85a8c
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_submit/gyro_odometer/package.xml
@@ -0,0 +1,34 @@
+
+
+
+ gyro_odometer
+ 0.1.0
+ The gyro_odometer package as a ROS2 node
+ Yamato Ando
+ Apache License 2.0
+ Yamato Ando
+
+ ament_cmake_auto
+
+ autoware_cmake
+
+ fmt
+ geometry_msgs
+ sensor_msgs
+ std_msgs
+ tf2
+ tf2_geometry_msgs
+ tf2_ros
+ tier4_autoware_utils
+
+ rclcpp
+ rclcpp_components
+
+ ament_cmake_ros
+ ament_lint_auto
+ autoware_lint_common
+
+
+ ament_cmake
+
+
diff --git a/aichallenge/workspace/src/aichallenge_submit/gyro_odometer/src/gyro_odometer_core.cpp b/aichallenge/workspace/src/aichallenge_submit/gyro_odometer/src/gyro_odometer_core.cpp
new file mode 100644
index 00000000..0f3fd80f
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_submit/gyro_odometer/src/gyro_odometer_core.cpp
@@ -0,0 +1,271 @@
+// Copyright 2015-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.
+
+#include "gyro_odometer/gyro_odometer_core.hpp"
+
+#ifdef ROS_DISTRO_GALACTIC
+#include
+#else
+#include
+#endif
+#include
+
+#include
+#include
+#include
+
+std::array transformCovariance(const std::array & cov)
+{
+ using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX;
+
+
+ std::array cov_transformed;
+ cov_transformed.fill(0.);
+ cov_transformed[COV_IDX::X_X] = max_cov;
+ cov_transformed[COV_IDX::Y_Y] = max_cov;
+ cov_transformed[COV_IDX::Z_Z] = max_cov;
+ return cov_transformed;
+}
+
+geometry_msgs::msg::TwistWithCovarianceStamped concatGyroAndOdometer(
+ const std::deque & vehicle_twist_queue,
+ const std::deque & gyro_queue)
+{
+ using COV_IDX_XYZ = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX;
+ using COV_IDX_XYZRPY = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
+
+ double vx_mean = 0;
+ geometry_msgs::msg::Vector3 gyro_mean{};
+ double vx_covariance_original = 0;
+ geometry_msgs::msg::Vector3 gyro_covariance_original{};
+ for (const auto & vehicle_twist : vehicle_twist_queue) {
+ vx_mean += vehicle_twist.twist.twist.linear.x;
+ vx_covariance_original += vehicle_twist.twist.covariance[0 * 6 + 0];
+ }
+ vx_mean /= vehicle_twist_queue.size();
+ vx_covariance_original /= vehicle_twist_queue.size();
+
+ for (const auto & gyro : gyro_queue) {
+ gyro_mean.x += gyro.angular_velocity.x;
+ gyro_mean.y += gyro.angular_velocity.y;
+ gyro_mean.z += gyro.angular_velocity.z;
+ gyro_covariance_original.x += gyro.angular_velocity_covariance[COV_IDX_XYZ::X_X];
+ gyro_covariance_original.y += gyro.angular_velocity_covariance[COV_IDX_XYZ::Y_Y];
+ gyro_covariance_original.z += gyro.angular_velocity_covariance[COV_IDX_XYZ::Z_Z];
+ }
+ gyro_mean.x /= gyro_queue.size();
+ gyro_mean.y /= gyro_queue.size();
+ gyro_mean.z /= gyro_queue.size();
+ gyro_covariance_original.x /= gyro_queue.size();
+ gyro_covariance_original.y /= gyro_queue.size();
+ gyro_covariance_original.z /= gyro_queue.size();
+
+ geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov;
+ const auto latest_vehicle_twist_stamp = rclcpp::Time(vehicle_twist_queue.back().header.stamp);
+ const auto latest_imu_stamp = rclcpp::Time(gyro_queue.back().header.stamp);
+ if (latest_vehicle_twist_stamp < latest_imu_stamp) {
+ twist_with_cov.header.stamp = latest_imu_stamp;
+ } else {
+ twist_with_cov.header.stamp = latest_vehicle_twist_stamp;
+ }
+ twist_with_cov.header.frame_id = gyro_queue.front().header.frame_id;
+ twist_with_cov.twist.twist.linear.x = vx_mean;
+ twist_with_cov.twist.twist.angular = gyro_mean;
+
+ // From a statistical point of view, here we reduce the covariances according to the number of
+ // observed data
+ twist_with_cov.twist.covariance[COV_IDX_XYZRPY::X_X] =
+ vx_covariance_original / vehicle_twist_queue.size();
+ twist_with_cov.twist.covariance[COV_IDX_XYZRPY::Y_Y] = 100000.0;
+ twist_with_cov.twist.covariance[COV_IDX_XYZRPY::Z_Z] = 100000.0;
+ twist_with_cov.twist.covariance[COV_IDX_XYZRPY::ROLL_ROLL] =
+ gyro_covariance_original.x / gyro_queue.size();
+ twist_with_cov.twist.covariance[COV_IDX_XYZRPY::PITCH_PITCH] =
+ gyro_covariance_original.y / gyro_queue.size();
+ twist_with_cov.twist.covariance[COV_IDX_XYZRPY::YAW_YAW] =
+ gyro_covariance_original.z / gyro_queue.size();
+
+ return twist_with_cov;
+}
+
+GyroOdometer::GyroOdometer(const rclcpp::NodeOptions & options)
+: Node("gyro_odometer", options),
+ output_frame_(declare_parameter("base_link", "base_link")),
+ message_timeout_sec_(declare_parameter("message_timeout_sec", 0.2)),
+ vehicle_twist_arrived_(false),
+ imu_arrived_(false)
+{
+ transform_listener_ = std::make_shared(this);
+
+ vehicle_twist_sub_ = create_subscription(
+ "vehicle/twist_with_covariance", rclcpp::QoS{100},
+ std::bind(&GyroOdometer::callbackVehicleTwist, this, std::placeholders::_1));
+
+ imu_sub_ = create_subscription(
+ "imu", rclcpp::QoS{100}, std::bind(&GyroOdometer::callbackImu, this, std::placeholders::_1));
+
+ twist_raw_pub_ = create_publisher("twist_raw", rclcpp::QoS{10});
+ twist_with_covariance_raw_pub_ = create_publisher(
+ "twist_with_covariance_raw", rclcpp::QoS{10});
+
+ twist_pub_ = create_publisher("twist", rclcpp::QoS{10});
+ twist_with_covariance_pub_ = create_publisher(
+ "twist_with_covariance", rclcpp::QoS{10});
+
+ // TODO(YamatoAndo) createTimer
+}
+
+GyroOdometer::~GyroOdometer() {}
+
+void GyroOdometer::callbackVehicleTwist(
+ const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_ptr)
+{
+ vehicle_twist_arrived_ = true;
+ if (!imu_arrived_) {
+ RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Imu msg is not subscribed");
+ vehicle_twist_queue_.clear();
+ gyro_queue_.clear();
+ return;
+ }
+
+ const double twist_dt = std::abs((this->now() - vehicle_twist_ptr->header.stamp).seconds());
+ if (twist_dt > message_timeout_sec_) {
+ const std::string error_msg = fmt::format(
+ "Twist msg is timeout. twist_dt: {}[sec], tolerance {}[sec]", twist_dt, message_timeout_sec_);
+ RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str());
+ vehicle_twist_queue_.clear();
+ gyro_queue_.clear();
+ return;
+ }
+
+ vehicle_twist_queue_.push_back(*vehicle_twist_ptr);
+
+ if (gyro_queue_.empty()) return;
+ const double imu_dt = std::abs((this->now() - gyro_queue_.back().header.stamp).seconds());
+ if (imu_dt > message_timeout_sec_) {
+ const std::string error_msg = fmt::format(
+ "Imu msg is timeout. twist_dt: {}[sec], tolerance {}[sec]", imu_dt, message_timeout_sec_);
+ RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str());
+ vehicle_twist_queue_.clear();
+ gyro_queue_.clear();
+ return;
+ }
+
+ const geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov_raw =
+ concatGyroAndOdometer(vehicle_twist_queue_, gyro_queue_);
+ publishData(twist_with_cov_raw);
+ vehicle_twist_queue_.clear();
+ gyro_queue_.clear();
+}
+
+void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr)
+{
+ imu_arrived_ = true;
+ if (!vehicle_twist_arrived_) {
+ RCLCPP_WARN_THROTTLE(
+ this->get_logger(), *this->get_clock(), 1000, "Twist msg is not subscribed");
+ vehicle_twist_queue_.clear();
+ gyro_queue_.clear();
+ return;
+ }
+
+ const double imu_dt = std::abs((this->now() - imu_msg_ptr->header.stamp).seconds());
+ if (imu_dt > message_timeout_sec_) {
+ const std::string error_msg = fmt::format(
+ "Imu msg is timeout. imu_dt: {}[sec], tolerance {}[sec]", imu_dt, message_timeout_sec_);
+ RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str());
+ vehicle_twist_queue_.clear();
+ gyro_queue_.clear();
+ return;
+ }
+
+ geometry_msgs::msg::TransformStamped::ConstSharedPtr tf_imu2base_ptr =
+ transform_listener_->getLatestTransform(imu_msg_ptr->header.frame_id, output_frame_);
+ if (!tf_imu2base_ptr) {
+ RCLCPP_ERROR(
+ this->get_logger(), "Please publish TF %s to %s", output_frame_.c_str(),
+ (imu_msg_ptr->header.frame_id).c_str());
+ vehicle_twist_queue_.clear();
+ gyro_queue_.clear();
+ return;
+ }
+
+ geometry_msgs::msg::Vector3Stamped angular_velocity;
+ angular_velocity.header = imu_msg_ptr->header;
+ angular_velocity.vector = imu_msg_ptr->angular_velocity;
+
+ geometry_msgs::msg::Vector3Stamped transformed_angular_velocity;
+ transformed_angular_velocity.header = tf_imu2base_ptr->header;
+ tf2::doTransform(angular_velocity, transformed_angular_velocity, *tf_imu2base_ptr);
+
+ sensor_msgs::msg::Imu gyro_base_link;
+ gyro_base_link.header = imu_msg_ptr->header;
+ gyro_base_link.header.frame_id = output_frame_;
+ gyro_base_link.angular_velocity = transformed_angular_velocity.vector;
+ gyro_base_link.angular_velocity_covariance =
+ transformCovariance(imu_msg_ptr->angular_velocity_covariance);
+
+ gyro_queue_.push_back(gyro_base_link);
+
+ if (vehicle_twist_queue_.empty()) return;
+ const double twist_dt =
+ std::abs((this->now() - vehicle_twist_queue_.back().header.stamp).seconds());
+ if (twist_dt > message_timeout_sec_) {
+ const std::string error_msg = fmt::format(
+ "Twist msg is timeout. twist_dt: {}[sec], tolerance {}[sec]", twist_dt, message_timeout_sec_);
+ RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str());
+ vehicle_twist_queue_.clear();
+ gyro_queue_.clear();
+ return;
+ }
+
+ const geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov_raw =
+ concatGyroAndOdometer(vehicle_twist_queue_, gyro_queue_);
+ publishData(twist_with_cov_raw);
+ vehicle_twist_queue_.clear();
+ gyro_queue_.clear();
+}
+
+void GyroOdometer::publishData(
+ const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw)
+{
+ geometry_msgs::msg::TwistStamped twist_raw;
+ twist_raw.header = twist_with_cov_raw.header;
+ twist_raw.twist = twist_with_cov_raw.twist.twist;
+
+ twist_raw_pub_->publish(twist_raw);
+ twist_with_covariance_raw_pub_->publish(twist_with_cov_raw);
+
+ geometry_msgs::msg::TwistWithCovarianceStamped twist_with_covariance = twist_with_cov_raw;
+ geometry_msgs::msg::TwistStamped twist = twist_raw;
+
+ // clear imu yaw bias if vehicle is stopped
+ if (
+ std::fabs(twist_with_cov_raw.twist.twist.angular.z) < 0.01 &&
+ std::fabs(twist_with_cov_raw.twist.twist.linear.x) < 0.01) {
+ twist.twist.angular.x = 0.0;
+ twist.twist.angular.y = 0.0;
+ twist.twist.angular.z = 0.0;
+ twist_with_covariance.twist.twist.angular.x = 0.0;
+ twist_with_covariance.twist.twist.angular.y = 0.0;
+ twist_with_covariance.twist.twist.angular.z = 0.0;
+ }
+
+ twist.twist.angular.x = 0.0;
+ twist.twist.angular.y = 0.0;
+ twist_with_covariance.twist.twist.angular.x = 0.0;
+ twist_with_covariance.twist.twist.angular.y = 0.0;
+ twist_pub_->publish(twist);
+ twist_with_covariance_pub_->publish(twist_with_covariance);
+}
diff --git a/aichallenge/workspace/src/aichallenge_submit/gyro_odometer/src/gyro_odometer_node.cpp b/aichallenge/workspace/src/aichallenge_submit/gyro_odometer/src/gyro_odometer_node.cpp
new file mode 100644
index 00000000..5bb62415
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_submit/gyro_odometer/src/gyro_odometer_node.cpp
@@ -0,0 +1,30 @@
+// Copyright 2015-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.
+
+#include "gyro_odometer/gyro_odometer_core.hpp"
+
+#include
+
+#include
+
+int main(int argc, char ** argv)
+{
+ rclcpp::init(argc, argv);
+ rclcpp::NodeOptions options;
+ auto node = std::make_shared(options);
+ rclcpp::spin(node);
+ rclcpp::shutdown();
+
+ return 0;
+}
diff --git a/aichallenge/workspace/src/aichallenge_submit/gyro_odometer/test/test_gyro_odometer_helper.cpp b/aichallenge/workspace/src/aichallenge_submit/gyro_odometer/test/test_gyro_odometer_helper.cpp
new file mode 100644
index 00000000..54e1d320
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_submit/gyro_odometer/test/test_gyro_odometer_helper.cpp
@@ -0,0 +1,46 @@
+// Copyright 2023 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.
+
+#include "test_gyro_odometer_helper.hpp"
+
+using geometry_msgs::msg::TwistWithCovarianceStamped;
+using sensor_msgs::msg::Imu;
+
+Imu generateSampleImu()
+{
+ Imu imu;
+ imu.header.frame_id = "base_link";
+ imu.angular_velocity.x = 0.1;
+ imu.angular_velocity.y = 0.2;
+ imu.angular_velocity.z = 0.3;
+ return imu;
+}
+
+TwistWithCovarianceStamped generateSampleVelocity()
+{
+ TwistWithCovarianceStamped twist;
+ twist.header.frame_id = "base_link";
+ twist.twist.twist.linear.x = 1.0;
+ return twist;
+}
+
+rclcpp::NodeOptions getNodeOptionsWithDefaultParams()
+{
+ rclcpp::NodeOptions node_options;
+
+ // for gyro_odometer
+ node_options.append_parameter_override("output_frame", "base_link");
+ node_options.append_parameter_override("message_timeout_sec", 1e12);
+ return node_options;
+}
diff --git a/aichallenge/workspace/src/aichallenge_submit/gyro_odometer/test/test_gyro_odometer_helper.hpp b/aichallenge/workspace/src/aichallenge_submit/gyro_odometer/test/test_gyro_odometer_helper.hpp
new file mode 100644
index 00000000..6e3aff0b
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_submit/gyro_odometer/test/test_gyro_odometer_helper.hpp
@@ -0,0 +1,30 @@
+// Copyright 2023 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 TEST_GYRO_ODOMETER_HELPER_HPP_
+#define TEST_GYRO_ODOMETER_HELPER_HPP_
+
+#include
+
+#include
+#include
+
+using geometry_msgs::msg::TwistWithCovarianceStamped;
+using sensor_msgs::msg::Imu;
+
+Imu generateSampleImu();
+TwistWithCovarianceStamped generateSampleVelocity();
+rclcpp::NodeOptions getNodeOptionsWithDefaultParams();
+
+#endif // TEST_GYRO_ODOMETER_HELPER_HPP_
diff --git a/aichallenge/workspace/src/aichallenge_submit/gyro_odometer/test/test_gyro_odometer_pubsub.cpp b/aichallenge/workspace/src/aichallenge_submit/gyro_odometer/test/test_gyro_odometer_pubsub.cpp
new file mode 100644
index 00000000..7f6416fb
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_submit/gyro_odometer/test/test_gyro_odometer_pubsub.cpp
@@ -0,0 +1,151 @@
+// Copyright 2023 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.
+
+#include "gyro_odometer/gyro_odometer_core.hpp"
+#include "test_gyro_odometer_helper.hpp"
+
+#include
+
+#include
+
+#include
+#include
+
+/*
+ * This test checks if twist is published from gyro_odometer
+ */
+using geometry_msgs::msg::TwistWithCovarianceStamped;
+using sensor_msgs::msg::Imu;
+
+class ImuGenerator : public rclcpp::Node
+{
+public:
+ ImuGenerator() : Node("imu_generator"), imu_pub(create_publisher("/imu", 1)) {}
+ rclcpp::Publisher::SharedPtr imu_pub;
+};
+
+class VelocityGenerator : public rclcpp::Node
+{
+public:
+ VelocityGenerator()
+ : Node("velocity_generator"),
+ vehicle_velocity_pub(
+ create_publisher("/vehicle/twist_with_covariance", 1))
+ {
+ }
+ rclcpp::Publisher::SharedPtr vehicle_velocity_pub;
+};
+
+class GyroOdometerValidator : public rclcpp::Node
+{
+public:
+ GyroOdometerValidator() : Node("gyro_odometer_validator"), received_latest_twist_ptr(nullptr)
+ {
+ twist_sub = create_subscription(
+ "/twist_with_covariance", 1, [this](const TwistWithCovarianceStamped::ConstSharedPtr msg) {
+ received_latest_twist_ptr = msg;
+ });
+ }
+
+ rclcpp::Subscription::SharedPtr twist_sub;
+ TwistWithCovarianceStamped::ConstSharedPtr received_latest_twist_ptr;
+};
+
+void spinSome(rclcpp::Node::SharedPtr node_ptr)
+{
+ for (int i = 0; i < 50; ++i) {
+ rclcpp::spin_some(node_ptr);
+ rclcpp::WallRate(100).sleep();
+ }
+}
+
+bool isTwistValid(
+ const TwistWithCovarianceStamped & twist, const TwistWithCovarianceStamped & twist_ground_truth)
+{
+ if (twist.twist.twist.linear.x != twist_ground_truth.twist.twist.linear.x) {
+ return false;
+ }
+ if (twist.twist.twist.linear.y != twist_ground_truth.twist.twist.linear.y) {
+ return false;
+ }
+ if (twist.twist.twist.linear.z != twist_ground_truth.twist.twist.linear.z) {
+ return false;
+ }
+ if (twist.twist.twist.angular.x != twist_ground_truth.twist.twist.angular.x) {
+ return false;
+ }
+ if (twist.twist.twist.angular.y != twist_ground_truth.twist.twist.angular.y) {
+ return false;
+ }
+ if (twist.twist.twist.angular.z != twist_ground_truth.twist.twist.angular.z) {
+ return false;
+ }
+ return true;
+}
+
+// IMU & Velocity test
+// Verify that the gyro_odometer successfully publishes the fused twist message when both IMU and
+// velocity data are provided
+TEST(GyroOdometer, TestGyroOdometerWithImuAndVelocity)
+{
+ Imu input_imu = generateSampleImu();
+ TwistWithCovarianceStamped input_velocity = generateSampleVelocity();
+
+ TwistWithCovarianceStamped expected_output_twist;
+ expected_output_twist.twist.twist.linear.x = input_velocity.twist.twist.linear.x;
+ expected_output_twist.twist.twist.angular.x = input_imu.angular_velocity.x;
+ expected_output_twist.twist.twist.angular.y = input_imu.angular_velocity.y;
+ expected_output_twist.twist.twist.angular.z = input_imu.angular_velocity.z;
+
+ auto gyro_odometer_node = std::make_shared(getNodeOptionsWithDefaultParams());
+ auto imu_generator = std::make_shared();
+ auto velocity_generator = std::make_shared();
+ auto gyro_odometer_validator_node = std::make_shared();
+
+ velocity_generator->vehicle_velocity_pub->publish(
+ input_velocity); // need this for now, which should eventually be removed
+ imu_generator->imu_pub->publish(input_imu);
+ velocity_generator->vehicle_velocity_pub->publish(input_velocity);
+
+ // gyro_odometer receives IMU and velocity, and publishes the fused twist data.
+ spinSome(gyro_odometer_node);
+
+ // validator node receives the fused twist data and store in "received_latest_twist_ptr".
+ spinSome(gyro_odometer_validator_node);
+
+ EXPECT_FALSE(gyro_odometer_validator_node->received_latest_twist_ptr == nullptr);
+ EXPECT_TRUE(isTwistValid(
+ *(gyro_odometer_validator_node->received_latest_twist_ptr), expected_output_twist));
+}
+
+// IMU-only test
+// Verify that the gyro_odometer does NOT publish any outputs when only IMU is provided
+TEST(GyroOdometer, TestGyroOdometerImuOnly)
+{
+ Imu input_imu = generateSampleImu();
+
+ auto gyro_odometer_node = std::make_shared(getNodeOptionsWithDefaultParams());
+ auto imu_generator = std::make_shared();
+ auto gyro_odometer_validator_node = std::make_shared();
+
+ imu_generator->imu_pub->publish(input_imu);
+
+ // gyro_odometer receives IMU
+ spinSome(gyro_odometer_node);
+
+ // validator node waits for the output fused twist from gyro_odometer
+ spinSome(gyro_odometer_validator_node);
+
+ EXPECT_TRUE(gyro_odometer_validator_node->received_latest_twist_ptr == nullptr);
+}
diff --git a/aichallenge/workspace/src/aichallenge_submit/gyro_odometer/test/test_main.cpp b/aichallenge/workspace/src/aichallenge_submit/gyro_odometer/test/test_main.cpp
new file mode 100644
index 00000000..fa27d8ae
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_submit/gyro_odometer/test/test_main.cpp
@@ -0,0 +1,26 @@
+// Copyright 2023 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.
+
+#include
+
+#include
+
+int main(int argc, char ** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ rclcpp::init(argc, argv);
+ bool result = RUN_ALL_TESTS();
+ rclcpp::shutdown();
+ return result;
+}
diff --git a/aichallenge/workspace/src/aichallenge_submit/imu_corrector/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/imu_corrector/CMakeLists.txt
new file mode 100644
index 00000000..31e1727b
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_submit/imu_corrector/CMakeLists.txt
@@ -0,0 +1,20 @@
+cmake_minimum_required(VERSION 3.14)
+project(imu_corrector)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(imu_corrector_node SHARED
+ src/imu_corrector_core.cpp
+ include/imu_corrector/imu_corrector_core.hpp
+)
+
+rclcpp_components_register_node(imu_corrector_node
+ PLUGIN "imu_corrector::ImuCorrector"
+ EXECUTABLE imu_corrector
+)
+
+ament_auto_package(INSTALL_TO_SHARE
+ launch
+ config
+)
diff --git a/aichallenge/workspace/src/aichallenge_submit/imu_corrector/README.md b/aichallenge/workspace/src/aichallenge_submit/imu_corrector/README.md
new file mode 100644
index 00000000..8a00c400
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_submit/imu_corrector/README.md
@@ -0,0 +1,58 @@
+# imu_corrector
+
+## Purpose
+
+`imu_corrector_node` is a node that correct imu data.
+
+1. Correct yaw rate offset $b$ by reading the parameter.
+2. Correct yaw rate standard deviation $\sigma$ by reading the parameter.
+
+Mathematically, we assume the following equation:
+
+$$
+\tilde{\omega}(t) = \omega(t) + b(t) + n(t)
+$$
+
+where $\tilde{\omega}$ denotes observed angular velocity, $\omega$ denotes true angular velocity, $b$ denotes an offset, and $n$ denotes a gaussian noise.
+We also assume that $n\sim\mathcal{N}(0, \sigma^2)$.
+
+
+
+
+## Inputs / Outputs
+
+### Input
+
+| Name | Type | Description |
+| -------- | ----------------------- | ------------ |
+| `~input` | `sensor_msgs::msg::Imu` | raw imu data |
+
+### Output
+
+| Name | Type | Description |
+| --------- | ----------------------- | ------------------ |
+| `~output` | `sensor_msgs::msg::Imu` | corrected imu data |
+
+## Parameters
+
+### Core Parameters
+
+| Name | Type | Description |
+| ---------------------------- | ------ | ------------------------------------------------ |
+| `angular_velocity_offset_x` | double | roll rate offset in imu_link [rad/s] |
+| `angular_velocity_offset_y` | double | pitch rate offset imu_link [rad/s] |
+| `angular_velocity_offset_z` | double | yaw rate offset imu_link [rad/s] |
+| `angular_velocity_stddev_xx` | double | roll rate standard deviation imu_link [rad/s] |
+| `angular_velocity_stddev_yy` | double | pitch rate standard deviation imu_link [rad/s] |
+| `angular_velocity_stddev_zz` | double | yaw rate standard deviation imu_link [rad/s] |
+| `acceleration_stddev` | double | acceleration standard deviation imu_link [m/s^2] |
+
+## Assumptions / Known limits
+
+## (Optional) Error detection and handling
+
+## (Optional) Performance characterization
+
+## (Optional) References/External links
+
+## (Optional) Future extensions / Unimplemented parts
diff --git a/aichallenge/workspace/src/aichallenge_submit/imu_corrector/config/imu_corrector.param.yaml b/aichallenge/workspace/src/aichallenge_submit/imu_corrector/config/imu_corrector.param.yaml
new file mode 100644
index 00000000..3156598f
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_submit/imu_corrector/config/imu_corrector.param.yaml
@@ -0,0 +1,8 @@
+/**:
+ ros__parameters:
+ angular_velocity_offset_x: 0.0 # [rad/s]
+ angular_velocity_offset_y: 0.0 # [rad/s]
+ angular_velocity_offset_z: 0.0 # [rad/s]
+ angular_velocity_stddev_xx: 0.03 # [rad/s]
+ angular_velocity_stddev_yy: 0.03 # [rad/s]
+ angular_velocity_stddev_zz: 0.03 # [rad/s]
diff --git a/aichallenge/workspace/src/aichallenge_submit/imu_corrector/include/imu_corrector/imu_corrector_core.hpp b/aichallenge/workspace/src/aichallenge_submit/imu_corrector/include/imu_corrector/imu_corrector_core.hpp
new file mode 100644
index 00000000..1c20dc69
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_submit/imu_corrector/include/imu_corrector/imu_corrector_core.hpp
@@ -0,0 +1,62 @@
+// Copyright 2020 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 IMU_CORRECTOR__IMU_CORRECTOR_CORE_HPP_
+#define IMU_CORRECTOR__IMU_CORRECTOR_CORE_HPP_
+
+#include "tier4_autoware_utils/ros/transform_listener.hpp"
+#include "tier4_autoware_utils/tier4_autoware_utils.hpp"
+
+#include
+
+#include
+
+#include
+#include
+
+#include
+#include
+
+namespace imu_corrector
+{
+class ImuCorrector : public rclcpp::Node
+{
+ using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX;
+
+public:
+ explicit ImuCorrector(const rclcpp::NodeOptions & node_options);
+
+private:
+ void callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr);
+
+ rclcpp::Subscription::SharedPtr imu_sub_;
+
+ rclcpp::Publisher::SharedPtr imu_pub_;
+
+ double angular_velocity_offset_x_imu_link_;
+ double angular_velocity_offset_y_imu_link_;
+ double angular_velocity_offset_z_imu_link_;
+
+ double angular_velocity_stddev_xx_imu_link_;
+ double angular_velocity_stddev_yy_imu_link_;
+ double angular_velocity_stddev_zz_imu_link_;
+
+ double accel_stddev_imu_link_;
+
+ std::shared_ptr transform_listener_;
+
+ std::string output_frame_;
+};
+} // namespace imu_corrector
+
+#endif // IMU_CORRECTOR__IMU_CORRECTOR_CORE_HPP_
diff --git a/aichallenge/workspace/src/aichallenge_submit/imu_corrector/launch/imu_corrector.launch.xml b/aichallenge/workspace/src/aichallenge_submit/imu_corrector/launch/imu_corrector.launch.xml
new file mode 100755
index 00000000..d87d6e77
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_submit/imu_corrector/launch/imu_corrector.launch.xml
@@ -0,0 +1,12 @@
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/aichallenge/workspace/src/aichallenge_submit/imu_corrector/package.xml b/aichallenge/workspace/src/aichallenge_submit/imu_corrector/package.xml
new file mode 100644
index 00000000..ce85a421
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_submit/imu_corrector/package.xml
@@ -0,0 +1,28 @@
+
+
+
+ imu_corrector
+ 1.0.0
+ The ROS2 imu_corrector package
+ Yamato Ando
+ Apache License 2.0
+ Yamato Ando
+
+ ament_cmake_auto
+
+ autoware_cmake
+
+ rclcpp
+ rclcpp_components
+ sensor_msgs
+ tf2
+ tf2_ros
+ tier4_autoware_utils
+
+ ament_lint_auto
+ autoware_lint_common
+
+
+ ament_cmake
+
+
diff --git a/aichallenge/workspace/src/aichallenge_submit/imu_corrector/src/imu_corrector_core.cpp b/aichallenge/workspace/src/aichallenge_submit/imu_corrector/src/imu_corrector_core.cpp
new file mode 100644
index 00000000..edc82f6d
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_submit/imu_corrector/src/imu_corrector_core.cpp
@@ -0,0 +1,120 @@
+// Copyright 2020 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.
+
+#include "imu_corrector/imu_corrector_core.hpp"
+
+std::array transformCovariance(const std::array & cov)
+{
+ using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX;
+
+ double max_cov = 0.0;
+ max_cov = std::max(max_cov, cov[COV_IDX::X_X]);
+ max_cov = std::max(max_cov, cov[COV_IDX::Y_Y]);
+ max_cov = std::max(max_cov, cov[COV_IDX::Z_Z]);
+
+ std::array cov_transformed;
+ cov_transformed.fill(0.);
+ cov_transformed[COV_IDX::X_X] = max_cov;
+ cov_transformed[COV_IDX::Y_Y] = max_cov;
+ cov_transformed[COV_IDX::Z_Z] = max_cov;
+ return cov_transformed;
+}
+
+geometry_msgs::msg::Vector3 transformVector3(
+ const geometry_msgs::msg::Vector3 & vec, const geometry_msgs::msg::TransformStamped & transform)
+{
+ geometry_msgs::msg::Vector3Stamped vec_stamped;
+ vec_stamped.vector = vec;
+
+ geometry_msgs::msg::Vector3Stamped vec_stamped_transformed;
+ tf2::doTransform(vec_stamped, vec_stamped_transformed, transform);
+ return vec_stamped_transformed.vector;
+}
+
+namespace imu_corrector
+{
+ImuCorrector::ImuCorrector(const rclcpp::NodeOptions & node_options)
+: Node("imu_corrector", node_options),
+ output_frame_(declare_parameter("base_link", "base_link"))
+{
+ transform_listener_ = std::make_shared(this);
+
+ angular_velocity_offset_x_imu_link_ = declare_parameter("angular_velocity_offset_x", 0.0);
+ angular_velocity_offset_y_imu_link_ = declare_parameter("angular_velocity_offset_y", 0.0);
+ angular_velocity_offset_z_imu_link_ = declare_parameter("angular_velocity_offset_z", 0.0);
+
+ angular_velocity_stddev_xx_imu_link_ =
+ declare_parameter("angular_velocity_stddev_xx", 0.03);
+ angular_velocity_stddev_yy_imu_link_ =
+ declare_parameter("angular_velocity_stddev_yy", 0.03);
+ angular_velocity_stddev_zz_imu_link_ =
+ declare_parameter("angular_velocity_stddev_zz", 0.03);
+
+ accel_stddev_imu_link_ = declare_parameter("acceleration_stddev", 10000.0);
+
+ imu_sub_ = create_subscription(
+ "input", rclcpp::QoS{1}, std::bind(&ImuCorrector::callbackImu, this, std::placeholders::_1));
+
+ imu_pub_ = create_publisher("output", rclcpp::QoS{10});
+}
+
+void ImuCorrector::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr)
+{
+ sensor_msgs::msg::Imu imu_msg;
+ imu_msg = *imu_msg_ptr;
+
+ imu_msg.angular_velocity.x -= angular_velocity_offset_x_imu_link_;
+ imu_msg.angular_velocity.y -= angular_velocity_offset_y_imu_link_;
+ imu_msg.angular_velocity.z -= angular_velocity_offset_z_imu_link_;
+
+ imu_msg.angular_velocity_covariance[COV_IDX::X_X] =
+ angular_velocity_stddev_xx_imu_link_ * angular_velocity_stddev_xx_imu_link_;
+ imu_msg.angular_velocity_covariance[COV_IDX::Y_Y] =
+ angular_velocity_stddev_yy_imu_link_ * angular_velocity_stddev_yy_imu_link_;
+ imu_msg.angular_velocity_covariance[COV_IDX::Z_Z] =
+ angular_velocity_stddev_zz_imu_link_ * angular_velocity_stddev_zz_imu_link_;
+ imu_msg.linear_acceleration_covariance[COV_IDX::X_X] =
+ accel_stddev_imu_link_ * accel_stddev_imu_link_;
+ imu_msg.linear_acceleration_covariance[COV_IDX::Y_Y] =
+ accel_stddev_imu_link_ * accel_stddev_imu_link_;
+ imu_msg.linear_acceleration_covariance[COV_IDX::Z_Z] =
+ accel_stddev_imu_link_ * accel_stddev_imu_link_;
+
+ geometry_msgs::msg::TransformStamped::ConstSharedPtr tf_imu2base_ptr =
+ transform_listener_->getLatestTransform(imu_msg.header.frame_id, output_frame_);
+ if (!tf_imu2base_ptr) {
+ RCLCPP_ERROR(
+ this->get_logger(), "Please publish TF %s to %s", output_frame_.c_str(),
+ (imu_msg.header.frame_id).c_str());
+ return;
+ }
+
+ sensor_msgs::msg::Imu imu_msg_base_link;
+ imu_msg_base_link.header.stamp = imu_msg_ptr->header.stamp;
+ imu_msg_base_link.header.frame_id = output_frame_;
+ imu_msg_base_link.linear_acceleration =
+ transformVector3(imu_msg.linear_acceleration, *tf_imu2base_ptr);
+ imu_msg_base_link.linear_acceleration_covariance =
+ transformCovariance(imu_msg.linear_acceleration_covariance);
+ imu_msg_base_link.angular_velocity = transformVector3(imu_msg.angular_velocity, *tf_imu2base_ptr);
+ imu_msg_base_link.angular_velocity_covariance =
+ transformCovariance(imu_msg.angular_velocity_covariance);
+
+ imu_pub_->publish(imu_msg_base_link);
+}
+
+} // namespace imu_corrector
+
+#include
+RCLCPP_COMPONENTS_REGISTER_NODE(imu_corrector::ImuCorrector)
diff --git a/aichallenge/workspace/src/aichallenge_submit/imu_gnss_poser/src/imu_gnss_poser_node.cpp b/aichallenge/workspace/src/aichallenge_submit/imu_gnss_poser/src/imu_gnss_poser_node.cpp
index 1d84a3a5..f48f9707 100644
--- a/aichallenge/workspace/src/aichallenge_submit/imu_gnss_poser/src/imu_gnss_poser_node.cpp
+++ b/aichallenge/workspace/src/aichallenge_submit/imu_gnss_poser/src/imu_gnss_poser_node.cpp
@@ -1,5 +1,6 @@
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+#include "geometry_msgs/msg/twist_with_covariance_stamped.hpp"
#include "sensor_msgs/msg/imu.hpp"
class ImuGnssPoser : public rclcpp::Node
@@ -10,6 +11,10 @@ class ImuGnssPoser : public rclcpp::Node
{
auto qos = rclcpp::QoS(rclcpp::KeepLast(10)).reliable();
pub_pose_ = this->create_publisher("/localization/imu_gnss_poser/pose_with_covariance", qos);
+ pub_initial_pose_3d_ = this->create_publisher("/localization/initial_pose3d", qos);
+ sub_twist_ = this->create_subscription(
+ "/localization/twist_with_covariance", qos,
+ std::bind(&ImuGnssPoser::twist_callback, this, std::placeholders::_1));
sub_gnss_ = this->create_subscription(
"/sensing/gnss/pose_with_covariance", qos,
std::bind(&ImuGnssPoser::gnss_callback, this, std::placeholders::_1));
@@ -25,26 +30,36 @@ class ImuGnssPoser : public rclcpp::Node
msg->pose.pose.orientation.y = imu_msg_.orientation.y;
msg->pose.pose.orientation.z = imu_msg_.orientation.z;
msg->pose.pose.orientation.w = imu_msg_.orientation.w;
- msg->pose.covariance[7*0] = 10.0;
- msg->pose.covariance[7*1] = 10.0;
- msg->pose.covariance[7*2] = 10.0;
- msg->pose.covariance[7*3] = 0.1;
- msg->pose.covariance[7*4] = 0.1;
- msg->pose.covariance[7*5] = 1.0;
+ msg->pose.covariance[7*0] = 0.1;
+ msg->pose.covariance[7*1] = 0.1;
+ msg->pose.covariance[7*2] = 0.1;
+ msg->pose.covariance[7*3] = 0.01;
+ msg->pose.covariance[7*4] = 0.01;
+ msg->pose.covariance[7*5] = 0.1;
pub_pose_->publish(*msg);
+ if (!is_ekf_initialized_)
+ pub_initial_pose_3d_->publish(*msg);
}
void imu_callback(sensor_msgs::msg::Imu::SharedPtr msg) {
imu_msg_ = *msg;
}
+ void twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg)
+ {
+ is_ekf_initialized_ = true;
+ }
+
rclcpp::Publisher::SharedPtr pub_pose_;
+ rclcpp::Publisher::SharedPtr pub_initial_pose_3d_;
rclcpp::Subscription::SharedPtr sub_gnss_;
+ rclcpp::Subscription::SharedPtr sub_twist_;
rclcpp::Subscription::SharedPtr sub_imu_;
sensor_msgs::msg::Imu imu_msg_;
+ bool is_ekf_initialized_ = {false};
};
-int main(int argc, char const *argv[])
+int main(int argc, char *argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared());
diff --git a/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/config/sensor_kit_calibration.yaml b/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/config/sensor_kit_calibration.yaml
index 36624a15..01acce21 100644
--- a/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/config/sensor_kit_calibration.yaml
+++ b/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/config/sensor_kit_calibration.yaml
@@ -10,6 +10,6 @@ sensor_kit_base_link:
x: 0.0
y: 0.0
z: 0.0
- roll: 3.14159265359
+ roll: 0.0
pitch: 0.0
- yaw: 3.14159265359
+ yaw: 0.0