clopema_motoros (package)

motoros package contain mostly ROS nodes that makes an interface between robot controller (MotoPlus application) and ROS. The functionality provided by these nodes include: current robot position streaming, commanding position, and input-output interface.

IO Interface

IO interface is provided by node io_interface. Currently it provides three services write_io, read_io and query_io. The write_io service has two parameters address, which is an IO signal address used by Motoman and boolean value and no return value. The read_io service has only address as a parameter and boolean output value.

The query_io service is an interface to raw data running between controller and ROS node. This raw message is same in both ways and contain max 10 (constant that can be changed in code) entries of address-query-or-response tuples. For each request in the outgoing (from the ROS point of view) there is a reply in the incoming message. The interpretation of the return value depend on the request see clopema_motoros.

Example usage of read_io and write_io using rosservice command line utility (assuming that the roscore is running):

$ rosrun clopema_motoros io_interface <robot_ip_address>               -- Start IO interface node                
... in another terminal ...
$ rosservice call /write_io 10033 True                                 -- Write 1 to #10033 address OUT#0020
$ rosservice call /read_io 10033                                       -- Read from #10033
value: True                                                            -- Returned value

Example usage of query_io using rosservice (assuming that the roscore is running):

$ rosrun clopema_motoros io_interface <robot_ip_address>               -- Start IO interface node 
... in another terminal ...
$ rosservice call /query_io [10030,30] [1,-1]                          -- Create query which will write 1 to #10030 and read #00030 simultaneously
                                                                       -- See "Commands and Replies" for more information
addresses: [10030, 30]                                                 -- Array of addresses
values: [0, 0]                                                         -- Array of values

Note that the write query also has a read response value. However, there is slight delay between write and when the written value can be red. Therefore, the value of a signal directly after writing is somehow random.

The C++ example of all IO Interface services is in the source:clopema-motoros|src/io_client_example.cpp

Requests and replies

IO_NOP     = -3     // No operation (Both reply and request)
IO_INVALID = -2     // Invalid request (Only reply)
IO_READ    = -1     // Read value (Only request)
IO_LOW     =  0     // Read/Write low (Both reply and request)
IO_HIGH    =  1     // Read/Write low (Both reply and request)

IO_READ

The retuned is current IO value (IO_LOW or IO_HIGH) or IO_INVALID when invalid signal number is given.

IO_LOW / IO_HIGH

The returned value is the IO value before the new value was set (IO_LOW or IO_HIGH) or IO_INVALID when invalid signal number is given.

Simple Gripper Interface (deprecated)

The simple gripper interface should demonstrate the usage of IO interface. It provides services for getting the current state of the gripper (which is represented by internal variable) and for setting new state. The state can be open (True) or close (False). When the node is launched it tries to close the gripper in order to set properly the internal state.

Example command line usage:

$ roslaunch clopema_motoros test_gripper_interface.launch  -- Launches io_interface, arm1_gripper and arm2_gripper
... in other terminal ...
$ rosservice call /arm1_gripper/set_open true              -- Open gripper
prev: False                                                -- Previous state of the gripper, True for open and False for close.
$ rosservice call /arm1_gripper/set_open false             -- Close gripper
prev: True                                                 -- The previous state was open

There is also simple GUI:

$ roslaunch clopema_demos gripper_gui_demo.launch

Gripper Interface (GripperCommandAction)

The new gripper interface uses standard GripperCommandAction The example of the C++ actiono is here and .

The gripper interface is started when start_robot.launch or virtual_robot.launch are started. Alternatively it is possible to start just the gripper and required IO interface by calling the following command.

roslaunch clopema_motoros test_gripper_interface.launch virtua:=true

The virtual parameter will switch from real IO interface that needs robot to be running to virtual that doesn't, but can't provide the gripper logic. The use of the virtual IO interface will result in timeout warning from the gripper interface.

Python example:

import actionlib
from control_msgs.msg import GripperCommandAction, GripperCommandGoal

r1_client = actionlib.SimpleActionClient('/r1_gripper/command', GripperCommandAction)
r1_client.wait_for_server()

open_goal = GripperCommandGoal()
open_goal.command.position = 1

r1_client.send_goal(open_goal)
r1_client.wait_for_result()

Command line example:

IO Error Monitor

While using RedCam for calibration we have been faced with a problem that the controller fires a IO signal when in error state. However, this signal is not persistent. The Error Monitor is just for this situation, it repeatedly reads predefined IO values a stores whether a HIGH signal has appeared.

Usage

For how to start the node see source:clopema-motoros|launch/test_io_error_monitor.launch.

rosparam

  • monitored_signals: List of monitored signal, the size is limited by the size of IO message.
  • read_period: Sleep time between each read in seconds.

get_error_status (IOErrorState) service

bool    clear
---
bool    error
int32[] error_signals

Speed control

Speed can be set or get by the set_robot_speed and get_robot_speed services. These are implemented in the source:clopema-motoros|src/joint_trajectory_handler_dual.cpp. The speed can be set from 0 to 1, the lowest recognised step is 1/10000. The speed change does not affect already active motion.

set_robot_speed.srv

float32 speed
---

get_robot_speed.srv

---
float32 speed