MARA - ROS2 API

NOTE: Work in progress. We're continuously improving the documentation, stay tuned for updates

Remember that the naming convention in H-ROS is the following:

  • nodes /hrim_<device_kind>_<device_name>
  • topic /hrim_<device_kind>_<device_name>/<topic_name>.
  • service /hrim_<device_kind>_<device_name>/<service_name>.

MARA makes use of HRIM, capturing relevant information to improve the user experience and ensure a correct operation of the whole robotic system. Six of the available topics/services are generic messages such as:

Note: The following models are provided in XML format. HRIM is used to generate ROS 2 communication artifacts out of them while enabling interoperability with other frameworks (ROS, OPC UA, etc.). Refer to https://github.com/AcutronicRobotics/HRIM for more information on the HRIM information model

  • GoalRotaryServo model allows to control the position, velocity or/and effort.
  • StateRotaryServo.msg publishes the status of the motor.

This service shows the device features using the service SpecsRotaryServo.srv. To visualize the information in the terminal type: ros2 service call /hrim_actuator_rotaryservo_000000000001/specs hrim_actuator_rotaryservo_srvs/SpecsRotaryServo {}

response:
hrim_actuator_rotaryservo_srvs.srv.SpecsRotaryServo_Response(control_type=4, range_min=-6.27, range_max=6.27, precision=8.722222e-05, rated_speed=1.46607657, reachable_speed=1.46607657, rated_torque=9.0, reachable_torque=13.0, temperature_range_min=-10.0, temperature_range_max=50.0)

This topic controls the position, velocity or/and effort by using the message GoalRotaryServo.msg. This message allows to control the joint in different ways depending the control type specified in the message. Here we have a list of the available different types of control:

  • CONTROL_TYPE_NONE:
  • CONTROL_TYPE_POSITION:
  • CONTROL_TYPE_EFFORT:
  • CONTROL_TYPE_VELOCITY:
  • CONTROL_TYPE_POSITION_VELOCITY:
  • CONTROL_TYPE_POSITION_EFFORT:
  • CONTROL_TYPE_VELOCITY_EFFORT:
  • CONTROL_TYPE_POSITION_VELOCITY_EFFORT:

This topic controls the position, velocity or/and effort by using the message GoalRotaryServo.msg. To visualize the topic in the terminal, just type: ros2 topic echo /hrim_actuator_rotaryservo_000000000001/goal

For example, in order to move an axis we will send the positon using a goal msg with a position-velocity control type. We could send for example a command to move the axis 1 to 0.5 rad position with a 0.1 rad/s velocity:

 $ ros2 topic pub -1 /hrim_actuator_rotaryservo_XXXXXXXXXXXX/goal_axis1 hrim_actuator_rotaryservo_msgs/GoalRotaryServo "{control_type: 4, position: 0.5, velocity: 0.1}"

To visualize the topic in the terminal, just type: ros2 topic echo /hrim_actuator_rotaryservo_XXXXXXXXXXXX/goal. We should observe the goals we are sending.

$ ros2 topic echo /hrim_actuator_rotaryservo_XXXXXXXXXXXX/goal

header:
  stamp:
    sec: 0
    nanosec: 0
  frame_id: ''
control_type: 4
position: 0.5
velocity: 0.10000000149011612
effort: 0.0

The state topic gives us information about the motor conditions and the causes in case of error by using the message StateRotaryServo.

To visualize the information about this topic in the terminal, you need to type ros2 topic echo /hrim_actuator_rotaryservo_XXXXXXXXXXXX/state:

$ ros2 topic echo /hrim_actuator_rotaryservo_XXXXXXXXXXXX/state

header:
  stamp:
    sec: 2198
    nanosec: 553000000
  frame_id: ''
goal: 0.0
position: -1.2887455911325674e-06
error: 1.2887455911325674e-06
velocity: -2.501547525126175e-07
effort: 0.0
load: 0.0
moving: false
fault: 0
control_type: 0

  • SpecsRotaryServo.srv is a custom message which reports the main features of the device.
  • EnableDisable.srv disables or enables the servo motor.
  • ClearFault.srv sends a request to clear any fault in the servo motor.
  • Stop.srv request to stops any ongoing movement.
  • OpenCloseBrake.srv opens or closes the servo motor brake.

This service shows the device features. To visualize the information in the terminal type: ros2 service call /hrim_actuator_rotaryservo_XXXXXXXXXXXX/specs hrim_actuator_rotaryservo_srvs/SpecsRotaryServo {}

$ ros2 service call /hrim_actuator_rotaryservo_XXXXXXXXXXXX/specs hrim_actuator_rotaryservo_srvs/SpecsRotaryServo {}

