Skip to content

Commit

Permalink
feat(trajectory_follower_node): use polling subscribers (#7403)
Browse files Browse the repository at this point in the history 
* use polling subscriber in trajectory follower node

Signed-off-by: mohammad alqudah <[email protected]>

* use polling subscriber in simple trajectory follower

Signed-off-by: mohammad alqudah <[email protected]>

---------

Signed-off-by: mohammad alqudah <[email protected]>
  • Loading branch information
@mkquda @KhalilSelyan
mkquda authored and KhalilSelyan committed Jul 22, 2024
1 parent d7b90a1 commit b4cb539
Show file tree
Hide file tree
Showing 4 changed files with 74 additions and 78 deletions.
43 changes: 28 additions & 15 deletions control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "tier4_autoware_utils/ros/logger_level_configure.hpp"
#include "tier4_autoware_utils/ros/polling_subscriber.hpp"
#include "tier4_autoware_utils/system/stop_watch.hpp"
#include "trajectory_follower_base/lateral_controller_base.hpp"
#include "trajectory_follower_base/longitudinal_controller_base.hpp"
Expand Down Expand Up @@ -75,21 +76,34 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node
std::shared_ptr<trajectory_follower::LongitudinalControllerBase> longitudinal_controller_;
std::shared_ptr<trajectory_follower::LateralControllerBase> lateral_controller_;

rclcpp::Subscription<autoware_planning_msgs::msg::Trajectory>::SharedPtr sub_ref_path_;
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odometry_;
rclcpp::Subscription<autoware_vehicle_msgs::msg::SteeringReport>::SharedPtr sub_steering_;
rclcpp::Subscription<geometry_msgs::msg::AccelWithCovarianceStamped>::SharedPtr sub_accel_;
rclcpp::Subscription<OperationModeState>::SharedPtr sub_operation_mode_;
// Subscribers
tier4_autoware_utils::InterProcessPollingSubscriber<autoware_planning_msgs::msg::Trajectory>
sub_ref_path_{this, "~/input/reference_trajectory"};

tier4_autoware_utils::InterProcessPollingSubscriber<nav_msgs::msg::Odometry> sub_odometry_{
this, "~/input/current_odometry"};

tier4_autoware_utils::InterProcessPollingSubscriber<autoware_vehicle_msgs::msg::SteeringReport>
sub_steering_{this, "~/input/current_steering"};

tier4_autoware_utils::InterProcessPollingSubscriber<
geometry_msgs::msg::AccelWithCovarianceStamped>
sub_accel_{this, "~/input/current_accel"};

tier4_autoware_utils::InterProcessPollingSubscriber<OperationModeState> sub_operation_mode_{
this, "~/input/current_operation_mode"};

// Publishers
rclcpp::Publisher<autoware_control_msgs::msg::Control>::SharedPtr control_cmd_pub_;
rclcpp::Publisher<Float64Stamped>::SharedPtr pub_processing_time_lat_ms_;
rclcpp::Publisher<Float64Stamped>::SharedPtr pub_processing_time_lon_ms_;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_marker_pub_;

autoware_planning_msgs::msg::Trajectory::SharedPtr current_trajectory_ptr_;
nav_msgs::msg::Odometry::SharedPtr current_odometry_ptr_;
autoware_vehicle_msgs::msg::SteeringReport::SharedPtr current_steering_ptr_;
geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr current_accel_ptr_;
OperationModeState::SharedPtr current_operation_mode_ptr_;
autoware_planning_msgs::msg::Trajectory::ConstSharedPtr current_trajectory_ptr_;
nav_msgs::msg::Odometry::ConstSharedPtr current_odometry_ptr_;
autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr current_steering_ptr_;
geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr current_accel_ptr_;
OperationModeState::ConstSharedPtr current_operation_mode_ptr_;

enum class LateralControllerMode {
INVALID = 0,
Expand All @@ -104,12 +118,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node
/**
* @brief compute control command, and publish periodically
*/
boost::optional<trajectory_follower::InputData> createInputData(rclcpp::Clock & clock) const;
boost::optional<trajectory_follower::InputData> createInputData(rclcpp::Clock & clock);
void callbackTimerControl();
void onTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr);
void onOdometry(const nav_msgs::msg::Odometry::SharedPtr msg);
void onSteering(const autoware_vehicle_msgs::msg::SteeringReport::SharedPtr msg);
void onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg);
bool processData(rclcpp::Clock & clock);
bool isTimeOut(const LongitudinalOutput & lon_out, const LateralOutput & lat_out);
LateralControllerMode getLateralControllerMode(const std::string & algorithm_name) const;
LongitudinalControllerMode getLongitudinalControllerMode(
Expand All @@ -125,6 +136,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node
void publishProcessingTime(
const double t_ms, const rclcpp::Publisher<Float64Stamped>::SharedPtr pub);
StopWatch<std::chrono::milliseconds> stop_watch_;

static constexpr double logger_throttle_interval = 5000;
};
} // namespace trajectory_follower_node
} // namespace autoware::motion::control
Expand Down
14 changes: 9 additions & 5 deletions .../trajectory_follower_node/include/trajectory_follower_node/simple_trajectory_follower.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,8 @@
#ifndef TRAJECTORY_FOLLOWER_NODE__SIMPLE_TRAJECTORY_FOLLOWER_HPP_
#define TRAJECTORY_FOLLOWER_NODE__SIMPLE_TRAJECTORY_FOLLOWER_HPP_

