#include <dynamixel_interface_controller.h>

Public Member Functions
	DynamixelInterfaceController ()

double	getLoopRate (void)

bool	initialise ()

void	loop (void)

bool	parseParameters (void)

	~DynamixelInterfaceController ()
	Destructor, deletes the objects holding the serial ports and disables the motors if required. More...

Private Member Functions
bool	initialiseDynamixel (PortInfo &port, DynamixelInfo &dynamixel)

bool	initialisePort (PortInfo &port)

void	jointStateCallback (const sensor_msgs::JointState::ConstPtr &joint_commands)

void	multiThreadedIO (PortInfo &port, sensor_msgs::JointState &read_msg, dynamixel_interface::DataPorts &dataport_msg, dynamixel_interface::ServoDiags &status_msg, bool perform_write) const

void	multiThreadedRead (PortInfo &port, sensor_msgs::JointState &read_msg, dynamixel_interface::DataPorts &dataports_msg, dynamixel_interface::ServoDiags &diags_msg) const

void	multiThreadedWrite (PortInfo &port, sensor_msgs::JointState joint_commands) const

bool	parsePortInformation (XmlRpc::XmlRpcValue ports)

bool	parseServoInformation (PortInfo &port, XmlRpc::XmlRpcValue servos)

Private Attributes
ros::Timer	broadcast_timer_
	Timer that controls the rate of the IO callback. More...

DynamixelControlMode	control_type_

uint	dataport_counter_ = 0
	counter for tracking dataport rate More...

uint	dataport_iters_ = 0
	publish when dataport_counter_ == this More...

ros::Publisher	dataport_publisher_
	Publishes the data from the external ports on dynamixel_pros. More...

double	dataport_rate_
	Rate at which the pro external dataport is read. More...

ros::Publisher	debug_publisher_
	Debug message publisher. More...

uint	diagnostics_counter_ = 0
	Counter for tracking diagnostics rate. More...

uint	diagnostics_iters_ = 0
	publish when diagnostic_counter_ == this More...

ros::Publisher	diagnostics_publisher_
	Publishes joint states from reads. More...

double	diagnostics_rate_
	Desired rate at which servo diagnostic information is published. More...

std::vector< PortInfo >	dynamixel_ports_
	method of control (position/velocity/torque) More...

double	global_max_vel_
	global joint speed limit More...

double	global_torque_limit_
	global joint torque limit More...

bool	ignore_input_velocity_
	can set driver to ignore profile velocity commands in position mode More...

bool	initialised_ = false
	Bool indicating if we are ready for operation. More...

ros::Publisher	joint_state_publisher_
	Publishes joint states from reads. More...

ros::Subscriber	joint_state_subscriber_
	Gets joint states for writes. More...

double	loop_rate_
	Desired loop rate (joint states are published at this rate) More...

std::unique_ptr< ros::NodeHandle >	nh_
	Handler for the ROS Node. More...

bool	parameters_parsed_ = false
	method of control (position/velocity/torque) More...

bool	read_dataport_
	Bool for telling threads to read dataport data. More...

bool	read_diagnostics_
	Bool for telling threads to read diagnostics data. More...

int	recv_queue_size_ = 1
	Receive queue size for desired_joint_states topic. More...

bool	stop_motors_on_shutdown_
	Indicates if the motors should be turned off when the controller stops. More...

sensor_msgs::JointState	write_msg_
	Stores the last message received from the write command topic. More...

std::mutex	write_mutex_
	Mutex for write_msg, as there are potentially multiple threads. More...

bool	write_ready_ = false
	Booleans indicating if we have received commands. More...

Detailed Description

This class forms a ROS Node that provides an interface with the dynamixel series of servos. The controller functions on a timer based callback for updating the motor states. Commands are Continuously sent to the motor and updated via callback once new messages are published to the command Topic. This class also provides a threaded interface for controlling multiple sets of dynamixels simultaneously and synchronously through different serial ports. This allows robots with many motors to reduce the overall IO time required for control.

Definition at line 148 of file dynamixel_interface_controller.h.

Constructor & Destructor Documentation

◆ DynamixelInterfaceController()

dynamixel_interface::DynamixelInterfaceController::DynamixelInterfaceController ( )

Constructor, loads the motor configuration information from the specified yaml file and intialises The motor states.

Definition at line 104 of file dynamixel_interface_controller.cpp.

◆ ~DynamixelInterfaceController()

dynamixel_interface::DynamixelInterfaceController::~DynamixelInterfaceController ( )

