HiepLM/mir_amr
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit"

Browse Source
This commit is contained in:
HiepLM
2026-05-28 10:29:58 +07:00
commit 167c52aeb6
2048 changed files with 740251 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

163
navigations/clear_costmap_recovery/CHANGELOG.rst Executable file
View File

@@ -0,0 +1,163 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package clear_costmap_recovery
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
1.17.3 (2023-01-10)
-------------------
* [ROS-O] various patches (`#1225 <https://github.com/ros-planning/navigation/issues/1225>`_)
* do not specify obsolete c++11 standard
this breaks with current versions of log4cxx.
* update pluginlib include paths
the non-hpp headers have been deprecated since kinetic
* use lambdas in favor of boost::bind
Using boost's _1 as a global system is deprecated since C++11.
The ROS packages in Debian removed the implicit support for the global symbols,
so this code fails to compile there without the patch.
* Contributors: Michael Görner
1.17.2 (2022-06-20)
-------------------
* check if the layer is derived from costmap_2d::CostmapLayer (`#1054 <https://github.com/ros-planning/navigation/issues/1054>`_)
Co-authored-by: Dima Dorezyuk <dorezyuk@magazino.eu>
* Add invert_area_to_clear to clear costmap recovery behavior (`#1030 <https://github.com/ros-planning/navigation/issues/1030>`_)
* Contributors: Dima Dorezyuk, G.Doisy
1.17.1 (2020-08-27)
-------------------
1.17.0 (2020-04-02)
-------------------
* Merge pull request `#982 <https://github.com/ros-planning/navigation/issues/982>`_ from ros-planning/noetic_prep
Noetic Migration
* increase required cmake version
* Contributors: Michael Ferguson
1.16.6 (2020-03-18)
-------------------
1.16.5 (2020-03-15)
-------------------
* [melodic] updated install for better portability. (`#973 <https://github.com/ros-planning/navigation/issues/973>`_)
* Contributors: Sean Yen
1.16.4 (2020-03-04)
-------------------
* [Windows][melodic] Navigation (except for map_server and amcl) Windows build bring up (`#851 <https://github.com/cobalt-robotics/navigation/issues/851>`_)
* Contributors: Sean Yen
1.16.3 (2019-11-15)
-------------------
* Merge pull request `#877 <https://github.com/ros-planning/navigation/issues/877>`_ from SteveMacenski/layer_clear_area-melodic
[melodic] moving clearing area method to costmap_layer so other applications can clear other types.
* Merge branch 'melodic-devel' into layer_clear_area-melodic
* Add force_updating and affected_maps parameters to tailor clear costmaps recovey (`#838 <https://github.com/ros-planning/navigation/issues/838>`_)
behavior
* clear area in layer for melodic
* Contributors: Jorge Santos Simón, Michael Ferguson, Steven Macenski, stevemacenski
1.16.2 (2018-07-31)
-------------------
1.16.1 (2018-07-28)
-------------------
1.16.0 (2018-07-25)
-------------------
* Switch to TF2 `#755 <https://github.com/ros-planning/navigation/issues/755>`_
* Contributors: Michael Ferguson, Vincent Rabaud
1.15.2 (2018-03-22)
-------------------
* Merge pull request `#673 <https://github.com/ros-planning/navigation/issues/673>`_ from ros-planning/email_update_lunar
update maintainer email (lunar)
* Merge pull request `#649 <https://github.com/ros-planning/navigation/issues/649>`_ from aaronhoy/lunar_add_ahoy
Add myself as a maintainer.
* Rebase PRs from Indigo/Kinetic (`#637 <https://github.com/ros-planning/navigation/issues/637>`_)
* Respect planner_frequency intended behavior (`#622 <https://github.com/ros-planning/navigation/issues/622>`_)
* Only do a getRobotPose when no start pose is given (`#628 <https://github.com/ros-planning/navigation/issues/628>`_)
Omit the unnecessary call to getRobotPose when the start pose was
already given, so that move_base can also generate a path in
situations where getRobotPose would fail.
This is actually to work around an issue of getRobotPose randomly
failing.
* Update gradient_path.cpp (`#576 <https://github.com/ros-planning/navigation/issues/576>`_)
* Update gradient_path.cpp
* Update navfn.cpp
* update to use non deprecated pluginlib macro (`#630 <https://github.com/ros-planning/navigation/issues/630>`_)
* update to use non deprecated pluginlib macro
* multiline version as well
* Print SDL error on IMG_Load failure in server_map (`#631 <https://github.com/ros-planning/navigation/issues/631>`_)
* Contributors: Aaron Hoy, David V. Lu!!, Michael Ferguson
1.15.1 (2017-08-14)
-------------------
1.15.0 (2017-08-07)
-------------------
* convert packages to format2
* Fix CMakeLists + package.xmls (`#548 <https://github.com/ros-planning/navigation/issues/548>`_)
* import only PCL common
* remove GCC warnings
* Contributors: Martin Günther, Mikael Arguedas, Vincent Rabaud
1.14.0 (2016-05-20)
-------------------
1.13.1 (2015-10-29)
-------------------
* proper locking during costmap update
* Contributors: Michael Ferguson
1.13.0 (2015-03-17)
-------------------
1.12.0 (2015-02-04)
-------------------
* update maintainer email
* Contributors: Michael Ferguson
1.11.15 (2015-02-03)
--------------------
* Add ARCHIVE_DESTINATION for static builds
* Contributors: Gary Servin
1.11.14 (2014-12-05)
--------------------
1.11.13 (2014-10-02)
--------------------
* Fix the build (layers library is exported by costmap_2d)
* Contributors: Michael Ferguson
1.11.12 (2014-10-01)
--------------------
* Clarify debug messages
* Initial Clearing Costmap parameter change
* Contributors: David Lu!!, Michael Ferguson
1.11.11 (2014-07-23)
--------------------
1.11.10 (2014-06-25)
--------------------
1.11.9 (2014-06-10)
-------------------
1.11.8 (2014-05-21)
-------------------
1.11.7 (2014-05-21)
-------------------
* update build to find eigen using cmake_modules
* Contributors: Michael Ferguson
1.11.5 (2014-01-30)
-------------------
* Misc. other commits from Groovy Branch
* Change maintainer from Hersh to Lu
1.11.4 (2013-09-27)
-------------------
* Package URL Updates
* Fix bounds setting