#include "tier4_autoware_utils/ros/polling_subscriber.hpp"

#include <rclcpp/rclcpp.hpp>

#include <autoware_control_msgs/msg/control.hpp>
Expand Down Expand Up @@ -42,20 +44,22 @@ class SimpleTrajectoryFollower : public rclcpp::Node
~SimpleTrajectoryFollower() = default;

private:
rclcpp::Subscription<Odometry>::SharedPtr sub_kinematics_;
rclcpp::Subscription<Trajectory>::SharedPtr sub_trajectory_;
tier4_autoware_utils::InterProcessPollingSubscriber<Odometry> sub_kinematics_{
this, "~/input/kinematics"};
tier4_autoware_utils::InterProcessPollingSubscriber<Trajectory> sub_trajectory_{
this, "~/input/trajectory"};
rclcpp::Publisher<Control>::SharedPtr pub_cmd_;
rclcpp::TimerBase::SharedPtr timer_;

Trajectory::SharedPtr trajectory_;
Odometry::SharedPtr odometry_;
Trajectory::ConstSharedPtr trajectory_;
Odometry::ConstSharedPtr odometry_;
TrajectoryPoint closest_traj_point_;
bool use_external_target_vel_;
double external_target_vel_;
double lateral_deviation_;

void onTimer();
bool checkData();
bool processData();
void updateClosest();
double calcSteerCmd();
double calcAccCmd();
Expand Down
75 changes: 25 additions & 50 deletions control/trajectory_follower_node/src/controller_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,17 +62,6 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control
throw std::domain_error("[LongitudinalController] invalid algorithm");
}

sub_ref_path_ = create_subscription<autoware_planning_msgs::msg::Trajectory>(
"~/input/reference_trajectory", rclcpp::QoS{1}, std::bind(&Controller::onTrajectory, this, _1));
sub_steering_ = create_subscription<autoware_vehicle_msgs::msg::SteeringReport>(
"~/input/current_steering", rclcpp::QoS{1}, std::bind(&Controller::onSteering, this, _1));
sub_odometry_ = create_subscription<nav_msgs::msg::Odometry>(
"~/input/current_odometry", rclcpp::QoS{1}, std::bind(&Controller::onOdometry, this, _1));
sub_accel_ = create_subscription<geometry_msgs::msg::AccelWithCovarianceStamped>(
"~/input/current_accel", rclcpp::QoS{1}, std::bind(&Controller::onAccel, this, _1));
sub_operation_mode_ = create_subscription<OperationModeState>(
"~/input/current_operation_mode", rclcpp::QoS{1},
[this](const OperationModeState::SharedPtr msg) { current_operation_mode_ptr_ = msg; });
control_cmd_pub_ = create_publisher<autoware_control_msgs::msg::Control>(
"~/output/control_cmd", rclcpp::QoS{1}.transient_local());
pub_processing_time_lat_ms_ =
Expand Down Expand Up @@ -112,24 +101,32 @@ Controller::LongitudinalControllerMode Controller::getLongitudinalControllerMode
return LongitudinalControllerMode::INVALID;
}

