6 import moveit_commander
8 from geometry_msgs.msg
import Wrench, WrenchStamped
10 from std_srvs.srv
import Empty, EmptyResponse, Trigger, TriggerResponse
11 from std_msgs.msg
import String
23 if req.axis
in (req.AXIS_X, req.AXIS_Y, req.AXIS_Z):
24 threshold = getattr(self._current_wrench.force, req.axis) + req.val
25 info =
'axis: {}, val: {}, threshold: {}'.format(req.axis, req.val, threshold)
26 rospy.loginfo(
'set_stop_force: {}'.format(info))
28 self.
add_handler(
lambda msg: getattr(msg.wrench.force, req.axis) > threshold,
lambda msg: self.
stop(), info)
30 self.
add_handler(
lambda msg: getattr(msg.wrench.force, req.axis) < threshold,
lambda msg: self.
stop(), info)
31 elif req.axis
in (req.AXIS_MX, req.AXIS_MY, req.AXIS_MZ):
32 threshold = getattr(self._current_wrench.torque, req.axis) + req.val
33 info =
'axis: {}, val: {}, threshold: {}'.format(req.axis, req.val, threshold)
34 rospy.loginfo(
'set_stop_force: {}'.format(info))
36 self.
add_handler(
lambda msg: getattr(msg.wrench.torque, req.axis) > threshold,
lambda msg: self.
stop(), info)
38 self.
add_handler(
lambda msg: getattr(msg.wrench.torque, req.axis) < threshold,
lambda msg: self.
stop(), info)
40 raise rospy.ServiceException(
'invalid axis: {}'.format(req.axis))
42 res = SetStopForceResponse()
46 rospy.loginfo(
'clear_stop_force')
54 res = TriggerResponse()
61 self._pub.publish(
'stop')
63 def add_handler(self, condition, command, info='', key='default'):
65 self.
_handlers[
'default'] = {
'condition': condition,
'command': command,
'info': info}
71 for k, x
in self._handlers.iteritems():
72 if x[
'condition'](msg):
73 rospy.loginfo(
'command is called!: {}'.format(x[
'info']))
85 self.
_pub = rospy.Publisher(
'/trajectory_execution_event', String, queue_size=10)
88 rospy.init_node(
'force_observer', anonymous=
True)
93 if __name__ ==
'__main__':
def add_handler(self, condition, command, info='', key='default')
def _cb_clear_stop_force(self, req)
def _cb_is_stopped(self, req)
def _cb_set_stop_force(self, req)
def _cb_wrench_stamped(self, msg)
def __init__(self, group_name)