SMACC2
cb_wait_nav2_nodes.cpp
Go to the documentation of this file.
1// Copyright 2021 RobosoftAI Inc.
2//
3// Licensed under the Apache License, Version 2.0 (the "License");
4// you may not use this file except in compliance with the License.
5// You may obtain a copy of the License at
6//
7// http://www.apache.org/licenses/LICENSE-2.0
8//
9// Unless required by applicable law or agreed to in writing, software
10// distributed under the License is distributed on an "AS IS" BASIS,
11// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12// See the License for the specific language governing permissions and
13// limitations under the License.
14
15/*****************************************************************************************************************
16 *
17 * Authors: Pablo Inigo Blasco, Brett Aldrich
18 *
19 ******************************************************************************************************************/
20
23
24namespace cl_nav2z
25{
26CbWaitNav2Nodes::CbWaitNav2Nodes(std::vector<Nav2Nodes> waitNodes)
27: CbWaitNav2Nodes("/bond", waitNodes)
28{
29}
30
31CbWaitNav2Nodes::CbWaitNav2Nodes(std::string topic, std::vector<Nav2Nodes> waitNodes)
32: topicname_(topic), waitNodes_(waitNodes)
33{
34}
35
36void CbWaitNav2Nodes::onMessageReceived(const bond::msg::Status & msg)
37{
38 auto value = fromString(msg.id);
39 bool updated = false;
40 // RCLCPP_INFO(getLogger(), "[CbWaitNav2Nodes] received '%s'", msg.id.c_str());
41
42 if (receivedAliveMsg_.count(value) && !receivedAliveMsg_[value])
43 {
44 RCLCPP_INFO(getLogger(), "[CbWaitNav2Nodes] '%s' alive received", msg.id.c_str());
45 receivedAliveMsg_[value] = true;
46 updated = true;
47 }
48
49 if (updated)
50 {
51 bool success = true;
52 std::stringstream ss;
53 for (auto & pair : receivedAliveMsg_)
54 {
55 if (!pair.second)
56 {
57 success = false;
58 }
59 ss << "- " << toString(pair.first) << ": " << (pair.second ? "ALIVE" : "WAITING")
60 << std::endl;
61 }
62
63 RCLCPP_INFO_STREAM(
64 getLogger(), "[CbWaitNav2Nodes] waiting nodes status:" << std::endl
65 << ss.str().c_str());
66
67 if (success)
68 {
69 RCLCPP_INFO(getLogger(), "[CbWaitNav2Nodes] success event");
70 this->postSuccessEvent();
71 sub_ = nullptr;
72 }
73 else
74 {
75 RCLCPP_INFO(getLogger(), "[CbWaitNav2Nodes] still missing nodes");
76 }
77 }
78}
79
81{
82 std::stringstream ss;
83 for (auto v : waitNodes_)
84 {
85 receivedAliveMsg_[v] = false;
86 ss << "[CbWaitNav2Nodes] - " << toString(v) << ": waiting" << std::endl;
87 }
88 RCLCPP_INFO(getLogger(), ss.str().c_str());
89
90 //rclcpp::SensorDataQoS qos;
91 rclcpp::SubscriptionOptions sub_option;
92
93 sub_ = getNode()->create_subscription<bond::msg::Status>(
94 topicname_, 20, std::bind(&CbWaitNav2Nodes::onMessageReceived, this, std::placeholders::_1),
95 sub_option);
96}
97
98std::string toString(Nav2Nodes value)
99{
100 switch (value)
101 {
103 return "planner_server";
105 return "controller_server";
107 return "recoveries_server";
109 return "bt_navigator";
111 return "map_server";
112 default:
113 return "";
114 }
115}
116
117Nav2Nodes fromString(std::string id)
118{
119 if (id == "planner_server")
121 else if (id == "controller_server")
123 else if (id == "recoveries_server")
125 else if (id == "bt_navigator")
127 else if (id == "map_server")
129 else
130 return Nav2Nodes::None;
131}
132
133} // namespace cl_nav2z
std::map< Nav2Nodes, bool > receivedAliveMsg_
std::vector< Nav2Nodes > waitNodes_
void onMessageReceived(const bond::msg::Status &msg)
rclcpp::Subscription< bond::msg::Status >::SharedPtr sub_
CbWaitNav2Nodes(std::string topicname, std::vector< Nav2Nodes > waitNodes={ Nav2Nodes::PlannerServer, Nav2Nodes::ControllerServer, Nav2Nodes::BtNavigator})
virtual rclcpp::Node::SharedPtr getNode()
std::string toString(Nav2Nodes value)
Nav2Nodes fromString(std::string str)