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

Compare commits

base: HiepLM:1de5a35cc7985bc9890e8de2542f85d8ab90e2d7
Branches Tags
HiepLM:3.0
..
compare: HiepLM:307a9c84f958a16964b5bce7106aa13ff26671cf
Branches Tags
HiepLM:3.0

No commits in common. "1de5a35cc7985bc9890e8de2542f85d8ab90e2d7" and "307a9c84f958a16964b5bce7106aa13ff26671cf" have entirely different histories.
1de5a35cc7 ... 307a9c84f9

25 changed files with 149 additions and 196 deletions
Show Stats Expand all files Collapse all files

12
CMakeLists.txt
View File

@ -141,14 +141,14 @@ endif()
# 2. Main packages (phụ thuộc vào cores) # 2. Main packages (phụ thuộc vào cores)
message(STATUS "[move_base] Shared library configured") message(STATUS "[move_base] Shared library configured")
if (NOT TARGET move_base) # if (NOT TARGET move_base)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Packages/move_base) # add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Packages/move_base)
endif() # endif()
# C API for .NET/C# integration # C API for .NET/C# integration
if (NOT TARGET navigation_c_api) # if (NOT TARGET navigation_c_api)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/APIs/c_api) # add_subdirectory(${CMAKE_SOURCE_DIR}/src/APIs/c_api)
endif() # endif()
message(STATUS "========================================") message(STATUS "========================================")
message(STATUS "All packages configured successfully") message(STATUS "All packages configured successfully")

7
src/APIs/c_api/CMakeLists.txt
View File

