Niryo robot tools commander¶
Provides functionalities to control end-effectors and accessories for Ned.
This package allows to manage the TCP (Tool Center Point) of the robot. If the functionality is activated, all the movements (in Cartesian coordinates [x, y, z, roll, pitch, yaw]) of the robot will be performed according to this TCP. The same program can then work with several tools by adapting the TCP transformation to them. By default this feature is disabled, but can be enabled through the robot services.
Tools Commander node¶
The ROS Node is made of services to equip tool, an action server for tool command and topics for the current tool or the tool state.
It belongs to the ROS namespace: /niryo_robot_tools_commander/
.
Action server - tools¶
Name | Message Type | Description |
---|---|---|
action_server |
ToolAction | Command the tool through an action server |
Publisher - tools¶
Name | Message Type | Description |
---|---|---|
current_id |
std_msgs/Int32 | Publishes the current tool ID |
tcp |
TCP | Publishes if the TCP (Tool Center Point) is enabled and transformation between the tool_link and the TCP |
Services - tools¶
Name | Message Type | Description |
---|---|---|
update_tool |
std_srvs/Trigger | Pings/scans for a dxl motor flashed with an ID corresponding to a tool and equip it (if found) |
equip_electromagnet |
SetInt | Equips the electromagnet with the motor ID given as parameter |
enable_tcp |
SetBool | Enables or disablse the TCP (Tool Center Point) functionality.
When we activate it, the transformation will be the last one saved since the robot started.
By default it will be the one of the equipped tool.
|
set_tcp |
SetTCP | Activates the TCP (Tool Center Point) functionality and defines a new TCP transformation. |
reset_tcp |
std_srvs/Trigger | Resets the TCP transformation. By default it will be the one of the equipped tool. |
Dependencies - tools¶
Action files - tools¶
ToolAction (Action)¶
# goal
niryo_robot_tools_commander/ToolCommand cmd
---
# result
int32 status
string message
---
# feedback
int32 progression
Messages files - tools¶
ToolCommand (Message)¶
# Gripper
int8 OPEN_GRIPPER = 1
int8 CLOSE_GRIPPER = 2
# Vacuump pump
int8 PULL_AIR_VACUUM_PUMP = 10
int8 PUSH_AIR_VACUUM_PUMP = 11
# Tools controlled by digital I/Os
int8 SETUP_DIGITAL_IO = 20
int8 ACTIVATE_DIGITAL_IO = 21
int8 DEACTIVATE_DIGITAL_IO = 22
uint8 cmd_type
# Gripper1= 11, Gripper2=12, Gripper3=13, VaccuumPump=31, Electromagnet=30
int8 tool_id
# if gripper Ned1/One
uint16 speed
# if gripper Ned2
uint8 max_torque_percentage
uint8 hold_torque_percentage
# if vacuum pump or electromagnet grove
bool activate
# if tool is set by digital outputs (electromagnet)
string gpio
TCP (Message)¶
bool enabled
geometry_msgs/Point position
niryo_robot_msgs/RPY rpy
geometry_msgs/Quaternion orientation