Skip to content

Commit

Permalink
refactor(control_evaluator): use class naming standard and use remapp…
Browse files Browse the repository at this point in the history
…ed param name (#7782)

use class naming standard and use remapped param name

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
  • Loading branch information
danielsanchezaran authored Jul 2, 2024
1 parent fd28010 commit ce09907
Show file tree
Hide file tree
Showing 4 changed files with 21 additions and 21 deletions.
2 changes: 1 addition & 1 deletion evaluator/autoware_control_evaluator/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@ ament_auto_add_library(control_evaluator_node SHARED
)

rclcpp_components_register_node(control_evaluator_node
PLUGIN "control_diagnostics::controlEvaluatorNode"
PLUGIN "control_diagnostics::ControlEvaluatorNode"
EXECUTABLE control_evaluator
)

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -48,10 +48,10 @@ using geometry_msgs::msg::AccelWithCovarianceStamped;
/**
* @brief Node for control evaluation
*/
class controlEvaluatorNode : public rclcpp::Node
class ControlEvaluatorNode : public rclcpp::Node
{
public:
explicit controlEvaluatorNode(const rclcpp::NodeOptions & node_options);
explicit ControlEvaluatorNode(const rclcpp::NodeOptions & node_options);
void removeOldDiagnostics(const rclcpp::Time & stamp);
void removeDiagnosticsByName(const std::string & name);
void addDiagnostic(const DiagnosticStatus & diag, const rclcpp::Time & stamp);
Expand Down Expand Up @@ -83,7 +83,7 @@ class controlEvaluatorNode : public rclcpp::Node
autoware::universe_utils::InterProcessPollingSubscriber<Odometry> odometry_sub_{
this, "~/input/odometry"};
autoware::universe_utils::InterProcessPollingSubscriber<AccelWithCovarianceStamped> accel_sub_{
this, "/localization/acceleration"};
this, "~/input/acceleration"};
autoware::universe_utils::InterProcessPollingSubscriber<Trajectory> traj_sub_{
this, "~/input/trajectory"};
autoware::universe_utils::InterProcessPollingSubscriber<LaneletRoute> route_subscriber_{
Expand Down
32 changes: 16 additions & 16 deletions evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -25,23 +25,23 @@

namespace control_diagnostics
{
controlEvaluatorNode::controlEvaluatorNode(const rclcpp::NodeOptions & node_options)
ControlEvaluatorNode::ControlEvaluatorNode(const rclcpp::NodeOptions & node_options)
: Node("control_evaluator", node_options)
{
using std::placeholders::_1;
control_diag_sub_ = create_subscription<DiagnosticArray>(
"~/input/diagnostics", 1, std::bind(&controlEvaluatorNode::onDiagnostics, this, _1));
"~/input/diagnostics", 1, std::bind(&ControlEvaluatorNode::onDiagnostics, this, _1));

// Publisher
metrics_pub_ = create_publisher<DiagnosticArray>("~/metrics", 1);

// Timer callback to publish evaluator diagnostics
using namespace std::literals::chrono_literals;
timer_ =
rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&controlEvaluatorNode::onTimer, this));
rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&ControlEvaluatorNode::onTimer, this));
}

void controlEvaluatorNode::getRouteData()
void ControlEvaluatorNode::getRouteData()
{
// route
{
Expand All @@ -64,7 +64,7 @@ void controlEvaluatorNode::getRouteData()
}
}

void controlEvaluatorNode::removeOldDiagnostics(const rclcpp::Time & stamp)
void ControlEvaluatorNode::removeOldDiagnostics(const rclcpp::Time & stamp)
{
constexpr double KEEP_TIME = 1.0;
diag_queue_.erase(
Expand All @@ -76,7 +76,7 @@ void controlEvaluatorNode::removeOldDiagnostics(const rclcpp::Time & stamp)
diag_queue_.end());
}

