15 #ifndef OPENNAV_DOCKING__UTILS_HPP_
16 #define OPENNAV_DOCKING__UTILS_HPP_
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"
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;
50 inline bool parseDockFile(
51 const std::string & yaml_filepath,
52 const nav2::LifecycleNode::SharedPtr & node,
57 yaml_file = YAML::LoadFile(yaml_filepath);
62 if (!yaml_file[
"docks"]) {
65 "Dock database (%s) does not contain 'docks'.", yaml_filepath.c_str());
69 auto yaml_docks = yaml_file[
"docks"];
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;
75 curr_dock.frame =
"map";
76 if (dock_attribs[
"frame"]) {
77 curr_dock.frame = dock_attribs[
"frame"].as<std::string>();
80 if (!dock_attribs[
"type"]) {
83 "Dock database (%s) entries do not contain 'type'.", yaml_filepath.c_str());
86 curr_dock.type = dock_attribs[
"type"].as<std::string>();
88 if (!dock_attribs[
"pose"]) {
91 "Dock database (%s) entries do not contain 'pose'.", yaml_filepath.c_str());
94 std::vector<double> pose_arr = dock_attribs[
"pose"].as<std::vector<double>>();
95 if (pose_arr.size() != 3u) {
98 "Dock database (%s) entries do not contain pose of size 3.", yaml_filepath.c_str());
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]);
105 if (dock_attribs[
"id"]) {
106 curr_dock.id = dock_attribs[
"id"].as<std::string>();
110 dock_db.emplace(dock_name, curr_dock);
122 inline bool parseDockParams(
123 const std::vector<std::string> & docks_param,
124 const nav2::LifecycleNode::SharedPtr & node,
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"));
133 curr_dock.type = node->declare_or_get_parameter<std::string>(dock_name +
".type");
135 RCLCPP_ERROR(node->get_logger(),
"Dock %s has no dock 'type'.", dock_name.c_str());
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!");
146 RCLCPP_ERROR(node->get_logger(),
"Dock %s has no valid 'pose'.", dock_name.c_str());
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]);
154 curr_dock.id = node->declare_or_get_parameter(dock_name +
".id", std::string(
""));
157 dock_db.emplace(dock_name, curr_dock);
168 inline geometry_msgs::msg::PoseStamped getDockPoseStamped(
169 const Dock * dock,
const rclcpp::Time & t)
171 geometry_msgs::msg::PoseStamped pose;
172 pose.pose = dock->pose;
173 pose.header.frame_id = dock->frame;
174 pose.header.stamp = t;
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)
185 auto transformed_pose = pose;
186 if (transformed_pose.header.frame_id == target_frame) {
187 return transformed_pose;
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))
196 "Transform error: failed to get a fresh transform from " +
197 transformed_pose.header.frame_id +
" to " + target_frame);
199 tf2::doTransform(transformed_pose, transformed_pose, transform);
200 return transformed_pose;
203 inline double l2Norm(
const geometry_msgs::msg::Pose & a,
const geometry_msgs::msg::Pose & b)
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);
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);
214 inline DockDirection getDockDirectionFromString(
const std::string & direction)
216 auto upper_direction = direction;
218 upper_direction.begin(), upper_direction.end(), upper_direction.begin(), ::toupper);
220 if (upper_direction ==
"FORWARD") {
221 return DockDirection::FORWARD;
222 }
else if (upper_direction ==
"BACKWARD") {
223 return DockDirection::BACKWARD;
225 return DockDirection::UNKNOWN;
A required transform was unavailable or stale.