Skip to content

Commit 9ced75c

Browse files
storrrrrrrrmstorrrrrrrrm
authored
feat(autoware_gyro_odometer): porting the package from Autoware Universe (#423)
Signed-off-by: suchang <[email protected]>
1 parent 0e46e19 commit 9ced75c

File tree

13 files changed

+866
-0
lines changed

13 files changed

+866
-0
lines changed

‎localization/autoware_gyro_odometer/CMakeLists.txt‎

Lines changed: 39 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,39 @@
1+
cmake_minimum_required(VERSION 3.14)
2+
project(autoware_gyro_odometer)
3+
4+
find_package(autoware_cmake REQUIRED)
5+
autoware_package()
6+
7+
ament_auto_add_library(${PROJECT_NAME} SHARED
8+
src/gyro_odometer_core.cpp
9+
)
10+
11+
target_link_libraries(${PROJECT_NAME} fmt)
12+
13+
if(BUILD_TESTING)
14+
ament_add_ros_isolated_gtest(test_gyro_odometer
15+
test/test_main.cpp
16+
test/test_gyro_odometer_pubsub.cpp
17+
test/test_gyro_odometer_helper.cpp
18+
)
19+
ament_target_dependencies(test_gyro_odometer
20+
rclcpp
21+
)
22+
target_link_libraries(test_gyro_odometer
23+
${PROJECT_NAME}
24+
)
25+
target_include_directories(test_gyro_odometer PRIVATE
26+
src
27+
)
28+
endif()
29+
30+
rclcpp_components_register_node(${PROJECT_NAME}
31+
PLUGIN "autoware::gyro_odometer::GyroOdometerNode"
32+
EXECUTABLE ${PROJECT_NAME}_node
33+
EXECUTOR SingleThreadedExecutor
34+
)
35+
36+
ament_auto_package(INSTALL_TO_SHARE
37+
launch
38+
config
39+
)

‎localization/autoware_gyro_odometer/README.md‎

Lines changed: 75 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,75 @@
1+
# autoware_gyro_odometer
2+
3+
## Overview
4+
5+
`autoware_gyro_odometer` is the package to estimate twist by combining imu and vehicle speed.
6+
7+
## Design
8+
9+
`autoware_gyro_odometer` is part of the perception or localization stack, providing reliable motion estimates for navigation and control. It is designed to estimate the robot's motion by combining vehicle twist data (linear and angular velocities) with angular velocity measurements from an IMU (Inertial Measurement Unit).
10+
11+
**Data Handling and Synchronization:**
12+
13+
- Message Queues: Uses queues to store vehicle twist and gyro messages to ensure data synchronization.
14+
- Message Timeouts: Checks for message timeouts to discard stale data, preventing incorrect estimations.
15+
16+
**Error Checks and Logging:**
17+
18+
- Timeout Handling: Logs errors and clears queues if messages exceed a defined time threshold.
19+
- Transformation Checks: Verifies that TF transforms between IMU and base frames are available; logs errors if not.
20+
21+
**Data Processing:**
22+
23+
- Transformation: Converts gyro data into the base frame using TF to ensure accurate angular velocity measurements.
24+
- Mean and Covariance Calculation: Averages multiple measurements to reduce noise and calculates covariances to represent data reliability.
25+
26+
**Output and Publishing:**
27+
28+
- Twist Estimation: Constructs output messages with estimated twist values and covariances.
29+
- Stationary Handling: Zeros out certain twist components when the robot is stationary to avoid noise.
30+
31+
## Inputs / Outputs
32+
33+
### Input
34+
35+
| Name | Type | Description |
36+
| ------------------------------- | ------------------------------------------------ | ---------------------------------- |
37+
| `vehicle/twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | twist with covariance from vehicle |
38+
| `imu` | `sensor_msgs::msg::Imu` | imu from sensor |
39+
40+
### Output
41+
42+
| Name | Type | Description |
43+
| ----------------------- | ------------------------------------------------ | ------------------------------- |
44+
| `twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | estimated twist with covariance |
45+
46+
## Parameters
47+
48+
{{ json_to_markdown("localization/autoware_gyro_odometer/schema/gyro_odometer.schema.json") }}
49+
50+
## Assumptions / Known limits
51+
52+
- [Assumption] The frame_id of input twist message must be set to base_link.
53+
54+
- [Assumption] The covariance in the input messages must be properly assigned.
55+
56+
- [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.
57+
58+
- [Limitation] The frequency of the output messages depends on the frequency of the input IMU message.
59+
60+
- [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.
61+
62+
## Diagnostics
63+
64+
<img src="./media/diagnostic.png" alt="drawing" width="600"/>
65+
66+
| Name | Description | Transition condition to Warning | Transition condition to Error |
67+
| -------------------------------- | ----------------------------------------------------------------------------------------- | ------------------------------- | ------------------------------------------------- |
68+
| `topic_time_stamp` | the time stamp of service calling. [nano second] | none | none |
69+
| `is_arrived_first_vehicle_twist` | whether the vehicle twist topic has been received even once. | not arrive yet | none |
70+
| `is_arrived_first_imu` | whether the imu topic has been received even once. | not arrive yet | none |
71+
| `vehicle_twist_time_stamp_dt` | the time difference between the current time and the latest vehicle twist topic. [second] | none | the time is **longer** than `message_timeout_sec` |
72+
| `imu_time_stamp_dt` | the time difference between the current time and the latest imu topic. [second] | none | the time is **longer** than `message_timeout_sec` |
73+
| `vehicle_twist_queue_size` | the size of vehicle_twist_queue. | none | none |
74+
| `imu_queue_size` | the size of gyro_queue. | none | none |
75+
| `is_succeed_transform_imu` | whether transform imu is succeed or not. | none | failed |

‎localization/autoware_gyro_odometer/config/gyro_odometer.param.yaml‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
/**:
2+
ros__parameters:
3+
output_frame: "base_link"
4+
message_timeout_sec: 0.2

‎localization/autoware_gyro_odometer/launch/gyro_odometer.launch.xml‎

Lines changed: 27 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,27 @@
1+
<launch>
2+
<arg name="input_vehicle_twist_with_covariance_topic" default="/sensing/vehicle_velocity_converter/twist_with_covariance" description="input twist with covariance topic name from vehicle"/>
3+
4+
<arg name="input_imu_topic" default="/sensing/imu/imu_data" description="input imu topic name"/>
5+
6+
<arg name="output_twist_raw_topic" default="gyro_twist_raw" description="output raw twist topic name"/>
7+
<arg name="output_twist_with_covariance_raw_topic" default="gyro_twist_with_covariance_raw" description="output raw twist with covariance topic name"/>
8+
9+
<arg name="output_twist_topic" default="gyro_twist" description="output twist topic name"/>
10+
<arg name="output_twist_with_covariance_topic" default="gyro_twist_with_covariance" description="output twist with covariance topic name"/>
11+
12+
<arg name="config_file" default="$(find-pkg-share autoware_gyro_odometer)/config/gyro_odometer.param.yaml"/>
13+
14+
<node pkg="autoware_gyro_odometer" exec="autoware_gyro_odometer_node" output="both">
15+
<remap from="vehicle/twist_with_covariance" to="$(var input_vehicle_twist_with_covariance_topic)"/>
16+
17+
<remap from="imu" to="$(var input_imu_topic)"/>
18+
19+
<remap from="twist_raw" to="$(var output_twist_raw_topic)"/>
20+
<remap from="twist_with_covariance_raw" to="$(var output_twist_with_covariance_raw_topic)"/>
21+
22+
<remap from="twist" to="$(var output_twist_topic)"/>
23+
<remap from="twist_with_covariance" to="$(var output_twist_with_covariance_topic)"/>
24+
25+
<param from="$(var config_file)"/>
26+
</node>
27+
</launch>

‎localization/autoware_gyro_odometer/media/diagnostic.png‎

40.2 KB
Loading

‎localization/autoware_gyro_odometer/package.xml‎

Lines changed: 41 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,41 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>autoware_gyro_odometer</name>
5+
<version>0.44.0</version>
6+
<description>The autoware_gyro_odometer package as a ROS 2 node</description>
7+
<maintainer email="[email protected]">Yamato Ando</maintainer>
8+
<maintainer email="[email protected]">Masahiro Sakamoto</maintainer>
9+
<maintainer email="[email protected]">Kento Yabuuchi</maintainer>
10+
<maintainer email="[email protected]">NGUYEN Viet Anh</maintainer>
11+
<maintainer email="[email protected]">Taiki Yamada</maintainer>
12+
<maintainer email="[email protected]">Shintaro Sakoda</maintainer>
13+
<maintainer email="[email protected]">Ryu Yamamoto</maintainer>
14+
<license>Apache License 2.0</license>
15+
<author email="[email protected]">Yamato Ando</author>
16+
17+
<buildtool_depend>ament_cmake_auto</buildtool_depend>
18+
<buildtool_depend>autoware_cmake</buildtool_depend>
19+
20+
<depend>autoware_localization_util</depend>
21+
<depend>autoware_utils_diagnostics</depend>
22+
<depend>autoware_utils_geometry</depend>
23+
<depend>autoware_utils_logging</depend>
24+
<depend>autoware_utils_tf</depend>
25+
<depend>diagnostic_msgs</depend>
26+
<depend>fmt</depend>
27+
<depend>geometry_msgs</depend>
28+
<depend>rclcpp</depend>
29+
<depend>rclcpp_components</depend>
30+
<depend>sensor_msgs</depend>
31+
<depend>tf2</depend>
32+
<depend>tf2_geometry_msgs</depend>
33+
34+
<test_depend>ament_cmake_ros</test_depend>
35+
<test_depend>ament_lint_auto</test_depend>
36+
<test_depend>autoware_lint_common</test_depend>
37+
38+
<export>
39+
<build_type>ament_cmake</build_type>
40+
</export>
41+
</package>

‎localization/autoware_gyro_odometer/schema/gyro_odometer.schema.json‎

Lines changed: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,38 @@
1+
{
2+
"$schema": "http://json-schema.org/draft-07/schema#",
3+
"title": "Parameters for gyro odometer",
4+
"type": "object",
5+
"definitions": {
6+
"gyro_odometer": {
7+
"type": "object",
8+
"properties": {
9+
"output_frame": {
10+
"type": "string",
11+
"description": "output's frame id",
12+
"default": "base_link"
13+
},
14+
"message_timeout_sec": {
15+
"type": "number",
16+
"description": "delay tolerance time for message",
17+
"default": 0.2
18+
}
19+
},
20+
"required": ["output_frame", "message_timeout_sec"],
21+
"additionalProperties": false
22+
}
23+
},
24+
"properties": {
25+
"/**": {
26+
"type": "object",
27+
"properties": {
28+
"ros__parameters": {
29+
"$ref": "#/definitions/gyro_odometer"
30+
}
31+
},
32+
"required": ["ros__parameters"],
33+
"additionalProperties": false
34+
}
35+
},
36+
"required": ["/**"],
37+
"additionalProperties": false
38+
}

0 commit comments

Comments
 (0)