00001 #ifndef PR2_OVERHEAD_GRASPING_FORCE_TORQUE_MONITOR_H 00002 #define PR2_OVERHEAD_GRASPING_FORCE_TORQUE_MONITOR_H 00003 #include <ros/ros.h> 00004 #include "geometry_msgs/Vector3Stamped.h" 00005 #include "std_msgs/Bool.h" 00006 #include "pr2_overhead_grasping/SensorPoint.h" 00007 #include "pr2_overhead_grasping/StartFTDetect.h" 00008 #include <std_srvs/Empty.h> 00009 #include <nodelet/nodelet.h> 00010 00011 namespace collision_detection { 00012 00013 class ForceTorqueMonitor : public nodelet::Nodelet { 00014 public: 00015 virtual void onInit(); 00016 void startDetection(); 00017 void stopDetection(); 00018 00019 protected: 00020 void checkCollision(geometry_msgs::Vector3Stamped::ConstPtr message); 00021 void saveCollision(pr2_overhead_grasping::SensorPoint::ConstPtr message); 00022 double z_thresh; 00023 double collision_time; 00024 double delay_time; 00025 int label; 00026 bool collision_detected; 00027 bool is_collision; 00028 int traj_ind; 00029 ros::Timer delay_timer; 00030 00031 bool srvStartDetection(pr2_overhead_grasping::StartFTDetect::Request&, pr2_overhead_grasping::StartFTDetect::Response&); 00032 bool srvStopDetection(std_srvs::Empty::Request&, std_srvs::Empty::Response&); 00033 void endCollision(const ros::TimerEvent& event); 00034 ros::Publisher coll_pub; 00035 ros::Publisher detect_pub; 00036 ros::Subscriber ft_sub; 00037 ros::Subscriber sf_sub; 00038 ros::ServiceServer start_srv; 00039 ros::ServiceServer stop_srv; 00040 }; 00041 } 00042 00043 00044 #endif // PR2_OVERHEAD_GRASPING_FORCE_TORQUE_MONITOR_H