Destructor, deletes the objects holding the serial ports and disables the motors if required.

Definition at line 111 of file dynamixel_interface_controller.cpp.

Member Function Documentation

◆ getLoopRate()

double dynamixel_interface::DynamixelInterfaceController::getLoopRate ( void )

inline

Gets the target loop rate for the controller

Returns: the target loop rate for the controller (Hz)

Definition at line 174 of file dynamixel_interface_controller.h.

◆ initialise()

bool dynamixel_interface::DynamixelInterfaceController::initialise ( )

Initialises the controller, performing the following steps:

for each port:
- Initialise driver
- for each dynamixel - load model information
  - validate compatibility with port
    Returns
    true on successful initialisation, false otherwise

Definition at line 571 of file dynamixel_interface_controller.cpp.

◆ initialiseDynamixel()

bool dynamixel_interface::DynamixelInterfaceController::initialiseDynamixel	(	PortInfo &	port,
		DynamixelInfo &	dynamixel
	)

private

Initialises the dynamixel. Pings the given id to make sure it exists, then loads it's model information and sets up the relevant registers

Parameters

[in]	port	the port for this dynamixel
[in]	dynamixel	the dynamixel to intialise

Returns: true on successful initialisation, false otherwise

Initialises the dynamixel. Pings the given id to make sure it exists, then loads it's model information and sets up the relevant registers

Returns: true on successful initialisation, false otherwise

Definition at line 711 of file dynamixel_interface_controller.cpp.

◆ initialisePort()

bool dynamixel_interface::DynamixelInterfaceController::initialisePort ( PortInfo & port )

private

Initialises the port, opening the driver, and validating all dynamixels

Parameters

[in] port the port object to initialise

Returns: true on successful initialisation, false otherwise

Initialises the port, opening the driver, and validating all dynamixels

Returns: true on successful initialisation, false otherwise

Definition at line 607 of file dynamixel_interface_controller.cpp.

◆ jointStateCallback()

void dynamixel_interface::DynamixelInterfaceController::jointStateCallback ( const sensor_msgs::JointState::ConstPtr & joint_commands )

private

Callback for recieving a command from the /desired_joint_state topic. The function atomically updates the class member variable containing the latest message and sets a flag indicating a new message has been received

Parameters

[in] joint_commands the command received from the topic

Definition at line 883 of file dynamixel_interface_controller.cpp.

◆ loop()

void dynamixel_interface::DynamixelInterfaceController::loop ( void )

main loop for performing IO, handles the creation and joining of IO threads for each port, so that IO for multiple usb devices can be threaded.

Definition at line 958 of file dynamixel_interface_controller.cpp.

◆ multiThreadedIO()

void dynamixel_interface::DynamixelInterfaceController::multiThreadedIO	(	PortInfo &	port,
		sensor_msgs::JointState &	read_msg,
		dynamixel_interface::DataPorts &	dataport_msg,
		dynamixel_interface::ServoDiags &	diags_msg,
		bool	perform_write
	)		const

private

Top level control function for each port's IO thread.

Parameters

[in]	port	the port for this thread to perform IO on
[out]	read_msg	the JointState this threads joint data is read into
[out]	dataport_msg	the DataPorts msg this thread reads dataport data into
[out]	status_msgs	the ServoDiags msg this thread reads diagnostic data into
[in]	perform_write	whether we are writing data this iteration

Definition at line 1105 of file dynamixel_interface_controller.cpp.

◆ multiThreadedRead()

void dynamixel_interface::DynamixelInterfaceController::multiThreadedRead	(	PortInfo &	port,
		sensor_msgs::JointState &	read_msg,
		dynamixel_interface::DataPorts &	dataport_msg,
		dynamixel_interface::ServoDiags &	diags_msg
	)		const

private

Function in thread to perform read on a port.

Parameters

[in]	port	the port for this thread to perform IO on
[out]	read_msg	the JointState this threads joint data is read into
[out]	dataport_msg	the DataPorts msg this thread reads dataport data into
[out]	status_msgs	the ServoDiags msg this thread reads diagnostic data into

Definition at line 1374 of file dynamixel_interface_controller.cpp.

◆ multiThreadedWrite()

void dynamixel_interface::DynamixelInterfaceController::multiThreadedWrite	(	PortInfo &	port,
		sensor_msgs::JointState	joint_commands
	)		const

private

Function called in each thread to perform a write on a port

Parameters

[in]	port_num	index used to retrieve port information from port list
[in]	joint_commands	message cointaining the commands for each joint

