Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #ifndef _dabo_coop_alg_node_h_
00026 #define _dabo_coop_alg_node_h_
00027
00028 #include <iri_base_algorithm/iri_base_algorithm.h>
00029 #include "dabo_coop_alg.h"
00030
00031
00032 #include <sensor_msgs/Joy.h>
00033
00034
00035 #include <iri_goal_database/get_goal.h>
00036
00037
00038 #include <iri_action_server/iri_action_server.h>
00039 #include <tibi_dabo_msgs/guiAction.h>
00040 #include <move_base_msgs/MoveBaseAction.h>
00041 #include <actionlib/client/simple_action_client.h>
00042 #include <actionlib/client/terminal_state.h>
00043 #include <tibi_dabo_msgs/sequenceAction.h>
00044
00045 typedef enum {dabo_idle,dabo_wait_car_goal,dabo_wait_car_done,dabo_wait_speech,dabo_wait_user_input,dabo_wait_reminder,dabo_wait_home,
00046 dabo_wait_goodbye,dabo_set_goal_tibi,dabo_wait_car_tibi_home} dabo_states;
00047
00048 typedef enum {dabo_none=-1,dabo_set_goal=0,dabo_car_done} dabo_cmds;
00049
00050 typedef enum {gui_none=-1,gui_done=0} gui_cmds;
00051
00052 typedef enum {gui_goal_none=-1,gui_set_goal=0, } gui_goal_cmds;
00053
00054 typedef enum {joy_none=-1,joy_cancel_goal=0,joy_replan_goal,joy_teleop} joy_cmds;
00055
00060 class DaboCoopAlgNode : public algorithm_base::IriBaseAlgorithm<DaboCoopAlgorithm>
00061 {
00062 private:
00063
00064
00065
00066 ros::Subscriber supervisor_subscriber_;
00067 void supervisor_callback(const sensor_msgs::Joy::ConstPtr& msg);
00068 CMutex supervisor_mutex_;
00069 bool new_joy_cmd;
00070 joy_cmds joy_cmd;
00071
00072
00073
00074
00075 ros::ServiceClient get_goal_client_;
00076 iri_goal_database::get_goal get_goal_srv_;
00077
00078
00079 IriActionServer<tibi_dabo_msgs::guiAction> car_dabo_action_aserver_;
00080 void car_dabo_actionStartCallback(const tibi_dabo_msgs::guiGoalConstPtr& goal);
00081 void car_dabo_actionStopCallback(void);
00082 bool car_dabo_actionIsFinishedCallback(void);
00083 bool car_dabo_actionHasSucceedCallback(void);
00084 void car_dabo_actionGetResultCallback(tibi_dabo_msgs::guiResultPtr& result);
00085 void car_dabo_actionGetFeedbackCallback(tibi_dabo_msgs::guiFeedbackPtr& feedback);
00086 bool new_car_cmd;
00087 dabo_cmds car_cmd;
00088 std::string car_location_id;
00089 bool car_done;
00090
00091 IriActionServer<tibi_dabo_msgs::guiAction> gui_action_aserver_;
00092 void gui_actionStartCallback(const tibi_dabo_msgs::guiGoalConstPtr& goal);
00093 void gui_actionStopCallback(void);
00094 bool gui_actionIsFinishedCallback(void);
00095 bool gui_actionHasSucceedCallback(void);
00096 void gui_actionGetResultCallback(tibi_dabo_msgs::guiResultPtr& result);
00097 void gui_actionGetFeedbackCallback(tibi_dabo_msgs::guiFeedbackPtr& feedback);
00098 bool new_gui_cmd;
00099 gui_cmds gui_cmd;
00100
00101
00102 actionlib::SimpleActionClient<tibi_dabo_msgs::guiAction> gui_action_client_client_;
00103 tibi_dabo_msgs::guiGoal gui_action_client_goal_;
00104 void gui_action_clientMakeActionRequest();
00105 void gui_action_clientDone(const actionlib::SimpleClientGoalState& state, const tibi_dabo_msgs::guiResultConstPtr& result);
00106 void gui_action_clientActive();
00107 void gui_action_clientFeedback(const tibi_dabo_msgs::guiFeedbackConstPtr& feedback);
00108
00109 actionlib::SimpleActionClient<tibi_dabo_msgs::guiAction> dabo_tibi_action_client_;
00110 tibi_dabo_msgs::guiGoal dabo_tibi_action_goal_;
00111 bool dabo_tibi_actionMakeActionRequest();
00112 void dabo_tibi_actionDone(const actionlib::SimpleClientGoalState& state, const tibi_dabo_msgs::guiResultConstPtr& result);
00113 void dabo_tibi_actionActive();
00114 void dabo_tibi_actionFeedback(const tibi_dabo_msgs::guiFeedbackConstPtr& feedback);
00115 bool dabo_tibi_action_done;
00116
00117 actionlib::SimpleActionClient<tibi_dabo_msgs::guiAction> dabo_car_action_client_;
00118 tibi_dabo_msgs::guiGoal dabo_car_action_goal_;
00119 bool dabo_car_actionMakeActionRequest();
00120 void dabo_car_actionDone(const actionlib::SimpleClientGoalState& state, const tibi_dabo_msgs::guiResultConstPtr& result);
00121 void dabo_car_actionActive();
00122 void dabo_car_actionFeedback(const tibi_dabo_msgs::guiFeedbackConstPtr& feedback);
00123 bool dabo_car_action_done;
00124
00125 actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> move_base_client_;
00126 move_base_msgs::MoveBaseGoal move_base_goal_;
00127 bool move_baseMakeActionRequest();
00128 void move_baseDone(const actionlib::SimpleClientGoalState& state, const move_base_msgs::MoveBaseResultConstPtr& result);
00129 void move_baseActive();
00130 void move_baseFeedback(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback);
00131 bool move_base_done;
00132
00133 actionlib::SimpleActionClient<tibi_dabo_msgs::sequenceAction> hri_client_client_;
00134 tibi_dabo_msgs::sequenceGoal hri_client_goal_;
00135 bool hri_clientMakeActionRequest();
00136 void hri_clientDone(const actionlib::SimpleClientGoalState& state, const tibi_dabo_msgs::sequenceResultConstPtr& result);
00137 void hri_clientActive();
00138 void hri_clientFeedback(const tibi_dabo_msgs::sequenceFeedbackConstPtr& feedback);
00139 bool hri_client_done;
00140
00141 dabo_states current_state;
00142 public:
00149 DaboCoopAlgNode(void);
00150
00157 ~DaboCoopAlgNode(void);
00158
00159 protected:
00172 void mainNodeThread(void);
00173
00186 void node_config_update(Config &config, uint32_t level);
00187
00194 void addNodeDiagnostics(void);
00195
00196
00197
00198
00199 };
00200
00201 #endif