66
navigations/clear_costmap_recovery/CMakeLists.txt Executable file
View File

@@ -0,0 +1,66 @@
cmake_minimum_required(VERSION 3.0.2)
project(clear_costmap_recovery)
find_package(catkin REQUIRED
COMPONENTS
cmake_modules
costmap_2d
nav_core
pluginlib
roscpp
tf2_ros
)
find_package(Eigen3 REQUIRED)
remove_definitions(-DDISABLE_LIBUSB-1.0)
include_directories(
include
${catkin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS}
)
add_definitions(${EIGEN3_DEFINITIONS})
catkin_package(
INCLUDE_DIRS include
LIBRARIES clear_costmap_recovery
CATKIN_DEPENDS
costmap_2d
nav_core
pluginlib
roscpp
tf2_ros
)
add_library(clear_costmap_recovery src/clear_costmap_recovery.cpp)
add_dependencies(clear_costmap_recovery ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(clear_costmap_recovery ${catkin_LIBRARIES})
## Configure Tests
if(CATKIN_ENABLE_TESTING)
# Find package test dependencies
find_package(rostest REQUIRED)
# Add the test folder to the include directories
include_directories(test)
# Create targets for test executables
add_rostest_gtest(clear_tester test/clear_tests.launch test/clear_tester.cpp)
target_link_libraries(clear_tester clear_costmap_recovery ${GTEST_LIBRARIES})
endif()
install(TARGETS clear_costmap_recovery
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
)
install(FILES ccr_plugin.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
PATTERN ".svn" EXCLUDE
)

7
navigations/clear_costmap_recovery/ccr_plugin.xml Executable file
View File

@@ -0,0 +1,7 @@
<library path="lib/libclear_costmap_recovery">
<class name="clear_costmap_recovery/ClearCostmapRecovery" type="clear_costmap_recovery::ClearCostmapRecovery" base_class_type="nav_core::RecoveryBehavior">
<description>
A recovery behavior that reverts the costmap to the static map outside of a user specified window.
</description>
</class>
</library>

88
navigations/clear_costmap_recovery/include/clear_costmap_recovery/clear_costmap_recovery.h Executable file
View File

@@ -0,0 +1,88 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* 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 Willow Garage, Inc. 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.
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#ifndef CLEAR_COSTMAP_RECOVERY_H_
#define CLEAR_COSTMAP_RECOVERY_H_
#include <nav_core/recovery_behavior.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <ros/ros.h>
#include <costmap_2d/costmap_layer.h>
namespace clear_costmap_recovery{
/**
* @class ClearCostmapRecovery
* @brief A recovery behavior that reverts the navigation stack's costmaps to the static map outside of a user-specified region.
*/
class ClearCostmapRecovery : public nav_core::RecoveryBehavior {
public:
/**
* @brief Constructor, make sure to call initialize in addition to actually initialize the object
* @param
* @return
*/
ClearCostmapRecovery();
/**
* @brief Initialization function for the ClearCostmapRecovery recovery behavior
* @param tf A pointer to a transform listener
* @param global_costmap A pointer to the global_costmap used by the navigation stack
* @param local_costmap A pointer to the local_costmap used by the navigation stack
*/
void initialize(std::string name, tf2_ros::Buffer* tf,
costmap_2d::Costmap2DROS* global_costmap, costmap_2d::Costmap2DROS* local_costmap);
/**
* @brief Run the ClearCostmapRecovery recovery behavior. Reverts the
* costmap to the static map outside of a user-specified window and
* clears unknown space around the robot.
*/
void runBehavior();
private:
void clear(costmap_2d::Costmap2DROS* costmap);
void clearMap(boost::shared_ptr<costmap_2d::CostmapLayer> costmap, double pose_x, double pose_y);
costmap_2d::Costmap2DROS* global_costmap_, *local_costmap_;
std::string name_;
tf2_ros::Buffer* tf_;
bool initialized_;
bool force_updating_; ///< force costmap update after clearing, so we don't need to wait for update thread
double reset_distance_;
bool invert_area_to_clear_;
std::string affected_maps_; ///< clear only local, global or both costmaps
std::set<std::string> clearable_layers_; ///< Layer names which will be cleared.
};
};
#endif

37
navigations/clear_costmap_recovery/package.xml Executable file
View File

@@ -0,0 +1,37 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="2">
<name>clear_costmap_recovery</name>
<version>1.17.3</version>
<description>
This package provides a recovery behavior for the navigation stack that attempts to clear space by reverting the costmaps used by the navigation stack to the static map outside of a given area.
</description>
<author>Eitan Marder-Eppstein</author>
<author>contradict@gmail.com</author>
<maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
<maintainer email="mfergs7@gmail.com">Michael Ferguson</maintainer>
<maintainer email="ahoy@fetchrobotics.com">Aaron Hoy</maintainer>
<license>BSD</license>
<url>http://wiki.ros.org/clear_costmap_recovery</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>cmake_modules</build_depend>
<depend>costmap_2d</depend>
<depend>eigen</depend>
<depend>nav_core</depend>
<depend>pluginlib</depend>
<depend>roscpp</depend>
<depend>tf2_ros</depend>
<test_depend>rostest</test_depend>
<export>
<nav_core plugin="${prefix}/ccr_plugin.xml" />
</export>
</package>

204
navigations/clear_costmap_recovery/src/clear_costmap_recovery.cpp Executable file
View File

@@ -0,0 +1,204 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* 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 Willow Garage, Inc. 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.
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#include <clear_costmap_recovery/clear_costmap_recovery.h>
#include <pluginlib/class_list_macros.hpp>
#include <boost/pointer_cast.hpp>
#include <vector>
// register this planner as a RecoveryBehavior plugin
PLUGINLIB_EXPORT_CLASS(clear_costmap_recovery::ClearCostmapRecovery, nav_core::RecoveryBehavior)
using costmap_2d::NO_INFORMATION;
namespace clear_costmap_recovery
{
ClearCostmapRecovery::ClearCostmapRecovery() : global_costmap_(NULL), local_costmap_(NULL),
tf_(NULL), initialized_(false) {}
void ClearCostmapRecovery::initialize(std::string name, tf2_ros::Buffer *tf,
costmap_2d::Costmap2DROS *global_costmap, costmap_2d::Costmap2DROS *local_costmap)
{
if (!initialized_)
{
name_ = name;
tf_ = tf;
global_costmap_ = global_costmap;
local_costmap_ = local_costmap;
// get some parameters from the parameter server
ros::NodeHandle private_nh("~/" + name_);
private_nh.param("reset_distance", reset_distance_, 3.0);
private_nh.param("invert_area_to_clear", invert_area_to_clear_, false);
private_nh.param("force_updating", force_updating_, false);
private_nh.param("affected_maps", affected_maps_, std::string("both"));
if (affected_maps_ != "local" && affected_maps_ != "global" && affected_maps_ != "both")
{
ROS_WARN("Wrong value for affected_maps parameter: '%s'; valid values are 'local', 'global' or 'both'; "
"defaulting to 'both'",
affected_maps_.c_str());
affected_maps_ = "both";
}
std::vector<std::string> clearable_layers_default, clearable_layers;
clearable_layers_default.push_back(std::string("obstacles"));
private_nh.param("layer_names", clearable_layers, clearable_layers_default);
for (unsigned i = 0; i < clearable_layers.size(); i++)
{
ROS_INFO("Recovery behavior will clear layer '%s'", clearable_layers[i].c_str());
clearable_layers_.insert(clearable_layers[i]);
}
initialized_ = true;
}
else
{
ROS_ERROR("You should not call initialize twice on this object, doing nothing ");
}
}
void ClearCostmapRecovery::runBehavior()
{
if (!initialized_)
{
ROS_ERROR("This object must be initialized before runBehavior is called");
return;
}
if (global_costmap_ == NULL || local_costmap_ == NULL)
{
ROS_ERROR("The costmaps passed to the ClearCostmapRecovery object cannot be NULL. Doing nothing.");
return;
}
if (!invert_area_to_clear_)
{
ROS_WARN("%s is Clearing %s costmap%s outside a square (%.2fm) large centered on the robot.", name_.c_str(),affected_maps_.c_str(),
affected_maps_ == "both" ? "s" : "", reset_distance_);
}
else
{
ROS_WARN("%s is Clearing %s costmap%s inside a square (%.2fm) large centered on the robot.", name_.c_str(), affected_maps_.c_str(),
affected_maps_ == "both" ? "s" : "", reset_distance_);
}
ros::WallTime t0 = ros::WallTime::now();
if (affected_maps_ == "global" || affected_maps_ == "both")
{
clear(global_costmap_);
if (force_updating_)
global_costmap_->updateMap();
ROS_DEBUG("Global costmap cleared in %fs", (ros::WallTime::now() - t0).toSec());
}
t0 = ros::WallTime::now();
if (affected_maps_ == "local" || affected_maps_ == "both")
{
clear(local_costmap_);
if (force_updating_)
local_costmap_->updateMap();
ROS_DEBUG("Local costmap cleared in %fs", (ros::WallTime::now() - t0).toSec());
}
}
void ClearCostmapRecovery::clear(costmap_2d::Costmap2DROS *costmap)
{
std::vector<boost::shared_ptr<costmap_2d::Layer>> *plugins = costmap->getLayeredCostmap()->getPlugins();
geometry_msgs::PoseStamped pose;
if (!costmap->getRobotPose(pose))
{
ROS_ERROR("Cannot clear map because pose cannot be retrieved");
return;
}
double x = pose.pose.position.x;
double y = pose.pose.position.y;
for (std::vector<boost::shared_ptr<costmap_2d::Layer>>::iterator pluginp = plugins->begin(); pluginp != plugins->end(); ++pluginp)
{
boost::shared_ptr<costmap_2d::Layer> plugin = *pluginp;
std::string name = plugin->getName();
int slash = name.rfind('/');
if (slash != std::string::npos)
{
name = name.substr(slash + 1);
}
if (clearable_layers_.count(name) != 0)
{
// check if the value is convertable
if (!dynamic_cast<costmap_2d::CostmapLayer *>(plugin.get()))
{
ROS_ERROR_STREAM("Layer " << name << " is not derived from costmap_2d::CostmapLayer");
continue;
}
boost::shared_ptr<costmap_2d::CostmapLayer> costmap;
costmap = boost::static_pointer_cast<costmap_2d::CostmapLayer>(plugin);
clearMap(costmap, x, y);
}
}
}
void ClearCostmapRecovery::clearMap(boost::shared_ptr<costmap_2d::CostmapLayer> costmap,
double pose_x, double pose_y)
{
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getMutex()));
double start_point_x = pose_x - reset_distance_ / 2;
double start_point_y = pose_y - reset_distance_ / 2;
double end_point_x = start_point_x + reset_distance_;
double end_point_y = start_point_y + reset_distance_;
int start_x, start_y, end_x, end_y;
costmap->worldToMapNoBounds(start_point_x, start_point_y, start_x, start_y);
costmap->worldToMapNoBounds(end_point_x, end_point_y, end_x, end_y);
costmap->clearArea(start_x, start_y, end_x, end_y, invert_area_to_clear_);
double ox = costmap->getOriginX(), oy = costmap->getOriginY();
double width = costmap->getSizeInMetersX(), height = costmap->getSizeInMetersY();
costmap->addExtraBounds(ox, oy, ox + width, oy + height);
return;
}
};

