feat(tier4_autoware_utils, obstacle_cruise): change to read topic by polling #6702

yuki-takagi-66 · 2024-03-28T09:12:10Z

Description

change to read topic by polling instead of best_effort callback

Tests performed

psim test and tier4 internal tests were performed

If we access to getData() without any call of updatelatestDate() following message will be shown
[component_container_mt-31] terminate called after throwing an instance of 'std::runtime_error'
[component_container_mt-31] what(): Bad_optional_access in class InterProcessPollingSubscriber
[component_container_mt-31] *** Aborted at 1712130709 (unix time) try "date -d @1712130709" if you are using GNU date ***
[component_container_mt-31] PC: @ 0x0 (unknown)
[component_container_mt-31] *** SIGABRT (@0x3e8002983f9) received by PID 2720761 (TID 0x7fe1de7ec640) from PID 2720761; stack trace: ***
[component_container_mt-31] @ 0x7fe1d84b6046 (unknown)
[component_container_mt-31] @ 0x7fe2136fb520 (unknown)
[component_container_mt-31] @ 0x7fe21374f9fc pthread_kill
[component_container_mt-31] @ 0x7fe2136fb476 raise
[component_container_mt-31] @ 0x7fe2136e17f3 abort
[component_container_mt-31] @ 0x7fe2139a3b9e (unknown)
[component_container_mt-31] @ 0x7fe2139af20c (unknown)
[component_container_mt-31] @ 0x7fe2139af277 std::terminate()
[component_container_mt-31] @ 0x7fe2139af4d8 __cxa_throw
[component_container_mt-31] @ 0x7fe20067cf8c _ZNK15motion_planning25ObstacleCruisePlannerNode18convertToObstaclesERKSt6vectorIN27autoware_auto_planning_msgs3msg16TrajectoryPoint_ISaIvEEESaIS6_EE.cold
[component_container_mt-31] @ 0x7fe2006ace54 motion_planning::ObstacleCruisePlannerNode::onTrajectory()
[component_container_mt-31] @ 0x7fe2006d7627 std::_Function_handler<>::_M_invoke()
[component_container_mt-31] @ 0x7fe2006d6462 ZNSt8__detail9__variant17__gen_vtable_implINS0_12_Multi_arrayIPFNS0_21__deduce_visit_resultIvEEOZN6rclcpp23AnySubscriptionCallbackIN27autoware_auto_planning_msgs3msg11Trajectory_ISaIvEEESA_E8dispatchESt10shared_ptrISB_ERKNS5_11MessageInfoEEUlOT_E_RSt7variantIJSt8functionIFvRKSB_EESN_IFvSP_SH_EESN_IFvRKNS5_17SerializedMessageEEESN_IFvSW_SH_EESN_IFvSt10unique_ptrISB_St14default_deleteISB_EEEESN_IFvS14_SH_EESN_IFvS11_ISU_S12_ISU_EEEESN_IFvS1A_SH_EESN_IFvSD_ISO_EEESN_IFvS1F_SH_EESN_IFvSD_ISV_EEESN_IFvS1K_SH_EESN_IFvRKS1F_EESN_IFvS1Q_SH_EESN_IFvRKS1K_EESN_IFvS1W_SH_EESN_IFvSE_EESN_IFvSE_SH_EESN_IFvSD_ISU_EEESN_IFvS25_SH_EEEEEJEEESt16integer_sequenceImJLm8EEEE14__visit_invokeESL_S2B
[component_container_mt-31] @ 0x7fe20075029a rclcpp::Subscription<>::handle_message()
[component_container_mt-31] @ 0x7fe213ccaba0 rclcpp::Executor::execute_subscription()
[component_container_mt-31] @ 0x7fe213ccc10e rclcpp::Executor::execute_any_executable()
[component_container_mt-31] @ 0x7fe213cd2432 rclcpp::executors::MultiThreadedExecutor::run()
[component_container_mt-31] @ 0x7fe2139dd253 (unknown)
[component_container_mt-31] @ 0x7fe21374dac3 (unknown)
[component_container_mt-31] @ 0x7fe2137dfa40 (unknown)
[component_container_mt-31] @ 0x0 (unknown)

Notes for reviewers

Interface changes

Effects on system behavior

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

I've confirmed the contribution guidelines.
The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

The PR follows the pull request guidelines.
The PR has been properly tested.
The PR has been reviewed by the code owners.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

There are no open discussions or they are tracked via tickets.
The PR is ready for merge.

After all checkboxes are checked, anyone who has write access can merge the PR.

ohsawa1204 · 2024-03-29T08:14:07Z

@yuki-takagi-66

thank you very much for your change!
From a technical point of view, let me point out one thing:

