00001 /* vim: set sw=4 sts=4 et foldmethod=syntax : */ 00002 00003 #ifndef SRC_GUARD_CHECKPOINT_MANAGER_H 00004 #define SRC_GUARD_CHECKPOINT_MANAGER_H 1 00005 00006 #include <ros/ros.h> 00007 #include "checkpoint_marker.h" 00008 #include <iri_checkpoint_nav/Checkpoint.h> 00009 #include <iri_checkpoint_nav/ActionCheckpointNav.h> 00010 #include <tf/transform_broadcaster.h> 00011 00012 #include <vector> 00013 00014 00015 class CheckpointManager 00016 { 00017 private: 00018 ros::ServiceServer srv_checkpoint_; 00019 ros::ServiceServer srv_action_; 00020 ros::NodeHandle nh_; 00021 00022 CheckpointMarker marker_; 00023 std::vector<CheckpointRequest> checkpoints_; 00024 std::vector<CheckpointRequest>::iterator current_goal_; 00025 00026 bool add_checkpoint_callback(iri_checkpoint_nav::Checkpoint::Request & req, 00027 iri_checkpoint_nav::Checkpoint::Response & res); 00028 bool action_callback(iri_checkpoint_nav::ActionCheckpointNav::Request & req, 00029 iri_checkpoint_nav::ActionCheckpointNav::Response & res); 00030 00031 /* 00032 * Orientation is always the direction from current goal to the next 00033 * goal. The only exception is in the last goal, when direction is get 00034 * from previous goal to current 00035 */ 00036 geometry_msgs::Quaternion generate_goal_orientation(); 00037 static geometry_msgs::Quaternion generate_default_orientation(); 00038 00039 void activate_current_checkpoint(); 00040 void visited_current_checkpoint(); 00041 void reset_checkpoints(); 00042 bool send_navigation_goal(const CheckpointRequest checkpoint); 00043 bool start_navigation(); 00044 00045 public: 00046 CheckpointManager(); 00047 }; 00048 00049 #endif