00001
00027 #include "sr_tactile_sensors/sr_gazebo_virtual_tactile_sensor.hpp"
00028 #include <boost/algorithm/string.hpp>
00029 #include <string>
00030 #include <math.h>
00031 #include <ros/ros.h>
00032
00033 namespace shadowrobot
00034 {
00035
00036
00037
00038 SrGazeboVirtualTactileSensor::SrGazeboVirtualTactileSensor(std::string name,
00039 std::string gazebo_bumper_topic) :
00040 SrGenericTactileSensor(name, ""),
00041 touch_value(0.0)
00042 {
00043 sub = nh.subscribe(gazebo_bumper_topic, 2, &SrGazeboVirtualTactileSensor::callback, this);
00044 }
00045
00046 SrGazeboVirtualTactileSensor::~SrGazeboVirtualTactileSensor()
00047 {}
00048
00049 void SrGazeboVirtualTactileSensor::callback(const gazebo_plugins::ContactsState& msg)
00050 {
00051 double tmp_value;
00052 const ::geometry_msgs::Vector3& v = msg.states[0].wrenches[0].force;
00053
00054 ROS_INFO("Touch by %s", msg.header.frame_id.c_str());
00055
00056
00057
00058 tmp_value = sqrt(pow(v.x, 2) + pow(v.y, 2) + pow(v.z, 2));
00059
00060 touch_mutex.lock();
00061 touch_value = tmp_value;
00062 touch_mutex.unlock();
00063 }
00064
00065 double SrGazeboVirtualTactileSensor::get_touch_data()
00066 {
00067 double return_value;
00068
00069 touch_mutex.lock();
00070 return_value = touch_value;
00071 touch_mutex.unlock();
00072
00073 return return_value;
00074 }
00075
00076
00077
00078
00079
00080 SrGazeboVirtualTactileSensorManager::SrGazeboVirtualTactileSensorManager() :
00081 SrTactileSensorManager()
00082 {
00083 std::vector<std::vector<std::string> > all_names = get_all_names();
00084
00085 for( unsigned int i=0; i<5; ++i)
00086 {
00087 tactile_sensors.push_back(
00088 boost::shared_ptr<SrGazeboVirtualTactileSensor>(
00089 new SrGazeboVirtualTactileSensor(all_names[0][i],
00090 "/"+all_names[0][i]+"distal_bumper/state")
00091 )
00092 );
00093 }
00094 }
00095
00096 SrGazeboVirtualTactileSensorManager::~SrGazeboVirtualTactileSensorManager()
00097 {}
00098 }
00099
00100
00109 int main(int argc, char** argv)
00110 {
00111 ros::init(argc, argv, "sr_tactile_sensor");
00112 ros::NodeHandle n;
00113
00114 shadowrobot::SrGazeboVirtualTactileSensorManager tact_sens_mgr;
00115
00116 while( ros::ok() )
00117 tact_sens_mgr.publish_all();
00118
00119 return 0;
00120 }
00121
00122
00123
00124
00125
00126