Dedicated callback group is created for each of traj_sub_, objects_sub_, odom_sub_, and acc_sub_ in PR #6702.
As for objects_sub_, odom_sub_, and acc_sub_, I think one common callback group is enough for them.
Please see attached figure below:

yuki-takagi-66 · 2024-03-29T09:02:12Z

@ohsawa1204
Thank you for the review.
I also recognize this problem and think this wastes a bit of computing resources.
However, I cannot design the code that resolve this problem with capsulizing the callback group from the user codes.
I feel that defining the common callback group as a static member of the class is not enough in this perspective.

Do you have any ideas to implement?
If you know how much computing resources are wasted by this problem, please let me know.

takam5f2 · 2024-03-29T09:16:16Z

common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_reciever.hpp

+    rclcpp::MessageInfo message_info;
+    T tmp;
+    // "while" is required for the case of Queue size (QoS) is 2 or lager
+    while (subscriber->take(tmp, message_info)) {


As Queue size is fixed to '1' above, I think that if (subscriber->take(tmp, message_info)) is less confusing.

Thank you. I've changed to if

https://github.com/autowarefoundation/autoware.universe/pull/6702/files#diff-6d7e094b3822fa518603939f3378c3dccb336e65c0530da0c4d4edd239294b80R46-R47

takayuki5168 · 2024-03-29T16:06:45Z

common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_reciever.hpp

+  typename rclcpp::Subscription<T>::SharedPtr subscriber;
+
+public:
+  explicit InterProcessPollingReceiver(rclcpp::Node * node, const std::string & topic_name)


explict is not required for the two arguments.

Yes. we can remove explicit.
But explicit makes sense even for multi arguments constructor.

determined to maintain explicit by a book written by Bjarne Stroustrup

takayuki5168 · 2024-03-29T16:09:29Z

common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_reciever.hpp

+      noexec_subscription_options);
+  };
+
+  void takeLastData(std::optional<T> & data)


takeLatestData sounds less confusing

In my knowledge, term "last" is preferred in this context.
For example, https://learn.microsoft.com/ja-jp/dotnet/api/system.collections.generic.queue-1?view=net-8.0

For the user of this code, we determine to use the term "latest"

takayuki5168 · 2024-03-29T16:11:07Z

common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_reciever.hpp

+      noexec_subscription_options);
+  };
+
+  void takeLastData(std::optional<T> & data)


std::optional<T> takeLatestData() sounds better. Any reason to add the argument as a reference?

takayuki5168 · 2024-03-29T16:13:05Z

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp

+  class PollingInputTopics
+  {
+  private:
+    tier4_autoware_utils::InterProcessPollingReceiver<Odometry> ego_odom_sub_;
+    tier4_autoware_utils::InterProcessPollingReceiver<PredictedObjects> objects_sub_;
+    tier4_autoware_utils::InterProcessPollingReceiver<AccelWithCovarianceStamped> acc_sub_;
+
+  public:
+    PollingInputTopics(rclcpp::Node * node)
+    : ego_odom_sub_(node, "~/input/odometry"),
+      objects_sub_(node, "~/input/objects"),
+      acc_sub_(node, "~/input/acceleration")
+    {
+    }
+    std::optional<Odometry> ego_odom_opt;
+    std::optional<PredictedObjects> objects_opt;
+    std::optional<AccelWithCovarianceStamped> ego_accel_opt;
+    bool takeData()
+    {
+      ego_odom_sub_.takeLastData(ego_odom_opt);
+      objects_sub_.takeLastData(objects_opt);
+      acc_sub_.takeLastData(ego_accel_opt);
+
+      if (!ego_odom_opt.has_value() || !objects_opt.has_value() || !ego_accel_opt.has_value()) {
+        return false;
+      }
+      return true;
+    };
+  } polling_topics_{this};


The motivation to create a class in tier4_autoware_utils is not to create member variables for the data of each topic on the node side.
We can call ego_odom_sub_.takeLastData directly in the main callback of the node.

@takayuki5168
I think we should discuss aside

delete this class and change to declare subscriber class on the node class

takayuki5168 · 2024-03-29T16:15:54Z

common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_reciever.hpp

