Skip to content

Commit be2c885

Browse files
YamatoAndopre-commit-ci[bot]SakodaShintaro
authored and
KhalilSelyan
committed
feat(gyro_odometer): add diagnostic (#7188)
* add diagnostic Signed-off-by: Yamato Ando <yamato.ando@tier4.jp> * fix diag time stamp Signed-off-by: Yamato Ando <yamato.ando@tier4.jp> * style(pre-commit): autofix * fix spellcheck Signed-off-by: Yamato Ando <yamato.ando@tier4.jp> * style(pre-commit): autofix --------- Signed-off-by: Yamato Ando <yamato.ando@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: SakodaShintaro <shintaro.sakoda@tier4.jp>
1 parent 677e69e commit be2c885

File tree

8 files changed

+361
-137
lines changed

8 files changed

+361
-137
lines changed

‎localization/gyro_odometer/CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@ autoware_package()
66

77
ament_auto_add_library(${PROJECT_NAME} SHARED
88
src/gyro_odometer_core.cpp
9+
src/diagnostics_module.cpp
910
)
1011

1112
target_link_libraries(${PROJECT_NAME} fmt)

‎localization/gyro_odometer/README.md

+15
Original file line numberDiff line numberDiff line change
@@ -34,3 +34,18 @@
3434
- [Limitation] The frequency of the output messages depends on the frequency of the input IMU message.
3535

3636
- [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.
37+
38+
## Diagnostics
39+
40+
<img src="./media/diagnostic.png" alt="drawing" width="600"/>
41+
42+
| Name | Description | Transition condition to Warning | Transition condition to Error |
43+
| -------------------------------- | ----------------------------------------------------------------------------------------- | ------------------------------- | ------------------------------------------------- |
44+
| `topic_time_stamp` | the time stamp of service calling. [nano second] | none | none |
45+
| `is_arrived_first_vehicle_twist` | whether the vehicle twist topic has been received even once. | not arrive yet | none |
46+
| `is_arrived_first_imu` | whether the imu topic has been received even once. | not arrive yet | none |
47+
| `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` |
48+
| `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` |
49+
| `vehicle_twist_queue_size` | the size of vehicle_twist_queue. | none | none |
50+
| `imu_queue_size` | the size of gyro_queue. | none | none |
51+
| `is_succeed_transform_imu` | whether transform imu is succeed or not. | none | failed |

‎localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp

+65
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,65 @@
1+
// Copyright 2023 Autoware Foundation
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef GYRO_ODOMETER__DIAGNOSTICS_MODULE_HPP_
16+
#define GYRO_ODOMETER__DIAGNOSTICS_MODULE_HPP_
17+
18+
#include <rclcpp/rclcpp.hpp>
19+
20+
#include <diagnostic_msgs/msg/diagnostic_array.hpp>
21+
22+
#include <string>
23+
#include <vector>
24+
25+
namespace autoware::gyro_odometer
26+
{
27+
28+
class DiagnosticsModule
29+
{
30+
public:
31+
DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name);
32+
void clear();
33+
void addKeyValue(const diagnostic_msgs::msg::KeyValue & key_value_msg);
34+
template <typename T>
35+
void addKeyValue(const std::string & key, const T & value);
36+
void updateLevelAndMessage(const int8_t level, const std::string & message);
37+
void publish(const rclcpp::Time & publish_time_stamp);
38+
39+
private:
40+
diagnostic_msgs::msg::DiagnosticArray createDiagnosticsArray(
41+
const rclcpp::Time & publish_time_stamp) const;
42+
43+
rclcpp::Clock::SharedPtr clock_;
44+
rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diagnostics_pub_;
45+
46+
diagnostic_msgs::msg::DiagnosticStatus diagnostics_status_msg_;
47+
};
48+
49+
template <typename T>
50+
void DiagnosticsModule::addKeyValue(const std::string & key, const T & value)
51+
{
52+
diagnostic_msgs::msg::KeyValue key_value;
53+
key_value.key = key;
54+
key_value.value = std::to_string(value);
55+
addKeyValue(key_value);
56+
}
57+
58+
template <>
59+
void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value);
60+
template <>
61+
void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value);
62+
63+
} // namespace autoware::gyro_odometer
64+
65+
#endif // GYRO_ODOMETER__DIAGNOSTICS_MODULE_HPP_

‎localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp

+6
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@
1515
#ifndef GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_
1616
#define GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_
1717

18+
#include "gyro_odometer/diagnostics_module.hpp"
1819
#include "tier4_autoware_utils/ros/logger_level_configure.hpp"
1920
#include "tier4_autoware_utils/ros/msg_covariance.hpp"
2021
#include "tier4_autoware_utils/ros/transform_listener.hpp"
@@ -52,6 +53,7 @@ class GyroOdometerNode : public rclcpp::Node
5253
void callbackVehicleTwist(
5354
const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_msg_ptr);
5455
void callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr);
56+
void concatGyroAndOdometer();
5557
void publishData(const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw);
5658

