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

update

Browse Source
This commit is contained in:
HiepLM
2025-12-30 10:25:25 +07:00
parent dad2726eb1
commit 82afc4c203
60 changed files with 162 additions and 162 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/Navigations/Cores/nav_core/CMakeLists.txt
View File

@@ -19,7 +19,7 @@ include_directories(
# Tạo INTERFACE library (header-only)
add_library(nav_core INTERFACE)
target_link_libraries(nav_core INTERFACE costmap_2d tf3 robot_protocol_msgs)
target_link_libraries(nav_core INTERFACE robot_costmap_2d tf3 robot_protocol_msgs)
# Set include directories
target_include_directories(nav_core

4
src/Navigations/Cores/nav_core/include/nav_core/base_global_planner.h
View File

@@ -38,7 +38,7 @@
#define NAV_CORE_BASE_GLOBAL_PLANNER_H
#include <robot_geometry_msgs/PoseStamped.h>
#include <costmap_2d/costmap_2d_robot.h>
#include <robot_costmap_2d/costmap_2d_robot.h>
#include <robot_protocol_msgs/Order.h>
#include <memory>
@@ -97,7 +97,7 @@ namespace nav_core {
* @param name The name of this planner
* @param costmap_robot A pointer to the wrapper of the costmap to use for planning
*/
virtual bool initialize(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot) = 0;
virtual bool initialize(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot) = 0;
/**
* @brief Virtual destructor for the interface

4
src/Navigations/Cores/nav_core/include/nav_core/base_local_planner.h
View File

@@ -39,7 +39,7 @@
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/Twist.h>
#include <costmap_2d/costmap_2d_robot.h>
#include <robot_costmap_2d/costmap_2d_robot.h>
#include <tf3/buffer_core.h>
#include <memory>
@@ -138,7 +138,7 @@ namespace nav_core
* @param tf A pointer to a transform listener
* @param costmap_robot The cost map to use for assigning costs to local plans
*/
virtual void initialize(std::string name, tf3::BufferCore *tf, costmap_2d::Costmap2DROBOT *costmap_robot) = 0;
virtual void initialize(std::string name, tf3::BufferCore *tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) = 0;
/**
* @brief Virtual destructor for the interface

4
src/Navigations/Cores/nav_core/include/nav_core/recovery_behavior.h
View File

@@ -37,7 +37,7 @@
#ifndef NAV_CORE_RECOVERY_BEHAVIOR_H
#define NAV_CORE_RECOVERY_BEHAVIOR_H
#include <costmap_2d/costmap_2d_robot.h>
#include <robot_costmap_2d/costmap_2d_robot.h>
#include <tf3/buffer_core.h>
#include <memory>
@@ -57,7 +57,7 @@ namespace nav_core {
* @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
*/
virtual void initialize(std::string name, tf3::BufferCore* tf, costmap_2d::Costmap2DROBOT* global_costmap, costmap_2d::Costmap2DROBOT* local_costmap) = 0;
virtual void initialize(std::string name, tf3::BufferCore* tf, robot_costmap_2d::Costmap2DROBOT* global_costmap, robot_costmap_2d::Costmap2DROBOT* local_costmap) = 0;
/**
* @brief Runs the RecoveryBehavior

2
src/Navigations/Cores/nav_core2/CMakeLists.txt
View File

@@ -24,7 +24,7 @@ file(GLOB HEADERS "include/nav_core2/*.h")
add_library(nav_core2 INTERFACE)
target_link_libraries(nav_core2 INTERFACE
costmap_2d
robot_costmap_2d
tf3
nav_grid
robot_nav_2d_msgs

8
src/Navigations/Cores/nav_core2/README.md
View File

@@ -3,12 +3,12 @@ A replacement interface for [nav_core](https://github.com/ros-planning/navigatio
There were a few key reasons for creating new interfaces rather than extending the old ones.
* Use `robot_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::Costmap2DROBOT`.
* Use a new `Costmap` interface as a plugin rather that forcing all implementations of the interfaces to use `robot_costmap_2d::Costmap2DROBOT`.
* 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::Costmap2DROBOT` has been a vital part of the navigation stack for years, but it was not without its flaws.
`robot_costmap_2d::Costmap2DROBOT` 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 `Costmap2DROBOT`-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 `Costmap2DROBOT` 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.
@@ -29,7 +29,7 @@ Let us compare the old [nav_core::BaseGlobalPlanner](https://github.com/ros-plan
| `nav_core` | `nav_core2` | comments |
|---|--|---|
|`void initialize(std::string, costmap_2d::Costmap2DROBOT*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers, and provides a TFListener|
|`void initialize(std::string, robot_costmap_2d::Costmap2DROBOT*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers, and provides a TFListener|
|`bool makePlan(const robot_geometry_msgs::PoseStamped&, const robot_geometry_msgs::PoseStamped&, std::vector<robot_geometry_msgs::PoseStamped>&)`|`robot_nav_2d_msgs::Path2D makePlan(const robot_nav_2d_msgs::Pose2DStamped&, const robot_nav_2d_msgs::Pose2DStamped&)`|Uses exceptions for errors instead of returning a bool, which frees up the return for the actual path.|
## Local Planner
@@ -37,7 +37,7 @@ Now let's compare the old [nav_core::BaseLocalPlanner](https://github.com/ros-pl
| `nav_core` | `nav_core2` | comments |
|---|--|---|
|`void initialize(std::string, tf::TransformListener*, costmap_2d::Costmap2DROBOT*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers|
|`void initialize(std::string, tf::TransformListener*, robot_costmap_2d::Costmap2DROBOT*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers|
|(no equivalent)|`void setGoalPose(const robot_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<robot_geometry_msgs::PoseStamped>&)`|`setPlan(const robot_nav_2d_msgs::Path2D&)`||
|`bool computeVelocityCommands(robot_geometry_msgs::Twist&)`|`robot_nav_2d_msgs::Twist2DStamped computeVelocityCommands(const robot_nav_2d_msgs::Pose2DStamped&, const robot_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.|

4
src/Navigations/Cores/nav_core2/include/nav_core2/local_planner.h
View File

@@ -42,7 +42,7 @@
#include <robot_nav_2d_msgs/Twist2DStamped.h>
#include <robot_geometry_msgs/Twist.h>
#include <string>
#include <costmap_2d/costmap_2d_robot.h>
#include <robot_costmap_2d/costmap_2d_robot.h>
#include <memory>
namespace nav_core2
@@ -77,7 +77,7 @@ namespace nav_core2
// TFListenerPtr tf, Costmap::Ptr costmap) = 0;
virtual void initialize(robot::NodeHandle &parent, const std::string &name,
TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap) = 0;
TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap) = 0;
/**
* @brief Sets the global goal for this local planner.

2
src/Navigations/Cores/nav_core_adapter/CMakeLists.txt
View File

@@ -59,7 +59,7 @@ target_include_directories(global_planner_adapter PRIVATE
# which has a different ABI (Costmap2DROBOT) and causes missing symbols.
set(_NAV_CORE_ADAPTER_RPATH
"${CMAKE_BINARY_DIR}/src/Navigations/Cores/nav_core_adapter"
"${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d"
"${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d"
"${CMAKE_BINARY_DIR}/src/Libraries/tf3"
"${CMAKE_BINARY_DIR}/src/Libraries/robot_time"
"${CMAKE_BINARY_DIR}/src/Libraries/robot_cpp"

2
src/Navigations/Cores/nav_core_adapter/README.md
View File

@@ -3,7 +3,7 @@ This package contains adapters for using `nav_core` plugins as `nav_core2` plugi
* Converting between 2d and 3d datatypes.
* Converting between returning false and throwing exceptions on failure.
We also provide an adapter for using a `costmap_2d::Costmap2DROBOT` as a plugin for the `nav_core2::Costmap` interface.
We also provide an adapter for using a `robot_costmap_2d::Costmap2DROBOT` as a plugin for the `nav_core2::Costmap` interface.
## Adapter Classes
* Global Planner Adapters

12
src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/costmap_adapter.h
View File

@@ -37,12 +37,12 @@
#include <nav_core2/common.h>
#include <nav_core2/costmap.h>
#include <costmap_2d/costmap_2d_robot.h>
#include <robot_costmap_2d/costmap_2d_robot.h>
#include <string>
namespace nav_core_adapter
{
nav_grid::NavGridInfo infoFromCostmap(costmap_2d::Costmap2DROBOT* costmap_robot);
nav_grid::NavGridInfo infoFromCostmap(robot_costmap_2d::Costmap2DROBOT* costmap_robot);
class CostmapAdapter : public nav_core2::Costmap
{
@@ -57,7 +57,7 @@ public:
* @param costmap_robot A Costmap2DROBOT object
* @param needs_destruction Whether to free the costmap_robot object when this class is destroyed
*/
void initialize(costmap_2d::Costmap2DROBOT* costmap_robot, bool needs_destruction = false);
void initialize(robot_costmap_2d::Costmap2DROBOT* costmap_robot, bool needs_destruction = false);
// Standard Costmap Interface
void initialize(const robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf) override;
@@ -72,11 +72,11 @@ public:
void updateInfo(const nav_grid::NavGridInfo& new_info) override;
// Get Costmap Pointer for Backwards Compatibility
costmap_2d::Costmap2DROBOT* getCostmap2DROBOT() const { return costmap_robot_; }
robot_costmap_2d::Costmap2DROBOT* getCostmap2DROBOT() const { return costmap_robot_; }
protected:
costmap_2d::Costmap2DROBOT* costmap_robot_;
costmap_2d::Costmap2D* costmap_;
robot_costmap_2d::Costmap2DROBOT* costmap_robot_;
robot_costmap_2d::Costmap2D* costmap_;
bool needs_destruction_;
};

2
src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/global_planner_adapter.h
View File

@@ -67,7 +67,7 @@ protected:
boost::function<nav_core::BaseGlobalPlanner::Ptr()> planner_loader_;
nav_core::BaseGlobalPlanner::Ptr planner_;
costmap_2d::Costmap2DROBOT* costmap_robot_;
robot_costmap_2d::Costmap2DROBOT* costmap_robot_;
nav_core2::Costmap::Ptr costmap_;
};

4
src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/local_planner_adapter.h
View File

@@ -77,7 +77,7 @@ namespace nav_core_adapter
virtual ~LocalPlannerAdapter();
// Standard ROS Local Planner Interface
void initialize(std::string name, tf3::BufferCore *tf, costmap_2d::Costmap2DROBOT *costmap_robot) override;
void initialize(std::string name, tf3::BufferCore *tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) override;
/**
* @brief Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base
@@ -193,7 +193,7 @@ namespace nav_core_adapter
TFListenerPtr tf_;
std::shared_ptr<CostmapAdapter> costmap_adapter_;
costmap_2d::Costmap2DROBOT *costmap_robot_;
robot_costmap_2d::Costmap2DROBOT *costmap_robot_;
boost::recursive_mutex configuration_mutex_;

6
src/Navigations/Cores/nav_core_adapter/src/costmap_adapter.cpp
View File

@@ -43,10 +43,10 @@
namespace nav_core_adapter
{
nav_grid::NavGridInfo infoFromCostmap(costmap_2d::Costmap2DROBOT* costmap_robot)
nav_grid::NavGridInfo infoFromCostmap(robot_costmap_2d::Costmap2DROBOT* costmap_robot)
{
nav_grid::NavGridInfo info;
costmap_2d::Costmap2D* costmap = costmap_robot->getCostmap();
robot_costmap_2d::Costmap2D* costmap = costmap_robot->getCostmap();
info.width = costmap->getSizeInCellsX();
info.height = costmap->getSizeInCellsY();
info.resolution = costmap->getResolution();
@@ -64,7 +64,7 @@ CostmapAdapter::~CostmapAdapter()
}
}
void CostmapAdapter::initialize(costmap_2d::Costmap2DROBOT* costmap_robot, bool needs_destruction)
void CostmapAdapter::initialize(robot_costmap_2d::Costmap2DROBOT* costmap_robot, bool needs_destruction)
{
costmap_robot_ = costmap_robot;
needs_destruction_ = needs_destruction;

2
src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp
View File

@@ -73,7 +73,7 @@ namespace nav_core_adapter
/**
* @brief Load the nav_core2 local planner and initialize it
*/
void LocalPlannerAdapter::initialize(std::string name, tf3::BufferCore *tf, costmap_2d::Costmap2DROBOT *costmap_robot)
void LocalPlannerAdapter::initialize(std::string name, tf3::BufferCore *tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot)
{
tf_ = createSharedPointerWithNoDelete<tf3::BufferCore>(tf);
costmap_robot_ = costmap_robot;

2
src/Navigations/Cores/nav_core_adapter/test/unload_test.cpp
View File

@@ -51,7 +51,7 @@ TEST(LocalPlannerAdapter, unload_local_planner)
nav_core_adapter::LocalPlannerAdapter* lpa = new nav_core_adapter::LocalPlannerAdapter();
costmap_2d::Costmap2DROBOT costmap_robot("local_costmap", tf);
robot_costmap_2d::Costmap2DROBOT costmap_robot("local_costmap", tf);
lpa->initialize("lpa", &tf, &costmap_robot);
delete lpa;

2
src/Navigations/Libraries/nav_grid/README.md
View File

@@ -17,7 +17,7 @@ Together, these components make a [`nav_grid::NavGridInfo`](include/nav_grid/nav
The default values are `width=0, height=0, resolution=1.0, frame_id="map", origin_x=0.0, origin_y=0.0`.
## Coordinate Conversion
One of the most common operations is to want to convert between the real world coordinates and the grid coordinates. These operations can be done with a `NavGridInfo` object and the methods in [`coordinate_conversion.h`](include/nav_grid/coordinate_conversion.h). They are derived from methods in [`costmap_2d.h`](https://github.com/ros-planning/navigation/blob/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/costmap_2d/include/costmap_2d/costmap_2d.h#L126), but with some key differences (beyond replacing `map` with `grid`).
One of the most common operations is to want to convert between the real world coordinates and the grid coordinates. These operations can be done with a `NavGridInfo` object and the methods in [`coordinate_conversion.h`](include/nav_grid/coordinate_conversion.h). They are derived from methods in [`robot_costmap_2d.h`](https://github.com/ros-planning/navigation/blob/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/robot_costmap_2d/include/robot_costmap_2d/robot_costmap_2d.h#L126), but with some key differences (beyond replacing `map` with `grid`).
* `gridToWorld` is the same as `mapToWorld`, as both return the world coordinates of the center of the specified cell.
* `worldToGrid` works like `worldToMapNoBounds`, but it results in either `int` or `double` coordinates depending on the output parameter types. As the result are not bounded by the grid, the results are signed.
* `worldToGridBounded` is a combination of `worldToMap` and `worldToMapEnforceBounds`. It returns a bool for whether the input coordinates are within the grid AND the output coordinates are forced to be within the grid. The output coordinates are therefore `unsigned int`.

2
src/Navigations/Libraries/nav_grid/include/nav_grid/coordinate_conversion.h
View File

@@ -91,7 +91,7 @@ inline void worldToGrid(const NavGridInfo& info, double wx, double wy, int& mx,
/**
* @brief Convert from world coordinates to grid coordinates
*
* Combined functionality from costmap_2d::worldToMap and costmap_2d::worldToMapEnforceBounds.
* Combined functionality from robot_costmap_2d::worldToMap and robot_costmap_2d::worldToMapEnforceBounds.
* The output parameters are set to grid indexes within the grid, even if the function returns false,
* meaning the coordinates are outside the grid.
*

2
src/Navigations/Libraries/nav_grid/include/nav_grid/nav_grid.h
View File

@@ -43,7 +43,7 @@ namespace nav_grid
{
/**
* @class NavGrid
* This class is a spiritual successor to the costmap_2d::Costmap2D class, with the key differences being that
* This class is a spiritual successor to the robot_costmap_2d::Costmap2D class, with the key differences being that
* the datatype and data storage methods are not specified, and the frame_id is specified.
*
* The templatized nature of the class allows you to store whatever you like at each grid location, including

6
src/Navigations/Packages/move_base/BUILD_INSTRUCTIONS.md
View File

@@ -86,11 +86,11 @@ Ngược lại, sẽ build ở **Standalone CMake mode**.
### Internal Packages (cần build trước):
- move_base_core
- nav_core
- costmap_2d
- robot_costmap_2d
- xmlrpcpp
- node_handle
- tf3_sensor_msgs
- tf3_geometry_msgs
- robot_tf3_sensor_msgs
- robot_tf3_geometry_msgs
### System Libraries:
- Boost

10
src/Navigations/Packages/move_base/CMakeLists.txt
View File

@@ -49,8 +49,8 @@ include_directories(
# ========================================================
set(CMAKE_SKIP_BUILD_RPATH FALSE)
set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/node_handle:${CMAKE_BINARY_DIR}/src/Libraries/tf3:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/xmlrpcpp")
set(CMAKE_INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/node_handle:${CMAKE_BINARY_DIR}/src/Libraries/tf3:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/xmlrpcpp")
set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/node_handle:${CMAKE_BINARY_DIR}/src/Libraries/tf3:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/xmlrpcpp")
set(CMAKE_INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/node_handle:${CMAKE_BINARY_DIR}/src/Libraries/tf3:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/xmlrpcpp")
set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
# ========================================================
@@ -68,11 +68,11 @@ set(PACKAGES_DIR
robot_std_msgs
move_base_core
nav_core
costmap_2d
robot_costmap_2d
plugins # Link với plugins library để có StaticLayer typeinfo
yaml-cpp
tf3_sensor_msgs
tf3_geometry_msgs
robot_tf3_sensor_msgs
robot_tf3_geometry_msgs
data_convert
dl
pthread

10
src/Navigations/Packages/move_base/include/move_base/move_base.h
View File

@@ -24,8 +24,8 @@
#include <boost/shared_ptr.hpp>
#include <boost/thread/recursive_mutex.hpp>
#include <costmap_2d/costmap_2d_robot.h>
#include <costmap_2d/costmap_2d.h>
#include <robot_costmap_2d/costmap_2d_robot.h>
#include <robot_costmap_2d/costmap_2d.h>
#include <robot_nav_msgs/GetPlan.h>
#include <robot_protocol_msgs/Order.h>
@@ -293,7 +293,7 @@ namespace move_base
bool isQuaternionValid(const robot_geometry_msgs::Quaternion &q);
bool getRobotPose(robot_geometry_msgs::PoseStamped &global_pose, costmap_2d::Costmap2DROBOT *costmap);
bool getRobotPose(robot_geometry_msgs::PoseStamped &global_pose, robot_costmap_2d::Costmap2DROBOT *costmap);
double distance(const robot_geometry_msgs::PoseStamped &p1, const robot_geometry_msgs::PoseStamped &p2);
@@ -317,8 +317,8 @@ namespace move_base
nav_core::BaseGlobalPlanner::Ptr planner_;
std::vector<nav_core::RecoveryBehavior::Ptr> recovery_behaviors_;
costmap_2d::Costmap2DROBOT *controller_costmap_robot_;
costmap_2d::Costmap2DROBOT *planner_costmap_robot_;
robot_costmap_2d::Costmap2DROBOT *controller_costmap_robot_;
robot_costmap_2d::Costmap2DROBOT *planner_costmap_robot_;
std::string robot_base_frame_, global_frame_;
std::vector<std::string> recovery_behavior_names_;
unsigned int recovery_index_;

46
src/Navigations/Packages/move_base/src/move_base.cpp
View File

@@ -9,15 +9,15 @@
#include <stdexcept>
#include <cstdlib>
#include <data_convert/data_convert.h>
#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <boost/algorithm/string.hpp>
#include <boost/thread.hpp>
#include <boost/dll/import.hpp>
#include <boost/dll/alias.hpp>
#include <robot/plugin_loader_helper.h>
#include <costmap_2d/static_layer.h>
#include <costmap_2d/voxel_layer.h>
#include <robot_costmap_2d/static_layer.h>
#include <robot_costmap_2d/voxel_layer.h>
#include <robot_nav_msgs/OccupancyGrid.h>
#include <robot_sensor_msgs/LaserScan.h>
@@ -177,14 +177,14 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
// create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
try
{
planner_costmap_robot_ = new costmap_2d::Costmap2DROBOT("global_costmap", *tf_);
planner_costmap_robot_ = new robot_costmap_2d::Costmap2DROBOT("global_costmap", *tf_);
planner_costmap_robot_->pause();
costmap_2d::LayeredCostmap* layered_costmap_ = planner_costmap_robot_->getLayeredCostmap();
for (std::vector<boost::shared_ptr<costmap_2d::Layer>>::const_iterator layer = layered_costmap_->getPlugins()->begin();
robot_costmap_2d::LayeredCostmap* layered_costmap_ = planner_costmap_robot_->getLayeredCostmap();
for (std::vector<boost::shared_ptr<robot_costmap_2d::Layer>>::const_iterator layer = layered_costmap_->getPlugins()->begin();
layer != layered_costmap_->getPlugins()->end();
++layer)
{
boost::shared_ptr<costmap_2d::StaticLayer> static_layer = boost::dynamic_pointer_cast<costmap_2d::StaticLayer>(*layer);
boost::shared_ptr<robot_costmap_2d::StaticLayer> static_layer = boost::dynamic_pointer_cast<robot_costmap_2d::StaticLayer>(*layer);
if (!static_layer)
continue;
robot_nav_msgs::OccupancyGrid *occupancy_grid = new robot_nav_msgs::OccupancyGrid();
@@ -203,7 +203,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
occupancy_grid->data.resize(occupancy_grid->info.width * occupancy_grid->info.height);
for (int i = 0; i < occupancy_grid->data.size(); i++)
{
occupancy_grid->data[i] = costmap_2d::NO_INFORMATION;
occupancy_grid->data[i] = robot_costmap_2d::NO_INFORMATION;
}
if (occupancy_grid)
static_layer->dataCallBack<robot_nav_msgs::OccupancyGrid>(*occupancy_grid, "map");
@@ -241,14 +241,14 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
try
{
controller_costmap_robot_ = new costmap_2d::Costmap2DROBOT("local_costmap", *tf_);
controller_costmap_robot_ = new robot_costmap_2d::Costmap2DROBOT("local_costmap", *tf_);
controller_costmap_robot_->pause();
costmap_2d::LayeredCostmap* layered_costmap_ = controller_costmap_robot_->getLayeredCostmap();
for (std::vector<boost::shared_ptr<costmap_2d::Layer>>::const_iterator layer = layered_costmap_->getPlugins()->begin();
robot_costmap_2d::LayeredCostmap* layered_costmap_ = controller_costmap_robot_->getLayeredCostmap();
for (std::vector<boost::shared_ptr<robot_costmap_2d::Layer>>::const_iterator layer = layered_costmap_->getPlugins()->begin();
layer != layered_costmap_->getPlugins()->end();
++layer)
{
boost::shared_ptr<costmap_2d::VoxelLayer> obstacle_layer = boost::dynamic_pointer_cast<costmap_2d::VoxelLayer>(*layer);
boost::shared_ptr<robot_costmap_2d::VoxelLayer> obstacle_layer = boost::dynamic_pointer_cast<robot_costmap_2d::VoxelLayer>(*layer);
if (!obstacle_layer)
continue;
robot_sensor_msgs::LaserScan *laser_scan = new robot_sensor_msgs::LaserScan();
@@ -401,7 +401,7 @@ bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, d
{
throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
}
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
try
{
@@ -496,7 +496,7 @@ bool move_base::MoveBase::dockTo(const std::string &maker, const robot_geometry_
{
throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
}
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
try
{
tc_->swapPlanner(docking_planner_name_);
@@ -559,7 +559,7 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped
throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
}
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
tc_->swapPlanner(go_straight_planner_name_);
if (nav_feedback_)
@@ -608,7 +608,7 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal,
{
throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
}
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
tc_->swapPlanner(rotate_planner_name_);
if (nav_feedback_)
@@ -640,7 +640,7 @@ void move_base::MoveBase::pause()
{
throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
}
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
if (!tc_)
{
throw std::runtime_error("Null 'tc_' pointer encountered");
@@ -668,7 +668,7 @@ void move_base::MoveBase::resume()
{
throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
}
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
if (!tc_)
{
throw std::runtime_error("Null 'tc_' pointer encountered");
@@ -689,7 +689,7 @@ void move_base::MoveBase::cancel()
{
throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
}
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
cancel_ctr_ = true;
}
@@ -811,7 +811,7 @@ bool move_base::MoveBase::makePlan(const robot_geometry_msgs::PoseStamped &goal,
return false;
}
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(planner_costmap_robot_->getCostmap()->getMutex()));
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(planner_costmap_robot_->getCostmap()->getMutex()));
// get the starting pose of the robot
robot_geometry_msgs::PoseStamped global_pose;
@@ -1028,7 +1028,7 @@ void move_base::MoveBase::clearCostmapWindows(double size_x, double size_y)
pt.y = y + size_y / 2;
clear_poly.push_back(pt);
planner_costmap_robot_->getCostmap()->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE);
planner_costmap_robot_->getCostmap()->setConvexPolygonCost(clear_poly, robot_costmap_2d::FREE_SPACE);
// clear the controller's costmap
getRobotPose(global_pose, controller_costmap_robot_);
@@ -1053,7 +1053,7 @@ void move_base::MoveBase::clearCostmapWindows(double size_x, double size_y)
pt.y = y + size_y / 2;
clear_poly.push_back(pt);
controller_costmap_robot_->getCostmap()->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE);
controller_costmap_robot_->getCostmap()->setConvexPolygonCost(clear_poly, robot_costmap_2d::FREE_SPACE);
}
void move_base::MoveBase::publishZeroVelocity()
@@ -1219,7 +1219,7 @@ bool move_base::MoveBase::isQuaternionValid(const robot_geometry_msgs::Quaternio
return true;
}
bool move_base::MoveBase::getRobotPose(robot_geometry_msgs::PoseStamped &global_pose, costmap_2d::Costmap2DROBOT *costmap)
bool move_base::MoveBase::getRobotPose(robot_geometry_msgs::PoseStamped &global_pose, robot_costmap_2d::Costmap2DROBOT *costmap)
{
tf3::toMsg(tf3::Transform::getIdentity(), global_pose.pose);