Nav2 Navigation Stack - rolling  main
ROS 2 Navigation Stack
utils.hpp
1 // Copyright (c) 2024 Open Navigation LLC
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 #ifndef OPENNAV_DOCKING__UTILS_HPP_
16 #define OPENNAV_DOCKING__UTILS_HPP_
17 
18 #include <string>
19 #include <vector>
20 
21 #include "yaml-cpp/yaml.h"
22 #include "nav2_ros_common/lifecycle_node.hpp"
23 #include "rclcpp/logging.hpp"
24 #include "rclcpp/parameter_value.hpp"
25 #include "rclcpp/time.hpp"
26 #include "nav2_util/geometry_utils.hpp"
27 #include "nav2_util/robot_utils.hpp"
28 #include "angles/angles.h"
29 #include "opennav_docking/types.hpp"
30 #include "opennav_docking_core/charging_dock.hpp"
31 #include "opennav_docking_core/docking_exceptions.hpp"
32 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
33 #include "tf2/utils.hpp"
34 
35 namespace utils
36 {
37 
38 using rclcpp::ParameterType::PARAMETER_STRING;
39 using rclcpp::ParameterType::PARAMETER_STRING_ARRAY;
40 using rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY;
41 using nav2_util::geometry_utils::orientationAroundZAxis;
42 using opennav_docking_core::DockDirection;
43 
50 inline bool parseDockFile(
51  const std::string & yaml_filepath,
52  const nav2::LifecycleNode::SharedPtr & node,
53  DockMap & dock_db)
54 {
55  YAML::Node yaml_file;
56  try {
57  yaml_file = YAML::LoadFile(yaml_filepath);
58  } catch (...) {
59  return false;
60  }
61 
62  if (!yaml_file["docks"]) {
63  RCLCPP_ERROR(
64  node->get_logger(),
65  "Dock database (%s) does not contain 'docks'.", yaml_filepath.c_str());
66  return false;
67  }
68 
69  auto yaml_docks = yaml_file["docks"];
70  Dock curr_dock;
71  for (const auto & yaml_dock : yaml_docks) {
72  std::string dock_name = yaml_dock.first.as<std::string>();
73  const YAML::Node & dock_attribs = yaml_dock.second;
74 
75  curr_dock.frame = "map";
76  if (dock_attribs["frame"]) {
77  curr_dock.frame = dock_attribs["frame"].as<std::string>();
78  }
79 
80  if (!dock_attribs["type"]) {
81  RCLCPP_ERROR(
82  node->get_logger(),
83  "Dock database (%s) entries do not contain 'type'.", yaml_filepath.c_str());
84  return false;
85  }
86  curr_dock.type = dock_attribs["type"].as<std::string>();
87 
88  if (!dock_attribs["pose"]) {
89  RCLCPP_ERROR(
90  node->get_logger(),
91  "Dock database (%s) entries do not contain 'pose'.", yaml_filepath.c_str());
92  return false;
93  }
94  std::vector<double> pose_arr = dock_attribs["pose"].as<std::vector<double>>();
95  if (pose_arr.size() != 3u) {
96  RCLCPP_ERROR(
97  node->get_logger(),
98  "Dock database (%s) entries do not contain pose of size 3.", yaml_filepath.c_str());
99  return false;
100  }
101  curr_dock.pose.position.x = pose_arr[0];
102  curr_dock.pose.position.y = pose_arr[1];
103  curr_dock.pose.orientation = orientationAroundZAxis(pose_arr[2]);
104 
105  if (dock_attribs["id"]) {
106  curr_dock.id = dock_attribs["id"].as<std::string>();
107  }
108 
109  // Insert into dock instance database
110  dock_db.emplace(dock_name, curr_dock);
111  }
112 
113  return true;
114 }
115 
122 inline bool parseDockParams(
123  const std::vector<std::string> & docks_param,
124  const nav2::LifecycleNode::SharedPtr & node,
125  DockMap & dock_db)
126 {
127  Dock curr_dock;
128  std::vector<double> pose_arr;
129  for (const auto & dock_name : docks_param) {
130  curr_dock.frame = node->declare_or_get_parameter(dock_name + ".frame", std::string("map"));
131 
132  try {
133  curr_dock.type = node->declare_or_get_parameter<std::string>(dock_name + ".type");
134  } catch (...) {
135  RCLCPP_ERROR(node->get_logger(), "Dock %s has no dock 'type'.", dock_name.c_str());
136  return false;
137  }
138 
139  pose_arr.clear();
140  try {
141  pose_arr = node->declare_or_get_parameter<std::vector<double>>(dock_name + ".pose");
142  if (pose_arr.size() != 3u) {
143  throw std::runtime_error("Dock pose is incorrect size!");
144  }
145  } catch (...) {
146  RCLCPP_ERROR(node->get_logger(), "Dock %s has no valid 'pose'.", dock_name.c_str());
147  return false;
148  }
149 
150  curr_dock.pose.position.x = pose_arr[0];
151  curr_dock.pose.position.y = pose_arr[1];
152  curr_dock.pose.orientation = orientationAroundZAxis(pose_arr[2]);
153 
154  curr_dock.id = node->declare_or_get_parameter(dock_name + ".id", std::string(""));
155 
156  // Insert into dock instance database
157  dock_db.emplace(dock_name, curr_dock);
158  }
159  return true;
160 }
161 
168 inline geometry_msgs::msg::PoseStamped getDockPoseStamped(
169  const Dock * dock, const rclcpp::Time & t)
170 {
171  geometry_msgs::msg::PoseStamped pose;
172  pose.pose = dock->pose;
173  pose.header.frame_id = dock->frame;
174  pose.header.stamp = t;
175  return pose;
176 }
177 
178 inline geometry_msgs::msg::PoseStamped transformPoseToFrame(
179  nav2::TransformBuffer & tf_buffer,
180  const geometry_msgs::msg::PoseStamped & pose,
181  const std::string & target_frame,
182  const rclcpp::Time & current_time,
183  const double transform_staleness_threshold)
184 {
185  auto transformed_pose = pose;
186  if (transformed_pose.header.frame_id == target_frame) {
187  return transformed_pose;
188  }
189 
190  geometry_msgs::msg::TransformStamped transform;
191  if (!nav2_util::lookupTransformWithStalenessCheck(
192  tf_buffer, target_frame, transformed_pose.header.frame_id, current_time,
193  transform_staleness_threshold, transform))
194  {
196  "Transform error: failed to get a fresh transform from " +
197  transformed_pose.header.frame_id + " to " + target_frame);
198  }
199  tf2::doTransform(transformed_pose, transformed_pose, transform);
200  return transformed_pose;
201 }
202 
203 inline double l2Norm(const geometry_msgs::msg::Pose & a, const geometry_msgs::msg::Pose & b)
204 {
205  double angle_a = tf2::getYaw(a.orientation);
206  double angle_b = tf2::getYaw(b.orientation);
207  double delta_angle = angles::shortest_angular_distance(angle_a, angle_b);
208  return sqrt(
209  (a.position.x - b.position.x) * (a.position.x - b.position.x) +
210  (a.position.y - b.position.y) * (a.position.y - b.position.y) +
211  delta_angle * delta_angle);
212 }
213 
214 inline DockDirection getDockDirectionFromString(const std::string & direction)
215 {
216  auto upper_direction = direction;
217  std::transform(
218  upper_direction.begin(), upper_direction.end(), upper_direction.begin(), ::toupper);
219 
220  if (upper_direction == "FORWARD") {
221  return DockDirection::FORWARD;
222  } else if (upper_direction == "BACKWARD") {
223  return DockDirection::BACKWARD;
224  } else {
225  return DockDirection::UNKNOWN;
226  }
227 }
228 
229 } // namespace utils
230 
231 #endif // OPENNAV_DOCKING__UTILS_HPP_
A required transform was unavailable or stale.
Definition: types.hpp:33