5759
rclcpp::Subscription<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr
@@ -74,8 +76,12 @@ class GyroOdometerNode : public rclcpp::Node
7476

7577
bool vehicle_twist_arrived_;
7678
bool imu_arrived_;
79+
rclcpp::Time latest_vehicle_twist_ros_time_;
80+
rclcpp::Time latest_imu_ros_time_;
7781
std::deque<geometry_msgs::msg::TwistWithCovarianceStamped> vehicle_twist_queue_;
7882
std::deque<sensor_msgs::msg::Imu> gyro_queue_;
83+
84+
std::unique_ptr<DiagnosticsModule> diagnostics_;
7985
};
8086

8187
} // namespace autoware::gyro_odometer

‎localization/gyro_odometer/media/diagnostic.png

40.2 KB
Loading

‎localization/gyro_odometer/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717
<buildtool_depend>ament_cmake_auto</buildtool_depend>
1818
<buildtool_depend>autoware_cmake</buildtool_depend>
1919

20+
<depend>diagnostic_msgs</depend>
2021
<depend>fmt</depend>
2122
<depend>geometry_msgs</depend>
2223
<depend>rclcpp</depend>

‎localization/gyro_odometer/src/diagnostics_module.cpp

+110
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,110 @@
1+
// Copyright 2023 Autoware Foundation
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "gyro_odometer/diagnostics_module.hpp"
16+
17+
#include <rclcpp/rclcpp.hpp>
18+
19+
#include <diagnostic_msgs/msg/diagnostic_array.hpp>
20+
21+
#include <algorithm>
22+
#include <string>
23+
24+
namespace autoware::gyro_odometer
25+
{
26+
27+
DiagnosticsModule::DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name)
28+
: clock_(node->get_clock())
29+
{
30+
diagnostics_pub_ =
31+
node->create_publisher<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics", 10);
32+
33+
diagnostics_status_msg_.name =
34+
std::string(node->get_name()) + std::string(": ") + diagnostic_name;
35+
diagnostics_status_msg_.hardware_id = node->get_name();
36+
}
37+
38+
void DiagnosticsModule::clear()
39+
{
40+
diagnostics_status_msg_.values.clear();
41+
diagnostics_status_msg_.values.shrink_to_fit();
42+
43+
diagnostics_status_msg_.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
44+
diagnostics_status_msg_.message = "";
45+
}
46+
47+
void DiagnosticsModule::addKeyValue(const diagnostic_msgs::msg::KeyValue & key_value_msg)
48+
{
49+
auto it = std::find_if(
50+
std::begin(diagnostics_status_msg_.values), std::end(diagnostics_status_msg_.values),
51+
[key_value_msg](const auto & arg) { return arg.key == key_value_msg.key; });
52+
53+
if (it != std::cend(diagnostics_status_msg_.values)) {
54+
it->value = key_value_msg.value;
55+
} else {
56+
diagnostics_status_msg_.values.push_back(key_value_msg);
57+
}
58+
}
59+
60+
template <>
61+
void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value)
62+
{
63+
diagnostic_msgs::msg::KeyValue key_value;
64+
key_value.key = key;
65+
key_value.value = value;
66+
addKeyValue(key_value);
67+
}
68+
69+
template <>
70+
void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value)
71+
{
72+
diagnostic_msgs::msg::KeyValue key_value;
73+
key_value.key = key;
74+
key_value.value = value ? "True" : "False";
75+
addKeyValue(key_value);
76+
}
77+
78+
void DiagnosticsModule::updateLevelAndMessage(const int8_t level, const std::string & message)
79+
{
80+
if ((level > diagnostic_msgs::msg::DiagnosticStatus::OK)) {
81+
if (!diagnostics_status_msg_.message.empty()) {
82+
diagnostics_status_msg_.message += "; ";
83+
}
84+
diagnostics_status_msg_.message += message;
85+
}
86+
if (level > diagnostics_status_msg_.level) {
87+
diagnostics_status_msg_.level = level;
88+
}
89+
}
90+
91+
void DiagnosticsModule::publish(const rclcpp::Time & publish_time_stamp)
92+
{
93+
diagnostics_pub_->publish(createDiagnosticsArray(publish_time_stamp));
94+
}
95+
96+
diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::createDiagnosticsArray(
97+
const rclcpp::Time & publish_time_stamp) const
98+
{
99+
diagnostic_msgs::msg::DiagnosticArray diagnostics_msg;
100+
diagnostics_msg.header.stamp = publish_time_stamp;
101+
diagnostics_msg.status.push_back(diagnostics_status_msg_);
102+
103+
if (diagnostics_msg.status.at(0).level == diagnostic_msgs::msg::DiagnosticStatus::OK) {
104+
diagnostics_msg.status.at(0).message = "OK";
105+
}
106+
107+
return diagnostics_msg;
108+
}
109+
110+
} // namespace autoware::gyro_odometer

0 commit comments

Comments
 (0)