File tree 2 files changed +8
-11
lines changed
control/operation_mode_transition_manager/src
2 files changed +8
-11
lines changed Original file line number Diff line number Diff line change @@ -22,14 +22,6 @@ namespace operation_mode_transition_manager
22
22
OperationModeTransitionManager::OperationModeTransitionManager (const rclcpp::NodeOptions & options)
23
23
: Node(" operation_mode_transition_manager" , options), compatibility_(this )
24
24
{
25
- sub_control_mode_report_ = create_subscription<ControlModeReport>(
26
- " control_mode_report" , 1 ,
27
- [this ](const ControlModeReport::SharedPtr msg) { control_mode_report_ = *msg; });
28
-
29
- sub_gate_operation_mode_ = create_subscription<OperationModeState>(
30
- " gate_operation_mode" , 1 ,
31
- [this ](const OperationModeState::SharedPtr msg) { gate_operation_mode_ = *msg; });
32
-
33
25
cli_control_mode_ = create_client<ControlModeCommand>(" control_mode_request" );
34
26
pub_debug_info_ = create_publisher<ModeChangeBase::DebugInfo>(" ~/debug_info" , 1 );
35
27
@@ -214,6 +206,9 @@ void OperationModeTransitionManager::processTransition()
214
206
215
207
void OperationModeTransitionManager::onTimer ()
216
208
{
209
+ control_mode_report_ = *sub_control_mode_report_.takeData ();
210
+ gate_operation_mode_ = *sub_gate_operation_mode_.takeData ();
211
+
217
212
for (const auto & [type, mode] : modes_) {
218
213
mode->update (current_mode_ == type && transition_);
219
214
}
Original file line number Diff line number Diff line change 21
21
#include < component_interface_specs/system.hpp>
22
22
#include < component_interface_utils/rclcpp.hpp>
23
23
#include < rclcpp/rclcpp.hpp>
24
+ #include < tier4_autoware_utils/ros/polling_subscriber.hpp>
24
25
25
26
#include < memory>
26
27
#include < unordered_map>
27
- #include < utility>
28
28
29
29
namespace operation_mode_transition_manager
30
30
{
@@ -49,8 +49,10 @@ class OperationModeTransitionManager : public rclcpp::Node
49
49
const ChangeOperationModeAPI::Service::Response::SharedPtr response);
50
50
51
51
using ControlModeCommandType = ControlModeCommand::Request::_mode_type;
52
- rclcpp::Subscription<ControlModeReport>::SharedPtr sub_control_mode_report_;
53
- rclcpp::Subscription<OperationModeState>::SharedPtr sub_gate_operation_mode_;
52
+ tier4_autoware_utils::InterProcessPollingSubscriber<ControlModeReport> sub_control_mode_report_{
53
+ this , " control_mode_report" };
54
+ tier4_autoware_utils::InterProcessPollingSubscriber<OperationModeState> sub_gate_operation_mode_{
55
+ this , " gate_operation_mode" };
54
56
rclcpp::Client<ControlModeCommand>::SharedPtr cli_control_mode_;
55
57
rclcpp::Publisher<ModeChangeBase::DebugInfo>::SharedPtr pub_debug_info_;
56
58
rclcpp::TimerBase::SharedPtr timer_;
You can’t perform that action at this time.
0 commit comments