force_torque_monitor.cpp
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     //nh_priv.getParam("label", label);
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       // we stop running this callback until the monitor is restarted
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         // we stop running this callback until the monitor is restarted
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 }


kelsey_sandbox
Author(s): kelsey
autogenerated on Wed Nov 27 2013 11:52:03