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

41
navigations/nav_core2/CMakeLists.txt Executable file
View File

@@ -0,0 +1,41 @@
cmake_minimum_required(VERSION 3.0.2)
project(nav_core2)
set_directory_properties(PROPERTIES COMPILE_OPTIONS "-std=c++11")
find_package(catkin REQUIRED
COMPONENTS nav_2d_msgs nav_grid tf2_ros
)
catkin_package(
CATKIN_DEPENDS nav_2d_msgs nav_grid tf2_ros
INCLUDE_DIRS include
LIBRARIES basic_costmap
)
add_library(basic_costmap src/basic_costmap.cpp)
target_link_libraries(
basic_costmap ${catkin_LIBRARIES}
)
include_directories(
include ${catkin_INCLUDE_DIRS}
)
if(CATKIN_ENABLE_TESTING)
find_package(roslint REQUIRED)
roslint_cpp()
roslint_add_test()
catkin_add_gtest(bounds_test test/bounds_test.cpp)
catkin_add_gtest(exception_test test/exception_test.cpp)
endif()
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
install(
TARGETS basic_costmap
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
)

52
navigations/nav_core2/README.md Executable file
View File

@@ -0,0 +1,52 @@
# nav_core2
A replacement interface for [nav_core](https://github.com/ros-planning/navigation/tree/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/nav_core/include/nav_core) that defines basic two dimensional planner interfaces.
There were a few key reasons for creating new interfaces rather than extending the old ones.
* Use `nav_2d_msgs` to eliminate unused data fields
* Use a new `Costmap` interface as a plugin rather that forcing all implementations of the interfaces to use `costmap_2d::Costmap2DROS`.
* Provide more data in the interfaces for easier testing.
* Use Exceptions rather than booleans to provide more information about the types of errors encountered.
## `Costmap`
`costmap_2d::Costmap2DROS` has been a vital part of the navigation stack for years, but it was not without its flaws.
* Initialization required a transform be available from the global frame to the base frame, which was later used to move rolling costmaps around (and a few other things). This made doing simple testing of any planner or other `Costmap2DROS`-based behavior annoying, because transforms had to be set up, often outside of the immediate code that was being tested.
* Initialization also started an update thread, which is also not always needed in testing.
* Using `Costmap2DROS` locked users into a layered costmap based approach, which made some tasks much easier, but didn't give users the freedom to change the implementation.
The [`nav_core2::Costmap`](include/nav_core2/costmap.h) interface extends the `nav_grid::NavGrid<unsigned char>` for abstracting away the data storage and coordinate manipulation, and provides a few other key methods for costmap functioning such as
* a mutex
* a way to potentially track changes to the costmap
* a public `update` method that can be called in whatever thread you please
The `Costmap` can be loaded using `pluginlib`, allowing for arbitrary implementations of underlying update algorithms, include the layered costmap approach.
One basic implementation is provided in [`BasicCostmap`](include/nav_core2/basic_costmap.h). You can also use the `nav_core_adapter::CostmapAdapter` class which implements the `Costmap` interface while allowing you to still use `Costmap2DROS`.
Note: One could also imagine the `Costmap` interface being templatized itself like `NavGrid` instead of forcing use of `unsigned char`. While this may be possible, it was decided that it was a bit ambitious and the use of templates would force all of the implementation into headers, and would ultimately obfuscate the operation of algorithms.
## Global Planner
Let us compare the old [nav_core::BaseGlobalPlanner](https://github.com/ros-planning/navigation/blob/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/nav_core/include/nav_core/base_global_planner.h) to the new [nav_core2/GlobalPlanner](include/nav_core2/global_planner.h).
| `nav_core` | `nav_core2` | comments |
|---|--|---|
|`void initialize(std::string, costmap_2d::Costmap2DROS*)`|`void initialize(const ros::NodeHandle& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers, and provides a TFListener|
|`bool makePlan(const geometry_msgs::PoseStamped&, const geometry_msgs::PoseStamped&, std::vector<geometry_msgs::PoseStamped>&)`|`nav_2d_msgs::Path2D makePlan(const nav_2d_msgs::Pose2DStamped&, const nav_2d_msgs::Pose2DStamped&)`|Uses exceptions for errors instead of returning a bool, which frees up the return for the actual path.|
## Local Planner
Now let's compare the old [nav_core::BaseLocalPlanner](https://github.com/ros-planning/navigation/blob/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/nav_core/include/nav_core/base_local_planner.h) to the new [nav_core2/LocalPlanner](include/nav_core2/local_planner.h).
| `nav_core` | `nav_core2` | comments |
|---|--|---|
|`void initialize(std::string, tf::TransformListener*, costmap_2d::Costmap2DROS*)`|`void initialize(const ros::NodeHandle& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers|
|(no equivalent)|`void setGoalPose(const nav_2d_msgs::Pose2DStamped&)`|Explicitly set the new goal location, rather than using the last pose of the global plan`
|`bool setPlan(const std::vector<geometry_msgs::PoseStamped>&)`|`setPlan(const nav_2d_msgs::Path2D&)`||
|`bool computeVelocityCommands(geometry_msgs::Twist&)`|`nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped&, const nav_2d_msgs::Twist2D&)`|Explicitly provides the current pose and velocity for more explicit data control and easier testing. Uses exceptions for errors instead of returning a bool, which frees up the return for the actual command.|
|`bool isGoalReached()` | `bool isGoalReached(const nav_2d_msgs::Pose2DStamped&, const nav_2d_msgs::Twist2D&)` | Explicitly provide the current pose and velocity for more explicit data control and easier testing. |
## Exceptions
A hierarchical collection of [exceptions](include/nav_core2/exceptions.h) is provided to allow for reacting to navigation failures in a more robust and contextual way.
![exception hierarchy tree](doc/exceptions.png)
Each exception has a corresponding integer "result code" that can be used in ROS interfaces where passing the C++ object is infeasible. Note that due to the hierarchy, the result_code will be for the child-most exception. For example, if you throw a `StartBoundsException` which has a corresponding result code of `6`, it could also be seen as a `InvalidStartPoseException`, `GlobalPlannerException`, `PlannerException` or `NavCore2Exception`, all of which would also have the result code of `6`.
## Bounds
For use in tracking `Costmap` changes and more, this package also provides an implementation of [bounding boxes](include/nav_core2/bounds.h). These are represented with the ranges `[min_x, max_x]` and `[min_y, max_y]` (inclusive).

BIN
navigations/nav_core2/doc/exceptions.png Executable file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 38 KiB

68
navigations/nav_core2/include/nav_core2/basic_costmap.h Executable file
View File

@@ -0,0 +1,68 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, Locus Robotics
* 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 copyright holder 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 HOLDER 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_CORE2_BASIC_COSTMAP_H
#define NAV_CORE2_BASIC_COSTMAP_H
#include <nav_core2/costmap.h>
#include <string>
#include <vector>
namespace nav_core2
{
class BasicCostmap : public nav_core2::Costmap
{
public:
// Standard Costmap Interface
mutex_t* getMutex() override { return &my_mutex_; }
// NavGrid Interface
void reset() override;
void setValue(const unsigned int x, const unsigned int y, const unsigned char& value) override;
unsigned char getValue(const unsigned int x, const unsigned int y) const override;
void setInfo(const nav_grid::NavGridInfo& new_info) override
{
info_ = new_info;
reset();
}
// Index Conversion
unsigned int getIndex(const unsigned int x, const unsigned int y) const;
protected:
mutex_t my_mutex_;
std::vector<unsigned char> data_;
};
} // namespace nav_core2
#endif // NAV_CORE2_BASIC_COSTMAP_H

209
navigations/nav_core2/include/nav_core2/bounds.h Executable file
View File

@@ -0,0 +1,209 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, Locus Robotics
* 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 copyright holder 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 HOLDER 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_CORE2_BOUNDS_H
#define NAV_CORE2_BOUNDS_H
#include <algorithm>
#include <limits>
#include <string>
namespace nav_core2
{
/**
* @brief Templatized method for checking if a value falls inside a one-dimensional range
* @param value The value to check
* @param min_value The minimum value of the range
* @param max_value The maximum value of the range
* @return True if the value is within the range
*/
template <typename NumericType>
inline bool inRange(const NumericType value, const NumericType min_value, const NumericType max_value)
{
return min_value <= value && value <= max_value;
}
/**
* @class GenericBounds
* @brief Templatized class that represents a two dimensional bounds with ranges [min_x, max_x] [min_y, max_y] inclusive
*/
template <typename NumericType>
struct GenericBounds
{
public:
/**
* @brief Constructor for an empty bounds
*/
GenericBounds()
{
reset();
}
/**
* @brief Constructor for a non-empty initial bounds
* @param x0 Initial min x
* @param y0 Initial min y
* @param x1 Initial max x
* @param y1 Initial max y
*/
GenericBounds(NumericType x0, NumericType y0, NumericType x1, NumericType y1)
: min_x_(x0), min_y_(y0), max_x_(x1), max_y_(y1) {}
/**
* @brief Reset the bounds to be empty
*/
void reset()
{
min_x_ = min_y_ = std::numeric_limits<NumericType>::max();
max_x_ = max_y_ = std::numeric_limits<NumericType>::lowest(); // -max
}
/**
* @brief Update the bounds to include the point (x, y)
*/
void touch(NumericType x, NumericType y)
{
min_x_ = std::min(x, min_x_);
min_y_ = std::min(y, min_y_);
max_x_ = std::max(x, max_x_);
max_y_ = std::max(y, max_y_);
}
/**
* @brief Update the bounds to include points (x0, y0) and (x1, y1)
* @param x0 smaller of two x values
* @param y0 smaller of two y values
* @param x1 larger of two x values
* @param y1 larger of two y values
*/
void update(NumericType x0, NumericType y0, NumericType x1, NumericType y1)
{
min_x_ = std::min(x0, min_x_);
min_y_ = std::min(y0, min_y_);
max_x_ = std::max(x1, max_x_);
max_y_ = std::max(y1, max_y_);
}
/**
* @brief Update the bounds to include the entirety of another bounds object
* @param other Another bounds object
*/
void merge(const GenericBounds<NumericType>& other)
{
update(other.min_x_, other.min_y_, other.max_x_, other.max_y_);
}
/**
* @brief Returns true if the range is empty
*/
bool isEmpty() const
{
return min_x_ > max_x_ && min_y_ > max_y_;
}
/**
* @brief Returns true if the point is inside this range
*/
bool contains(NumericType x, NumericType y) const
{
return inRange(x, min_x_, max_x_) && inRange(y, min_y_, max_y_);
}
/**
* @brief returns true if the two bounds overlap each other
*/
bool overlaps(const GenericBounds<NumericType>& other) const
{
return !isEmpty() && !other.isEmpty()
&& max_y_ >= other.min_y_ && min_y_ <= other.max_y_
&& max_x_ >= other.min_x_ && min_x_ <= other.max_x_;
}
/**
* @brief comparison operator that requires all fields are equal
*/
bool operator==(const GenericBounds<NumericType>& other) const
{
return min_x_ == other.min_x_ && min_y_ == other.min_y_ &&
max_x_ == other.max_x_ && max_y_ == other.max_y_;
}
bool operator!=(const GenericBounds<NumericType>& other) const
{
return !operator==(other);
}
/**
* @brief Returns a string representation of the bounds
*/
std::string toString() const
{
if (!isEmpty())
{
return "(" + std::to_string(min_x_) + "," + std::to_string(min_y_) + "):(" +
std::to_string(max_x_) + "," + std::to_string(max_y_) + ")";
}
else
{
return "(empty bounds)";
}
}
NumericType getMinX() const { return min_x_; }
NumericType getMinY() const { return min_y_; }
NumericType getMaxX() const { return max_x_; }
NumericType getMaxY() const { return max_y_; }
protected:
NumericType min_x_, min_y_, max_x_, max_y_;
};
using Bounds = GenericBounds<double>;
inline unsigned int getDimension(unsigned int min_v, unsigned int max_v)
{
return (min_v > max_v) ? 0 : max_v - min_v + 1;
}
class UIntBounds : public GenericBounds<unsigned int>
{
public:
using GenericBounds<unsigned int>::GenericBounds;
unsigned int getWidth() const { return getDimension(min_x_, max_x_); }
unsigned int getHeight() const { return getDimension(min_y_, max_y_); }
};
} // namespace nav_core2
#endif // NAV_CORE2_BOUNDS_H

42
navigations/nav_core2/include/nav_core2/common.h Executable file
View File

@@ -0,0 +1,42 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Locus Robotics
* 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 copyright holder 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 HOLDER 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_CORE2_COMMON_H
#define NAV_CORE2_COMMON_H
#include <tf2_ros/buffer.h>
#include <memory>
using TFListenerPtr = std::shared_ptr<tf2_ros::Buffer>;
#endif // NAV_CORE2_COMMON_H

156
navigations/nav_core2/include/nav_core2/costmap.h Executable file
View File

@@ -0,0 +1,156 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, Locus Robotics
* 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 copyright holder 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 HOLDER 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_CORE2_COSTMAP_H
#define NAV_CORE2_COSTMAP_H
#include <nav_grid/nav_grid.h>
#include <nav_core2/common.h>
#include <nav_core2/bounds.h>
#include <boost/thread.hpp>
#include <memory>
#include <string>
namespace nav_core2
{
class Costmap : public nav_grid::NavGrid<unsigned char>
{
public:
static const unsigned char NO_INFORMATION = 255;
static const unsigned char LETHAL_OBSTACLE = 254;
static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253;
static const unsigned char FREE_SPACE = 0;
using Ptr = std::shared_ptr<Costmap>;
/**
* @brief Virtual Destructor
*/
virtual ~Costmap() {}
/**
* @brief Initialization function for the Costmap
*
* ROS parameters/topics are expected to be in the parent/name namespace.
* It is suggested that all NodeHandles in the costmap use the parent NodeHandle's callback queue.
*
* @param parent NodeHandle to derive other NodeHandles from
* @param name The namespace for the costmap
* @param tf A pointer to a transform listener
*/
virtual void initialize(const ros::NodeHandle& parent, const std::string& name, TFListenerPtr tf) {}
inline unsigned char getCost(const unsigned int x, const unsigned int y)
{
return getValue(x, y);
}
inline unsigned char getCost(const nav_grid::Index& index)
{
return getValue(index.x, index.y);
}
inline void setCost(const unsigned int x, const unsigned int y, const unsigned char cost)
{
setValue(x, y, cost);
}
inline void setCost(const nav_grid::Index& index, const unsigned char cost)
{
setValue(index, cost);
}
/**
* @brief Update the values in the costmap
*
* Note that this method can throw CostmapExceptions to indicate problems, like when it would be unsafe to navigate.
* e.g. If the costmap expects laser data at a given rate, but laser data hasn't shown up in a while, this method
* might throw a CostmapDataLagException.
*/
virtual void update() {}
using mutex_t = boost::recursive_mutex;
/**
* @brief Accessor for boost mutex
*/
virtual mutex_t* getMutex() = 0;
/**
* @brief Flag to indicate whether this costmap is able to track how much has changed
*/
virtual bool canTrackChanges() { return false; }
/**
* @brief If canTrackChanges, get the bounding box for how much of the costmap has changed
*
* Rather than querying based on time stamps (which can require an arbitrary amount of storage)
* we instead query based on a namespace. The return bounding box reports how much of the costmap
* has changed since the last time this method was called with a particular namespace. If a namespace
* is new, then it returns a bounding box for the whole costmap. The max values are inclusive.
*
* Example Sequence with a 5x5 costmap: (results listed (min_x,min_y):(max_x, max_y))
* 0) getChangeBounds("A") --> (0,0):(4,4)
* 1) getChangeBounds("B") --> (0,0):(4,4)
* 2) update cell 1, 1
* 3) getChangeBounds("C") --> (0,0):(4,4)
* 4) getChangeBounds("A") --> (1,1):(1,1)
* 5) getChangeBounds("A") --> (empty bounds) (i.e. nothing was updated since last call)
* 6) updateCell 2, 4
* 7) getChangeBounds("A") --> (2,4):(2,4)
* 8) getChangeBounds("B") --> (1,1):(2,4)
*
* @param ns The namespace
* @return Updated bounds
* @throws std::runtime_error If canTrackChanges is false, the returned bounds would be meaningless
*/
virtual UIntBounds getChangeBounds(const std::string& ns)
{
if (!canTrackChanges())
{
throw std::runtime_error("You called 'getChangeBounds()' on a derived Costmap type that is not capable of "
"tracking changes (i.e. canTrackChanges() returns false). You shouldn't do that.");
}
else
{
throw std::runtime_error("You called 'getChangeBounds()' on a derived Costmap type that is capable of tracking "
"changes but has not properly implemented this function. You should yell at the author "
"of the derived Costmap.");
}
return UIntBounds();
}
};
} // namespace nav_core2
#endif // NAV_CORE2_COSTMAP_H

321
navigations/nav_core2/include/nav_core2/exceptions.h Executable file
View File

@@ -0,0 +1,321 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Locus Robotics
* 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 copyright holder 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 HOLDER 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_CORE2_EXCEPTIONS_H
#define NAV_CORE2_EXCEPTIONS_H
#include <nav_2d_msgs/Pose2DStamped.h>
#include <stdexcept>
#include <exception>
#include <string>
#include <memory>
/**************************************************
* The nav_core2 Planning Exception Hierarchy!!
* (with arbitrary integer result codes)
**************************************************
* NavCore2Exception
* 0 CostmapException
* 1 CostmapSafetyException
* 2 CostmapDataLagException
* 3 PlannerException
* 4 GlobalPlannerException
* 5 InvalidStartPoseException
* 6 StartBoundsException
* 7 OccupiedStartException
* 8 InvalidGoalPoseException
* 9 GoalBoundsException
* 10 OccupiedGoalException
* 11 NoGlobalPathException
* 12 GlobalPlannerTimeoutException
* 13 LocalPlannerException
* 14 IllegalTrajectoryException
* 15 NoLegalTrajectoriesException
* 16 PlannerTFException
*
* -1 Unknown
**************************************************/
namespace nav_core2
{
inline std::string poseToString(const nav_2d_msgs::Pose2DStamped& pose)
{
return "(" + std::to_string(pose.pose.x) + ", " + std::to_string(pose.pose.y) + ", " + std::to_string(pose.pose.theta)
+ " : " + pose.header.frame_id + ")";
}
class NavCore2Exception: public std::runtime_error
{
public:
explicit NavCore2Exception(const std::string& description, int result_code)
: std::runtime_error(description), result_code_(result_code) {}
int getResultCode() const { return result_code_; }
protected:
int result_code_;
};
using NavCore2ExceptionPtr = std::exception_ptr;
/**
* @brief Handy function for getting the result code
*/
inline int getResultCode(const NavCore2ExceptionPtr& e_ptr)
{
if (e_ptr == nullptr)
{
return -1;
}
try
{
std::rethrow_exception(e_ptr);
}
catch (const NavCore2Exception& e)
{
return e.getResultCode();
}
catch (...)
{
// Will end up here if current_exception returned a non-NavCore2Exception
return -1;
}
}
/**
* @class CostmapException
* @brief Extensible exception class for all costmap-related problems
*/
class CostmapException: public NavCore2Exception
{
public:
explicit CostmapException(const std::string& description, int result_code = 0)
: NavCore2Exception(description, result_code) {}
};
/**
* @class CostmapSafetyException
* @brief General container for exceptions thrown when the costmap thinks any movement would be unsafe
*/
class CostmapSafetyException: public CostmapException
{
public:
explicit CostmapSafetyException(const std::string& description, int result_code = 1)
: CostmapException(description, result_code) {}
};
/**
* @class CostmapDataLagException
* @brief Indicates costmap is out of date because data in not up to date.
*
* Functions similarly to `!Costmap2DROS::isCurrent()`
*/
class CostmapDataLagException: public CostmapSafetyException
{
public:
explicit CostmapDataLagException(const std::string& description, int result_code = 2)
: CostmapSafetyException(description, result_code) {}
};
/**
* @class PlannerException
* @brief Parent type of all exceptions defined within
*/
class PlannerException: public NavCore2Exception
{
public:
explicit PlannerException(const std::string& description, int result_code = 3)
: NavCore2Exception(description, result_code) {}
};
/**
* @class GlobalPlannerException
* @brief General container for exceptions thrown from the Global Planner
*/
class GlobalPlannerException: public PlannerException
{
public:
explicit GlobalPlannerException(const std::string& description, int result_code = 4)
: PlannerException(description, result_code) {}
};
/**
* @class LocalPlannerException
* @brief General container for exceptions thrown from the Local Planner
*/
class LocalPlannerException: public PlannerException
{
public:
explicit LocalPlannerException(const std::string& description, int result_code = 13)
: PlannerException(description, result_code) {}
};
/**
* @class PlannerTFException
* @brief Thrown when either the global or local planner cannot complete its operation due to TF errors
*/
class PlannerTFException: public PlannerException
{
public:
explicit PlannerTFException(const std::string& description, int result_code = 16)
: PlannerException(description, result_code) {}
};
/**
* @class InvalidStartPoseException
* @brief Exception thrown when there is a problem at the start location for the global planner
*/
class InvalidStartPoseException: public GlobalPlannerException
{
public:
explicit InvalidStartPoseException(const std::string& description, int result_code = 5)
: GlobalPlannerException(description, result_code) {}
InvalidStartPoseException(const nav_2d_msgs::Pose2DStamped& pose, const std::string& problem, int result_code = 5) :
InvalidStartPoseException("The starting pose " + poseToString(pose) + " is " + problem, result_code) {}
};
/**
* @class StartBoundsException
* @brief Exception thrown when the start location of the global planner is out of the expected bounds
*/
class StartBoundsException: public InvalidStartPoseException
{
public:
explicit StartBoundsException(const std::string& description, int result_code = 6)
: InvalidStartPoseException(description, result_code) {}
explicit StartBoundsException(const nav_2d_msgs::Pose2DStamped& pose) :
InvalidStartPoseException(pose, "out of bounds", 6) {}
};
/**
* @class OccupiedStartException
* @brief Exception thrown when the start location of the global planner is occupied in the costmap
*/
class OccupiedStartException: public InvalidStartPoseException
{
public:
explicit OccupiedStartException(const std::string& description, int result_code = 7)
: InvalidStartPoseException(description, result_code) {}
explicit OccupiedStartException(const nav_2d_msgs::Pose2DStamped& pose) :
InvalidStartPoseException(pose, "occupied", 7) {}
};
/**
* @class InvalidGoalPoseException
* @brief Exception thrown when there is a problem at the goal location for the global planner
*/
class InvalidGoalPoseException: public GlobalPlannerException
{
public:
explicit InvalidGoalPoseException(const std::string& description, int result_code = 8)
: GlobalPlannerException(description, result_code) {}
InvalidGoalPoseException(const nav_2d_msgs::Pose2DStamped& pose, const std::string& problem, int result_code = 8) :
GlobalPlannerException("The goal pose " + poseToString(pose) + " is " + problem, result_code) {}
};
/**
* @class GoalBoundsException
* @brief Exception thrown when the goal location of the global planner is out of the expected bounds
*/
class GoalBoundsException: public InvalidGoalPoseException
{
public:
explicit GoalBoundsException(const std::string& description, int result_code = 9)
: InvalidGoalPoseException(description, result_code) {}
explicit GoalBoundsException(const nav_2d_msgs::Pose2DStamped& pose) :
InvalidGoalPoseException(pose, "out of bounds", 9) {}
};
/**
* @class OccupiedGoalException
* @brief Exception thrown when the goal location of the global planner is occupied in the costmap
*/
class OccupiedGoalException: public InvalidGoalPoseException
{
public:
explicit OccupiedGoalException(const std::string& description, int result_code = 10)
: InvalidGoalPoseException(description, result_code) {}
explicit OccupiedGoalException(const nav_2d_msgs::Pose2DStamped& pose) :
InvalidGoalPoseException(pose, "occupied", 10) {}
};
/**
* @class NoGlobalPathException
* @brief Exception thrown when the global planner cannot find a path from the start to the goal
*/
class NoGlobalPathException: public GlobalPlannerException
{
public:
explicit NoGlobalPathException(const std::string& description, int result_code = 11)
: GlobalPlannerException(description, result_code) {}
NoGlobalPathException() : GlobalPlannerException("No global path found.") {}
};
/**
* @class GlobalPlannerTimeoutException
* @brief Exception thrown when the global planner has spent too long looking for a path
*/
class GlobalPlannerTimeoutException: public GlobalPlannerException
{
public:
explicit GlobalPlannerTimeoutException(const std::string& description, int result_code = 12)
: GlobalPlannerException(description, result_code) {}
};
/**
* @class IllegalTrajectoryException
* @brief Thrown when one of the critics encountered a fatal error
*/
class IllegalTrajectoryException: public LocalPlannerException
{
public:
IllegalTrajectoryException(const std::string& critic_name, const std::string& description, int result_code = 14)
: LocalPlannerException(description, result_code), critic_name_(critic_name) {}
std::string getCriticName() const { return critic_name_; }
protected:
std::string critic_name_;
};
/**
* @class NoLegalTrajectoriesException
* @brief Thrown when all the trajectories explored are illegal
*/
class NoLegalTrajectoriesException: public LocalPlannerException
{
public:
explicit NoLegalTrajectoriesException(const std::string& description, int result_code = 15)
: LocalPlannerException(description, result_code) {}
};
} // namespace nav_core2
#endif // NAV_CORE2_EXCEPTIONS_H

83
navigations/nav_core2/include/nav_core2/global_planner.h Executable file
View File

@@ -0,0 +1,83 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Locus Robotics
* 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 copyright holder 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 HOLDER 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_CORE2_GLOBAL_PLANNER_H
#define NAV_CORE2_GLOBAL_PLANNER_H
#include <nav_core2/common.h>
#include <nav_core2/costmap.h>
#include <nav_2d_msgs/Path2D.h>
#include <nav_2d_msgs/Pose2DStamped.h>
#include <string>
namespace nav_core2
{
/**
* @class GlobalPlanner
* @brief Provides an interface for global planners used in navigation.
*/
class GlobalPlanner
{
public:
/**
* @brief Virtual Destructor
*/
virtual ~GlobalPlanner() {}
/**
* @brief Initialization function for the GlobalPlanner
*
* ROS parameters/topics are expected to be in the parent/name namespace.
* It is suggested that all NodeHandles in the planner use the parent NodeHandle's callback queue.
*
* @param parent NodeHandle to derive other NodeHandles from
* @param name The name of this planner
* @param tf A pointer to a transform listener
* @param costmap A pointer to the costmap
*/
virtual void initialize(const ros::NodeHandle& parent, const std::string& name,
TFListenerPtr tf, Costmap::Ptr costmap) = 0;
/**
* @brief Run the global planner to make a plan starting at the start and ending at the goal.
* @param start The starting pose of the robot
* @param goal The goal pose of the robot
* @return The sequence of poses to get from start to goal, if any
*/
virtual nav_2d_msgs::Path2D makePlan(const nav_2d_msgs::Pose2DStamped& start,
const nav_2d_msgs::Pose2DStamped& goal) = 0;
};
} // namespace nav_core2
#endif // NAV_CORE2_GLOBAL_PLANNER_H

113
navigations/nav_core2/include/nav_core2/local_planner.h Executable file
View File

@@ -0,0 +1,113 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Locus Robotics
* 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 copyright holder 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 HOLDER 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_CORE2_LOCAL_PLANNER_H
#define NAV_CORE2_LOCAL_PLANNER_H
#include <nav_core2/common.h>
#include <nav_core2/costmap.h>
#include <nav_2d_msgs/Path2D.h>
#include <nav_2d_msgs/Pose2DStamped.h>
#include <nav_2d_msgs/Twist2D.h>
#include <nav_2d_msgs/Twist2DStamped.h>
#include <string>
namespace nav_core2
{
/**
* @class LocalPlanner
* @brief Provides an interface for local planners used in navigation.
*/
class LocalPlanner
{
public:
/**
* @brief Virtual Destructor
*/
virtual ~LocalPlanner() {}
/**
* @brief Constructs the local planner
*
* ROS parameters/topics are expected to be in the parent/name namespace.
* It is suggested that all NodeHandles in the planner use the parent NodeHandle's callback queue.
*
* @param parent NodeHandle to derive other NodeHandles from
* @param name The name to give this instance of the local planner
* @param tf A pointer to a transform listener
* @param costmap A pointer to the costmap
*/
virtual void initialize(const ros::NodeHandle& parent, const std::string& name,
TFListenerPtr tf, Costmap::Ptr costmap) = 0;
/**
* @brief Sets the global goal for this local planner.
* @param goal_pose The Goal Pose
*/
virtual void setGoalPose(const nav_2d_msgs::Pose2DStamped& goal_pose) = 0;
/**
* @brief Sets the global plan for this local planner.
*
* @param path The global plan
*/
virtual void setPlan(const nav_2d_msgs::Path2D& path) = 0;
/**
* @brief Compute the best command given the current pose, velocity and goal
*
* Get the local plan, given an initial pose, velocity and goal pose.
* It is presumed that the global plan is already set.
*
* @param pose Current robot pose
* @param velocity Current robot velocity
* @return The best computed velocity
*/
virtual nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped& pose,
const nav_2d_msgs::Twist2D& velocity) = 0;
/**
* @brief Check to see whether the robot has reached its goal
*
* This tests whether the robot has fully reached the goal, given the current pose and velocity.
*
* @param query_pose The pose to check, in local costmap coordinates.
* @param velocity Velocity to check
* @return True if the goal conditions have been met
*/
virtual bool isGoalReached(const nav_2d_msgs::Pose2DStamped& pose, const nav_2d_msgs::Twist2D& velocity) = 0;
};
} // namespace nav_core2
#endif // NAV_CORE2_LOCAL_PLANNER_H

17
navigations/nav_core2/package.xml Executable file
View File

@@ -0,0 +1,17 @@
<?xml version="1.0"?>
<package format="2">
<name>nav_core2</name>
<version>0.3.0</version>
<description>
Interfaces for Costmap, LocalPlanner and GlobalPlanner. Replaces nav_core.
</description>
<maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
<license>BSD</license>
<buildtool_depend>catkin</buildtool_depend>
<depend>nav_2d_msgs</depend>
<depend>nav_grid</depend>
<depend>tf2_ros</depend>
<test_depend>roslint</test_depend>
<test_depend>rosunit</test_depend>
</package>

60
navigations/nav_core2/src/basic_costmap.cpp Executable file
View File

@@ -0,0 +1,60 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, Locus Robotics
* 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 copyright holder 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 HOLDER 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.
*/
#include <nav_core2/basic_costmap.h>
namespace nav_core2
{
void BasicCostmap::reset()
{
data_.assign(info_.width * info_.height, this->default_value_);
}
unsigned int BasicCostmap::getIndex(const unsigned int x, const unsigned int y) const
{
return y * info_.width + x;
}
unsigned char BasicCostmap::getValue(const unsigned int x, const unsigned int y) const
{
return data_[getIndex(x, y)];
}
void BasicCostmap::setValue(const unsigned int x, const unsigned int y, const unsigned char& value)
{
data_[getIndex(x, y)] = value;
}
} // namespace nav_core2

124
navigations/nav_core2/test/bounds_test.cpp Executable file
View File

@@ -0,0 +1,124 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, Locus Robotics
* 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 copyright holder 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 HOLDER 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.
*/
#include <gtest/gtest.h>
#include <nav_core2/bounds.h>
using nav_core2::Bounds;
using nav_core2::UIntBounds;
TEST(Bounds, test_bounds_simple)
{
Bounds b;
EXPECT_TRUE(b.isEmpty());
b.touch(5.0, 6.0);
EXPECT_EQ(5.0, b.getMinX());
EXPECT_EQ(5.0, b.getMinX());
EXPECT_EQ(5.0, b.getMaxX());
EXPECT_EQ(6.0, b.getMinY());
EXPECT_EQ(6.0, b.getMaxY());
EXPECT_TRUE(b.contains(5.0, 6.0));
EXPECT_FALSE(b.contains(5.5, 6.0));
EXPECT_FALSE(b.contains(5.5, 4.0));
EXPECT_FALSE(b.isEmpty());
Bounds b2 = b;
EXPECT_EQ(5.0, b2.getMinX());
EXPECT_EQ(5.0, b2.getMaxX());
EXPECT_EQ(6.0, b2.getMinY());
EXPECT_EQ(6.0, b2.getMaxY());
b.reset();
EXPECT_EQ(5.0, b2.getMinX());
EXPECT_EQ(5.0, b2.getMaxX());
EXPECT_EQ(6.0, b2.getMinY());
EXPECT_EQ(6.0, b2.getMaxY());
EXPECT_FALSE(b.contains(5.0, 6.0));
EXPECT_FALSE(b.contains(5.5, 6.0));
EXPECT_TRUE(b2.contains(5.0, 6.0));
EXPECT_FALSE(b.contains(5.5, 6.0));
EXPECT_TRUE(b.isEmpty());
Bounds b3;
b3.touch(1.0, 5.0);
b3.touch(4.0, 2.0);
EXPECT_TRUE(b3.contains(3.0, 3.0));
EXPECT_FALSE(b3.contains(0.0, 3.0));
EXPECT_FALSE(b3.contains(5.0, 3.0));
EXPECT_FALSE(b3.contains(3.0, 6.0));
EXPECT_FALSE(b3.contains(3.0, 1.0));
}
TEST(Bounds, test_dimensions)
{
UIntBounds empty;
UIntBounds square(0, 0, 5, 5);
UIntBounds rectangle(1, 4, 3, 15);
EXPECT_EQ(empty.getWidth(), 0u);
EXPECT_EQ(empty.getHeight(), 0u);
EXPECT_EQ(square.getWidth(), 6u);
EXPECT_EQ(square.getHeight(), 6u);
EXPECT_EQ(rectangle.getWidth(), 3u);
EXPECT_EQ(rectangle.getHeight(), 12u);
}
TEST(Bounds, test_bounds_overlap)
{
UIntBounds b0(0, 0, 5, 5);
UIntBounds b1(0, 0, 5, 5);
UIntBounds b2(0, 0, 3, 3);
UIntBounds b3(3, 0, 4, 4);
UIntBounds b4(4, 0, 4, 4);
UIntBounds b5(1, 4, 3, 15);
UIntBounds b6(10, 10, 10, 10);
EXPECT_TRUE(b0.overlaps(b0));
EXPECT_TRUE(b0.overlaps(b1));
EXPECT_TRUE(b0.overlaps(b2));
EXPECT_TRUE(b2.overlaps(b0));
EXPECT_TRUE(b0.overlaps(b3));
EXPECT_TRUE(b2.overlaps(b3));
EXPECT_FALSE(b2.overlaps(b4));
EXPECT_TRUE(b0.overlaps(b5));
EXPECT_TRUE(b5.overlaps(b0));
EXPECT_FALSE(b0.overlaps(b6));
EXPECT_FALSE(b6.overlaps(b0));
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

133
navigations/nav_core2/test/exception_test.cpp Executable file
View File

@@ -0,0 +1,133 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2019, Locus Robotics
* 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 copyright holder 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 HOLDER 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.
*/
#include <gtest/gtest.h>
#include <nav_core2/exceptions.h>
#include <string>
TEST(Exceptions, direct_code_access)
{
// Make sure the caught exceptions have the same types as the thrown exception
// (This version does not create any copies of the exception)
try
{
throw nav_core2::GlobalPlannerTimeoutException("test case");
}
catch (nav_core2::GlobalPlannerTimeoutException& x)
{
EXPECT_EQ(x.getResultCode(), 12);
}
try
{
throw nav_core2::GlobalPlannerTimeoutException("test case");
}
catch (nav_core2::PlannerException& x)
{
EXPECT_EQ(x.getResultCode(), 12);
}
}
TEST(Exceptions, indirect_code_access)
{
// Make sure the caught exceptions have the same types as the thrown exception
// (This version copies the exception to a new object with the parent type)
nav_core2::PlannerException e("");
try
{
throw nav_core2::GlobalPlannerTimeoutException("test case");
}
catch (nav_core2::GlobalPlannerTimeoutException& x)
{
e = x;
}
EXPECT_EQ(e.getResultCode(), 12);
}
TEST(Exceptions, rethrow)
{
// This version tests the ability to catch specific exceptions when rethrown
// A copy of the exception is made and rethrown, with the goal being able to catch the specific type
// and not the parent type.
nav_core2::NavCore2ExceptionPtr e;
try
{
throw nav_core2::GlobalPlannerTimeoutException("test case");
}
catch (nav_core2::GlobalPlannerTimeoutException& x)
{
e = std::current_exception();
}
EXPECT_EQ(nav_core2::getResultCode(e), 12);
try
{
std::rethrow_exception(e);
}
catch (nav_core2::GlobalPlannerTimeoutException& x)
{
EXPECT_EQ(x.getResultCode(), 12);
SUCCEED();
}
catch (nav_core2::PlannerException& x)
{
FAIL() << "PlannerException caught instead of specific exception";
}
}
TEST(Exceptions, weird_exception)
{
nav_core2::NavCore2ExceptionPtr e;
// Check what happens with no exception
EXPECT_EQ(nav_core2::getResultCode(e), -1);
// Check what happens with a non NavCore2Exception
try
{
std::string().at(1); // this generates an std::out_of_range
}
catch (...)
{
e = std::current_exception();
}
EXPECT_EQ(nav_core2::getResultCode(e), -1);
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}