00001 #ifndef SCAVTASKWHITEBOARD_H 00002 #define SCAVTASKWHITEBOARD_H 00003 00004 #include <string> 00005 #include <ros/ros.h> 00006 #include <ros/package.h> 00007 #include <boost/thread.hpp> 00008 #include <boost/bind.hpp> 00009 00010 #include "ScavTask.h" 00011 #include "SearchPlanner.h" 00012 00013 struct Pose { 00014 float x; float y; 00015 Pose() {} 00016 Pose(float xx, float yy) : x(xx), y(yy) {} 00017 }; 00018 00019 class ScavTaskWhiteBoard : public ScavTask { 00020 public: 00021 00022 ScavTaskWhiteBoard() {} 00023 ScavTaskWhiteBoard(ros::NodeHandle *node_handle, std::string path_of_dir); 00024 00025 void executeTask(int timeout, TaskResult &result, std::string &record); 00026 void visionThread(); 00027 void motionThread(); 00028 void stopEarly(); 00029 00030 void callback_human_detected(const geometry_msgs::PoseStamped::ConstPtr& msg); 00031 void callback_image(const sensor_msgs::ImageConstPtr& msg); 00032 00033 SearchPlanner *search_planner; 00034 std::string directory; 00035 00036 bool task_completed; 00037 00038 bool inRectangle(Pose p, Pose top_left, Pose top_right, Pose bottom_left); 00039 }; 00040 00041 #endif