Public Member Functions | Private Member Functions | Private Attributes | List of all members
mbf_costmap_nav::CostmapControllerExecution Class Reference

The CostmapControllerExecution binds a local costmap to the AbstractControllerExecution and uses the nav_core/BaseLocalPlanner class as base plugin interface. This class makes move_base_flex compatible to the old move_base. More...

#include <costmap_controller_execution.h>

Inheritance diagram for mbf_costmap_nav::CostmapControllerExecution:
Inheritance graph
[legend]

Public Member Functions

 CostmapControllerExecution (const std::string &controller_name, const mbf_costmap_core::CostmapController::Ptr &controller_ptr, const ros::Publisher &vel_pub, const ros::Publisher &goal_pub, const TFPtr &tf_listener_ptr, const CostmapWrapper::Ptr &costmap_ptr, const MoveBaseFlexConfig &config)
 Constructor. More...
 
virtual ~CostmapControllerExecution ()
 Destructor. More...
 
- Public Member Functions inherited from mbf_abstract_nav::AbstractControllerExecution
 AbstractControllerExecution (const std::string &name, const mbf_abstract_core::AbstractController::Ptr &controller_ptr, const ros::Publisher &vel_pub, const ros::Publisher &goal_pub, const TFPtr &tf_listener_ptr, const MoveBaseFlexConfig &config)
 
virtual bool cancel ()
 
ros::Time getLastPluginCallTime () const
 
ControllerState getState () const
 
geometry_msgs::TwistStamped getVelocityCmd () const
 
bool isMoving () const
 
bool isPatienceExceeded () const
 
void reconfigure (const MoveBaseFlexConfig &config)
 
bool setControllerFrequency (double frequency)
 
void setNewPlan (const std::vector< geometry_msgs::PoseStamped > &plan, bool tolerance_from_action=false, double action_dist_tolerance=1.0, double action_angle_tolerance=3.1415)
 
virtual bool start ()
 
virtual ~AbstractControllerExecution ()
 
- Public Member Functions inherited from mbf_abstract_nav::AbstractExecutionBase
 AbstractExecutionBase (const std::string &name)
 
const std::string & getMessage () const
 
const std::string & getName () const
 
uint32_t getOutcome () const
 
void join ()
 
virtual void reconfigure (MoveBaseFlexConfig &_cfg)
 
virtual void stop ()
 
boost::cv_status waitForStateUpdate (boost::chrono::microseconds const &duration)
 
virtual ~AbstractExecutionBase ()
 

Private Member Functions

virtual uint32_t computeVelocityCmd (const geometry_msgs::PoseStamped &robot_pose, const geometry_msgs::TwistStamped &robot_velocity, geometry_msgs::TwistStamped &vel_cmd, std::string &message)
 Request plugin for a new velocity command. We override this method so we can lock the local costmap before calling the planner. More...
 
void postRun ()
 Implementation-specific cleanup function, called right after execution. This method overrides abstract execution empty implementation with underlying map-specific cleanup code. More...
 
void preRun ()
 Implementation-specific setup function, called right before execution. This method overrides abstract execution empty implementation with underlying map-specific setup code. More...
 
bool safetyCheck ()
 Implementation-specific safety check, called during execution to ensure it's safe to drive. This method overrides abstract execution empty implementation with underlying map-specific checks, more precisely if controller costmap is current. More...
 
mbf_abstract_nav::MoveBaseFlexConfig toAbstract (const MoveBaseFlexConfig &config)
 

Private Attributes

std::string controller_name_
 name of the controller plugin assigned by the class loader More...
 
const CostmapWrapper::Ptrcostmap_ptr_
 Shared pointer to thr local costmap. More...
 
