Niryo robot tools commander package

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.

The namespace used is: /niryo_robot_tools_commander/

Action server - tools

Tools Package Action Server
Name Message Type Description
action_server ToolAction Command the tool through an action server

Publisher - tools

Tools Package Publishers
Name Message Type Description
current_id std_msgs/Int32 Publish the current tool ID
tcp TCP Publish if the TCP (Tool Center Point) is enabled and transformation between the tool_link and the TCP

Services - tools

Tools Package Services
Name Message Type Description
update_tool std_srvs/Trigger Ping/scan for a dxl motor flashed with an ID corresponding to a tool and equip it (if found)
equip_electromagnet SetInt Equip the electromagnet with the motor ID given as parameter
enable_tcp SetBool
Enable or disable 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 Activate the TCP (Tool Center Point) functionality and defines a new TCP transformation.
reset_tcp std_srvs/Trigger Reset the TCP transformation. By default it will be the one of the equipped tool.

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
uint8 OPEN_GRIPPER = 1
uint8 CLOSE_GRIPPER = 2

# Vacuump pump
uint8 PULL_AIR_VACUUM_PUMP = 10
uint8 PUSH_AIR_VACUUM_PUMP = 11

# Tools controlled by digital I/Os
uint8 SETUP_DIGITAL_IO = 20
uint8 ACTIVATE_DIGITAL_IO = 21
uint8 DEACTIVATE_DIGITAL_IO = 22

uint8 cmd_type

# Gripper1= 11, Gripper2=12, Gripper3=13, VaccuumPump=31, Electromagnet=30
uint8 tool_id

# if gripper close
uint16 gripper_close_speed

# if gripper open
uint16 gripper_open_speed

# if vacuum pump or electromagnet grove
bool activate

# if tool is set by digital outputs (electromagnet)
# if gpio<0 get value in memory
int8 gpio

TCP (Message)

bool enabled

geometry_msgs/Point position
niryo_robot_msgs/RPY rpy
geometry_msgs/Quaternion orientation

Services files - tools

SetTCP (Service)

geometry_msgs/Point position

#Only one of the two is required.
#If both are filled, the quaternion will be chosen by default
niryo_robot_msgs/RPY rpy
geometry_msgs/Quaternion orientation
---
int32 status
string message