feat: migrate explore.h
Juan Galvis
parent
49666a3743
commit
9f8bcd3b5c
|
|
@ -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
|
||||||
Loading…
Reference in New Issue