@ -10,16 +10,12 @@ if(NOT CMAKE_BUILD_TYPE)
set(CMAKE_BUILD_TYPE Release) set(CMAKE_BUILD_TYPE Release)
endif() endif()
# Find Boost
find_package(Boost REQUIRED)
# Dependencies # Dependencies
set(PACKAGES_DIR set(PACKAGES_DIR
move_base_core move_base_core
move_base move_base
tf3 tf3
robot_time robot_time
robot_cpp
geometry_msgs geometry_msgs
) )
@ -39,9 +35,6 @@ add_library(nav_c_api SHARED ${SOURCES} ${HEADERS})
target_link_libraries(nav_c_api target_link_libraries(nav_c_api
PUBLIC PUBLIC
${PACKAGES_DIR} ${PACKAGES_DIR}
PRIVATE
Boost::boost
dl
) )
set_target_properties(nav_c_api PROPERTIES set_target_properties(nav_c_api PROPERTIES

46
src/APIs/c_api/src/nav_c_api.cpp
View File

@ -66,7 +66,7 @@ namespace {
} }
// Convert C++ NavFeedback to C NavFeedback // Convert C++ NavFeedback to C NavFeedback
void cpp_to_c_nav_feedback(const robot::move_base_core::NavFeedback& cpp_feedback, NavFeedback* c_feedback) { void cpp_to_c_nav_feedback(const move_base_core::NavFeedback& cpp_feedback, NavFeedback* c_feedback) {
c_feedback->navigation_state = static_cast<NavigationState>(static_cast<int>(cpp_feedback.navigation_state)); c_feedback->navigation_state = static_cast<NavigationState>(static_cast<int>(cpp_feedback.navigation_state));
if (!cpp_feedback.feed_back_str.empty()) { if (!cpp_feedback.feed_back_str.empty()) {
c_feedback->feed_back_str = strdup(cpp_feedback.feed_back_str.c_str()); c_feedback->feed_back_str = strdup(cpp_feedback.feed_back_str.c_str());
@ -96,8 +96,8 @@ extern "C" void nav_c_api_free_string(char* str) {
// ============================================================================ // ============================================================================
extern "C" char* navigation_state_to_string(NavigationState state) { extern "C" char* navigation_state_to_string(NavigationState state) {
robot::move_base_core::State cpp_state = static_cast<robot::move_base_core::State>(static_cast<int>(state)); move_base_core::State cpp_state = static_cast<move_base_core::State>(static_cast<int>(state));
std::string str = robot::move_base_core::toString(cpp_state); std::string str = move_base_core::toString(cpp_state);
return strdup(str.c_str()); return strdup(str.c_str());
} }
@ -117,7 +117,7 @@ extern "C" bool navigation_offset_goal_2d(double pose_x, double pose_y, double p
pose.y = pose_y; pose.y = pose_y;
pose.theta = pose_theta; pose.theta = pose_theta;
geometry_msgs::PoseStamped result = robot::move_base_core::offset_goal(pose, std::string(frame_id), offset_distance); geometry_msgs::PoseStamped result = move_base_core::offset_goal(pose, std::string(frame_id), offset_distance);
cpp_to_c_pose_stamped(result, out_goal); cpp_to_c_pose_stamped(result, out_goal);
return true; return true;
} }
@ -129,7 +129,7 @@ extern "C" bool navigation_offset_goal_stamped(const PoseStamped* in_pose, doubl
} }
geometry_msgs::PoseStamped cpp_pose = c_to_cpp_pose_stamped(in_pose); geometry_msgs::PoseStamped cpp_pose = c_to_cpp_pose_stamped(in_pose);
geometry_msgs::PoseStamped result = robot::move_base_core::offset_goal(cpp_pose, offset_distance); geometry_msgs::PoseStamped result = move_base_core::offset_goal(cpp_pose, offset_distance);
cpp_to_c_pose_stamped(result, out_goal); cpp_to_c_pose_stamped(result, out_goal);
return true; return true;
} }
@ -154,7 +154,7 @@ extern "C" NavigationHandle navigation_create(void) {
extern "C" void navigation_destroy(NavigationHandle handle) { extern "C" void navigation_destroy(NavigationHandle handle) {
if (handle) { if (handle) {
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle); move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
delete nav; delete nav;
} }
} }
@ -226,16 +226,16 @@ extern "C" bool navigation_initialize(NavigationHandle handle, TFListenerHandle
} }
try { try {
printf("[%s:%d]\n Initialize navigation\n", __FILE__, __LINE__); printf("[%s:%d]\n Initialize navigation\n", __FILE__, __LINE__);
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle); move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
auto* tf_ptr = static_cast<std::shared_ptr<tf3::BufferCore>*>(tf_handle); auto* tf_ptr = static_cast<std::shared_ptr<tf3::BufferCore>*>(tf_handle);
robot::PluginLoaderHelper loader; robot::PluginLoaderHelper loader;
std::string lib_path = loader.findLibraryPath("MoveBase"); std::string lib_path = loader.findLibraryPath("MoveBase");
auto create_plugin = boost::dll::import_alias<robot::move_base_core::BaseNavigation::Ptr()>( auto create_plugin = boost::dll::import_alias<move_base_core::BaseNavigation::Ptr()>(
lib_path, lib_path,
"MoveBase", "MoveBase",
boost::dll::load_mode::append_decorations); boost::dll::load_mode::append_decorations);
robot::move_base_core::BaseNavigation::Ptr nav_ptr = create_plugin(); move_base_core::BaseNavigation::Ptr nav_ptr = create_plugin();
printf("[%s:%d]\n Navigation created\n", __FILE__, __LINE__); printf("[%s:%d]\n Navigation created\n", __FILE__, __LINE__);
if (nav_ptr == nullptr) { if (nav_ptr == nullptr) {
printf("[%s:%d]\n Error: Failed to create navigation\n", __FILE__, __LINE__); printf("[%s:%d]\n Error: Failed to create navigation\n", __FILE__, __LINE__);
@ -259,7 +259,7 @@ extern "C" bool navigation_set_robot_footprint(NavigationHandle handle, const Po
} }
try { try {
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle); move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
std::vector<geometry_msgs::Point> footprint; std::vector<geometry_msgs::Point> footprint;
footprint.reserve(point_count); footprint.reserve(point_count);
for (size_t i = 0; i < point_count; ++i) { for (size_t i = 0; i < point_count; ++i) {
@ -283,7 +283,7 @@ extern "C" bool navigation_move_to(NavigationHandle handle, const PoseStamped* g
} }
try { try {
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle); move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal); geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
return nav->moveTo(cpp_goal, xy_goal_tolerance, yaw_goal_tolerance); return nav->moveTo(cpp_goal, xy_goal_tolerance, yaw_goal_tolerance);
} catch (...) { } catch (...) {
@ -299,7 +299,7 @@ extern "C" bool navigation_dock_to(NavigationHandle handle, const char* marker,
} }
try { try {
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle); move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal); geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
return nav->dockTo(std::string(marker), cpp_goal, xy_goal_tolerance, yaw_goal_tolerance); return nav->dockTo(std::string(marker), cpp_goal, xy_goal_tolerance, yaw_goal_tolerance);
} catch (...) { } catch (...) {
@ -314,7 +314,7 @@ extern "C" bool navigation_move_straight_to(NavigationHandle handle, const PoseS
} }
try { try {
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle); move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal); geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
return nav->moveStraightTo(cpp_goal, xy_goal_tolerance); return nav->moveStraightTo(cpp_goal, xy_goal_tolerance);
} catch (...) { } catch (...) {
@ -329,7 +329,7 @@ extern "C" bool navigation_rotate_to(NavigationHandle handle, const PoseStamped*
} }
try { try {
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle); move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal); geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
return nav->rotateTo(cpp_goal, yaw_goal_tolerance); return nav->rotateTo(cpp_goal, yaw_goal_tolerance);
} catch (...) { } catch (...) {
@ -340,7 +340,7 @@ extern "C" bool navigation_rotate_to(NavigationHandle handle, const PoseStamped*
extern "C" void navigation_pause(NavigationHandle handle) { extern "C" void navigation_pause(NavigationHandle handle) {
if (handle) { if (handle) {
try { try {
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle); move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
nav->pause(); nav->pause();
} catch (...) { } catch (...) {
// Ignore exceptions // Ignore exceptions
@ -351,7 +351,7 @@ extern "C" void navigation_pause(NavigationHandle handle) {
extern "C" void navigation_resume(NavigationHandle handle) { extern "C" void navigation_resume(NavigationHandle handle) {
if (handle) { if (handle) {
try { try {
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle); move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
nav->resume(); nav->resume();
} catch (...) { } catch (...) {
// Ignore exceptions // Ignore exceptions
@ -362,7 +362,7 @@ extern "C" void navigation_resume(NavigationHandle handle) {
extern "C" void navigation_cancel(NavigationHandle handle) { extern "C" void navigation_cancel(NavigationHandle handle) {
if (handle) { if (handle) {
try { try {
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle); move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
nav->cancel(); nav->cancel();
} catch (...) { } catch (...) {
// Ignore exceptions // Ignore exceptions
@ -377,7 +377,7 @@ extern "C" bool navigation_set_twist_linear(NavigationHandle handle,
} }
try { try {
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle); move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
geometry_msgs::Vector3 linear; geometry_msgs::Vector3 linear;
linear.x = linear_x; linear.x = linear_x;
linear.y = linear_y; linear.y = linear_y;
@ -395,7 +395,7 @@ extern "C" bool navigation_set_twist_angular(NavigationHandle handle,
} }
try { try {
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle); move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
geometry_msgs::Vector3 angular; geometry_msgs::Vector3 angular;
angular.x = angular_x; angular.x = angular_x;
angular.y = angular_y; angular.y = angular_y;
@ -412,7 +412,7 @@ extern "C" bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseS
} }
try { try {
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle); move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
geometry_msgs::PoseStamped cpp_pose; geometry_msgs::PoseStamped cpp_pose;
if (nav->getRobotPose(cpp_pose)) { if (nav->getRobotPose(cpp_pose)) {
cpp_to_c_pose_stamped(cpp_pose, out_pose); cpp_to_c_pose_stamped(cpp_pose, out_pose);
@ -430,7 +430,7 @@ extern "C" bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D* ou
} }
try { try {
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle); move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
geometry_msgs::Pose2D cpp_pose; geometry_msgs::Pose2D cpp_pose;
if (nav->getRobotPose(cpp_pose)) { if (nav->getRobotPose(cpp_pose)) {
cpp_to_c_pose_2d(cpp_pose, out_pose); cpp_to_c_pose_2d(cpp_pose, out_pose);
@ -448,8 +448,8 @@ extern "C" bool navigation_get_feedback(NavigationHandle handle, NavFeedback* ou
} }
try { try {
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle); move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
robot::move_base_core::NavFeedback cpp_feedback; move_base_core::NavFeedback cpp_feedback;
if (nav->getFeedback(cpp_feedback)) { if (nav->getFeedback(cpp_feedback)) {
cpp_to_c_nav_feedback(cpp_feedback, out_feedback); cpp_to_c_nav_feedback(cpp_feedback, out_feedback);
return true; return true;

2
src/Libraries/costmap_2d

@ -1 +1 @@
Subproject commit a28f05c6d5cb6cf2d8c4ffb52a637ecfa2878db2 Subproject commit 0344c31e5b4185da41bfb253db720d9c91b802bd

1
src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/bounds.h
View File

@ -39,6 +39,7 @@
#include <nav_core2/bounds.h> #include <nav_core2/bounds.h>
#include <vector> #include <vector>
using namespace robot;
/** /**
* @brief A set of utility functions for Bounds objects interacting with other messages/types * @brief A set of utility functions for Bounds objects interacting with other messages/types
*/ */

4
src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/conversions.h
View File

@ -98,8 +98,8 @@ namespace robot_nav_2d_utils
nav_msgs::MapMetaData infoToInfo(const nav_grid::NavGridInfo &info); nav_msgs::MapMetaData infoToInfo(const nav_grid::NavGridInfo &info);
// Bounds Transformations // Bounds Transformations
robot_nav_2d_msgs::UIntBounds toMsg(const nav_core2::UIntBounds &bounds); ::robot_nav_2d_msgs::UIntBounds toMsg(const robot::nav_core2::UIntBounds &bounds);
nav_core2::UIntBounds fromMsg(const robot_nav_2d_msgs::UIntBounds &msg); robot::nav_core2::UIntBounds fromMsg(const ::robot_nav_2d_msgs::UIntBounds &msg);
} // namespace robot_nav_2d_utils } // namespace robot_nav_2d_utils

2
src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/tf_help.h
View File

@ -83,7 +83,7 @@ namespace robot_nav_2d_utils
* @param frame_id Frame to transform the pose into * @param frame_id Frame to transform the pose into
* @return The resulting transformed pose * @return The resulting transformed pose
*/ */
geometry_msgs::Pose2D transformStampedPose(const TFListenerPtr tf, const ::robot_nav_2d_msgs::Pose2DStamped &pose, ::geometry_msgs::Pose2D transformStampedPose(const TFListenerPtr tf, const ::robot_nav_2d_msgs::Pose2DStamped &pose,
const ::std::string &frame_id); const ::std::string &frame_id);
} // namespace robot_nav_2d_utils } // namespace robot_nav_2d_utils

1
src/Libraries/robot_nav_2d_utils/src/bounds.cpp
View File

@ -38,6 +38,7 @@
#include <stdexcept> #include <stdexcept>
#include <vector> #include <vector>
using namespace robot;
namespace robot_nav_2d_utils namespace robot_nav_2d_utils
{ {
nav_core2::Bounds getFullBounds(const nav_grid::NavGridInfo &info) nav_core2::Bounds getFullBounds(const nav_grid::NavGridInfo &info)

6
src/Libraries/robot_nav_2d_utils/src/conversions.cpp
View File

@ -318,7 +318,7 @@ namespace robot_nav_2d_utils
return metadata; return metadata;
} }
robot_nav_2d_msgs::UIntBounds toMsg(const nav_core2::UIntBounds &bounds) robot_nav_2d_msgs::UIntBounds toMsg(const robot::nav_core2::UIntBounds &bounds)
{ {
robot_nav_2d_msgs::UIntBounds msg; robot_nav_2d_msgs::UIntBounds msg;
msg.min_x = bounds.getMinX(); msg.min_x = bounds.getMinX();
@ -328,9 +328,9 @@ namespace robot_nav_2d_utils
return msg; return msg;
} }
nav_core2::UIntBounds fromMsg(const robot_nav_2d_msgs::UIntBounds &msg) robot::nav_core2::UIntBounds fromMsg(const robot_nav_2d_msgs::UIntBounds &msg)
{ {
return nav_core2::UIntBounds(msg.min_x, msg.min_y, msg.max_x, msg.max_y); return robot::nav_core2::UIntBounds(msg.min_x, msg.min_y, msg.max_x, msg.max_y);
} }
} // namespace robot_nav_2d_utils } // namespace robot_nav_2d_utils

8
src/Navigations/Cores/move_base_core/include/move_base_core/common.h
View File

@ -11,8 +11,6 @@
#include <tf3/buffer_core.h> #include <tf3/buffer_core.h>
#include <memory> #include <memory>
namespace robot using TFListenerPtr = std::shared_ptr<tf3::BufferCore>;
{
using TFListenerPtr = std::shared_ptr<tf3::BufferCore>; #endif // NAV_CORE2_COMMON_H
} // namespace robot
#endif // NAV_CORE2_COMMON_H

23
src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h
View File

@ -8,7 +8,6 @@
#include <geometry_msgs/PoseStamped.h> #include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose2D.h> #include <geometry_msgs/Pose2D.h>
#include <geometry_msgs/Vector3.h> #include <geometry_msgs/Vector3.h>
#include <robot_nav_2d_msgs/Twist2DStamped.h>
// robot protocol msgs // robot protocol msgs
#include <robot_protocol_msgs/Order.h> #include <robot_protocol_msgs/Order.h>
@ -20,9 +19,6 @@
#include <robot/time.h> #include <robot/time.h>
#include <move_base_core/common.h> #include <move_base_core/common.h>
namespace robot
{
namespace move_base_core namespace move_base_core
{ {
// Navigation states, including planning and controller status // Navigation states, including planning and controller status
@ -303,28 +299,13 @@ namespace move_base_core
*/ */
virtual bool getRobotPose(geometry_msgs::Pose2D &pose) = 0; virtual bool getRobotPose(geometry_msgs::Pose2D &pose) = 0;
/**
* @brief Get the robot’s twist.
* @param twist Output parameter with the robot’s current twist.
* @return True if twist was successfully retrieved.
*/
virtual bool getTwist(robot_nav_2d_msgs::Twist2DStamped &twist) = 0;
/**
* @brief Get the navigation feedback.
* @param feedback Output parameter with the navigation feedback.
* @return True if feedback was successfully retrieved.
*/
virtual bool getFeedback(NavFeedback &feedback) = 0;
protected:
// Shared feedback data for navigation status tracking // Shared feedback data for navigation status tracking
std::shared_ptr<NavFeedback> nav_feedback_; std::shared_ptr<NavFeedback> nav_feedback_;
std::shared_ptr<robot_nav_2d_msgs::Twist2DStamped> twist_;
protected:
BaseNavigation() = default; BaseNavigation() = default;
}; };
} // namespace move_base_core } // namespace move_base_core
};
#endif // _MOVE_BASE_CORE_NAVIGATION_H_INCLUDED_ #endif // _MOVE_BASE_CORE_NAVIGATION_H_INCLUDED_

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

@ -27,7 +27,7 @@ target_link_libraries(nav_core2 INTERFACE
costmap_2d costmap_2d
tf3 tf3
nav_grid nav_grid
robot_nav_2d_msgs nav_2d_msgs
robot_cpp robot_cpp
) )

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

@ -2,7 +2,7 @@
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. 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. 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 `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 `costmap_2d::Costmap2DROBOT`.
* Provide more data in the interfaces for easier testing. * Provide more data in the interfaces for easier testing.
* Use Exceptions rather than booleans to provide more information about the types of errors encountered. * Use Exceptions rather than booleans to provide more information about the types of errors encountered.
@ -30,7 +30,7 @@ Let us compare the old [nav_core::BaseGlobalPlanner](https://github.com/ros-plan
| `nav_core` | `nav_core2` | comments | | `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, 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 geometry_msgs::PoseStamped&, const geometry_msgs::PoseStamped&, std::vector<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.| |`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 ## 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). 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).
@ -38,10 +38,10 @@ Now let's compare the old [nav_core::BaseLocalPlanner](https://github.com/ros-pl
| `nav_core` | `nav_core2` | comments | | `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*, 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` |(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 robot_nav_2d_msgs::Path2D&)`|| |`bool setPlan(const std::vector<geometry_msgs::PoseStamped>&)`|`setPlan(const nav_2d_msgs::Path2D&)`||
|`bool computeVelocityCommands(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.| |`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 robot_nav_2d_msgs::Pose2DStamped&, const robot_nav_2d_msgs::Twist2D&)` | Explicitly provide the current pose and velocity for more explicit data control and easier testing. | |`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 ## 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. 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.

16
src/Navigations/Cores/nav_core2/include/nav_core2/exceptions.h
View File

@ -34,7 +34,7 @@
#ifndef NAV_CORE2_EXCEPTIONS_H #ifndef NAV_CORE2_EXCEPTIONS_H
#define NAV_CORE2_EXCEPTIONS_H #define NAV_CORE2_EXCEPTIONS_H
#include <robot_nav_2d_msgs/Pose2DStamped.h> #include <nav_2d_msgs/Pose2DStamped.h>
#include <stdexcept> #include <stdexcept>
#include <exception> #include <exception>
#include <string> #include <string>
@ -69,7 +69,7 @@
namespace nav_core2 namespace nav_core2
{ {
inline std::string poseToString(const robot_nav_2d_msgs::Pose2DStamped& pose) 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) return "(" + std::to_string(pose.pose.x) + ", " + std::to_string(pose.pose.y) + ", " + std::to_string(pose.pose.theta)
+ " : " + pose.header.frame_id + ")"; + " : " + pose.header.frame_id + ")";
@ -199,7 +199,7 @@ class InvalidStartPoseException: public GlobalPlannerException
public: public:
explicit InvalidStartPoseException(const std::string& description, int result_code = 5) explicit InvalidStartPoseException(const std::string& description, int result_code = 5)
: GlobalPlannerException(description, result_code) {} : GlobalPlannerException(description, result_code) {}
InvalidStartPoseException(const robot_nav_2d_msgs::Pose2DStamped& pose, const std::string& problem, int result_code = 5) : 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) {} InvalidStartPoseException("The starting pose " + poseToString(pose) + " is " + problem, result_code) {}
}; };
@ -212,7 +212,7 @@ class StartBoundsException: public InvalidStartPoseException
public: public:
explicit StartBoundsException(const std::string& description, int result_code = 6) explicit StartBoundsException(const std::string& description, int result_code = 6)
: InvalidStartPoseException(description, result_code) {} : InvalidStartPoseException(description, result_code) {}
explicit StartBoundsException(const robot_nav_2d_msgs::Pose2DStamped& pose) : explicit StartBoundsException(const nav_2d_msgs::Pose2DStamped& pose) :
InvalidStartPoseException(pose, "out of bounds", 6) {} InvalidStartPoseException(pose, "out of bounds", 6) {}
}; };
@ -225,7 +225,7 @@ class OccupiedStartException: public InvalidStartPoseException
public: public:
explicit OccupiedStartException(const std::string& description, int result_code = 7) explicit OccupiedStartException(const std::string& description, int result_code = 7)
: InvalidStartPoseException(description, result_code) {} : InvalidStartPoseException(description, result_code) {}
explicit OccupiedStartException(const robot_nav_2d_msgs::Pose2DStamped& pose) : explicit OccupiedStartException(const nav_2d_msgs::Pose2DStamped& pose) :
InvalidStartPoseException(pose, "occupied", 7) {} InvalidStartPoseException(pose, "occupied", 7) {}
}; };
@ -238,7 +238,7 @@ class InvalidGoalPoseException: public GlobalPlannerException
public: public:
explicit InvalidGoalPoseException(const std::string& description, int result_code = 8) explicit InvalidGoalPoseException(const std::string& description, int result_code = 8)
: GlobalPlannerException(description, result_code) {} : GlobalPlannerException(description, result_code) {}
InvalidGoalPoseException(const robot_nav_2d_msgs::Pose2DStamped& pose, const std::string& problem, int result_code = 8) : 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) {} GlobalPlannerException("The goal pose " + poseToString(pose) + " is " + problem, result_code) {}
}; };
@ -251,7 +251,7 @@ class GoalBoundsException: public InvalidGoalPoseException
public: public:
explicit GoalBoundsException(const std::string& description, int result_code = 9) explicit GoalBoundsException(const std::string& description, int result_code = 9)
: InvalidGoalPoseException(description, result_code) {} : InvalidGoalPoseException(description, result_code) {}
explicit GoalBoundsException(const robot_nav_2d_msgs::Pose2DStamped& pose) : explicit GoalBoundsException(const nav_2d_msgs::Pose2DStamped& pose) :
InvalidGoalPoseException(pose, "out of bounds", 9) {} InvalidGoalPoseException(pose, "out of bounds", 9) {}
}; };
@ -264,7 +264,7 @@ class OccupiedGoalException: public InvalidGoalPoseException
public: public:
explicit OccupiedGoalException(const std::string& description, int result_code = 10) explicit OccupiedGoalException(const std::string& description, int result_code = 10)
: InvalidGoalPoseException(description, result_code) {} : InvalidGoalPoseException(description, result_code) {}
explicit OccupiedGoalException(const robot_nav_2d_msgs::Pose2DStamped& pose) : explicit OccupiedGoalException(const nav_2d_msgs::Pose2DStamped& pose) :
InvalidGoalPoseException(pose, "occupied", 10) {} InvalidGoalPoseException(pose, "occupied", 10) {}
}; };

8
src/Navigations/Cores/nav_core2/include/nav_core2/global_planner.h
View File

@ -36,8 +36,8 @@
#include <nav_core2/common.h> #include <nav_core2/common.h>
#include <nav_core2/costmap.h> #include <nav_core2/costmap.h>
#include <robot_nav_2d_msgs/Path2D.h> #include <nav_2d_msgs/Path2D.h>
#include <robot_nav_2d_msgs/Pose2DStamped.h> #include <nav_2d_msgs/Pose2DStamped.h>
#include <string> #include <string>
#include <memory> #include <memory>
@ -77,8 +77,8 @@ public:
* @param goal The goal 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 * @return The sequence of poses to get from start to goal, if any
*/ */
virtual robot_nav_2d_msgs::Path2D makePlan(const robot_nav_2d_msgs::Pose2DStamped& start, virtual nav_2d_msgs::Path2D makePlan(const nav_2d_msgs::Pose2DStamped& start,
const robot_nav_2d_msgs::Pose2DStamped& goal) = 0; const nav_2d_msgs::Pose2DStamped& goal) = 0;
}; };
} // namespace nav_core2 } // namespace nav_core2

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

@ -36,10 +36,10 @@
#include <nav_core2/common.h> #include <nav_core2/common.h>
#include <nav_core2/costmap.h> #include <nav_core2/costmap.h>
#include <robot_nav_2d_msgs/Path2D.h> #include <nav_2d_msgs/Path2D.h>
#include <robot_nav_2d_msgs/Pose2DStamped.h> #include <nav_2d_msgs/Pose2DStamped.h>
#include <robot_nav_2d_msgs/Twist2D.h> #include <nav_2d_msgs/Twist2D.h>
#include <robot_nav_2d_msgs/Twist2DStamped.h> #include <nav_2d_msgs/Twist2DStamped.h>
#include <geometry_msgs/Twist.h> #include <geometry_msgs/Twist.h>
#include <string> #include <string>
#include <costmap_2d/costmap_2d_robot.h> #include <costmap_2d/costmap_2d_robot.h>
@ -83,14 +83,14 @@ namespace nav_core2
* @brief Sets the global goal for this local planner. * @brief Sets the global goal for this local planner.
* @param goal_pose The Goal Pose * @param goal_pose The Goal Pose
*/ */
virtual void setGoalPose(const robot_nav_2d_msgs::Pose2DStamped &goal_pose) = 0; virtual void setGoalPose(const nav_2d_msgs::Pose2DStamped &goal_pose) = 0;
/** /**
* @brief Sets the global plan for this local planner. * @brief Sets the global plan for this local planner.
* *
* @param path The global plan * @param path The global plan
*/ */
virtual void setPlan(const robot_nav_2d_msgs::Path2D &path) = 0; virtual void setPlan(const nav_2d_msgs::Path2D &path) = 0;
/** /**
* @brief Compute the best command given the current pose, velocity and goal * @brief Compute the best command given the current pose, velocity and goal
@ -102,8 +102,8 @@ namespace nav_core2
* @param velocity Current robot velocity * @param velocity Current robot velocity
* @return The best computed velocity * @return The best computed velocity
*/ */
virtual robot_nav_2d_msgs::Twist2DStamped computeVelocityCommands(const robot_nav_2d_msgs::Pose2DStamped &pose, virtual nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped &pose,
const robot_nav_2d_msgs::Twist2D &velocity) = 0; const nav_2d_msgs::Twist2D &velocity) = 0;
/** /**
* @brief set velocity for x, y asix of the robot * @brief set velocity for x, y asix of the robot
@ -166,7 +166,7 @@ namespace nav_core2
* @param velocity Velocity to check * @param velocity Velocity to check
* @return True if the goal conditions have been met * @return True if the goal conditions have been met
*/ */
virtual bool isGoalReached(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) = 0; virtual bool isGoalReached(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) = 0;
}; };
} // namespace nav_core2 } // namespace nav_core2

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

