示例#1
0
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)
示例#2
0
    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()