先完成上一篇任务。
以下是look_away的完整代码，接下来是逐步解释发生的事情。 您可以将此代码复制并粘贴到您在目录中创建的look_away脚本中：
~/catkin_ws/src/simple_arm/scripts
Look Away 代码
#!/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
代码解释
导入语句
#!/usr/bin/env python
import math
import rospy
from sensor_msgs.msg import Image, JointState
from simple_arm.srv import *
导入的模块类似于simple_arm中的模块，除了这次，我们已经导入了图像消息类型以便可以使用相机数据。
LookAway类的初始化部分
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()
我们为这个节点定义一个类，以更好地跟踪机器人手臂当前的运动状态和位置历史。 就像之前的节点定义一样，节点使用ropsy.init_node进行初始化，并在方法结束时使用rospy.spin()来阻塞，直到节点收到关闭请求。
第一个订阅者self.sub1订阅/ simple_arm / joint_states主题。 只有当手臂没有移动时才写入该节点以检查摄像机，并且通过订阅/ simple_arm / joint_states，可以跟踪手臂位置的变化。 该主题的消息类型是JointState，并且每条消息都将消息数据传递给joint_states_callback函数。
第二个订阅者self.sub2订阅了/ rgb_camera / image_raw主题。 这里的消息类型是Image，每个消息都会调用look_away_callback函数。
ServiceProxy是rospy如何从节点调用服务的。 这里的ServiceProxy是使用您希望与服务类定义一起调用的服务的名称创建的：在本例中为/ arm_mover / safe_move和GoToPosition。 实际的服务调用将在下面的look_away_callback方法中进行。
辅助方法
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
代码中定义了两个辅助方法：uniform_image和coord_equal。 uniform_image方法将图像作为输入并检查图像中的所有颜色值是否与第一个像素的值相同。 这基本上检查图像中的所有颜色值是否相同。
如果坐标coord_1和coord_2具有等于指定容差的相等分量，则coord_equal方法返回True。
回调函数
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)
当self.sub1在 /simple_arm / joint_states主题上收到消息时，消息将传递给变量数据中的joint_states_callback。joint_states_callback使用coord_equal辅助函数来检查数据中提供的当前联合状态是否与以前的联合状态相同，这些联合状态存储在self.last_position中。如果当前和以前的联合状态相同（达到指定的容差），则手臂已停止移动，因此self.arm_moving标志设置为False。如果当前和以前的联合状态不同，那么手臂仍在移动。 在这种情况下，该方法使用当前位置数据更新self.last_position并将self.arm_moving设置为True。
look_away_callback正从/ rgb_camera / image_raw主题接收数据。 该方法的第一行验证手臂是否移动，并检查图像是否均匀。 如果手臂没有移动并且图像是统一的，则创建GoToPositionRequest（）消息并使用safe_move服务发送，从而将两个关节角度移动到1.57。 该方法还会记录一条消息，警告您相机检测到统一的图像以及经过的时间以返回更好的图像。