@ -11,7 +11,7 @@ if(NOT CMAKE_BUILD_TYPE)
set(CMAKE_BUILD_TYPE Release) set(CMAKE_BUILD_TYPE Release)
endif() endif()
set(PACKAGES_DIR geometry_msgs std_msgs nav_core nav_core2 robot_nav_2d_utils pthread) set(PACKAGES_DIR geometry_msgs std_msgs nav_core nav_core2 nav_2d_utils pthread)
find_package(Boost REQUIRED COMPONENTS filesystem system) find_package(Boost REQUIRED COMPONENTS filesystem system)

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

@ -57,8 +57,8 @@ public:
// Nav Core 2 Global Planner Interface // Nav Core 2 Global Planner Interface
void initialize(const robot::NodeHandle& parent, const std::string& name, void initialize(const robot::NodeHandle& parent, const std::string& name,
TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override; TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
robot_nav_2d_msgs::Path2D makePlan(const robot_nav_2d_msgs::Pose2DStamped& start, nav_2d_msgs::Path2D makePlan(const nav_2d_msgs::Pose2DStamped& start,
const robot_nav_2d_msgs::Pose2DStamped& goal) override; const nav_2d_msgs::Pose2DStamped& goal) override;
static nav_core2::GlobalPlanner::Ptr create(); static nav_core2::GlobalPlanner::Ptr create();

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

@ -39,7 +39,7 @@
#include <nav_core/base_local_planner.h> #include <nav_core/base_local_planner.h>
#include <nav_core2/local_planner.h> #include <nav_core2/local_planner.h>
#include <nav_core_adapter/costmap_adapter.h> #include <nav_core_adapter/costmap_adapter.h>
#include <robot_nav_2d_utils/odom_subscriber.h> #include <nav_2d_utils/odom_subscriber.h>
#include <boost/dll/import.hpp> #include <boost/dll/import.hpp>
#include <memory> #include <memory>
#include <string> #include <string>
@ -168,22 +168,22 @@ namespace nav_core_adapter
/** /**
* @brief Get the robot pose from the costmap and store as Pose2DStamped * @brief Get the robot pose from the costmap and store as Pose2DStamped
*/ */
bool getRobotPose(robot_nav_2d_msgs::Pose2DStamped &pose2d); bool getRobotPose(nav_2d_msgs::Pose2DStamped &pose2d);
/** /**
* @brief See if the back of the global plan matches the most recent goal pose * @brief See if the back of the global plan matches the most recent goal pose
* @return True if the plan has changed * @return True if the plan has changed
*/ */
bool hasGoalChanged(const robot_nav_2d_msgs::Pose2DStamped &new_goal); bool hasGoalChanged(const nav_2d_msgs::Pose2DStamped &new_goal);
// The most recent goal pose // The most recent goal pose
robot_nav_2d_msgs::Pose2DStamped last_goal_; nav_2d_msgs::Pose2DStamped last_goal_;
bool has_active_goal_; bool has_active_goal_;
/** /**
* @brief helper class for subscribing to odometry * @brief helper class for subscribing to odometry
*/ */
std::shared_ptr<robot_nav_2d_utils::OdomSubscriber> odom_sub_; std::shared_ptr<nav_2d_utils::OdomSubscriber> odom_sub_;
// Plugin handling // Plugin handling
boost::function<nav_core2::LocalPlanner::Ptr()> planner_loader_; boost::function<nav_core2::LocalPlanner::Ptr()> planner_loader_;

14
src/Navigations/Cores/nav_core_adapter/src/global_planner_adapter.cpp
View File

@ -34,8 +34,8 @@
#include <nav_core_adapter/global_planner_adapter.h> #include <nav_core_adapter/global_planner_adapter.h>
#include <nav_core_adapter/costmap_adapter.h> #include <nav_core_adapter/costmap_adapter.h>
#include <robot_nav_2d_utils/conversions.h> #include <nav_2d_utils/conversions.h>
#include <robot_nav_2d_utils/tf_help.h> #include <nav_2d_utils/tf_help.h>
#include <nav_core2/exceptions.h> #include <nav_core2/exceptions.h>
#include <boost/dll/alias.hpp> #include <boost/dll/alias.hpp>
#include <memory> #include <memory>
@ -78,18 +78,18 @@ namespace nav_core_adapter
planner_->initialize(planner_name, costmap_robot_); planner_->initialize(planner_name, costmap_robot_);
} }
robot_nav_2d_msgs::Path2D GlobalPlannerAdapter::makePlan(const robot_nav_2d_msgs::Pose2DStamped& start, nav_2d_msgs::Path2D GlobalPlannerAdapter::makePlan(const nav_2d_msgs::Pose2DStamped& start,
const robot_nav_2d_msgs::Pose2DStamped& goal) const nav_2d_msgs::Pose2DStamped& goal)
{ {
geometry_msgs::PoseStamped start3d = robot_nav_2d_utils::pose2DToPoseStamped(start), geometry_msgs::PoseStamped start3d = nav_2d_utils::pose2DToPoseStamped(start),
goal3d = robot_nav_2d_utils::pose2DToPoseStamped(goal); goal3d = nav_2d_utils::pose2DToPoseStamped(goal);
std::vector<geometry_msgs::PoseStamped> plan; std::vector<geometry_msgs::PoseStamped> plan;
bool ret = planner_->makePlan(start3d, goal3d, plan); bool ret = planner_->makePlan(start3d, goal3d, plan);
if (!ret) if (!ret)
{ {
throw nav_core2::PlannerException("Generic Global Planner Error"); throw nav_core2::PlannerException("Generic Global Planner Error");
} }
return robot_nav_2d_utils::posesToPath2D(plan); return nav_2d_utils::posesToPath2D(plan);
} }
nav_core2::GlobalPlanner::Ptr GlobalPlannerAdapter::create() nav_core2::GlobalPlanner::Ptr GlobalPlannerAdapter::create()

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

