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

update

Browse Source
This commit is contained in:
HiepLM 2025-12-29 14:43:20 +07:00
parent 6411f729b5
commit b22ac17c71
9 changed files with 130 additions and 77 deletions
Show Stats Download Patch File Download Diff File 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,12 +10,16 @@ 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
) )
@ -35,6 +39,9 @@ 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 move_base_core::NavFeedback& cpp_feedback, NavFeedback* c_feedback) { void cpp_to_c_nav_feedback(const robot::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) {
move_base_core::State cpp_state = static_cast<move_base_core::State>(static_cast<int>(state)); robot::move_base_core::State cpp_state = static_cast<robot::move_base_core::State>(static_cast<int>(state));
std::string str = move_base_core::toString(cpp_state); std::string str = robot::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 = move_base_core::offset_goal(pose, std::string(frame_id), offset_distance); geometry_msgs::PoseStamped result = robot::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 = move_base_core::offset_goal(cpp_pose, offset_distance); geometry_msgs::PoseStamped result = robot::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) {
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle); robot::move_base_core::BaseNavigation* nav = static_cast<robot::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__);
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle); robot::move_base_core::BaseNavigation* nav = static_cast<robot::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<move_base_core::BaseNavigation::Ptr()>( auto create_plugin = boost::dll::import_alias<robot::move_base_core::BaseNavigation::Ptr()>(
lib_path, lib_path,
"MoveBase", "MoveBase",
boost::dll::load_mode::append_decorations); boost::dll::load_mode::append_decorations);
move_base_core::BaseNavigation::Ptr nav_ptr = create_plugin(); robot::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 {
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle); robot::move_base_core::BaseNavigation* nav = static_cast<robot::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 {
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle); robot::move_base_core::BaseNavigation* nav = static_cast<robot::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 {
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle); robot::move_base_core::BaseNavigation* nav = static_cast<robot::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 {
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle); robot::move_base_core::BaseNavigation* nav = static_cast<robot::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 {
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle); robot::move_base_core::BaseNavigation* nav = static_cast<robot::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 {
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle); robot::move_base_core::BaseNavigation* nav = static_cast<robot::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 {
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle); robot::move_base_core::BaseNavigation* nav = static_cast<robot::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 {
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle); robot::move_base_core::BaseNavigation* nav = static_cast<robot::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 {
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle); robot::move_base_core::BaseNavigation* nav = static_cast<robot::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 {
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle); robot::move_base_core::BaseNavigation* nav = static_cast<robot::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 {
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle); robot::move_base_core::BaseNavigation* nav = static_cast<robot::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 {
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle); robot::move_base_core::BaseNavigation* nav = static_cast<robot::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 {
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle); robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
move_base_core::NavFeedback cpp_feedback; robot::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;

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

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

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

@ -8,6 +8,7 @@
#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>
@ -19,6 +20,9 @@
#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
@ -299,13 +303,28 @@ namespace move_base_core
*/ */
virtual bool getRobotPose(geometry_msgs::Pose2D &pose) = 0; virtual bool getRobotPose(geometry_msgs::Pose2D &pose) = 0;
// Shared feedback data for navigation status tracking /**
std::shared_ptr<NavFeedback> nav_feedback_; * @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: protected:
// Shared feedback data for navigation status tracking
std::shared_ptr<NavFeedback> nav_feedback_;
std::shared_ptr<robot_nav_2d_msgs::Twist2DStamped> twist_;
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_

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

@ -148,7 +148,7 @@ else()
data_convert data_convert
dl dl
pthread pthread
nav_2d_utils robot_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/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/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" 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"
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 move_base_core::BaseNavigation class MoveBase : public robot::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(TFListenerPtr tf); MoveBase(robot::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(TFListenerPtr tf) override; virtual void initialize(robot::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,25 +196,40 @@ 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
@ -291,7 +306,7 @@ namespace move_base
private: private:
bool initialized_; bool initialized_;
TFListenerPtr tf_; robot::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_;

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

@ -31,7 +31,7 @@ move_base::MoveBase::MoveBase()
{ {
} }
move_base::MoveBase::MoveBase(TFListenerPtr tf) move_base::MoveBase::MoveBase(robot::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(TFListenerPtr tf) void move_base::MoveBase::initialize(robot::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,10 +334,10 @@ void move_base::MoveBase::initialize(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<move_base_core::NavFeedback>(); nav_feedback_ = std::make_shared<robot::move_base_core::NavFeedback>();
if (nav_feedback_) if (nav_feedback_)
{ {
nav_feedback_->navigation_state = move_base_core::State::PLANNING; nav_feedback_->navigation_state = robot::move_base_core::State::PLANNING;
nav_feedback_->is_ready = true; nav_feedback_->is_ready = true;
} }
else else
@ -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 = move_base_core::State::REJECTED; nav_feedback_->navigation_state = robot::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 = move_base_core::State::PLANNING; nav_feedback_->navigation_state = robot::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 = move_base_core::State::REJECTED; nav_feedback_->navigation_state = robot::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 = move_base_core::State::REJECTED; nav_feedback_->navigation_state = robot::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 = move_base_core::State::PLANNING; nav_feedback_->navigation_state = robot::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 = move_base_core::State::REJECTED; nav_feedback_->navigation_state = robot::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 = move_base_core::State::PLANNING; nav_feedback_->navigation_state = robot::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 = move_base_core::State::REJECTED; nav_feedback_->navigation_state = robot::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 = move_base_core::State::PLANNING; nav_feedback_->navigation_state = robot::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,20 +625,12 @@ bool move_base::MoveBase::rotateTo(const geometry_msgs::PoseStamped &goal, doubl
{ {
if (nav_feedback_) if (nav_feedback_)
{ {
nav_feedback_->navigation_state = move_base_core::State::REJECTED; nav_feedback_->navigation_state = robot::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;
} }
@ -1311,11 +1303,29 @@ geometry_msgs::PoseStamped move_base::MoveBase::goalToGlobalFrame(const geometry
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)
// { // {
// } // }
move_base_core::BaseNavigation::Ptr move_base::MoveBase::create() robot::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 <nav_2d_utils/parameters.h> #include <robot_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<move_base_core::BaseNavigation::Ptr()>( auto create_plugin = boost::dll::import_alias<robot::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__);
move_base_core::BaseNavigation::Ptr nav_ptr = create_plugin(); robot::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__);