The qbrobotics Control interface provides all the common structures to control both the qbhand and qbmove devices. More...

#include <qb_device_control.h>

Public Member Functions
trajectory_msgs::JointTrajectory	getCustomTrajectory (const std::vector< std::vector< trajectory_msgs::JointTrajectoryPoint >> &joint_points, const std::string &controller)
	Build a joint trajectory for the given controller using the given points, which must match the controller joints. More...

std::vector< trajectory_msgs::JointTrajectoryPoint >	getSinusoidalPoints (const double &amplitude, const double &period, const int &samples_per_period, const int &periods)
	Build a single-joint sine wave point trajectory from the given parameters. More...

std::vector< trajectory_msgs::JointTrajectoryPoint >	getTrapezoidalPoints (const double &amplitude, const double &period, const double &ramp_duration, const int &periods)
	Build a single-joint trapezoidal wave point trajectory from the given parameters. More...

trajectory_msgs::JointTrajectory	getWaypointTrajectory (ros::NodeHandle &node_handle, const std::string &controller)
	Retrieve a set of reference waypoints from the Parameter Server. More...

void	move (const trajectory_msgs::JointTrajectory &joint_trajectory, const std::string &controller)
	Build a `control_msgs::FollowJointTrajectoryAction` goal with the given joint trajectory and make a call to the Action Server relative to the provided controller (using the just created goal). More...

	qbDeviceControl ()
	Initialize the control structures needed by `ros_control:` the `combined_robot_hw::CombinedRobotHW` to combine several device hardware interfaces in a single control node, the `controller_manager::ControllerManager` to exploit the read/update/write control loop, and Action Clients to be able to send reference trajectories to their relative Action Server properly set up during the system bringup. More...

bool	waitForResult (const ros::Duration &timeout, const std::string &controller)
	Wait until the action is completed or the given timeout is reached. More...

virtual	~qbDeviceControl ()
	Stop timer and spinner structures. More...

Protected Member Functions
void	move ()
	Make all the calls to the Action Servers relative to all the waypoint trajectories previously parsed. More...

Protected Attributes
std::map< std::string, std::unique_ptr< actionlib::SimpleActionClient< control_msgs::FollowJointTrajectoryAction > > >	action_clients_

ros::CallbackQueuePtr	callback_queue_

ros::WallDuration	control_duration_

ros::WallTimer	control_setup_timer_

ros::WallTimer	control_timer_

std::map< std::string, std::string >	controller_device_name_

std::map< std::string, std::vector< std::string > >	controller_joints_

controller_manager::ControllerManager	controller_manager_

std::vector< std::string >	controllers_

std::vector< std::string >	device_names_

combined_robot_hw::CombinedRobotHW	devices_

ros::Publisher	frequency_publisher_

ros::WallTimer	frequency_timer_

ros::ServiceServer	get_async_measurements_server_

ros::ServiceClient	get_measurements_client_

bool	init_success_

std::map< std::string, trajectory_msgs::JointTrajectory >	joint_trajectories_

ros::NodeHandle	node_handle_

ros::NodeHandle	node_handle_control_

ros::ServiceServer	set_async_commands_server_

ros::ServiceServer	set_async_pid_server_

ros::ServiceClient	set_commands_client_

ros::ServiceClient	set_pid_client_

ros::AsyncSpinner	spinner_

std::mutex	sync_protector_

Private Member Functions
void	actionActiveCallback (const std::string &controller)
	Do nothing apart from debug info. More...

void	actionDoneCallback (const actionlib::SimpleClientGoalState &state, const control_msgs::FollowJointTrajectoryResultConstPtr &result, const std::string &controller)
	Restart the waypoint trajectory automatically if waypoints have been retrieved during the initialization. More...

void	actionFeedbackCallback (const control_msgs::FollowJointTrajectoryFeedbackConstPtr &feedback, const std::string &controller)
	Do nothing apart from debug info. More...

