5 from std_srvs.srv
import Empty, EmptyResponse
6 from geometry_msgs.msg
import WrenchStamped, Wrench
19 self._zero_offset_buffer.append(msg)
23 rospy.logwarn(
"reset!")
27 ws.header = msg.header
28 ws.wrench.force.x = msg.wrench.force.x - self._zero_offset.force.x
29 ws.wrench.force.y = msg.wrench.force.y - self._zero_offset.force.y
30 ws.wrench.force.z = msg.wrench.force.z - self._zero_offset.force.z
31 ws.wrench.torque.x = msg.wrench.torque.x - self._zero_offset.torque.x
32 ws.wrench.torque.y = msg.wrench.torque.y - self._zero_offset.torque.y
33 ws.wrench.torque.z = msg.wrench.torque.z - self._zero_offset.torque.z
40 w.force.x += x.wrench.force.x
41 w.force.y += x.wrench.force.y
42 w.force.z += x.wrench.force.z
43 w.torque.x += x.wrench.torque.x
44 w.torque.y += x.wrench.torque.y
45 w.torque.z += x.wrench.torque.z
60 self.
_reset_time = rospy.Time.now() + rospy.Duration(1)
62 rospy.logwarn(
"cb_reset!")
64 return EmptyResponse()
70 self.
_pub = rospy.Publisher(
'force', WrenchStamped, queue_size=10)
71 rospy.Subscriber(
'raw_force', WrenchStamped, self.
cb_raw_force)
72 rospy.Service(
'~reset', Empty, self.
cb_reset)
76 if __name__ ==
'__main__':
77 rospy.init_node(
'force_publisher')
def cb_raw_force(self, msg)
def cb_get_current_force(self, req)
def _avg_wrench(self, wrench_list)