void Controller::onTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg)
bool Controller::processData(rclcpp::Clock & clock)
{
current_trajectory_ptr_ = msg;
}

void Controller::onOdometry(const nav_msgs::msg::Odometry::SharedPtr msg)
{
current_odometry_ptr_ = msg;
}
bool is_ready = true;

const auto & logData = [&clock, this](const std::string & data_type) {
std::string msg = "Waiting for " + data_type + " data";
RCLCPP_INFO_THROTTLE(get_logger(), clock, logger_throttle_interval, msg.c_str());
};

const auto & getData = [&logData](auto & dest, auto & sub, const std::string & data_type = "") {
const auto temp = sub.takeData();
if (temp) {
dest = temp;
return true;
}
if (!data_type.empty()) logData(data_type);
return false;
};

void Controller::onSteering(const autoware_vehicle_msgs::msg::SteeringReport::SharedPtr msg)
{
current_steering_ptr_ = msg;
}
is_ready &= getData(current_accel_ptr_, sub_accel_, "acceleration");
is_ready &= getData(current_steering_ptr_, sub_steering_, "steering");
is_ready &= getData(current_trajectory_ptr_, sub_ref_path_, "trajectory");
is_ready &= getData(current_odometry_ptr_, sub_odometry_, "odometry");
is_ready &= getData(current_operation_mode_ptr_, sub_operation_mode_, "operation mode");

void Controller::onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg)
{
current_accel_ptr_ = msg;
return is_ready;
}

bool Controller::isTimeOut(
Expand All @@ -152,31 +149,9 @@ bool Controller::isTimeOut(
return false;
}

boost::optional<trajectory_follower::InputData> Controller::createInputData(
rclcpp::Clock & clock) const
boost::optional<trajectory_follower::InputData> Controller::createInputData(rclcpp::Clock & clock)
{
if (!current_trajectory_ptr_) {
RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for trajectory.");
return {};
}

if (!current_odometry_ptr_) {
RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for current odometry.");
return {};
}

if (!current_steering_ptr_) {
RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for current steering.");
return {};
}

if (!current_accel_ptr_) {
RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for current accel.");
return {};
}

if (!current_operation_mode_ptr_) {
RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for current operation mode.");
if (!processData(clock)) {
return {};
}

Expand Down
20 changes: 12 additions & 8 deletions control/trajectory_follower_node/src/simple_trajectory_follower.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,11 +31,6 @@ SimpleTrajectoryFollower::SimpleTrajectoryFollower(const rclcpp::NodeOptions & o
{
pub_cmd_ = create_publisher<Control>("output/control_cmd", 1);

sub_kinematics_ = create_subscription<Odometry>(
"input/kinematics", 1, [this](const Odometry::SharedPtr msg) { odometry_ = msg; });
sub_trajectory_ = create_subscription<Trajectory>(
"input/trajectory", 1, [this](const Trajectory::SharedPtr msg) { trajectory_ = msg; });

use_external_target_vel_ = declare_parameter<bool>("use_external_target_vel");
external_target_vel_ = declare_parameter<float>("external_target_vel");
lateral_deviation_ = declare_parameter<float>("lateral_deviation");
Expand All @@ -47,7 +42,7 @@ SimpleTrajectoryFollower::SimpleTrajectoryFollower(const rclcpp::NodeOptions & o

void SimpleTrajectoryFollower::onTimer()
{
if (!checkData()) {
if (!processData()) {
RCLCPP_INFO(get_logger(), "data not ready");
return;
}
Expand Down Expand Up @@ -110,9 +105,18 @@ double SimpleTrajectoryFollower::calcAccCmd()
return acc;
}

bool SimpleTrajectoryFollower::checkData()
bool SimpleTrajectoryFollower::processData()
{
return (trajectory_ && odometry_);
bool is_ready = true;
const auto & getData = [](auto & dest, auto & sub) {
const auto temp = sub.takeData();
if (!temp) return false;
dest = temp;
return true;
};
is_ready &= getData(odometry_, sub_kinematics_);
is_ready &= getData(trajectory_, sub_trajectory_);
return is_ready;
}

} // namespace simple_trajectory_follower
Expand Down

0 comments on commit b4cb539

Please sign in to comment.