95
navigations/clear_costmap_recovery/test/clear_tester.cpp Executable file
View File

@@ -0,0 +1,95 @@
#include <ros/ros.h>
#include <gtest/gtest.h>
#include <clear_costmap_recovery/clear_costmap_recovery.h>
#include <costmap_2d/testing_helper.h>
#include <tf2_ros/transform_listener.h>
tf2_ros::Buffer* transformer;
tf2_ros::TransformListener* tfl;
using costmap_2d::LETHAL_OBSTACLE;
void testClearBehavior(std::string name,
double distance,
bool obstacles,
bool static_map,
costmap_2d::Costmap2DROS* global_costmap,
costmap_2d::Costmap2DROS* local_costmap){
clear_costmap_recovery::ClearCostmapRecovery clear = clear_costmap_recovery::ClearCostmapRecovery();
ros::NodeHandle clr("~/" + name);
clr.setParam("reset_distance", distance);
std::vector<std::string> clearable_layers;
if(obstacles)
clearable_layers.push_back( std::string("obstacles") );
if(static_map)
clearable_layers.push_back( std::string("static_map") );
clr.setParam("layer_names", clearable_layers);
clear.initialize(name, transformer, global_costmap, local_costmap);
clear.runBehavior();
}
void testCountLethal(std::string name, double distance, bool obstacles, bool static_map, int global_lethal, int local_lethal=0)
{
costmap_2d::Costmap2DROS global(name + "/global", *transformer);
costmap_2d::Costmap2DROS local(name + "/local" , *transformer);
boost::shared_ptr<costmap_2d::ObstacleLayer> olayer;
std::vector<boost::shared_ptr<costmap_2d::Layer> >* plugins = global.getLayeredCostmap()->getPlugins();
for (std::vector<boost::shared_ptr<costmap_2d::Layer> >::iterator pluginp = plugins->begin(); pluginp != plugins->end(); ++pluginp) {
boost::shared_ptr<costmap_2d::Layer> plugin = *pluginp;
if(plugin->getName().find("obstacles")!=std::string::npos){
olayer = boost::static_pointer_cast<costmap_2d::ObstacleLayer>(plugin);
addObservation(&(*olayer), 5.0, 0.0, MAX_Z/2, 0, 0, MAX_Z/2);
addObservation(&(*olayer), 0.0, 5.0, MAX_Z/2, 0, 0, MAX_Z/2);
}
}
global.updateMap();
local.updateMap();
olayer->clearStaticObservations(true, true);
testClearBehavior("clear", distance, obstacles, static_map, &global, &local);
global.updateMap();
local.updateMap();
printMap(*global.getCostmap());
ASSERT_EQ(countValues(*global.getCostmap(), LETHAL_OBSTACLE), global_lethal);
ASSERT_EQ(countValues( *local.getCostmap(), LETHAL_OBSTACLE), local_lethal);
}
TEST(ClearTester, basicTest){
testCountLethal("base", 3.0, true, false, 20);
}
TEST(ClearTester, bigRadiusTest){
testCountLethal("base", 20.0, true, false, 22);
}
TEST(ClearTester, clearNoLayersTest){
testCountLethal("base", 20.0, false, false, 22);
}
TEST(ClearTester, clearBothTest){
testCountLethal("base", 3.0, true, true, 0);
}
TEST(ClearTester, clearBothTest2){
testCountLethal("base", 12.0, true, true, 6);
}
int main(int argc, char** argv){
ros::init(argc, argv, "clear_tests");
testing::InitGoogleTest(&argc, argv);
transformer = new tf2_ros::Buffer(ros::Duration(10));
tfl = new tf2_ros::TransformListener(*transformer);
return RUN_ALL_TESTS();
}

6
navigations/clear_costmap_recovery/test/clear_tests.launch Executable file
View File

@@ -0,0 +1,6 @@
<launch>
<node name="ms" pkg="map_server" type="map_server" args="$(find costmap_2d)/test/TenByTen.yaml"/>
<node name="static_tf" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 0 /map /base_link 100" />
<test time-limit="300" test-name="clear_tests" pkg="clear_costmap_recovery" type="clear_tester" />
<rosparam command="load" file="$(find clear_costmap_recovery)/test/params.yaml" ns="clear_tests"/>
</launch>

13
navigations/clear_costmap_recovery/test/params.yaml Executable file
View File

@@ -0,0 +1,13 @@
base:
global:
plugins:
- {name: static_map, type: "costmap_2d::StaticLayer"}
- {name: obstacles, type: "costmap_2d::ObstacleLayer"}
- {name: inflation, type: "costmap_2d::InflationLayer"}
obstacles:
track_unknown_space: true
local:
plugins: []
robot_radius: .5
clear:
clearing_distance: 7.0