bool lock_costmap_
 Whether to lock costmap before calling the controller (see issue #4 for details) More...
 

Additional Inherited Members

- Public Types inherited from mbf_abstract_nav::AbstractControllerExecution
enum  ControllerState
 
typedef boost::shared_ptr< AbstractControllerExecutionPtr
 
- Public Attributes inherited from mbf_abstract_nav::AbstractControllerExecution
 ARRIVED_GOAL
 
 CANCELED
 
 EMPTY_PLAN
 
 GOT_LOCAL_CMD
 
 INITIALIZED
 
 INTERNAL_ERROR
 
 INVALID_PLAN
 
 MAX_RETRIES
 
 NO_LOCAL_CMD
 
 NO_PLAN
 
 PAT_EXCEEDED
 
 PLANNING
 
 STARTED
 
 STOPPED
 
- Static Public Attributes inherited from mbf_abstract_nav::AbstractControllerExecution
static const double DEFAULT_CONTROLLER_FREQUENCY
 
- Protected Member Functions inherited from mbf_abstract_nav::AbstractControllerExecution
virtual void run ()
 
void setVelocityCmd (const geometry_msgs::TwistStamped &vel_cmd_stamped)
 
- Protected Attributes inherited from mbf_abstract_nav::AbstractControllerExecution
mbf_abstract_core::AbstractController::Ptr controller_
 
std::string global_frame_
 
ros::Time last_call_time_
 
ros::Time last_valid_cmd_time_
 
int max_retries_
 
ros::Duration patience_
 
std::string plugin_name_
 
std::string robot_frame_
 
ros::Time start_time_
 
const TFPtrtf_listener_ptr
 
- Protected Attributes inherited from mbf_abstract_nav::AbstractExecutionBase
bool cancel_
 
boost::condition_variable condition_
 
std::string message_
 
std::string name_
 
uint32_t outcome_
 
boost::thread thread_
 

Detailed Description

The CostmapControllerExecution binds a local costmap to the AbstractControllerExecution and uses the nav_core/BaseLocalPlanner class as base plugin interface. This class makes move_base_flex compatible to the old move_base.

Definition at line 60 of file costmap_controller_execution.h.

Constructor & Destructor Documentation

◆ CostmapControllerExecution()

mbf_costmap_nav::CostmapControllerExecution::CostmapControllerExecution ( const std::string &  controller_name,
const mbf_costmap_core::CostmapController::Ptr controller_ptr,
const ros::Publisher vel_pub,
const ros::Publisher goal_pub,
const TFPtr tf_listener_ptr,
const CostmapWrapper::Ptr costmap_ptr,
const MoveBaseFlexConfig &  config 
)

Constructor.

Parameters
controller_nameName of the controller to use.
controller_ptrShared pointer to the plugin to use.
vel_pubVelocity commands publisher.
goal_pubGoal pose publisher (just vor visualization).
tf_listener_ptrShared pointer to a common tf listener.
costmap_ptrShared pointer to the local costmap.
configCurrent server configuration (dynamic).

Definition at line 45 of file costmap_controller_execution.cpp.

◆ ~CostmapControllerExecution()

mbf_costmap_nav::CostmapControllerExecution::~CostmapControllerExecution ( )
virtual

Destructor.

Definition at line 61 of file costmap_controller_execution.cpp.

Member Function Documentation

◆ computeVelocityCmd()

uint32_t mbf_costmap_nav::CostmapControllerExecution::computeVelocityCmd ( const geometry_msgs::PoseStamped &  robot_pose,
const geometry_msgs::TwistStamped &  robot_velocity,
geometry_msgs::TwistStamped &  vel_cmd,
std::string &  message 
)
privatevirtual

Request plugin for a new velocity command. We override this method so we can lock the local costmap before calling the planner.

Parameters
posethe current pose of the robot.
velocitythe current velocity of the robot.
cmd_velWill be filled with the velocity command to be passed to the robot base.
messageOptional more detailed outcome as a string.
Returns
Result code as described on ExePath action result and plugin's header.

Reimplemented from mbf_abstract_nav::AbstractControllerExecution.

Definition at line 77 of file costmap_controller_execution.cpp.

◆ postRun()

void mbf_costmap_nav::CostmapControllerExecution::postRun ( )
inlineprivatevirtual

Implementation-specific cleanup function, called right after execution. This method overrides abstract execution empty implementation with underlying map-specific cleanup code.

Reimplemented from mbf_abstract_nav::AbstractExecutionBase.

Definition at line 103 of file costmap_controller_execution.h.

◆ preRun()

void mbf_costmap_nav::CostmapControllerExecution::preRun ( )
inlineprivatevirtual

Implementation-specific setup function, called right before execution. This method overrides abstract execution empty implementation with underlying map-specific setup code.

Reimplemented from mbf_abstract_nav::AbstractExecutionBase.

Definition at line 94 of file costmap_controller_execution.h.

◆ safetyCheck()

bool mbf_costmap_nav::CostmapControllerExecution::safetyCheck ( )
privatevirtual

Implementation-specific safety check, called during execution to ensure it's safe to drive. This method overrides abstract execution empty implementation with underlying map-specific checks, more precisely if controller costmap is current.

Returns
True if costmap is current, false otherwise.

Reimplemented from mbf_abstract_nav::AbstractControllerExecution.

Definition at line 92 of file costmap_controller_execution.cpp.

◆ toAbstract()

mbf_abstract_nav::MoveBaseFlexConfig mbf_costmap_nav::CostmapControllerExecution::toAbstract ( const MoveBaseFlexConfig &  config)
private

Definition at line 65 of file costmap_controller_execution.cpp.

Member Data Documentation

◆ controller_name_

std::string mbf_costmap_nav::CostmapControllerExecution::controller_name_
private

name of the controller plugin assigned by the class loader

Definition at line 140 of file costmap_controller_execution.h.

◆ costmap_ptr_

const CostmapWrapper::Ptr& mbf_costmap_nav::CostmapControllerExecution::costmap_ptr_
private

Shared pointer to thr local costmap.

Definition at line 134 of file costmap_controller_execution.h.

◆ lock_costmap_

bool mbf_costmap_nav::CostmapControllerExecution::lock_costmap_
private

Whether to lock costmap before calling the controller (see issue #4 for details)

Definition at line 137 of file costmap_controller_execution.h.


The documentation for this class was generated from the following files:


mbf_costmap_nav
Author(s): Sebastian Pütz
autogenerated on Mon Feb 28 2022 22:49:55