void	controlCallback (const ros::WallTimerEvent &timer_event)
	Call the `update()` each time the timer triggers. More...

void	controlSetupCallback (const ros::WallTimerEvent &timer_event)
	Initialize the control timer and automatically start the waypoint trajectory if it is not empty. More...

std::string	extractDeviceName (const std::string &controller)
	Extract the device names associated to the given controller (each controller name is assumed to start with the device name). More...

void	frequencyCallback (const ros::WallTimerEvent &timer_event)
	Publish the real control loop frequency in Hz in the relative topic. More...

bool	getAsyncMeasurementsCallback (qb_device_srvs::GetMeasurementsRequest &request, qb_device_srvs::GetMeasurementsResponse &response)
	Make a call to the same type of service provided by the Communication Handler. More...

void	initActionClients ()
	Retrieve all the controllers which have their parameters set in the Parameter Server and initialize their relative Action Clients. More...

bool	parseVector (const XmlRpc::XmlRpcValue &xml_value, const std::string &controller, std::vector< double > &vector)
	Parse the given `XmlRpcValue` as a `std::vector`, since the `XmlRpc::XmlRpcValue` class does not handle this conversion yet. More...

void	parseWaypoints ()
	Parse all the waypoints set up in the Parameter Server at <robot_namespace>/waypoints. More...

bool	setAsyncCommandsCallback (qb_device_srvs::SetCommandsRequest &request, qb_device_srvs::SetCommandsResponse &response)
	Make a call to the same type of service provided by the Communication Handler. More...

bool	setAsyncPIDCallback (qb_device_srvs::SetPIDRequest &request, qb_device_srvs::SetPIDResponse &response)
	Make a call to the same type of service provided by the Communication Handler. More...

void	update (const ros::WallTime &time, const ros::WallDuration &period)
	Read the current state from the HW, update all active controllers, and send the new references to the HW. More...

template<class T >
T	xmlCast (XmlRpc::XmlRpcValue xml_value)
	Cast an `XmlRpcValue` from `TypeDouble`, `TypeInt` or `TypeBoolean` to the specified template type. More...

Private Attributes
int	counter_

Detailed Description

The qbrobotics Control interface provides all the common structures to control both the qbhand and qbmove devices.

It exploits the qb_device_hardware_interface::qbDeviceHW for serial communication with the hardware, but it needs to be initialized with the proper derived HW interface, e.g. qbHandHW for the qbhand. The core part is given by the update() which is called each time the timer triggers (the control loop frequency is settable from the Parameter Server), and which implements the general control law: sensors reading from the device, control action computation according to the loaded controller, and actuator reference commands writing to the hardware; plus possible spare asynchronous requests at the end of each loop. This class also provides a simple waypoints trajectory control which is automatically used in case use_waypoints ROS param is set in the Parameter Server. Last but not least, this class exploits the combined_robot_hw::CombinedRobotHW to be able to control several devices in a single control node, if the Parameter Server is properly set up.

Definition at line 59 of file qb_device_control.h.

Constructor & Destructor Documentation

qbDeviceControl::qbDeviceControl ( )

Initialize the control structures needed by ros_control: the combined_robot_hw::CombinedRobotHW to combine several device hardware interfaces in a single control node, the controller_manager::ControllerManager to exploit the read/update/write control loop, and Action Clients to be able to send reference trajectories to their relative Action Server properly set up during the system bringup.

Many of the parameters are retrieved from the Parameter Server which plays a crucial role in the initialization, e.g. a set of reference waypoints can be retrieved to fill the joint trajectories for the Action Clients, so the user need to be aware of all the required parameters (cfr. the ROS wiki).

See also: initActionClients(), parseWaypoints(), controlSetupCallback(), qb_device_hardware_interface::qbDeviceHW::init()

Definition at line 32 of file qb_device_control.cpp.

qbDeviceControl::~qbDeviceControl ( )

virtual

Stop timer and spinner structures.

