00001 /* 00002 * Base class for localizing jockeys 00003 * 00004 */ 00005 00006 #ifndef _LAMA_JOCKEYS_LOCALIZING_JOCKEY_H_ 00007 #define _LAMA_JOCKEYS_LOCALIZING_JOCKEY_H_ 00008 00009 #include <string> 00010 00011 #include <ros/ros.h> 00012 #include <actionlib/server/simple_action_server.h> 00013 00014 #include <lama_jockeys/jockey.h> 00015 #include <lama_jockeys/LocalizeAction.h> 00016 #include <lama_jockeys/LocalizeGoal.h> 00017 #include <lama_jockeys/LocalizeFeedback.h> 00018 00019 namespace lama_jockeys { 00020 00021 typedef actionlib::SimpleActionServer<lama_jockeys::LocalizeAction> LocalizeServer; 00022 00023 class LocalizingJockey : public Jockey 00024 { 00025 protected: 00026 00027 LocalizingJockey(const std::string& name); 00028 00029 virtual void onGetVertexDescriptor(); 00030 virtual void onGetEdgesDescriptors(); 00031 virtual void onLocalizeInVertex(); 00032 virtual void onLocalizeEdge(); 00033 virtual void onGetDissimilarity(); 00034 virtual void onInterrupt(); 00035 virtual void onContinue(); 00036 00037 void initAction(); 00038 00039 // NodeHandle instance must be created before this line. Otherwise strange 00040 // error may occur (this is done in Jockey). 00041 LocalizeServer server_; 00042 LocalizeResult result_; 00043 LocalizeFeedback feedback_; 00044 00045 // In case of INTERRUPT and CONTINUE, the descritptor attribute 00046 // of current goal are irrelevant. 00047 // This information needs to be saved for use after a CONTINUE action. 00048 LocalizeGoal goal_; 00049 00050 private: 00051 00052 void goalCallback(const lama_jockeys::LocalizeGoalConstPtr& goal); 00053 void preemptCallback(); 00054 00055 // Change the visibility to avoid double calls. 00056 using Jockey::interrupt; 00057 using Jockey::resume; 00058 }; 00059 00060 } // namespace lama_jockeys 00061 00062 #endif // _LAMA_JOCKEYS_LOCALIZING_JOCKEY_H_