From b22ac17c7172f0d7f670663962dd1d1b9544d2d5 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Mon, 29 Dec 2025 14:43:20 +0700 Subject: [PATCH] update --- CMakeLists.txt | 12 ++-- src/APIs/c_api/CMakeLists.txt | 7 +++ src/APIs/c_api/src/nav_c_api.cpp | 46 +++++++-------- .../include/move_base_core/common.h | 8 ++- .../include/move_base_core/navigation.h | 23 +++++++- .../Packages/move_base/CMakeLists.txt | 6 +- .../move_base/include/move_base/move_base.h | 41 ++++++++----- .../Packages/move_base/src/move_base.cpp | 58 +++++++++++-------- .../Packages/move_base/src/move_base_main.cpp | 6 +- 9 files changed, 130 insertions(+), 77 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e4fb638..f7d9932 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -141,14 +141,14 @@ endif() # 2. Main packages (phụ thuộc vào cores) message(STATUS "[move_base] Shared library configured") -# if (NOT TARGET move_base) -# add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Packages/move_base) -# endif() +if (NOT TARGET move_base) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Packages/move_base) +endif() # C API for .NET/C# integration -# if (NOT TARGET navigation_c_api) -# add_subdirectory(${CMAKE_SOURCE_DIR}/src/APIs/c_api) -# endif() +if (NOT TARGET navigation_c_api) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/APIs/c_api) +endif() message(STATUS "========================================") message(STATUS "All packages configured successfully") diff --git a/src/APIs/c_api/CMakeLists.txt b/src/APIs/c_api/CMakeLists.txt index 1fd72c7..4e01628 100644 --- a/src/APIs/c_api/CMakeLists.txt +++ b/src/APIs/c_api/CMakeLists.txt @@ -10,12 +10,16 @@ if(NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) endif() +# Find Boost +find_package(Boost REQUIRED) + # Dependencies set(PACKAGES_DIR move_base_core move_base tf3 robot_time + robot_cpp geometry_msgs ) @@ -35,6 +39,9 @@ add_library(nav_c_api SHARED ${SOURCES} ${HEADERS}) target_link_libraries(nav_c_api PUBLIC ${PACKAGES_DIR} + PRIVATE + Boost::boost + dl ) set_target_properties(nav_c_api PROPERTIES diff --git a/src/APIs/c_api/src/nav_c_api.cpp b/src/APIs/c_api/src/nav_c_api.cpp index 79f9ac4..8ff27c4 100644 --- a/src/APIs/c_api/src/nav_c_api.cpp +++ b/src/APIs/c_api/src/nav_c_api.cpp @@ -66,7 +66,7 @@ namespace { } // 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(static_cast(cpp_feedback.navigation_state)); if (!cpp_feedback.feed_back_str.empty()) { 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) { - move_base_core::State cpp_state = static_cast(static_cast(state)); - std::string str = move_base_core::toString(cpp_state); + robot::move_base_core::State cpp_state = static_cast(static_cast(state)); + std::string str = robot::move_base_core::toString(cpp_state); 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.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); 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 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); return true; } @@ -154,7 +154,7 @@ extern "C" NavigationHandle navigation_create(void) { extern "C" void navigation_destroy(NavigationHandle handle) { if (handle) { - move_base_core::BaseNavigation* nav = static_cast(handle); + robot::move_base_core::BaseNavigation* nav = static_cast(handle); delete nav; } } @@ -226,16 +226,16 @@ extern "C" bool navigation_initialize(NavigationHandle handle, TFListenerHandle } try { printf("[%s:%d]\n Initialize navigation\n", __FILE__, __LINE__); - move_base_core::BaseNavigation* nav = static_cast(handle); + robot::move_base_core::BaseNavigation* nav = static_cast(handle); auto* tf_ptr = static_cast*>(tf_handle); robot::PluginLoaderHelper loader; std::string lib_path = loader.findLibraryPath("MoveBase"); - auto create_plugin = boost::dll::import_alias( + auto create_plugin = boost::dll::import_alias( lib_path, "MoveBase", 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__); if (nav_ptr == nullptr) { 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 { - move_base_core::BaseNavigation* nav = static_cast(handle); + robot::move_base_core::BaseNavigation* nav = static_cast(handle); std::vector footprint; footprint.reserve(point_count); 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 { - move_base_core::BaseNavigation* nav = static_cast(handle); + robot::move_base_core::BaseNavigation* nav = static_cast(handle); geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal); return nav->moveTo(cpp_goal, xy_goal_tolerance, yaw_goal_tolerance); } catch (...) { @@ -299,7 +299,7 @@ extern "C" bool navigation_dock_to(NavigationHandle handle, const char* marker, } try { - move_base_core::BaseNavigation* nav = static_cast(handle); + robot::move_base_core::BaseNavigation* nav = static_cast(handle); 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); } catch (...) { @@ -314,7 +314,7 @@ extern "C" bool navigation_move_straight_to(NavigationHandle handle, const PoseS } try { - move_base_core::BaseNavigation* nav = static_cast(handle); + robot::move_base_core::BaseNavigation* nav = static_cast(handle); geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal); return nav->moveStraightTo(cpp_goal, xy_goal_tolerance); } catch (...) { @@ -329,7 +329,7 @@ extern "C" bool navigation_rotate_to(NavigationHandle handle, const PoseStamped* } try { - move_base_core::BaseNavigation* nav = static_cast(handle); + robot::move_base_core::BaseNavigation* nav = static_cast(handle); geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal); return nav->rotateTo(cpp_goal, yaw_goal_tolerance); } catch (...) { @@ -340,7 +340,7 @@ extern "C" bool navigation_rotate_to(NavigationHandle handle, const PoseStamped* extern "C" void navigation_pause(NavigationHandle handle) { if (handle) { try { - move_base_core::BaseNavigation* nav = static_cast(handle); + robot::move_base_core::BaseNavigation* nav = static_cast(handle); nav->pause(); } catch (...) { // Ignore exceptions @@ -351,7 +351,7 @@ extern "C" void navigation_pause(NavigationHandle handle) { extern "C" void navigation_resume(NavigationHandle handle) { if (handle) { try { - move_base_core::BaseNavigation* nav = static_cast(handle); + robot::move_base_core::BaseNavigation* nav = static_cast(handle); nav->resume(); } catch (...) { // Ignore exceptions @@ -362,7 +362,7 @@ extern "C" void navigation_resume(NavigationHandle handle) { extern "C" void navigation_cancel(NavigationHandle handle) { if (handle) { try { - move_base_core::BaseNavigation* nav = static_cast(handle); + robot::move_base_core::BaseNavigation* nav = static_cast(handle); nav->cancel(); } catch (...) { // Ignore exceptions @@ -377,7 +377,7 @@ extern "C" bool navigation_set_twist_linear(NavigationHandle handle, } try { - move_base_core::BaseNavigation* nav = static_cast(handle); + robot::move_base_core::BaseNavigation* nav = static_cast(handle); geometry_msgs::Vector3 linear; linear.x = linear_x; linear.y = linear_y; @@ -395,7 +395,7 @@ extern "C" bool navigation_set_twist_angular(NavigationHandle handle, } try { - move_base_core::BaseNavigation* nav = static_cast(handle); + robot::move_base_core::BaseNavigation* nav = static_cast(handle); geometry_msgs::Vector3 angular; angular.x = angular_x; angular.y = angular_y; @@ -412,7 +412,7 @@ extern "C" bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseS } try { - move_base_core::BaseNavigation* nav = static_cast(handle); + robot::move_base_core::BaseNavigation* nav = static_cast(handle); geometry_msgs::PoseStamped cpp_pose; if (nav->getRobotPose(cpp_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 { - move_base_core::BaseNavigation* nav = static_cast(handle); + robot::move_base_core::BaseNavigation* nav = static_cast(handle); geometry_msgs::Pose2D cpp_pose; if (nav->getRobotPose(cpp_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 { - move_base_core::BaseNavigation* nav = static_cast(handle); - move_base_core::NavFeedback cpp_feedback; + robot::move_base_core::BaseNavigation* nav = static_cast(handle); + robot::move_base_core::NavFeedback cpp_feedback; if (nav->getFeedback(cpp_feedback)) { cpp_to_c_nav_feedback(cpp_feedback, out_feedback); return true; diff --git a/src/Navigations/Cores/move_base_core/include/move_base_core/common.h b/src/Navigations/Cores/move_base_core/include/move_base_core/common.h index b454a25..985320b 100644 --- a/src/Navigations/Cores/move_base_core/include/move_base_core/common.h +++ b/src/Navigations/Cores/move_base_core/include/move_base_core/common.h @@ -11,6 +11,8 @@ #include #include -using TFListenerPtr = std::shared_ptr; - -#endif // NAV_CORE2_COMMON_H +namespace robot +{ + using TFListenerPtr = std::shared_ptr; +} // namespace robot +#endif // NAV_CORE2_COMMON_H diff --git a/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h b/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h index cc153b1..008a62f 100755 --- a/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h +++ b/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h @@ -8,6 +8,7 @@ #include #include #include +#include // robot protocol msgs #include @@ -19,6 +20,9 @@ #include #include + +namespace robot +{ namespace move_base_core { // Navigation states, including planning and controller status @@ -299,13 +303,28 @@ namespace move_base_core */ virtual bool getRobotPose(geometry_msgs::Pose2D &pose) = 0; - // Shared feedback data for navigation status tracking - std::shared_ptr 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: + // Shared feedback data for navigation status tracking + std::shared_ptr nav_feedback_; + std::shared_ptr twist_; BaseNavigation() = default; }; } // namespace move_base_core +}; #endif // _MOVE_BASE_CORE_NAVIGATION_H_INCLUDED_ diff --git a/src/Navigations/Packages/move_base/CMakeLists.txt b/src/Navigations/Packages/move_base/CMakeLists.txt index 2b24b31..6072eaa 100644 --- a/src/Navigations/Packages/move_base/CMakeLists.txt +++ b/src/Navigations/Packages/move_base/CMakeLists.txt @@ -148,7 +148,7 @@ else() data_convert dl pthread - nav_2d_utils + robot_nav_2d_utils ) target_link_libraries(move_base @@ -196,8 +196,8 @@ endif() # Set RPATH for executable to find libraries in build directory first # Use RPATH instead of RUNPATH for higher priority 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" - 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_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/robot_nav_2d_utils" BUILD_WITH_INSTALL_RPATH FALSE INSTALL_RPATH_USE_LINK_PATH TRUE SKIP_BUILD_RPATH FALSE diff --git a/src/Navigations/Packages/move_base/include/move_base/move_base.h b/src/Navigations/Packages/move_base/include/move_base/move_base.h index 1e699bd..67db766 100644 --- a/src/Navigations/Packages/move_base/include/move_base/move_base.h +++ b/src/Navigations/Packages/move_base/include/move_base/move_base.h @@ -58,7 +58,7 @@ namespace move_base * @class MoveBase * @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: /** @@ -66,14 +66,14 @@ namespace move_base * @param name The name of the action * @param tf A reference to a TransformListener */ - MoveBase(TFListenerPtr tf); + MoveBase(robot::TFListenerPtr tf); MoveBase(); /** * @brief Initialize the navigation system. * @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. @@ -196,25 +196,40 @@ namespace move_base */ 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 */ 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 * @param goal A reference to the goal to pursue * @return True if processing of the goal is done, false otherwise */ - 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: + bool executeCycle(geometry_msgs::PoseStamped &goal); /** * @brief Set velocity for yaw goal tolerance of the robot @@ -291,7 +306,7 @@ namespace move_base private: bool initialized_; - TFListenerPtr tf_; + robot::TFListenerPtr tf_; robot::NodeHandle private_nh_; boost::function planner_loader_; diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index 6bf0dea..660d22e 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -31,7 +31,7 @@ move_base::MoveBase::MoveBase() { } -move_base::MoveBase::MoveBase(TFListenerPtr tf) +move_base::MoveBase::MoveBase(robot::TFListenerPtr tf) : initialized_(false), planner_costmap_robot_(NULL), controller_costmap_robot_(NULL), planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL), @@ -89,7 +89,7 @@ move_base::MoveBase::~MoveBase() 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__); if (!initialized_) @@ -334,14 +334,14 @@ void move_base::MoveBase::initialize(TFListenerPtr tf) // we'll start executing recovery behaviors at the beginning of our list recovery_index_ = 0; - nav_feedback_ = std::make_shared(); + nav_feedback_ = std::make_shared(); 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; } else - { + { 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_) { - 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_->goal_checked = false; } @@ -416,7 +416,7 @@ bool move_base::MoveBase::moveTo(const geometry_msgs::PoseStamped &goal, double 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_->goal_checked = false; } @@ -455,7 +455,7 @@ bool move_base::MoveBase::dockTo(const std::string &maker, const geometry_msgs:: { 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; fb_ss << "The system has not been '" << maker << "'"; 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_) { - 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_->goal_checked = false; } @@ -510,7 +510,7 @@ bool move_base::MoveBase::dockTo(const std::string &maker, const geometry_msgs:: 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_->goal_checked = false; } @@ -539,7 +539,7 @@ bool move_base::MoveBase::moveStraightTo(const geometry_msgs::PoseStamped &goal, { 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_->goal_checked = false; } @@ -565,7 +565,7 @@ bool move_base::MoveBase::moveStraightTo(const geometry_msgs::PoseStamped &goal, 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_->goal_checked = false; } @@ -587,7 +587,7 @@ bool move_base::MoveBase::rotateTo(const geometry_msgs::PoseStamped &goal, doubl { 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_->goal_checked = false; } @@ -614,7 +614,7 @@ bool move_base::MoveBase::rotateTo(const geometry_msgs::PoseStamped &goal, doubl 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_->goal_checked = false; } @@ -625,20 +625,12 @@ bool move_base::MoveBase::rotateTo(const geometry_msgs::PoseStamped &goal, doubl { 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_->goal_checked = 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(); return true; } @@ -1311,11 +1303,29 @@ geometry_msgs::PoseStamped move_base::MoveBase::goalToGlobalFrame(const geometry 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) // { // } -move_base_core::BaseNavigation::Ptr move_base::MoveBase::create() +robot::move_base_core::BaseNavigation::Ptr move_base::MoveBase::create() { return std::make_shared(); } diff --git a/src/Navigations/Packages/move_base/src/move_base_main.cpp b/src/Navigations/Packages/move_base/src/move_base_main.cpp index 3c3dfdf..c6b4818 100644 --- a/src/Navigations/Packages/move_base/src/move_base_main.cpp +++ b/src/Navigations/Packages/move_base/src/move_base_main.cpp @@ -30,7 +30,7 @@ #include #include #include -#include +#include #include #include #include @@ -90,12 +90,12 @@ int main(int argc, char** argv) robot::log_info("[%s:%d]\n Loading MoveBase library...", __FILE__, __LINE__); robot::PluginLoaderHelper loader; std::string path_file_so = loader.findLibraryPath("MoveBase"); - auto create_plugin = boost::dll::import_alias( + auto create_plugin = boost::dll::import_alias( path_file_so, "MoveBase", boost::dll::load_mode::append_decorations); 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) { robot::log_error("[%s:%d]\n Error: Failed to create navigation", __FILE__, __LINE__);