Definition at line 67 of file qb_device_control.cpp.

Member Function Documentation

void qbDeviceControl::actionActiveCallback ( const std::string & controller )

private

Do nothing apart from debug info.

Parameters

controller The action controller name.

Definition at line 73 of file qb_device_control.cpp.

void qbDeviceControl::actionDoneCallback	(	const actionlib::SimpleClientGoalState &	state,
		const control_msgs::FollowJointTrajectoryResultConstPtr &	result,
		const std::string &	controller
	)

private

Restart the waypoint trajectory automatically if waypoints have been retrieved during the initialization.

Parameters

state	The final state of the action.
result	The error code and error message (`0` if succeeds).
controller	The action controller name.

See also: move(const std::map<double, std::vector<double>> &)

Definition at line 77 of file qb_device_control.cpp.

void qbDeviceControl::actionFeedbackCallback	(	const control_msgs::FollowJointTrajectoryFeedbackConstPtr &	feedback,
		const std::string &	controller
	)

private

Do nothing apart from debug info.

Parameters

feedback	The action feedback state.
controller	The action controller name.

Definition at line 93 of file qb_device_control.cpp.

void qbDeviceControl::controlCallback ( const ros::WallTimerEvent & timer_event )

private

Call the update() each time the timer triggers.

Parameters

timer_event The timer event struct which stores timing info.

See also: update()

Definition at line 99 of file qb_device_control.cpp.

void qbDeviceControl::controlSetupCallback ( const ros::WallTimerEvent & timer_event )

private

Initialize the control timer and automatically start the waypoint trajectory if it is not empty.

Parameters

timer_event The timer event struct which stores timing info.

See also: controlCallback()

Definition at line 107 of file qb_device_control.cpp.

std::string qbDeviceControl::extractDeviceName ( const std::string & controller )

private

Extract the device names associated to the given controller (each controller name is assumed to start with the device name).

Parameters

controller The action controller name.

Returns: The device name associated to the controller.

Definition at line 121 of file qb_device_control.cpp.

void qbDeviceControl::frequencyCallback ( const ros::WallTimerEvent & timer_event )

private

Publish the real control loop frequency in Hz in the relative topic.

Parameters

timer_event The timer event struct which stores timing info.

See also: controlCallback()

Definition at line 132 of file qb_device_control.cpp.

bool qbDeviceControl::getAsyncMeasurementsCallback	(	qb_device_srvs::GetMeasurementsRequest &	request,
		qb_device_srvs::GetMeasurementsResponse &	response
	)

private

Make a call to the same type of service provided by the Communication Handler.

The peculiarity is that this method preserves the synchrony of the control loop, i.e. it makes the request only in the "asynchronous time slot" at the end of the control loop (after the read/update/write sequence).

Warning: The asynchronous calls must be invoked with a very low frequency w.r.t. the one of the control loop; this to prevent performance degradation. Note that there is no automatic limitation of these invocations.

Parameters

request	The request of the given service (see qb_device_srvs::GetMeasurements for details).
response	The response of the given service (see qb_device_srvs::GetMeasurements for details).

Returns: true if the call succeed (actually response.success may be false).

See also: qb_device_communication_handler::getMeasurementsCallback()

Definition at line 136 of file qb_device_control.cpp.

trajectory_msgs::JointTrajectory qbDeviceControl::getCustomTrajectory	(	const std::vector< std::vector< trajectory_msgs::JointTrajectoryPoint >> &	joint_points,
		const std::string &	controller
	)

Build a joint trajectory for the given controller using the given points, which must match the controller joints.

Warning: All the single-joint trajectories must be of the same size and it is assumed that the timing is the same.

Parameters

joint_points	The vector of single-joint point trajectories.
controller	The action controller name (used for joint names).

Returns: The filled trajectory_msgs::JointTrajectory ROS message.

See also: move(const trajectory_msgs::JointTrajectory &, const std::string &), getSinusoidalPoints(), getStepPoints()

