Skip to content

Commit 9cdd706

Browse files
danielsanchezarankyoichi-sugaharapre-commit-ci[bot]
authored
feat(control_evaluator): add control_evaluator for v0.3.19 (#1559)
* WIP add control evaluator Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * change for control_evaluator's build success Signed-off-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp> * chore: fix package.xml to avoid rosdep error Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * feat: add IMU subscriber to control evaluator node Signed-off-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp> * ci(pre-commit): autofix --------- Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> Signed-off-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp> Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> Co-authored-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
1 parent c6d7459 commit 9cdd706

File tree

15 files changed

+678
-0
lines changed

15 files changed

+678
-0
lines changed

‎control/autoware_autonomous_emergency_braking/package.xml

+2
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,8 @@
1616
<buildtool_depend>autoware_cmake</buildtool_depend>
1717
<test_depend>ament_cmake_ros</test_depend>
1818
<test_depend>ament_lint_auto</test_depend>
19+
<test_depend>ament_lint_auto</test_depend>
20+
<test_depend>autoware_lint_common</test_depend>
1921
<test_depend>autoware_lint_common</test_depend>
2022
<test_depend>autoware_test_utils</test_depend>
2123

‎control/autoware_control_evaluator/CMakeLists.txt

+24
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,24 @@
1+
cmake_minimum_required(VERSION 3.14)
2+
project(autoware_control_evaluator)
3+
4+
find_package(autoware_cmake REQUIRED)
5+
autoware_package()
6+
7+
find_package(pluginlib REQUIRED)
8+
9+
ament_auto_add_library(control_evaluator_node SHARED
10+
src/control_evaluator_node.cpp
11+
src/metrics/deviation_metrics.cpp
12+
)
13+
14+
rclcpp_components_register_node(control_evaluator_node
15+
PLUGIN "control_diagnostics::ControlEvaluatorNode"
16+
EXECUTABLE control_evaluator
17+
)
18+
19+
20+
ament_auto_package(
21+
INSTALL_TO_SHARE
22+
param
23+
launch
24+
)

‎control/autoware_control_evaluator/README.md

+5
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
# Planning Evaluator
2+
3+
## Purpose
4+
5+
This package provides nodes that generate metrics to evaluate the quality of control.

‎control/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp

+110
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,110 @@
1+
// Copyright 2024 Tier IV, Inc.
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 AUTOWARE__CONTROL_EVALUATOR__CONTROL_EVALUATOR_NODE_HPP_
16+
#define AUTOWARE__CONTROL_EVALUATOR__CONTROL_EVALUATOR_NODE_HPP_
17+
18+
#include "autoware/control_evaluator/metrics/deviation_metrics.hpp"
19+
20+
#include <rclcpp/rclcpp.hpp>
21+
#include <route_handler/route_handler.hpp>
22+
#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
23+
24+
#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
25+
#include <autoware_auto_planning_msgs/msg/route.hpp>
26+
#include <diagnostic_msgs/msg/diagnostic_array.hpp>
27+
#include <nav_msgs/msg/odometry.hpp>
28+
#include <sensor_msgs/msg/imu.hpp>
29+
30+
#include <deque>
31+
#include <optional>
32+
#include <string>
33+
#include <utility>
34+
#include <vector>
35+
36+
namespace control_diagnostics
37+
{
38+
39+
using autoware_auto_planning_msgs::msg::Trajectory;
40+
using diagnostic_msgs::msg::DiagnosticArray;
41+
using diagnostic_msgs::msg::DiagnosticStatus;
42+
using geometry_msgs::msg::Point;
43+
using geometry_msgs::msg::Pose;
44+
using nav_msgs::msg::Odometry;
45+
using LaneletMapBin = autoware_auto_mapping_msgs::msg::HADMapBin;
46+
using LaneletRoute = autoware_auto_planning_msgs::msg::HADMapRoute;
47+
using geometry_msgs::msg::AccelWithCovarianceStamped;
48+
using sensor_msgs::msg::Imu;
49+
50+
/**
51+
* @brief Node for control evaluation
52+
*/
53+
class ControlEvaluatorNode : public rclcpp::Node
54+
{
55+
public:
56+
explicit ControlEvaluatorNode(const rclcpp::NodeOptions & node_options);
57+
DiagnosticStatus generateLateralDeviationDiagnosticStatus(
58+
const Trajectory & traj, const Point & ego_point);
59+
DiagnosticStatus generateYawDeviationDiagnosticStatus(
60+
const Trajectory & traj, const Pose & ego_pose);
61+
std::optional<DiagnosticStatus> generateStopDiagnosticStatus(
62+
const DiagnosticArray & diag, const std::string & function_name);
63+
64+
DiagnosticStatus generateAEBDiagnosticStatus(const DiagnosticStatus & diag);
65+
DiagnosticStatus generateLaneletDiagnosticStatus(const Pose & ego_pose) const;
66+
DiagnosticStatus generateKinematicStateDiagnosticStatus(const Odometry & odom, const Imu & imu);
67+
68+
void onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg);
69+
void onTimer();
70+
71+
private:
72+
// The diagnostics cycle is faster than timer, and each node publishes diagnostic separately.
73+
// takeData() in onTimer() with a polling subscriber will miss a topic, so save all topics with
74+
// onDiagnostics().
75+
rclcpp::Subscription<DiagnosticArray>::SharedPtr control_diag_sub_;
76+
77+
autoware::universe_utils::InterProcessPollingSubscriber<Odometry> odometry_sub_{
78+
this, "~/input/odometry"};
79+
// autoware::universe_utils::InterProcessPollingSubscriber<AccelWithCovarianceStamped> accel_sub_{
80+
// this, "~/input/acceleration"};
81+
autoware::universe_utils::InterProcessPollingSubscriber<Imu> imu_sub_{this, "~/input/imu"};
82+
autoware::universe_utils::InterProcessPollingSubscriber<Trajectory> traj_sub_{
83+
this, "~/input/trajectory"};
84+
autoware::universe_utils::InterProcessPollingSubscriber<
85+
LaneletRoute, autoware::universe_utils::polling_policy::Newest>
86+
route_subscriber_{this, "~/input/route", rclcpp::QoS{1}.transient_local()};
87+
autoware::universe_utils::InterProcessPollingSubscriber<
88+
LaneletMapBin, autoware::universe_utils::polling_policy::Newest>
89+
vector_map_subscriber_{this, "~/input/vector_map", rclcpp::QoS{1}.transient_local()};
90+
91+
rclcpp::Publisher<DiagnosticArray>::SharedPtr metrics_pub_;
92+
93+
// update Route Handler
94+
void getRouteData();
95+
96+
// Calculator
97+
// Metrics
98+
std::deque<rclcpp::Time> stamps_;
99+
100+
// queue for diagnostics and time stamp
101+
std::deque<std::pair<DiagnosticStatus, rclcpp::Time>> diag_queue_;
102+
const std::vector<std::string> target_functions_ = {"autonomous_emergency_braking"};
103+
104+
route_handler::RouteHandler route_handler_;
105+
rclcpp::TimerBase::SharedPtr timer_;
106+
std::optional<Imu> prev_imu_{std::nullopt};
107+
};
108+
} // namespace control_diagnostics
109+
110+
#endif // AUTOWARE__CONTROL_EVALUATOR__CONTROL_EVALUATOR_NODE_HPP_

