|
| 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__SIMPLE_NON_CHARGING_DOCK_HPP_ |
| 16 | +#define OPENNAV_DOCKING__SIMPLE_NON_CHARGING_DOCK_HPP_ |
| 17 | + |
| 18 | +#include <string> |
| 19 | +#include <memory> |
| 20 | +#include <vector> |
| 21 | + |
| 22 | +#include "geometry_msgs/msg/pose_stamped.hpp" |
| 23 | +#include "sensor_msgs/msg/battery_state.hpp" |
| 24 | +#include "sensor_msgs/msg/joint_state.hpp" |
| 25 | +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" |
| 26 | +#include "tf2/utils.h" |
| 27 | + |
| 28 | +#include "opennav_docking_core/non_charging_dock.hpp" |
| 29 | +#include "opennav_docking/pose_filter.hpp" |
| 30 | + |
| 31 | +namespace opennav_docking |
| 32 | +{ |
| 33 | + |
| 34 | +class SimpleNonChargingDock : public opennav_docking_core::NonChargingDock |
| 35 | +{ |
| 36 | +public: |
| 37 | + /** |
| 38 | + * @brief Constructor |
| 39 | + */ |
| 40 | + SimpleNonChargingDock() |
| 41 | + : NonChargingDock() |
| 42 | + {} |
| 43 | + |
| 44 | + /** |
| 45 | + * @param parent pointer to user's node |
| 46 | + * @param name The name of this planner |
| 47 | + * @param tf A pointer to a TF buffer |
| 48 | + */ |
| 49 | + virtual void configure( |
| 50 | + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, |
| 51 | + const std::string & name, std::shared_ptr<tf2_ros::Buffer> tf); |
| 52 | + |
| 53 | + /** |
| 54 | + * @brief Method to cleanup resources used on shutdown. |
| 55 | + */ |
| 56 | + virtual void cleanup() {} |
| 57 | + |
| 58 | + /** |
| 59 | + * @brief Method to active Behavior and any threads involved in execution. |
| 60 | + */ |
| 61 | + virtual void activate() {} |
| 62 | + |
| 63 | + /** |
| 64 | + * @brief Method to deactive Behavior and any threads involved in execution. |
| 65 | + */ |
| 66 | + virtual void deactivate() {} |
| 67 | + |
| 68 | + /** |
| 69 | + * @brief Method to obtain the dock's staging pose. This method should likely |
| 70 | + * be using TF and the dock's pose information to find the staging pose from |
| 71 | + * a static or parameterized staging pose relative to the docking pose |
| 72 | + * @param pose Dock with pose |
| 73 | + * @param frame Dock's frame of pose |
| 74 | + * @return PoseStamped of staging pose in the specified frame |
| 75 | + */ |
| 76 | + virtual geometry_msgs::msg::PoseStamped getStagingPose( |
| 77 | + const geometry_msgs::msg::Pose & pose, const std::string & frame); |
| 78 | + |
| 79 | + /** |
| 80 | + * @brief Method to obtain the refined pose of the dock, usually based on sensors |
| 81 | + * @param pose The initial estimate of the dock pose. |
| 82 | + * @param frame The frame of the initial estimate. |
| 83 | + */ |
| 84 | + virtual bool getRefinedPose(geometry_msgs::msg::PoseStamped & pose, std::string id); |
| 85 | + |
| 86 | + /** |
| 87 | + * @copydoc opennav_docking_core::ChargingDock::isDocked |
| 88 | + */ |
| 89 | + virtual bool isDocked(); |
| 90 | + |
| 91 | +protected: |
| 92 | + void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr state); |
| 93 | + |
| 94 | + // Optionally subscribe to a detected dock pose topic |
| 95 | + rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr dock_pose_sub_; |
| 96 | + rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr dock_pose_pub_; |
| 97 | + rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr filtered_dock_pose_pub_; |
| 98 | + rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr staging_pose_pub_; |
| 99 | + // If subscribed to a detected pose topic, will contain latest message |
| 100 | + geometry_msgs::msg::PoseStamped detected_dock_pose_; |
| 101 | + // This is the actual dock pose once it has the specified translation/rotation applied |
| 102 | + // If not subscribed to a topic, this is simply the database dock pose |
| 103 | + geometry_msgs::msg::PoseStamped dock_pose_; |
| 104 | + |
| 105 | + // Optionally subscribe to joint state message, used to determine if stalled |
| 106 | + rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_sub_; |
| 107 | + std::vector<std::string> stall_joint_names_; |
| 108 | + double stall_velocity_threshold_, stall_effort_threshold_; |
| 109 | + bool is_stalled_; |
| 110 | + |
| 111 | + // An external reference (such as image_proc::TrackMarkerNode) can be used to detect dock |
| 112 | + bool use_external_detection_pose_; |
| 113 | + double external_detection_timeout_; |
| 114 | + tf2::Quaternion external_detection_rotation_; |
| 115 | + double external_detection_translation_x_; |
| 116 | + double external_detection_translation_y_; |
| 117 | + |
| 118 | + // Filtering of detected poses |
| 119 | + std::shared_ptr<PoseFilter> filter_; |
| 120 | + |
| 121 | + // If not using an external pose reference, this is the distance threshold |
| 122 | + double docking_threshold_; |
| 123 | + std::string base_frame_id_; |
| 124 | + // Offset for staging pose relative to dock pose |
| 125 | + double staging_x_offset_; |
| 126 | + double staging_yaw_offset_; |
| 127 | + |
| 128 | + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; |
| 129 | + std::shared_ptr<tf2_ros::Buffer> tf2_buffer_; |
| 130 | +}; |
| 131 | + |
| 132 | +} // namespace opennav_docking |
| 133 | + |
| 134 | +#endif // OPENNAV_DOCKING__SIMPLE_NON_CHARGING_DOCK_HPP_ |
0 commit comments