def display_publsisher(path, pan_target):
img = cv2.imread(path)
msg = cv_bridge.CvBridge().cv2_to_imgmsg(img, encoding="bgr8")
pub = rospy.Publisher('/robot/xdisplay', Image, latch=True, queue_size=1)
pub.publish(msg)
rospy.sleep(1)
for i in range(1, 10):
pub3 = rospy.Publisher('/robot/head/command_head_pan',
HeadPanCommand,
queue_size=1)
msg = HeadPanCommand()
msg.target = pan_target
msg.speed_ratio = 100
msg.enable_pan_request = True
pub3.publish(msg)
# Sleep to allow for image to be published.
rospy.sleep(0.1)
def spin(self):
while not rospy.is_shutdown():
msg = HeadPanCommand()
msg.enable_pan_request = msg.REQUEST_PAN_DISABLE
# move the head if we are executing an action
if not self._done:
# set the target
msg.target = self._target
# speed ratio will be a function of goal time
msg.speed_ratio = np.clip(
1 / (2 * (self._goal_duration.to_sec() + 1e-8)), 0.01, 1)
elif self._enable_noise:
# add noise
# TODO add perlin noise
noise = 0
msg.target = self._target + noise
self.pub.publish(msg)
self._noise_rate.sleep()