‎control/autoware_control_evaluator/include/autoware/control_evaluator/metrics/deviation_metrics.hpp

+48
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,48 @@
1+
// Copyright 2024 TIER IV, Inc.
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 AUTOWARE__CONTROL_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_
16+
#define AUTOWARE__CONTROL_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_
17+
18+
#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
19+
#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
20+
21+
namespace control_diagnostics
22+
{
23+
namespace metrics
24+
{
25+
using autoware_auto_planning_msgs::msg::Trajectory;
26+
using geometry_msgs::msg::Point;
27+
using geometry_msgs::msg::Pose;
28+
29+
/**
30+
* @brief calculate lateral deviation of the given trajectory from the reference trajectory
31+
* @param [in] ref reference trajectory
32+
* @param [in] point input point
33+
* @return lateral deviation
34+
*/
35+
double calcLateralDeviation(const Trajectory & traj, const Point & point);
36+
37+
/**
38+
* @brief calculate yaw deviation of the given trajectory from the reference trajectory
39+
* @param [in] traj input trajectory
40+
* @param [in] pose input pose
41+
* @return yaw deviation
42+
*/
43+
double calcYawDeviation(const Trajectory & traj, const Pose & pose);
44+
45+
} // namespace metrics
46+
} // namespace control_diagnostics
47+
48+
#endif // AUTOWARE__CONTROL_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_

