39 std::string touch_name) :
42 std::string full_topic_touch =
"touch/" + name;
62 n_tilde(
"~"), publish_rate(20.0)
70 double tmp[5] = {117, 117, 113, 111, 0};
77 if (threshold_xmlrpc.
size() == 5)
79 for (
int i = 0; i < threshold_xmlrpc.
size(); ++i)
83 tmp[i] =
static_cast<double>(threshold_xmlrpc[i]);
86 ROS_ERROR(
"grasp_touch_thresholds value %d is not a double, not loading", i + 1);
90 ROS_ERROR(
"grasp_touch_thresholds must be of size 5, using default values");
93 ROS_ERROR(
"grasp_touch_thresholds must be an array, using default values");
96 ROS_WARN(
"grasp_touch_thresholds not set, using default values");
116 sr_robot_msgs::is_hand_occupied::Response &res)
118 bool is_occupied =
true;
125 ROS_DEBUG(
"is_hand_occupied_thresholds %d with val %f is smaller than threshold %f", i,
131 res.hand_occupied = is_occupied;
137 sr_robot_msgs::which_fingers_are_touching::Response &res)
139 std::vector<double> touch_values(5);
142 double value_tmp = 0.0;
146 if (value_tmp < req.force_thresholds[i])
148 touch_values[i] = 0.0;
152 touch_values[i] = value_tmp;
155 res.touch_forces = touch_values;
161 std::vector<std::vector<std::string> > all_names_vector;
162 std::vector<std::string> names, sensor_touch_names;
167 bool bad_params =
false;
174 list_size = my_list.
size();
175 for (int32_t i = 0; i < list_size; ++i)
181 names.push_back(static_cast<std::string>(my_list[i]));
189 if (my_list.
size() != list_size)
193 list_size = my_list.
size();
194 for (int32_t i = 0; i < list_size; ++i)
200 sensor_touch_names.push_back(static_cast<std::string>(my_list[i]));
205 ROS_ERROR(
"Error while reading the parameter for the tactile sensors; using standard parameters");
207 sensor_touch_names.clear();
209 names.push_back(
"ff");
210 names.push_back(
"mf");
211 names.push_back(
"rf");
212 names.push_back(
"lf");
213 names.push_back(
"th");
215 sensor_touch_names.push_back(
"FF_Touch");
216 sensor_touch_names.push_back(
"MF_Touch");
217 sensor_touch_names.push_back(
"RF_Touch");
218 sensor_touch_names.push_back(
"LF_Touch");
219 sensor_touch_names.push_back(
"TH_Touch");
222 all_names_vector.push_back(names);
223 all_names_vector.push_back(sensor_touch_names);
225 return all_names_vector;
This is a generic parent class for the tactile sensors used in the Shadow Robot Dextrous Hand...
void publish(const boost::shared_ptr< M > &message) const
std_msgs::Float64 msg_touch
~SrTactileSensorManager()
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
Type const & getType() const
std::vector< boost::shared_ptr< SrGenericTactileSensor > > tactile_sensors
void publish_current_values()
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
SrGenericTactileSensor(std::string name, std::string touch_name)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool which_fingers_are_touching_cb(sr_robot_msgs::which_fingers_are_touching::Request &req, sr_robot_msgs::which_fingers_are_touching::Response &res)
std::vector< double > is_hand_occupied_thresholds
bool getParam(const std::string &key, std::string &s) const
ros::ServiceServer which_fingers_are_touching_server
virtual double get_touch_data()=0
bool is_hand_occupied_cb(sr_robot_msgs::is_hand_occupied::Request &req, sr_robot_msgs::is_hand_occupied::Response &res)
ROSCPP_DECL void spinOnce()
virtual ~SrGenericTactileSensor()
std::vector< std::vector< std::string > > get_all_names()
ros::ServiceServer is_hand_occupied_server