Definition at line 1123 of file dynamixel_interface_controller.cpp.

◆ parseParameters()

bool dynamixel_interface::DynamixelInterfaceController::parseParameters ( void )

Parses param information from the rosparam server

Returns: true if all params parsed successfully, false otherwise

don't parse parameters during operation

Definition at line 131 of file dynamixel_interface_controller.cpp.

◆ parsePortInformation()

bool dynamixel_interface::DynamixelInterfaceController::parsePortInformation ( XmlRpc::XmlRpcValue ports )

private

Parses the information in the yaml file for each port

Parameters

[in] ports the xml structure to be parsed

Returns: true if all params parsed false otherwise

Parses the information in the yaml file for each port

Parameters

[in] ports the xml structure to be parsed

don't parse parameters during operation

Definition at line 267 of file dynamixel_interface_controller.cpp.

◆ parseServoInformation()

bool dynamixel_interface::DynamixelInterfaceController::parseServoInformation	(	PortInfo &	port,
		XmlRpc::XmlRpcValue	servos
	)

private

Parses the information in the yaml file for each servo

Parameters

[out]	port	the port object to parse the servo info into
[out]	servos	the xml structure to be parsed

Returns: true if all params parsed, false otherwise

Parses the information in the yaml file for each servo

Parameters

[in]	port	the port object to parse the servo info into
[in]	servos	the xml structure to be parsed

don't parse parameters during operation

Definition at line 420 of file dynamixel_interface_controller.cpp.

Member Data Documentation

◆ broadcast_timer_

ros::Timer dynamixel_interface::DynamixelInterfaceController::broadcast_timer_

private

Timer that controls the rate of the IO callback.

Definition at line 248 of file dynamixel_interface_controller.h.

◆ control_type_

DynamixelControlMode dynamixel_interface::DynamixelInterfaceController::control_type_

private

Definition at line 260 of file dynamixel_interface_controller.h.

◆ dataport_counter_

uint dynamixel_interface::DynamixelInterfaceController::dataport_counter_ = 0

private

counter for tracking dataport rate

Definition at line 257 of file dynamixel_interface_controller.h.

◆ dataport_iters_

uint dynamixel_interface::DynamixelInterfaceController::dataport_iters_ = 0

private

publish when dataport_counter_ == this

Definition at line 258 of file dynamixel_interface_controller.h.

◆ dataport_publisher_

ros::Publisher dynamixel_interface::DynamixelInterfaceController::dataport_publisher_

private

Publishes the data from the external ports on dynamixel_pros.

Definition at line 245 of file dynamixel_interface_controller.h.

◆ dataport_rate_

double dynamixel_interface::DynamixelInterfaceController::dataport_rate_

private

Rate at which the pro external dataport is read.

Definition at line 273 of file dynamixel_interface_controller.h.

◆ debug_publisher_

ros::Publisher dynamixel_interface::DynamixelInterfaceController::debug_publisher_

private

Debug message publisher.

Definition at line 246 of file dynamixel_interface_controller.h.

◆ diagnostics_counter_

uint dynamixel_interface::DynamixelInterfaceController::diagnostics_counter_ = 0

private

Counter for tracking diagnostics rate.

Definition at line 252 of file dynamixel_interface_controller.h.

◆ diagnostics_iters_

uint dynamixel_interface::DynamixelInterfaceController::diagnostics_iters_ = 0

private

publish when diagnostic_counter_ == this

Definition at line 253 of file dynamixel_interface_controller.h.

◆ diagnostics_publisher_

ros::Publisher dynamixel_interface::DynamixelInterfaceController::diagnostics_publisher_

private

Publishes joint states from reads.

Definition at line 244 of file dynamixel_interface_controller.h.

◆ diagnostics_rate_

double dynamixel_interface::DynamixelInterfaceController::diagnostics_rate_

private

Desired rate at which servo diagnostic information is published.

Definition at line 272 of file dynamixel_interface_controller.h.

◆ dynamixel_ports_

std::vector<PortInfo> dynamixel_interface::DynamixelInterfaceController::dynamixel_ports_

private

method of control (position/velocity/torque)

Definition at line 234 of file dynamixel_interface_controller.h.

◆ global_max_vel_

double dynamixel_interface::DynamixelInterfaceController::global_max_vel_

private

global joint speed limit

Definition at line 268 of file dynamixel_interface_controller.h.

◆ global_torque_limit_

double dynamixel_interface::DynamixelInterfaceController::global_torque_limit_

private

global joint torque limit

