mh5_controllers reference

class ActiveJointController

class mh5_controllers::ActiveJointController : public controller_interface::Controller<mh5_hardware::ActiveJointInterface>

Controller that can swithc on or off the torque on a group of Dynamixel servos.

Requires mh5_harware::ActiveJointInterfaces to be registered with the hardware interface. Reads “groups” parameter from the param server, which should contain a list of groups that can be toggled in the same time. It is possible to nest groups in each other as long as they build on each other.

Advertises a service /torque_control/switch_torque of type mh5_controllers/ActivateJoint. The name passed in calls to this service can be individual joints or groups of joints.

 rosservice call /torque_control/swtich_torque "{name: "head_p", state: true}"

of for a group:

 rosservice call /torque_control/swtich_torque "{name: "head", state: true}"

Will simply turn on or off the torque on all the servos associated with the group.

Public Functions

inline ActiveJointController()

Construct a new Active Joint Controller object using a mh5_hardware::ActiveJointInterface interface.

inline ~ActiveJointController()

Destroy the Active Joint Controller object. Shuts also down the ROS service.

bool init(mh5_hardware::ActiveJointInterface *hw, ros::NodeHandle &n)

Initializes the controller by reading the joint list from the parameter server under “groups”. If no parameter is provided it will create a group “all” and assign all avaialable resources to this group. If groups are defined then they should be first listed in the “groups” parameter, then each one of them should be listed separately with the joints, or subgroups that are included. If subgroups are used they have to be fully defined first, befire they are used in a superior group.

This function also advertises the ROS service: /[controller name]/switch_torque

Parameters

• hw – the hardware interface that will provide the access to the repoces

• n – the nodehandle of the initiator controller

Returns

true if there is at least one joint that has been successfully identified and registered with this controller

Returns

false if either no “joints” parameter was available in the param server or no joints has been successfully retrieved from the hardware interface.

inline void starting(const ros::Time &time)

Does nothing in this case. Used for completing the controller interface.

Parameters

time –

void update(const ros::Time&, const ros::Duration&)

Does the actual update of the joints’ torque activation member. Please note that this controller only sets the field as provided by the mh5_hardware::ActiveJointInterface and it is not actually triggering any communication with the actual servos. It is the hardware interface respoonsibility to replicate this requests to the device.

Private Functions

bool torqueCB(mh5_controllers::ActivateJoint::Request &req, mh5_controllers::ActivateJoint::Response &res)

Callback for processing “switch_torque” calls. Checks if the requested group exists or if there is a joint by that name.

Parameters

• req – the service request; group/joint name + desired state

• res – the service response; if things are successful + detailed message

Returns

true always

bool rebootCB(mh5_controllers::ActivateJoint::Request &req, mh5_controllers::ActivateJoint::Response &res)

Callback for processing “reboot” calls. Checks if the requested group exists or if there is a joint by that name.

Parameters

• req – the service request; group/joint name + desired state

• res – the service response; if things are successful + detailed message

Returns

true always

Private Members

std::map<std::string, std::vector<mh5_hardware::JointTorqueAndReboot>> joints_

Map group->list of joint handles.

realtime_tools::RealtimeBuffer<ActivateJoint::Request> torque_commands_buffer_

Holds torque activation commands to be processed during the update() processings. The service callbacks only store “true” or “false” in this buffer depending on the command processed.

realtime_tools::RealtimeBuffer<ActivateJoint::Request> reboot_commands_buffer_

Holds reboot commands to be processed during the update() processings. The service callbacks only store “true” or “false” in this buffer depending on the command processed.

ros::ServiceServer torque_srv_

ROS Service that responds to the “switch_torque” calls.

ros::ServiceServer reboot_srv_

ROS Service that responds to the “reboot” calls.

class ExtendedJointTrajectoryController

class mh5_controllers::ExtendedJointTrajectoryController : public controller_interface::MultiInterfaceController<hardware_interface::PosVelJointInterface, mh5_hardware::ActiveJointInterface>

Public Functions

inline ExtendedJointTrajectoryController()

bool init(hardware_interface::RobotHW *robot_hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)

void starting(const ros::Time &time)

void stopping(const ros::Time &time)

void update(const ros::Time &time, const ros::Duration &period)

Private Members

mh5_controllers::BaseJointTrajectoryController *pos_controller_

mh5_controllers::ActiveJointController *act_controller_

class CommunicationStatsController

class mh5_controllers::CommunicationStatsController : public controller_interface::Controller<mh5_hardware::CommunicationStatsInterface>

Publishes communication ststistics for all the Dynamixel loops registered in the hardware interface. Requires mh5_hardware::CommunicationStatsInterface to access the statistics for all loops. If combined HW interface is used please note that this will get all the loops, across all the physical HW interfaces that the combined HW interface will start.

The messages are publish as diagnostic_msgs::DiagnosticArray under topic “diagnostics”. Aggregators can be used to process thsese raw diagnostic messages and publish them to a RobotMonitor.

Public Functions

inline CommunicationStatsController()

Construct a new Communication Stats Controller object; defaults the publish period to 0.0.

bool init(mh5_hardware::CommunicationStatsInterface *hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)

Initializes the controller. Reads the parameter server “publish_period” [expressed in seconds] and uses it for sheduling the publishing of the communication information. It defaults to 30s if no value is avaialable. Please note that the publishing period is also used to reset the short time communication statistics that are provided by the mh5_hardware::CommunicationStatsInterface.

It will setup the realtime publisher and allocate the message structure to accomodate the data from the CommunicationStatsInterface.

Parameters

• hw – the hardware providing the loops; could be a Combined HW Interface

• root_nh – the top Node Handler

• controller_nh – the node handler of the controller; used to access the parameter server

Returns

true if controller was initialized sucessfully

void starting(const ros::Time &time)

Resets the last_publish_time_ to the provided time.

Parameters

time – when the controller was started

void update(const ros::Time&, const ros::Duration&)

Performs the actual publishing of statistics by accesing the inteface data. It will check the last time the message was published and does not do any publish if it is less than publish_period_ desired for these message publishing.

Please note that after the massage is published it invokes the setReset(true) for the CommunicationStatsInterface to reset to 0 the short-term statistics.

virtual void stopping(const ros::Time&)

Provided for completion of the controller interface.

Private Members

std::vector<mh5_hardware::CommunicationStatsHandle> communication_states_

Holds the list of handles to all the loops across all the HW interfaces.

std::shared_ptr<realtime_tools::RealtimePublisher<diagnostic_msgs::DiagnosticArray>> realtime_pub_

Publisher object.

ros::Time last_publish_time_

Keeps the last publish time. Updated every time we publish a new message.

double publish_period_

The desired publishing period in seconds for the diagnostoc messages.