@@ -27,495 +27,494 @@
# include <robot/time.h>
# include <move_base_core/common.h>
namespace robot
{
namespace move_base_core
{
// Navigation states, including planning and controller status
enum class State
namespace move_base_core
{
PENDING ,
ACTIVE ,
PREEMPTED ,
SUCCEEDED ,
ABORTED ,
REJECTED ,
PREEMPTING ,
RECALLING ,
RECALLED ,
LOST ,
PLANNING ,
CONTROLLING ,
CLEARING ,
PAUSED
} ;
/**
* @brief Feedback structure to describe current navigation status
*/
struct NavFeedback
{
State navigation_state ; // Current navigation state
std : : string feed_back_str ; // Description or debug message
robot_geometry_msgs : : Pose2D current_pose ; // Robot’
bool goal_checked ; // Whether the current goal is verified
bool is_ready ; // Robot is ready for commands
} ;
/**
* @brief Planner data output structure.
*/
struct PlannerDataOutput
{
robot_nav_2d_msgs : : Path2D plan ;
robot_nav_msgs : : OccupancyGrid costmap ;
robot_map_msgs : : OccupancyGridUpdate costmap_update ;
bool is_costmap_updated ;
robot_geometry_msgs : : PolygonStamped footprint ;
} ;
/**
* @brief Convert a State enum to its string representation.
* Useful for debugging/logging or displaying in UI.
*
* @param state Enum value of move_base_core::State
* @return std::string The corresponding string, e.g. "PENDING"
*/
inline std : : string toString ( move_base_core : : State state )
{
using move_base_core : : State ;
switch ( state )
// Navigation states, including planning and controller status
enum class State
{
case State : : :
return " PENDING " ; // Chờ xử lý
case State : : ACTIVE :
return " ACTIVE " ; // Đang hoạt động
case State : : PREEMP TED:
return " PREEMPTED " ; // Đã bị huỷ bởi yêu cầu mới
case State : : SUCCEEDED :
return " SUCCEEDED " ; // Thành công
case State : : ABORT ED:
return " ABORTED " ; // Bị lỗi
case State : : REJECTED :
return " REJECTED " ; // Từ chối bởi planner hoặc controller
case State : : PREEMPT ING:
return " PREEMPTING " ; // Đang huỷ bỏ theo yêu cầu
case State : : RECALLING :
return " RECALLING " ; // Đang huỷ bỏ nội bộ
case State : : RECALLED :
return " RECALLED " ; // Đã được thu hồi
case State : : LOST :
return " LOST " ; // Mất trạng thái
case State : : PLANNING :
return " PLANNING " ; // Đang lập kế hoạch đường đi
case State : : CONTROLLING :
return " CONTROLLING " ; // Đang điều khiển robot di chuyển theo plan
case State : : CLEARING :
return " CLEARING " ; // Đang dọn dẹp bản đồ / costmap
case State : : PAUSED :
return " PAUSED " ; // Tạm dừng
default :
return " UNKNOWN_STATE " ; // Không xác định
,
ACTIVE ,
PREEMPTED ,
SUCCEEDED ,
ABOR TED ,
REJECTED ,
PREEMPTING ,
RECALLING ,
RECALL ED ,
LOST ,
PLANNING ,
CONTROLLING ,
CLEAR ING ,
PAUSED
} ;
/**
* @brief Feedback structure to describe current navigation status
*/
struct NavFeedback
{
State navigation_state ; // Current navigation state
std : : string feed_back_str ; // Description or debug message
robot_geometry_msgs : : Pose2D current_pose ; // Robot’
bool goal_checked ; // Whether the current goal is verified
bool is_ready ; // Robot is ready for commands
} ;
/**
* @brief Planner data output structure.
*/
struct PlannerDataOutput
{
robot_nav_2d_msgs : : Path2D plan ;
robot_nav_msgs : : OccupancyGrid costmap ;
robot_map_msgs : : OccupancyGridUpdate costmap_update ;
bool is_costmap_updated ;
robot_geometry_msgs : : PolygonStamped footprint ;
} ;
/**
* @brief Convert a State enum to its string representation.
* Useful for debugging/logging or displaying in UI.
*
* @param state Enum value of move_base_core::State
* @return std::string The corresponding string, e.g. "PENDING"
*/
inline std : : string toString ( move_base_core : : State state )
{
using move_base_core : : State ;
switch ( state )
{
case State : : PENDING :
return " PENDING " ; // Chờ xử lý
case State : : ACTIVE :
return " ACTIVE " ; // Đang hoạt động
case State : : PREEMPTED :
return " PREEMPTED " ; // Đã bị huỷ bởi yêu cầu mới
case State : : SUCCEEDED :
return " SUCCEEDED " ; // Thành công
case State : : ABORTED :
return " ABORTED " ; // Bị lỗi
case State : : REJECTED :
return " REJECTED " ; // Từ chối bởi planner hoặc controller
case State : : PREEMPTING :
return " PREEMPTING " ; // Đang huỷ bỏ theo yêu cầu
case State : : RECALLING :
return " RECALLING " ; // Đang huỷ bỏ nội bộ
case State : : RECALLED :
return " RECALLED " ; // Đã được thu hồi
case State : : LOST :
return " LOST " ; // Mất trạng thái
case State : : PLANNING :
return " PLANNING " ; // Đang lập kế hoạch đường đi
case State : : CONTROLLING :
return " CONTROLLING " ; // Đang điều khiển robot di chuyển theo plan
case State : : CLEARING :
return " CLEARING " ; // Đang dọn dẹp bản đồ / costmap
case State : : PAUSED :
return " PAUSED " ; // Tạm dừng
default :
return " UNKNOWN_STATE " ; // Không xác định
}
}
}
/**
* @brief Creates a target pose (PoseStamped) by offsetting a given 2D pose along its heading direction.
*
* This function calculates a new position by moving forward (or backward if negative)
* a certain `offset_distance` from the current position, following the given heading angle (theta).
*
* @param pose The original 2D pose (x, y, theta) in the local plane.
* @param frame_id The coordinate frame in which the output PoseStamped will be expressed.
* @param offset_distance The distance to offset along the heading direction, in meters.
* Positive moves forward, negative moves backward.
* @return A new PoseStamped offset from the input pose, in the given frame.
*/
inline robot_geometry_msgs : : PoseStamped offset_goal ( const robot_geometry_msgs : : Pose2D & pose , const std : : string & frame_id , double offset_distance )
{
robot_geometry_msgs : : PoseStamped goal ;
goal . header . frame_id = frame_id ;
goal . header . stamp = robot : : Time : : now ( ) ;
goal . pose . position . x = pose . x + offset_distance * cos ( pose . theta ) ;
goal . pose . position . y = pose . y + offset_distance * sin ( pose . theta ) ;
tf3 : : Quaternion q ;
q . setRPY ( 0 , 0 , pose . theta ) ;
goal . pose . orientation = tf3 : : toMsg ( q ) ;
return goal ;
}
/**
* @brief Overloaded version: creates an offset target pose from a given PoseStamped.
*
* This function extracts the 2D position and orientation (yaw) from the given PoseStamped,
* offsets it forward (or backward) along the current heading direction,
* and returns a new PoseStamped in the same frame.
*
* @param pose The original pose with full position and orientation.
* @param offset_distance Distance to offset along the current heading direction (in meters).
* @return A new PoseStamped offset from the original pose.
*/
inline robot_geometry_msgs : : PoseStamped offset_goal ( const robot_geometry_msgs : : PoseStamped & pose , double offset_distance )
{
robot_geometry_msgs : : Pose2D pose2d ;
pose2d . x = pose . pose . position . x ;
pose2d . y = pose . pose . position . y ;
// pose2d.theta = tf2::getYaw(pose.pose.orientation);
return offset_goal ( pose2d , pose . header . frame_id , offset_distance ) ;
}
/**
* @class BaseNavigation
* @brief Abstract interface for robot navigation modules.
*
* Provides core methods for setting goals, moving, rotating, and handling motion control.
* All navigation logic must implement this interface.
*/
class BaseNavigation
{
public :
using Ptr = std : : shared_ptr < BaseNavigation > ;
virtual ~ BaseNavigation ( ) { }
/**
* @brief Initialize the navigation system .
* @param tf Shared pointer to the TF listener or buffer.
*/
virtual void initialize ( TFListenerPtr tf ) = 0 ;
/**
* @brief Set the robot's footprint (outline shape) in the global frame.
* This can be used for planning or collision checking.
* @brief Creates a target pose (PoseStamped) by offsetting a given 2D pose along its heading direction .
*
* @param fprt A vector of points representing the robot's footprint polygon.
* The points should be ordered counter-clockwise .
* Example:
* This function calculates a new position by moving forward (or backward if negative)
* a certain `offset_distance` from the current position, following the given heading angle (theta) .
*
^ Y
|
| P3(-0.3, 0.2) P2(0.3, 0.2)
| ●---------------●
| | |
| | Robot | (view Top )
| | |
| ●---------------●
| P4(-0.3, -0.2) P1(0.3, -0.2)
+-------------------------------> X
std::vector<robot_geometry_msgs::Point> footprint;
1. footprint.push_back(make_point(0.3, -0.2));
2. footprint.push_back(make_point(0.3, 0.2));
3. footprint.push_back(make_point(-0.3, 0.2));
4. footprint.push_back(make_point(-0.3, -0.2));
* @param pose The original 2D pose (x, y, theta) in the local plane.
* @param frame_id The coordinate frame in which the output PoseStamped will be expressed.
* @param offset_distance The distance to offset along the heading direction, in meters.
* Positive moves forward, negative moves backward.
* @return A new PoseStamped offset from the input pose, in the given frame.
*/
virtual void setRobotFootprint ( const std : : vector < robot_geometry_msgs : : Point > & fprt ) = 0 ;
inline robot_geometry_msgs : : PoseStamped offset_goal ( const robot_geometry_msgs : : Pose2D & pose , const std : : string & frame_id , double offset_distance )
{
robot_geometry_msgs : : PoseStamped goal ;
goal . header . frame_id = frame_id ;
goal . header . stamp = robot : : Time : : now ( ) ;
goal . pose . position . x = pose . x + offset_distance * cos ( pose . theta ) ;
goal . pose . position . y = pose . y + offset_distance * sin ( pose . theta ) ;
tf3 : : Quaternion q ;
q . setRPY ( 0 , 0 , pose . theta ) ;
goal . pose . orientation = tf3 : : toMsg ( q ) ;
return goal ;
}
/**
* @brief Get the robot's footprint (outline shape) in the global frame .
* @return A vector of points representing the robot's footprint polygon.
* @brief Overloaded version: creates an offset target pose from a given PoseStamped .
*
* This function extracts the 2D position and orientation (yaw) from the given PoseStamped,
* offsets it forward (or backward) along the current heading direction,
* and returns a new PoseStamped in the same frame.
*
* @param pose The original pose with full position and orientation.
* @param offset_distance Distance to offset along the current heading direction (in meters).
* @return A new PoseStamped offset from the original pose.
*/
virtual std : : vector < robot_geometry_msgs : : Point > getRobotFootprint ( ) = 0 ;
inline robot_geometry_msgs : : PoseStamped offset_goal ( const robot_geometry_msgs : : PoseStamped & pose , double offset_distance )
{
robot_geometry_msgs : : Pose2D pose2d ;
pose2d . x = pose . pose . position . x ;
pose2d . y = pose . pose . position . y ;
// pose2d.theta = tf2::getYaw(pose.pose.orientation);
return offset_goal ( pose2d , pose . header . frame_id , offset_distance ) ;
}
/**
* @brief Add a static map to the n avigation system.
* @param map_name The name of the map .
* @param map The map to add.
* @class BaseN avigation
* @brief Abstract interface for robot navigation modules .
*
* Provides core methods for setting goals, moving, rotating, and handling motion control.
* All navigation logic must implement this interface.
*/
virtual void addStaticMap ( const std : : string & map_name , robot_nav_msgs : : OccupancyGrid map ) = 0 ;
class BaseNavigation
{
public :
using Ptr = std : : shared_ptr < BaseNavigation > ;
/**
* @brief Add a laser scan to the navigation system.
* @param laser_scan_name The name of the laser scan.
* @param laser_scan The laser scan to add.
*/
virtual void addLaserScan ( const std : : string & laser_scan_name , robot_sensor_msgs : : LaserScan laser_scan ) = 0 ;
/**
* @brief Add a point cloud to the navigation system.
* @param point_cloud_name The name of the point cloud.
* @param point_cloud The point cloud to add.
*/
virtual void addPointCloud ( const std : : string & point_cloud_name , robot_sensor_msgs : : PointCloud point_cloud ) = 0 ;
/**
* @brief Add a point cloud2 to the navigation system.
* @param point_cloud2_name The name of the point cloud2.
* @param point_cloud2 The point cloud2 to add.
*/
virtual void addPointCloud2 ( const std : : string & point_cloud2_name , robot_sensor_msgs : : PointCloud2 point_cloud2 ) = 0 ;
/**
* @brief Get a static map from the navigation system.
* @param map_name The name of the map.
* @return The map.
*/
virtual robot_nav_msgs : : OccupancyGrid getStaticMap ( const std : : string & map_name ) = 0 ;
/**
* @brief Get a laser scan from the navigation system.
* @param laser_scan_name The name of the laser scan.
* @return The laser scan.
*/
virtual robot_sensor_msgs : : LaserScan getLaserScan ( const std : : string & laser_scan_name ) = 0 ;
/**
* @brief Get a point cloud from the navigation system.
* @param point_cloud_name The name of the point cloud.
* @return The point cloud.
*/
virtual robot_sensor_msgs : : PointCloud getPointCloud ( const std : : string & point_cloud_name ) = 0 ;
/**
* @brief Get a point cloud2 from the navigation system.
* @param point_cloud2_name The name of the point cloud2.
* @return The point cloud2.
*/
virtual robot_sensor_msgs : : PointCloud2 getPointCloud2 ( const std : : string & point_cloud2_name ) = 0 ;
/**
* @brief Get all static maps from the navigation system.
* @return The static maps.
*/
virtual std : : map < std : : string , robot_nav_msgs : : OccupancyGrid > getAllStaticMaps ( ) = 0 ;
/**
* @brief Get all laser scans from the navigation system.
* @return The laser scans.
*/
virtual std : : map < std : : string , robot_sensor_msgs : : LaserScan > getAllLaserScans ( ) = 0 ;
/**
* @brief Get all point clouds from the navigation system.
* @return The point clouds.
*/
virtual std : : map < std : : string , robot_sensor_msgs : : PointCloud > getAllPointClouds ( ) = 0 ;
/**
* @brief Get all point cloud2s from the navigation system.
* @return The point cloud2s.
*/
virtual std : : map < std : : string , robot_sensor_msgs : : PointCloud2 > getAllPointCloud2s ( ) = 0 ;
/**
* @brief Remove a static map from the navigation system.
* @param map_name The name of the map.
* @return True if the map was removed successfully.
*/
virtual bool removeStaticMap ( const std : : string & map_name ) = 0 ;
/**
* @brief Remove a laser scan from the navigation system.
* @param laser_scan_name The name of the laser scan.
* @return True if the laser scan was removed successfully.
*/
virtual bool removeLaserScan ( const std : : string & laser_scan_name ) = 0 ;
/**
* @brief Remove a point cloud from the navigation system.
* @param point_cloud_name The name of the point cloud.
* @return True if the point cloud was removed successfully.
*/
virtual bool removePointCloud ( const std : : string & point_cloud_name ) = 0 ;
/**
* @brief Remove a point cloud2 from the navigation system.
* @param point_cloud2_name The name of the point cloud2.
* @return True if the point cloud2 was removed successfully.
*/
virtual bool removePointCloud2 ( const std : : string & point_cloud2_name ) = 0 ;
/**
* @brief Remove all static maps from the navigation system.
* @return True if the static maps were removed successfully.
*/
virtual bool removeAllStaticMaps ( ) = 0 ;
/**
* @brief Remove all laser scans from the navigation system.
* @return True if the laser scans were removed successfully.
*/
virtual bool removeAllLaserScans ( ) = 0 ;
/**
* @brief Remove all point clouds from the navigation system.
* @return True if the point clouds were removed successfully.
*/
virtual bool removeAllPointClouds ( ) = 0 ;
/**
* @brief Remove all point cloud2s from the navigation system.
* @return True if the point cloud2s were removed successfully.
*/
virtual bool removeAllPointCloud2s ( ) = 0 ;
/**
* @brief Remove all data from the navigation system.
* @return True if the data was removed successfully.
*/
virtual bool removeAllData ( ) = 0 ;
virtual ~ BaseNavigation ( ) { }
/**
* @brief Add an odometry to the navigation system.
* @param odometry_name The name of the odometry .
* @param odometry The odometry to add.
*/
virtual void addOdometry ( const std : : string & odometry_name , robot_nav_msgs : : Odometry odometry ) = 0 ;
/**
* @brief Initialize the navigation system.
* @param tf Shared pointer to the TF listener or buffer .
*/
virtual void initialize ( TFListenerPtr tf ) = 0 ;
/**
* @brief Send a goal for the robot to navigate to .
* @param goal Target pose i n t he global frame .
* @param xy_goal_tolerance Acceptable error in X/Y (meters).
* @param yaw_goal_tolerance Acceptable angular error (radians) .
* @return True if goal was accepted and sent successfully .
*/
virtual bool moveTo ( const robot_geometry_msgs : : PoseStamped & goal ,
double xy_goal_tolerance = 0.0 ,
double yaw_goal_tolerance = 0.0 ) = 0 ;
/**
* @brief Set the robot's footprint (outline shape) in the global frame .
* This can be used for planning or collisio n c hecking .
*
* @param fprt A vector of points representing the robot's footprint polygon .
* The points should be ordered counter-clockwise .
* Example:
*
^ Y
|
| P3(-0.3, 0.2) P2(0.3, 0.2)
| ●---------------●
| | |
| | Robot | (view Top )
| | |
| ●---------------●
| P4(-0.3, -0.2) P1(0.3, -0.2)
+-------------------------------> X
/**
* @brief Send a goal for the robot to navigate to.
* @param msg Order message.
* @param goal Target pose in the global frame.
* @param xy_goal_tolerance Acceptable error in X/Y (meters).
* @param yaw_goal_tolerance Acceptable angular error (radians).
* @return True if goal was accepted and sent successfully.
*/
virtual bool moveTo ( const robot_protocol_msgs : : Order & msg ,
const robot_geometry_msgs : : PoseStamped & goal ,
double xy_goal_tolerance = 0.0 ,
double yaw_goal_tolerance = 0.0 ) = 0 ;
std::vector<robot_geometry_msgs::Point> footprint;
1. footprint.push_back(make_point(0.3, -0.2));
2. footprint.push_back(make_point(0.3, 0.2));
3. footprint.push_back(make_point(-0.3, 0.2));
4. footprint.push_back(make_point(-0.3, -0.2));
*/
virtual void setRobotFootprint ( const std : : vector < robot_geometry_msgs : : Point > & fprt ) = 0 ;
/**
* @brief Send a docking goal to a predefined marker (e.g. charger) .
* @param maker Marker name or ID .
* @param goal Target pose for docking.
* @param xy_goal_tolerance Acceptable XY error (meters).
* @param yaw_goal_tolerance Acceptable heading error (radians).
* @return True if docking command succeeded.
*/
virtual bool dockTo ( const std : : string & maker ,
const robot_geometry_msgs : : PoseStamped & goal ,
double xy_goal_tolerance = 0.0 ,
double yaw_goal_tolerance = 0.0 ) = 0 ;
/**
* @brief Get the robot's footprint (outline shape) in the global frame .
* @return A vector of points representing the robot's footprint polygon .
*/
virtual std : : vector < robot_geometry_msgs : : Point > getRobotFootprint ( ) = 0 ;
/**
* @brief Sen d a docking goal to a predefined marker (e.g. charger) .
* @param msg Order message .
* @param goal Target pose for docking .
* @param xy_goal_tolerance Acceptable XY error (meters).
* @param yaw_goal_tolerance Acceptable heading error (radians).
* @return True if docking command succeeded.
*/
virtual bool dockTo ( const robot_protocol_msgs : : Order & msg ,
const robot_geometry_msgs : : PoseStamped & goal ,
double xy_goal_tolerance = 0.0 ,
double yaw_goal_tolerance = 0.0 ) = 0 ;
/**
* @brief Ad d a static map to the navigation system .
* @param map_name The name of the map .
* @param map The map to add .
*/
virtual void addStaticMap ( const std : : string & map_name , robot_nav_msgs : : OccupancyGrid map ) = 0 ;
/**
* @brief Move straight toward the target position (X-axis) .
* @param goal Target pose .
* @param xy_goal_tolerance Acceptable positional error (meters) .
* @return True if command issued successfully.
*/
virtual bool moveStraightTo ( const robot_geometry_msgs : : PoseStamped & goal ,
double xy_goal_tolerance = 0.0 ) = 0 ;
/**
* @brief Add a laser scan to the navigation system .
* @param laser_scan_name The name of the laser scan .
* @param laser_scan The laser scan to add .
*/
virtual void addLaserScan ( const std : : string & laser_scan_name , robot_sensor_msgs : : LaserScan laser_scan ) = 0 ;
/**
* @brief Rotate in place to align with target orientation .
* @param goal Pose containing desired heading (only Z-axis used) .
* @param yaw_goal_tolerance Acceptable angular error (radians) .
* @return True if rotation command was sent successfully.
*/
virtual bool rotateTo ( const robot_geometry_msgs : : PoseStamped & goal ,
/**
* @brief Add a point cloud to the navigation system .
* @param point_cloud_name The name of the point cloud .
* @param point_cloud The point cloud to add .
*/
virtual void addPointCloud ( const std : : string & point_cloud_name , robot_sensor_msgs : : PointCloud point_cloud ) = 0 ;
/**
* @brief Add a point cloud2 to the navigation system.
* @param point_cloud2_name The name of the point cloud2.
* @param point_cloud2 The point cloud2 to add.
*/
virtual void addPointCloud2 ( const std : : string & point_cloud2_name , robot_sensor_msgs : : PointCloud2 point_cloud2 ) = 0 ;
/**
* @brief Get a static map from the navigation system.
* @param map_name The name of the map.
* @return The map.
*/
virtual robot_nav_msgs : : OccupancyGrid getStaticMap ( const std : : string & map_name ) = 0 ;
/**
* @brief Get a laser scan from the navigation system.
* @param laser_scan_name The name of the laser scan.
* @return The laser scan.
*/
virtual robot_sensor_msgs : : LaserScan getLaserScan ( const std : : string & laser_scan_name ) = 0 ;
/**
* @brief Get a point cloud from the navigation system.
* @param point_cloud_name The name of the point cloud.
* @return The point cloud.
*/
virtual robot_sensor_msgs : : PointCloud getPointCloud ( const std : : string & point_cloud_name ) = 0 ;
/**
* @brief Get a point cloud2 from the navigation system.
* @param point_cloud2_name The name of the point cloud2.
* @return The point cloud2.
*/
virtual robot_sensor_msgs : : PointCloud2 getPointCloud2 ( const std : : string & point_cloud2_name ) = 0 ;
/**
* @brief Get all static maps from the navigation system.
* @return The static maps.
*/
virtual std : : map < std : : string , robot_nav_msgs : : OccupancyGrid > getAllStaticMaps ( ) = 0 ;
/**
* @brief Get all laser scans from the navigation system.
* @return The laser scans.
*/
virtual std : : map < std : : string , robot_sensor_msgs : : LaserScan > getAllLaserScans ( ) = 0 ;
/**
* @brief Get all point clouds from the navigation system.
* @return The point clouds.
*/
virtual std : : map < std : : string , robot_sensor_msgs : : PointCloud > getAllPointClouds ( ) = 0 ;
/**
* @brief Get all point cloud2s from the navigation system.
* @return The point cloud2s.
*/
virtual std : : map < std : : string , robot_sensor_msgs : : PointCloud2 > getAllPointCloud2s ( ) = 0 ;
/**
* @brief Remove a static map from the navigation system.
* @param map_name The name of the map.
* @return True if the map was removed successfully.
*/
virtual bool removeStaticMap ( const std : : string & map_name ) = 0 ;
/**
* @brief Remove a laser scan from the navigation system.
* @param laser_scan_name The name of the laser scan.
* @return True if the laser scan was removed successfully.
*/
virtual bool removeLaserScan ( const std : : string & laser_scan_name ) = 0 ;
/**
* @brief Remove a point cloud from the navigation system.
* @param point_cloud_name The name of the point cloud.
* @return True if the point cloud was removed successfully.
*/
virtual bool removePointCloud ( const std : : string & point_cloud_name ) = 0 ;
/**
* @brief Remove a point cloud2 from the navigation system.
* @param point_cloud2_name The name of the point cloud2.
* @return True if the point cloud2 was removed successfully.
*/
virtual bool removePointCloud2 ( const std : : string & point_cloud2_name ) = 0 ;
/**
* @brief Remove all static maps from the navigation system.
* @return True if the static maps were removed successfully.
*/
virtual bool removeAllStaticMaps ( ) = 0 ;
/**
* @brief Remove all laser scans from the navigation system.
* @return True if the laser scans were removed successfully.
*/
virtual bool removeAllLaserScans ( ) = 0 ;
/**
* @brief Remove all point clouds from the navigation system.
* @return True if the point clouds were removed successfully.
*/
virtual bool removeAllPointClouds ( ) = 0 ;
/**
* @brief Remove all point cloud2s from the navigation system.
* @return True if the point cloud2s were removed successfully.
*/
virtual bool removeAllPointCloud2s ( ) = 0 ;
/**
* @brief Remove all data from the navigation system.
* @return True if the data was removed successfully.
*/
virtual bool removeAllData ( ) = 0 ;
/**
* @brief Add an odometry to the navigation system.
* @param odometry_name The name of the odometry.
* @param odometry The odometry to add.
*/
virtual void addOdometry ( const std : : string & odometry_name , robot_nav_msgs : : Odometry odometry ) = 0 ;
/**
* @brief Send a goal for the robot to navigate to.
* @param goal Target pose in the global frame.
* @param xy_goal_tolerance Acceptable error in X/Y (meters).
* @param yaw_goal_tolerance Acceptable angular error (radians).
* @return True if goal was accepted and sent successfully.
*/
virtual bool moveTo ( const robot_geometry_msgs : : PoseStamped & goal ,
double xy_goal_tolerance = 0.0 ,
double yaw_goal_tolerance = 0.0 ) = 0 ;
/**
* @brief Pause the robot's movement (e.g. during obstacle avoidance) .
*/
virtual void pause ( ) = 0 ;
/**
* @brief Send a goal for the robot to navigate to .
* @param msg Order message.
* @param goal Target pose in the global frame.
* @param xy_goal_tolerance Acceptable error in X/Y (meters).
* @param yaw_goal_tolerance Acceptable angular error (radians).
* @return True if goal was accepted and sent successfully.
*/
virtual bool moveTo ( const robot_protocol_msgs : : Order & msg ,
const robot_geometry_msgs : : PoseStamped & goal ,
double xy_goal_tolerance = 0.0 ,
double yaw_goal_tolerance = 0.0 ) = 0 ;
/**
* @brief Resume motion after a pause .
*/
virtual void resume ( ) = 0 ;
/**
* @brief Send a docking goal to a predefined marker (e.g. charger) .
* @param maker Marker name or ID.
* @param goal Target pose for docking.
* @param xy_goal_tolerance Acceptable XY error (meters).
* @param yaw_goal_tolerance Acceptable heading error (radians).
* @return True if docking command succeeded.
*/
virtual bool dockTo ( const std : : string & maker ,
const robot_geometry_msgs : : PoseStamped & goal ,
double xy_goal_tolerance = 0.0 ,
double yaw_goal_tolerance = 0.0 ) = 0 ;
/**
* @brief Cancel the current goal and stop the robot .
*/
virtual void cancel ( ) = 0 ;
/**
* @brief Send a docking goal to a predefined marker (e.g. charger) .
* @param msg Order message.
* @param goal Target pose for docking.
* @param xy_goal_tolerance Acceptable XY error (meters).
* @param yaw_goal_tolerance Acceptable heading error (radians).
* @return True if docking command succeeded.
*/
virtual bool dockTo ( const robot_protocol_msgs : : Order & msg ,
const robot_geometry_msgs : : PoseStamped & goal ,
double xy_goal_tolerance = 0.0 ,
double yaw_goal_tolerance = 0.0 ) = 0 ;
/**
* @brief Send limited linear velocity command .
* @param linear Linear velocity vector (x, y, z) .
* @return True if the command was accepted .
*/
virtual bool setTwistLinear ( const robot_geometry_msgs : : Vector3 & linear ) = 0 ;
/**
* @brief Move straight toward the target position (X-axis) .
* @param goal Target pose .
* @param xy_goal_tolerance Acceptable positional error (meters) .
* @return True if command issued successfully.
*/
virtual bool moveStraightTo ( const robot_geometry_msgs : : PoseStamped & goal ,
double xy_goal_tolerance = 0.0 ) = 0 ;
/**
* @brief Send limited angular velocity command .
* @param angular Angular velocity vector (x, y, z ).
* @return True if the command was accepted .
*/
virtual bool setTwistAngular ( const robot_geometry_msgs : : Vector3 & angular ) = 0 ;
/**
* @brief Rotate in place to align with target orientation .
* @param goal Pose containing desired heading (only Z-axis used ).
* @param yaw_goal_tolerance Acceptable angular error (radians) .
* @return True if rotation command was sent successfully.
*/
virtual bool rotateTo ( const robot_geometry_msgs : : PoseStamped & goal ,
double yaw_goal_tolerance = 0.0 ) = 0 ;
/**
* @brief Get the robot’ pose as a PoseStamped .
* @param pose Output parameter with the robot’
* @return True if pose was successfully retrieved.
*/
virtual bool getRobotPose ( robot_geometry_msgs : : PoseStamped & pose ) = 0 ;
/**
* @brief Pause the robot' s movement (e.g. during obstacle avoidance) .
*/
virtual void pause ( ) = 0 ;
/**
* @brief Get the robot’ se.
* @param pose Output parameter with the robot’
* @return True if pose was successfully retrieved.
*/
virtual bool getRobotPose ( robot_geometry_msgs : : Pose2D & pose ) = 0 ;
/**
* @brief Resume motion after a pau se.
*/
virtual void resume ( ) = 0 ;
/**
* @brief Get the robot’ t.
* @return The robot’
*/
virtual robot_nav_2d_msgs : : Twist2DStamped getTwist ( ) = 0 ;
/**
* @brief Cancel the current goal and stop the robo t.
*/
virtual void cancel ( ) = 0 ;
/**
* @brief Get the navigation feedback .
* @return Pointer to the navigation feedback .
*/
virtual NavFeedback * getFeedback ( ) = 0 ;
/**
* @brief Send limited linear velocity command .
* @param linear Linear velocity vector (x, y, z) .
* @return True if the command was accepted.
*/
virtual bool setTwistLinear ( const robot_geometry_msgs : : Vector3 & linear ) = 0 ;
/**
* @brief Get the global data .
* @return The global data .
*/
virtual PlannerDataOutput getGlobalData ( ) = 0 ;
/**
* @brief Send limited angular velocity command .
* @param angular Angular velocity vector (x, y, z) .
* @return True if the command was accepted.
*/
virtual bool setTwistAngular ( const robot_geometry_msgs : : Vector3 & angular ) = 0 ;
/**
* @brief Get the local data .
* @return The local data .
*/
virtual PlannerDataOutput getLocalData ( ) = 0 ;
protected :
// Shared feedback data for navigation status tracking
std : : shared_ptr < NavFeedback > nav_feedback_ ;
robot_nav_2d_msgs : : Twist2DStamped twist_ ;
robot_nav_msgs : : Odometry odometry_ ;
PlannerDataOutput global_data_ ;
PlannerDataOutput local_data_ ;
/**
* @brief Get the robot’ .
* @param pose Output parameter with the robot’ .
* @return True if pose was successfully retrieved.
*/
virtual bool getRobotPose ( robot_geometry_msgs : : PoseStamped & pose ) = 0 ;
std : : map < std : : string , robot_nav_msgs : : OccupancyGrid > static_maps_ ;
std : : map < std : : string , robot_sensor_msgs : : LaserScan > laser_scans_ ;
std : : map < std : : string , robot_sensor_msgs : : PointCloud > point_clouds_ ;
std : : map < std : : string , robot_sensor_msgs : : PointCloud2 > point_cloud2s_ ;
/**
* @brief Get the robot’
* @param pose Output parameter with the robot’
* @return True if pose was successfully retrieved.
*/
virtual bool getRobotPose ( robot_geometry_msgs : : Pose2D & pose ) = 0 ;
BaseNavigation ( ) = default ;
} ;
/**
* @brief Get the robot’
* @return The robot’
*/
virtual robot_nav_2d_msgs : : Twist2DStamped getTwist ( ) = 0 ;
} // namespace move_base_core
/**
* @brief Get the navigation feedback.
* @return Pointer to the navigation feedback.
*/
virtual NavFeedback * getFeedback ( ) = 0 ;
/**
* @brief Get the global data.
* @return The global data.
*/
virtual PlannerDataOutput getGlobalData ( ) = 0 ;
/**
* @brief Get the local data.
* @return The local data.
*/
virtual PlannerDataOutput getLocalData ( ) = 0 ;
protected :
// Shared feedback data for navigation status tracking
std : : shared_ptr < NavFeedback > nav_feedback_ ;
robot_nav_2d_msgs : : Twist2DStamped twist_ ;
robot_nav_msgs : : Odometry odometry_ ;
PlannerDataOutput global_data_ ;
PlannerDataOutput local_data_ ;
std : : map < std : : string , robot_nav_msgs : : OccupancyGrid > static_maps_ ;
std : : map < std : : string , robot_sensor_msgs : : LaserScan > laser_scans_ ;
std : : map < std : : string , robot_sensor_msgs : : PointCloud > point_clouds_ ;
std : : map < std : : string , robot_sensor_msgs : : PointCloud2 > point_cloud2s_ ;
BaseNavigation ( ) = default ;
} ;
} // namespace move_base_core
} ;
# endif // _MOVE_BASE_CORE_NAVIGATION_H_INCLUDED_