Definition at line 269 of file dynamixel_interface_controller.h.

◆ ignore_input_velocity_

bool dynamixel_interface::DynamixelInterfaceController::ignore_input_velocity_

private

can set driver to ignore profile velocity commands in position mode

Definition at line 266 of file dynamixel_interface_controller.h.

◆ initialised_

bool dynamixel_interface::DynamixelInterfaceController::initialised_ = false

private

Bool indicating if we are ready for operation.

Definition at line 263 of file dynamixel_interface_controller.h.

◆ joint_state_publisher_

ros::Publisher dynamixel_interface::DynamixelInterfaceController::joint_state_publisher_

private

Publishes joint states from reads.

Definition at line 243 of file dynamixel_interface_controller.h.

◆ joint_state_subscriber_

ros::Subscriber dynamixel_interface::DynamixelInterfaceController::joint_state_subscriber_

private

Gets joint states for writes.

Definition at line 241 of file dynamixel_interface_controller.h.

◆ loop_rate_

double dynamixel_interface::DynamixelInterfaceController::loop_rate_

private

Desired loop rate (joint states are published at this rate)

Definition at line 271 of file dynamixel_interface_controller.h.

◆ nh_

std::unique_ptr<ros::NodeHandle> dynamixel_interface::DynamixelInterfaceController::nh_

private

Handler for the ROS Node.

Definition at line 232 of file dynamixel_interface_controller.h.

◆ parameters_parsed_

bool dynamixel_interface::DynamixelInterfaceController::parameters_parsed_ = false

private

method of control (position/velocity/torque)

Bool indicating if we have parsed parameters

Definition at line 262 of file dynamixel_interface_controller.h.

◆ read_dataport_

bool dynamixel_interface::DynamixelInterfaceController::read_dataport_

private

Bool for telling threads to read dataport data.

Definition at line 256 of file dynamixel_interface_controller.h.

◆ read_diagnostics_

bool dynamixel_interface::DynamixelInterfaceController::read_diagnostics_

private

Bool for telling threads to read diagnostics data.

Definition at line 251 of file dynamixel_interface_controller.h.

◆ recv_queue_size_

int dynamixel_interface::DynamixelInterfaceController::recv_queue_size_ = 1

private

Receive queue size for desired_joint_states topic.

Definition at line 274 of file dynamixel_interface_controller.h.

◆ stop_motors_on_shutdown_

bool dynamixel_interface::DynamixelInterfaceController::stop_motors_on_shutdown_

private

Indicates if the motors should be turned off when the controller stops.

Definition at line 265 of file dynamixel_interface_controller.h.

◆ write_msg_

sensor_msgs::JointState dynamixel_interface::DynamixelInterfaceController::write_msg_

private

Stores the last message received from the write command topic.

Definition at line 236 of file dynamixel_interface_controller.h.

◆ write_mutex_

std::mutex dynamixel_interface::DynamixelInterfaceController::write_mutex_

private

Mutex for write_msg, as there are potentially multiple threads.

Definition at line 238 of file dynamixel_interface_controller.h.

◆ write_ready_

bool dynamixel_interface::DynamixelInterfaceController::write_ready_ = false

private

Booleans indicating if we have received commands.

Definition at line 239 of file dynamixel_interface_controller.h.

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

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ DynamixelInterfaceController()

◆ ~DynamixelInterfaceController()

Member Function Documentation

◆ getLoopRate()

◆ initialise()

◆ initialiseDynamixel()

◆ initialisePort()

◆ jointStateCallback()

◆ loop()

◆ multiThreadedIO()

◆ multiThreadedRead()

◆ multiThreadedWrite()

◆ parseParameters()

◆ parsePortInformation()

◆ parseServoInformation()

Member Data Documentation

◆ broadcast_timer_

◆ control_type_

◆ dataport_counter_

◆ dataport_iters_

◆ dataport_publisher_

◆ dataport_rate_

◆ debug_publisher_

◆ diagnostics_counter_

◆ diagnostics_iters_

◆ diagnostics_publisher_

◆ diagnostics_rate_

◆ dynamixel_ports_

◆ global_max_vel_

◆ global_torque_limit_

◆ ignore_input_velocity_

◆ initialised_

◆ joint_state_publisher_

◆ joint_state_subscriber_

◆ loop_rate_

◆ nh_

◆ parameters_parsed_

◆ read_dataport_

◆ read_diagnostics_

◆ recv_queue_size_

◆ stop_motors_on_shutdown_

◆ write_msg_

◆ write_mutex_

◆ write_ready_