Definition at line 186 of file qb_device_control.cpp.

std::vector< trajectory_msgs::JointTrajectoryPoint > qbDeviceControl::getSinusoidalPoints	(	const double &	amplitude,
		const double &	period,
		const int &	samples_per_period,
		const int &	periods
	)

Build a single-joint sine wave point trajectory from the given parameters.

Parameters

amplitude	The amplitude of the sine wave position trajectory for the motor [rad].
period	The period of the sine wave position trajectory for the motor [s].
samples_per_period	The number of samples in a whole period.
periods	The number of periods concatenated in the trajectory.

Returns: The vector of trajectory_msgs::JointTrajectoryPoint properly filled.

See also: getCustomTrajectory(), getTrapezoidalPoints()

Definition at line 146 of file qb_device_control.cpp.

std::vector< trajectory_msgs::JointTrajectoryPoint > qbDeviceControl::getTrapezoidalPoints	(	const double &	amplitude,
		const double &	period,
		const double &	ramp_duration,
		const int &	periods
	)

Build a single-joint trapezoidal wave point trajectory from the given parameters.

Parameters

amplitude	The amplitude of the step wave position trajectory for the motor [rad]. Note that the wave is symmetrical [-A, A].
period	The period of the step wave position trajectory for the motor [s].
ramp_duration	The duration of the ramp from `-amplitude` to `amplitude` [s].
periods	The number of periods concatenated in the trajectory.

Returns: The vector of trajectory_msgs::JointTrajectoryPoint properly filled.

See also: getCustomTrajectory(), getSinusoidalPoints()

Definition at line 166 of file qb_device_control.cpp.

trajectory_msgs::JointTrajectory qbDeviceControl::getWaypointTrajectory	(	ros::NodeHandle &	node_handle,
		const std::string &	controller
	)

Retrieve a set of reference waypoints from the Parameter Server.

The waypoints are composed by a vector where each element must contains:

a field named time which can be either a single value or an interval for which joint_positions hold;
a list named joint_positions of names which contain the vector of joint position references of the relative device, valid for the above time interval. Additionally (but it is not required), there can also be specified:
a list named joint_velocities;
a list named joint_accelerations; which have an obvious interpretation. When these are not present in the Parameter Server, it is assigned the value 0.0 to each joint.
Warning
Please note that each of the above list must contain the exact number and order of joints of the relative controller to which is paired.

Parameters

node_handle The Node Handle in which the waypoints are parsed, under .../waypoints (if present).

controller The action controller name (used for joint names).

Returns
The filled trajectory_msgs::JointTrajectory ROS message.

See also
move(const trajectory_msgs::JointTrajectory &, const std::string &), parseVector(), xmlCast()

Definition at line 214 of file qb_device_control.cpp.

void qbDeviceControl::initActionClients ( )

private

Retrieve all the controllers which have their parameters set in the Parameter Server and initialize their relative Action Clients.

Also set few additional member variables.

Definition at line 257 of file qb_device_control.cpp.

void qbDeviceControl::move	(	const trajectory_msgs::JointTrajectory &	joint_trajectory,
		const std::string &	controller
	)

Build a control_msgs::FollowJointTrajectoryAction goal with the given joint trajectory and make a call to the Action Server relative to the provided controller (using the just created goal).

Parameters

joint_trajectory	The waypoint trajectory properly filled for all the joints of the controller.
controller	The action controller name.

See also: move(), waitForResult()

Definition at line 282 of file qb_device_control.cpp.

void qbDeviceControl::move ( )

protected

Make all the calls to the Action Servers relative to all the waypoint trajectories previously parsed.

See also: move(const trajectory_msgs::JointTrajectory &, const std::string &), waitForResult()

Definition at line 276 of file qb_device_control.cpp.

bool qbDeviceControl::parseVector	(	const XmlRpc::XmlRpcValue &	xml_value,
		const std::string &	controller,
		std::vector< double > &	vector
	)

private

