add humble-navigation2
This commit is contained in:
X-lanni
@@ -0,0 +1,81 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(nav2_navfn_planner)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(nav2_common REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(rclcpp_action REQUIRED)
|
||||
find_package(rclcpp_lifecycle REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(visualization_msgs REQUIRED)
|
||||
find_package(nav2_util REQUIRED)
|
||||
find_package(nav2_core REQUIRED)
|
||||
find_package(nav2_msgs REQUIRED)
|
||||
find_package(nav_msgs REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(builtin_interfaces REQUIRED)
|
||||
find_package(tf2_ros REQUIRED)
|
||||
find_package(nav2_costmap_2d REQUIRED)
|
||||
find_package(pluginlib REQUIRED)
|
||||
|
||||
nav2_package()
|
||||
|
||||
include_directories(
|
||||
include
|
||||
)
|
||||
|
||||
set(library_name nav2_navfn_planner)
|
||||
|
||||
set(dependencies
|
||||
rclcpp
|
||||
rclcpp_action
|
||||
rclcpp_lifecycle
|
||||
std_msgs
|
||||
visualization_msgs
|
||||
nav2_util
|
||||
nav2_msgs
|
||||
nav_msgs
|
||||
geometry_msgs
|
||||
builtin_interfaces
|
||||
tf2_ros
|
||||
nav2_costmap_2d
|
||||
nav2_core
|
||||
pluginlib
|
||||
)
|
||||
|
||||
add_library(${library_name} SHARED
|
||||
src/navfn_planner.cpp
|
||||
src/navfn.cpp
|
||||
)
|
||||
|
||||
ament_target_dependencies(${library_name}
|
||||
${dependencies}
|
||||
)
|
||||
|
||||
pluginlib_export_plugin_description_file(nav2_core global_planner_plugin.xml)
|
||||
|
||||
install(TARGETS ${library_name}
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION bin
|
||||
)
|
||||
|
||||
install(DIRECTORY include/
|
||||
DESTINATION include/
|
||||
)
|
||||
|
||||
install(FILES global_planner_plugin.xml
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
find_package(ament_cmake_gtest REQUIRED)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
add_subdirectory(test)
|
||||
endif()
|
||||
|
||||
ament_export_include_directories(include)
|
||||
ament_export_libraries(${library_name})
|
||||
ament_export_dependencies(${dependencies})
|
||||
ament_package()
|
||||
@@ -0,0 +1,7 @@
|
||||
# Navfn Planner
|
||||
|
||||
The NavfnPlanner is a global planner plugin for the Nav2 Planner server. It implements the Navigation Function planner with either A\* or Dij. expansions. It is largely equivalent to its counterpart in ROS 1 Navigation. The Navfn planner assumes a circular robot (or a robot that can be approximated as circular for the purposes of global path planning) and operates on a weighted costmap.
|
||||
|
||||
The `global_planner` package from ROS (1) is a refactor on NavFn to make it more easily understandable, but it lacks in run-time performance and introduces suboptimal behaviors. As NavFn has been extremely stable for about 10 years at the time of porting, the maintainers felt no compelling reason to port over another, largely equivalent (but poorer functioning) planner.
|
||||
|
||||
See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-navfn.html) for additional parameter descriptions.
|
||||
@@ -0,0 +1,5 @@
|
||||
<library path="nav2_navfn_planner">
|
||||
<class name="nav2_navfn_planner/NavfnPlanner" type="nav2_navfn_planner::NavfnPlanner" base_class_type="nav2_core::GlobalPlanner">
|
||||
<description></description>
|
||||
</class>
|
||||
</library>
|
||||
@@ -0,0 +1,282 @@
|
||||
// Copyright (c) 2008, Willow Garage, Inc.
|
||||
// All rights reserved.
|
||||
//
|
||||
// Software License Agreement (BSD License 2.0)
|
||||
//
|
||||
// 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 Willow Garage 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.
|
||||
|
||||
//
|
||||
// Navigation function computation
|
||||
// Uses Dijkstra's method
|
||||
// Modified for Euclidean-distance computation
|
||||
//
|
||||
|
||||
#ifndef NAV2_NAVFN_PLANNER__NAVFN_HPP_
|
||||
#define NAV2_NAVFN_PLANNER__NAVFN_HPP_
|
||||
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
|
||||
namespace nav2_navfn_planner
|
||||
{
|
||||
|
||||
// cost defs
|
||||
#define COST_UNKNOWN_ROS 255 // 255 is unknown cost
|
||||
#define COST_OBS 254 // 254 for forbidden regions
|
||||
#define COST_OBS_ROS 253 // ROS values of 253 are obstacles
|
||||
|
||||
// navfn cost values are set to
|
||||
// COST_NEUTRAL + COST_FACTOR * costmap_cost_value.
|
||||
// Incoming costmap cost values are in the range 0 to 252.
|
||||
// With COST_NEUTRAL of 50, the COST_FACTOR needs to be about 0.8 to
|
||||
// ensure the input values are spread evenly over the output range, 50
|
||||
// to 253. If COST_FACTOR is higher, cost values will have a plateau
|
||||
// around obstacles and the planner will then treat (for example) the
|
||||
// whole width of a narrow hallway as equally undesirable and thus
|
||||
// will not plan paths down the center.
|
||||
|
||||
#define COST_NEUTRAL 50 // Set this to "open space" value
|
||||
#define COST_FACTOR 0.8 // Used for translating costs in NavFn::setCostmap()
|
||||
|
||||
// Define the cost type in the case that it is not set. However, this allows
|
||||
// clients to modify it without changing the file. Arguably, it is better to require it to
|
||||
// be defined by a user explicitly
|
||||
#ifndef COSTTYPE
|
||||
#define COSTTYPE unsigned char // Whatever is used...
|
||||
#endif
|
||||
|
||||
// potential defs
|
||||
#define POT_HIGH 1.0e10 // unassigned cell potential
|
||||
|
||||
// priority buffers
|
||||
#define PRIORITYBUFSIZE 10000
|
||||
|
||||
/**
|
||||
Navigation function call.
|
||||
\param costmap Cost map array, of type COSTTYPE; origin is upper left
|
||||
NOTE: will be modified to have a border of obstacle costs
|
||||
\param nx Width of map in cells
|
||||
\param ny Height of map in cells
|
||||
\param goal X,Y position of goal cell
|
||||
\param start X,Y position of start cell
|
||||
|
||||
Returns length of plan if found, and fills an array with x,y interpolated
|
||||
positions at about 1/2 cell resolution; else returns 0.
|
||||
*/
|
||||
int create_nav_plan_astar(
|
||||
const COSTTYPE * costmap, int nx, int ny,
|
||||
int * goal, int * start,
|
||||
float * plan, int nplan);
|
||||
|
||||
/**
|
||||
* @class NavFn
|
||||
* @brief Navigation function class. Holds buffers for costmap, navfn map. Maps are pixel-based.
|
||||
* Origin is upper left, x is right, y is down.
|
||||
*/
|
||||
class NavFn
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Constructs the planner
|
||||
* @param nx The x size of the map
|
||||
* @param ny The y size of the map
|
||||
*/
|
||||
NavFn(int nx, int ny);
|
||||
|
||||
~NavFn();
|
||||
|
||||
/**
|
||||
* @brief Sets or resets the size of the map
|
||||
* @param nx The x size of the map
|
||||
* @param ny The y size of the map
|
||||
*/
|
||||
void setNavArr(int nx, int ny);
|
||||
int nx, ny, ns; /**< size of grid, in pixels */
|
||||
|
||||
/**
|
||||
* @brief Set up the cost array for the planner, usually from ROS
|
||||
* @param cmap The costmap
|
||||
* @param isROS Whether or not the costmap is coming in in ROS format
|
||||
* @param allow_unknown Whether or not the planner should be allowed to plan through
|
||||
* unknown space
|
||||
*/
|
||||
void setCostmap(const COSTTYPE * cmap, bool isROS = true, bool allow_unknown = true);
|
||||
|
||||
/**
|
||||
* @brief Calculates a plan using the A* heuristic, returns true if one is found
|
||||
* @return True if a plan is found, false otherwise
|
||||
*/
|
||||
bool calcNavFnAstar();
|
||||
|
||||
/**
|
||||
* @brief Caclulates the full navigation function using Dijkstra
|
||||
*/
|
||||
bool calcNavFnDijkstra(bool atStart = false);
|
||||
|
||||
/**
|
||||
* @brief Accessor for the x-coordinates of a path
|
||||
* @return The x-coordinates of a path
|
||||
*/
|
||||
float * getPathX();
|
||||
|
||||
/**
|
||||
* @brief Accessor for the y-coordinates of a path
|
||||
* @return The y-coordinates of a path
|
||||
*/
|
||||
float * getPathY();
|
||||
|
||||
/**
|
||||
* @brief Accessor for the length of a path
|
||||
* @return The length of a path, 0 if not found
|
||||
*/
|
||||
int getPathLen();
|
||||
|
||||
/**
|
||||
* @brief Gets the cost of the path found the last time a navigation function was computed
|
||||
* @return The cost of the last path found
|
||||
*/
|
||||
float getLastPathCost();
|
||||
|
||||
/** cell arrays */
|
||||
COSTTYPE * costarr; /**< cost array in 2D configuration space */
|
||||
float * potarr; /**< potential array, navigation function potential */
|
||||
bool * pending; /**< pending cells during propagation */
|
||||
int nobs; /**< number of obstacle cells */
|
||||
|
||||
/** block priority buffers */
|
||||
int * pb1, * pb2, * pb3; /**< storage buffers for priority blocks */
|
||||
int * curP, * nextP, * overP; /**< priority buffer block ptrs */
|
||||
int curPe, nextPe, overPe; /**< end points of arrays */
|
||||
|
||||
/** block priority thresholds */
|
||||
float curT; /**< current threshold */
|
||||
float priInc; /**< priority threshold increment */
|
||||
|
||||
/** goal and start positions */
|
||||
/**
|
||||
* @brief Sets the goal position for the planner.
|
||||
* Note: the navigation cost field computed gives the cost to get to a given point
|
||||
* from the goal, not from the start.
|
||||
* @param goal the goal position
|
||||
*/
|
||||
void setGoal(int * goal);
|
||||
|
||||
/**
|
||||
* @brief Sets the start position for the planner.
|
||||
* Note: the navigation cost field computed gives the cost to get to a given point
|
||||
* from the goal, not from the start.
|
||||
* @param start the start position
|
||||
*/
|
||||
void setStart(int * start);
|
||||
|
||||
int goal[2];
|
||||
int start[2];
|
||||
/**
|
||||
* @brief Initialize cell k with cost v for propagation
|
||||
* @param k the cell to initialize
|
||||
* @param v the cost to give to the cell
|
||||
*/
|
||||
void initCost(int k, float v);
|
||||
|
||||
/** propagation */
|
||||
|
||||
/**
|
||||
* @brief Updates the cell at index n
|
||||
* @param n The index to update
|
||||
*/
|
||||
void updateCell(int n);
|
||||
|
||||
/**
|
||||
* @brief Updates the cell at index n using the A* heuristic
|
||||
* @param n The index to update
|
||||
*/
|
||||
void updateCellAstar(int n);
|
||||
|
||||
/**
|
||||
* @brief Set up navigation potential arrays for new propagation
|
||||
* @param keepit whether or not use COST_NEUTRAL
|
||||
*/
|
||||
void setupNavFn(bool keepit = false);
|
||||
|
||||
/**
|
||||
* @brief Run propagation for <cycles> iterations, or until start is reached using
|
||||
* breadth-first Dijkstra method
|
||||
* @param cycles The maximum number of iterations to run for
|
||||
* @param atStart Whether or not to stop when the start point is reached
|
||||
* @return true if the start point is reached
|
||||
*/
|
||||
bool propNavFnDijkstra(int cycles, bool atStart = false);
|
||||
|
||||
/**
|
||||
* @brief Run propagation for <cycles> iterations, or until start is reached using
|
||||
* the best-first A* method with Euclidean distance heuristic
|
||||
* @param cycles The maximum number of iterations to run for
|
||||
* @return true if the start point is reached
|
||||
*/
|
||||
bool propNavFnAstar(int cycles); /**< returns true if start point found */
|
||||
|
||||
/** gradient and paths */
|
||||
float * gradx, * grady; /**< gradient arrays, size of potential array */
|
||||
float * pathx, * pathy; /**< path points, as subpixel cell coordinates */
|
||||
int npath; /**< number of path points */
|
||||
int npathbuf; /**< size of pathx, pathy buffers */
|
||||
|
||||
float last_path_cost_; /**< Holds the cost of the path found the last time A* was called */
|
||||
|
||||
/**
|
||||
* @brief Calculates the path for at mose <n> cycles
|
||||
* @param n The maximum number of cycles to run for
|
||||
* @return The lenght of the path found, 0 if none
|
||||
*/
|
||||
int calcPath(int n, int * st = NULL);
|
||||
|
||||
/**
|
||||
* @brief Calculate gradient at a cell
|
||||
* @param n Cell number <n>
|
||||
* @return float norm
|
||||
*/
|
||||
float gradCell(int n); /**< calculates gradient at cell <n>, returns norm */
|
||||
|
||||
float pathStep; /**< step size for following gradient */
|
||||
|
||||
/** display callback */
|
||||
/**< <n> is the number of cycles between updates */
|
||||
// void display(void fn(NavFn * nav), int n = 100);
|
||||
// int displayInt; /**< save second argument of display() above */
|
||||
// void (* displayFn)(NavFn * nav); /**< display function itself */
|
||||
|
||||
/** save costmap */
|
||||
/**< write out costmap and start/goal states as fname.pgm and fname.txt */
|
||||
// void savemap(const char * fname);
|
||||
};
|
||||
|
||||
} // namespace nav2_navfn_planner
|
||||
|
||||
#endif // NAV2_NAVFN_PLANNER__NAVFN_HPP_
|
||||
@@ -0,0 +1,234 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
// Copyright (c) 2018 Simbe Robotics
|
||||
// Copyright (c) 2019 Samsung Research America
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_NAVFN_PLANNER__NAVFN_PLANNER_HPP_
|
||||
#define NAV2_NAVFN_PLANNER__NAVFN_PLANNER_HPP_
|
||||
|
||||
#include <chrono>
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
#include "geometry_msgs/msg/point.hpp"
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "nav2_core/global_planner.hpp"
|
||||
#include "nav_msgs/msg/path.hpp"
|
||||
#include "nav2_navfn_planner/navfn.hpp"
|
||||
#include "nav2_util/robot_utils.hpp"
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
|
||||
#include "nav2_util/geometry_utils.hpp"
|
||||
|
||||
namespace nav2_navfn_planner
|
||||
{
|
||||
|
||||
class NavfnPlanner : public nav2_core::GlobalPlanner
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief constructor
|
||||
*/
|
||||
NavfnPlanner();
|
||||
|
||||
/**
|
||||
* @brief destructor
|
||||
*/
|
||||
~NavfnPlanner();
|
||||
|
||||
/**
|
||||
* @brief Configuring plugin
|
||||
* @param parent Lifecycle node pointer
|
||||
* @param name Name of plugin map
|
||||
* @param tf Shared ptr of TF2 buffer
|
||||
* @param costmap_ros Costmap2DROS object
|
||||
*/
|
||||
void configure(
|
||||
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
|
||||
std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;
|
||||
|
||||
/**
|
||||
* @brief Cleanup lifecycle node
|
||||
*/
|
||||
void cleanup() override;
|
||||
|
||||
/**
|
||||
* @brief Activate lifecycle node
|
||||
*/
|
||||
void activate() override;
|
||||
|
||||
/**
|
||||
* @brief Deactivate lifecycle node
|
||||
*/
|
||||
void deactivate() override;
|
||||
|
||||
|
||||
/**
|
||||
* @brief Creating a plan from start and goal poses
|
||||
* @param start Start pose
|
||||
* @param goal Goal pose
|
||||
* @return nav_msgs::Path of the generated path
|
||||
*/
|
||||
nav_msgs::msg::Path createPlan(
|
||||
const geometry_msgs::msg::PoseStamped & start,
|
||||
const geometry_msgs::msg::PoseStamped & goal) override;
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Compute a plan given start and goal poses, provided in global world frame.
|
||||
* @param start Start pose
|
||||
* @param goal Goal pose
|
||||
* @param tolerance Relaxation constraint in x and y
|
||||
* @param plan Path to be computed
|
||||
* @return true if can find the path
|
||||
*/
|
||||
bool makePlan(
|
||||
const geometry_msgs::msg::Pose & start,
|
||||
const geometry_msgs::msg::Pose & goal, double tolerance,
|
||||
nav_msgs::msg::Path & plan);
|
||||
|
||||
/**
|
||||
* @brief Compute the navigation function given a seed point in the world to start from
|
||||
* @param world_point Point in world coordinate frame
|
||||
* @return true if can compute
|
||||
*/
|
||||
bool computePotential(const geometry_msgs::msg::Point & world_point);
|
||||
|
||||
/**
|
||||
* @brief Compute a plan to a goal from a potential - must call computePotential first
|
||||
* @param goal Goal pose
|
||||
* @param plan Path to be computed
|
||||
* @return true if can compute a plan path
|
||||
*/
|
||||
bool getPlanFromPotential(
|
||||
const geometry_msgs::msg::Pose & goal,
|
||||
nav_msgs::msg::Path & plan);
|
||||
|
||||
/**
|
||||
* @brief Remove artifacts at the end of the path - originated from planning on a discretized world
|
||||
* @param goal Goal pose
|
||||
* @param plan Computed path
|
||||
*/
|
||||
void smoothApproachToGoal(
|
||||
const geometry_msgs::msg::Pose & goal,
|
||||
nav_msgs::msg::Path & plan);
|
||||
|
||||
/**
|
||||
* @brief Compute the potential, or navigation cost, at a given point in the world
|
||||
* must call computePotential first
|
||||
* @param world_point Point in world coordinate frame
|
||||
* @return double point potential (navigation cost)
|
||||
*/
|
||||
double getPointPotential(const geometry_msgs::msg::Point & world_point);
|
||||
|
||||
// Check for a valid potential value at a given point in the world
|
||||
// - must call computePotential first
|
||||
// - currently unused
|
||||
// bool validPointPotential(const geometry_msgs::msg::Point & world_point);
|
||||
// bool validPointPotential(const geometry_msgs::msg::Point & world_point, double tolerance);
|
||||
|
||||
/**
|
||||
* @brief Compute the squared distance between two points
|
||||
* @param p1 Point 1
|
||||
* @param p2 Point 2
|
||||
* @return double squared distance between two points
|
||||
*/
|
||||
inline double squared_distance(
|
||||
const geometry_msgs::msg::Pose & p1,
|
||||
const geometry_msgs::msg::Pose & p2)
|
||||
{
|
||||
double dx = p1.position.x - p2.position.x;
|
||||
double dy = p1.position.y - p2.position.y;
|
||||
return dx * dx + dy * dy;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Transform a point from world to map frame
|
||||
* @param wx double of world X coordinate
|
||||
* @param wy double of world Y coordinate
|
||||
* @param mx int of map X coordinate
|
||||
* @param my int of map Y coordinate
|
||||
* @return true if can transform
|
||||
*/
|
||||
bool worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my);
|
||||
|
||||
/**
|
||||
* @brief Transform a point from map to world frame
|
||||
* @param mx double of map X coordinate
|
||||
* @param my double of map Y coordinate
|
||||
* @param wx double of world X coordinate
|
||||
* @param wy double of world Y coordinate
|
||||
*/
|
||||
void mapToWorld(double mx, double my, double & wx, double & wy);
|
||||
|
||||
/**
|
||||
* @brief Set the corresponding cell cost to be free space
|
||||
* @param mx int of map X coordinate
|
||||
* @param my int of map Y coordinate
|
||||
*/
|
||||
void clearRobotCell(unsigned int mx, unsigned int my);
|
||||
|
||||
/**
|
||||
* @brief Determine if a new planner object should be made
|
||||
* @return true if planner object is out of date
|
||||
*/
|
||||
bool isPlannerOutOfDate();
|
||||
|
||||
// Planner based on ROS1 NavFn algorithm
|
||||
std::unique_ptr<NavFn> planner_;
|
||||
|
||||
// TF buffer
|
||||
std::shared_ptr<tf2_ros::Buffer> tf_;
|
||||
|
||||
// Clock
|
||||
rclcpp::Clock::SharedPtr clock_;
|
||||
|
||||
// Logger
|
||||
rclcpp::Logger logger_{rclcpp::get_logger("NavfnPlanner")};
|
||||
|
||||
// Global Costmap
|
||||
nav2_costmap_2d::Costmap2D * costmap_;
|
||||
|
||||
// The global frame of the costmap
|
||||
std::string global_frame_, name_;
|
||||
|
||||
// Whether or not the planner should be allowed to plan through unknown space
|
||||
bool allow_unknown_, use_final_approach_orientation_;
|
||||
|
||||
// If the goal is obstructed, the tolerance specifies how many meters the planner
|
||||
// can relax the constraint in x and y before failing
|
||||
double tolerance_;
|
||||
|
||||
// Whether to use the astar planner or default dijkstras
|
||||
bool use_astar_;
|
||||
|
||||
// parent node weak ptr
|
||||
rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
|
||||
|
||||
// Dynamic parameters handler
|
||||
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
|
||||
|
||||
/**
|
||||
* @brief Callback executed when a paramter change is detected
|
||||
* @param parameters list of changed parameters
|
||||
*/
|
||||
rcl_interfaces::msg::SetParametersResult
|
||||
dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
|
||||
};
|
||||
|
||||
} // namespace nav2_navfn_planner
|
||||
|
||||
#endif // NAV2_NAVFN_PLANNER__NAVFN_PLANNER_HPP_
|
||||
@@ -0,0 +1,37 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>nav2_navfn_planner</name>
|
||||
<version>1.1.18</version>
|
||||
<description>TODO</description>
|
||||
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
|
||||
<maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
<license>BSD-3-Clause</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclcpp_action</depend>
|
||||
<depend>rclcpp_lifecycle</depend>
|
||||
<depend>visualization_msgs</depend>
|
||||
<depend>nav2_util</depend>
|
||||
<depend>nav2_msgs</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>builtin_interfaces</depend>
|
||||
<depend>nav2_common</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
<depend>nav2_costmap_2d</depend>
|
||||
<depend>nav2_core</depend>
|
||||
<depend>pluginlib</depend>
|
||||
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_cmake_gtest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
<nav2_core plugin="${prefix}/global_planner_plugin.xml" />
|
||||
</export>
|
||||
</package>
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,553 @@
|
||||
// Copyright (c) 2018 Intel Corporation
|
||||
// Copyright (c) 2018 Simbe Robotics
|
||||
// Copyright (c) 2019 Samsung Research America
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
// Navigation Strategy based on:
|
||||
// Brock, O. and Oussama K. (1999). High-Speed Navigation Using
|
||||
// the Global Dynamic Window Approach. IEEE.
|
||||
// https://cs.stanford.edu/group/manips/publications/pdfs/Brock_1999_ICRA.pdf
|
||||
|
||||
// #define BENCHMARK_TESTING
|
||||
|
||||
#include "nav2_navfn_planner/navfn_planner.hpp"
|
||||
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <iomanip>
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "builtin_interfaces/msg/duration.hpp"
|
||||
#include "nav2_navfn_planner/navfn.hpp"
|
||||
#include "nav2_util/costmap.hpp"
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
#include "nav2_costmap_2d/cost_values.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
using namespace std::chrono; // NOLINT
|
||||
using nav2_util::declare_parameter_if_not_declared;
|
||||
using rcl_interfaces::msg::ParameterType;
|
||||
using std::placeholders::_1;
|
||||
|
||||
namespace nav2_navfn_planner
|
||||
{
|
||||
|
||||
NavfnPlanner::NavfnPlanner()
|
||||
: tf_(nullptr), costmap_(nullptr)
|
||||
{
|
||||
}
|
||||
|
||||
NavfnPlanner::~NavfnPlanner()
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
logger_, "Destroying plugin %s of type NavfnPlanner",
|
||||
name_.c_str());
|
||||
}
|
||||
|
||||
void
|
||||
NavfnPlanner::configure(
|
||||
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
|
||||
std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
|
||||
{
|
||||
tf_ = tf;
|
||||
name_ = name;
|
||||
costmap_ = costmap_ros->getCostmap();
|
||||
global_frame_ = costmap_ros->getGlobalFrameID();
|
||||
|
||||
node_ = parent;
|
||||
auto node = parent.lock();
|
||||
clock_ = node->get_clock();
|
||||
logger_ = node->get_logger();
|
||||
|
||||
RCLCPP_INFO(
|
||||
logger_, "Configuring plugin %s of type NavfnPlanner",
|
||||
name_.c_str());
|
||||
|
||||
// Initialize parameters
|
||||
// Declare this plugin's parameters
|
||||
declare_parameter_if_not_declared(node, name + ".tolerance", rclcpp::ParameterValue(0.5));
|
||||
node->get_parameter(name + ".tolerance", tolerance_);
|
||||
declare_parameter_if_not_declared(node, name + ".use_astar", rclcpp::ParameterValue(false));
|
||||
node->get_parameter(name + ".use_astar", use_astar_);
|
||||
declare_parameter_if_not_declared(node, name + ".allow_unknown", rclcpp::ParameterValue(true));
|
||||
node->get_parameter(name + ".allow_unknown", allow_unknown_);
|
||||
declare_parameter_if_not_declared(
|
||||
node, name + ".use_final_approach_orientation", rclcpp::ParameterValue(false));
|
||||
node->get_parameter(name + ".use_final_approach_orientation", use_final_approach_orientation_);
|
||||
|
||||
// Create a planner based on the new costmap size
|
||||
planner_ = std::make_unique<NavFn>(
|
||||
costmap_->getSizeInCellsX(),
|
||||
costmap_->getSizeInCellsY());
|
||||
}
|
||||
|
||||
void
|
||||
NavfnPlanner::activate()
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
logger_, "Activating plugin %s of type NavfnPlanner",
|
||||
name_.c_str());
|
||||
// Add callback for dynamic parameters
|
||||
auto node = node_.lock();
|
||||
dyn_params_handler_ = node->add_on_set_parameters_callback(
|
||||
std::bind(&NavfnPlanner::dynamicParametersCallback, this, _1));
|
||||
}
|
||||
|
||||
void
|
||||
NavfnPlanner::deactivate()
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
logger_, "Deactivating plugin %s of type NavfnPlanner",
|
||||
name_.c_str());
|
||||
dyn_params_handler_.reset();
|
||||
}
|
||||
|
||||
void
|
||||
NavfnPlanner::cleanup()
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
logger_, "Cleaning up plugin %s of type NavfnPlanner",
|
||||
name_.c_str());
|
||||
planner_.reset();
|
||||
}
|
||||
|
||||
nav_msgs::msg::Path NavfnPlanner::createPlan(
|
||||
const geometry_msgs::msg::PoseStamped & start,
|
||||
const geometry_msgs::msg::PoseStamped & goal)
|
||||
{
|
||||
#ifdef BENCHMARK_TESTING
|
||||
steady_clock::time_point a = steady_clock::now();
|
||||
#endif
|
||||
|
||||
// Update planner based on the new costmap size
|
||||
if (isPlannerOutOfDate()) {
|
||||
planner_->setNavArr(
|
||||
costmap_->getSizeInCellsX(),
|
||||
costmap_->getSizeInCellsY());
|
||||
}
|
||||
|
||||
nav_msgs::msg::Path path;
|
||||
|
||||
// Corner case of the start(x,y) = goal(x,y)
|
||||
if (start.pose.position.x == goal.pose.position.x &&
|
||||
start.pose.position.y == goal.pose.position.y)
|
||||
{
|
||||
unsigned int mx, my;
|
||||
costmap_->worldToMap(start.pose.position.x, start.pose.position.y, mx, my);
|
||||
if (costmap_->getCost(mx, my) == nav2_costmap_2d::LETHAL_OBSTACLE) {
|
||||
RCLCPP_WARN(logger_, "Failed to create a unique pose path because of obstacles");
|
||||
return path;
|
||||
}
|
||||
path.header.stamp = clock_->now();
|
||||
path.header.frame_id = global_frame_;
|
||||
geometry_msgs::msg::PoseStamped pose;
|
||||
pose.header = path.header;
|
||||
pose.pose.position.z = 0.0;
|
||||
|
||||
pose.pose = start.pose;
|
||||
// if we have a different start and goal orientation, set the unique path pose to the goal
|
||||
// orientation, unless use_final_approach_orientation=true where we need it to be the start
|
||||
// orientation to avoid movement from the local planner
|
||||
if (start.pose.orientation != goal.pose.orientation && !use_final_approach_orientation_) {
|
||||
pose.pose.orientation = goal.pose.orientation;
|
||||
}
|
||||
path.poses.push_back(pose);
|
||||
return path;
|
||||
}
|
||||
|
||||
if (!makePlan(start.pose, goal.pose, tolerance_, path)) {
|
||||
RCLCPP_WARN(
|
||||
logger_, "%s: failed to create plan with "
|
||||
"tolerance %.2f.", name_.c_str(), tolerance_);
|
||||
}
|
||||
|
||||
|
||||
#ifdef BENCHMARK_TESTING
|
||||
steady_clock::time_point b = steady_clock::now();
|
||||
duration<double> time_span = duration_cast<duration<double>>(b - a);
|
||||
std::cout << "It took " << time_span.count() * 1000 << std::endl;
|
||||
#endif
|
||||
|
||||
return path;
|
||||
}
|
||||
|
||||
bool
|
||||
NavfnPlanner::isPlannerOutOfDate()
|
||||
{
|
||||
if (!planner_.get() ||
|
||||
planner_->nx != static_cast<int>(costmap_->getSizeInCellsX()) ||
|
||||
planner_->ny != static_cast<int>(costmap_->getSizeInCellsY()))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool
|
||||
NavfnPlanner::makePlan(
|
||||
const geometry_msgs::msg::Pose & start,
|
||||
const geometry_msgs::msg::Pose & goal, double tolerance,
|
||||
nav_msgs::msg::Path & plan)
|
||||
{
|
||||
// clear the plan, just in case
|
||||
plan.poses.clear();
|
||||
|
||||
plan.header.stamp = clock_->now();
|
||||
plan.header.frame_id = global_frame_;
|
||||
|
||||
double wx = start.position.x;
|
||||
double wy = start.position.y;
|
||||
|
||||
RCLCPP_DEBUG(
|
||||
logger_, "Making plan from (%.2f,%.2f) to (%.2f,%.2f)",
|
||||
start.position.x, start.position.y, goal.position.x, goal.position.y);
|
||||
|
||||
unsigned int mx, my;
|
||||
if (!worldToMap(wx, wy, mx, my)) {
|
||||
RCLCPP_WARN(
|
||||
logger_,
|
||||
"Cannot create a plan: the robot's start position is off the global"
|
||||
" costmap. Planning will always fail, are you sure"
|
||||
" the robot has been properly localized?");
|
||||
return false;
|
||||
}
|
||||
|
||||
// clear the starting cell within the costmap because we know it can't be an obstacle
|
||||
clearRobotCell(mx, my);
|
||||
|
||||
std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
|
||||
|
||||
// make sure to resize the underlying array that Navfn uses
|
||||
planner_->setNavArr(
|
||||
costmap_->getSizeInCellsX(),
|
||||
costmap_->getSizeInCellsY());
|
||||
|
||||
planner_->setCostmap(costmap_->getCharMap(), true, allow_unknown_);
|
||||
|
||||
lock.unlock();
|
||||
|
||||
int map_start[2];
|
||||
map_start[0] = mx;
|
||||
map_start[1] = my;
|
||||
|
||||
wx = goal.position.x;
|
||||
wy = goal.position.y;
|
||||
|
||||
if (!worldToMap(wx, wy, mx, my)) {
|
||||
RCLCPP_WARN(
|
||||
logger_,
|
||||
"The goal sent to the planner is off the global costmap."
|
||||
" Planning will always fail to this goal.");
|
||||
return false;
|
||||
}
|
||||
|
||||
int map_goal[2];
|
||||
map_goal[0] = mx;
|
||||
map_goal[1] = my;
|
||||
|
||||
planner_->setStart(map_goal);
|
||||
planner_->setGoal(map_start);
|
||||
if (use_astar_) {
|
||||
planner_->calcNavFnAstar();
|
||||
} else {
|
||||
planner_->calcNavFnDijkstra(true);
|
||||
}
|
||||
|
||||
double resolution = costmap_->getResolution();
|
||||
geometry_msgs::msg::Pose p, best_pose;
|
||||
|
||||
bool found_legal = false;
|
||||
|
||||
p = goal;
|
||||
double potential = getPointPotential(p.position);
|
||||
if (potential < POT_HIGH) {
|
||||
// Goal is reachable by itself
|
||||
best_pose = p;
|
||||
found_legal = true;
|
||||
} else {
|
||||
// Goal is not reachable. Trying to find nearest to the goal
|
||||
// reachable point within its tolerance region
|
||||
double best_sdist = std::numeric_limits<double>::max();
|
||||
|
||||
p.position.y = goal.position.y - tolerance;
|
||||
while (p.position.y <= goal.position.y + tolerance) {
|
||||
p.position.x = goal.position.x - tolerance;
|
||||
while (p.position.x <= goal.position.x + tolerance) {
|
||||
potential = getPointPotential(p.position);
|
||||
double sdist = squared_distance(p, goal);
|
||||
if (potential < POT_HIGH && sdist < best_sdist) {
|
||||
best_sdist = sdist;
|
||||
best_pose = p;
|
||||
found_legal = true;
|
||||
}
|
||||
p.position.x += resolution;
|
||||
}
|
||||
p.position.y += resolution;
|
||||
}
|
||||
}
|
||||
|
||||
if (found_legal) {
|
||||
// extract the plan
|
||||
if (getPlanFromPotential(best_pose, plan)) {
|
||||
smoothApproachToGoal(best_pose, plan);
|
||||
|
||||
// If use_final_approach_orientation=true, interpolate the last pose orientation from the
|
||||
// previous pose to set the orientation to the 'final approach' orientation of the robot so
|
||||
// it does not rotate.
|
||||
// And deal with corner case of plan of length 1
|
||||
if (use_final_approach_orientation_) {
|
||||
size_t plan_size = plan.poses.size();
|
||||
if (plan_size == 1) {
|
||||
plan.poses.back().pose.orientation = start.orientation;
|
||||
} else if (plan_size > 1) {
|
||||
double dx, dy, theta;
|
||||
auto last_pose = plan.poses.back().pose.position;
|
||||
auto approach_pose = plan.poses[plan_size - 2].pose.position;
|
||||
// Deal with the case of NavFn producing a path with two equal last poses
|
||||
if (std::abs(last_pose.x - approach_pose.x) < 0.0001 &&
|
||||
std::abs(last_pose.y - approach_pose.y) < 0.0001 && plan_size > 2)
|
||||
{
|
||||
approach_pose = plan.poses[plan_size - 3].pose.position;
|
||||
}
|
||||
dx = last_pose.x - approach_pose.x;
|
||||
dy = last_pose.y - approach_pose.y;
|
||||
theta = atan2(dy, dx);
|
||||
plan.poses.back().pose.orientation =
|
||||
nav2_util::geometry_utils::orientationAroundZAxis(theta);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
RCLCPP_ERROR(
|
||||
logger_,
|
||||
"Failed to create a plan from potential when a legal"
|
||||
" potential was found. This shouldn't happen.");
|
||||
}
|
||||
}
|
||||
|
||||
return !plan.poses.empty();
|
||||
}
|
||||
|
||||
void
|
||||
NavfnPlanner::smoothApproachToGoal(
|
||||
const geometry_msgs::msg::Pose & goal,
|
||||
nav_msgs::msg::Path & plan)
|
||||
{
|
||||
// Replace the last pose of the computed path if it's actually further away
|
||||
// to the second to last pose than the goal pose.
|
||||
if (plan.poses.size() >= 2) {
|
||||
auto second_to_last_pose = plan.poses.end()[-2];
|
||||
auto last_pose = plan.poses.back();
|
||||
if (
|
||||
squared_distance(last_pose.pose, second_to_last_pose.pose) >
|
||||
squared_distance(goal, second_to_last_pose.pose))
|
||||
{
|
||||
plan.poses.back().pose = goal;
|
||||
return;
|
||||
}
|
||||
}
|
||||
geometry_msgs::msg::PoseStamped goal_copy;
|
||||
goal_copy.pose = goal;
|
||||
goal_copy.header = plan.header;
|
||||
plan.poses.push_back(goal_copy);
|
||||
}
|
||||
|
||||
bool
|
||||
NavfnPlanner::getPlanFromPotential(
|
||||
const geometry_msgs::msg::Pose & goal,
|
||||
nav_msgs::msg::Path & plan)
|
||||
{
|
||||
// clear the plan, just in case
|
||||
plan.poses.clear();
|
||||
|
||||
// Goal should be in global frame
|
||||
double wx = goal.position.x;
|
||||
double wy = goal.position.y;
|
||||
|
||||
// the potential has already been computed, so we won't update our copy of the costmap
|
||||
unsigned int mx, my;
|
||||
if (!worldToMap(wx, wy, mx, my)) {
|
||||
RCLCPP_WARN(
|
||||
logger_,
|
||||
"The goal sent to the navfn planner is off the global costmap."
|
||||
" Planning will always fail to this goal.");
|
||||
return false;
|
||||
}
|
||||
|
||||
int map_goal[2];
|
||||
map_goal[0] = mx;
|
||||
map_goal[1] = my;
|
||||
|
||||
planner_->setStart(map_goal);
|
||||
|
||||
const int & max_cycles = (costmap_->getSizeInCellsX() >= costmap_->getSizeInCellsY()) ?
|
||||
(costmap_->getSizeInCellsX() * 4) : (costmap_->getSizeInCellsY() * 4);
|
||||
|
||||
int path_len = planner_->calcPath(max_cycles);
|
||||
if (path_len == 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
auto cost = planner_->getLastPathCost();
|
||||
RCLCPP_DEBUG(
|
||||
logger_,
|
||||
"Path found, %d steps, %f cost\n", path_len, cost);
|
||||
|
||||
// extract the plan
|
||||
float * x = planner_->getPathX();
|
||||
float * y = planner_->getPathY();
|
||||
int len = planner_->getPathLen();
|
||||
|
||||
for (int i = len - 1; i >= 0; --i) {
|
||||
// convert the plan to world coordinates
|
||||
double world_x, world_y;
|
||||
mapToWorld(x[i], y[i], world_x, world_y);
|
||||
|
||||
geometry_msgs::msg::PoseStamped pose;
|
||||
pose.header = plan.header;
|
||||
pose.pose.position.x = world_x;
|
||||
pose.pose.position.y = world_y;
|
||||
pose.pose.position.z = 0.0;
|
||||
pose.pose.orientation.x = 0.0;
|
||||
pose.pose.orientation.y = 0.0;
|
||||
pose.pose.orientation.z = 0.0;
|
||||
pose.pose.orientation.w = 1.0;
|
||||
plan.poses.push_back(pose);
|
||||
}
|
||||
|
||||
return !plan.poses.empty();
|
||||
}
|
||||
|
||||
double
|
||||
NavfnPlanner::getPointPotential(const geometry_msgs::msg::Point & world_point)
|
||||
{
|
||||
unsigned int mx, my;
|
||||
if (!worldToMap(world_point.x, world_point.y, mx, my)) {
|
||||
return std::numeric_limits<double>::max();
|
||||
}
|
||||
|
||||
unsigned int index = my * planner_->nx + mx;
|
||||
return planner_->potarr[index];
|
||||
}
|
||||
|
||||
// bool
|
||||
// NavfnPlanner::validPointPotential(const geometry_msgs::msg::Point & world_point)
|
||||
// {
|
||||
// return validPointPotential(world_point, tolerance_);
|
||||
// }
|
||||
|
||||
// bool
|
||||
// NavfnPlanner::validPointPotential(
|
||||
// const geometry_msgs::msg::Point & world_point, double tolerance)
|
||||
// {
|
||||
// const double resolution = costmap_->getResolution();
|
||||
|
||||
// geometry_msgs::msg::Point p = world_point;
|
||||
// double potential = getPointPotential(p);
|
||||
// if (potential < POT_HIGH) {
|
||||
// // world_point is reachable by itself
|
||||
// return true;
|
||||
// } else {
|
||||
// // world_point, is not reachable. Trying to find any
|
||||
// // reachable point within its tolerance region
|
||||
// p.y = world_point.y - tolerance;
|
||||
// while (p.y <= world_point.y + tolerance) {
|
||||
// p.x = world_point.x - tolerance;
|
||||
// while (p.x <= world_point.x + tolerance) {
|
||||
// potential = getPointPotential(p);
|
||||
// if (potential < POT_HIGH) {
|
||||
// return true;
|
||||
// }
|
||||
// p.x += resolution;
|
||||
// }
|
||||
// p.y += resolution;
|
||||
// }
|
||||
// }
|
||||
|
||||
// return false;
|
||||
// }
|
||||
|
||||
bool
|
||||
NavfnPlanner::worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my)
|
||||
{
|
||||
if (wx < costmap_->getOriginX() || wy < costmap_->getOriginY()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
mx = static_cast<int>(
|
||||
std::round((wx - costmap_->getOriginX()) / costmap_->getResolution()));
|
||||
my = static_cast<int>(
|
||||
std::round((wy - costmap_->getOriginY()) / costmap_->getResolution()));
|
||||
|
||||
if (mx < costmap_->getSizeInCellsX() && my < costmap_->getSizeInCellsY()) {
|
||||
return true;
|
||||
}
|
||||
|
||||
RCLCPP_ERROR(
|
||||
logger_,
|
||||
"worldToMap failed: mx,my: %d,%d, size_x,size_y: %d,%d", mx, my,
|
||||
costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY());
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void
|
||||
NavfnPlanner::mapToWorld(double mx, double my, double & wx, double & wy)
|
||||
{
|
||||
wx = costmap_->getOriginX() + mx * costmap_->getResolution();
|
||||
wy = costmap_->getOriginY() + my * costmap_->getResolution();
|
||||
}
|
||||
|
||||
void
|
||||
NavfnPlanner::clearRobotCell(unsigned int mx, unsigned int my)
|
||||
{
|
||||
// TODO(orduno): check usage of this function, might instead be a request to
|
||||
// world_model / map server
|
||||
costmap_->setCost(mx, my, nav2_costmap_2d::FREE_SPACE);
|
||||
}
|
||||
|
||||
rcl_interfaces::msg::SetParametersResult
|
||||
NavfnPlanner::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
|
||||
{
|
||||
rcl_interfaces::msg::SetParametersResult result;
|
||||
for (auto parameter : parameters) {
|
||||
const auto & type = parameter.get_type();
|
||||
const auto & name = parameter.get_name();
|
||||
|
||||
if (type == ParameterType::PARAMETER_DOUBLE) {
|
||||
if (name == name_ + ".tolerance") {
|
||||
tolerance_ = parameter.as_double();
|
||||
}
|
||||
} else if (type == ParameterType::PARAMETER_BOOL) {
|
||||
if (name == name_ + ".use_astar") {
|
||||
use_astar_ = parameter.as_bool();
|
||||
} else if (name == name_ + ".allow_unknown") {
|
||||
allow_unknown_ = parameter.as_bool();
|
||||
} else if (name == name_ + ".use_final_approach_orientation") {
|
||||
use_final_approach_orientation_ = parameter.as_bool();
|
||||
}
|
||||
}
|
||||
}
|
||||
result.successful = true;
|
||||
return result;
|
||||
}
|
||||
|
||||
} // namespace nav2_navfn_planner
|
||||
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
PLUGINLIB_EXPORT_CLASS(nav2_navfn_planner::NavfnPlanner, nav2_core::GlobalPlanner)
|
||||
@@ -0,0 +1,10 @@
|
||||
# Test dynamic parameters
|
||||
ament_add_gtest(test_dynamic_parameters
|
||||
test_dynamic_parameters.cpp
|
||||
)
|
||||
ament_target_dependencies(test_dynamic_parameters
|
||||
${dependencies}
|
||||
)
|
||||
target_link_libraries(test_dynamic_parameters
|
||||
${library_name}
|
||||
)
|
||||
@@ -0,0 +1,64 @@
|
||||
// Copyright (c) 2021, Samsung Research America
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License. Reserved.
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
#include "nav2_navfn_planner/navfn_planner.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
class RclCppFixture
|
||||
{
|
||||
public:
|
||||
RclCppFixture() {rclcpp::init(0, nullptr);}
|
||||
~RclCppFixture() {rclcpp::shutdown();}
|
||||
};
|
||||
RclCppFixture g_rclcppfixture;
|
||||
|
||||
TEST(NavfnTest, testDynamicParameter)
|
||||
{
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("Navfntest");
|
||||
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("global_costmap");
|
||||
costmap->on_configure(rclcpp_lifecycle::State());
|
||||
auto planner =
|
||||
std::make_unique<nav2_navfn_planner::NavfnPlanner>();
|
||||
auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
|
||||
planner->configure(node, "test", tf, costmap);
|
||||
planner->activate();
|
||||
|
||||
auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
|
||||
node->get_node_base_interface(), node->get_node_topics_interface(),
|
||||
node->get_node_graph_interface(),
|
||||
node->get_node_services_interface());
|
||||
|
||||
auto results = rec_param->set_parameters_atomically(
|
||||
{rclcpp::Parameter("test.tolerance", 1.0),
|
||||
rclcpp::Parameter("test.use_astar", true),
|
||||
rclcpp::Parameter("test.allow_unknown", true),
|
||||
rclcpp::Parameter("test.use_final_approach_orientation", true)});
|
||||
|
||||
rclcpp::spin_until_future_complete(
|
||||
node->get_node_base_interface(),
|
||||
results);
|
||||
|
||||
EXPECT_EQ(node->get_parameter("test.tolerance").as_double(), 1.0);
|
||||
EXPECT_EQ(node->get_parameter("test.use_astar").as_bool(), true);
|
||||
EXPECT_EQ(node->get_parameter("test.allow_unknown").as_bool(), true);
|
||||
EXPECT_EQ(node->get_parameter("test.use_final_approach_orientation").as_bool(), true);
|
||||
}
|
||||
Reference in New Issue
Block a user