00001
00028 #include "sr_tactile_sensors/sr_generic_tactile_sensor.hpp"
00029
00030 namespace shadowrobot
00031 {
00032
00033
00034
00035 SrGenericTactileSensor::SrGenericTactileSensor(std::string name,
00036 std::string touch_name ) :
00037 n_tilde("~")
00038 {
00039 std::string full_topic_touch = "touch/"+name;
00040
00041 touch_pub = n_tilde.advertise<std_msgs::Float64>(full_topic_touch, 10);
00042 }
00043
00044 SrGenericTactileSensor::~SrGenericTactileSensor()
00045 {}
00046
00047 void SrGenericTactileSensor::publish_current_values()
00048 {
00049 msg_touch.data = get_touch_data();
00050
00051 touch_pub.publish(msg_touch);
00052 }
00053
00054
00055
00056
00057 SrTactileSensorManager::SrTactileSensorManager() :
00058 n_tilde("~"), publish_rate(20.0)
00059 {
00060 double publish_freq;
00061 n_tilde.param("publish_frequency", publish_freq, 20.0);
00062 publish_rate = ros::Rate(publish_freq);
00063 }
00064
00065 SrTactileSensorManager::~SrTactileSensorManager()
00066 {}
00067
00068 std::vector<std::vector<std::string> > SrTactileSensorManager::get_all_names()
00069 {
00070 std::vector<std::vector<std::string> > all_names_vector;
00071 std::vector<std::string> names, sensor_touch_names;
00072
00073 std::string tmp;
00074 XmlRpc::XmlRpcValue my_list;
00075 int list_size;
00076 bool bad_params = false;
00077
00078 n_tilde.getParam("display_names", my_list);
00079 if(my_list.getType() != XmlRpc::XmlRpcValue::TypeArray)
00080 bad_params = true;
00081 list_size = my_list.size();
00082 for (int32_t i = 0; i < list_size; ++i)
00083 {
00084 if(my_list[i].getType() != XmlRpc::XmlRpcValue::TypeString)
00085 bad_params = true;
00086 names.push_back( static_cast<std::string>(my_list[i]) );
00087 }
00088
00089 n_tilde.getParam("sensor_touch_names", my_list);
00090 if(my_list.getType() != XmlRpc::XmlRpcValue::TypeArray)
00091 bad_params = true;
00092 if(my_list.size() != list_size)
00093 bad_params = true;
00094 list_size = my_list.size();
00095 for (int32_t i = 0; i < list_size; ++i)
00096 {
00097 if(my_list[i].getType() != XmlRpc::XmlRpcValue::TypeString)
00098 bad_params = true;
00099 sensor_touch_names.push_back( static_cast<std::string>(my_list[i]) );
00100 }
00101
00102 if(bad_params)
00103 {
00104 ROS_ERROR("Error while reading the parameter for the tactile sensors; using standard parameters");
00105 names.clear();
00106 sensor_touch_names.clear();
00107
00108 names.push_back("ff");
00109 names.push_back("mf");
00110 names.push_back("rf");
00111 names.push_back("lf");
00112 names.push_back("th");
00113
00114 sensor_touch_names.push_back("FF_Touch");
00115 sensor_touch_names.push_back("MF_Touch");
00116 sensor_touch_names.push_back("RF_Touch");
00117 sensor_touch_names.push_back("LF_Touch");
00118 sensor_touch_names.push_back("TH_Touch");
00119 }
00120
00121 all_names_vector.push_back(names);
00122 all_names_vector.push_back(sensor_touch_names);
00123
00124 return all_names_vector;
00125 }
00126
00127 void SrTactileSensorManager::publish_all()
00128 {
00129 for(unsigned int i=0; i < tactile_sensors.size(); ++i)
00130 tactile_sensors[i]->publish_current_values();
00131
00132 publish_rate.sleep();
00133 ros::spinOnce();
00134 }
00135 }
00136
00137
00138
00139
00140
00141
00142