00001 00028 #include "sr_tactile_sensors/sr_real_tactile_sensor.hpp" 00029 00030 namespace shadowrobot 00031 { 00032 /********************************** 00033 * TACTILE SENSOR * 00034 **********************************/ 00035 SrRealTactileSensor::SrRealTactileSensor(std::string name, 00036 std::string touch_name ) : 00037 SrGenericTactileSensor(name, touch_name) 00038 { 00039 /* We need to attach the program to the robot, or fail if we cannot. */ 00040 if (robot_init() < 0) 00041 { 00042 ROS_FATAL("Robot interface broken\n"); 00043 ROS_BREAK(); 00044 } 00045 00046 res_touch = robot_name_to_sensor(touch_name.c_str(), &sensor_touch); 00047 00048 if(res_touch) 00049 { 00050 ROS_ERROR("Can't open sensor %s", touch_name.c_str()); 00051 } 00052 } 00053 00054 SrRealTactileSensor::~SrRealTactileSensor() 00055 {} 00056 00057 double SrRealTactileSensor::get_touch_data() 00058 { 00059 if(res_touch) 00060 return -1000.0; 00061 00062 return robot_read_sensor(&sensor_touch); 00063 } 00064 00065 00066 /********************************** 00067 * TACTILE SENSOR MANAGER * 00068 **********************************/ 00069 SrRealTactileSensorManager::SrRealTactileSensorManager() : 00070 SrTactileSensorManager() 00071 { 00072 std::vector<std::vector<std::string> > all_names = get_all_names(); 00073 00074 for( unsigned int i=0; i < all_names[0].size() ; ++i) 00075 { 00076 tactile_sensors.push_back( 00077 boost::shared_ptr<SrRealTactileSensor>( 00078 new SrRealTactileSensor(all_names[0][i], 00079 all_names[1][i]) )); 00080 } 00081 } 00082 00083 SrRealTactileSensorManager::~SrRealTactileSensorManager() 00084 {} 00085 } 00086 00087 00098 int main(int argc, char** argv) 00099 { 00100 ros::init(argc, argv, "sr_tactile_sensor"); 00101 ros::NodeHandle n; 00102 00103 shadowrobot::SrRealTactileSensorManager tact_sens_mgr; 00104 00105 while( ros::ok() ) 00106 tact_sens_mgr.publish_all(); 00107 00108 return 0; 00109 } 00110 00111 /* For the emacs weenies in the crowd. 00112 Local Variables: 00113 c-basic-offset: 2 00114 End: 00115 */