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 _tibi_dabo_guide_nav_alg_node_h_
00026 #define _tibi_dabo_guide_nav_alg_node_h_
00027
00028 #include <iri_base_algorithm/iri_base_algorithm.h>
00029 #include "tibi_dabo_guide_nav_alg.h"
00030
00031
00032 #include <std_msgs/Int32.h>
00033 #include <iri_perception_msgs/peopleTrackingArray.h>
00034
00035
00036 #include <iri_diff_local_planner/change_max_vel.h>
00037 #include <iri_goal_database/get_goal.h>
00038 #include <dynamic_reconfigure/Reconfigure.h>
00039
00040
00041 #include <tibi_dabo_msgs/sequenceAction.h>
00042 #include <tibi_dabo_msgs/guideGoalAction.h>
00043 #include <iri_action_server/iri_action_server.h>
00044 #include <actionlib/client/simple_action_client.h>
00045 #include <actionlib/client/terminal_state.h>
00046 #include <move_base_msgs/MoveBaseAction.h>
00047
00048 #include <tf/transform_listener.h>
00049
00050 typedef enum {guide_idle,guide_wait_goal,guide_slow_down,guide_cancel_goal,guide_turn_arround,guide_wait_person_hri,guide_wait_person,guide_person_lost_hri,
00051 guide_person_lost,guide_ignored,guide_face_person} guiding_states;
00052
00057 class TibiDaboGuideNavAlgNode : public algorithm_base::IriBaseAlgorithm<TibiDaboGuideNavAlgorithm>
00058 {
00059 private:
00060
00061
00062
00063
00064 ros::Subscriber peopleSet_subscriber_;
00065 void peopleSet_callback(const iri_perception_msgs::peopleTrackingArray::ConstPtr& msg);
00066 CMutex peopleSet_mutex_;
00067
00068
00069
00070
00071 ros::ServiceClient change_speed_client_;
00072 iri_diff_local_planner::change_max_vel change_speed_srv_;
00073 ros::ServiceClient get_goal_client_;
00074 iri_goal_database::get_goal get_goal_srv_;
00075
00076
00077 IriActionServer<tibi_dabo_msgs::guideGoalAction> move_guide_aserver_;
00078 void move_guideStartCallback(const tibi_dabo_msgs::guideGoalGoalConstPtr& goal);
00079 void move_guideStopCallback(void);
00080 bool move_guideIsFinishedCallback(void);
00081 bool move_guideHasSucceedCallback(void);
00082 void move_guideGetResultCallback(tibi_dabo_msgs::guideGoalResultPtr& result);
00083 void move_guideGetFeedbackCallback(tibi_dabo_msgs::guideGoalFeedbackPtr& feedback);
00084 int target_id;
00085
00086
00087 actionlib::SimpleActionClient<tibi_dabo_msgs::sequenceAction> hri_client_client_;
00088 tibi_dabo_msgs::sequenceGoal hri_client_goal_;
00089 void hri_clientMakeActionRequest();
00090 void hri_clientDone(const actionlib::SimpleClientGoalState& state, const tibi_dabo_msgs::sequenceResultConstPtr& result);
00091 void hri_clientActive();
00092 void hri_clientFeedback(const tibi_dabo_msgs::sequenceFeedbackConstPtr& feedback);
00093 bool hri_client_done;
00094
00095 actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> move_base_client_;
00096 move_base_msgs::MoveBaseGoal move_base_goal_;
00097 void move_baseMakeActionRequest();
00098 void move_baseDone(const actionlib::SimpleClientGoalState& state, const move_base_msgs::MoveBaseResultConstPtr& result);
00099 void move_baseActive();
00100 void move_baseFeedback(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback);
00101 bool move_base_done;
00102
00103 bool new_guide_request;
00104 std::string location_id;
00105 std::string robot_id;
00106
00107 double distanceThresholdFar;
00108 double distanceThresholdNear;
00109 guiding_states current_state;
00110 std::string robot_frame;
00111
00112 geometry_msgs::PoseStamped person_pose;
00113 double person_distance;
00114 bool person_lost;
00115 bool success;
00116
00117 tf::TransformListener tf_listener_;
00118 public:
00125 TibiDaboGuideNavAlgNode(void);
00126
00133 ~TibiDaboGuideNavAlgNode(void);
00134
00135 protected:
00148 void mainNodeThread(void);
00149
00162 void node_config_update(Config &config, uint32_t level);
00163
00170 void addNodeDiagnostics(void);
00171
00172
00173
00174
00175 };
00176
00177 #endif