‎control/autoware_control_evaluator/launch/control_evaluator.launch.xml

+21
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
<launch>
2+
<arg name="input/diagnostics" default="/diagnostics"/>
3+
<arg name="input/odometry" default="/localization/kinematic_state"/>
4+
<arg name="input/acceleration" default="/localization/acceleration"/>
5+
<arg name="input/trajectory" default="/planning/scenario_planning/trajectory"/>
6+
<arg name="map_topic_name" default="/map/vector_map"/>
7+
<arg name="route_topic_name" default="/planning/mission_planning/route"/>
8+
<!-- control evaluator -->
9+
<group>
10+
<node name="control_evaluator" exec="control_evaluator" pkg="autoware_control_evaluator">
11+
<param from="$(find-pkg-share autoware_control_evaluator)/param/control_evaluator.defaults.yaml"/>
12+
<remap from="~/input/diagnostics" to="$(var input/diagnostics)"/>
13+
<remap from="~/input/odometry" to="$(var input/odometry)"/>
14+
<remap from="~/input/acceleration" to="$(var input/acceleration)"/>
15+
<remap from="~/input/trajectory" to="$(var input/trajectory)"/>
16+
<remap from="~/metrics" to="/control/control_evaluator/metrics"/>
17+
<remap from="~/input/vector_map" to="$(var map_topic_name)"/>
18+
<remap from="~/input/route" to="$(var route_topic_name)"/>
19+
</node>
20+
</group>
21+
</launch>

‎control/autoware_control_evaluator/package.xml

+34
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
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_control_evaluator</name>
5+
<version>0.1.0</version>
6+
<description>ROS 2 node for evaluating control</description>
7+
<maintainer email="daniel.sanchez@tier4.jp">Daniel SANCHEZ</maintainer>
8+
<maintainer email="takayuki.murooka@tier4.jp">takayuki MUROOKA</maintainer>
9+
<license>Apache License 2.0</license>
10+
11+
<author email="daniel.sanchez@tier4.jp">Daniel SANCHEZ</author>
12+
<author email="takayuki.murooka@tier4.jp">takayuki MUROOKA</author>
13+
14+
<buildtool_depend>ament_cmake_auto</buildtool_depend>
15+
<buildtool_depend>autoware_cmake</buildtool_depend>
16+
17+
<depend>autoware_auto_planning_msgs</depend>
18+
<depend>autoware_evaluator_utils</depend>
19+
<depend>diagnostic_msgs</depend>
20+
<depend>pluginlib</depend>
21+
<depend>rclcpp</depend>
22+
<depend>rclcpp_components</depend>
23+
<depend>route_handler</depend>
24+
<depend>tier4_autoware_utils</depend>
25+
<!-- <depend>nav_msgs</depend> -->
26+
27+
<test_depend>ament_cmake_ros</test_depend>
28+
<test_depend>ament_lint_auto</test_depend>
29+
<test_depend>autoware_lint_common</test_depend>
30+
31+
<export>
32+
<build_type>ament_cmake</build_type>
33+
</export>
34+
</package>

‎control/autoware_control_evaluator/param/control_evaluator.defaults.yaml

+2
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,2 @@
1+
/**:
2+
ros__parameters:

0 commit comments

Comments
 (0)