Skip to content

Commit 2ece6fa

Browse files
danielsanchezarandanielsanchezaran
committed
WIP add control evaluator
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
1 parent 7d79468 commit 2ece6fa

File tree

14 files changed

+675
-0
lines changed

14 files changed

+675
-0
lines changed

‎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

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

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

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

+35
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
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_evaluator_utils</depend>
18+
<depend>autoware_motion_utils</depend>
19+
<depend>autoware_planning_msgs</depend>
20+
<depend>autoware_route_handler</depend>
21+
<depend>autoware_universe_utils</depend>
22+
<depend>diagnostic_msgs</depend>
23+
<depend>pluginlib</depend>
24+
<depend>rclcpp</depend>
25+
<depend>rclcpp_components</depend>
26+
<!-- <depend>nav_msgs</depend> -->
27+
28+
<test_depend>ament_cmake_ros</test_depend>
29+
<test_depend>ament_lint_auto</test_depend>
30+
<test_depend>autoware_lint_common</test_depend>
31+
32+
<export>
33+
<build_type>ament_cmake</build_type>
34+
</export>
35+
</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)