Compare commits
No commits in common. "1de5a35cc7985bc9890e8de2542f85d8ab90e2d7" and "307a9c84f958a16964b5bce7106aa13ff26671cf" have entirely different histories.
1de5a35cc7
...
307a9c84f9
|
|
@ -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,16 +10,12 @@ 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
|
||||
)
|
||||
|
||||
|
|
@ -39,9 +35,6 @@ 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 robot::move_base_core::NavFeedback& cpp_feedback, NavFeedback* c_feedback) {
|
||||
void cpp_to_c_nav_feedback(const move_base_core::NavFeedback& cpp_feedback, NavFeedback* c_feedback) {
|
||||
c_feedback->navigation_state = static_cast<NavigationState>(static_cast<int>(cpp_feedback.navigation_state));
|
||||
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) {
|
||||
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);
|
||||
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);
|
||||
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 = robot::move_base_core::offset_goal(pose, std::string(frame_id), offset_distance);
|
||||
geometry_msgs::PoseStamped result = move_base_core::offset_goal(pose, std::string(frame_id), offset_distance);
|
||||
cpp_to_c_pose_stamped(result, out_goal);
|
||||
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 = robot::move_base_core::offset_goal(cpp_pose, offset_distance);
|
||||
geometry_msgs::PoseStamped result = move_base_core::offset_goal(cpp_pose, offset_distance);
|
||||
cpp_to_c_pose_stamped(result, out_goal);
|
||||
return true;
|
||||
}
|
||||
|
|
@ -154,7 +154,7 @@ extern "C" NavigationHandle navigation_create(void) {
|
|||
|
||||
extern "C" void navigation_destroy(NavigationHandle handle) {
|
||||
if (handle) {
|
||||
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
|
||||
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
|
||||
delete nav;
|
||||
}
|
||||
}
|
||||
|
|
@ -226,16 +226,16 @@ extern "C" bool navigation_initialize(NavigationHandle handle, TFListenerHandle
|
|||
}
|
||||
try {
|
||||
printf("[%s:%d]\n Initialize navigation\n", __FILE__, __LINE__);
|
||||
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
|
||||
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
|
||||
auto* tf_ptr = static_cast<std::shared_ptr<tf3::BufferCore>*>(tf_handle);
|
||||
robot::PluginLoaderHelper loader;
|
||||
std::string lib_path = loader.findLibraryPath("MoveBase");
|
||||
auto create_plugin = boost::dll::import_alias<robot::move_base_core::BaseNavigation::Ptr()>(
|
||||
auto create_plugin = boost::dll::import_alias<move_base_core::BaseNavigation::Ptr()>(
|
||||
lib_path,
|
||||
"MoveBase",
|
||||
boost::dll::load_mode::append_decorations);
|
||||
|
||||
robot::move_base_core::BaseNavigation::Ptr nav_ptr = create_plugin();
|
||||
move_base_core::BaseNavigation::Ptr nav_ptr = create_plugin();
|
||||
printf("[%s:%d]\n Navigation created\n", __FILE__, __LINE__);
|
||||
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 {
|
||||
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
|
||||
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
|
||||
std::vector<geometry_msgs::Point> footprint;
|
||||
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 {
|
||||
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
|
||||
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
|
||||
geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
|
||||
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 {
|
||||
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
|
||||
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
|
||||
geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
|
||||
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 {
|
||||
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
|
||||
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
|
||||
geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
|
||||
return nav->moveStraightTo(cpp_goal, xy_goal_tolerance);
|
||||
} catch (...) {
|
||||
|
|
@ -329,7 +329,7 @@ extern "C" bool navigation_rotate_to(NavigationHandle handle, const PoseStamped*
|
|||
}
|
||||
|
||||
try {
|
||||
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
|
||||
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
|
||||
geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
|
||||
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 {
|
||||
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
|
||||
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
|
||||
nav->pause();
|
||||
} catch (...) {
|
||||
// Ignore exceptions
|
||||
|
|
@ -351,7 +351,7 @@ extern "C" void navigation_pause(NavigationHandle handle) {
|
|||
extern "C" void navigation_resume(NavigationHandle handle) {
|
||||
if (handle) {
|
||||
try {
|
||||
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
|
||||
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
|
||||
nav->resume();
|
||||
} catch (...) {
|
||||
// Ignore exceptions
|
||||
|
|
@ -362,7 +362,7 @@ extern "C" void navigation_resume(NavigationHandle handle) {
|
|||
extern "C" void navigation_cancel(NavigationHandle handle) {
|
||||
if (handle) {
|
||||
try {
|
||||
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
|
||||
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
|
||||
nav->cancel();
|
||||
} catch (...) {
|
||||
// Ignore exceptions
|
||||
|
|
@ -377,7 +377,7 @@ extern "C" bool navigation_set_twist_linear(NavigationHandle handle,
|
|||
}
|
||||
|
||||
try {
|
||||
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
|
||||
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
|
||||
geometry_msgs::Vector3 linear;
|
||||
linear.x = linear_x;
|
||||
linear.y = linear_y;
|
||||
|
|
@ -395,7 +395,7 @@ extern "C" bool navigation_set_twist_angular(NavigationHandle handle,
|
|||
}
|
||||
|
||||
try {
|
||||
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
|
||||
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
|
||||
geometry_msgs::Vector3 angular;
|
||||
angular.x = angular_x;
|
||||
angular.y = angular_y;
|
||||
|
|
@ -412,7 +412,7 @@ extern "C" bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseS
|
|||
}
|
||||
|
||||
try {
|
||||
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
|
||||
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
|
||||
geometry_msgs::PoseStamped cpp_pose;
|
||||
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 {
|
||||
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
|
||||
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
|
||||
geometry_msgs::Pose2D cpp_pose;
|
||||
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 {
|
||||
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
|
||||
robot::move_base_core::NavFeedback cpp_feedback;
|
||||
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(handle);
|
||||
move_base_core::NavFeedback cpp_feedback;
|
||||
if (nav->getFeedback(cpp_feedback)) {
|
||||
cpp_to_c_nav_feedback(cpp_feedback, out_feedback);
|
||||
return true;
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
Subproject commit a28f05c6d5cb6cf2d8c4ffb52a637ecfa2878db2
|
||||
Subproject commit 0344c31e5b4185da41bfb253db720d9c91b802bd
|
||||
|
|
@ -39,6 +39,7 @@
|
|||
#include <nav_core2/bounds.h>
|
||||
#include <vector>
|
||||
|
||||
using namespace robot;
|
||||
/**
|
||||
* @brief A set of utility functions for Bounds objects interacting with other messages/types
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -98,8 +98,8 @@ namespace robot_nav_2d_utils
|
|||
nav_msgs::MapMetaData infoToInfo(const nav_grid::NavGridInfo &info);
|
||||
|
||||
// Bounds Transformations
|
||||
robot_nav_2d_msgs::UIntBounds toMsg(const nav_core2::UIntBounds &bounds);
|
||||
nav_core2::UIntBounds fromMsg(const robot_nav_2d_msgs::UIntBounds &msg);
|
||||
::robot_nav_2d_msgs::UIntBounds toMsg(const robot::nav_core2::UIntBounds &bounds);
|
||||
robot::nav_core2::UIntBounds fromMsg(const ::robot_nav_2d_msgs::UIntBounds &msg);
|
||||
|
||||
} // namespace robot_nav_2d_utils
|
||||
|
||||
|
|
|
|||
|
|
@ -83,7 +83,7 @@ namespace robot_nav_2d_utils
|
|||
* @param frame_id Frame to transform the pose into
|
||||
* @return The resulting transformed pose
|
||||
*/
|
||||
geometry_msgs::Pose2D transformStampedPose(const TFListenerPtr tf, const ::robot_nav_2d_msgs::Pose2DStamped &pose,
|
||||
::geometry_msgs::Pose2D transformStampedPose(const TFListenerPtr tf, const ::robot_nav_2d_msgs::Pose2DStamped &pose,
|
||||
const ::std::string &frame_id);
|
||||
|
||||
} // namespace robot_nav_2d_utils
|
||||
|
|
|
|||
|
|
@ -38,6 +38,7 @@
|
|||
#include <stdexcept>
|
||||
#include <vector>
|
||||
|
||||
using namespace robot;
|
||||
namespace robot_nav_2d_utils
|
||||
{
|
||||
nav_core2::Bounds getFullBounds(const nav_grid::NavGridInfo &info)
|
||||
|
|
|
|||
|
|
@ -318,7 +318,7 @@ namespace robot_nav_2d_utils
|
|||
return metadata;
|
||||
}
|
||||
|
||||
robot_nav_2d_msgs::UIntBounds toMsg(const nav_core2::UIntBounds &bounds)
|
||||
robot_nav_2d_msgs::UIntBounds toMsg(const robot::nav_core2::UIntBounds &bounds)
|
||||
{
|
||||
robot_nav_2d_msgs::UIntBounds msg;
|
||||
msg.min_x = bounds.getMinX();
|
||||
|
|
@ -328,9 +328,9 @@ namespace robot_nav_2d_utils
|
|||
return msg;
|
||||
}
|
||||
|
||||
nav_core2::UIntBounds fromMsg(const robot_nav_2d_msgs::UIntBounds &msg)
|
||||
robot::nav_core2::UIntBounds fromMsg(const robot_nav_2d_msgs::UIntBounds &msg)
|
||||
{
|
||||
return nav_core2::UIntBounds(msg.min_x, msg.min_y, msg.max_x, msg.max_y);
|
||||
return robot::nav_core2::UIntBounds(msg.min_x, msg.min_y, msg.max_x, msg.max_y);
|
||||
}
|
||||
|
||||
} // namespace robot_nav_2d_utils
|
||||
|
|
|
|||
|
|
@ -11,8 +11,6 @@
|
|||
#include <tf3/buffer_core.h>
|
||||
#include <memory>
|
||||
|
||||
namespace robot
|
||||
{
|
||||
using TFListenerPtr = std::shared_ptr<tf3::BufferCore>;
|
||||
} // namespace robot
|
||||
#endif // NAV_CORE2_COMMON_H
|
||||
using TFListenerPtr = std::shared_ptr<tf3::BufferCore>;
|
||||
|
||||
#endif // NAV_CORE2_COMMON_H
|
||||
|
|
|
|||
|
|
@ -8,7 +8,6 @@
|
|||
#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>
|
||||
|
|
@ -20,9 +19,6 @@
|
|||
#include <robot/time.h>
|
||||
#include <move_base_core/common.h>
|
||||
|
||||
|
||||
namespace robot
|
||||
{
|
||||
namespace move_base_core
|
||||
{
|
||||
// Navigation states, including planning and controller status
|
||||
|
|
@ -303,28 +299,13 @@ namespace move_base_core
|
|||
*/
|
||||
virtual bool getRobotPose(geometry_msgs::Pose2D &pose) = 0;
|
||||
|
||||
/**
|
||||
* @brief Get the robot’s twist.
|
||||
* @param twist Output parameter with the robot’s current twist.
|
||||
* @return True if twist was successfully retrieved.
|
||||
*/
|
||||
virtual bool getTwist(robot_nav_2d_msgs::Twist2DStamped &twist) = 0;
|
||||
|
||||
/**
|
||||
* @brief Get the navigation feedback.
|
||||
* @param feedback Output parameter with the navigation feedback.
|
||||
* @return True if feedback was successfully retrieved.
|
||||
*/
|
||||
virtual bool getFeedback(NavFeedback &feedback) = 0;
|
||||
|
||||
protected:
|
||||
// Shared feedback data for navigation status tracking
|
||||
std::shared_ptr<NavFeedback> nav_feedback_;
|
||||
std::shared_ptr<robot_nav_2d_msgs::Twist2DStamped> twist_;
|
||||
|
||||
protected:
|
||||
BaseNavigation() = default;
|
||||
};
|
||||
|
||||
} // namespace move_base_core
|
||||
};
|
||||
|
||||
#endif // _MOVE_BASE_CORE_NAVIGATION_H_INCLUDED_
|
||||
|
|
|
|||
|
|
@ -27,7 +27,7 @@ target_link_libraries(nav_core2 INTERFACE
|
|||
costmap_2d
|
||||
tf3
|
||||
nav_grid
|
||||
robot_nav_2d_msgs
|
||||
nav_2d_msgs
|
||||
robot_cpp
|
||||
)
|
||||
|
||||
|
|
|
|||
|
|
@ -2,7 +2,7 @@
|
|||
A replacement interface for [nav_core](https://github.com/ros-planning/navigation/tree/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/nav_core/include/nav_core) that defines basic two dimensional planner interfaces.
|
||||
|
||||
There were a few key reasons for creating new interfaces rather than extending the old ones.
|
||||
* Use `robot_nav_2d_msgs` to eliminate unused data fields
|
||||
* Use `nav_2d_msgs` to eliminate unused data fields
|
||||
* Use a new `Costmap` interface as a plugin rather that forcing all implementations of the interfaces to use `costmap_2d::Costmap2DROBOT`.
|
||||
* Provide more data in the interfaces for easier testing.
|
||||
* Use Exceptions rather than booleans to provide more information about the types of errors encountered.
|
||||
|
|
@ -30,7 +30,7 @@ Let us compare the old [nav_core::BaseGlobalPlanner](https://github.com/ros-plan
|
|||
| `nav_core` | `nav_core2` | comments |
|
||||
|---|--|---|
|
||||
|`void initialize(std::string, costmap_2d::Costmap2DROBOT*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers, and provides a TFListener|
|
||||
|`bool makePlan(const geometry_msgs::PoseStamped&, const geometry_msgs::PoseStamped&, std::vector<geometry_msgs::PoseStamped>&)`|`robot_nav_2d_msgs::Path2D makePlan(const robot_nav_2d_msgs::Pose2DStamped&, const robot_nav_2d_msgs::Pose2DStamped&)`|Uses exceptions for errors instead of returning a bool, which frees up the return for the actual path.|
|
||||
|`bool makePlan(const geometry_msgs::PoseStamped&, const geometry_msgs::PoseStamped&, std::vector<geometry_msgs::PoseStamped>&)`|`nav_2d_msgs::Path2D makePlan(const nav_2d_msgs::Pose2DStamped&, const nav_2d_msgs::Pose2DStamped&)`|Uses exceptions for errors instead of returning a bool, which frees up the return for the actual path.|
|
||||
|
||||
## Local Planner
|
||||
Now let's compare the old [nav_core::BaseLocalPlanner](https://github.com/ros-planning/navigation/blob/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/nav_core/include/nav_core/base_local_planner.h) to the new [nav_core2/LocalPlanner](include/nav_core2/local_planner.h).
|
||||
|
|
@ -38,10 +38,10 @@ Now let's compare the old [nav_core::BaseLocalPlanner](https://github.com/ros-pl
|
|||
| `nav_core` | `nav_core2` | comments |
|
||||
|---|--|---|
|
||||
|`void initialize(std::string, tf::TransformListener*, costmap_2d::Costmap2DROBOT*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers|
|
||||
|(no equivalent)|`void setGoalPose(const robot_nav_2d_msgs::Pose2DStamped&)`|Explicitly set the new goal location, rather than using the last pose of the global plan`
|
||||
|`bool setPlan(const std::vector<geometry_msgs::PoseStamped>&)`|`setPlan(const robot_nav_2d_msgs::Path2D&)`||
|
||||
|`bool computeVelocityCommands(geometry_msgs::Twist&)`|`robot_nav_2d_msgs::Twist2DStamped computeVelocityCommands(const robot_nav_2d_msgs::Pose2DStamped&, const robot_nav_2d_msgs::Twist2D&)`|Explicitly provides the current pose and velocity for more explicit data control and easier testing. Uses exceptions for errors instead of returning a bool, which frees up the return for the actual command.|
|
||||
|`bool isGoalReached()` | `bool isGoalReached(const robot_nav_2d_msgs::Pose2DStamped&, const robot_nav_2d_msgs::Twist2D&)` | Explicitly provide the current pose and velocity for more explicit data control and easier testing. |
|
||||
|(no equivalent)|`void setGoalPose(const nav_2d_msgs::Pose2DStamped&)`|Explicitly set the new goal location, rather than using the last pose of the global plan`
|
||||
|`bool setPlan(const std::vector<geometry_msgs::PoseStamped>&)`|`setPlan(const nav_2d_msgs::Path2D&)`||
|
||||
|`bool computeVelocityCommands(geometry_msgs::Twist&)`|`nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped&, const nav_2d_msgs::Twist2D&)`|Explicitly provides the current pose and velocity for more explicit data control and easier testing. Uses exceptions for errors instead of returning a bool, which frees up the return for the actual command.|
|
||||
|`bool isGoalReached()` | `bool isGoalReached(const nav_2d_msgs::Pose2DStamped&, const nav_2d_msgs::Twist2D&)` | Explicitly provide the current pose and velocity for more explicit data control and easier testing. |
|
||||
|
||||
## Exceptions
|
||||
A hierarchical collection of [exceptions](include/nav_core2/exceptions.h) is provided to allow for reacting to navigation failures in a more robust and contextual way.
|
||||
|
|
|
|||
|
|
@ -34,7 +34,7 @@
|
|||
#ifndef NAV_CORE2_EXCEPTIONS_H
|
||||
#define NAV_CORE2_EXCEPTIONS_H
|
||||
|
||||
#include <robot_nav_2d_msgs/Pose2DStamped.h>
|
||||
#include <nav_2d_msgs/Pose2DStamped.h>
|
||||
#include <stdexcept>
|
||||
#include <exception>
|
||||
#include <string>
|
||||
|
|
@ -69,7 +69,7 @@
|
|||
namespace nav_core2
|
||||
{
|
||||
|
||||
inline std::string poseToString(const robot_nav_2d_msgs::Pose2DStamped& pose)
|
||||
inline std::string poseToString(const nav_2d_msgs::Pose2DStamped& pose)
|
||||
{
|
||||
return "(" + std::to_string(pose.pose.x) + ", " + std::to_string(pose.pose.y) + ", " + std::to_string(pose.pose.theta)
|
||||
+ " : " + pose.header.frame_id + ")";
|
||||
|
|
@ -199,7 +199,7 @@ class InvalidStartPoseException: public GlobalPlannerException
|
|||
public:
|
||||
explicit InvalidStartPoseException(const std::string& description, int result_code = 5)
|
||||
: GlobalPlannerException(description, result_code) {}
|
||||
InvalidStartPoseException(const robot_nav_2d_msgs::Pose2DStamped& pose, const std::string& problem, int result_code = 5) :
|
||||
InvalidStartPoseException(const nav_2d_msgs::Pose2DStamped& pose, const std::string& problem, int result_code = 5) :
|
||||
InvalidStartPoseException("The starting pose " + poseToString(pose) + " is " + problem, result_code) {}
|
||||
};
|
||||
|
||||
|
|
@ -212,7 +212,7 @@ class StartBoundsException: public InvalidStartPoseException
|
|||
public:
|
||||
explicit StartBoundsException(const std::string& description, int result_code = 6)
|
||||
: InvalidStartPoseException(description, result_code) {}
|
||||
explicit StartBoundsException(const robot_nav_2d_msgs::Pose2DStamped& pose) :
|
||||
explicit StartBoundsException(const nav_2d_msgs::Pose2DStamped& pose) :
|
||||
InvalidStartPoseException(pose, "out of bounds", 6) {}
|
||||
};
|
||||
|
||||
|
|
@ -225,7 +225,7 @@ class OccupiedStartException: public InvalidStartPoseException
|
|||
public:
|
||||
explicit OccupiedStartException(const std::string& description, int result_code = 7)
|
||||
: InvalidStartPoseException(description, result_code) {}
|
||||
explicit OccupiedStartException(const robot_nav_2d_msgs::Pose2DStamped& pose) :
|
||||
explicit OccupiedStartException(const nav_2d_msgs::Pose2DStamped& pose) :
|
||||
InvalidStartPoseException(pose, "occupied", 7) {}
|
||||
};
|
||||
|
||||
|
|
@ -238,7 +238,7 @@ class InvalidGoalPoseException: public GlobalPlannerException
|
|||
public:
|
||||
explicit InvalidGoalPoseException(const std::string& description, int result_code = 8)
|
||||
: GlobalPlannerException(description, result_code) {}
|
||||
InvalidGoalPoseException(const robot_nav_2d_msgs::Pose2DStamped& pose, const std::string& problem, int result_code = 8) :
|
||||
InvalidGoalPoseException(const nav_2d_msgs::Pose2DStamped& pose, const std::string& problem, int result_code = 8) :
|
||||
GlobalPlannerException("The goal pose " + poseToString(pose) + " is " + problem, result_code) {}
|
||||
};
|
||||
|
||||
|
|
@ -251,7 +251,7 @@ class GoalBoundsException: public InvalidGoalPoseException
|
|||
public:
|
||||
explicit GoalBoundsException(const std::string& description, int result_code = 9)
|
||||
: InvalidGoalPoseException(description, result_code) {}
|
||||
explicit GoalBoundsException(const robot_nav_2d_msgs::Pose2DStamped& pose) :
|
||||
explicit GoalBoundsException(const nav_2d_msgs::Pose2DStamped& pose) :
|
||||
InvalidGoalPoseException(pose, "out of bounds", 9) {}
|
||||
};
|
||||
|
||||
|
|
@ -264,7 +264,7 @@ class OccupiedGoalException: public InvalidGoalPoseException
|
|||
public:
|
||||
explicit OccupiedGoalException(const std::string& description, int result_code = 10)
|
||||
: InvalidGoalPoseException(description, result_code) {}
|
||||
explicit OccupiedGoalException(const robot_nav_2d_msgs::Pose2DStamped& pose) :
|
||||
explicit OccupiedGoalException(const nav_2d_msgs::Pose2DStamped& pose) :
|
||||
InvalidGoalPoseException(pose, "occupied", 10) {}
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -36,8 +36,8 @@
|
|||
|
||||
#include <nav_core2/common.h>
|
||||
#include <nav_core2/costmap.h>
|
||||
#include <robot_nav_2d_msgs/Path2D.h>
|
||||
#include <robot_nav_2d_msgs/Pose2DStamped.h>
|
||||
#include <nav_2d_msgs/Path2D.h>
|
||||
#include <nav_2d_msgs/Pose2DStamped.h>
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
|
|
@ -77,8 +77,8 @@ public:
|
|||
* @param goal The goal pose of the robot
|
||||
* @return The sequence of poses to get from start to goal, if any
|
||||
*/
|
||||
virtual robot_nav_2d_msgs::Path2D makePlan(const robot_nav_2d_msgs::Pose2DStamped& start,
|
||||
const robot_nav_2d_msgs::Pose2DStamped& goal) = 0;
|
||||
virtual nav_2d_msgs::Path2D makePlan(const nav_2d_msgs::Pose2DStamped& start,
|
||||
const nav_2d_msgs::Pose2DStamped& goal) = 0;
|
||||
};
|
||||
} // namespace nav_core2
|
||||
|
||||
|
|
|
|||
|
|
@ -36,10 +36,10 @@
|
|||
|
||||
#include <nav_core2/common.h>
|
||||
#include <nav_core2/costmap.h>
|
||||
#include <robot_nav_2d_msgs/Path2D.h>
|
||||
#include <robot_nav_2d_msgs/Pose2DStamped.h>
|
||||
#include <robot_nav_2d_msgs/Twist2D.h>
|
||||
#include <robot_nav_2d_msgs/Twist2DStamped.h>
|
||||
#include <nav_2d_msgs/Path2D.h>
|
||||
#include <nav_2d_msgs/Pose2DStamped.h>
|
||||
#include <nav_2d_msgs/Twist2D.h>
|
||||
#include <nav_2d_msgs/Twist2DStamped.h>
|
||||
#include <geometry_msgs/Twist.h>
|
||||
#include <string>
|
||||
#include <costmap_2d/costmap_2d_robot.h>
|
||||
|
|
@ -83,14 +83,14 @@ namespace nav_core2
|
|||
* @brief Sets the global goal for this local planner.
|
||||
* @param goal_pose The Goal Pose
|
||||
*/
|
||||
virtual void setGoalPose(const robot_nav_2d_msgs::Pose2DStamped &goal_pose) = 0;
|
||||
virtual void setGoalPose(const nav_2d_msgs::Pose2DStamped &goal_pose) = 0;
|
||||
|
||||
/**
|
||||
* @brief Sets the global plan for this local planner.
|
||||
*
|
||||
* @param path The global plan
|
||||
*/
|
||||
virtual void setPlan(const robot_nav_2d_msgs::Path2D &path) = 0;
|
||||
virtual void setPlan(const nav_2d_msgs::Path2D &path) = 0;
|
||||
|
||||
/**
|
||||
* @brief Compute the best command given the current pose, velocity and goal
|
||||
|
|
@ -102,8 +102,8 @@ namespace nav_core2
|
|||
* @param velocity Current robot velocity
|
||||
* @return The best computed velocity
|
||||
*/
|
||||
virtual robot_nav_2d_msgs::Twist2DStamped computeVelocityCommands(const robot_nav_2d_msgs::Pose2DStamped &pose,
|
||||
const robot_nav_2d_msgs::Twist2D &velocity) = 0;
|
||||
virtual nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped &pose,
|
||||
const nav_2d_msgs::Twist2D &velocity) = 0;
|
||||
|
||||
/**
|
||||
* @brief set velocity for x, y asix of the robot
|
||||
|
|
@ -166,7 +166,7 @@ namespace nav_core2
|
|||
* @param velocity Velocity to check
|
||||
* @return True if the goal conditions have been met
|
||||
*/
|
||||
virtual bool isGoalReached(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) = 0;
|
||||
virtual bool isGoalReached(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) = 0;
|
||||
};
|
||||
} // namespace nav_core2
|
||||
|
||||
|
|
|
|||
|
|
@ -11,7 +11,7 @@ if(NOT CMAKE_BUILD_TYPE)
|
|||
set(CMAKE_BUILD_TYPE Release)
|
||||
endif()
|
||||
|
||||
set(PACKAGES_DIR geometry_msgs std_msgs nav_core nav_core2 robot_nav_2d_utils pthread)
|
||||
set(PACKAGES_DIR geometry_msgs std_msgs nav_core nav_core2 nav_2d_utils pthread)
|
||||
|
||||
find_package(Boost REQUIRED COMPONENTS filesystem system)
|
||||
|
||||
|
|
|
|||
|
|
@ -57,8 +57,8 @@ public:
|
|||
// Nav Core 2 Global Planner Interface
|
||||
void initialize(const robot::NodeHandle& parent, const std::string& name,
|
||||
TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
|
||||
robot_nav_2d_msgs::Path2D makePlan(const robot_nav_2d_msgs::Pose2DStamped& start,
|
||||
const robot_nav_2d_msgs::Pose2DStamped& goal) override;
|
||||
nav_2d_msgs::Path2D makePlan(const nav_2d_msgs::Pose2DStamped& start,
|
||||
const nav_2d_msgs::Pose2DStamped& goal) override;
|
||||
|
||||
static nav_core2::GlobalPlanner::Ptr create();
|
||||
|
||||
|
|
|
|||
|
|
@ -39,7 +39,7 @@
|
|||
#include <nav_core/base_local_planner.h>
|
||||
#include <nav_core2/local_planner.h>
|
||||
#include <nav_core_adapter/costmap_adapter.h>
|
||||
#include <robot_nav_2d_utils/odom_subscriber.h>
|
||||
#include <nav_2d_utils/odom_subscriber.h>
|
||||
#include <boost/dll/import.hpp>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
|
@ -168,22 +168,22 @@ namespace nav_core_adapter
|
|||
/**
|
||||
* @brief Get the robot pose from the costmap and store as Pose2DStamped
|
||||
*/
|
||||
bool getRobotPose(robot_nav_2d_msgs::Pose2DStamped &pose2d);
|
||||
bool getRobotPose(nav_2d_msgs::Pose2DStamped &pose2d);
|
||||
|
||||
/**
|
||||
* @brief See if the back of the global plan matches the most recent goal pose
|
||||
* @return True if the plan has changed
|
||||
*/
|
||||
bool hasGoalChanged(const robot_nav_2d_msgs::Pose2DStamped &new_goal);
|
||||
bool hasGoalChanged(const nav_2d_msgs::Pose2DStamped &new_goal);
|
||||
|
||||
// The most recent goal pose
|
||||
robot_nav_2d_msgs::Pose2DStamped last_goal_;
|
||||
nav_2d_msgs::Pose2DStamped last_goal_;
|
||||
bool has_active_goal_;
|
||||
|
||||
/**
|
||||
* @brief helper class for subscribing to odometry
|
||||
*/
|
||||
std::shared_ptr<robot_nav_2d_utils::OdomSubscriber> odom_sub_;
|
||||
std::shared_ptr<nav_2d_utils::OdomSubscriber> odom_sub_;
|
||||
|
||||
// Plugin handling
|
||||
boost::function<nav_core2::LocalPlanner::Ptr()> planner_loader_;
|
||||
|
|
|
|||
|
|
@ -34,8 +34,8 @@
|
|||
|
||||
#include <nav_core_adapter/global_planner_adapter.h>
|
||||
#include <nav_core_adapter/costmap_adapter.h>
|
||||
#include <robot_nav_2d_utils/conversions.h>
|
||||
#include <robot_nav_2d_utils/tf_help.h>
|
||||
#include <nav_2d_utils/conversions.h>
|
||||
#include <nav_2d_utils/tf_help.h>
|
||||
#include <nav_core2/exceptions.h>
|
||||
#include <boost/dll/alias.hpp>
|
||||
#include <memory>
|
||||
|
|
@ -78,18 +78,18 @@ namespace nav_core_adapter
|
|||
planner_->initialize(planner_name, costmap_robot_);
|
||||
}
|
||||
|
||||
robot_nav_2d_msgs::Path2D GlobalPlannerAdapter::makePlan(const robot_nav_2d_msgs::Pose2DStamped& start,
|
||||
const robot_nav_2d_msgs::Pose2DStamped& goal)
|
||||
nav_2d_msgs::Path2D GlobalPlannerAdapter::makePlan(const nav_2d_msgs::Pose2DStamped& start,
|
||||
const nav_2d_msgs::Pose2DStamped& goal)
|
||||
{
|
||||
geometry_msgs::PoseStamped start3d = robot_nav_2d_utils::pose2DToPoseStamped(start),
|
||||
goal3d = robot_nav_2d_utils::pose2DToPoseStamped(goal);
|
||||
geometry_msgs::PoseStamped start3d = nav_2d_utils::pose2DToPoseStamped(start),
|
||||
goal3d = nav_2d_utils::pose2DToPoseStamped(goal);
|
||||
std::vector<geometry_msgs::PoseStamped> plan;
|
||||
bool ret = planner_->makePlan(start3d, goal3d, plan);
|
||||
if (!ret)
|
||||
{
|
||||
throw nav_core2::PlannerException("Generic Global Planner Error");
|
||||
}
|
||||
return robot_nav_2d_utils::posesToPath2D(plan);
|
||||
return nav_2d_utils::posesToPath2D(plan);
|
||||
}
|
||||
|
||||
nav_core2::GlobalPlanner::Ptr GlobalPlannerAdapter::create()
|
||||
|
|
|
|||
|
|
@ -36,9 +36,9 @@
|
|||
#include <nav_core_adapter/local_planner_adapter.h>
|
||||
#include <nav_core_adapter/costmap_adapter.h>
|
||||
#include <nav_core_adapter/shared_pointers.h>
|
||||
#include <robot_nav_2d_utils/conversions.h>
|
||||
#include <robot_nav_2d_utils/tf_help.h>
|
||||
#include <robot_nav_2d_utils/parameters.h>
|
||||
#include <nav_2d_utils/conversions.h>
|
||||
#include <nav_2d_utils/tf_help.h>
|
||||
#include <nav_2d_utils/parameters.h>
|
||||
#include <nav_core2/exceptions.h>
|
||||
#include <boost/dll/alias.hpp>
|
||||
#include <boost/dll/import.hpp>
|
||||
|
|
@ -90,7 +90,7 @@ namespace nav_core_adapter
|
|||
}
|
||||
else
|
||||
{
|
||||
planner_name = robot_nav_2d_utils::searchAndGetParam(adapter_nh_, "position_planner_name", std::string("dwb_local_planner::DWBLocalPlanner"));
|
||||
planner_name = nav_2d_utils::searchAndGetParam(adapter_nh_, "position_planner_name", std::string("dwb_local_planner::DWBLocalPlanner"));
|
||||
adapter_nh_.setParam("planner_name", planner_name);
|
||||
}
|
||||
try
|
||||
|
|
@ -174,15 +174,15 @@ namespace nav_core_adapter
|
|||
}
|
||||
|
||||
// Get the Pose
|
||||
robot_nav_2d_msgs::Pose2DStamped pose2d;
|
||||
nav_2d_msgs::Pose2DStamped pose2d;
|
||||
if (!getRobotPose(pose2d))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
// Get the Velocity
|
||||
robot_nav_2d_msgs::Twist2D velocity = odom_sub_->getTwist();
|
||||
robot_nav_2d_msgs::Twist2DStamped cmd_vel_2d;
|
||||
nav_2d_msgs::Twist2D velocity = odom_sub_->getTwist();
|
||||
nav_2d_msgs::Twist2DStamped cmd_vel_2d;
|
||||
try
|
||||
{
|
||||
cmd_vel_2d = planner_->computeVelocityCommands(pose2d, velocity);
|
||||
|
|
@ -192,7 +192,7 @@ namespace nav_core_adapter
|
|||
std::cerr << "computeVelocityCommands exception: " << e.what() << std::endl;
|
||||
return false;
|
||||
}
|
||||
cmd_vel = robot_nav_2d_utils::twist2Dto3D(cmd_vel_2d.velocity);
|
||||
cmd_vel = nav_2d_utils::twist2Dto3D(cmd_vel_2d.velocity);
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
@ -263,7 +263,7 @@ namespace nav_core_adapter
|
|||
geometry_msgs::Twist LocalPlannerAdapter::getActualTwist()
|
||||
{
|
||||
if (odom_sub_)
|
||||
return robot_nav_2d_utils::twist2Dto3D(odom_sub_->getTwist());
|
||||
return nav_2d_utils::twist2Dto3D(odom_sub_->getTwist());
|
||||
else
|
||||
throw std::runtime_error("Failed to get twist");
|
||||
}
|
||||
|
|
@ -326,11 +326,11 @@ namespace nav_core_adapter
|
|||
{
|
||||
boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
|
||||
// Get the Pose
|
||||
robot_nav_2d_msgs::Pose2DStamped pose2d;
|
||||
nav_2d_msgs::Pose2DStamped pose2d;
|
||||
if (!getRobotPose(pose2d))
|
||||
return false;
|
||||
|
||||
robot_nav_2d_msgs::Twist2D velocity = odom_sub_->getTwist();
|
||||
nav_2d_msgs::Twist2D velocity = odom_sub_->getTwist();
|
||||
bool ret = planner_->isGoalReached(pose2d, velocity);
|
||||
if (ret)
|
||||
{
|
||||
|
|
@ -345,12 +345,12 @@ namespace nav_core_adapter
|
|||
bool LocalPlannerAdapter::setPlan(const std::vector<geometry_msgs::PoseStamped> &orig_global_plan)
|
||||
{
|
||||
boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
|
||||
robot_nav_2d_msgs::Path2D path = robot_nav_2d_utils::posesToPath2D(orig_global_plan);
|
||||
nav_2d_msgs::Path2D path = nav_2d_utils::posesToPath2D(orig_global_plan);
|
||||
try
|
||||
{
|
||||
if (path.poses.size() > 0)
|
||||
{
|
||||
robot_nav_2d_msgs::Pose2DStamped goal_pose;
|
||||
nav_2d_msgs::Pose2DStamped goal_pose;
|
||||
goal_pose = path.poses.back();
|
||||
|
||||
if (!has_active_goal_ || hasGoalChanged(goal_pose))
|
||||
|
|
@ -370,7 +370,7 @@ namespace nav_core_adapter
|
|||
}
|
||||
}
|
||||
|
||||
bool LocalPlannerAdapter::hasGoalChanged(const robot_nav_2d_msgs::Pose2DStamped &new_goal)
|
||||
bool LocalPlannerAdapter::hasGoalChanged(const nav_2d_msgs::Pose2DStamped &new_goal)
|
||||
{
|
||||
if (last_goal_.header.frame_id != new_goal.header.frame_id ||
|
||||
last_goal_.header.stamp.toSec() != new_goal.header.stamp.toSec())
|
||||
|
|
@ -382,7 +382,7 @@ namespace nav_core_adapter
|
|||
last_goal_.pose.theta != new_goal.pose.theta;
|
||||
}
|
||||
|
||||
bool LocalPlannerAdapter::getRobotPose(robot_nav_2d_msgs::Pose2DStamped &pose2d)
|
||||
bool LocalPlannerAdapter::getRobotPose(nav_2d_msgs::Pose2DStamped &pose2d)
|
||||
{
|
||||
geometry_msgs::PoseStamped current_pose;
|
||||
if (!costmap_robot_->getRobotPose(current_pose))
|
||||
|
|
@ -390,7 +390,7 @@ namespace nav_core_adapter
|
|||
std::cout << "Could not get robot pose" << std::endl;
|
||||
return false;
|
||||
}
|
||||
pose2d = robot_nav_2d_utils::poseStampedToPose2D(current_pose);
|
||||
pose2d = nav_2d_utils::poseStampedToPose2D(current_pose);
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -148,7 +148,7 @@ else()
|
|||
data_convert
|
||||
dl
|
||||
pthread
|
||||
robot_nav_2d_utils
|
||||
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/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_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_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 robot::move_base_core::BaseNavigation
|
||||
class MoveBase : public 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(robot::TFListenerPtr tf);
|
||||
MoveBase(TFListenerPtr tf);
|
||||
MoveBase();
|
||||
|
||||
/**
|
||||
* @brief Initialize the navigation system.
|
||||
* @param tf Shared pointer to the TF listener or buffer.
|
||||
*/
|
||||
virtual void initialize(robot::TFListenerPtr tf) override;
|
||||
virtual void initialize(TFListenerPtr tf) override;
|
||||
|
||||
/**
|
||||
* @brief Set the robot's footprint (outline shape) in the global frame.
|
||||
|
|
@ -196,40 +196,25 @@ 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);
|
||||
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
|
||||
|
|
@ -306,7 +291,7 @@ namespace move_base
|
|||
|
||||
private:
|
||||
bool initialized_;
|
||||
robot::TFListenerPtr tf_;
|
||||
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(robot::TFListenerPtr tf)
|
||||
move_base::MoveBase::MoveBase(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(robot::TFListenerPtr tf)
|
||||
void move_base::MoveBase::initialize(TFListenerPtr tf)
|
||||
{
|
||||
printf("[%s:%d]\n ========== Begin: initialize() ==========\n", __FILE__, __LINE__);
|
||||
if (!initialized_)
|
||||
|
|
@ -334,10 +334,10 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
|
|||
|
||||
// we'll start executing recovery behaviors at the beginning of our list
|
||||
recovery_index_ = 0;
|
||||
nav_feedback_ = std::make_shared<robot::move_base_core::NavFeedback>();
|
||||
nav_feedback_ = std::make_shared<move_base_core::NavFeedback>();
|
||||
if (nav_feedback_)
|
||||
{
|
||||
nav_feedback_->navigation_state = robot::move_base_core::State::PLANNING;
|
||||
nav_feedback_->navigation_state = 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 = robot::move_base_core::State::REJECTED;
|
||||
nav_feedback_->navigation_state = move_base_core::State::REJECTED;
|
||||
nav_feedback_->feed_back_str = std::string("The system has not been installed yet");
|
||||
nav_feedback_->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 = robot::move_base_core::State::PLANNING;
|
||||
nav_feedback_->navigation_state = move_base_core::State::PLANNING;
|
||||
nav_feedback_->feed_back_str = std::string("Planning");
|
||||
nav_feedback_->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 = robot::move_base_core::State::REJECTED;
|
||||
nav_feedback_->navigation_state = 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 = robot::move_base_core::State::REJECTED;
|
||||
nav_feedback_->navigation_state = move_base_core::State::REJECTED;
|
||||
nav_feedback_->feed_back_str = std::string("The system has not been installed yet");
|
||||
nav_feedback_->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 = robot::move_base_core::State::PLANNING;
|
||||
nav_feedback_->navigation_state = move_base_core::State::PLANNING;
|
||||
nav_feedback_->feed_back_str = std::string("Planning");
|
||||
nav_feedback_->goal_checked = false;
|
||||
}
|
||||
|
|
@ -539,7 +539,7 @@ bool move_base::MoveBase::moveStraightTo(const geometry_msgs::PoseStamped &goal,
|
|||
{
|
||||
if (nav_feedback_)
|
||||
{
|
||||
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
|
||||
nav_feedback_->navigation_state = move_base_core::State::REJECTED;
|
||||
nav_feedback_->feed_back_str = std::string("The system has not been installed yet");
|
||||
nav_feedback_->goal_checked = false;
|
||||
}
|
||||
|
|
@ -565,7 +565,7 @@ bool move_base::MoveBase::moveStraightTo(const geometry_msgs::PoseStamped &goal,
|
|||
|
||||
if (nav_feedback_)
|
||||
{
|
||||
nav_feedback_->navigation_state = robot::move_base_core::State::PLANNING;
|
||||
nav_feedback_->navigation_state = move_base_core::State::PLANNING;
|
||||
nav_feedback_->feed_back_str = std::string("Planning");
|
||||
nav_feedback_->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 = robot::move_base_core::State::REJECTED;
|
||||
nav_feedback_->navigation_state = move_base_core::State::REJECTED;
|
||||
nav_feedback_->feed_back_str = std::string("The system has not been installed yet");
|
||||
nav_feedback_->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 = robot::move_base_core::State::PLANNING;
|
||||
nav_feedback_->navigation_state = move_base_core::State::PLANNING;
|
||||
nav_feedback_->feed_back_str = std::string("Planning");
|
||||
nav_feedback_->goal_checked = false;
|
||||
}
|
||||
|
|
@ -625,12 +625,20 @@ bool move_base::MoveBase::rotateTo(const geometry_msgs::PoseStamped &goal, doubl
|
|||
{
|
||||
if (nav_feedback_)
|
||||
{
|
||||
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
|
||||
nav_feedback_->navigation_state = move_base_core::State::REJECTED;
|
||||
nav_feedback_->feed_back_str = std::string("Coudn't get robot pose");
|
||||
nav_feedback_->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;
|
||||
}
|
||||
|
|
@ -1275,7 +1283,10 @@ geometry_msgs::PoseStamped move_base::MoveBase::goalToGlobalFrame(const geometry
|
|||
geometry_msgs::PoseStamped goal_pose, global_pose;
|
||||
goal_pose = goal_pose_msg;
|
||||
|
||||
// just get the latest available transform... for accuracy they should send
|
||||
// goals in the frame of the planner
|
||||
goal_pose.header.stamp = robot::Time(); // latest available
|
||||
|
||||
try
|
||||
{
|
||||
tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, data_convert::convertTime(goal_pose.header.stamp));
|
||||
|
|
@ -1296,32 +1307,15 @@ geometry_msgs::PoseStamped move_base::MoveBase::goalToGlobalFrame(const geometry
|
|||
std::cerr << "Extrapolation Error looking up robot pose: " << ex.what() << std::endl;
|
||||
return goal_pose_msg;
|
||||
}
|
||||
|
||||
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)
|
||||
// {
|
||||
// }
|
||||
|
||||
robot::move_base_core::BaseNavigation::Ptr move_base::MoveBase::create()
|
||||
move_base_core::BaseNavigation::Ptr move_base::MoveBase::create()
|
||||
{
|
||||
return std::make_shared<move_base::MoveBase>();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -30,7 +30,7 @@
|
|||
#include <move_base/move_base.h>
|
||||
#include <iostream>
|
||||
#include <robot/node_handle.h>
|
||||
#include <robot_nav_2d_utils/parameters.h>
|
||||
#include <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<robot::move_base_core::BaseNavigation::Ptr()>(
|
||||
auto create_plugin = boost::dll::import_alias<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__);
|
||||
robot::move_base_core::BaseNavigation::Ptr nav_ptr = create_plugin();
|
||||
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