Parse the given XmlRpcValue as a std::vector, since the XmlRpc::XmlRpcValue class does not handle this conversion yet.

Parameters

	xml_value	The value retrieved from the Parameter Server to be parsed as a `std::vector`.
	controller	The action controller name.
[out]	vector	The `std::vector` parsed from the `XmlRpcValue`.

Returns: true on success.

See also: parseWaypoints(), xmlCast()

Definition at line 292 of file qb_device_control.cpp.

void qbDeviceControl::parseWaypoints ( )

private

Parse all the waypoints set up in the Parameter Server at <robot_namespace>/waypoints.

See also: getWaypointTrajectory()

Definition at line 303 of file qb_device_control.cpp.

bool qbDeviceControl::setAsyncCommandsCallback	(	qb_device_srvs::SetCommandsRequest &	request,
		qb_device_srvs::SetCommandsResponse &	response
	)

private

Make a call to the same type of service provided by the Communication Handler.

The peculiarity is that this method preserves the synchrony of the control loop, i.e. it makes the request only in the "asynchronous time slot" at the end of the control loop (after the read/update/write sequence).

Warning: The asynchronous calls must be invoked with a very low frequency w.r.t. the one of the control loop; this to prevent performance degradation. Note that there is no automatic limitation of these invocations.

Parameters

request	The request of the given service (see qb_device_srvs::SetCommands for details).
response	The response of the given service (see qb_device_srvs::SetCommands for details).

Returns: true if the call succeed (actually response.success may be false).

See also: qb_device_communication_handler::setCommandsCallback()

Definition at line 313 of file qb_device_control.cpp.

bool qbDeviceControl::setAsyncPIDCallback	(	qb_device_srvs::SetPIDRequest &	request,
		qb_device_srvs::SetPIDResponse &	response
	)

private

Make a call to the same type of service provided by the Communication Handler.

The peculiarity is that this method preserves the synchrony of the control loop, i.e. it makes the request only in the "asynchronous time slot" at the end of the control loop (after the read/update/write sequence).

Warning: The asynchronous calls must be invoked with a very low frequency w.r.t. the one of the control loop; this to prevent performance degradation. Note that there is no automatic limitation of these invocations.

Parameters

request	The request of the given service (see qb_device_srvs::SetPID for details).
response	The response of the given service (see qb_device_srvs::SetPID for details).

Returns: true if the call succeed (actually response.success may be false).

See also: qb_device_communication_handler::setPIDCallback()

Definition at line 323 of file qb_device_control.cpp.

void qbDeviceControl::update	(	const ros::WallTime &	time,
		const ros::WallDuration &	period
	)

private

Read the current state from the HW, update all active controllers, and send the new references to the HW.

The control references follow the current implementation of the controllers in use.

Note: During this method, no asynchronous call is executed to preserve the order of reads and writes to the all the devices. When this method ends, the pending asynchronous request are served.

Parameters

time	The current time.
period	The time passed since the last call to this method, i.e. the control period.

Definition at line 333 of file qb_device_control.cpp.

bool qbDeviceControl::waitForResult	(	const ros::Duration &	timeout,
		const std::string &	controller
	)

Wait until the action is completed or the given timeout is reached.

Parameters

timeout	The maximum amount of time to wait.
controller	The action controller name.

Returns: true if action has ended.

Definition at line 344 of file qb_device_control.cpp.

template<class T >

T qbDeviceControl::xmlCast ( XmlRpc::XmlRpcValue xml_value )

private

Cast an XmlRpcValue from TypeDouble, TypeInt or TypeBoolean to the specified template type.

This is necessary since XmlRpc does not handle conversion among basic types: it throws an exception if an improper cast is invoked though (e.g. from int to double).

Template Parameters

T	The type to cast to.

Parameters

xml_value The wrong-casted value.

Returns: The casted value.

See also: parseWaypoints(), parseVector()

Definition at line 349 of file qb_device_control.cpp.

