action_executor
The Action Executor is in charge of supervising robot actions. It receives actions to execute and stop or suspend orders from the robot decision and calls lower level modules to perform the given action in the best possible way.
For each action to execute, the action executor:
- checks its precondition and find the possible refinement for high level objects
- plans the necessary trajectories
- executes the action
- aplies/checks the post conditions
The actions currently implemented are:
- Pick
- Place
- Drop
- Move to (an arm configuration)
- Place reachable (by an agent)
- Scan
- Pick and drop
- Pick and place
- Pick and place reachable
The node is composed of an action server which allows the robot to execute an action.
The definition of the action can be found here
- action_executor/stop: allows to stop the execution of the current action (std_srvs/Empty).
- /action_executor/current_robot_action: current action of the robot (supervisor_msgs/Action)
- /data_manager/add_data/previous_actions: add the action the robot just performed (or failed to perform) in the previous action (supervisor_msgs/ActionsList).
- /gtp/trajectory: replace the publisher of gtp when we are working of pre-computed trajectories to execute (gtp_ros_msgs/GTPTraj).
- /agent_monitor/factList: facts list from the agent monitor node, used to monitor the human distance to the robot and objects of interest during execution (toaster_msgs/FactsList).
- /human_monitor/current_humans_action: actions list from the agent monitor node, used to check conflicts with the humans actions (supervisor_msgs/Action).
- /gtp/ros_trajectory: trajectories published by gtp, used to save trajectories in the save mode (gtp_ros_msgs/GTPTraj).
- noExec: if true, the robot does not realy execute the action (to be used in simulation, bool)
- noPlanning: if true, the robot does not plan for the action to perform (to be used in simulation with noExec at true, bool)
- saveMode: flag to indicate if the robot should save or load the trajectories (string: save, none or load)
- saveFilePath: path to the file where the trajectories should be saved (string)
- action_executor/restPosition/(right/left): gtp rest positions for the robot arms (string)
- action_executor/shouldUseRightHand: flag to impose the use of the right arm during action execution (bool)
- action_executor/nbPlanMaxGTP: nb max of time trying to get a plan from gtp (int)
- action_executor/humanSafetyJoint: human joint to monitor for safety during action execution (string)
- action_executor/safetyThreshold: distance to human safety joint under which one the robot should stop (double)
- action_executor/toWatchThreshold: distance to human safety joint under which one the robot should find an other object refinement (double)
- action_executor/gripperThreshold: value under which one a gripper is considered empty (double)
- timeScan: time to scan an object in sec (used by the scan action, double)
- timeWaitScan: time to wait for the head focus before scaning an object in sec (used by the scan action, double)
- planningSupport/"object": for a place action, the name of the object will be replace by this parameter for planning (string)
- points/"support"/"object"/(x/y/theta): precise 2D emplacement where to place an object on a support (used for planning, double)
- action_executor/humanCost: name of the human considered to compute costs (string)
- action_executor/uniqueSupports: list of support which can only have one object on it (vector)
To add a new action to be executed by the robot you should:
- create a new class which inherit from VirtualAction (include/action_executor/Actions/.h and src/Actions/.cpp, with action the name the action to implement)
- templates to fill this new class can be founds in include/action_executor/Actions/template.h and src/Actions/template.cpp
- add the new .cpp in the CMakeList
- add a dependencie to the .h in include/action_executor/action_executor.h
- add a new case in the function initializeAction of src/action_executor.cpp
For actions which need motion planning, it is possible to save the computed trajectories for a given action and then replay this same action with the same trajectories.
To save the trajectories of a given action, the node should be run with the parameter saveMode to save. The trajectories will be saved in a .yaml file (path to give in the saveFilePath parameter). Then, to replay these trajectories, the .yaml file should be loaded in the ros parameters (e.g. include it in the launch file) and the node should be run with the parameter saveMode to load.
To simply use the node without saving or loading trajectories, the saveMode parameter should be set to none.