SMACC2
cp_costmap_switch.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
16
17namespace cl_nav2z
18{
19std::array<std::string, 4> CostmapSwitch::layerNames = {
20 "global_costmap/obstacles_layer",
21 "local_costmap/obstacles_layer"
22 "global_costmap/inflater_layer",
23 "local_costmap/inflater_layer"};
24
26 std::string costmapName, std::string enablePropertyName)
27{
28 RCLCPP_INFO(getLogger(), "[CostmapSwitch] registering costmap type: %s", costmapName.c_str());
29 auto proxy = std::make_shared<CostmapProxy>(
30 this->moveBaseClient_->getName() + "/" + costmapName, enablePropertyName, getNode());
31 costmapProxies[costmapName] = proxy;
32}
33
35
37{
38 this->moveBaseClient_ = dynamic_cast<cl_nav2z::ClNav2Z *>(owner_);
39
40 if (this->moveBaseClient_ == nullptr)
41 {
42 RCLCPP_ERROR(getLogger(), "the owner of the CostmapSwitch must be a ClNav2Z");
43 }
44
53}
54
56{
57 return CostmapSwitch::layerNames[(int)layertype];
58}
59
60bool CostmapSwitch::exists(std::string layerName)
61{
62 if (!this->costmapProxies.count(layerName))
63 {
64 return false;
65 }
66
67 return true;
68}
69
70void CostmapSwitch::enable(std::string layerName)
71{
72 RCLCPP_INFO(getLogger(), "[CostmapSwitch] enabling %s", layerName.c_str());
73
74 if (!exists(layerName))
75 {
76 RCLCPP_ERROR(getLogger(), "[CostmapSwitch] costmap %s does not exist", layerName.c_str());
77 return;
78 }
79 else
80 {
81 RCLCPP_INFO(
82 getLogger(), "[CostmapSwitch] costmap %s found. Calling dynamic reconfigure server.",
83 layerName.c_str());
84 costmapProxies[layerName]->setCostmapEnabled(true);
85 }
86}
87
89{
90 this->enable(getStandardCostmapName(layerType));
91}
92
93void CostmapSwitch::disable(std::string layerName)
94{
95 RCLCPP_INFO(getLogger(), "[CostmapSwitch] disabling %s", layerName.c_str());
96
97 if (!exists(layerName))
98 {
99 RCLCPP_ERROR(getLogger(), "[CostmapSwitch] costmap %s does not exist", layerName.c_str());
100 return;
101 }
102 else
103 {
104 RCLCPP_INFO(
105 getLogger(), "[CostmapSwitch] costmap %s found. Calling dynamic reconfigure server.",
106 layerName.c_str());
107 costmapProxies[layerName]->setCostmapEnabled(false);
108 }
109}
110
112{
113 this->disable(getStandardCostmapName(layerType));
114}
115
116//-------------------------------------------------------------------------
117
119 std::string /*costmap_name*/, std::string /*enablePropertyName*/, rclcpp::Node::SharedPtr nh)
120: nh_(nh)
121{
122 // this->costmapName_ = costmap_name + "/set_parameters";
123 // dynamic_reconfigure::BoolParameter enableField;
124 // enableField.name = "enabled";
125 // enableField.value = true;
126
127 // enableReq.bools.push_back(enableField);
128
129 // enableField.value = false;
130 // disableReq.bools.push_back(enableField);
131 RCLCPP_ERROR(nh->get_logger(), "costmap switch not implemented %s", costmapName_.c_str());
132}
133
135{
136 // dynamic_reconfigure::ReconfigureRequest srv_req;
137 // dynamic_reconfigure::ReconfigureResponse srv_resp;
138
139 // if (value)
140 // srv_req.config = enableReq;
141 // else
142 // srv_req.config = disableReq;
143
144 // if (ros::service::exists(costmapName_, true))
145 // {
146 // RCLCPP_INFO(getLogger(),"sending dynamic reconfigure request: %s", costmapName_.c_str());
147 // ros::service::call(costmapName_, srv_req, srv_resp);
148 // }
149 // else
150 // {
151 // RCLCPP_WARN(getLogger(),"could not call dynamic reconfigure server. It does not exist: %s", costmapName_.c_str());
152 // }
153
154 RCLCPP_ERROR(nh_->get_logger(), "costmap switch not implemented %s", costmapName_.c_str());
155}
156
157// void CostmapProxy::dynreconfCallback(const dynamic_reconfigure::Config::ConstPtr &configuration_update)
158// {
159// // auto gp = std::find_if(configuration_update->strs.begin(), configuration_update->strs.begin(),
160// // [&](const dynamic_reconfigure::StrParameter &p) { return p.name == "base_global_planner" && p.value == desired_global_planner_; });
161// }
162} // namespace cl_nav2z
CostmapProxy(std::string costmap_name, std::string enablePropertyName, rclcpp::Node::SharedPtr nh)
void setCostmapEnabled(bool value)
rclcpp::Node::SharedPtr nh_
cl_nav2z::ClNav2Z * moveBaseClient_
bool exists(std::string layerName)
static std::array< std::string, 4 > layerNames
static std::string getStandardCostmapName(StandardLayers layertype)
void disable(std::string layerName)
void registerProxyFromDynamicReconfigureServer(std::string costmapName, std::string enablePropertyName="enabled")
void enable(std::string layerName)
std::map< std::string, std::shared_ptr< CostmapProxy > > costmapProxies
ISmaccClient * owner_
Definition: component.hpp:77
rclcpp::Logger getLogger()
rclcpp::Node::SharedPtr getNode()