Skip to content

Instantly share code, notes, and snippets.

@timarnold
Created January 5, 2018 18:11
Show Gist options
  • Save timarnold/458f7b1b4fa0ded9ef0ea65fda10009b to your computer and use it in GitHub Desktop.
Save timarnold/458f7b1b4fa0ded9ef0ea65fda10009b to your computer and use it in GitHub Desktop.
#!/usr/bin/env python
import math
import rospy
from sensor_msgs.msg import Image, JointState
from simple_arm.srv import *
class LookAway(object):
def __init__(self):
rospy.init_node('look_away')
self.sub1 = rospy.Subscriber('/simple_arm/joint_states',
JointState, self.joint_states_callback)
self.sub2 = rospy.Subscriber("rgb_camera/image_raw",
Image, self.look_away_callback)
self.safe_move = rospy.ServiceProxy('/arm_mover/safe_move',
GoToPosition)
self.last_position = None
self.arm_moving = False
rospy.spin()
def uniform_image(self, image):
return all(value == image[0] for value in image)
def coord_equal(self, coord_1, coord_2):
if coord_1 is None or coord_2 is None:
return False
tolerance = .0005
result = abs(coord_1[0] - coord_2[0]) <= abs(tolerance)
result = result and abs(coord_1[1] - coord_2[1]) <= abs(tolerance)
return result
def joint_states_callback(self, data):
if self.coord_equal(data.position, self.last_position):
self.arm_moving = False
else:
self.last_position = data.position
self.arm_moving = True
def look_away_callback(self, data):
if not self.arm_moving and self.uniform_image(data.data):
try:
rospy.wait_for_service('/arm_mover/safe_move')
msg = GoToPositionRequest()
msg.joint_1 = 1.57
msg.joint_2 = 1.57
response = self.safe_move(msg)
rospy.logwarn("Camera detecting uniform image. \
Elapsed time to look at something nicer:\n%s",
response)
except rospy.ServiceException, e:
rospy.logwarn("Service call failed: %s", e)
if __name__ == '__main__':
try:
LookAway()
except rospy.ROSInterruptException:
pass
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment