NODE mission_manager

the main node of the architecture, launched at last, and combining the ROSPlan flow with the aRMOR ontology.

During the coding phase, the objective was to not modify this node since the version v1.0.0 … and we did it! With a very little adding.

The node implements the high-level mission control of the robot, whereas the ROSPlan framework is used to get the precise plan referred to the high-level actions required by the mission manager. The mission manager has the main role to combine ROSPlan and the aRMOR ontology, as the navigation unit does, for instance. It is, technically speaking, a state machine, mixing the landmarks of the ROSPlan side with updates and queries with aRMOR.

An advantage of such approach is to hide the details of the problem: for instance, thinking in terms of landmarks instead of entire plan makes the system updatable in a simple way, because changing the PDDL doesn’t affect the high-level solution. The only condition is that, at least at a high level of abstraction, the way to accomplish the objective can be described “with the same terms”. For instance, the second part of the project used the manipulation action to collect the hints, whereas the third one used cameras; but, aat high level of abstraction, the “policy” is always the same, so it has been sufficient to change a bit the implementation of the actions dispatched by ROSPlan to make it work.

Authors

Francesco Ganci

Version

v1.0

Note

the node supports the develop mode (since version v1.0.0, even if not standard)

Defines

NODE_NAME

LOGSQUARE(str)

OUTLABEL

TLOG(msg)

TWARN(msg)

TERR(msg)

WAITKEY_ENABLED

WAITKEY

DEVELOP_PRINT_ENABLED

WTLOG(msg)

WTWARN(msg)

WTERR(msg)

SERVICE_ROSPLAN_PIPELINE

SERVICE_ARMOR_ADD

SERVICE_ARMOR_FIND

SERVICE_ARMOR_DEL

SERVICE_ARMOR_BACKUP

SERVICE_ORACLE_SOLUTION

TOPIC_ORACLE_HINT

MAX_FAULT_COUNT

how many times the system can fail something

LANDMARK_REPLAN

landmar REPLAN identifier

LANDMARK_COLLECT

landmark COLLECT identifier

LANDMARK_SOLVE

landmark SOLVE identifier

MISSION_STATUS_REPLAN

landmark REPLAN — the system is restarting the exploration

MISSION_STATUS_COLLECT

landmark COLLECT — the system is sill looking for hypotheses

MISSION_STATUS_SOLVE

landmark SOLVE — the system has a solution to prsent

MISSION_STATUS_ASK_ORACLE

propose the solution to the Oracle ans check if the mystery has been solved

MISSION_STATUS_ASK_ONTOLOGY

look for a complete hypothesis inside the ontology

MISSION_STATUS_GET_HINT

receive hints from the Oracle (simple wait)

MISSION_STATUS_COUNT_FAULT

simple wait: the system could fail at most MAX_FAULT_COUNT times

MISSION_STATUS_FAULT

the mission has failed

Functions

void shut_msg(int sig)

int main(int argc, char *argv[])

class node_mission_manager

Public Functions

inline node_mission_manager()

node class constructor (open all the interfaces)

inline void spin()

node working cycle

This is where the state machine has been implemented. Currently the state machine implements 8 states.

MISSION_STATUS_REPLAN corresponds to the landmark REPLAN: the node calls that landmark via pipeline manager (so, planning and dispatch). Notice that this action is used also as a intermediate step for passing from ASK_ONTOLOGY to SOLVE.

MISSION_STATUS_COLLECT is the second landmark COLLECT: the mission manager makes a plan towards one waypoint (which one is not important) and dispatch a plan making the robot to collect the hint in that position.

MISSION_STATUS_ASK_ONTOLOGY is performed after the COLLECT. Here the mission manager asks to the aRMOR framework if there are consistent hypotheses to propose to the Oracle.

The common cycle is REPLAN -> COLLECT -> ASK_ONTOLOGY. If there are no hypotheses reasy to be proposed to the Oracle, the cycle restarts from COLLECT, and in case all the waypoints have been explored (in this case the kb_interface blocks the loading phase because the landmark COLLECT is no longer applicable) the cycle restarts from REPLAN, then COLLECT and again the same route.

MISSION_STATUS_SOLVE happens when the robot has at least one consistent hypothesis. In this case, the robot tries to move to the center of the environment for proposing its solution to the Oracle. In case the robot has finished the available paths (topologically speaking) the transition issues a intermediate MISSION_STATUS_REPLAN which cleans all the flags for the explored landmarks. The robot moves to the center.

the last status is MISSION_STATUS_ASK_ORACLE in which the mission manager interrogates the Oracle to understand if the solution is the winning one, or it is false. if false, the system restarts from MISSION_STATUS_REPLAN.

Something could go wrong for any reason. There are some well known cases, handled by the policy pointed out until now. But there are many other cases in which, for instance, making a service call doesn’t work, or maybe some expected errors from the planner. For these problematic cases, the state machine implements a recovery policy.

the state MISSION_STATUS_COUNT_FAULT decrements a “fault counter”, which is initialised according to the macro MAX_FAULT_COUNT . The next status is decided according to the mission status which has generated the error. If the counter becomes zero, the machine breaks in status MISSION_STATUS_FAULT, meaning that too many errors have occurred. This strategy is motivated by the fast that sometimes, during the tests, it has been observed that when the workload of the system becomes significant, some service call could fail unexpectedly.

Note

if there are more than one hypothesis, the robot will ask for only one of them, then, if the solution is wrong, the robot will always collect at least one other hint before the other solution proposal.

inline robocluedo_rosplan_msgs::RosplanPipelineManagerService::Response make_plan(int landmark)

make a plan ready to be executed (and call the service)

inline robocluedo_rosplan_msgs::RosplanPipelineManagerService::Response execute_plan(int landmark)

execute the plan

inline void cbk_oracle_hint(const erl2::ErlOracle::ConstPtr &hint)

receive a hint from the Oracle

inline bool is_valid_hint(const erl2::ErlOracle::ConstPtr &hint)

check wether the hint is valid or not

inline robocluedo_rosplan_msgs::RosplanPipelineManagerService create_planning_request(int landmark)

make a planning request without parsing and execution

inline robocluedo_rosplan_msgs::RosplanPipelineManagerService create_exec_request(int landmark = -1)

make a planning request without loading and solving

inline void set_standard_response(robocluedo_rosplan_msgs::RosplanPipelineManagerService &cmd, bool failure = false)

set a standard response for the service planning pipeline request

inline void set_request(robocluedo_rosplan_msgs::RosplanPipelineManagerService &cmd, bool load, bool solve, bool parse, bool exec, int landmark)

quick set request

Private Functions

template<class T>
inline bool open_client(std::string cl_name, ros::ServiceClient &cl_handle)

open a client

Private Members

ros::NodeHandle nh

the node handle

ros::ServiceClient cl_rosplan_pipeline

client rosplan pipeline manager

ros::ServiceClient cl_armor_add

client for armor add hint

ros::ServiceClient cl_armor_find

client for armor find consistent hypotheses

ros::ServiceClient cl_armor_del

client for armor discard hypotheses

ros::ServiceClient cl_armor_backup

client for armo backup request

ros::ServiceClient cl_oracle_solution

client for requiring the solution to the oracle

ros::Subscriber sub_oracle_hint

subscription oracle hints

int mission_status = MISSION_STATUS_REPLAN

state of the mission