Aikido
aikido::control::ros::RosPositionCommandExecutor Class Reference

This class uses actionlib to command an action of the type pr_control_msgs/SetPosition. More...

#include <aikido/control/ros/RosPositionCommandExecutor.hpp>

Inheritance diagram for aikido::control::ros::RosPositionCommandExecutor:
aikido::control::PositionCommandExecutor

Public Member Functions

 RosPositionCommandExecutor (::ros::NodeHandle node, const std::string &serverName, std::vector< std::string > jointNames, std::chrono::milliseconds connectionTimeout=std::chrono::milliseconds{1000}, std::chrono::milliseconds connectionPollingPeriod=std::chrono::milliseconds{20})
 Constructor. More...
 
virtual ~RosPositionCommandExecutor ()
 
std::future< void > execute (const Eigen::VectorXd &goalPositions) override
 Sends positions to ROS server for execution. More...
 
void step (const std::chrono::system_clock::time_point &timepoint) override
 Step to a point in time. More...
 
- Public Member Functions inherited from aikido::control::PositionCommandExecutor
virtual ~PositionCommandExecutor ()=default
 

Private Types

using RosPositionActionClient = actionlib::ActionClient< pr_control_msgs::SetPositionAction >
 
using GoalHandle = RosPositionActionClient::GoalHandle
 

Private Member Functions

void transitionCallback (GoalHandle handle)
 

Private Attributes

::ros::NodeHandle mNode
 
::ros::CallbackQueue mCallbackQueue
 
RosPositionActionClient mClient
 
RosPositionActionClient::GoalHandle mGoalHandle
 
std::vector< std::string > mJointNames
 
std::chrono::milliseconds mConnectionTimeout
 
std::chrono::milliseconds mConnectionPollingPeriod
 
bool mInProgress
 
std::unique_ptr< std::promise< void > > mPromise
 
std::mutex mMutex
 Manages access to mInProgress, mPromise. More...
 

Additional Inherited Members

- Protected Attributes inherited from aikido::control::PositionCommandExecutor
std::chrono::system_clock::time_point mTimeOfPreviousCall
 Time of previous call to step. More...
 

Detailed Description

This class uses actionlib to command an action of the type pr_control_msgs/SetPosition.

It specifies a set of target positions and sends it to the ROS server for execution

Member Typedef Documentation

◆ GoalHandle

using aikido::control::ros::RosPositionCommandExecutor::GoalHandle = RosPositionActionClient::GoalHandle
private

◆ RosPositionActionClient

using aikido::control::ros::RosPositionCommandExecutor::RosPositionActionClient = actionlib::ActionClient<pr_control_msgs::SetPositionAction>
private

Constructor & Destructor Documentation

◆ RosPositionCommandExecutor()

aikido::control::ros::RosPositionCommandExecutor::RosPositionCommandExecutor ( ::ros::NodeHandle  node,
const std::string &  serverName,
std::vector< std::string >  jointNames,
std::chrono::milliseconds  connectionTimeout = std::chrono::milliseconds{1000},
std::chrono::milliseconds  connectionPollingPeriod = std::chrono::milliseconds{20} 
)

Constructor.

Parameters
[in]nodeROS node handle for action client.
[in]serverNameName of the server to send trajectory to.
[in]jointNamesThe names of the joints to set position targets for
[in]connectionTimeoutTimeout for server connection.
[in]connectionPollingPeriodPolling period for server connection.

◆ ~RosPositionCommandExecutor()

virtual aikido::control::ros::RosPositionCommandExecutor::~RosPositionCommandExecutor ( )
virtual

Member Function Documentation

◆ execute()

std::future<void> aikido::control::ros::RosPositionCommandExecutor::execute ( const Eigen::VectorXd &  goalPositions)
overridevirtual

Sends positions to ROS server for execution.

Parameters
[in]goalPositionsVector of target positions for each joint

Implements aikido::control::PositionCommandExecutor.

◆ step()

void aikido::control::ros::RosPositionCommandExecutor::step ( const std::chrono::system_clock::time_point &  timepoint)
overridevirtual

Step to a point in time.

Note
timepoint can be a time in the future to enable faster than real-time execution.
Parameters
timepointTime to simulate to

To be executed on a separate thread. Regularly checks for the completion of a sent trajectory.

Implements aikido::control::PositionCommandExecutor.

◆ transitionCallback()

void aikido::control::ros::RosPositionCommandExecutor::transitionCallback ( GoalHandle  handle)
private

Member Data Documentation

◆ mCallbackQueue

::ros::CallbackQueue aikido::control::ros::RosPositionCommandExecutor::mCallbackQueue
private

◆ mClient

RosPositionActionClient aikido::control::ros::RosPositionCommandExecutor::mClient
private

◆ mConnectionPollingPeriod

std::chrono::milliseconds aikido::control::ros::RosPositionCommandExecutor::mConnectionPollingPeriod
private

◆ mConnectionTimeout

std::chrono::milliseconds aikido::control::ros::RosPositionCommandExecutor::mConnectionTimeout
private

◆ mGoalHandle

RosPositionActionClient::GoalHandle aikido::control::ros::RosPositionCommandExecutor::mGoalHandle
private

◆ mInProgress

bool aikido::control::ros::RosPositionCommandExecutor::mInProgress
private

◆ mJointNames

std::vector<std::string> aikido::control::ros::RosPositionCommandExecutor::mJointNames
private

◆ mMutex

std::mutex aikido::control::ros::RosPositionCommandExecutor::mMutex
mutableprivate

Manages access to mInProgress, mPromise.

◆ mNode

::ros::NodeHandle aikido::control::ros::RosPositionCommandExecutor::mNode
private

◆ mPromise

std::unique_ptr<std::promise<void> > aikido::control::ros::RosPositionCommandExecutor::mPromise
private