00001 #include <ias_drawer_executive/Accelerometer.h> 00002 00003 00004 Accelerometer::Accelerometer(int side) 00005 { 00006 side_=side; 00007 acc_client_=new DetectContactAC(side ? "l_gripper_sensor_controller/event_detector" : "r_gripper_sensor_controller/event_detector",true); 00008 ROS_INFO("waiting for accelerometer AC server to come up for side %d",side); 00009 acc_client_->waitForServer(ros::Duration(5.0)); 00010 } 00011 00012 00013 Accelerometer::~Accelerometer() 00014 { 00015 delete acc_client_; 00016 } 00017 00018 00019 Accelerometer *Accelerometer::instance_[] = {0,0}; 00020 00021 void 00022 Accelerometer::accelCallback(const pr2_msgs::AccelerometerState::ConstPtr& msg) 00023 { 00024 00025 } 00026 00027 Accelerometer *Accelerometer::getInstance(int side) 00028 { 00029 if (!instance_[side]) instance_[side] = new Accelerometer(side); 00030 return instance_[side]; 00031 } 00032 00033 00034 void Accelerometer::detectContact(double magnitude) 00035 { 00036 pr2_gripper_sensor_msgs::PR2GripperEventDetectorGoal detect_goal; 00037 detect_goal.command.trigger_conditions=pr2_gripper_sensor_msgs::PR2GripperEventDetectorCommand::FINGER_SIDE_IMPACT_OR_SLIP_OR_ACC;//detect_goal.command.trigger_conditions.ACC; 00038 detect_goal.command.acceleration_trigger_magnitude=magnitude; 00039 00040 ROS_INFO("sending detect_acc goal"); 00041 acc_client_->sendGoal(detect_goal); 00042 acc_client_->waitForResult(ros::Duration(100.0)); 00043 if (acc_client_->getState()==actionlib::SimpleClientGoalState::SUCCEEDED) 00044 ROS_INFO("detect_acc succeeded"); 00045 else 00046 ROS_INFO("detect_acc failed"); 00047 }