LocalGuidance
A library that generates velocity references to follow a path.
local_guidance_loco_ctrl_manager::LocalGuidanceLocoCtrlManager Class Reference

#include <LocalGuidanceLocoCtrlManager.hpp>

Public Member Functions

 LocalGuidanceLocoCtrlManager (ros::NodeHandle &nodeHandle, bool simulationMode, std::string twistCommandTopicName)
 
 ~LocalGuidanceLocoCtrlManager ()
 
bool startLocomotionController (const LocomotionController &requestedLocomotionController)
 
bool activateCurrentLocomotionControllerModeIfPresent ()
 
bool activateCurrentLocomotionControllerSubModeIfPresent ()
 
bool stopCurrentLocomotionController ()
 
bool getInfoCurrentLocomotionController (LocomotionController *currentLocomotionController) const
 
std::string getActiveLocomotionControllerName ()
 

Private Member Functions

LocoCtrlInfoInterfacePtr instantiateLocoCtrlInfo (const LocomotionController &locomotionController)
 
bool readPersistentParameters ()
 
void subscribeToServers ()
 
bool sendSwitchLocomotionControllerRequest (const std::string &locomotionControllerName)
 
bool switchTwistSource (bool takeOver)
 
bool takeOverTwistSource ()
 
bool returnTwistSource ()
 
bool addTwistSourceToMux ()
 
bool activateLocomotionController (const LocomotionController &locomotionController)
 
bool sendSoftEmergencyStop ()
 

Private Attributes

ros::NodeHandle & nodeHandle_
 Ros node handle. More...
 
std::unique_ptr< LowLevelControllerInterfacelowLevelControllerInterface_ {nullptr}
 Low level controller interface. More...
 
LocoCtrlInfoInterfacePtr locoCtrlInfoInterface_ {nullptr}
 Locomotion controller interface. More...
 
bool simulationMode_ {false}
 Simulation mode. More...
 
std::string twistCommandTopicName_
 Name of the input twist for the twist_mux node. More...
 
ros::ServiceClient switchLocomotionControllerClient_
 Switch locomotion controller client. More...
 
ros::ServiceClient getNameActiveLocomotionControllerClient_
 Get name active locomotion controller client. More...
 
ros::ServiceClient selectSourceInTwistMuxClient_
 Select which twist command is used in the twist mux. More...
 
ros::ServiceClient sendSoftEmergencyStopClient_
 Sends request for soft emergency stop. More...
 
ros::ServiceClient addSourceInTwistMuxClient_
 Add twist command to twist mux. More...
 
bool twistSourceHasBeenTakenOver_ {false}
 Indicates if twist source has been taken over. More...
 
std::string previouslyActiveSourceInTwistMuxInputName_
 Name of the previously active input twist of the twist_mux node. More...
 

Constructor & Destructor Documentation

local_guidance_loco_ctrl_manager::LocalGuidanceLocoCtrlManager::LocalGuidanceLocoCtrlManager ( ros::NodeHandle &  nodeHandle,
bool  simulationMode,
std::string  twistCommandTopicName 
)
explicit

Constructor.

Parameters
[in]nodeHandleROS node handle.
[in]simulationModetells if the robot is simulated in gazebo or not. If it's not simulate low level ctrl will be activated.
[in]twistCommandTopicNamename of the topic where twist commands will be published.
local_guidance_loco_ctrl_manager::LocalGuidanceLocoCtrlManager::~LocalGuidanceLocoCtrlManager ( )

Destructor.

Member Function Documentation

bool local_guidance_loco_ctrl_manager::LocalGuidanceLocoCtrlManager::activateCurrentLocomotionControllerModeIfPresent ( )

