Go to the documentation of this file.00001 #include "pr2_overhead_grasping/force_torque_monitor.h"
00002 #include <pluginlib/class_list_macros.h>
00003 PLUGINLIB_DECLARE_CLASS(collision_detection, force_torque_monitor, collision_detection::ForceTorqueMonitor, nodelet::Nodelet)
00004
00005 namespace collision_detection {
00006
00007 void ForceTorqueMonitor::onInit() {
00008 ros::NodeHandle nh = getMTNodeHandle();
00009 ros::NodeHandle nh_priv = getMTPrivateNodeHandle();
00010
00011 std::string arm_str;
00012 nh_priv.getParam("arm", arm_str);
00013 char arm = arm_str[0];
00014 nh_priv.getParam("z_thresh", z_thresh);
00015 nh_priv.getParam("delay_time", delay_time);
00016
00017
00018 collision_detected = true;
00019 collision_time = ros::Time::now().toSec() - 1000.0;
00020
00021 ft_sub = nh.subscribe("force_torque_ft1_Vec3", 1,
00022 &ForceTorqueMonitor::checkCollision, this);
00023 detect_pub = nh.advertise<std_msgs::Bool>("force_torque_collision", 1);
00024 NODELET_INFO("[force_torque_monitor] Publishing on force_torque_collision");
00025 start_srv = nh.advertiseService("ft_start_detection", &ForceTorqueMonitor::srvStartDetection, this);
00026 NODELET_INFO("[force_torque_monitor] Service advertised at ft_start_detection");
00027 stop_srv = nh.advertiseService("ft_stop_detection", &ForceTorqueMonitor::srvStopDetection, this);
00028 NODELET_INFO("[force_torque_monitor] Service advertised at ft_stop_detection");
00029 coll_pub = nh.advertise<pr2_overhead_grasping::SensorPoint>("collision_data", 20);
00030 NODELET_INFO("[force_torque_monitor] Publishing on collision_data");
00031
00032 if(arm == 'r') {
00033 sf_sub = nh.subscribe("r_arm_features", 1,
00034 &ForceTorqueMonitor::saveCollision, this);
00035 } else {
00036 sf_sub = nh.subscribe("l_arm_features", 1,
00037 &ForceTorqueMonitor::saveCollision, this);
00038 }
00039 traj_ind = 0;
00040 }
00041
00042 void ForceTorqueMonitor::checkCollision(geometry_msgs::Vector3Stamped::ConstPtr force) {
00043 if(!is_collision)
00044 return;
00045 if(collision_detected)
00046
00047 return;
00048 if(force->vector.z >= z_thresh) {
00049 collision_time = ros::Time::now().toSec();
00050 collision_detected = true;
00051 ros::NodeHandle nh = getMTNodeHandle();
00052 delay_timer = nh.createTimer(ros::Duration(delay_time), &ForceTorqueMonitor::endCollision, this);
00053 }
00054 }
00055
00056 void ForceTorqueMonitor::endCollision(const ros::TimerEvent& event) {
00057 std_msgs::Bool bool_true;
00058 bool_true.data = true;
00059 detect_pub.publish(bool_true);
00060 delay_timer.stop();
00061 }
00062
00063 void ForceTorqueMonitor::saveCollision(pr2_overhead_grasping::SensorPoint::ConstPtr sp) {
00064 double cur_delay = ros::Time::now().toSec() - collision_time;
00065 if(is_collision) {
00066 if(!collision_detected || cur_delay > delay_time)
00067
00068 return;
00069 } else {
00070 if(collision_detected)
00071 return;
00072 }
00073
00074 pr2_overhead_grasping::SensorPoint new_sp(*sp);
00075 new_sp.detect_delay = cur_delay;
00076 new_sp.label = label;
00077 new_sp.traj_id = label * 1000 + traj_ind;
00078 coll_pub.publish(new_sp);
00079 }
00080
00081 void ForceTorqueMonitor::startDetection() {
00082 collision_detected = false;
00083 }
00084
00085 void ForceTorqueMonitor::stopDetection() {
00086 collision_time = ros::Time::now().toSec() - 1000.0;
00087 collision_detected = true;
00088 traj_ind++;
00089 }
00090
00091 bool ForceTorqueMonitor::srvStartDetection(pr2_overhead_grasping::StartFTDetect::Request& req, pr2_overhead_grasping::StartFTDetect::Response& resp) {
00092 label = req.label;
00093 is_collision = req.is_collision;
00094 startDetection();
00095 return true;
00096 }
00097
00098 bool ForceTorqueMonitor::srvStopDetection(std_srvs::Empty::Request&, std_srvs::Empty::Response&) {
00099 stopDetection();
00100 return true;
00101 }
00102
00103 }