Aikido
aikido::control::ros::RosJointGroupCommandClient Class Reference

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

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

Public Member Functions

 RosJointGroupCommandClient (const ::ros::NodeHandle &node, const std::string &serverName, const std::vector< std::string > jointNames, const std::chrono::milliseconds connectionTimeout=std::chrono::milliseconds{1000}, const std::chrono::milliseconds connectionPollingPeriod=std::chrono::milliseconds{20})
 Constructor. More...
 
virtual ~RosJointGroupCommandClient ()
 
std::future< int > execute (ExecutorType type, const std::vector< double > &goal, ::ros::Duration timeout)
 Send command to ROS server for execution. More...
 
void step ()
 Step to a point in time. More...
 
void cancel ()
 Cancel current command. More...
 

Private Types

using JointGroupCommandActionClient = actionlib::ActionClient< pr_control_msgs::JointGroupCommandAction >
 
using GoalHandle = JointGroupCommandActionClient::GoalHandle
 

Private Member Functions

void transitionCallback (GoalHandle handle)
 

Private Attributes

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

Detailed Description

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

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

Member Typedef Documentation

◆ GoalHandle

using aikido::control::ros::RosJointGroupCommandClient::GoalHandle = JointGroupCommandActionClient::GoalHandle
private

◆ JointGroupCommandActionClient

using aikido::control::ros::RosJointGroupCommandClient::JointGroupCommandActionClient = actionlib::ActionClient<pr_control_msgs::JointGroupCommandAction>
private

Constructor & Destructor Documentation

◆ RosJointGroupCommandClient()

aikido::control::ros::RosJointGroupCommandClient::RosJointGroupCommandClient ( const ::ros::NodeHandle &  node,
const std::string &  serverName,
const std::vector< std::string >  jointNames,
const std::chrono::milliseconds  connectionTimeout = std::chrono::milliseconds{1000},
const 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.

◆ ~RosJointGroupCommandClient()

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

Member Function Documentation

◆ cancel()

void aikido::control::ros::RosJointGroupCommandClient::cancel ( )

Cancel current command.

Should trigger exception for active std::promise

◆ execute()

std::future<int> aikido::control::ros::RosJointGroupCommandClient::execute ( ExecutorType  type,
const std::vector< double > &  goal,
::ros::Duration  timeout 
)

Send command to ROS server for execution.

Parameters
[in]typeSelecting between Position, Velocity, or Effort
[in]goalVector of target positions for each joint
[in]timeoutHow long until command should expire

◆ step()

void aikido::control::ros::RosJointGroupCommandClient::step ( )

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 command.

◆ transitionCallback()

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

Member Data Documentation

◆ mCallbackQueue

::ros::CallbackQueue aikido::control::ros::RosJointGroupCommandClient::mCallbackQueue
private

◆ mClient

JointGroupCommandActionClient aikido::control::ros::RosJointGroupCommandClient::mClient
private

◆ mConnectionPollingPeriod

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

◆ mConnectionTimeout

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

◆ mGoalHandle

JointGroupCommandActionClient::GoalHandle aikido::control::ros::RosJointGroupCommandClient::mGoalHandle
private

◆ mInProgress

bool aikido::control::ros::RosJointGroupCommandClient::mInProgress
private

◆ mJointNames

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

◆ mMutex

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

Manages access to mInProgress, mPromise.

◆ mNode

const ::ros::NodeHandle aikido::control::ros::RosJointGroupCommandClient::mNode
private

◆ mPromise

std::unique_ptr<std::promise<int> > aikido::control::ros::RosJointGroupCommandClient::mPromise
private