update
This commit is contained in:
parent
6411f729b5
commit
b22ac17c71
|
|
@ -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")
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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<NavigationState>(static_cast<int>(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<move_base_core::State>(static_cast<int>(state));
|
||||
std::string str = move_base_core::toString(cpp_state);
|
||||
robot::move_base_core::State cpp_state = static_cast<robot::move_base_core::State>(static_cast<int>(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<move_base_core::BaseNavigation*>(handle);
|
||||
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(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<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);
|
||||
robot::PluginLoaderHelper loader;
|
||||
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,
|
||||
"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<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;
|
||||
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<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);
|
||||
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<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);
|
||||
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<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);
|
||||
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<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);
|
||||
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<move_base_core::BaseNavigation*>(handle);
|
||||
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(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<move_base_core::BaseNavigation*>(handle);
|
||||
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(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<move_base_core::BaseNavigation*>(handle);
|
||||
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(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<move_base_core::BaseNavigation*>(handle);
|
||||
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(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<move_base_core::BaseNavigation*>(handle);
|
||||
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(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<move_base_core::BaseNavigation*>(handle);
|
||||
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(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<move_base_core::BaseNavigation*>(handle);
|
||||
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(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<move_base_core::BaseNavigation*>(handle);
|
||||
move_base_core::NavFeedback cpp_feedback;
|
||||
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
|
||||
robot::move_base_core::NavFeedback cpp_feedback;
|
||||
if (nav->getFeedback(cpp_feedback)) {
|
||||
cpp_to_c_nav_feedback(cpp_feedback, out_feedback);
|
||||
return true;
|
||||
|
|
|
|||
|
|
@ -11,6 +11,8 @@
|
|||
#include <tf3/buffer_core.h>
|
||||
#include <memory>
|
||||
|
||||
using TFListenerPtr = std::shared_ptr<tf3::BufferCore>;
|
||||
|
||||
namespace robot
|
||||
{
|
||||
using TFListenerPtr = std::shared_ptr<tf3::BufferCore>;
|
||||
} // namespace robot
|
||||
#endif // NAV_CORE2_COMMON_H
|
||||
|
|
|
|||
|
|
@ -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_
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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,11 +196,34 @@ 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
|
||||
|
|
@ -208,14 +231,6 @@ namespace move_base
|
|||
*/
|
||||
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
|
||||
* @param yaw_goal_tolerance the value command
|
||||
|
|
@ -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_;
|
||||
|
|
|
|||
|
|
@ -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,10 +334,10 @@ 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
|
||||
|
|
@ -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>();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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__);
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user