Member Data Documentation

std::map<std::string, std::unique_ptr<actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> > > qb_device_control::qbDeviceControl::action_clients_

protected

Definition at line 170 of file qb_device_control.h.

ros::CallbackQueuePtr qb_device_control::qbDeviceControl::callback_queue_

protected

Definition at line 149 of file qb_device_control.h.

ros::WallDuration qb_device_control::qbDeviceControl::control_duration_

protected

Definition at line 163 of file qb_device_control.h.

ros::WallTimer qb_device_control::qbDeviceControl::control_setup_timer_

protected

Definition at line 160 of file qb_device_control.h.

ros::WallTimer qb_device_control::qbDeviceControl::control_timer_

protected

Definition at line 161 of file qb_device_control.h.

std::map<std::string, std::string> qb_device_control::qbDeviceControl::controller_device_name_

protected

Definition at line 168 of file qb_device_control.h.

std::map<std::string, std::vector<std::string> > qb_device_control::qbDeviceControl::controller_joints_

protected

Definition at line 169 of file qb_device_control.h.

controller_manager::ControllerManager qb_device_control::qbDeviceControl::controller_manager_

protected

Definition at line 176 of file qb_device_control.h.

std::vector<std::string> qb_device_control::qbDeviceControl::controllers_

protected

Definition at line 167 of file qb_device_control.h.

int qb_device_control::qbDeviceControl::counter_

private

Definition at line 185 of file qb_device_control.h.

std::vector<std::string> qb_device_control::qbDeviceControl::device_names_

protected

Definition at line 166 of file qb_device_control.h.

combined_robot_hw::CombinedRobotHW qb_device_control::qbDeviceControl::devices_

protected

Definition at line 174 of file qb_device_control.h.

ros::Publisher qb_device_control::qbDeviceControl::frequency_publisher_

protected

Definition at line 153 of file qb_device_control.h.

ros::WallTimer qb_device_control::qbDeviceControl::frequency_timer_

protected

Definition at line 162 of file qb_device_control.h.

ros::ServiceServer qb_device_control::qbDeviceControl::get_async_measurements_server_

protected

Definition at line 157 of file qb_device_control.h.

ros::ServiceClient qb_device_control::qbDeviceControl::get_measurements_client_

protected

Definition at line 154 of file qb_device_control.h.

bool qb_device_control::qbDeviceControl::init_success_

protected

Definition at line 175 of file qb_device_control.h.

std::map<std::string, trajectory_msgs::JointTrajectory> qb_device_control::qbDeviceControl::joint_trajectories_

protected

Definition at line 171 of file qb_device_control.h.

ros::NodeHandle qb_device_control::qbDeviceControl::node_handle_

protected

Definition at line 151 of file qb_device_control.h.

ros::NodeHandle qb_device_control::qbDeviceControl::node_handle_control_

protected

Definition at line 152 of file qb_device_control.h.

ros::ServiceServer qb_device_control::qbDeviceControl::set_async_commands_server_

protected

Definition at line 158 of file qb_device_control.h.

ros::ServiceServer qb_device_control::qbDeviceControl::set_async_pid_server_

protected

Definition at line 159 of file qb_device_control.h.

ros::ServiceClient qb_device_control::qbDeviceControl::set_commands_client_

protected

Definition at line 155 of file qb_device_control.h.

ros::ServiceClient qb_device_control::qbDeviceControl::set_pid_client_

protected

Definition at line 156 of file qb_device_control.h.

ros::AsyncSpinner qb_device_control::qbDeviceControl::spinner_

protected

Definition at line 150 of file qb_device_control.h.

std::mutex qb_device_control::qbDeviceControl::sync_protector_

protected

Definition at line 165 of file qb_device_control.h.

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

node_handle	The Node Handle in which the waypoints are parsed, under .../waypoints (if present).
controller	The action controller name (used for joint names).

Public Member Functions

Protected Member Functions

Protected Attributes

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation