Niryo_robot_commander

This package is the one dealing with all commander related stuff.
It is composed of only one node, which is running separately the arm commander and the tool commander.

Commander node

The ROS Node is made to interact with:
  • The Arm through MoveIt!
  • The tools through the tool controller.
All commands are firstly received on the actionlib server which:
  • Handles concurrent requests.
  • Checks if the command can’t be processed due to other factors (ex: learning mode).
  • Validates parameters.
  • Calls required controllers and returns appropriate status and message.

The namespace used is: /niryo_robot_commander/

Parameters - Commander

Commander’s Parameters
Name Description
reference_frame
Reference frame used by MoveIt! for moveP.
Default : ‘world’
move_group_commander_name Name of the group that MoveIt is controlling. By default: “arm”
jog_timer_rate_sec Publish rate for jog controller
simu_gripper If you are using the simulated Gripper and want to control the Gripper

Action Server - Commander

Commander Package Action Servers
Name Message Type Description
robot_action RobotMove Command the arm and tools through an action server

Services - Commander

Commander Package Services
Name Message Type Description
is_active GetBool Indicate whereas a command is actually running or not
stop_command Trigger Stop the actual command
set_max_velocity_scaling_factor SetInt Set a percentage of maximum speed
/niryo_robot/kinematics/forward GetFK Compute a Forward Kinematic
/niryo_robot/kinematics/inverse GetIK Compute a Inverse Kinematic

Messages - Commander

Commander Package Messages
Name Description
ArmMoveCommand Message to command the arm
RobotCommand Message to command the robot (arm + tool)
ShiftPose Message for shifting pose
PausePlanExecution Pause movement execution

All these services are available as soon as the node is started.

Action, services & messages files - Commander

RobotMove (Action)

# goal
niryo_robot_commander/RobotCommand cmd
---
# result
int32 status
string message
---
# feedback
niryo_robot_msgs/RobotState state

GetFK (Service)

float32[] joints
---
niryo_robot_msgs/RobotState pose

GetIK (Service)

niryo_robot_msgs/RobotState pose
---
bool success
float32[] joints

JogShift (Service)

int32 JOINTS_SHIFT = 1
int32 POSE_SHIFT = 2

int32 cmd

float32[] shift_values

---
int32 status
string message

ArmMoveCommand (Message)

int32 cmd_type

float64[] joints
geometry_msgs/Point position
niryo_robot_msgs/RPY rpy
geometry_msgs/Quaternion orientation
niryo_robot_commander/ShiftPose shift

geometry_msgs/Pose[] list_poses
float32 dist_smoothing

PausePlanExecution (Message)

int8 STANDBY = 0
int8 PLAY = 1
int8 PAUSE = 2
int8 RESUME = 3
int8 CANCEL = 4

int8 state

RobotCommand (Message)

int32 MOVE_ONLY = 1
int32 TOOL_ONLY = 2

int32 cmd_type

niryo_robot_commander/ArmMoveCommand arm_cmd

niryo_robot_tools/ToolCommand tool_cmd

# In the future, allow a tool command to be launched at the same time as an Arm command
# 3 choices : arm only, arm + tool, tool only

ShiftPose (Message)

int32 axis_number
float64 value