25 def __init__(self, topic_name, finger_length, finger_width):
30 self.
cells_x = rospy.get_param(
"~cells_x", 6)
31 self.
cells_y = rospy.get_param(
"~cells_y", 14)
32 rospy.logdebug(
"size of the tactile matrix is %ix%i patches", self.
cells_x, self.
cells_y)
37 self.
range = rospy.get_param(
"~output_range", 3500)
38 rospy.logdebug(
"output range: %f", self.
range)
44 rospy.logdebug(
"subscribed to bumper states topic: %s", topic_name)
50 Update the pad with the Gazebo contact information. 52 :param states: gazebo_msgs.msg.ContactsState 56 matrix = self.
update(states, matrix)
57 matrix = self.
smooth(matrix)
64 Update the provided matrix with the contact information from the Gazebo 67 :param states: gazebo_msgs.msg.ContactsState 68 :param matrix: Float[] 72 if (len(states.states) == 0):
75 for i
in range(len(states.states)):
76 state = states.states[i]
77 for j
in range(len(state.contact_positions)):
79 if (state.contact_positions[j].x < 0.0):
86 normalized_x = (state.contact_positions[j].y / self.
finger_width) + 0.5
87 normalized_y = state.contact_positions[j].z / self.
finger_length 91 x = round(normalized_x * self.
cells_x)
92 y = round(normalized_y * self.
cells_y)
94 force = -state.wrenches[j].force.x
95 if (force < 0.0): force = 0.0
99 matrix[index] += force
105 Create a new matrix that contains the current force on each cell. Initialize 106 all cells with zeros. 119 Run a moving average on the provided matrix. 121 :param matrix: Float[] 131 for dx
in range(-1, 2):
132 for dy
in range(-1, 2):
136 if (index_x < 0
or index_x >= self.
cells_x):
continue 137 if (index_y < 0
or index_y >= self.
cells_y):
continue 143 smoothed_matrix[index] = sum / count
145 return smoothed_matrix
149 Calculate the average matrix from the force buffer. 155 sample_factor = 1.0 / filter_length
158 force = (1.0 - sample_factor) * matrix_buffer[i]
159 force += sample_factor * current_matrix[i]
166 Get the current forces as tactile matrix. matrix_id is the identifier of the 167 tactile matrix and determines which pad produced the data. 169 :param matrix_id: Integer 170 :return: schunk_sdh.msg.TactileMatrix 173 matrix = TactileMatrix()
174 matrix.matrix_id = matrix_id
182 if (force < 0.0): force = 0.0
184 matrix.tactile_array.append(force)
190 Map the two-dimensional coordinate of a tactile patch to an index in the 191 one-dimensional data array. The coordinates are bound to the upper and lower 212 Constants that determine which indices the finger parts have in the 213 schunk_sdh.msg.TactileSensor matrix. 232 self.
pub = rospy.Publisher(
"tactile_data", TactileSensor, queue_size=1)
233 rospy.loginfo(
"'tactile_data' topic advertized")
237 Publish the current state of the simulated tactile sensors. 240 msg = TactileSensor()
241 msg.header.stamp = rospy.Time.now()
243 msg.tactile_matrix.append(self.
pads[i].tactile_matrix(i))
244 self.pub.publish(msg)
249 if __name__ ==
"__main__":
250 rospy.init_node(
'tactile_sensors')
255 while not rospy.is_shutdown():
def tactile_matrix(self, matrix_id)
def create_empty_force_matrix(self)
def get_index(self, x, y)
def contact_callback(self, states)
def __init__(self, topic_name, finger_length, finger_width)
def update(self, states, matrix)
def time_average(self, matrix_buffer, current_matrix, filter_length)