void controlEvaluatorNode::removeDiagnosticsByName(const std::string & name)
void ControlEvaluatorNode::removeDiagnosticsByName(const std::string & name)
{
diag_queue_.erase(
std::remove_if(
Expand All @@ -87,13 +87,13 @@ void controlEvaluatorNode::removeDiagnosticsByName(const std::string & name)
diag_queue_.end());
}

void controlEvaluatorNode::addDiagnostic(
void ControlEvaluatorNode::addDiagnostic(
const diagnostic_msgs::msg::DiagnosticStatus & diag, const rclcpp::Time & stamp)
{
diag_queue_.push_back(std::make_pair(diag, stamp));
}

void controlEvaluatorNode::updateDiagnosticQueue(
void ControlEvaluatorNode::updateDiagnosticQueue(
const DiagnosticArray & input_diagnostics, const std::string & function,
const rclcpp::Time & stamp)
{
Expand All @@ -110,15 +110,15 @@ void controlEvaluatorNode::updateDiagnosticQueue(
removeOldDiagnostics(stamp);
}

void controlEvaluatorNode::onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg)
void ControlEvaluatorNode::onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg)
{
// add target diagnostics to the queue and remove old ones
for (const auto & function : target_functions_) {
updateDiagnosticQueue(*diag_msg, function, now());
}
}

DiagnosticStatus controlEvaluatorNode::generateAEBDiagnosticStatus(const DiagnosticStatus & diag)
DiagnosticStatus ControlEvaluatorNode::generateAEBDiagnosticStatus(const DiagnosticStatus & diag)
{
DiagnosticStatus status;
status.level = status.OK;
Expand All @@ -131,7 +131,7 @@ DiagnosticStatus controlEvaluatorNode::generateAEBDiagnosticStatus(const Diagnos
return status;
}

DiagnosticStatus controlEvaluatorNode::generateLaneletDiagnosticStatus(const Pose & ego_pose) const
DiagnosticStatus ControlEvaluatorNode::generateLaneletDiagnosticStatus(const Pose & ego_pose) const
{
const auto current_lanelets = [&]() {
lanelet::ConstLanelet closest_route_lanelet;
Expand Down Expand Up @@ -162,7 +162,7 @@ DiagnosticStatus controlEvaluatorNode::generateLaneletDiagnosticStatus(const Pos
return status;
}

DiagnosticStatus controlEvaluatorNode::generateKinematicStateDiagnosticStatus(
DiagnosticStatus ControlEvaluatorNode::generateKinematicStateDiagnosticStatus(
const Odometry & odom, const AccelWithCovarianceStamped & accel_stamped)
{
DiagnosticStatus status;
Expand Down Expand Up @@ -198,7 +198,7 @@ DiagnosticStatus controlEvaluatorNode::generateKinematicStateDiagnosticStatus(
return status;
}

DiagnosticStatus controlEvaluatorNode::generateLateralDeviationDiagnosticStatus(
DiagnosticStatus ControlEvaluatorNode::generateLateralDeviationDiagnosticStatus(
const Trajectory & traj, const Point & ego_point)
{
const double lateral_deviation = metrics::calcLateralDeviation(traj, ego_point);
Expand All @@ -214,7 +214,7 @@ DiagnosticStatus controlEvaluatorNode::generateLateralDeviationDiagnosticStatus(
return status;
}

DiagnosticStatus controlEvaluatorNode::generateYawDeviationDiagnosticStatus(
DiagnosticStatus ControlEvaluatorNode::generateYawDeviationDiagnosticStatus(
const Trajectory & traj, const Pose & ego_pose)
{
const double yaw_deviation = metrics::calcYawDeviation(traj, ego_pose);
Expand All @@ -230,7 +230,7 @@ DiagnosticStatus controlEvaluatorNode::generateYawDeviationDiagnosticStatus(
return status;
}

void controlEvaluatorNode::onTimer()
void ControlEvaluatorNode::onTimer()
{
DiagnosticArray metrics_msg;
const auto traj = traj_sub_.takeData();
Expand Down Expand Up @@ -278,4 +278,4 @@ void controlEvaluatorNode::onTimer()
} // namespace control_diagnostics

#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(control_diagnostics::controlEvaluatorNode)
RCLCPP_COMPONENTS_REGISTER_NODE(control_diagnostics::ControlEvaluatorNode)
2 changes: 1 addition & 1 deletion launch/tier4_control_launch/launch/control.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -376,7 +376,7 @@ def launch_setup(context, *args, **kwargs):
# control evaluator
control_evaluator_component = ComposableNode(
package="autoware_control_evaluator",
plugin="control_diagnostics::controlEvaluatorNode",
plugin="control_diagnostics::ControlEvaluatorNode",
name="control_evaluator",
remappings=[
("~/input/diagnostics", "/diagnostics"),
Expand Down

0 comments on commit ce09907

Please sign in to comment.