Go to the documentation of this file.00001
00028 #include "sr_tactile_sensors/sr_virtual_tactile_sensor.hpp"
00029 #include <boost/algorithm/string.hpp>
00030 #include <string>
00031
00032 namespace shadowrobot
00033 {
00034
00035
00036
00037 SrVirtualTactileSensor::SrVirtualTactileSensor(std::string name,
00038 std::string touch_name ) :
00039 SrGenericTactileSensor(name, touch_name),
00040 touch_value(0.0)
00041 {
00042 if(name.find("th")!=std::string::npos )
00043 {
00044
00045 std::string tmp = boost::to_upper_copy(name);
00046 tmp += "J2";
00047 names_joints_linked.push_back(tmp);
00048 tmp = boost::to_upper_copy(name);
00049 tmp += "J1";
00050 names_joints_linked.push_back(tmp);
00051 }
00052 else
00053 {
00054
00055 std::string tmp = boost::to_upper_copy(name);
00056 tmp += "J3";
00057 names_joints_linked.push_back(tmp);
00058 tmp = boost::to_upper_copy(name);
00059 tmp += "J0";
00060 names_joints_linked.push_back(tmp);
00061 }
00062
00063 sub = nh.subscribe("/srh/shadowhand_data", 2, &SrVirtualTactileSensor::callback, this);
00064 }
00065
00066 SrVirtualTactileSensor::~SrVirtualTactileSensor()
00067 {}
00068
00069 void SrVirtualTactileSensor::callback(const sr_robot_msgs::joints_dataConstPtr& msg)
00070 {
00071 double tmp_value = 0.0;
00072 int msg_length = msg->joints_list_length;
00073 for(unsigned short index_msg=0; index_msg < msg_length; ++index_msg)
00074 {
00075
00076 std::string sensor_name = msg->joints_list[index_msg].joint_name;
00077
00078 for(unsigned short index_name=0; index_name < names_joints_linked.size() ; ++index_name )
00079 {
00080 if(sensor_name.compare(names_joints_linked[index_name]) == 0)
00081 {
00082
00083 tmp_value += msg->joints_list[index_msg].joint_position;
00084 break;
00085 }
00086 }
00087 }
00088
00089 touch_mutex.lock();
00090 touch_value = tmp_value;
00091 touch_mutex.unlock();
00092 }
00093
00094 double SrVirtualTactileSensor::get_touch_data()
00095 {
00096 double return_value;
00097 touch_mutex.lock();
00098 return_value = touch_value;
00099 touch_mutex.unlock();
00100
00101 return return_value;
00102 }
00103
00104
00105
00106
00107 SrVirtualTactileSensorManager::SrVirtualTactileSensorManager() :
00108 SrTactileSensorManager()
00109 {
00110 std::vector<std::vector<std::string> > all_names = get_all_names();
00111
00112 for( unsigned int i=0; i< all_names[0].size() ; ++i)
00113 {
00114 tactile_sensors.push_back(
00115 boost::shared_ptr<SrVirtualTactileSensor>(
00116 new SrVirtualTactileSensor(all_names[0][i],
00117 all_names[1][i]) ));
00118 }
00119 }
00120
00121 SrVirtualTactileSensorManager::~SrVirtualTactileSensorManager()
00122 {}
00123 }
00124
00133 int main(int argc, char** argv)
00134 {
00135 ros::init(argc, argv, "sr_tactile_sensor");
00136 ros::NodeHandle n;
00137
00138 shadowrobot::SrVirtualTactileSensorManager tact_sens_mgr;
00139
00140 while( ros::ok() )
00141 tact_sens_mgr.publish_all();
00142
00143 return 0;
00144 }
00145
00146
00147
00148
00149
00150