+{
+
+template <typename T>
+class InterProcessPollingReceiver


InterProcessPollingReceiver is hard to understand for me.
InterProcessSubscriber sounds easy to understand.

@takam5f2 @ohsawa1204 What do you think?

InterProcessPollingReceiver is hard to understand for me. InterProcessSubscriber sounds easy to understand.

@takam5f2 @ohsawa1204 What do you think?

@takayuki5168
I agree!

OK, I see.
However, InterProcess denotes less meaning in this context, hence we need some additional modifier term.
How do you think?

To recognize the difference to between the conventional call back subscriber, we use InterProcessPollingSubscriber as the name of this class

ohsawa1204 · 2024-04-01T01:10:30Z

@ohsawa1204 Thank you for the review. I also recognize this problem and think this wastes a bit of computing resources. However, I cannot design the code that resolve this problem with capsulizing the callback group from the user codes. I feel that defining the common callback group as a static member of the class is not enough in this perspective.

Do you have any ideas to implement? If you know how much computing resources are wasted by this problem, please let me know.

@yuki-takagi-66
CallbackGroup is defined in callback_group.hpp and seems not so large.
sizeof(CallbackGroup) shows 240 bytes in my environment.
There are about 1000 subscriptions in whole autoware which may not be so precise, and if 1000 subscriptions have dedicated callback group for each, about 240KB are wasted.
It seems not so serious and I guess there is no other negative impact than memory resource.

yuki-takagi-66 · 2024-04-01T02:38:44Z

@ohsawa1204 About unnecessary call back group issue
Thank you. I'll ignore this issue with assuming 240KB is small enough (at least currently)

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp

takayuki5168 · 2024-04-02T17:44:31Z

common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp

+    }
+    return data.has_value();
+  };
+  const std::optional<T> & getData() const { return data; };


Any reason to add & here?

Also, if we do not check whether the optional is not nullopt or not on the node side, it may be better to return T instead of std::optional.

Any reason to add & here?

While I'm not familiar with the reference in C++
Maybe & is required to prevent copy.
https://learn.microsoft.com/ja-jp/cpp/cpp/reference-type-function-returns?view=msvc-170

takam5f2 · 2024-04-03T02:32:47Z

common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp

