SMACC2
goal_checker_switcher.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 ******************************************************************************************************************/
21
22namespace cl_nav2z
23{
25 std::string goal_checker_selector_topic, std::string default_goal_checker_name)
26: goal_checker_selector_topic_(goal_checker_selector_topic),
27 default_goal_checker_name_(default_goal_checker_name)
28{
29}
30
32
34{
35 rclcpp::QoS qos(rclcpp::KeepLast(1));
36 qos.transient_local().reliable();
37
39 getNode()->create_publisher<std_msgs::msg::String>(goal_checker_selector_topic_, qos);
40}
41
43{
44 setGoalCheckerId(default_goal_checker_name_); // default id in navigation2 stack
45}
46
47void GoalCheckerSwitcher::setGoalCheckerId(std::string goalcheckerid)
48{
49 RCLCPP_INFO_STREAM(getLogger(), "[GoalCheckerSwitcher] Setting goal checker: " << goalcheckerid);
50
51 // controller_server_node_->wait_for_service();
52 // std::vector<rclcpp::Parameter> params{ rclcpp::Parameter("current_goal_checker", goalcheckerid) };
53 // auto futureResults = controller_server_node_->set_parameters(params);
54
55 std_msgs::msg::String msg;
56 msg.data = goalcheckerid;
57 this->goal_checker_selector_pub_->publish(msg);
58
59 // int i = 0;
60 // for (auto& res : futureResults.get())
61 // {
62 // RCLCPP_INFO_STREAM(getLogger(), "[GoalCheckerSwitcher] parameter result: "
63 // << params[i].get_name() << "=" << params[i].as_string()
64 // << ". Result: " << res.successful);
65 // i++;
66
67 // if (!res.successful)
68 // RCLCPP_ERROR_STREAM(this->getLogger(), "[GoalCheckerSwitcher] goal checker could not properly
69 // switch "
70 // "the goal checker of the controller_server");
71 // }
72}
73
74} // namespace cl_nav2z
GoalCheckerSwitcher(std::string goal_checker_selector_topic="goal_checker_selector", std::string default_goal_checker_name="goal_checker")
void setGoalCheckerId(std::string goal_checker_id)
rclcpp::Publisher< std_msgs::msg::String >::SharedPtr goal_checker_selector_pub_
rclcpp::Logger getLogger()
rclcpp::Node::SharedPtr getNode()