@ -36,9 +36,9 @@
#include <nav_core_adapter/local_planner_adapter.h> #include <nav_core_adapter/local_planner_adapter.h>
#include <nav_core_adapter/costmap_adapter.h> #include <nav_core_adapter/costmap_adapter.h>
#include <nav_core_adapter/shared_pointers.h> #include <nav_core_adapter/shared_pointers.h>
#include <robot_nav_2d_utils/conversions.h> #include <nav_2d_utils/conversions.h>
#include <robot_nav_2d_utils/tf_help.h> #include <nav_2d_utils/tf_help.h>
#include <robot_nav_2d_utils/parameters.h> #include <nav_2d_utils/parameters.h>
#include <nav_core2/exceptions.h> #include <nav_core2/exceptions.h>
#include <boost/dll/alias.hpp> #include <boost/dll/alias.hpp>
#include <boost/dll/import.hpp> #include <boost/dll/import.hpp>
@ -90,7 +90,7 @@ namespace nav_core_adapter
} }
else else
{ {
planner_name = robot_nav_2d_utils::searchAndGetParam(adapter_nh_, "position_planner_name", std::string("dwb_local_planner::DWBLocalPlanner")); planner_name = nav_2d_utils::searchAndGetParam(adapter_nh_, "position_planner_name", std::string("dwb_local_planner::DWBLocalPlanner"));
adapter_nh_.setParam("planner_name", planner_name); adapter_nh_.setParam("planner_name", planner_name);
} }
try try
@ -174,15 +174,15 @@ namespace nav_core_adapter
} }
// Get the Pose // Get the Pose
robot_nav_2d_msgs::Pose2DStamped pose2d; nav_2d_msgs::Pose2DStamped pose2d;
if (!getRobotPose(pose2d)) if (!getRobotPose(pose2d))
{ {
return false; return false;
} }
// Get the Velocity // Get the Velocity
robot_nav_2d_msgs::Twist2D velocity = odom_sub_->getTwist(); nav_2d_msgs::Twist2D velocity = odom_sub_->getTwist();
robot_nav_2d_msgs::Twist2DStamped cmd_vel_2d; nav_2d_msgs::Twist2DStamped cmd_vel_2d;
try try
{ {
cmd_vel_2d = planner_->computeVelocityCommands(pose2d, velocity); cmd_vel_2d = planner_->computeVelocityCommands(pose2d, velocity);
@ -192,7 +192,7 @@ namespace nav_core_adapter
std::cerr << "computeVelocityCommands exception: " << e.what() << std::endl; std::cerr << "computeVelocityCommands exception: " << e.what() << std::endl;
return false; return false;
} }
cmd_vel = robot_nav_2d_utils::twist2Dto3D(cmd_vel_2d.velocity); cmd_vel = nav_2d_utils::twist2Dto3D(cmd_vel_2d.velocity);
return true; return true;
} }
@ -263,7 +263,7 @@ namespace nav_core_adapter
geometry_msgs::Twist LocalPlannerAdapter::getActualTwist() geometry_msgs::Twist LocalPlannerAdapter::getActualTwist()
{ {
if (odom_sub_) if (odom_sub_)
return robot_nav_2d_utils::twist2Dto3D(odom_sub_->getTwist()); return nav_2d_utils::twist2Dto3D(odom_sub_->getTwist());
else else
throw std::runtime_error("Failed to get twist"); throw std::runtime_error("Failed to get twist");
} }
@ -326,11 +326,11 @@ namespace nav_core_adapter
{ {
boost::recursive_mutex::scoped_lock ecl(configuration_mutex_); boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
// Get the Pose // Get the Pose
robot_nav_2d_msgs::Pose2DStamped pose2d; nav_2d_msgs::Pose2DStamped pose2d;
if (!getRobotPose(pose2d)) if (!getRobotPose(pose2d))
return false; return false;
robot_nav_2d_msgs::Twist2D velocity = odom_sub_->getTwist(); nav_2d_msgs::Twist2D velocity = odom_sub_->getTwist();
bool ret = planner_->isGoalReached(pose2d, velocity); bool ret = planner_->isGoalReached(pose2d, velocity);
if (ret) if (ret)
{ {
@ -345,12 +345,12 @@ namespace nav_core_adapter
bool LocalPlannerAdapter::setPlan(const std::vector<geometry_msgs::PoseStamped> &orig_global_plan) bool LocalPlannerAdapter::setPlan(const std::vector<geometry_msgs::PoseStamped> &orig_global_plan)
{ {
boost::recursive_mutex::scoped_lock ecl(configuration_mutex_); boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
robot_nav_2d_msgs::Path2D path = robot_nav_2d_utils::posesToPath2D(orig_global_plan); nav_2d_msgs::Path2D path = nav_2d_utils::posesToPath2D(orig_global_plan);
try try
{ {
if (path.poses.size() > 0) if (path.poses.size() > 0)
{ {
robot_nav_2d_msgs::Pose2DStamped goal_pose; nav_2d_msgs::Pose2DStamped goal_pose;
goal_pose = path.poses.back(); goal_pose = path.poses.back();
if (!has_active_goal_ || hasGoalChanged(goal_pose)) if (!has_active_goal_ || hasGoalChanged(goal_pose))
@ -370,7 +370,7 @@ namespace nav_core_adapter
} }
} }
bool LocalPlannerAdapter::hasGoalChanged(const robot_nav_2d_msgs::Pose2DStamped &new_goal) bool LocalPlannerAdapter::hasGoalChanged(const nav_2d_msgs::Pose2DStamped &new_goal)
{ {
if (last_goal_.header.frame_id != new_goal.header.frame_id || if (last_goal_.header.frame_id != new_goal.header.frame_id ||
last_goal_.header.stamp.toSec() != new_goal.header.stamp.toSec()) last_goal_.header.stamp.toSec() != new_goal.header.stamp.toSec())
@ -382,7 +382,7 @@ namespace nav_core_adapter
last_goal_.pose.theta != new_goal.pose.theta; last_goal_.pose.theta != new_goal.pose.theta;
} }
bool LocalPlannerAdapter::getRobotPose(robot_nav_2d_msgs::Pose2DStamped &pose2d) bool LocalPlannerAdapter::getRobotPose(nav_2d_msgs::Pose2DStamped &pose2d)
{ {
geometry_msgs::PoseStamped current_pose; geometry_msgs::PoseStamped current_pose;
if (!costmap_robot_->getRobotPose(current_pose)) if (!costmap_robot_->getRobotPose(current_pose))
@ -390,7 +390,7 @@ namespace nav_core_adapter
std::cout << "Could not get robot pose" << std::endl; std::cout << "Could not get robot pose" << std::endl;
return false; return false;
} }
pose2d = robot_nav_2d_utils::poseStampedToPose2D(current_pose); pose2d = nav_2d_utils::poseStampedToPose2D(current_pose);
return true; return true;
} }

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

@ -148,7 +148,7 @@ else()
data_convert data_convert
dl dl
pthread pthread
robot_nav_2d_utils nav_2d_utils
) )
target_link_libraries(move_base target_link_libraries(move_base
@ -196,8 +196,8 @@ endif()
# Set RPATH for executable to find libraries in build directory first # Set RPATH for executable to find libraries in build directory first
# Use RPATH instead of RUNPATH for higher priority # Use RPATH instead of RUNPATH for higher priority
set_target_properties(move_base_main PROPERTIES set_target_properties(move_base_main PROPERTIES
BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Navigations/Packages/move_base:${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:${CMAKE_BINARY_DIR}/src/Libraries/robot_nav_2d_utils" BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Navigations/Packages/move_base:${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:${CMAKE_BINARY_DIR}/src/Libraries/nav_2d_utils"
INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Navigations/Packages/move_base:${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:${CMAKE_BINARY_DIR}/src/Libraries/robot_nav_2d_utils" INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Navigations/Packages/move_base:${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:${CMAKE_BINARY_DIR}/src/Libraries/nav_2d_utils"
BUILD_WITH_INSTALL_RPATH FALSE BUILD_WITH_INSTALL_RPATH FALSE
INSTALL_RPATH_USE_LINK_PATH TRUE INSTALL_RPATH_USE_LINK_PATH TRUE
SKIP_BUILD_RPATH FALSE SKIP_BUILD_RPATH FALSE

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

@ -58,7 +58,7 @@ namespace move_base
* @class MoveBase * @class MoveBase
* @brief A class that uses the actionlib::ActionServer interface that moves the robot base to a goal location. * @brief A class that uses the actionlib::ActionServer interface that moves the robot base to a goal location.
*/ */
class MoveBase : public robot::move_base_core::BaseNavigation class MoveBase : public move_base_core::BaseNavigation
{ {
public: public:
/** /**
@ -66,14 +66,14 @@ namespace move_base
* @param name The name of the action * @param name The name of the action
* @param tf A reference to a TransformListener * @param tf A reference to a TransformListener
*/ */
MoveBase(robot::TFListenerPtr tf); MoveBase(TFListenerPtr tf);
MoveBase(); MoveBase();
/** /**
* @brief Initialize the navigation system. * @brief Initialize the navigation system.
* @param tf Shared pointer to the TF listener or buffer. * @param tf Shared pointer to the TF listener or buffer.
*/ */
virtual void initialize(robot::TFListenerPtr tf) override; virtual void initialize(TFListenerPtr tf) override;
/** /**
* @brief Set the robot's footprint (outline shape) in the global frame. * @brief Set the robot's footprint (outline shape) in the global frame.
@ -196,40 +196,25 @@ namespace move_base
*/ */
virtual bool getRobotPose(geometry_msgs::Pose2D &pose) override; virtual bool getRobotPose(geometry_msgs::Pose2D &pose) override;
/**
* @brief Get the robot’s twist.
* @param twist Output parameter with the robot’s current twist.
* @return True if twist was successfully retrieved.
*/
virtual bool getTwist(robot_nav_2d_msgs::Twist2DStamped &twist) override;
/**
* @brief Get the navigation feedback.
* @param feedback Output parameter with the navigation feedback.
* @return True if feedback was successfully retrieved.
*/
virtual bool getFeedback(robot::move_base_core::NavFeedback &feedback) override;
/** /**
* @brief Destructor - Cleans up * @brief Destructor - Cleans up
*/ */
virtual ~MoveBase(); virtual ~MoveBase();
/**
* @brief Create a new MoveBase
* @return A shared pointer to the new MoveBase
*/
static robot::move_base_core::BaseNavigation::Ptr create();
private:
/** /**
* @brief Performs a control cycle * @brief Performs a control cycle
* @param goal A reference to the goal to pursue * @param goal A reference to the goal to pursue
* @return True if processing of the goal is done, false otherwise * @return True if processing of the goal is done, false otherwise
*/ */
bool executeCycle(geometry_msgs::PoseStamped &goal); bool executeCycle(geometry_msgs::PoseStamped &goal);
/**
* @brief Create a new MoveBase
* @return A shared pointer to the new MoveBase
*/
static move_base_core::BaseNavigation::Ptr create();
private:
/** /**
* @brief Set velocity for yaw goal tolerance of the robot * @brief Set velocity for yaw goal tolerance of the robot
@ -306,7 +291,7 @@ namespace move_base
private: private:
bool initialized_; bool initialized_;
robot::TFListenerPtr tf_; TFListenerPtr tf_;
robot::NodeHandle private_nh_; robot::NodeHandle private_nh_;
boost::function<nav_core::BaseGlobalPlanner::Ptr()> planner_loader_; boost::function<nav_core::BaseGlobalPlanner::Ptr()> planner_loader_;

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

@ -31,7 +31,7 @@ move_base::MoveBase::MoveBase()
{ {
} }
move_base::MoveBase::MoveBase(robot::TFListenerPtr tf) move_base::MoveBase::MoveBase(TFListenerPtr tf)
: initialized_(false), : initialized_(false),
planner_costmap_robot_(NULL), controller_costmap_robot_(NULL), planner_costmap_robot_(NULL), controller_costmap_robot_(NULL),
planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL), planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
@ -89,7 +89,7 @@ move_base::MoveBase::~MoveBase()
tf_.reset(); tf_.reset();
} }
void move_base::MoveBase::initialize(robot::TFListenerPtr tf) void move_base::MoveBase::initialize(TFListenerPtr tf)
{ {
printf("[%s:%d]\n ========== Begin: initialize() ==========\n", __FILE__, __LINE__); printf("[%s:%d]\n ========== Begin: initialize() ==========\n", __FILE__, __LINE__);
if (!initialized_) if (!initialized_)
@ -334,14 +334,14 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
// we'll start executing recovery behaviors at the beginning of our list // we'll start executing recovery behaviors at the beginning of our list
recovery_index_ = 0; recovery_index_ = 0;
nav_feedback_ = std::make_shared<robot::move_base_core::NavFeedback>(); nav_feedback_ = std::make_shared<move_base_core::NavFeedback>();
if (nav_feedback_) if (nav_feedback_)
{ {
nav_feedback_->navigation_state = robot::move_base_core::State::PLANNING; nav_feedback_->navigation_state = move_base_core::State::PLANNING;
nav_feedback_->is_ready = true; nav_feedback_->is_ready = true;
} }
else else
{ {
robot::log_error("[%s:%d]\n ERROR: nav_feedback_ is nullptr", __FILE__, __LINE__); robot::log_error("[%s:%d]\n ERROR: nav_feedback_ is nullptr", __FILE__, __LINE__);
} }
@ -377,7 +377,7 @@ bool move_base::MoveBase::moveTo(const geometry_msgs::PoseStamped &goal, double
{ {
if (nav_feedback_) if (nav_feedback_)
{ {
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; nav_feedback_->navigation_state = move_base_core::State::REJECTED;
nav_feedback_->feed_back_str = std::string("The system has not been installed yet"); nav_feedback_->feed_back_str = std::string("The system has not been installed yet");
nav_feedback_->goal_checked = false; nav_feedback_->goal_checked = false;
} }
@ -416,7 +416,7 @@ bool move_base::MoveBase::moveTo(const geometry_msgs::PoseStamped &goal, double
if (nav_feedback_) if (nav_feedback_)
{ {
nav_feedback_->navigation_state = robot::move_base_core::State::PLANNING; nav_feedback_->navigation_state = move_base_core::State::PLANNING;
nav_feedback_->feed_back_str = std::string("Planning"); nav_feedback_->feed_back_str = std::string("Planning");
nav_feedback_->goal_checked = false; nav_feedback_->goal_checked = false;
} }
@ -455,7 +455,7 @@ bool move_base::MoveBase::dockTo(const std::string &maker, const geometry_msgs::
{ {
if (nav_feedback_) if (nav_feedback_)
{ {
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; nav_feedback_->navigation_state = move_base_core::State::REJECTED;
std::stringstream fb_ss; std::stringstream fb_ss;
fb_ss << "The system has not been '" << maker << "'"; fb_ss << "The system has not been '" << maker << "'";
nav_feedback_->feed_back_str = fb_ss.str(); nav_feedback_->feed_back_str = fb_ss.str();
@ -472,7 +472,7 @@ bool move_base::MoveBase::dockTo(const std::string &maker, const geometry_msgs::
{ {
if (nav_feedback_) if (nav_feedback_)
{ {
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; nav_feedback_->navigation_state = move_base_core::State::REJECTED;
nav_feedback_->feed_back_str = std::string("The system has not been installed yet"); nav_feedback_->feed_back_str = std::string("The system has not been installed yet");
nav_feedback_->goal_checked = false; nav_feedback_->goal_checked = false;
} }
@ -510,7 +510,7 @@ bool move_base::MoveBase::dockTo(const std::string &maker, const geometry_msgs::
if (nav_feedback_) if (nav_feedback_)
{ {
nav_feedback_->navigation_state = robot::move_base_core::State::PLANNING; nav_feedback_->navigation_state = move_base_core::State::PLANNING;
nav_feedback_->feed_back_str = std::string("Planning"); nav_feedback_->feed_back_str = std::string("Planning");
nav_feedback_->goal_checked = false; nav_feedback_->goal_checked = false;
} }
@ -539,7 +539,7 @@ bool move_base::MoveBase::moveStraightTo(const geometry_msgs::PoseStamped &goal,
{ {
if (nav_feedback_) if (nav_feedback_)
{ {
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; nav_feedback_->navigation_state = move_base_core::State::REJECTED;
nav_feedback_->feed_back_str = std::string("The system has not been installed yet"); nav_feedback_->feed_back_str = std::string("The system has not been installed yet");
nav_feedback_->goal_checked = false; nav_feedback_->goal_checked = false;
} }
@ -565,7 +565,7 @@ bool move_base::MoveBase::moveStraightTo(const geometry_msgs::PoseStamped &goal,
if (nav_feedback_) if (nav_feedback_)
{ {
nav_feedback_->navigation_state = robot::move_base_core::State::PLANNING; nav_feedback_->navigation_state = move_base_core::State::PLANNING;
nav_feedback_->feed_back_str = std::string("Planning"); nav_feedback_->feed_back_str = std::string("Planning");
nav_feedback_->goal_checked = false; nav_feedback_->goal_checked = false;
} }
@ -587,7 +587,7 @@ bool move_base::MoveBase::rotateTo(const geometry_msgs::PoseStamped &goal, doubl
{ {
if (nav_feedback_) if (nav_feedback_)
{ {
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; nav_feedback_->navigation_state = move_base_core::State::REJECTED;
nav_feedback_->feed_back_str = std::string("The system has not been installed yet"); nav_feedback_->feed_back_str = std::string("The system has not been installed yet");
nav_feedback_->goal_checked = false; nav_feedback_->goal_checked = false;
} }
@ -614,7 +614,7 @@ bool move_base::MoveBase::rotateTo(const geometry_msgs::PoseStamped &goal, doubl
if (nav_feedback_) if (nav_feedback_)
{ {
nav_feedback_->navigation_state = robot::move_base_core::State::PLANNING; nav_feedback_->navigation_state = move_base_core::State::PLANNING;
nav_feedback_->feed_back_str = std::string("Planning"); nav_feedback_->feed_back_str = std::string("Planning");
nav_feedback_->goal_checked = false; nav_feedback_->goal_checked = false;
} }
@ -625,12 +625,20 @@ bool move_base::MoveBase::rotateTo(const geometry_msgs::PoseStamped &goal, doubl
{ {
if (nav_feedback_) if (nav_feedback_)
{ {
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; nav_feedback_->navigation_state = move_base_core::State::REJECTED;
nav_feedback_->feed_back_str = std::string("Coudn't get robot pose"); nav_feedback_->feed_back_str = std::string("Coudn't get robot pose");
nav_feedback_->goal_checked = false; nav_feedback_->goal_checked = false;
} }
return false; return false;
} }
// move_base_msgs::MoveBaseActionGoal action_goal;
// action_goal.header.stamp = robot::Time::now();
// action_goal.goal.target_pose = goal;
// action_goal.goal.target_pose.pose.position.x = pose.pose.position.x + 0.05 * cos(tf2::getYaw(pose.pose.orientation));
// action_goal.goal.target_pose.pose.position.y = pose.pose.position.y + 0.05 * sin(tf2::getYaw(pose.pose.orientation));
// action_goal_pub_.publish(action_goal);
lock.unlock(); lock.unlock();
return true; return true;
} }
@ -1275,7 +1283,10 @@ geometry_msgs::PoseStamped move_base::MoveBase::goalToGlobalFrame(const geometry
geometry_msgs::PoseStamped goal_pose, global_pose; geometry_msgs::PoseStamped goal_pose, global_pose;
goal_pose = goal_pose_msg; goal_pose = goal_pose_msg;
// just get the latest available transform... for accuracy they should send
// goals in the frame of the planner
goal_pose.header.stamp = robot::Time(); // latest available goal_pose.header.stamp = robot::Time(); // latest available
try try
{ {
tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, data_convert::convertTime(goal_pose.header.stamp)); tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, data_convert::convertTime(goal_pose.header.stamp));
@ -1296,32 +1307,15 @@ geometry_msgs::PoseStamped move_base::MoveBase::goalToGlobalFrame(const geometry
std::cerr << "Extrapolation Error looking up robot pose: " << ex.what() << std::endl; std::cerr << "Extrapolation Error looking up robot pose: " << ex.what() << std::endl;
return goal_pose_msg; return goal_pose_msg;
} }
return global_pose; return global_pose;
} }
bool move_base::MoveBase::getTwist(robot_nav_2d_msgs::Twist2DStamped &twist)
{
if (!twist_) {
return false;
}
twist = *twist_;
return true;
}
bool move_base::MoveBase::getFeedback(robot::move_base_core::NavFeedback &feedback)
{
if (!nav_feedback_) {
return false;
}
feedback = *nav_feedback_;
return true;
}
// void wakePlanner(const robot::TimerEvent &event) // void wakePlanner(const robot::TimerEvent &event)
// { // {
// } // }
robot::move_base_core::BaseNavigation::Ptr move_base::MoveBase::create() move_base_core::BaseNavigation::Ptr move_base::MoveBase::create()
{ {
return std::make_shared<move_base::MoveBase>(); return std::make_shared<move_base::MoveBase>();
} }

6
src/Navigations/Packages/move_base/src/move_base_main.cpp
View File

@ -30,7 +30,7 @@
#include <move_base/move_base.h> #include <move_base/move_base.h>
#include <iostream> #include <iostream>
#include <robot/node_handle.h> #include <robot/node_handle.h>
#include <robot_nav_2d_utils/parameters.h> #include <nav_2d_utils/parameters.h>
#include <boost/dll/import.hpp> #include <boost/dll/import.hpp>
#include <robot/console.h> #include <robot/console.h>
#include <tf3/buffer_core.h> #include <tf3/buffer_core.h>
@ -90,12 +90,12 @@ int main(int argc, char** argv)
robot::log_info("[%s:%d]\n Loading MoveBase library...", __FILE__, __LINE__); robot::log_info("[%s:%d]\n Loading MoveBase library...", __FILE__, __LINE__);
robot::PluginLoaderHelper loader; robot::PluginLoaderHelper loader;
std::string path_file_so = loader.findLibraryPath("MoveBase"); std::string path_file_so = loader.findLibraryPath("MoveBase");
auto create_plugin = boost::dll::import_alias<robot::move_base_core::BaseNavigation::Ptr()>( auto create_plugin = boost::dll::import_alias<move_base_core::BaseNavigation::Ptr()>(
path_file_so, path_file_so,
"MoveBase", boost::dll::load_mode::append_decorations); "MoveBase", boost::dll::load_mode::append_decorations);
robot::log_info("[%s:%d]\n Creating MoveBase instance...", __FILE__, __LINE__); robot::log_info("[%s:%d]\n Creating MoveBase instance...", __FILE__, __LINE__);
robot::move_base_core::BaseNavigation::Ptr nav_ptr = create_plugin(); move_base_core::BaseNavigation::Ptr nav_ptr = create_plugin();
if (nav_ptr == nullptr) { if (nav_ptr == nullptr) {
robot::log_error("[%s:%d]\n Error: Failed to create navigation", __FILE__, __LINE__); robot::log_error("[%s:%d]\n Error: Failed to create navigation", __FILE__, __LINE__);