response:
hrim_actuator_rotaryservo_srvs.srv.SpecsRotaryServo_Response(control_type=4, range_min=-6.27, range_max=6.27, precision=8.722222e-05, rated_speed=1.46607657, reachable_speed=1.46607657, rated_torque=9.0, reachable_torque=13.0, temperature_range_min=-10.0, temperature_range_max=50.0)

This service request the HAN's module to enable the servo. Once the servo is enabled the module will accept commands for both axis. In order to enable the servo using the command line you need to type: ros2 service call /hrim_actuator_rotaryservo_XXXXXXXXXXXX/enable hrim_actuator_rotaryservo_srvs/EnableDisable {}

$ ros2 service call /hrim_actuator_rotaryservo_XXXXXXXXXXXX/enable hrim_actuator_rotaryservo_srvs/EnableDisable {enable: true}

response:
hrim_actuator_rotaryservo_srvs.srv.EnableDisable_Response(success=True, message="module is enabled")

This service request the HAN's module to enable the servo. Once the servo is enabled the module will accept commands for both axis. In order to enable the servo using the command line you need to type: ros2 service call /hrim_actuator_rotaryservo_XXXXXXXXXXXX/disable hrim_actuator_rotaryservo_srvs/EnableDisable {}

$ ros2 service call /hrim_actuator_rotaryservo_XXXXXXXXXXXX/disable hrim_actuator_rotaryservo_srvs/EnableDisable {enable: false}

response:
hrim_actuator_rotaryservo_srvs.srv.EnableDisable_Response(success=True, message="module is disabled")

This service sends a request to stop a movement of a specific axis. For example if we want to stop the movement of the axis 1 we would write in the terminal type: ros2 service call /hrim_actuator_rotaryservo_XXXXXXXXXXXX/stop_axis1 hrim_actuator_rotaryservo_srvs/Stop {}

$ ros2 service call /hrim_actuator_rotaryservo_XXXXXXXXXXXX/stop_axis1 hrim_actuator_rotaryservo_srvs/Stop {}

response:
hrim_actuator_rotaryservo_srvs.srv.EnableRotaryServo_Response(success=True, message="motor is stopped")

This service sends a request to clear any fault in the servo motor.

When the module is not enabled, you can open the brake of the corresponding module using this service. For example:

$ ros2 service call /hrim_actuator_rotaryservo_XXXXXXXXXXXX/open_brake1 hrim_actuator_rotaryservo_srvs/OpenCloseBrake {open: true}

response:
hrim_actuator_rotaryservo_srvs.srv.OpenCloseBrake_Response(success=True, message="brake is openned")

When the module is not enabled, you can close the brake of the corresponding module using this service. For example:

$ ros2 service call /hrim_actuator_rotaryservo_XXXXXXXXXXXX/close_brake1 hrim_actuator_rotaryservo_srvs/OpenCloseBrake {open: true}

response:
hrim_actuator_rotaryservo_srvs.srv.OpenCloseBrake_Response(success=True, message="brake is closed")

  • GoalJointTrajectory action that allows to move the joint using a trajectory msg.

The joint trajectory action provides an action interface for tracking trajectory execution. It passes trajectory goals to the modular joint controller, and reports success when they have finished executing. The joint trajectory action can also enforce constraints on the trajectory, and abort trajectory execution when the constraints are violated.

The joint trajectory action interface receives goals each of which describes the trajectory that the controller will attempt to execute. This message contains:

  • The joint name of the axis.
  • a set of waypoints, each of them containing the desired position, velocity and acceleration at each joint. It is assumed that the values in the waypoints are ordered.

The JointTrajectory message has two fields for managing time:

  • the stamp field in the header of the message.
  • a time_from_start value for each waypoint.

The stamp in the header determines when the execution of the trajectory starts. Once the time in the stamp field passes, the controller will begin to move towards the first waypoint in the trajectory. The controller attempts to achieve each waypoint at the time obtained by adding that waypoint's time_from_start value to the time in stamp. The time_from_start values must be monotonically non-decreasing or else the trajectory will be considered invalid.

The joint trajectory action interface sends feedback about the state of the joint during the execution of the trajectory. This message contains:

  • actual returns the current position of the motor.
  • desired returns the desired position of the motor.
  • error returns the error between the current position and the desired position.