+{
+
+template <typename T>
+class InterProcessPollingSubscriber


@yuki-takagi-66

I am wondering if another wrapper class is created for a different use.
InterProcessPollingSubscriber is wrapper class that wraps a subscriber whose queue size is 1.
On the other hand, if another developer wants to use a subscriber whose queue size is multiple, does he have to create another class.

This comment may seem that I disagree with using use the wrapper function, but I don't disagree.
I want to align understanding because it will be used commonly.

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

codecov · 2024-04-03T11:34:50Z

Codecov Report

Attention: Patch coverage is 33.33333% with 12 lines in your changes are missing coverage. Please review.

Project coverage is 14.90%. Comparing base (8948859) to head (d38ec7f).
Report is 9 commits behind head on main.

Files	Patch %	Lines
planning/obstacle_cruise_planner/src/node.cpp	33.33%	5 Missing and 7 partials ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #6702      +/-   ##
==========================================
- Coverage   14.94%   14.90%   -0.04%     
==========================================
  Files        1943     1944       +1     
  Lines      133953   135017    +1064     
  Branches    39841    40517     +676     
==========================================
+ Hits        20020    20130     +110     
- Misses      91656    92483     +827     
- Partials    22277    22404     +127

Flag	Coverage Δ		*Carryforward flag
differential	`16.97% <33.33%> (?)`
total	`14.94% <ø> (-0.01%)`	⬇️	Carriedforward from 8948859

*This pull request uses carry forward flags. Click here to find out more.

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

…polling (autowarefoundation#6702) change to read topic by polling Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

…polling (autowarefoundation#6702) (#1224) change to read topic by polling Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

…polling (autowarefoundation#6702) change to read topic by polling Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

…polling (#6702) change to read topic by polling Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

Owen-Liuyuxuan · 2024-04-10T04:55:29Z

Is there documentation / code comments for understanding the meaning / usage of the Polling Subscriber?

…polling (autowarefoundation#6702) change to read topic by polling Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

github-actions bot added component:planning Route planning, decision-making, and navigation. (auto-assigned) component:common Common packages from the autoware-common repository. (auto-assigned) labels Mar 28, 2024

yuki-takagi-66 changed the title ~~prototype~~ feat(tier4_autoware_utils, obstacle_cruise): change to read topic by polling Mar 29, 2024

yuki-takagi-66 marked this pull request as ready for review March 29, 2024 06:09

yuki-takagi-66 requested review from kosuke55, satoshi-ota, takayuki5168, soblin and TakaHoribe as code owners March 29, 2024 06:09

takam5f2 reviewed Mar 29, 2024

View reviewed changes

takayuki5168 reviewed Mar 29, 2024

View reviewed changes

yuki-takagi-66 force-pushed the feat/disable_recieve_callback branch 2 times, most recently from 3cc8c87 to 0e2a259 Compare April 2, 2024 05:29

yuki-takagi-66 added 8 commits April 2, 2024 17:56

prototype

5758493

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

move to tier4/autoware_utils

8d24fbc

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

remove unused change

4c569d9

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

fix spell

bcdf07c

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

change while to if

8659816

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

reflect the review comments

8fa1e84

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

upadate

6cbc75f

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

update

293191e

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

yuki-takagi-66 force-pushed the feat/disable_recieve_callback branch from 0e2a259 to 293191e Compare April 2, 2024 08:58

takayuki5168 reviewed Apr 2, 2024

View reviewed changes

common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp Show resolved Hide resolved

takayuki5168 reviewed Apr 2, 2024

View reviewed changes

takam5f2 reviewed Apr 3, 2024

View reviewed changes

change to return raw T instead of optional<T>

3c7bdef

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

yuki-takagi-66 assigned takayuki5168 Apr 3, 2024

yuki-takagi-66 added the tag:run-build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) label Apr 3, 2024

takayuki5168 approved these changes Apr 3, 2024

View reviewed changes

yuki-takagi-66 added 2 commits April 3, 2024 17:16

add underbar

fe39cc6

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

add include

d38ec7f

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

yuki-takagi-66 merged commit c0fcd75 into autowarefoundation:main Apr 3, 2024
26 of 28 checks passed

yuki-takagi-66 deleted the feat/disable_recieve_callback branch April 3, 2024 12:00

yuki-takagi-66 mentioned this pull request Apr 4, 2024

feat(tier4_autoware_utils, obstacle_cruise): change to read topic by polling (#6702) tier4/autoware.universe#1224

Merged

7 tasks

satoshi-ota mentioned this pull request Apr 9, 2024

fix(tier4_autoware_utils): fix -Werror=unused-parameter #6764

Merged

4 tasks

esteve pushed a commit that referenced this pull request Apr 9, 2024

feat(tier4_autoware_utils, obstacle_cruise): change to read topic by …

3c94529

…polling (#6702) change to read topic by polling Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

feat(tier4_autoware_utils, obstacle_cruise): change to read topic by polling #6702

feat(tier4_autoware_utils, obstacle_cruise): change to read topic by polling #6702

yuki-takagi-66 commented Mar 28, 2024 •

edited

Loading

ohsawa1204 commented Mar 29, 2024

yuki-takagi-66 commented Mar 29, 2024 •

edited

Loading

takam5f2 Mar 29, 2024

yuki-takagi-66 Mar 29, 2024

takayuki5168 Mar 29, 2024

yuki-takagi-66 Apr 1, 2024

yuki-takagi-66 Apr 1, 2024

takayuki5168 Mar 29, 2024

yuki-takagi-66 Apr 1, 2024

yuki-takagi-66 Apr 1, 2024 •

edited

Loading

takayuki5168 Mar 29, 2024

takayuki5168 Mar 29, 2024

yuki-takagi-66 Apr 1, 2024

yuki-takagi-66 Apr 1, 2024

takayuki5168 Mar 29, 2024

ohsawa1204 Apr 1, 2024

yuki-takagi-66 Apr 1, 2024

yuki-takagi-66 Apr 1, 2024

ohsawa1204 commented Apr 1, 2024 •

edited

Loading

yuki-takagi-66 commented Apr 1, 2024

takayuki5168 Apr 2, 2024

takayuki5168 Apr 2, 2024

yuki-takagi-66 Apr 3, 2024

takam5f2 Apr 3, 2024 •

edited

Loading

codecov bot commented Apr 3, 2024 •

edited

Loading

Owen-Liuyuxuan commented Apr 10, 2024

feat(tier4_autoware_utils, obstacle_cruise): change to read topic by polling #6702

feat(tier4_autoware_utils, obstacle_cruise): change to read topic by polling #6702

Conversation

yuki-takagi-66 commented Mar 28, 2024 • edited Loading

Description

Related links

Tests performed

Notes for reviewers

Interface changes

Effects on system behavior

Pre-review checklist for the PR author

In-review checklist for the PR reviewers

Post-review checklist for the PR author

ohsawa1204 commented Mar 29, 2024

yuki-takagi-66 commented Mar 29, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

yuki-takagi-66 Apr 1, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

ohsawa1204 commented Apr 1, 2024 • edited Loading

yuki-takagi-66 commented Apr 1, 2024

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

takam5f2 Apr 3, 2024 • edited Loading

Choose a reason for hiding this comment

codecov bot commented Apr 3, 2024 • edited Loading

Codecov Report

Owen-Liuyuxuan commented Apr 10, 2024

yuki-takagi-66 commented Mar 28, 2024 •

edited

Loading

yuki-takagi-66 commented Mar 29, 2024 •

edited

Loading

yuki-takagi-66 Apr 1, 2024 •

edited

Loading

ohsawa1204 commented Apr 1, 2024 •

edited

Loading

takam5f2 Apr 3, 2024 •

edited

Loading

codecov bot commented Apr 3, 2024 •

edited

Loading