Skip to content

Commit

Permalink
pre-commit and fix typo
Browse files Browse the repository at this point in the history
  • Loading branch information
xtk8532704 committed Oct 30, 2024
1 parent 9e3e197 commit 09b26f7
Show file tree
Hide file tree
Showing 22 changed files with 78 additions and 93 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -34,10 +34,10 @@
#include <sensor_msgs/msg/imu.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <tier4_debug_msgs/msg/float32_stamped.hpp>
#include <visualization_msgs/msg/marker.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
#include <tier4_metric_msgs/msg/metric.hpp>
#include <tier4_metric_msgs/msg/metric_array.hpp>
#include <visualization_msgs/msg/marker.hpp>
#include <visualization_msgs/msg/marker_array.hpp>

#include <boost/optional.hpp>

Expand Down
2 changes: 1 addition & 1 deletion control/autoware_autonomous_emergency_braking/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -42,8 +42,8 @@
<depend>tf2_geometry_msgs</depend>
<depend>tf2_ros</depend>
<depend>tier4_debug_msgs</depend>
<depend>visualization_msgs</depend>
<depend>tier4_metric_msgs</depend>
<depend>visualization_msgs</depend>

<export>
<build_type>ament_cmake</build_type>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -51,10 +51,8 @@ class ControlEvaluatorNode : public rclcpp::Node
{
public:
explicit ControlEvaluatorNode(const rclcpp::NodeOptions & node_options);
void AddLateralDeviationMetricMsg(
const Trajectory & traj, const Point & ego_point);
void AddYawDeviationMetricMsg(
const Trajectory & traj, const Pose & ego_pose);
void AddLateralDeviationMetricMsg(const Trajectory & traj, const Point & ego_point);
void AddYawDeviationMetricMsg(const Trajectory & traj, const Pose & ego_pose);
void AddGoalLongitudinalDeviationMetricMsg(const Pose & ego_pose);
void AddGoalLateralDeviationMetricMsg(const Pose & ego_pose);
void AddGoalYawDeviationMetricMsg(const Pose & ego_pose);
Expand Down
2 changes: 1 addition & 1 deletion evaluator/autoware_control_evaluator/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,6 @@

<depend>autoware_motion_utils</depend>
<depend>autoware_planning_msgs</depend>
<depend>tier4_metric_msgs</depend>
<depend>autoware_route_handler</depend>
<depend>autoware_test_utils</depend>
<depend>autoware_universe_utils</depend>
Expand All @@ -28,6 +27,7 @@
<depend>rclcpp_components</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>tier4_metric_msgs</depend>

<test_depend>ament_cmake_ros</test_depend>
<test_depend>ament_lint_auto</test_depend>
Expand Down
35 changes: 15 additions & 20 deletions evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -79,21 +79,21 @@ void ControlEvaluatorNode::AddLaneletMetricMsg(const Pose & ego_pose)
MetricMsg metric_msg;

{
metric_msg.name = base_name+ "lane_id";
metric_msg.value = std::to_string(current_lane.id());
metrics_msg_.metric_array.push_back(metric_msg);
metric_msg.name = base_name + "lane_id";
metric_msg.value = std::to_string(current_lane.id());
metrics_msg_.metric_array.push_back(metric_msg);
}

{
metric_msg.name = base_name + "s";
metric_msg.value = std::to_string(arc_coordinates.length);
metrics_msg_.metric_array.push_back(metric_msg);
metric_msg.name = base_name + "s";
metric_msg.value = std::to_string(arc_coordinates.length);
metrics_msg_.metric_array.push_back(metric_msg);
}

{
metric_msg.name = base_name + "t";
metric_msg.value = std::to_string(arc_coordinates.distance);
metrics_msg_.metric_array.push_back(metric_msg);
metric_msg.name = base_name + "t";
metric_msg.value = std::to_string(arc_coordinates.distance);
metrics_msg_.metric_array.push_back(metric_msg);
}
return;
}
Expand All @@ -103,7 +103,7 @@ void ControlEvaluatorNode::AddKinematicStateMetricMsg(
{
const std::string base_name = "kinematic_state/";
MetricMsg metric_msg;

metric_msg.name = base_name + "vel";
metric_msg.value = std::to_string(odom.twist.twist.linear.x);
metrics_msg_.metric_array.push_back(metric_msg);
Expand Down Expand Up @@ -148,8 +148,7 @@ void ControlEvaluatorNode::AddLateralDeviationMetricMsg(
return;
}

void ControlEvaluatorNode::AddYawDeviationMetricMsg(
const Trajectory & traj, const Pose & ego_pose)
void ControlEvaluatorNode::AddYawDeviationMetricMsg(const Trajectory & traj, const Pose & ego_pose)
{
const double yaw_deviation = metrics::calcYawDeviation(traj, ego_pose);

Expand All @@ -160,8 +159,7 @@ void ControlEvaluatorNode::AddYawDeviationMetricMsg(
return;
}

void ControlEvaluatorNode::AddGoalLongitudinalDeviationMetricMsg(
const Pose & ego_pose)
void ControlEvaluatorNode::AddGoalLongitudinalDeviationMetricMsg(const Pose & ego_pose)
{
const double longitudinal_deviation =
metrics::calcLongitudinalDeviation(route_handler_.getGoalPose(), ego_pose.position);
Expand All @@ -173,8 +171,7 @@ void ControlEvaluatorNode::AddGoalLongitudinalDeviationMetricMsg(
return;
}

void ControlEvaluatorNode::AddGoalLateralDeviationMetricMsg(
const Pose & ego_pose)
void ControlEvaluatorNode::AddGoalLateralDeviationMetricMsg(const Pose & ego_pose)
{
const double lateral_deviation =
metrics::calcLateralDeviation(route_handler_.getGoalPose(), ego_pose.position);
Expand All @@ -186,8 +183,7 @@ void ControlEvaluatorNode::AddGoalLateralDeviationMetricMsg(
return;
}

void ControlEvaluatorNode::AddGoalYawDeviationMetricMsg(
const Pose & ego_pose)
void ControlEvaluatorNode::AddGoalYawDeviationMetricMsg(const Pose & ego_pose)
{
const double yaw_deviation = metrics::calcYawDeviation(route_handler_.getGoalPose(), ego_pose);

Expand Down Expand Up @@ -230,7 +226,6 @@ void ControlEvaluatorNode::onTimer()
metrics_pub_->publish(metrics_msg_);
metrics_msg_ = MetricArrayMsg{};
}

Check notice on line 228 in evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Complex Method

ControlEvaluatorNode::onTimer decreases in cyclomatic complexity from 12 to 9, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

}
} // namespace control_diagnostics

Expand Down
6 changes: 3 additions & 3 deletions evaluator/autoware_planning_evaluator/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -58,9 +58,9 @@ Adding a new metric `M` requires the following steps:
Each metric is published on a topic named after the metric name.
| Name | Type | Description |
| ----------- | --------------------------------------- | ------------------------------------------------------- |
| `~/metrics` | `tier4_metric_msgs::msg::MetricArray` | MetricArray with many metrics of `tier4_metric_msgs::msg::Metric`|
| Name | Type | Description |
| ----------- | ------------------------------------- | ----------------------------------------------------------------- |
| `~/metrics` | `tier4_metric_msgs::msg::MetricArray` | MetricArray with many metrics of `tier4_metric_msgs::msg::Metric` |
When shut down, the evaluation node writes the values of the metrics measured during its lifetime
to a file as specified by the `output_file` parameter.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -96,8 +96,7 @@ class PlanningEvaluatorNode : public rclcpp::Node
/**
* @brief publish the given metric statistic
*/
void AddMetricMsg(
const Metric & metric, const Stat<double> & metric_stat);
void AddMetricMsg(const Metric & metric, const Stat<double> & metric_stat);

/**
* @brief publish current ego lane info
Expand Down
2 changes: 1 addition & 1 deletion evaluator/autoware_planning_evaluator/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,6 @@
<depend>autoware_planning_msgs</depend>
<depend>autoware_route_handler</depend>
<depend>autoware_universe_utils</depend>
<depend>tier4_metric_msgs</depend>
<depend>eigen</depend>
<depend>geometry_msgs</depend>
<depend>nav_msgs</depend>
Expand All @@ -28,6 +27,7 @@
<depend>rclcpp_components</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>tier4_metric_msgs</depend>

<test_depend>ament_cmake_ros</test_depend>
<test_depend>ament_lint_auto</test_depend>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -118,8 +118,7 @@ void PlanningEvaluatorNode::getRouteData()
}
}

void PlanningEvaluatorNode::AddLaneletMetricMsg(
const Odometry::ConstSharedPtr ego_state_ptr)
void PlanningEvaluatorNode::AddLaneletMetricMsg(const Odometry::ConstSharedPtr ego_state_ptr)
{
const auto & ego_pose = ego_state_ptr->pose.pose;
const auto current_lanelets = [&]() {
Expand All @@ -140,21 +139,21 @@ void PlanningEvaluatorNode::AddLaneletMetricMsg(
MetricMsg metric_msg;

{
metric_msg.name = base_name+ "lane_id";
metric_msg.value = std::to_string(current_lane.id());
metrics_msg_.metric_array.push_back(metric_msg);
metric_msg.name = base_name + "lane_id";
metric_msg.value = std::to_string(current_lane.id());
metrics_msg_.metric_array.push_back(metric_msg);
}

{
metric_msg.name = base_name + "s";
metric_msg.value = std::to_string(arc_coordinates.length);
metrics_msg_.metric_array.push_back(metric_msg);
metric_msg.name = base_name + "s";
metric_msg.value = std::to_string(arc_coordinates.length);
metrics_msg_.metric_array.push_back(metric_msg);
}

{
metric_msg.name = base_name + "t";
metric_msg.value = std::to_string(arc_coordinates.distance);
metrics_msg_.metric_array.push_back(metric_msg);
metric_msg.name = base_name + "t";
metric_msg.value = std::to_string(arc_coordinates.distance);
metrics_msg_.metric_array.push_back(metric_msg);
}
return;
}
Expand Down Expand Up @@ -196,27 +195,26 @@ void PlanningEvaluatorNode::AddKinematicStateMetricMsg(
return;
}

void PlanningEvaluatorNode::AddMetricMsg(
const Metric & metric, const Stat<double> & metric_stat)
void PlanningEvaluatorNode::AddMetricMsg(const Metric & metric, const Stat<double> & metric_stat)
{
const std::string base_name = metric_to_str.at(metric) + "/";
MetricMsg metric_msg;
{
metric_msg.name = base_name + "min";
metric_msg.value = boost::lexical_cast<decltype(metric_msg.value)>(metric_stat.min());
metrics_msg_.metric_array.push_back(metric_msg);
metric_msg.name = base_name + "min";
metric_msg.value = boost::lexical_cast<decltype(metric_msg.value)>(metric_stat.min());
metrics_msg_.metric_array.push_back(metric_msg);
}

{
metric_msg.name = base_name + "max";
metric_msg.value = boost::lexical_cast<decltype(metric_msg.value)>(metric_stat.max());
metrics_msg_.metric_array.push_back(metric_msg);
metric_msg.name = base_name + "max";
metric_msg.value = boost::lexical_cast<decltype(metric_msg.value)>(metric_stat.max());
metrics_msg_.metric_array.push_back(metric_msg);
}

{
metric_msg.name = base_name + "mean";
metric_msg.value = boost::lexical_cast<decltype(metric_msg.value)>(metric_stat.mean());
metrics_msg_.metric_array.push_back(metric_msg);
metric_msg.name = base_name + "mean";
metric_msg.value = boost::lexical_cast<decltype(metric_msg.value)>(metric_stat.mean());
metrics_msg_.metric_array.push_back(metric_msg);
}

return;
Expand Down
14 changes: 8 additions & 6 deletions evaluator/scenario_simulator_v2_adapter/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -4,14 +4,15 @@

This package provides a node to convert various messages from the Autoware into `tier4_simulation_msgs::msg::UserDefinedValue` messages for the scenario_simulator_v2.
Currently, this node supports conversion of:

- `tier4_metric_msgs::msg::MetricArray` for metric topics

## Inner-workings / Algorithms

- For `tier4_metric_msgs::msg::MetricArray`,
The node subscribes to all topics listed in the parameter `metric_topic_list`.
Each time such message is received, it is converted into as many `UserDefinedValue` messages as the number of `Metric` objects.
The format of the output topic is detailed in the _output_ section.
- For `tier4_metric_msgs::msg::MetricArray`,
The node subscribes to all topics listed in the parameter `metric_topic_list`.
Each time such message is received, it is converted into as many `UserDefinedValue` messages as the number of `Metric` objects.
The format of the output topic is detailed in the _output_ section.

## Inputs / Outputs

Expand All @@ -24,7 +25,7 @@ The node listens to `MetricArray` messages on the topics specified in `metric_to
The node outputs `UserDefinedValue` messages that are converted from the received messages.

- For `MetricArray` messages,
The name of the output topics are generated from the corresponding input topic, the name of the metric.
The name of the output topics are generated from the corresponding input topic, the name of the metric.
- For example, we might listen to topic `/planning/planning_evaluator/metrics` and receive a `MetricArray` with 2 metrics:
- metric with `name: "metricA/x"`
- metric with `name: "metricA/y"`
Expand All @@ -33,9 +34,10 @@ The name of the output topics are generated from the corresponding input topic,
- `/planning/planning_evaluator/metrics/metricA/y`

## Parameters

{{ json_to_markdown("src/autoware/universe/evaluator/scenario_simulator_v2_adapter/schema/scenario_simulator_v2_adapter.schema.json") }}

## Assumptions / Known limitsmetrics_x
## Assumptions / Known limits

Values in the `Metric` objects of a `MetricArray` are assumed to be of type `double`.

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -4,4 +4,4 @@
metric_topic_list:
- /planning/planning_evaluator/metrics
- /control/control_evaluator/metrics
- /system/processing_time_checker/metrics
- /system/processing_time_checker/metrics
Original file line number Diff line number Diff line change
Expand Up @@ -17,10 +17,10 @@

#include <rclcpp/rclcpp.hpp>

#include <tier4_metric_msgs/msg/metric.hpp>
#include <tier4_metric_msgs/msg/metric_array.hpp>
#include "tier4_simulation_msgs/msg/user_defined_value.hpp"
#include "tier4_simulation_msgs/msg/user_defined_value_type.hpp"
#include <tier4_metric_msgs/msg/metric.hpp>
#include <tier4_metric_msgs/msg/metric_array.hpp>

#include <memory>
#include <string>
Expand Down
2 changes: 1 addition & 1 deletion evaluator/scenario_simulator_v2_adapter/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -12,10 +12,10 @@

<buildtool_depend>ament_cmake_auto</buildtool_depend>
<buildtool_depend>autoware_cmake</buildtool_depend>
<depend>tier4_metric_msgs</depend>
<depend>pluginlib</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
<depend>tier4_metric_msgs</depend>
<depend>tier4_simulation_msgs</depend>

<test_depend>ament_cmake_gtest</test_depend>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,22 +12,15 @@
"exclusiveMinimum": 2,
"description": "The scanning and update frequency of the checker."
},
"processing_time_topic_list": {
"type": "array",
"items": {
"type": "string"
},
"description": "The topic name list of the processing time."
},
"metric_topic_list": {
"type": "array",
"items": {
"type": "string"
},
"description": "The topic name list of the metrics."
"description": "The topic name list of the processing time."
}
},
"required": ["update_rate", "processing_time_topic_list", "metric_topic_list"]
"required": ["update_rate", "metric_topic_list"]
}
},
"properties": {
Expand All @@ -42,4 +35,4 @@
}
},
"required": ["/**"]
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -16,10 +16,10 @@

#include <rclcpp/rclcpp.hpp>

#include <tier4_metric_msgs/msg/metric.hpp>
#include <tier4_metric_msgs/msg/metric_array.hpp>
#include "tier4_simulation_msgs/msg/user_defined_value.hpp"
#include "tier4_simulation_msgs/msg/user_defined_value_type.hpp"
#include <tier4_metric_msgs/msg/metric.hpp>
#include <tier4_metric_msgs/msg/metric_array.hpp>

#include <gtest/gtest.h>

Expand Down
2 changes: 1 addition & 1 deletion planning/autoware_obstacle_cruise_planner/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -38,8 +38,8 @@
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>tier4_debug_msgs</depend>
<depend>tier4_planning_msgs</depend>
<depend>tier4_metric_msgs</depend>
<depend>tier4_planning_msgs</depend>
<depend>visualization_msgs</depend>

<test_depend>ament_cmake_ros</test_depend>
Expand Down
Loading

0 comments on commit 09b26f7

Please sign in to comment.