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

update

Browse Source
This commit is contained in:
HiepLM
2025-12-29 14:43:20 +07:00
parent 6411f729b5
commit b22ac17c71
9 changed files with 130 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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 <memory>
using TFListenerPtr = std::shared_ptr<tf3::BufferCore>;
#endif // NAV_CORE2_COMMON_H
namespace robot
{
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/Pose2D.h>
#include <geometry_msgs/Vector3.h>
#include <robot_nav_2d_msgs/Twist2DStamped.h>
// robot protocol msgs
#include <robot_protocol_msgs/Order.h>
@@ -19,6 +20,9 @@
#include <robot/time.h>
#include <move_base_core/common.h>
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<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:
// Shared feedback data for navigation status tracking
std::shared_ptr<NavFeedback> nav_feedback_;
std::shared_ptr<robot_nav_2d_msgs::Twist2DStamped> twist_;
BaseNavigation() = default;
};
} // namespace move_base_core
};
#endif // _MOVE_BASE_CORE_NAVIGATION_H_INCLUDED_

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

@@ -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

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

@@ -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<nav_core::BaseGlobalPlanner::Ptr()> planner_loader_;

58
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),
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<move_base_core::NavFeedback>();
nav_feedback_ = std::make_shared<robot::move_base_core::NavFeedback>();
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<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 <iostream>
#include <robot/node_handle.h>
#include <nav_2d_utils/parameters.h>
#include <robot_nav_2d_utils/parameters.h>
#include <boost/dll/import.hpp>
#include <robot/console.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::PluginLoaderHelper loader;
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,
"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__);