The joint trajectory action interface returns a message when the trajectory finished (because it reaches the goal or it's cancel). This message contains:

  • error shows the number code of the error.
  • error_string if an error has happened the field returns a string with more details about the error.

Note the Han´s Robot Joints are two axis modules. This means that each device has topics specific for each axis. Therefore, in the case of goal topic we will have two different topics, goal_axis1 and goal_axis2. However, other topics work at a device level, for example enable.

The following table shows more information about the topics:

Topic Name Rate Pub/Sub
goal /hrim_actuator_rotaryservo_XXXXXXXXXXXX/goal_axis1 200 Hz Sub
goal /hrim_actuator_rotaryservo_XXXXXXXXXXXX/goal_axis2 200 Hz Sub
state /hrim_actuator_rotaryservo_XXXXXXXXXXXX/state_axis1 200 Hz Pub
state /hrim_actuator_rotaryservo_XXXXXXXXXXXX/state_axis2 200 Hz Pub

The following table shows more information about the services:

Service Name
specs /hrim_actuator_rotaryservo_XXXXXXXXXXXX/specs
enable servo /hrim_actuator_rotaryservo_XXXXXXXXXXXX/enable
disable servo /hrim_actuator_rotaryservo_XXXXXXXXXXXX/disable
clear fault /hrim_actuator_rotaryservo_XXXXXXXXXXXX/clear_fault
stop /hrim_actuator_rotaryservo_XXXXXXXXXXXX/stop_axis1
stop /hrim_actuator_rotaryservo_XXXXXXXXXXXX/stop_axis2
close brake /hrim_actuator_rotaryservo_XXXXXXXXXXXX/close_brake_axis1
close brake /hrim_actuator_rotaryservo_XXXXXXXXXXXX/close_brake_axis2
open brake /hrim_actuator_rotaryservo_XXXXXXXXXXXX/open_brake_axis1
open brake /hrim_actuator_rotaryservo_XXXXXXXXXXXX/open_brake_axis2

The following table shows more information about the actions:

Action Name
goal joint trajectory /hrim_actuator_rotaryservo_XXXXXXXXXXXX/trajectory_axis1
goal joint trajectory /hrim_actuator_rotaryservo_XXXXXXXXXXXX/trajectory_axis2

To visualize the nodes that are running in the ROS 2.0 network in the terminal, you should run ros2 node list:

/hrim_actuator_rotaryservo_000000000001
/hrim_actuator_rotaryservo_000000000002
/hrim_actuator_rotaryservo_000000000003

To visualize the topics that are been exposed by MARA, you should run ros2 topic list. Then, you will see the following topics:

/hrim_actuator_rotaryservo_000000000001/goal_axis1
/hrim_actuator_rotaryservo_000000000001/goal_axis2
/hrim_actuator_rotaryservo_000000000001/power
/hrim_actuator_rotaryservo_000000000001/state_axis1
/hrim_actuator_rotaryservo_000000000001/state_axis2
/hrim_actuator_rotaryservo_000000000001/state_comm
/hrim_actuator_rotaryservo_000000000001/status

To visualize the services that are been exposed by MARA, you should run ros2 service list. Then, you will see the following topics:

....
/hrim_actuator_rotaryservo_000000000001/id
/hrim_actuator_rotaryservo_000000000001/module_3d
/hrim_actuator_rotaryservo_000000000001/module_urdf
/hrim_actuator_rotaryservo_000000000001/specs
/hrim_actuator_rotaryservo_000000000001/specs_comm
/hrim_actuator_rotaryservo_000000000002/id
/hrim_actuator_rotaryservo_000000000002/module_3d
/hrim_actuator_rotaryservo_000000000002/module_urdf
/hrim_actuator_rotaryservo_000000000002/specs
/hrim_actuator_rotaryservo_000000000002/specs_comm
/hrim_actuator_rotaryservo_000000000003/id
/hrim_actuator_rotaryservo_000000000003/module_3d
/hrim_actuator_rotaryservo_000000000003/module_urdf
/hrim_actuator_rotaryservo_000000000003/specs
/hrim_actuator_rotaryservo_000000000003/specs_comm
....

We have two actions available per motor:

...
/hrim_actuator_rotaryservo_000000000001/trajectory_axis2
/hrim_actuator_rotaryservo_000000000001/trajectory_axis2
/hrim_actuator_rotaryservo_000000000002/trajectory_axis1
/hrim_actuator_rotaryservo_000000000002/trajectory_axis2
/hrim_actuator_rotaryservo_000000000003/trajectory_axis1
/hrim_actuator_rotaryservo_000000000003/trajectory_axis2
...

There is no command to visualize actions alone, but we can run the following command to see the topics of the actions:

Actions are compossed by topics and services. However, these topics are not meant to be used directly but using action interfaces. Action topics and services are hidden but it is possible to visualize them from the comomand line using the --include-hidden-topics and --include-hidden-services parameters.

$ ros2 topic --include-hidden-topics list

/hrim_actuator_rotaryservo_XXXXXXXXXXXX/trajectory_axis1/_action/feedback
/hrim_actuator_rotaryservo_XXXXXXXXXXXX/trajectory_axis1/_action/status
/hrim_actuator_rotaryservo_XXXXXXXXXXXX/trajectory_axis2/_action/feedback
/hrim_actuator_rotaryservo_XXXXXXXXXXXX/trajectory_axis2/_action/status
$ ros2 service --include-hidden-services list

/hrim_actuator_rotaryservo_XXXXXXXXXXXX/trajectory_axis1/_action/cancel_goal
/hrim_actuator_rotaryservo_XXXXXXXXXXXX/trajectory_axis2/_action/cancel_goal

There are also generic topics/services common to all type of components.

  • StateCommunication.msg is a custom message which reports the state of the device network.
  • Power.msg that publishes the power consumption.
  • Status.msg which inform about the resources that are consumed, SpecsCommunication.msg and StateCommunication.msg that are created to inform about communication aspects.

To visualize the topic, you need to type ros2 topic echo /hrim_actuator_rotaryservo_000000000001/power

header:
  stamp:
    sec: 1277
    nanosec: 848000000
  frame_id: ''
voltage: 48.0
current_consumption: 0.10000000149011612
current_surplus: 0.0
power_consumption: 4.800000190734863
power_surplus: 0.0

To visualize the topic, you need to type ros2 topic echo /hrim_actuator_rotaryservo_000000000001/status

header:
  stamp:
    sec: 1326
    nanosec: 580000000
  frame_id: ''
instance_id: 0
system_cpu: 0.0
core_temperature: 0.0
system_ram_total: 0
system_ram_used: 0
system_ram_free: 0
system_io_in: 0.0
system_io_out: 0.0
system_ipv4_received: 0.0
system_ipv4_sent: 0.0
ipv4_tcpsock: 0
ipv4_tcppackets_received: 0.0
ipv4_tcppackets_sent: 0.0
ipv4_tcp_errors: 0.0
ipv4_udppackets_received: 0.0
ipv4_udppackets_sent: 0.0
ipv4_udp_errors: 0.0
cpu_interrupts: 0.0
cpu_context_switches: 0.0
softnet_processed: 0
softnet_dropped: 0
softnet_squeezed: 0
softnet_received_rps: 0
softnet_flow_limit_count: 0
softirqs_hi: 0
softirqs_timer: 0
softirqs_net_tx: 0
softirqs_net_rx: 0
softirqs_block: 0
softirqs_irq_poll: 0
softirqs_tasklet: 0
softirqs_sched: 0
softirqs_hrtimer: 0
softirqs_rcu: 0
load1: 0.0
load5: 0.0
load15: 0.0

The communication state topic gives us information about the network conditions using the message SpecsCommunication.msg. To visualize the information about this topic in the terminal, you need to type ros2 topic echo /hrim_actuator_rotaryservo_000000000001/state_comm:

header:
  stamp:
    sec: 720
    nanosec: 205000000
  frame_id: ''
comm_rate: 0.0
size_msgs: 0.0

  • ID.srv which publishes the general identity of the component.
  • Simulation3D.srv and SimulationURDF.srv.msg that sends the device 3D model and related information.
  • SpecsCommunication.srv is a custom message which reports the specs of the device network.

To visualize the topic, you need to type ros2 service call /hrim_actuator_rotaryservo_000000000001/id hrim_generic_srvs/ID {}

response:
hrim_generic_srvs.srv.ID_Response(device_kind_id=2, device_name='', vendor_id=0, product_id=0, instance_id=0, hrim_version='Coliza', hrim_version='Ardent')

The communication specs service gives us information about the specs of the device network using the service StateCommunication. To visualize the information about this service in the terminal, you need to type ros2 service call /hrim_actuator_rotaryservo_000000000001/specs_comm hrim_generic_srvs/SpecsCommunication {}:

response:
hrim_generic_srvs.srv.SpecsCommunication_Response(min_comm_rate=0.0, max_comm_rate=0.0, max_size_msgs=0.0)

The following table shows more information about generic topics:

Topic Name Rate Pub/Sub
status /hrim_actuator_rotaryservo_XXXXXXXXXXXX/status 1 Hz Pub
power /hrim_actuator_rotaryservo_XXXXXXXXXXXX/power 1 Hz Pub
state comm /hrim_actuator_rotaryservo_XXXXXXXXXXXX/state_comm 1 Hz Pub

The following table shows more information about generic services:

Service Name
id /hrim_actuator_rotaryservo_XXXXXXXXXXXX/id
module_3d /hrim_actuator_rotaryservo_XXXXXXXXXXXX/module_3d
module_urdf /hrim_actuator_rotaryservo_XXXXXXXXXXXX/module_urdf
specs comm /hrim_actuator_rotaryservo_XXXXXXXXXXXX/specs_comm