Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(autoware_control_validator): add polling subcribers #7426

Merged
merged 2 commits into from
Jun 11, 2024
Merged

feat(autoware_control_validator): add polling subcribers #7426

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
bbb569e
add polling subs
danielsanchezaran Jun 11, 2024
e184746
delete extra line
danielsanchezaran Jun 11, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 5 additions & 3 deletions control/autoware_control_validator/include/autoware_control_validator/control_validator.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
#include "autoware_control_validator/debug_marker.hpp"
#include "autoware_control_validator/msg/control_validator_status.hpp"
#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp"
#include "tier4_autoware_utils/ros/polling_subscriber.hpp"

#include <diagnostic_updater/diagnostic_updater.hpp>
#include <rclcpp/rclcpp.hpp>
Expand Down Expand Up @@ -48,7 +49,6 @@ class ControlValidator : public rclcpp::Node
public:
explicit ControlValidator(const rclcpp::NodeOptions & options);

void onReferenceTrajectory(const Trajectory::ConstSharedPtr msg);
void onPredictedTrajectory(const Trajectory::ConstSharedPtr msg);

bool checkValidMaxDistanceDeviation(const Trajectory & predicted_trajectory);
Expand All @@ -68,9 +68,11 @@ class ControlValidator : public rclcpp::Node

void setStatus(DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg);

rclcpp::Subscription<Odometry>::SharedPtr sub_kinematics_;
rclcpp::Subscription<Trajectory>::SharedPtr sub_reference_traj_;
rclcpp::Subscription<Trajectory>::SharedPtr sub_predicted_traj_;
tier4_autoware_utils::InterProcessPollingSubscriber<Odometry> sub_kinematics_{
this, "~/input/kinematics"};
tier4_autoware_utils::InterProcessPollingSubscriber<Trajectory> sub_reference_traj_{
this, "~/input/reference_trajectory"};
rclcpp::Publisher<ControlValidatorStatus>::SharedPtr pub_status_;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_markers_;

Expand Down
21 changes: 2 additions & 19 deletions control/autoware_control_validator/src/control_validator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,13 +28,6 @@ ControlValidator::ControlValidator(const rclcpp::NodeOptions & options)
: Node("control_validator", options)
{
using std::placeholders::_1;

sub_kinematics_ = create_subscription<Odometry>(
"~/input/kinematics", 1,
[this](const Odometry::ConstSharedPtr msg) { current_kinematics_ = msg; });
sub_reference_traj_ = create_subscription<Trajectory>(
"~/input/reference_trajectory", 1,
std::bind(&ControlValidator::onReferenceTrajectory, this, _1));
sub_predicted_traj_ = create_subscription<Trajectory>(
"~/input/predicted_trajectory", 1,
std::bind(&ControlValidator::onPredictedTrajectory, this, _1));
Expand Down Expand Up @@ -116,21 +109,11 @@ bool ControlValidator::isDataReady()
return true;
}

void ControlValidator::onReferenceTrajectory(const Trajectory::ConstSharedPtr msg)
{
if (msg->points.size() < 2) {
RCLCPP_ERROR(get_logger(), "planning trajectory size is invalid (%lu)", msg->points.size());
return;
}

current_reference_trajectory_ = msg;

return;
}

void ControlValidator::onPredictedTrajectory(const Trajectory::ConstSharedPtr msg)
{
current_predicted_trajectory_ = msg;
current_reference_trajectory_ = sub_reference_traj_.takeData();
current_kinematics_ = sub_kinematics_.takeData();

if (!isDataReady()) return;

Expand Down
Loading