def __init__(self, topic='/sensor_values'):
self.bx = Baxter('left')
self.joint_v = {name: 0.0 for name in self.bx.limb.joint_names()}
self.stop = False
self.nh = rospy.init_node('SafetyStop')
self.sensor_sub = rospy.Subscriber(topic,
Int32MultiArray,
self.udpate_joint_v,
queue_size=1)
class SafetyStop(object):
"""In theory, if this node is running, no matter what happens, if
something gets too close to the sensor tip, the robot will stop
moving until the obstacle is cleared.
"""
def __init__(self, topic="/sensor_values"):
self.bx = Baxter("left")
self.joint_v = {name: 0.0 for name in self.bx.limb.joint_names()}
self.stop = False
self.nh = rospy.init_node("SafetyStop")
self.sensor_sub = rospy.Subscriber(topic, Int32MultiArray, self.udpate_joint_v, queue_size=1)
def _cartesian_v_from_sensor_values(self, values):
tip = values[[7, 15]]
if np.any(tip < 10 ** 4):
self.stop = True
else:
self.stop = False
return [0] * 6
def update_joint_v(self, msg):
arr = np.array(msg.data)
cartesian_v = self._cartesian_v_from_sensor_values(arr)
self.joint_v = self.bx.compute_joint_velocities(cartesian_v)
class SafetyStop(object):
"""In theory, if this node is running, no matter what happens, if
something gets too close to the sensor tip, the robot will stop
moving until the obstacle is cleared.
"""
def __init__(self, topic='/sensor_values'):
self.bx = Baxter('left')
self.joint_v = {name: 0.0 for name in self.bx.limb.joint_names()}
self.stop = False
self.nh = rospy.init_node('SafetyStop')
self.sensor_sub = rospy.Subscriber(topic,
Int32MultiArray,
self.udpate_joint_v,
queue_size=1)
def _cartesian_v_from_sensor_values(self, values):
tip = values[[7, 15]]
if np.any(tip < 10**4):
self.stop = True
else:
self.stop = False
return [0] * 6
def update_joint_v(self, msg):
arr = np.array(msg.data)
cartesian_v = self._cartesian_v_from_sensor_values(arr)
self.joint_v = self.bx.compute_joint_velocities(cartesian_v)
def __init__(self, topic="/sensor_values"):
self.bx = Baxter("left")
self.joint_v = {name: 0.0 for name in self.bx.limb.joint_names()}
self.stop = False
self.nh = rospy.init_node("SafetyStop")
self.sensor_sub = rospy.Subscriber(topic, Int32MultiArray, self.udpate_joint_v, queue_size=1)
