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(goal_distance_calculator): change to read topic by polling #7316

Closed
wants to merge 6 commits into from
Closed

feat(goal_distance_calculator): change to read topic by polling #7316

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
3f8f5c8
delete unnecessary member
Autumn60 Jun 6, 2024
ebdd17e
replace rclcpp::Subscription to autoware_utils::InterProcessPollingSu…
Autumn60 Jun 6, 2024
2b723be
format by clang-format
Autumn60 Jun 6, 2024
012c23e
delete unnecessary callback func
Autumn60 Jun 11, 2024
cda066e
unified subscribing
Autumn60 Jun 11, 2024
61d1a56
Merge branch 'main' into feat/goal_distance_calculator/sub_by_polling
Autumn60 Jun 14, 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
10 changes: 4 additions & 6 deletions ...al_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@

#include <rclcpp/rclcpp.hpp>
#include <tier4_autoware_utils/ros/debug_publisher.hpp>
#include <tier4_autoware_utils/ros/polling_subscriber.hpp>
#include <tier4_autoware_utils/ros/self_pose_listener.hpp>

#include <autoware_planning_msgs/msg/lanelet_route.hpp>
Expand All @@ -44,16 +45,13 @@ class GoalDistanceCalculatorNode : public rclcpp::Node

private:
// Subscriber
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr sub_initial_pose_;
tier4_autoware_utils::SelfPoseListener self_pose_listener_;
rclcpp::Subscription<autoware_planning_msgs::msg::LaneletRoute>::SharedPtr sub_route_;
tier4_autoware_utils::InterProcessPollingSubscriber<autoware_planning_msgs::msg::LaneletRoute>
sub_route_{this, "/planning/mission_planning/route"};

// Data Buffer
geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_;
autoware_planning_msgs::msg::LaneletRoute::SharedPtr route_;

// Callback
void onRoute(const autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr & msg);
autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr route_;

// Publisher
tier4_autoware_utils::DebugPublisher debug_publisher_;
Expand Down
14 changes: 2 additions & 12 deletions common/goal_distance_calculator/src/goal_distance_calculator_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,12 +33,6 @@ GoalDistanceCalculatorNode::GoalDistanceCalculatorNode(const rclcpp::NodeOptions
self_pose_listener_(this),
debug_publisher_(this, "goal_distance_calculator")
{
using std::placeholders::_1;

static constexpr std::size_t queue_size = 1;
rclcpp::QoS durable_qos(queue_size);
durable_qos.transient_local();

// Node Parameter
node_param_.update_rate = declare_parameter<double>("update_rate");
node_param_.oneshot = declare_parameter<bool>("oneshot");
Expand All @@ -47,11 +41,6 @@ GoalDistanceCalculatorNode::GoalDistanceCalculatorNode(const rclcpp::NodeOptions
goal_distance_calculator_ = std::make_unique<GoalDistanceCalculator>();
goal_distance_calculator_->setParam(param_);

// Subscriber
sub_route_ = create_subscription<autoware_planning_msgs::msg::LaneletRoute>(
"/planning/mission_planning/route", queue_size,
[&](const autoware_planning_msgs::msg::LaneletRoute::SharedPtr msg_ptr) { route_ = msg_ptr; });

// Wait for first self pose
self_pose_listener_.waitForFirstPose();

Expand All @@ -70,7 +59,7 @@ bool GoalDistanceCalculatorNode::isDataReady()
return false;
}

if (!route_) {
if (route_) {
RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "waiting for route msg...");
return false;
}
Expand All @@ -92,6 +81,7 @@ bool GoalDistanceCalculatorNode::isDataTimeout()
void GoalDistanceCalculatorNode::onTimer()
{
current_pose_ = self_pose_listener_.getCurrentPose();
route_ = sub_route_.takeData();

if (!isDataReady()) {
return;
Expand Down
Loading