forked from ros2/demos
-
Notifications
You must be signed in to change notification settings - Fork 0
/
lifecycle_listener.cpp
89 lines (75 loc) · 2.78 KB
/
lifecycle_listener.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
// Copyright 2016 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <functional>
#include <memory>
#include <string>
#include "lifecycle_msgs/msg/transition_event.hpp"
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
/// LifecycleListener class as a simple listener node
/**
* We subscribe to two topics
* - lifecycle_chatter: The data topic from the talker
* - lc_talker__transition_event: The topic publishing
* notifications about state changes of the node
* lc_talker
*/
class LifecycleListener : public rclcpp::Node
{
public:
explicit LifecycleListener(const std::string & node_name)
: Node(node_name)
{
// Data topic from the lc_talker node
sub_data_ = this->create_subscription<std_msgs::msg::String>(
"lifecycle_chatter", 10,
[this](std_msgs::msg::String::ConstSharedPtr msg) {return this->data_callback(msg);});
// Notification event topic. All state changes
// are published here as TransitionEvents with
// a start and goal state indicating the transition
sub_notification_ = this->create_subscription<lifecycle_msgs::msg::TransitionEvent>(
"/lc_talker/transition_event",
10,
[this](lifecycle_msgs::msg::TransitionEvent::ConstSharedPtr msg) {
return this->notification_callback(msg);
}
);
}
void data_callback(std_msgs::msg::String::ConstSharedPtr msg)
{
RCLCPP_INFO(get_logger(), "data_callback: %s", msg->data.c_str());
}
void notification_callback(lifecycle_msgs::msg::TransitionEvent::ConstSharedPtr msg)
{
RCLCPP_INFO(
get_logger(), "notify callback: Transition from state %s to %s",
msg->start_state.label.c_str(), msg->goal_state.label.c_str());
}
private:
std::shared_ptr<rclcpp::Subscription<std_msgs::msg::String>> sub_data_;
std::shared_ptr<rclcpp::Subscription<lifecycle_msgs::msg::TransitionEvent>>
sub_notification_;
};
int main(int argc, char ** argv)
{
// force flush of the stdout buffer.
// this ensures a correct sync of all prints
// even when executed simultaneously within the launch file.
setvbuf(stdout, NULL, _IONBF, BUFSIZ);
rclcpp::init(argc, argv);
auto lc_listener = std::make_shared<LifecycleListener>("lc_listener");
rclcpp::spin(lc_listener);
rclcpp::shutdown();
return 0;
}