Activates the mode (if there's a mode) of the current locomotion controller.

Returns
true in case of success. false in case there is a mode but it was not possible to activate it.
bool local_guidance_loco_ctrl_manager::LocalGuidanceLocoCtrlManager::activateCurrentLocomotionControllerSubModeIfPresent ( )

Activates the sub-mode (if there's a sub-mode) of the current locomotion controller.

Returns
true in case of success. false in case there is a sub-mode but it was not possible to activate it.
bool local_guidance_loco_ctrl_manager::LocalGuidanceLocoCtrlManager::activateLocomotionController ( const LocomotionController locomotionController)
private

Activates specified locomotion controller.

Parameters
locomotionControllerlocomotion controller to activate.
Returns
true in case of success.
bool local_guidance_loco_ctrl_manager::LocalGuidanceLocoCtrlManager::addTwistSourceToMux ( )
private

Adds a twist source to the mux where commands will be sent.

Returns
true if it was possible to add the twist source.
std::string local_guidance_loco_ctrl_manager::LocalGuidanceLocoCtrlManager::getActiveLocomotionControllerName ( )

Returns the active locomotion controller name.

Returns
name of the active locomotion controller.
bool local_guidance_loco_ctrl_manager::LocalGuidanceLocoCtrlManager::getInfoCurrentLocomotionController ( LocomotionController currentLocomotionController) const

Returns the current locomotion controller

Parameters
[out]currentLocomotionControllercurrent locomotion controller.
Returns
true if there is an active (current) locomotion controller whose information were returned.
LocoCtrlInfoInterfacePtr local_guidance_loco_ctrl_manager::LocalGuidanceLocoCtrlManager::instantiateLocoCtrlInfo ( const LocomotionController locomotionController)
private

Tries to instantiate a LocoCtrlInfoInterface obj for the specified locomotion controller.

Parameters
[in]locomotionController
Returns
LocoCtrlInfoInterface obj in case of success, nullptr otherwise.
bool local_guidance_loco_ctrl_manager::LocalGuidanceLocoCtrlManager::readPersistentParameters ( )
private

Reads and verifies persistent (kept in class memory) ROS parameters.

Returns
true if successful.
bool local_guidance_loco_ctrl_manager::LocalGuidanceLocoCtrlManager::returnTwistSource ( )
private

Returns the twist source to the original source, before taking it over with takeOverTwistSource().

Returns
true if it was possible to return twist source to the value before it was taken over.
bool local_guidance_loco_ctrl_manager::LocalGuidanceLocoCtrlManager::sendSoftEmergencyStop ( )
private

Sends soft emergency stop to highlevel controller.

Returns
true in case of success.
bool local_guidance_loco_ctrl_manager::LocalGuidanceLocoCtrlManager::sendSwitchLocomotionControllerRequest ( const std::string &  locomotionControllerName)
private

Sends a request rocoma_msgs::SwitchController to switch locomotion controller.

Parameters
[in]locomotionControllerNamename of the locomotion controller to switch.
Returns
true in case of success.
bool local_guidance_loco_ctrl_manager::LocalGuidanceLocoCtrlManager::startLocomotionController ( const LocomotionController requestedLocomotionController)

Starts a locomotion controller, which in case of success becomes the current locomotion controller.

Parameters
[in]requestedLocomotionControllerlocomotion controller to start.
Returns
true if the parameters of the locomotion controller were retrieved successfully and it was started.
bool local_guidance_loco_ctrl_manager::LocalGuidanceLocoCtrlManager::stopCurrentLocomotionController ( )

Stops the current locomotion controller, is there is any active one.

Returns
true if the current locomotion controller was stopped successfully.
void local_guidance_loco_ctrl_manager::LocalGuidanceLocoCtrlManager::subscribeToServers ( )
private

Subscribes to ROS service and action servers.

bool local_guidance_loco_ctrl_manager::LocalGuidanceLocoCtrlManager::switchTwistSource ( bool  takeOver)
private

Switches twist source.

Parameters
takeOverif true, takes over the twist source, otherwise it is reset to previous one before taking over.
Returns
true if successful.
bool local_guidance_loco_ctrl_manager::LocalGuidanceLocoCtrlManager::takeOverTwistSource ( )
private

Takes over the twist source.

Returns
true if it was possible to take over the twist source.

Member Data Documentation

ros::ServiceClient local_guidance_loco_ctrl_manager::LocalGuidanceLocoCtrlManager::addSourceInTwistMuxClient_
private

Add twist command to twist mux.

ros::ServiceClient local_guidance_loco_ctrl_manager::LocalGuidanceLocoCtrlManager::getNameActiveLocomotionControllerClient_
private

Get name active locomotion controller client.

LocoCtrlInfoInterfacePtr local_guidance_loco_ctrl_manager::LocalGuidanceLocoCtrlManager::locoCtrlInfoInterface_ {nullptr}
private

Locomotion controller interface.

std::unique_ptr<LowLevelControllerInterface> local_guidance_loco_ctrl_manager::LocalGuidanceLocoCtrlManager::lowLevelControllerInterface_ {nullptr}
private

Low level controller interface.

ros::NodeHandle& local_guidance_loco_ctrl_manager::LocalGuidanceLocoCtrlManager::nodeHandle_
private

Ros node handle.

std::string local_guidance_loco_ctrl_manager::LocalGuidanceLocoCtrlManager::previouslyActiveSourceInTwistMuxInputName_
private

Name of the previously active input twist of the twist_mux node.

ros::ServiceClient local_guidance_loco_ctrl_manager::LocalGuidanceLocoCtrlManager::selectSourceInTwistMuxClient_
private

Select which twist command is used in the twist mux.

ros::ServiceClient local_guidance_loco_ctrl_manager::LocalGuidanceLocoCtrlManager::sendSoftEmergencyStopClient_
private

Sends request for soft emergency stop.

bool local_guidance_loco_ctrl_manager::LocalGuidanceLocoCtrlManager::simulationMode_ {false}
private

Simulation mode.

ros::ServiceClient local_guidance_loco_ctrl_manager::LocalGuidanceLocoCtrlManager::switchLocomotionControllerClient_
private

Switch locomotion controller client.

std::string local_guidance_loco_ctrl_manager::LocalGuidanceLocoCtrlManager::twistCommandTopicName_
private

Name of the input twist for the twist_mux node.

bool local_guidance_loco_ctrl_manager::LocalGuidanceLocoCtrlManager::twistSourceHasBeenTakenOver_ {false}
private

Indicates if twist source has been taken over.


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