Class AbstractControllerExecution

Inheritance Relationships

Base Type

Class Documentation

class AbstractControllerExecution : public mbf_abstract_nav::AbstractExecutionBase

The AbstractControllerExecution class loads and binds the controller plugin. It contains a thread running the plugin in a cycle to move the robot. An internal state is saved and will be pulled by server, to monitor controller execution. Due to a state change it wakes up all threads connected to the condition variable.

Public Types

enum ControllerState

Internal states.

Values:

enumerator INITIALIZED

Controller has been initialized successfully.

enumerator STARTED

Controller has been started.

enumerator PLANNING

Executing the plugin.

enumerator NO_PLAN

The controller has been started without a plan.

enumerator MAX_RETRIES

Exceeded the maximum number of retries without a valid command.

enumerator PAT_EXCEEDED

Exceeded the patience time without a valid command.

enumerator EMPTY_PLAN

Received an empty plan.

enumerator INVALID_PLAN

Received an invalid plan that the controller plugin rejected.

enumerator NO_LOCAL_CMD

Received no velocity command by the plugin, in the current cycle.

enumerator GOT_LOCAL_CMD

Got a valid velocity command from the plugin.

enumerator ARRIVED_GOAL

The robot arrived the goal.

enumerator CANCELED

The controller has been canceled.

enumerator STOPPED

The controller has been stopped!

enumerator INTERNAL_ERROR

An internal error occurred.

enumerator ROBOT_DISABLED

The robot is stuck and ignored velocity command.

typedef std::shared_ptr<AbstractControllerExecution> Ptr

Public Functions

AbstractControllerExecution(const std::string &name, const mbf_abstract_core::AbstractController::Ptr &controller_ptr, const mbf_utility::RobotInformation::ConstPtr &robot_info, const rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr &vel_pub, const rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr &goal_pub, const rclcpp::Node::SharedPtr &node_handle)

Constructor.

Parameters:

  • name – Name of this execution

  • controller_ptr – Pointer to the controller plugin

  • robot_info – Current robot state

  • vel_pub – Velocity publisher

  • goal_pub – Current goal publisher

  • config – Initial configuration for this execution

virtual ~AbstractControllerExecution()

Destructor.

virtual bool start()

Starts the controller, a valid plan should be given in advance.

Returns:

false if the thread is already running, true if starting the controller succeeded!

void setNewPlan(const std::vector<geometry_msgs::msg::PoseStamped> &plan, bool tolerance_from_action = false, double action_dist_tolerance = 1.0, double action_angle_tolerance = 3.1415)

Sets a new plan to the controller execution.

Parameters:

  • plan – A vector of stamped poses.

  • tolerance_from_action – flag that will be set to true when the new plan (action) has tolerance

  • action_dist_tolerance – distance to goal tolerance specific for this new plan (action)

  • action_angle_tolerance – angle to goal tolerance specific for this new plan (action)

virtual bool cancel()

Cancel the controller execution. Normally called upon aborting the navigation. This calls the cancel method of the controller plugin. If the plugins returns true, it becomes responsible of stopping, and we will keep requesting velocity commands until it returns CANCELED. If it returns false (meaning cancel is not implemented, or that the controller defers handling it), MBF will set the cancel_ flag to true, and wait for the control loop to stop.

Returns:

true, if the controller handles the stooping, or the control loop stops within a cycle time.

ControllerState getState() const

Return the current state of the controller execution. Thread communication safe.

Returns:

current state, enum value of ControllerState

rclcpp::Time getLastPluginCallTime() const

Returns the time of the last plugin call.

Returns:

Time of the last plugin call

geometry_msgs::msg::TwistStamped getVelocityCmd() const

Returns the last velocity command calculated by the plugin. Set by setVelocityCmd method. Note that it doesn’t need to be a valid command sent to the robot, as we report also failed calls to the plugin on controller action feedback.

Returns:

The last valid velocity command.

bool isPatienceExceeded() const

Checks whether the patience duration time has been exceeded, ot not.

Returns:

true, if the patience has been exceeded.

bool setControllerFrequency(double frequency)

Sets the controller frequency.

Parameters:

frequency – The controller frequency

Returns:

true, if the controller frequency has been changed / set succesfully, false otherwise

rcl_interfaces::msg::SetParametersResult reconfigure(std::vector<rclcpp::Parameter> parameters)

Is called by the server thread to reconfigure the controller execution, if a user uses dynamic reconfigure to reconfigure the current state.

Parameters:

config – The dynamic reconfigure config.

bool isMoving() const

Returns whether the robot should normally move or not. True if the controller seems to work properly.

Returns:

true, if the robot should normally move, false otherwise

Public Static Attributes

static const double DEFAULT_CONTROLLER_FREQUENCY

Protected Functions

virtual uint32_t computeVelocityCmd(const geometry_msgs::msg::PoseStamped &pose, const geometry_msgs::msg::TwistStamped &velocity, geometry_msgs::msg::TwistStamped &vel_cmd, std::string &message)

Request plugin for a new velocity command, given the current position, orientation, and velocity of the robot. We use this virtual method to give concrete implementations as move_base the chance to override it and do additional stuff, for example locking the costmap.

Parameters:

  • pose – the current pose of the robot.

  • velocity – the current velocity of the robot.

  • cmd_vel – Will be filled with the velocity command to be passed to the robot base.

  • message – Optional more detailed outcome as a string.

Returns:

Result code as described on ExePath action result and plugin’s header.

void setVelocityCmd(const geometry_msgs::msg::TwistStamped &vel_cmd_stamped)

Sets the velocity command, to make it available for another thread.

Parameters:

vel_cmd_stamped – current velocity command

bool checkCmdVelIgnored(const geometry_msgs::msg::Twist &cmd_vel)

Check if the robot is ignoring the cmd_vel longer than threshold time.

Parameters:

cmd_vel – the latest cmd_vel being published by the controller

Returns:

true if cmd_vel is being ignored by the robot longer than tolerance time, false otherwise

virtual void run()

The main run method, a thread will execute this method. It contains the main controller execution loop.

inline virtual bool safetyCheck()

Check if its safe to drive. This method gets called at every controller cycle, stopping the robot if its not. When overridden by child class, gives a chance to the specific execution implementation to stop the robot if it detects something wrong on the underlying map.

Returns:

Always true, unless overridden by child class.

Protected Attributes

std::string plugin_name_

the name of the loaded plugin

mbf_abstract_core::AbstractController::Ptr controller_

The local planer to calculate the velocity command.

rclcpp::Time last_call_time_

The current cycle start time of the last cycle run. Will by updated each cycle.

rclcpp::Time start_time_

The time the controller has been started.

rclcpp::Time last_valid_cmd_time_

The time the controller responded with a success output (output < 10).

rclcpp::Time first_ignored_time_

The time when the robot started ignoring velocity commands.

int max_retries_

The maximum number of retries.

rclcpp::Duration patience_

The time / duration of patience, before changing the state.

std::string robot_frame_

the frame of the robot, which will be used to determine its position.

std::string global_frame_

the global frame the robot is controlling in.

rclcpp::Node::SharedPtr node_handle_