00001 #ifndef SCAVTASKHUMANFOLLOWING_H 00002 #define SCAVTASKHUMANFOLLOWING_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 #include <move_base_msgs/MoveBaseAction.h> 00014 #include <move_base_msgs/MoveBaseActionResult.h> 00015 #include <actionlib/client/simple_action_client.h> 00016 #include <sound_play/SoundRequest.h> 00017 00018 typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient; 00019 00020 namespace scav_task_human_following { 00021 00022 class ScavTaskHumanFollowing : public ScavTask { 00023 public: 00024 00025 ScavTaskHumanFollowing() : ac("") {} 00026 ScavTaskHumanFollowing(ros::NodeHandle *node_handle, std::string path_of_dir); 00027 00028 void executeTask(int timeout, TaskResult &result, std::string &record); 00029 void visionThread(); 00030 void motionThread(); 00031 void stopEarly(); 00032 00033 void callback_human_detected(const geometry_msgs::PoseStamped::ConstPtr& msg); 00034 void callback_image(const sensor_msgs::ImageConstPtr& msg); 00035 void callback_ac_followed(const actionlib::SimpleClientGoalState& state, 00036 const move_base_msgs::MoveBaseResultConstPtr& result); 00037 void callback_ac_reached(const actionlib::SimpleClientGoalState& state, 00038 const move_base_msgs::MoveBaseResultConstPtr& result); 00039 void callback_ac_done(const actionlib::SimpleClientGoalState& state, 00040 const move_base_msgs::MoveBaseResultConstPtr& result); 00041 00042 void amclPoseCallback( 00043 const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg); 00044 00045 00046 void moveToPose(const geometry_msgs::Pose); 00047 00048 SearchPlannerSimple *search_planner_simple; 00049 std::string directory; 00050 00051 bool task_completed; 00052 00053 private: 00054 static const double human_following_pose_offset = 1; 00055 static const double human_pose_delta_threshold = 1.0; 00056 00057 geometry_msgs::PoseStamped human_pose; 00058 geometry_msgs::PoseStamped human_following_pose; 00059 geometry_msgs::Pose current_pose; 00060 int human_detected; 00061 ros::Time detected_time; 00062 ros::Publisher cmd_vel_pub; 00063 ros::Publisher sound_pub; 00064 MoveBaseClient ac; 00065 00066 }; 00067 } 00068 #endif