feat: migrate explore.h

explore/include/explore/explore.h

@@ -0,0 +1,128 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Robert Bosch LLC.
+ *  Copyright (c) 2015-2016, Jiri Horner.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Jiri Horner nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *********************************************************************/
+#ifndef NAV_EXPLORE_H_
+#define NAV_EXPLORE_H_
+
+#include <explore/costmap_client.h>
+#include <explore/frontier_search.h>
+#include <geometry_msgs/msg/pose_stamped.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <chrono>
+#include <cmath>
+#include <geometry_msgs/msg/point.hpp>
+#include <memory>
+#include <mutex>
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/color_rgba.hpp>
+#include <string>
+#include <vector>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include "nav2_msgs/action/navigate_to_pose.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+
+using namespace std::placeholders;
+
+namespace explore
+{
+/**
+ * @class Explore
+ * @brief A class adhering to the robot_actions::Action interface that moves the
+ * robot base to explore its environment.
+ */
+class Explore : public rclcpp::Node
+{
+public:
+  Explore();
+  ~Explore();
+
+  void start();
+  void stop();
+
+  using NavigationGoalHandle =
+      rclcpp_action::ClientGoalHandle<nav2_msgs::action::NavigateToPose>;
+
+private:
+  /**
+   * @brief  Make a global plan
+   */
+  void makePlan();
+
+  // /**
+  //  * @brief  Publish a frontiers as markers
+  //  */
+  void visualizeFrontiers(
+      const std::vector<frontier_exploration::Frontier>& frontiers);
+
+  bool goalOnBlacklist(const geometry_msgs::msg::Point& goal);
+
+  NavigationGoalHandle::SharedPtr navigation_goal_handle_;
+  // void
+  // goal_response_callback(std::shared_future<NavigationGoalHandle::SharedPtr>
+  // future);
+  void reachedGoal(const NavigationGoalHandle::WrappedResult& result,
+                   const geometry_msgs::msg::Point& frontier_goal);
+
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr
+      marker_array_publisher_;
+  rclcpp::Logger logger_ = rclcpp::get_logger("ExploreNode");
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+
+  Costmap2DClient costmap_client_;
+  rclcpp_action::Client<nav2_msgs::action::NavigateToPose>::SharedPtr
+      move_base_client_;
+  frontier_exploration::FrontierSearch search_;
+  rclcpp::TimerBase::SharedPtr exploring_timer_;
+  // rclcpp::TimerBase::SharedPtr oneshot_;
+
+  std::vector<geometry_msgs::msg::Point> frontier_blacklist_;
+  geometry_msgs::msg::Point prev_goal_;
+  double prev_distance_;
+  rclcpp::Time last_progress_;
+  size_t last_markers_count_;
+
+  // parameters
+  double planner_frequency_;
+  double potential_scale_, orientation_scale_, gain_scale_;
+  double progress_timeout_;
+  bool visualize_;
+};
+}  // namespace explore
+
+#endif