Ejemplo n.º 1
0
def main():
    args = parse_args()

    left_cm = CameraInfoManager(namespace='left')
    left_cm.setURL('file://{}'.format(os.path.abspath(args.l)))
    left_cm.loadCameraInfo()
    left_msg = left_cm.getCameraInfo()

    right_cm = CameraInfoManager(namespace='right')
    right_cm.setURL('file://{}'.format(os.path.abspath(args.r)))
    right_cm.loadCameraInfo()
    right_msg = right_cm.getCameraInfo()

    in_bag = rosbag.Bag(args.bagfile)

    in_basename = os.path.splitext(
        os.path.basename(os.path.abspath(args.bagfile)))[0]
    out_filepath = os.path.join(os.path.dirname(os.path.abspath(args.bagfile)),
                                in_basename + '_wcaminfo.bag')

    print 'Writing to', out_filepath

    with rosbag.Bag(out_filepath, 'w') as out_bag:
        for topic, msg, t in in_bag.read_messages():
            if topic == '/cam0/image_raw':
                left_msg.header = msg.header
                out_bag.write('/cam/left/camera_info', left_msg, t)
                out_bag.write('/cam/left/image_raw', msg, t)
            elif topic == '/cam1/image_raw':
                right_msg.header = msg.header
                out_bag.write('/cam/right/camera_info', right_msg, t)
                out_bag.write('/cam/right/image_raw', msg, t)
            else:
                out_bag.write(topic, msg, t)
Ejemplo n.º 2
0
class RaspicamRos2(Node):
    def __init__(self, args):
        super().__init__('raspicam_ros2')

        # vars
        self._topic_name = 'raspicam'
        if (len(args) > 1):
            self._topic_name = args[1]
        self._camera_info_manager = CameraInfoManager(self,
                                                      'raspicam',
                                                      namespace='/' +
                                                      self._topic_name)
        camera_info_url = 'file://' + os.path.dirname(
            os.path.abspath(__file__)) + '/config/raspicam_416x320.yaml'

        # pubs
        self._img_pub = self.create_publisher(
            Image,
            '/' + self._topic_name + '/image',
            qos_profile=rclpy.qos.qos_profile_sensor_data)
        self._camera_info_pub = self.create_publisher(
            CameraInfo, '/' + self._topic_name + '/camera_info')

        # camera info manager
        self._camera_info_manager.setURL(camera_info_url)
        self._camera_info_manager.loadCameraInfo()

    def _get_stamp(self, time_float):
        time_frac_int = modf(time_float)
        stamp = Time()
        stamp.sec = int(time_frac_int[1])
        stamp.nanosec = int(time_frac_int[0] * 1000000000) & 0xffffffff
        return stamp

    def publish_image(self, image):
        img = Image()
        img.encoding = 'rgb8'
        img.width = 416
        img.height = 320
        img.step = img.width * 3
        img.data = image
        img.header.frame_id = 'raspicam'
        img.header.stamp = self._get_stamp(time())
        self._img_pub.publish(img)
        camera_info = self._camera_info_manager.getCameraInfo()
        camera_info.header = img.header
        self._camera_info_pub.publish(camera_info)

    def run(self):
        with PiCamera() as camera:
            camera.resolution = (416, 320)
            # Construct the frame processor and start recording data to it
            frame_processor = FrameProcessor(self)
            camera.start_recording(frame_processor, 'rgb')
            try:
                while True:
                    camera.wait_recording(1)
            finally:
                camera.stop_recording()
Ejemplo n.º 3
0
def main():
    """Main routine to run DOPE"""

    # Initialize ROS node
    # rospy.init_node('dope')
    dopenode = DopeNode()
    image_path = \
        "/media/aditya/A69AFABA9AFA85D9/Cruzr/code/Dataset_Synthesizer/Test/Zed/NewMap1_turbosquid_can_only/000000.left.png"
    # image_path = \
    #     "/media/aditya/A69AFABA9AFA85D9/Cruzr/code/Dataset_Synthesizer/Test/Zed/NewMap1_dope/000001.left.png"
    camera_ns = rospy.get_param('camera', 'dope/webcam')
    info_manager = CameraInfoManager(cname='dope_webcam_{}'.format(0),
                                     namespace=camera_ns)
    try:
        camera_info_url = rospy.get_param('~camera_info_url')
        if not info_manager.setURL(camera_info_url):
            rospy.logwarn('Camera info URL invalid: %s', camera_info_url)
    except KeyError:
        # we don't have a camera_info_url, so we'll keep the
        # default ('file://${ROS_HOME}/camera_info/${NAME}.yaml')
        pass
    info_manager.loadCameraInfo()
    if not info_manager.isCalibrated():
        rospy.logwarn('Camera is not calibrated, please supply a valid camera_info_url parameter!')
    camera_info = info_manager.getCameraInfo()
    dopenode.run_on_image(image_path, camera_info)
Ejemplo n.º 4
0
class CozmoRos(object):
    """
    The Cozmo ROS driver object.

    """
    def __init__(self, coz):
        """

        :type   coz:    cozmo.Robot
        :param  coz:    The cozmo SDK robot handle (object).
        
        """

        # vars
        self._cozmo = coz
        self._lin_vel = .0
        self._ang_vel = .0
        self._cmd_lin_vel = .0
        self._cmd_ang_vel = .0
        self._last_pose = self._cozmo.pose
        self._wheel_vel = (0, 0)
        self._optical_frame_orientation = quaternion_from_euler(
            -np.pi / 2., .0, -np.pi / 2.)
        self._camera_info_manager = CameraInfoManager(
            'cozmo_camera', namespace='/cozmo_camera')

        # tf
        self._tfb = TransformBroadcaster()

        # params
        self._odom_frame = rospy.get_param('~odom_frame', 'odom')
        self._footprint_frame = rospy.get_param('~footprint_frame',
                                                'base_footprint')
        self._base_frame = rospy.get_param('~base_frame', 'base_link')
        self._head_frame = rospy.get_param('~head_frame', 'head_link')
        self._camera_frame = rospy.get_param('~camera_frame', 'camera_link')
        self._camera_optical_frame = rospy.get_param('~camera_optical_frame',
                                                     'cozmo_camera')
        camera_info_url = rospy.get_param('~camera_info_url', '')

        # pubs
        self._joint_state_pub = rospy.Publisher('joint_states',
                                                JointState,
                                                queue_size=1)
        self._odom_pub = rospy.Publisher('odom', Odometry, queue_size=1)
        self._imu_pub = rospy.Publisher('imu', Imu, queue_size=1)
        self._battery_pub = rospy.Publisher('battery',
                                            BatteryState,
                                            queue_size=1)
        # Note: camera is published under global topic (preceding "/")
        self._image_pub = rospy.Publisher('/cozmo_camera/image',
                                          Image,
                                          queue_size=10)
        self._camera_info_pub = rospy.Publisher('/cozmo_camera/camera_info',
                                                CameraInfo,
                                                queue_size=10)

        # subs
        self._backpack_led_sub = rospy.Subscriber('backpack_led',
                                                  ColorRGBA,
                                                  self._set_backpack_led,
                                                  queue_size=1)
        self._twist_sub = rospy.Subscriber('cmd_vel',
                                           Twist,
                                           self._twist_callback,
                                           queue_size=1)
        self._say_sub = rospy.Subscriber('say',
                                         String,
                                         self._say_callback,
                                         queue_size=1)
        self._head_sub = rospy.Subscriber('head_angle',
                                          Float64,
                                          self._move_head,
                                          queue_size=1)
        self._lift_sub = rospy.Subscriber('lift_height',
                                          Float64,
                                          self._move_lift,
                                          queue_size=1)

        # diagnostics
        self._diag_array = DiagnosticArray()
        self._diag_array.header.frame_id = self._base_frame
        diag_status = DiagnosticStatus()
        diag_status.hardware_id = 'Cozmo Robot'
        diag_status.name = 'Cozmo Status'
        diag_status.values.append(KeyValue(key='Battery Voltage', value=''))
        diag_status.values.append(KeyValue(key='Head Angle', value=''))
        diag_status.values.append(KeyValue(key='Lift Height', value=''))
        self._diag_array.status.append(diag_status)
        self._diag_pub = rospy.Publisher('/diagnostics',
                                         DiagnosticArray,
                                         queue_size=1)

        # camera info manager
        self._camera_info_manager.setURL(camera_info_url)
        self._camera_info_manager.loadCameraInfo()

    def _publish_diagnostics(self):
        # alias
        diag_status = self._diag_array.status[0]

        # fill diagnostics array
        battery_voltage = self._cozmo.battery_voltage
        diag_status.values[0].value = '{:.2f} V'.format(battery_voltage)
        diag_status.values[1].value = '{:.2f} deg'.format(
            self._cozmo.head_angle.degrees)
        diag_status.values[2].value = '{:.2f} mm'.format(
            self._cozmo.lift_height.distance_mm)
        if battery_voltage > 3.5:
            diag_status.level = DiagnosticStatus.OK
            diag_status.message = 'Everything OK!'
        elif battery_voltage > 3.4:
            diag_status.level = DiagnosticStatus.WARN
            diag_status.message = 'Battery low! Go charge soon!'
        else:
            diag_status.level = DiagnosticStatus.ERROR
            diag_status.message = 'Battery very low! Cozmo will power off soon!'

        # update message stamp and publish
        self._diag_array.header.stamp = rospy.Time.now()
        self._diag_pub.publish(self._diag_array)

    def _move_head(self, cmd):
        """
        Move head to given angle.
        
        :type   cmd:    Float64
        :param  cmd:    The message containing angle in degrees. [-25 - 44.5]
        
        """
        action = self._cozmo.set_head_angle(radians(cmd.data * np.pi / 180.),
                                            duration=0.1,
                                            in_parallel=True)
        action.wait_for_completed()

    def _move_lift(self, cmd):
        """
        Move lift to given height.

        :type   cmd:    Float64
        :param  cmd:    A value between [0 - 1], the SDK auto
                        scales it to the according height.

        """
        action = self._cozmo.set_lift_height(height=cmd.data,
                                             duration=0.2,
                                             in_parallel=True)
        action.wait_for_completed()

    def _set_backpack_led(self, msg):
        """
        Set the color of the backpack LEDs.

        :type   msg:    ColorRGBA
        :param  msg:    The color to be set.

        """
        # setup color as integer values
        color = [int(x * 255) for x in [msg.r, msg.g, msg.b, msg.a]]
        # create lights object with duration
        light = cozmo.lights.Light(cozmo.lights.Color(rgba=color),
                                   on_period_ms=1000)
        # set lights
        self._cozmo.set_all_backpack_lights(light)

    def _twist_callback(self, cmd):
        """
        Set commanded velocities from Twist message.

        The commands are actually send/set during run loop, so delay
        is in worst case up to 1 / update_rate seconds.

        :type   cmd:    Twist
        :param  cmd:    The commanded velocities.

        """
        # compute differential wheel speed
        axle_length = 0.07  # 7cm
        self._cmd_lin_vel = cmd.linear.x
        self._cmd_ang_vel = cmd.angular.z
        rv = self._cmd_lin_vel + (self._cmd_ang_vel * axle_length * 0.5)
        lv = self._cmd_lin_vel - (self._cmd_ang_vel * axle_length * 0.5)
        self._wheel_vel = (lv * 1000., rv * 1000.)  # convert to mm / s

    def _say_callback(self, msg):
        """
        The callback for incoming text messages to be said.

        :type   msg:    String
        :param  msg:    The text message to say.

        """
        self._cozmo.say_text(msg.data).wait_for_completed()

    def _publish_objects(self):
        """
        Publish detected object as transforms between odom_frame and object_frame.

        """

        for obj in self._cozmo.world.visible_objects:
            now = rospy.Time.now()
            x = obj.pose.position.x * 0.001
            y = obj.pose.position.y * 0.001
            z = obj.pose.position.z * 0.001
            q = (obj.pose.rotation.q1, obj.pose.rotation.q2,
                 obj.pose.rotation.q3, obj.pose.rotation.q0)
            self._tfb.send_transform((x, y, z), q, now,
                                     'cube_' + str(obj.object_id),
                                     self._odom_frame)

    def _publish_image(self):
        """
        Publish latest camera image as Image with CameraInfo.

        """
        # only publish if we have a subscriber
        if self._image_pub.get_num_connections() == 0:
            return

        # get latest image from cozmo's camera
        camera_image = self._cozmo.world.latest_image
        if camera_image is not None:
            # convert image to gray scale as it is gray although
            img = camera_image.raw_image.convert('L')
            ros_img = Image()
            ros_img.encoding = 'mono8'
            ros_img.width = img.size[0]
            ros_img.height = img.size[1]
            ros_img.step = ros_img.width
            ros_img.data = img.tobytes()
            ros_img.header.frame_id = 'cozmo_camera'
            cozmo_time = camera_image.image_recv_time
            ros_img.header.stamp = rospy.Time.from_sec(cozmo_time)
            # publish images and camera info
            self._image_pub.publish(ros_img)
            camera_info = self._camera_info_manager.getCameraInfo()
            camera_info.header = ros_img.header
            self._camera_info_pub.publish(camera_info)

    def _publish_joint_state(self):
        """
        Publish joint states as JointStates.

        """
        # only publish if we have a subscriber
        if self._joint_state_pub.get_num_connections() == 0:
            return

        js = JointState()
        js.header.stamp = rospy.Time.now()
        js.header.frame_id = 'cozmo'
        js.name = ['head', 'lift']
        js.position = [
            self._cozmo.head_angle.radians,
            self._cozmo.lift_height.distance_mm * 0.001
        ]
        js.velocity = [0.0, 0.0]
        js.effort = [0.0, 0.0]
        self._joint_state_pub.publish(js)

    def _publish_imu(self):
        """
        Publish inertia data as Imu message.

        """
        # only publish if we have a subscriber
        if self._imu_pub.get_num_connections() == 0:
            return

        imu = Imu()
        imu.header.stamp = rospy.Time.now()
        imu.header.frame_id = self._base_frame
        imu.orientation.w = self._cozmo.pose.rotation.q0
        imu.orientation.x = self._cozmo.pose.rotation.q1
        imu.orientation.y = self._cozmo.pose.rotation.q2
        imu.orientation.z = self._cozmo.pose.rotation.q3
        imu.angular_velocity.x = self._cozmo.gyro.x
        imu.angular_velocity.y = self._cozmo.gyro.y
        imu.angular_velocity.z = self._cozmo.gyro.z
        imu.linear_acceleration.x = self._cozmo.accelerometer.x * 0.001
        imu.linear_acceleration.y = self._cozmo.accelerometer.y * 0.001
        imu.linear_acceleration.z = self._cozmo.accelerometer.z * 0.001
        self._imu_pub.publish(imu)

    def _publish_battery(self):
        """
        Publish battery as BatteryState message.

        """
        # only publish if we have a subscriber
        if self._battery_pub.get_num_connections() == 0:
            return

        battery = BatteryState()
        battery.header.stamp = rospy.Time.now()
        battery.voltage = self._cozmo.battery_voltage
        battery.present = True
        if self._cozmo.is_on_charger:  # is_charging always return False
            battery.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_CHARGING
        else:
            battery.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING
        self._battery_pub.publish(battery)

    def _publish_odometry(self):
        """
        Publish current pose as Odometry message.

        """
        # only publish if we have a subscriber
        if self._odom_pub.get_num_connections() == 0:
            return

        now = rospy.Time.now()
        odom = Odometry()
        odom.header.frame_id = self._odom_frame
        odom.header.stamp = now
        odom.child_frame_id = self._footprint_frame
        odom.pose.pose.position.x = self._cozmo.pose.position.x * 0.001
        odom.pose.pose.position.y = self._cozmo.pose.position.y * 0.001
        odom.pose.pose.position.z = self._cozmo.pose.position.z * 0.001
        q = quaternion_from_euler(.0, .0, self._cozmo.pose_angle.radians)
        odom.pose.pose.orientation.x = q[0]
        odom.pose.pose.orientation.y = q[1]
        odom.pose.pose.orientation.z = q[2]
        odom.pose.pose.orientation.w = q[3]
        odom.pose.covariance = np.diag([1e-2, 1e-2, 1e-2, 1e3, 1e3,
                                        1e-1]).ravel()
        odom.twist.twist.linear.x = self._lin_vel
        odom.twist.twist.angular.z = self._ang_vel
        odom.twist.covariance = np.diag([1e-2, 1e3, 1e3, 1e3, 1e3,
                                         1e-2]).ravel()
        self._odom_pub.publish(odom)

    def _publish_tf(self, update_rate):
        """
        Broadcast current transformations and update
        measured velocities for odometry twist.

        Published transforms:

        odom_frame -> footprint_frame
        footprint_frame -> base_frame
        base_frame -> head_frame
        head_frame -> camera_frame
        camera_frame -> camera_optical_frame

        """
        now = rospy.Time.now()
        x = self._cozmo.pose.position.x * 0.001
        y = self._cozmo.pose.position.y * 0.001
        z = self._cozmo.pose.position.z * 0.001

        # compute current linear and angular velocity from pose change
        # Note: Sign for linear velocity is taken from commanded velocities!
        # Note: The angular velocity can also be taken from gyroscopes!
        delta_pose = self._last_pose - self._cozmo.pose
        dist = np.sqrt(delta_pose.position.x**2 + delta_pose.position.y**2 +
                       delta_pose.position.z**2) / 1000.0
        self._lin_vel = dist * update_rate * np.sign(self._cmd_lin_vel)
        self._ang_vel = -delta_pose.rotation.angle_z.radians * update_rate

        # publish odom_frame -> footprint_frame
        q = quaternion_from_euler(.0, .0, self._cozmo.pose_angle.radians)
        self._tfb.send_transform((x, y, 0.0), q, now, self._footprint_frame,
                                 self._odom_frame)

        # publish footprint_frame -> base_frame
        q = quaternion_from_euler(.0, -self._cozmo.pose_pitch.radians, .0)
        self._tfb.send_transform((0.0, 0.0, 0.02), q, now, self._base_frame,
                                 self._footprint_frame)

        # publish base_frame -> head_frame
        q = quaternion_from_euler(.0, -self._cozmo.head_angle.radians, .0)
        self._tfb.send_transform((0.02, 0.0, 0.05), q, now, self._head_frame,
                                 self._base_frame)

        # publish head_frame -> camera_frame
        self._tfb.send_transform((0.025, 0.0, -0.015), (0.0, 0.0, 0.0, 1.0),
                                 now, self._camera_frame, self._head_frame)

        # publish camera_frame -> camera_optical_frame
        q = self._optical_frame_orientation
        self._tfb.send_transform((0.0, 0.0, 0.0), q, now,
                                 self._camera_optical_frame,
                                 self._camera_frame)

        # store last pose
        self._last_pose = deepcopy(self._cozmo.pose)

    def run(self, update_rate=10):
        """
        Publish data continuously with given rate.

        :type   update_rate:    int
        :param  update_rate:    The update rate.

        """

        r = rospy.Rate(update_rate)
        while not rospy.is_shutdown():
            self._publish_tf(update_rate)
            self._publish_image()
            self._publish_objects()
            self._publish_joint_state()
            self._publish_imu()
            self._publish_battery()
            self._publish_odometry()
            self._publish_diagnostics()
            # send message repeatedly to avoid idle mode.
            # This might cause low battery soon
            # TODO improve this!
            self._cozmo.drive_wheels(*self._wheel_vel)
            # sleep
            r.sleep()
        # stop events on cozmo
        self._cozmo.stop_all_motors()
Ejemplo n.º 5
0
class CozmoRos(object):
    """
    The Cozmo ROS driver object.

    """
    def __init__(self, coz):
        """

        :type   coz:    cozmo.Robot
        :param  coz:    The cozmo SDK robot handle (object).
        
        """

        # vars
        self._cozmo = coz
        self._lin_vel = .0
        self._ang_vel = .0
        self._cmd_lin_vel = .0
        self._cmd_ang_vel = .0
        self._last_pose = self._cozmo.pose
        self._wheel_vel = (0, 0)
        self._optical_frame_orientation = quaternion_from_euler(
            -np.pi / 2., .0, -np.pi / 2.)
        self._camera_info_manager = CameraInfoManager(
            'cozmo_camera', namespace='/cozmo_camera')

        # tf
        self._tfb = TransformBroadcaster()

        # params
        self._odom_frame = rospy.get_param('~odom_frame', 'odom')
        self._footprint_frame = rospy.get_param('~footprint_frame',
                                                'base_footprint')
        self._base_frame = rospy.get_param('~base_frame', 'base_link')
        self._head_frame = rospy.get_param('~head_frame', 'head_link')
        self._camera_frame = rospy.get_param('~camera_frame', 'camera_link')
        self._camera_optical_frame = rospy.get_param('~camera_optical_frame',
                                                     'cozmo_camera')
        camera_info_url = rospy.get_param('~camera_info_url', '')

        # pubs
        self._joint_state_pub = rospy.Publisher('joint_states',
                                                JointState,
                                                queue_size=1)
        self._odom_pub = rospy.Publisher('odom', Odometry, queue_size=1)
        self._imu_pub = rospy.Publisher('imu', Imu, queue_size=1)
        self._battery_pub = rospy.Publisher('battery',
                                            BatteryState,
                                            queue_size=1)
        # Note: camera is published under global topic (preceding "/")
        self._image_pub = rospy.Publisher('/cozmo_camera/image',
                                          Image,
                                          queue_size=10)
        self._camera_info_pub = rospy.Publisher('/cozmo_camera/camera_info',
                                                CameraInfo,
                                                queue_size=10)

        # subs
        self._backpack_led_sub = rospy.Subscriber('backpack_led',
                                                  ColorRGBA,
                                                  self._set_backpack_led,
                                                  queue_size=1)
        self._twist_sub = rospy.Subscriber('cmd_vel',
                                           Twist,
                                           self._twist_callback,
                                           queue_size=1)
        self._say_sub = rospy.Subscriber('say',
                                         String,
                                         self._say_callback,
                                         queue_size=1)
        self._head_sub = rospy.Subscriber('head_angle',
                                          Float64,
                                          self._move_head,
                                          queue_size=1)
        self._lift_sub = rospy.Subscriber('lift_height',
                                          Float64,
                                          self._move_lift,
                                          queue_size=1)

        # diagnostics
        self._diag_array = DiagnosticArray()
        self._diag_array.header.frame_id = self._base_frame
        diag_status = DiagnosticStatus()
        diag_status.hardware_id = 'Cozmo Robot'
        diag_status.name = 'Cozmo Status'
        diag_status.values.append(KeyValue(key='Battery Voltage', value=''))
        diag_status.values.append(KeyValue(key='Head Angle', value=''))
        diag_status.values.append(KeyValue(key='Lift Height', value=''))
        self._diag_array.status.append(diag_status)
        self._diag_pub = rospy.Publisher('/diagnostics',
                                         DiagnosticArray,
                                         queue_size=1)

        # camera info manager
        self._camera_info_manager.setURL(camera_info_url)
        self._camera_info_manager.loadCameraInfo()

        # Raise the head
        self.move_head(10)  # level the head
        self.move_lift(0)  # raise the lift

        # Start the tasks
        self.task = 0
        self.goal_pose = self._cozmo.pose
        self.action = None

        self._cozmo.add_event_handler(cozmo.objects.EvtObjectAppeared,
                                      self.handle_object_appeared)
        self._cozmo.add_event_handler(cozmo.objects.EvtObjectDisappeared,
                                      self.handle_object_disappeared)
        self.front_wall = self._cozmo.world.define_custom_wall(
            CustomObjectTypes.CustomType00, CustomObjectMarkers.Hexagons2, 300,
            44, 63, 63, True)
        self.ramp_bottom = self._cozmo.world.define_custom_wall(
            CustomObjectTypes.CustomType01, CustomObjectMarkers.Triangles5,
            100, 100, 63, 63, True)
        self.ramp_top = self._cozmo.world.define_custom_wall(
            CustomObjectTypes.CustomType02, CustomObjectMarkers.Diamonds2, 5,
            5, 44, 44, True)

        self.drop_spot = self._cozmo.world.define_custom_wall(
            CustomObjectTypes.CustomType03, CustomObjectMarkers.Circles4, 70,
            70, 63, 63, True)
        self.back_wall = self._cozmo.world.define_custom_wall(
            CustomObjectTypes.CustomType04, CustomObjectMarkers.Diamonds4, 300,
            50, 63, 63, True)
        self.drop_target = self._cozmo.world.define_custom_wall(
            CustomObjectTypes.CustomType05, CustomObjectMarkers.Hexagons5, 5,
            5, 32, 32, True)
        self.drop_clue = self._cozmo.world.define_custom_wall(
            CustomObjectTypes.CustomType06, CustomObjectMarkers.Triangles3, 5,
            5, 32, 32, True)
        self.front_wall_pose = None
        self.ramp_bottom_pose = None
        self.drop_spot_pose = None
        self.back_wall_pose = None
        self.drop_target_pose = None
        self.drop_clue_pose = None
        self.cube_found = False
        self.target_cube = None

        self.is_up_top = False  # Is Cozmo on the platform

    def _publish_diagnostics(self):
        # alias
        diag_status = self._diag_array.status[0]

        # fill diagnostics array
        battery_voltage = self._cozmo.battery_voltage
        diag_status.values[0].value = '{:.2f} V'.format(battery_voltage)
        diag_status.values[1].value = '{:.2f} deg'.format(
            self._cozmo.head_angle.degrees)
        diag_status.values[2].value = '{:.2f} mm'.format(
            self._cozmo.lift_height.distance_mm)
        if battery_voltage > 3.5:
            diag_status.level = DiagnosticStatus.OK
            diag_status.message = 'Everything OK!'
        elif battery_voltage > 3.4:
            diag_status.level = DiagnosticStatus.WARN
            diag_status.message = 'Battery low! Go charge soon!'
        else:
            diag_status.level = DiagnosticStatus.ERROR
            diag_status.message = 'Battery very low! Cozmo will power off soon!'

        # update message stamp and publish
        self._diag_array.header.stamp = rospy.Time.now()
        self._diag_pub.publish(self._diag_array)

    def _move_head(self, cmd):
        """
        Move head to given angle.
        
        :type   cmd:    Float64
        :param  cmd:    The message containing angle in degrees. [-25 - 44.5]
        
        """
        self.move_head(cmd.data)

    def move_head(self, angle):
        angle = cozmo.util.radians(angle * np.pi / 180.)
        action = self._cozmo.set_head_angle(angle,
                                            duration=0.1,
                                            in_parallel=True)
        action.wait_for_completed()

    def _move_lift(self, cmd):
        """
        Move lift to given height.

        :type   cmd:    Float64
        :param  cmd:    A value between [0 - 1], the SDK auto
                        scales it to the according height.

        """
        self.move_lift(cmd.data)

    def move_lift(self, level, duration=0.2):
        action = self._cozmo.set_lift_height(height=level,
                                             duration=0.2,
                                             in_parallel=True)
        action.wait_for_completed()

    def _set_backpack_led(self, msg):
        """
        Set the color of the backpack LEDs.

        :type   msg:    ColorRGBA
        :param  msg:    The color to be set.

        """
        # setup color as integer values
        color = [int(x * 255) for x in [msg.r, msg.g, msg.b, msg.a]]
        # create lights object with duration
        light = cozmo.lights.Light(cozmo.lights.Color(rgba=color),
                                   on_period_ms=1000)
        # set lights
        self._cozmo.set_all_backpack_lights(light)

    def _twist_callback(self, cmd):
        """
        Set commanded velocities from Twist message.

        The commands are actually send/set during run loop, so delay
        is in worst case up to 1 / update_rate seconds.

        :type   cmd:    Twist
        :param  cmd:    The commanded velocities.

        """
        self.set_velocity(cmd.linear.x, cmd.angular.z)

    def set_velocity(self, linear, angular):
        # compute differential wheel speed
        axle_length = 0.07  # 7cm
        self._cmd_lin_vel = linear
        self._cmd_ang_vel = angular
        rv = self._cmd_lin_vel + (self._cmd_ang_vel * axle_length * 0.5)
        lv = self._cmd_lin_vel - (self._cmd_ang_vel * axle_length * 0.5)
        self._wheel_vel = (lv * 1000., rv * 1000.)  # convert to mm / s

    def _say_callback(self, msg):
        """
        The callback for incoming text messages to be said.

        :type   msg:    String
        :param  msg:    The text message to say.

        """
        self._cozmo.say_text(msg.data).wait_for_completed()

    def _publish_objects(self):
        """
        Publish detected object as transforms between odom_frame and object_frame.

        """

        for obj in self._cozmo.world.visible_objects:
            now = rospy.Time.now()
            x = obj.pose.position.x * 0.001
            y = obj.pose.position.y * 0.001
            z = obj.pose.position.z * 0.001
            q = (obj.pose.rotation.q1, obj.pose.rotation.q2,
                 obj.pose.rotation.q3, obj.pose.rotation.q0)
            self._tfb.send_transform((x, y, z), q, now,
                                     'cube_' + str(obj.object_id),
                                     self._odom_frame)

            try:
                if obj.cube_id and self.target_cube != obj:
                    self._tfb.send_transform((x, y, z), q, now,
                                             'cube_' + str(obj.object_id),
                                             self._odom_frame)
                    print("Found {}".format(obj.cube_id))
                    if not self.cube_found and self.robots_distance_to_object(
                            self._cozmo, obj) < 400:
                        self.target_cube = obj
                        self.cube_found = True
                        print("Locking on to {}".format(obj.cube_id))
                    else:
                        if self.cube_found:
                            print("Found that one already!")
                        else:
                            print("Cube too far away!")

            except:
                # print('OBJECT IS NOT A LIGHT CUBE')
                if (obj == self._cozmo.world.charger):
                    return
                if (obj.object_type == CustomObjectTypes.CustomType00
                        and (self.front_wall_pose == None
                             or not self.front_wall_pose.is_accurate)):
                    self.front_wall_pose = obj.pose
                    self._tfb.send_transform((x, y, z), q, now, 'Front',
                                             self._odom_frame)
                    print('*** Comzmo has found the front wall! ***')
                if (obj.object_type == CustomObjectTypes.CustomType01
                        and (self.ramp_bottom_pose == None
                             or not self.ramp_bottom_pose.is_accurate)):
                    self.ramp_bottom_pose = obj.pose
                    self._tfb.send_transform((x, y, z), q, now, 'Ramp',
                                             self._odom_frame)
                    print('*** Comzmo has found the front wall! ***')
                if (obj.object_type == CustomObjectTypes.CustomType03
                        and (self.drop_spot_pose == None
                             or not self.drop_spot_pose.is_accurate)):
                    self.drop_spot_pose = obj.pose
                    self._tfb.send_transform((x, y, z), q, now, 'Drop',
                                             self._odom_frame)
                    print('*** Comzmo has found the drop Spot! ***')
                if (obj.object_type == CustomObjectTypes.CustomType04
                        and (self.back_wall_pose == None
                             or not self.back_wall_pose.is_accurate)):
                    self.back_wall_pose = obj.pose
                    self._tfb.send_transform((x, y, z), q, now, 'Back',
                                             self._odom_frame)
                    print('*** Comzmo has found the back wall! ***')
                if (obj.object_type == CustomObjectTypes.CustomType05
                        and (self.drop_target_pose == None
                             or not self.drop_target_pose.is_accurate)):
                    self.drop_target_pose = obj.pose
                    self._tfb.send_transform((x, y, z), q, now, 'Target',
                                             self._odom_frame)
                    print('*** Comzmo has found the Dropt Target! ***')
                if (obj.object_type == CustomObjectTypes.CustomType06
                        and (self.drop_clue_pose == None
                             or not self.drop_clue_pose.is_accurate)):
                    self.drop_clue_pose = obj.pose
                    self._tfb.send_transform((x, y, z), q, now, 'Clue',
                                             self._odom_frame)
                    print('*** Comzmo has found the Dropt Clue! ***')

    def _publish_image(self):
        """
        Publish latest camera image as Image with CameraInfo.

        """
        # only publish if we have a subscriber
        if self._image_pub.get_num_connections() == 0:
            return

        # get latest image from cozmo's camera
        camera_image = self._cozmo.world.latest_image
        if camera_image is not None:
            # convert image to gray scale as it is gray although
            img = camera_image.raw_image.convert('L')
            ros_img = Image()
            ros_img.encoding = 'mono8'
            ros_img.width = img.size[0]
            ros_img.height = img.size[1]
            ros_img.step = ros_img.width
            ros_img.data = img.tobytes()
            ros_img.header.frame_id = 'cozmo_camera'
            cozmo_time = camera_image.image_recv_time
            ros_img.header.stamp = rospy.Time.from_sec(cozmo_time)
            # publish images and camera info
            self._image_pub.publish(ros_img)
            camera_info = self._camera_info_manager.getCameraInfo()
            camera_info.header = ros_img.header
            self._camera_info_pub.publish(camera_info)

    def _publish_joint_state(self):
        """
        Publish joint states as JointStates.

        """
        # only publish if we have a subscriber
        if self._joint_state_pub.get_num_connections() == 0:
            return

        js = JointState()
        js.header.stamp = rospy.Time.now()
        js.header.frame_id = 'cozmo'
        js.name = ['head', 'lift']
        js.position = [
            self._cozmo.head_angle.radians,
            self._cozmo.lift_height.distance_mm * 0.001
        ]
        js.velocity = [0.0, 0.0]
        js.effort = [0.0, 0.0]
        self._joint_state_pub.publish(js)

    def _publish_imu(self):
        """
        Publish inertia data as Imu message.

        """
        # only publish if we have a subscriber
        if self._imu_pub.get_num_connections() == 0:
            return

        imu = Imu()
        imu.header.stamp = rospy.Time.now()
        imu.header.frame_id = self._base_frame
        imu.orientation.w = self._cozmo.pose.rotation.q0
        imu.orientation.x = self._cozmo.pose.rotation.q1
        imu.orientation.y = self._cozmo.pose.rotation.q2
        imu.orientation.z = self._cozmo.pose.rotation.q3
        imu.angular_velocity.x = self._cozmo.gyro.x
        imu.angular_velocity.y = self._cozmo.gyro.y
        imu.angular_velocity.z = self._cozmo.gyro.z
        imu.linear_acceleration.x = self._cozmo.accelerometer.x * 0.001
        imu.linear_acceleration.y = self._cozmo.accelerometer.y * 0.001
        imu.linear_acceleration.z = self._cozmo.accelerometer.z * 0.001
        self._imu_pub.publish(imu)

    def _publish_battery(self):
        """
        Publish battery as BatteryState message.

        """
        # only publish if we have a subscriber
        if self._battery_pub.get_num_connections() == 0:
            return

        battery = BatteryState()
        battery.header.stamp = rospy.Time.now()
        battery.voltage = self._cozmo.battery_voltage
        battery.present = True
        if self._cozmo.is_on_charger:  # is_charging always return False
            battery.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_CHARGING
        else:
            battery.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING
        self._battery_pub.publish(battery)

    def _publish_odometry(self):
        """
        Publish current pose as Odometry message.

        """
        # only publish if we have a subscriber
        if self._odom_pub.get_num_connections() == 0:
            return

        now = rospy.Time.now()
        odom = Odometry()
        odom.header.frame_id = self._odom_frame
        odom.header.stamp = now
        odom.child_frame_id = self._footprint_frame
        odom.pose.pose.position.x = self._cozmo.pose.position.x * 0.001
        odom.pose.pose.position.y = self._cozmo.pose.position.y * 0.001
        odom.pose.pose.position.z = self._cozmo.pose.position.z * 0.001
        q = quaternion_from_euler(.0, .0, self._cozmo.pose_angle.radians)
        odom.pose.pose.orientation.x = q[0]
        odom.pose.pose.orientation.y = q[1]
        odom.pose.pose.orientation.z = q[2]
        odom.pose.pose.orientation.w = q[3]
        odom.pose.covariance = np.diag([1e-2, 1e-2, 1e-2, 1e3, 1e3,
                                        1e-1]).ravel()
        odom.twist.twist.linear.x = self._lin_vel
        odom.twist.twist.angular.z = self._ang_vel
        odom.twist.covariance = np.diag([1e-2, 1e3, 1e3, 1e3, 1e3,
                                         1e-2]).ravel()
        self._odom_pub.publish(odom)

    def _publish_tf(self, update_rate):
        """
        Broadcast current transformations and update
        measured velocities for odometry twist.

        Published transforms:

        odom_frame -> footprint_frame
        footprint_frame -> base_frame
        base_frame -> head_frame
        head_frame -> camera_frame
        camera_frame -> camera_optical_frame

        """
        now = rospy.Time.now()
        x = self._cozmo.pose.position.x * 0.001
        y = self._cozmo.pose.position.y * 0.001
        z = self._cozmo.pose.position.z * 0.001

        # compute current linear and angular velocity from pose change
        # Note: Sign for linear velocity is taken from commanded velocities!
        # Note: The angular velocity can also be taken from gyroscopes!
        delta_pose = self._last_pose - self._cozmo.pose
        dist = np.sqrt(delta_pose.position.x**2 + delta_pose.position.y**2 +
                       delta_pose.position.z**2) / 1000.0
        self._lin_vel = dist * update_rate * np.sign(self._cmd_lin_vel)
        self._ang_vel = -delta_pose.rotation.angle_z.radians * update_rate

        # publish odom_frame -> footprint_frame
        q = quaternion_from_euler(.0, .0, self._cozmo.pose_angle.radians)
        self._tfb.send_transform((x, y, 0.0), q, now, self._footprint_frame,
                                 self._odom_frame)

        # publish footprint_frame -> base_frame
        q = quaternion_from_euler(.0, -self._cozmo.pose_pitch.radians, .0)
        self._tfb.send_transform((0.0, 0.0, 0.02), q, now, self._base_frame,
                                 self._footprint_frame)

        # publish base_frame -> head_frame
        q = quaternion_from_euler(.0, -self._cozmo.head_angle.radians, .0)
        self._tfb.send_transform((0.02, 0.0, 0.05), q, now, self._head_frame,
                                 self._base_frame)

        # publish head_frame -> camera_frame
        self._tfb.send_transform((0.025, 0.0, -0.015), (0.0, 0.0, 0.0, 1.0),
                                 now, self._camera_frame, self._head_frame)

        # publish camera_frame -> camera_optical_frame
        q = self._optical_frame_orientation
        self._tfb.send_transform((0.0, 0.0, 0.0), q, now,
                                 self._camera_optical_frame,
                                 self._camera_frame)

        # store last pose
        self._last_pose = deepcopy(self._cozmo.pose)

    def run(self, update_rate=10):
        """
        Publish data continuously with given rate.

        :type   update_rate:    int
        :param  update_rate:    The update rate.

        """
        r = rospy.Rate(update_rate)
        while not rospy.is_shutdown():
            self._publish_tf(update_rate)
            self._publish_image()
            self._publish_objects()
            self._publish_joint_state()
            self._publish_imu()
            self._publish_battery()
            self._publish_odometry()
            self._publish_diagnostics()
            # send message repeatedly to avoid idle mode.
            # This might cause low battery soon
            # TODO improve this!
            if self.is_up_top:
                self.move_cubes()
            else:
                self.climb_ramp()

            self._cozmo.drive_wheels(*self._wheel_vel)
            # sleep
            r.sleep()
        # stop events on cozmo
        self._cozmo.stop_all_motors()

    def move_cubes(self):
        if self.action != None and self.action.is_running:
            return

        if self.task == 0:  # move to a start location
            if (self.drop_spot_pose == None
                    or not self.drop_spot_pose.is_accurate):
                self.move_head(0)
                self.action = self._cozmo.turn_in_place(cozmo.util.degrees(10),
                                                        in_parallel=True)
                print("Oops! Let me find where I am.")
                pass
            else:
                #_loc=self.pose_with_distance_from_target(self._cozmo,self.front_wall,50)
                #print(_loc)
                offset = cozmo.util.Pose(-120, 25, 0, 0, 0, 0, 0)
                offset1 = self.drop_spot_pose.define_pose_relative_this(offset)

                self.action = self._cozmo.go_to_pose(offset1)
                self.cube_found = False
                self.task = 1
                print("*" * 5 + " Moved to start position! " + "*" * 5)
        elif self.task == 1:
            if (not self.cube_found):
                self.action = self._cozmo.turn_in_place(cozmo.util.degrees(20),
                                                        in_parallel=True)
                print("Looking for a cube...")
            else:
                self.action = self._cozmo.pickup_object(
                    self.target_cube, True, False, 2)
                print("Picking up a cube!")
                self.task = 2
        elif self.task == 2:
            if (self.drop_spot_pose == None
                    or not self.drop_spot_pose.is_accurate):
                self.move_head(0)
                self.action = self._cozmo.turn_in_place(cozmo.util.degrees(20),
                                                        in_parallel=True)
                print("Oops! Let me find where I am.")
                pass
            else:
                offset = cozmo.util.Pose(-150, 40, 0, 0, 0, 0, 0)
                offset1 = self.drop_spot_pose.define_pose_relative_this(offset)
                self.action = self._cozmo.go_to_pose(offset1)
                self.task = 3
                print("*" * 5 + " Move to relocalize! " + "*" * 5)
        elif self.task == 3:
            offset = cozmo.util.Pose(-100, 40, 0, 0, 0, 0, 0)
            offset1 = self.drop_spot_pose.define_pose_relative_this(offset)
            self.action = self._cozmo.go_to_pose(offset1)
            #self.action = self._cozmo.drive_straight(cozmo.util.distance_mm(40), cozmo.util.speed_mmps(10), in_parallel=True)
            self.task = 4
            print("*" * 5 + " Moving toward drop position! " + "*" * 5)
        elif self.task == 4:
            self.action = self._cozmo.turn_in_place(cozmo.util.degrees(-90),
                                                    in_parallel=True)
            print("Turning into position...")
            self.task = 5
        elif self.task == 5:
            #self.action = self._cozmo.place_object_on_ground_here(self.target_cube)
            self.action = self._cozmo.drive_straight(
                cozmo.util.distance_mm(-30),
                cozmo.util.speed_mmps(10),
                in_parallel=True)
            self.task = 6
        elif self.task == 6:
            if (self.drop_clue_pose == None
                    or not self.drop_clue_pose.is_accurate):
                self.action = self._cozmo.drive_straight(
                    cozmo.util.distance_mm(-10),
                    cozmo.util.speed_mmps(10),
                    in_parallel=True)
                print("Oops! Let me find the clue.")
                pass
            else:
                offset = cozmo.util.Pose(-50, -60, 0, 0, 0, 0, 0)
                offset1 = self.drop_clue_pose.define_pose_relative_this(offset)
                self.action = self._cozmo.go_to_pose(offset1)
                #self.action = self._cozmo.drive_straight(cozmo.util.distance_mm(40), cozmo.util.speed_mmps(10), in_parallel=True)
                self.task = 7
                print("*" * 5 + " Moving to find drop position! " + "*" * 5)
        elif self.task == 7:
            if (self.drop_target_pose == None
                    or not self.drop_target_pose.is_accurate):
                self.move_head(-25)
                self.action = self._cozmo.drive_straight(
                    cozmo.util.distance_mm(-10),
                    cozmo.util.speed_mmps(10),
                    in_parallel=True)
                print("Oops! Let me find the target.")
                pass
            else:
                self.action = self._cozmo.drive_straight(
                    cozmo.util.distance_mm(15),
                    cozmo.util.speed_mmps(10),
                    in_parallel=True)
                #offset = cozmo.util.Pose(0, 0, -40, 0, 0, 0, 0)
                #offset1 =self.drop_target_pose.define_pose_relative_this(offset)
                #self.action=self._cozmo.go_to_pose(offset1)
                #self.action = self._cozmo.drive_straight(cozmo.util.distance_mm(40), cozmo.util.speed_mmps(10), in_parallel=True)
                self.task = 8
                print("*" * 5 + " Moving to drop position! " + "*" * 5)
        elif self.task == 8:
            #self.action = self._cozmo.place_object_on_ground_here(self.target_cube)
            self.move_lift(0, 1.0)
            self.move_head(0)
            self.action = self._cozmo.drive_straight(
                cozmo.util.distance_mm(-30),
                cozmo.util.speed_mmps(10),
                in_parallel=True)
            self.target_cube = None  # Just placed it, back away
            self.cube_found = False  # Forget about it
            self.task = 9
            print("*" * 5 + " Placed Cube! " + "*" * 5)
        elif self.task == 9:
            if (self.back_wall_pose == None
                    or not self.back_wall_pose.is_accurate):
                self.move_head(10)
                self.action = self._cozmo.turn_in_place(cozmo.util.degrees(20),
                                                        in_parallel=True)
                print("Oops! Let me find where I am.")
            else:
                offset = cozmo.util.Pose(-150, 40, 0, 0, 0, 0, 0)
                offset1 = self.back_wall_pose.define_pose_relative_this(offset)
                self.action = self._cozmo.go_to_pose(offset1)
                self.task = 10
                print("*" * 5 + " Move to relocalize! " + "*" * 5)
        elif self.task == 10:
            if (self.drop_spot_pose == None
                    or not self.drop_spot_pose.is_accurate):
                self.action = self._cozmo.turn_in_place(cozmo.util.degrees(10),
                                                        in_parallel=True)
                print("*" * 5 + " Relocalizing... " + "*" * 5)
            else:
                self.task = 0
                self.back_wall_pose.invalidate()
                self.drop_clue_pose.invalidate()
                self.drop_target_pose.invalidate()
                self.drop_spot_pose.invalidate()

    def climb_ramp(self):
        if self.action != None and self.action.is_running:
            return

        if self.task == 0:  # set goal
            d_x = 50
            d_y = 50
            d_angle = cozmo.util.degrees(0)
            self.goal_pose = cozmo.util.pose_z_angle(d_x, d_y, 0, d_angle, 0)
            self.action = self._cozmo.go_to_pose(self.goal_pose,
                                                 relative_to_robot=True,
                                                 in_parallel=True,
                                                 num_retries=2)

            self.task = 1
            print("*" * 10 + " Step 1 Done! " + "*" * 10)

        elif self.task == 1 and (self.front_wall_pose != None
                                 and self.front_wall_pose.is_accurate):
            #_loc=self.pose_with_distance_from_target(self._cozmo,self.front_wall,50)
            #print(_loc)
            offset = cozmo.util.Pose(-150, 0, 0, 0, 0, 0, 0)
            offset1 = self.front_wall_pose.define_pose_relative_this(offset)

            self.action = self._cozmo.go_to_pose(offset1)
            self.task = 2
            print("*" * 10 + " Step 2 Done! " + "*" * 10)

        elif self.task == 2:  # set goal
            d_x = 0
            d_y = 300
            d_angle = cozmo.util.degrees(-90)
            self.goal_pose = cozmo.util.pose_z_angle(d_x, d_y, 0, d_angle, 0)
            self.action = self._cozmo.go_to_pose(self.goal_pose,
                                                 relative_to_robot=True,
                                                 in_parallel=True,
                                                 num_retries=2)

            self.task = 3
            print("*" * 10 + " Step 3 Done! " + "*" * 10)

        elif self.task == 3 and (self.ramp_bottom_pose != None
                                 and self.ramp_bottom_pose.is_accurate):
            #_loc=self.pose_with_distance_from_target(self._cozmo,self.front_wall,50)
            #print(_loc)
            offset = cozmo.util.Pose(-45, 40, 0, 0, 0, 0, 0)
            offset1 = self.ramp_bottom_pose.define_pose_relative_this(offset)

            self.action = self._cozmo.go_to_pose(offset1)
            self.task = 4
            print("*" * 10 + " Step 4 Done! " + "*" * 10)

        elif self.task == 4:  # move forward
            self.action = self._cozmo.turn_in_place(cozmo.util.degrees(-90),
                                                    in_parallel=True)
            self.task = 5
            print("*" * 10 + " Step 5 Done! " + "*" * 10)

        elif self.task == 5:
            print("*" * 10 + " At the ramp! " + "*" * 10)
            self.action = self._cozmo.drive_straight(
                cozmo.util.distance_mm(200),
                cozmo.util.speed_mmps(10),
                in_parallel=True)
            self.task = 6
            print("*" * 10 + " Step 6 Done! " + "*" * 10)

        elif self.task == 6 and (self.drop_spot_pose != None
                                 and self.drop_spot_pose.is_accurate):
            #_loc=self.pose_with_distance_from_target(self._cozmo,self.front_wall,50)
            #print(_loc)
            offset = cozmo.util.Pose(-200, 20, 0, 0, 0, 0, 0)
            offset1 = self.drop_spot_pose.define_pose_relative_this(offset)

            self.action = self._cozmo.go_to_pose(offset1)
            self.task = 7
            print("*" * 10 + " Step 7 Done! " + "*" * 10)
        elif self.task == 7:
            self.task = 0  # Reset the tasks
            self.is_up_top = True  # Cozmo made it up
            print("*" * 10 + " Made it to the top! " + "*" * 10)

    def handle_object_appeared(self, evt, **kw):
        # This will be called whenever an EvtObjectAppeared is dispatched -
        # whenever an Object comes into view.
        try:
            print("*** Cozmo started seeing a %s" % str(evt.obj.object_type))
            if isinstance(evt.obj, CustomObject):
                print("*** Cozmo started seeing a %s" %
                      str(evt.obj.object_type))
        except:
            print("*** Cozmo found a cube?")

    def handle_object_disappeared(self, evt, **kw):
        # This will be called whenever an EvtObjectDisappeared is dispatched -
        # whenever an Object goes out of view.
        try:
            print("!!! Cozmo stopped seeing a %s" % str(evt.obj.object_type))
            if isinstance(evt.obj, CustomObject):
                print("!!! Cozmo stopped seeing a %s" %
                      str(evt.obj.object_type))
        except:
            print("*** Cozmo lost sight of a cube?")

    def robots_distance_to_object(self, robot, target):
        object_vector = np.array(
            (target.pose.position.x - robot.pose.position.x,
             target.pose.position.y - robot.pose.position.y))
        dist = np.sqrt((object_vector**2).sum())
        return dist

    def pose_with_distance_from_target(self, robot, target, dist):
        dist_scalar = (dist / self.robots_distance_to_object(robot, target))
        dest_pos = target.pose.position + cozmo.util.Vector3(
            (robot.pose.position.x - target.pose.position.x) * dist_scalar,
            (robot.pose.position.y - target.pose.position.y) * dist_scalar,
            0.0)
        return dest_pos
class CozmoRos(object):
    """
    The Cozmo ROS driver object.

    """
    def __init__(self, coz):
        """

        :type   coz:    cozmo.Robot
        :param  coz:    The cozmo SDK robot handle (object).

        """

        # vars
        self._cozmo = coz
        self._lin_vel = .0
        self._ang_vel = .0
        self._cmd_lin_vel = .0
        self._cmd_ang_vel = .0
        self._last_pose = self._cozmo.pose
        self._wheel_vel = (0, 0)
        self._optical_frame_orientation = quaternion_from_euler(
            -np.pi / 2., .0, -np.pi / 2.)
        self._camera_info_manager = CameraInfoManager(
            'cozmo_camera', namespace='/cozmo_camera')

        self.loc1Found = False
        self.loc2Found = False
        self.loc3Found = False
        self.rampFound = False
        self.allLocFound = False
        self.topBackWallFound = False
        self.frontOfBoxFound = False
        self.topSideWallFound = False
        self.readyToPickUp = False
        self.deliveredCube1 = None
        self.deliveredCube2 = None
        self.deliveredCube3 = None
        # tf
        self._tfb = TransformBroadcaster()

        # params
        self._odom_frame = rospy.get_param('~odom_frame', 'odom')
        self._footprint_frame = rospy.get_param('~footprint_frame',
                                                'base_footprint')
        self._base_frame = rospy.get_param('~base_frame', 'base_link')
        self._head_frame = rospy.get_param('~head_frame', 'head_link')
        self._camera_frame = rospy.get_param('~camera_frame', 'camera_link')
        self._camera_optical_frame = rospy.get_param('~camera_optical_frame',
                                                     'cozmo_camera')
        camera_info_url = rospy.get_param('~camera_info_url', '')

        # pubs
        self._joint_state_pub = rospy.Publisher('joint_states',
                                                JointState,
                                                queue_size=1)
        self._odom_pub = rospy.Publisher('odom', Odometry, queue_size=1)
        self._imu_pub = rospy.Publisher('imu', Imu, queue_size=1)
        self._battery_pub = rospy.Publisher('battery',
                                            BatteryState,
                                            queue_size=1)
        # Note: camera is published under global topic (preceding "/")
        self._image_pub = rospy.Publisher('/cozmo_camera/image',
                                          Image,
                                          queue_size=10)
        self._camera_info_pub = rospy.Publisher('/cozmo_camera/camera_info',
                                                CameraInfo,
                                                queue_size=10)

        # subs
        self._backpack_led_sub = rospy.Subscriber('backpack_led',
                                                  ColorRGBA,
                                                  self._set_backpack_led,
                                                  queue_size=1)
        self._twist_sub = rospy.Subscriber('cmd_vel',
                                           Twist,
                                           self._twist_callback,
                                           queue_size=1)
        self._say_sub = rospy.Subscriber('say',
                                         String,
                                         self._say_callback,
                                         queue_size=1)
        self._head_sub = rospy.Subscriber('head_angle',
                                          Float64,
                                          self._move_head,
                                          queue_size=1)
        self._lift_sub = rospy.Subscriber('lift_height',
                                          Float64,
                                          self._move_lift,
                                          queue_size=1)

        # diagnostics
        self._diag_array = DiagnosticArray()
        self._diag_array.header.frame_id = self._base_frame
        diag_status = DiagnosticStatus()
        diag_status.hardware_id = 'Cozmo Robot'
        diag_status.name = 'Cozmo Status'
        diag_status.values.append(KeyValue(key='Battery Voltage', value=''))
        diag_status.values.append(KeyValue(key='Head Angle', value=''))
        diag_status.values.append(KeyValue(key='Lift Height', value=''))
        self._diag_array.status.append(diag_status)
        self._diag_pub = rospy.Publisher('/diagnostics',
                                         DiagnosticArray,
                                         queue_size=1)

        # camera info manager
        self._camera_info_manager.setURL(camera_info_url)
        self._camera_info_manager.loadCameraInfo()
        self.twist = Twist()
        self.custom_objects()
#self.image_sub = rospy.Subscriber('camera/rgb/image_raw',

#							   Image, self.image_callback)

    def custom_objects(self):
        # Add event handlers for whenever Cozmo sees a new object
        #robot.add_event_handler(cozmo.objects.EvtObjectAppeared, handle_object_appeared)
        #robot.add_event_handler(cozmo.objects.EvtObjectDisappeared, handle_object_disappeared)

        self.targetLocation1 = self._cozmo.world.define_custom_cube(
            CustomObjectTypes.CustomType00, CustomObjectMarkers.Hexagons3, 5,
            63, 63, True)

        self.targetLocation2 = self._cozmo.world.define_custom_cube(
            CustomObjectTypes.CustomType01, CustomObjectMarkers.Triangles2, 5,
            63, 63, True)

        self.targetLocation3 = self._cozmo.world.define_custom_cube(
            CustomObjectTypes.CustomType02, CustomObjectMarkers.Circles2, 5,
            63, 63, True)
        self.ramp = self._cozmo.world.define_custom_cube(
            CustomObjectTypes.CustomType03, CustomObjectMarkers.Diamonds2, 1,
            45, 45, True)
        self.topBackWall = self._cozmo.world.define_custom_cube(
            CustomObjectTypes.CustomType04, CustomObjectMarkers.Diamonds4, 1,
            45, 45, True)
        self.frontOfBox = self._cozmo.world.define_custom_cube(
            CustomObjectTypes.CustomType05, CustomObjectMarkers.Hexagons2, 1,
            63, 63, True)
        self.topSideWall = self._cozmo.world.define_custom_cube(
            CustomObjectTypes.CustomType06, CustomObjectMarkers.Circles4, 1,
            45, 45, True)

        if ((self.targetLocation1 is not None)
                and (self.targetLocation2 is not None)
                and (self.targetLocation3 is not None)
                and (self.ramp is not None)):
            print("All objects defined successfully!")
        else:
            print("One or more object definitions failed!")
            return

    def _publish_diagnostics(self):
        # alias
        diag_status = self._diag_array.status[0]

        # fill diagnostics array
        battery_voltage = self._cozmo.battery_voltage
        diag_status.values[0].value = '{:.2f} V'.format(battery_voltage)
        diag_status.values[1].value = '{:.2f} deg'.format(
            self._cozmo.head_angle.degrees)
        diag_status.values[2].value = '{:.2f} mm'.format(
            self._cozmo.lift_height.distance_mm)
        if battery_voltage > 3.5:
            diag_status.level = DiagnosticStatus.OK
            diag_status.message = 'Everything OK!'
        elif battery_voltage > 3.4:
            diag_status.level = DiagnosticStatus.WARN
            diag_status.message = 'Battery low! Go charge soon!'
        else:
            diag_status.level = DiagnosticStatus.ERROR
            diag_status.message = 'Battery very low! Cozmo will power off soon!'

        # update message stamp and publish
        self._diag_array.header.stamp = rospy.Time.now()
        self._diag_pub.publish(self._diag_array)
        """
        Move lift to given height.
        """

    def _resetHead(self):
        action1 = self._cozmo.set_head_angle(radians(0.20),
                                             duration=0.1,
                                             in_parallel=True)
        action1.wait_for_completed()

        action2 = self._cozmo.set_lift_height(0,
                                              duration=0.2,
                                              in_parallel=True)
        action2.wait_for_completed()

    def _move_head(self, cmd):
        """
        Move head to given angle.

        :type   cmd:    Float64
        :param  cmd:    The message containing angle in degrees. [-25 - 44.5]

        """
        action = self._cozmo.set_head_angle(radians(cmd.data * np.pi / 180.),
                                            duration=0.1,
                                            in_parallel=True)
        action.wait_for_completed()

    def _move_lift(self, cmd):
        """
        Move lift to given height.

        :type   cmd:    Float64
        :param  cmd:    A value between [0 - 1], the SDK auto
                        scales it to the according height.

        """
        action = self._cozmo.set_lift_height(height=cmd.data,
                                             duration=0.2,
                                             in_parallel=True)
        action.wait_for_completed()

    def _set_backpack_led(self, msg):
        """
        Set the color of the backpack LEDs.

        :type   msg:    ColorRGBA
        :param  msg:    The color to be set.

        """
        # setup color as integer values
        color = [int(x * 255) for x in [msg.r, msg.g, msg.b, msg.a]]
        # create lights object with duration
        light = cozmo.lights.Light(cozmo.lights.Color(rgba=color),
                                   on_period_ms=1000)
        # set lights
        self._cozmo.set_all_backpack_lights(light)

    def _twist_callback(self, cmd):
        """
        Set commanded velocities from Twist message.

        The commands are actually send/set during run loop, so delay
        is in worst case up to 1 / update_rate seconds.

        :type   cmd:    Twist
        :param  cmd:    The commanded velocities.

        """
        # compute differential wheel speed
        axle_length = 0.07  # 7cm
        self._cmd_lin_vel = cmd.linear.x
        self._cmd_ang_vel = cmd.angular.z
        rv = self._cmd_lin_vel + (self._cmd_ang_vel * axle_length * 0.5)
        lv = self._cmd_lin_vel - (self._cmd_ang_vel * axle_length * 0.5)
        self._wheel_vel = (lv * 1000., rv * 1000.)  # convert to mm / s

    def _say_callback(self, msg):
        """
        The callback for incoming text messages to be said.

        :type   msg:    String
        :param  msg:    The text message to say.

        """
        self._cozmo.say_text(msg.data).wait_for_completed()

    def robots_distance_to_object(self, robot, target):
        object_vector = np.array(
            (target.pose.position.x - robot.pose.position.x,
             target.pose.position.y - robot.pose.position.y))
        dist = np.sqrt((object_vector**2).sum())
        return dist

    def pose_with_distance_from_target(self, robot, target, dist):
        dist_scalar = (dist / self.robots_distance_to_object(robot, target))
        dest_pos = target.pose.position + cozmo.util.Vector3(
            (robot.pose.position.x - target.pose.position.x) * dist_scalar,
            (robot.pose.position.y - target.pose.position.y) * dist_scalar,
            0.0)
        return dest_pos

    def _publish_objects(self):
        """
        Publish detected object as transforms between odom_frame and object_frame.

        """

        for obj in self._cozmo.world.visible_objects:
            now = rospy.Time.now()
            x = obj.pose.position.x * 0.001
            y = obj.pose.position.y * 0.001
            z = obj.pose.position.z * 0.001
            q = (obj.pose.rotation.q1, obj.pose.rotation.q2,
                 obj.pose.rotation.q3, obj.pose.rotation.q0)
            #self._cozmo.go_to_pose(Pose(obj.pose.x/2,obj.pose.y/2,obj.pose.z/2,obj.pose.rotation.q1, obj.pose.rotation.q2, obj.pose.rotation.q3, obj.pose.rotation.q0)).wait_for_completed()

            try:

                if (obj.cube_id):
                    self._tfb.send_transform((x, y, z), q, now,
                                             'cube_' + str(obj.object_id),
                                             self._odom_frame)
                    # print('found cube '+str(obj.cube_id))
                    if (self.allLocFound
                            and self.readyToPickUp):  #self.allLocFound
                        num = obj.cube_id

                        # if(not self._checkDistToLoc(num,obj)):
                        #     print('cube '+ str(obj.cube_id)+ 'has already been delivered')
                        #     return
                        if (not self._checkDistToSelf(obj)):
                            print("cube too far")
                            continue

                        self.action = self._cozmo.pickup_object(
                            obj, True, False, 2
                        )  #dock_with_cube(obj, approach_angle=cozmo.util.degrees(0), num_retries=2)
                        self.action.wait_for_completed()
                        if (self.action.has_succeeded):
                            if (num == 1):
                                if (self.loc1Pose.is_accurate):
                                    #
                                    if (self.deliveredCube1 == None):
                                        offset = cozmo.util.Pose(
                                            -100, 50, 0, 0, 0, 0, 0)
                                        offset1 = self.targetLocation1.pose.define_pose_relative_this(
                                            offset)
                                        self.action = self._cozmo.go_to_pose(
                                            offset1)
                                        self.action.wait_for_completed()
                                        if (self.action.has_succeeded):
                                            self.action = self._cozmo.place_object_on_ground_here(
                                                obj)
                                            self.action.wait_for_completed()
                                            self.readyToPickUp = False
                                            self.deliveredCube1 = obj

                                    else:
                                        offset = cozmo.util.Pose(
                                            -100, 0, 0, 0, 0, 0, 0)
                                        offset1 = self.targetLocation1.pose.define_pose_relative_this(
                                            offset)
                                        self.action = self._cozmo.go_to_pose(
                                            offset1)
                                        self.action.wait_for_completed()
                                        if (self.action.has_succeeded):
                                            self.action = self._cozmo.place_object_on_ground_here(
                                                obj)
                                            self.action.wait_for_completed()
                                            print("delivered 2nd cube")
                                            self.readyToPickUp = False

                                        else:
                                            self.action2 = self._cozmo.set_lift_height(
                                                0,
                                                duration=0.5,
                                                in_parallel=True)
                                            self.action2.wait_for_completed()
                                            self.readyToPickUp = False

                                else:
                                    print(
                                        "loc1 is no longer accurate, need to relocate"
                                    )
                                self.action = self._cozmo.go_to_pose(
                                    cozmo.util.Pose(0, 0, 0, 0, 0, 0, 0))
                                self.action.wait_for_completed()
                                self.allLocFound = False
                                self.frontOfBoxFound = False
                                self.loc1Found = False
                                self.loc2Found = False
                                self.loc3Found = False
                                self._resetHead()

                            elif (num == 2):
                                if (self.loc2Pose.is_accurate):
                                    #
                                    if (self.deliveredCube2 == None):
                                        offset = cozmo.util.Pose(
                                            -100, -50, 0, 0, 0, 0, 0)
                                        offset1 = self.targetLocation2.pose.define_pose_relative_this(
                                            offset)
                                        self.action = self._cozmo.go_to_pose(
                                            offset1)
                                        self.action.wait_for_completed()
                                        if (self.action.has_succeeded):
                                            self.action = self._cozmo.place_object_on_ground_here(
                                                obj)
                                            self.action.wait_for_completed()
                                            self.readyToPickUp = False
                                            self.deliveredCube2 = obj
                                    else:
                                        offset = cozmo.util.Pose(
                                            -100, 50, 0, 0, 0, 0, 0)
                                        offset1 = self.targetLocation2.pose.define_pose_relative_this(
                                            offset)
                                        self.action = self._cozmo.go_to_pose(
                                            offset1)
                                        self.action.wait_for_completed()
                                        if (self.action.has_succeeded):
                                            self.action = self._cozmo.place_object_on_ground_here(
                                                obj)
                                            self.action.wait_for_complete()
                                            print("delivered 2nd cube")
                                        else:
                                            self.action2 = self._cozmo.set_lift_height(
                                                0,
                                                duration=0.5,
                                                in_parallel=True)
                                            self.action2.wait_for_completed()
                                            self.readyToPickUp = False

                                else:
                                    print(
                                        "the side wall is no longer accurate, need to relocate"
                                    )
                                self.action = self._cozmo.go_to_pose(
                                    cozmo.util.Pose(0, 0, 0, 0, 0, 0, 0))
                                self.action.wait_for_completed()
                                self.allLocFound = False
                                self.frontOfBoxFound = False
                                self.loc1Found = False
                                self.loc2Found = False
                                self.loc3Found = False
                                self._resetHead()

                            elif (num == 3):
                                if (self.loc3Pose.is_accurate):
                                    #
                                    if (self.deliveredCube3 == None):
                                        offset = cozmo.util.Pose(
                                            -120, -70, 0, 0, 0, 0, 0)
                                        offset1 = self.targetLocation3.pose.define_pose_relative_this(
                                            offset)
                                        self.action = self._cozmo.go_to_pose(
                                            offset1)
                                        self.action.wait_for_completed()
                                        if (self.action.has_succeeded):
                                            self.action = self._cozmo.place_object_on_ground_here(
                                                obj)
                                            self.action.wait_for_completed()
                                            self.readyToPickUp = False
                                            self.deliveredCube3 = obj

                                    else:
                                        offset = cozmo.util.Pose(
                                            -120, 70, 0, 0, 0, 0, 0)
                                        offset1 = self.targetLocation3.pose.define_pose_relative_this(
                                            offset)
                                        self.action = self._cozmo.go_to_pose(
                                            offset1)
                                        self.action.wait_for_completed()
                                        if (self.action.has_succeeded):
                                            self.action = self._cozmo.place_object_on_ground_here(
                                                obj)
                                            self.action.wait_for_completed()
                                            self.readyToPickUp = False
                                            print("delivered 2nd cube")
                                        else:
                                            self.action2 = self._cozmo.set_lift_height(
                                                0,
                                                duration=0.5,
                                                in_parallel=True)
                                            self.action2.wait_for_completed()
                                            self.readyToPickUp = False

                                else:
                                    print(
                                        "the side wall is no longer accurate, need to relocate"
                                    )
                                self.action = self._cozmo.go_to_pose(
                                    cozmo.util.Pose(0, 0, 0, 0, 0, 0, 0))
                                self.action.wait_for_completed()
                                self.allLocFound = False
                                self.frontOfBoxFound = False
                                self.loc1Found = False
                                self.loc2Found = False
                                self.loc3Found = False
                                self._resetHead()

                            print("result:", self.action.result)
                        else:
                            self.readyToPickUp = False

            except:
                # print('OBJECT IS NOT A LIGHT CUBE')
                if (obj == self._cozmo.world.charger):
                    continue
                if (obj.object_type == CustomObjectTypes.CustomType03
                        and (not self.rampFound)):
                    self.rampPose = obj.pose
                    self.rampFound = True
                    self._tfb.send_transform((x, y, z), q, now, 'Ramp',
                                             self._odom_frame)
                    print('comzmo has found ramp')

                elif (obj.object_type == CustomObjectTypes.CustomType00
                      and (not self.loc1Found)):
                    self.loc1Pose = obj.pose
                    self.targetLocation1 = obj
                    self.loc1Found = True
                    self._tfb.send_transform((x, y, z), q, now, 'Endpoint 1',
                                             self._odom_frame)
                    print('comzmo has found loc1')

                elif (obj.object_type == CustomObjectTypes.CustomType01
                      and (not self.loc2Found)):
                    self.loc2Pose = obj.pose
                    self.loc2Found = True
                    self.targetLocation2 = obj
                    self._tfb.send_transform((x, y, z), q, now, 'Endpoint 2',
                                             self._odom_frame)
                    print('comzmo has found loc2')

                elif (obj.object_type == CustomObjectTypes.CustomType02
                      and (not self.loc3Found)):
                    self.loc3Pose = obj.pose
                    self.loc3Found = True
                    self.targetLocation3 = obj
                    self._tfb.send_transform((x, y, z), q, now, 'Endpoint 3',
                                             self._odom_frame)
                    print('comzmo has found loc3')
                elif (obj.object_type == CustomObjectTypes.CustomType04
                      and (not self.topBackWall)):
                    self.topBackPose = obj.pose
                    self.topBackWallFound = True
                    self.topBackWall = obj

                    self._tfb.send_transform((x, y, z), q, now, 'topBackWall',
                                             self._odom_frame)
                    print('comzmo has found topBackWall')
                elif (obj.object_type == CustomObjectTypes.CustomType05
                      and (not self.frontOfBoxFound)):
                    self.frontOfBoxPose = obj.pose
                    self.frontOfBoxFound = True
                    self.frontOfBox = obj
                    offset = cozmo.util.Pose(-190, -130, 0, 0, 0, 0, 0)
                    self.observationPose = self.frontOfBox.pose.define_pose_relative_this(
                        offset)
                    self._tfb.send_transform((x, y, z), q, now, 'frontOfBox',
                                             self._odom_frame)
                    print('comzmo has found frontOfBox')
                elif (obj.object_type == CustomObjectTypes.CustomType06
                      and (not self.topSideWallFound
                           or not self.topSideWall.pose.is_accurate)):
                    self.topSideWallPose = obj.pose
                    self.topSideWallFound = True
                    self.topSideWall = obj
                    self._tfb.send_transform((x, y, z), q, now, 'topSideWallk',
                                             self._odom_frame)
                    print('comzmo has found topSideWalk')

            if (self.loc1Found and self.loc2Found and self.loc3Found
                    and self.frontOfBoxFound):
                self.allLocFound = True
                # if(self.frontOfBoxFound)
                # self.go_to_pose
            # else:
            #     self._cozmo.turn_in_place(degrees(20),in_parallel=True)

    def _publish_image(self):
        """
        Publish latest camera image as Image with CameraInfo.
        #
        """
        # only publish if we have a subscriber
        if self._image_pub.get_num_connections() == 0:
            return

        # get latest image from cozmo's camera
        camera_image = self._cozmo.world.latest_image
        if camera_image is not None:
            # convert image to gray scale as it is gray although
            img = camera_image.raw_image.convert('L')
            ros_img = Image()
            ros_img.encoding = 'mono8'
            ros_img.width = img.size[0]
            ros_img.height = img.size[1]
            ros_img.step = ros_img.width
            ros_img.data = img.tobytes()
            ros_img.header.frame_id = 'cozmo_camera'
            cozmo_time = camera_image.image_recv_time
            ros_img.header.stamp = rospy.Time.from_sec(cozmo_time)
            # publish images and camera info
            self._image_pub.publish(ros_img)
            camera_info = self._camera_info_manager.getCameraInfo()
            camera_info.header = ros_img.header
            self._camera_info_pub.publish(camera_info)

    def _publish_joint_state(self):
        """
        Publish joint states as JointStates.

        """
        # only publish if we have a subscriber
        if self._joint_state_pub.get_num_connections() == 0:
            return

        js = JointState()
        js.header.stamp = rospy.Time.now()
        js.header.frame_id = 'cozmo'
        js.name = ['head', 'lift']
        js.position = [
            self._cozmo.head_angle.radians,
            self._cozmo.lift_height.distance_mm * 0.001
        ]
        js.velocity = [0.0, 0.0]
        js.effort = [0.0, 0.0]
        self._joint_state_pub.publish(js)

    def _publish_imu(self):
        """
        Publish inertia data as Imu message.

        """
        # only publish if we have a subscriber
        if self._imu_pub.get_num_connections() == 0:
            return

        imu = Imu()
        imu.header.stamp = rospy.Time.now()
        imu.header.frame_id = self._base_frame
        imu.orientation.w = self._cozmo.pose.rotation.q0
        imu.orientation.x = self._cozmo.pose.rotation.q1
        imu.orientation.y = self._cozmo.pose.rotation.q2
        imu.orientation.z = self._cozmo.pose.rotation.q3
        imu.angular_velocity.x = self._cozmo.gyro.x
        imu.angular_velocity.y = self._cozmo.gyro.y
        imu.angular_velocity.z = self._cozmo.gyro.z
        imu.linear_acceleration.x = self._cozmo.accelerometer.x * 0.001
        imu.linear_acceleration.y = self._cozmo.accelerometer.y * 0.001
        imu.linear_acceleration.z = self._cozmo.accelerometer.z * 0.001
        self._imu_pub.publish(imu)

    def _publish_battery(self):
        """
        Publish battery as BatteryState message.

        """
        # only publish if we have a subscriber
        if self._battery_pub.get_num_connections() == 0:
            return

        battery = BatteryState()
        battery.header.stamp = rospy.Time.now()
        battery.voltage = self._cozmo.battery_voltage
        battery.present = True
        if self._cozmo.is_on_charger:  # is_charging always return False
            battery.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_CHARGING
        else:
            battery.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING
        self._battery_pub.publish(battery)

    def _publish_odometry(self):
        """
        Publish current pose as Odometry message.

        """
        # only publish if we have a subscriber
        if self._odom_pub.get_num_connections() == 0:
            return

        now = rospy.Time.now()
        odom = Odometry()
        odom.header.frame_id = self._odom_frame
        odom.header.stamp = now
        odom.child_frame_id = self._footprint_frame
        odom.pose.pose.position.x = self._cozmo.pose.position.x * 0.001
        odom.pose.pose.position.y = self._cozmo.pose.position.y * 0.001
        odom.pose.pose.position.z = self._cozmo.pose.position.z * 0.001
        q = quaternion_from_euler(.0, .0, self._cozmo.pose_angle.radians)
        odom.pose.pose.orientation.x = q[0]
        odom.pose.pose.orientation.y = q[1]
        odom.pose.pose.orientation.z = q[2]
        odom.pose.pose.orientation.w = q[3]
        odom.pose.covariance = np.diag([1e-2, 1e-2, 1e-2, 1e3, 1e3,
                                        1e-1]).ravel()
        odom.twist.twist.linear.x = self._lin_vel
        odom.twist.twist.angular.z = self._ang_vel
        odom.twist.covariance = np.diag([1e-2, 1e3, 1e3, 1e3, 1e3,
                                         1e-2]).ravel()
        self._odom_pub.publish(odom)

    def _publish_tf(self, update_rate):
        """
        Broadcast current transformations and update
        measured velocities for odometry twist.

        Published transforms:

        odom_frame -> footprint_frame
        footprint_frame -> base_frame
        base_frame -> head_frame
        head_frame -> camera_frame
        camera_frame -> camera_optical_frame

        """
        now = rospy.Time.now()
        x = self._cozmo.pose.position.x * 0.001
        y = self._cozmo.pose.position.y * 0.001
        z = self._cozmo.pose.position.z * 0.001

        # compute current linear and angular velocity from pose change
        # Note: Sign for linear velocity is taken from commanded velocities!
        # Note: The angular velocity can also be taken from gyroscopes!
        delta_pose = self._last_pose - self._cozmo.pose
        dist = np.sqrt(delta_pose.position.x**2 + delta_pose.position.y**2 +
                       delta_pose.position.z**2) / 1000.0
        self._lin_vel = dist * update_rate * np.sign(self._cmd_lin_vel)
        self._ang_vel = -delta_pose.rotation.angle_z.radians * update_rate

        # publish odom_frame -> footprint_frame
        q = quaternion_from_euler(.0, .0, self._cozmo.pose_angle.radians)
        self._tfb.send_transform((x, y, 0.0), q, now, self._footprint_frame,
                                 self._odom_frame)

        # publish footprint_frame -> base_frame
        q = quaternion_from_euler(.0, -self._cozmo.pose_pitch.radians, .0)
        self._tfb.send_transform((0.0, 0.0, 0.02), q, now, self._base_frame,
                                 self._footprint_frame)

        # publish base_frame -> head_frame
        q = quaternion_from_euler(.0, -self._cozmo.head_angle.radians, .0)
        self._tfb.send_transform((0.02, 0.0, 0.05), q, now, self._head_frame,
                                 self._base_frame)

        # publish head_frame -> camera_frame
        self._tfb.send_transform((0.025, 0.0, -0.015), (0.0, 0.0, 0.0, 1.0),
                                 now, self._camera_frame, self._head_frame)

        # publish camera_frame -> camera_optical_frame
        q = self._optical_frame_orientation
        self._tfb.send_transform((0.0, 0.0, 0.0), q, now,
                                 self._camera_optical_frame,
                                 self._camera_frame)

        # store last pose
        self._last_pose = deepcopy(self._cozmo.pose)

    def _checkDistToLoc(self, num, obj):
        if (num == 1):
            p1 = np.array([
                obj.pose.position.x, obj.pose.position.y, obj.pose.position.z
            ])
            p2 = np.array([
                self.loc1Pose.position.x, self.loc1Pose.position.y,
                self.loc1Pose.position.z
            ])
            #print(self.loc1Pose.position)
            #print(obj.pose.position)
            squared_dis = ((p2[0] - p1[0])**2) + ((p2[1] - p1[1])**2)
            dist = np.sqrt(squared_dis)
            #print(dist)
        elif (num == 2):
            p1 = np.array([
                obj.pose.position.x, obj.pose.position.y, obj.pose.position.z
            ])
            p2 = np.array([
                self.loc2Pose.position.x, self.loc2Pose.position.y,
                self.loc2Pose.position.z
            ])
            squared_dis = ((p2[0] - p1[0])**2) + ((p2[1] - p1[1])**2)
            dist = np.sqrt(squared_dis)
            #print(dist)

        elif (num == 3):
            p1 = np.array([
                obj.pose.position.x, obj.pose.position.y, obj.pose.position.z
            ])
            p2 = np.array([
                self.loc3Pose.position.x, self.loc3Pose.position.y,
                self.loc3Pose.position.z
            ])
            squared_dis = ((p2[0] - p1[0])**2) + ((p2[1] - p1[1])**2)
            dist = np.sqrt(squared_dis)
            #print(dist)
        if (dist < 100):
            return False
        else:
            return True

    def _checkDistToSelf(self, obj):
        p1 = np.array(
            [obj.pose.position.x, obj.pose.position.y, obj.pose.position.z])
        p2 = np.array([
            self._cozmo.pose.position.x, self._cozmo.pose.position.y,
            self._cozmo.pose.position.z
        ])
        dif = abs(self.frontOfBox.pose.position.x - obj.pose.position.x)
        #print(self.loc1Pose.position)
        #print(obj.pose.position)
        squared_dis = ((p2[0] - p1[0])**2) + ((p2[1] - p1[1])**2)
        dist = np.sqrt(squared_dis)
        print(self.frontOfBox.pose.position)
        print(obj.pose.position)
        print(dist)
        print(dif)
        if (dist > 235):
            return False
        return True

    def _findlocations(self):
        if (self.allLocFound == False):
            self.action = self._cozmo.turn_in_place(degrees(20),
                                                    in_parallel=True)
            self.action.wait_for_completed()
        else:
            if (not self.readyToPickUp):
                print("locs found moving to ready ")
                self.action = self._cozmo.go_to_pose(self.observationPose)
                self.action.wait_for_completed()
                self.readyToPickUp = True
                self._resetHead()

    def run(self, update_rate=10):
        """
        Publish data continuously with given rate.

        :type   update_rate:    int
        :param  update_rate:    The update rate.

        """
        r = rospy.Rate(update_rate)
        self._resetHead()
        while not rospy.is_shutdown():
            self._publish_tf(update_rate)
            self._findlocations()
            self._publish_image()
            self._publish_objects()
            self._publish_joint_state()
            self._publish_imu()
            self._publish_battery()
            self._publish_odometry()
            self._publish_diagnostics()

            # if(self.allLocFound==False):
            #     self._cozmo.turn_in_place(degrees(20))

            # send message repeatedly to avoid idle mode.
            # This might cause low battery soon
            # TODO improve this!    if(not self.allLocFound):

            self._cozmo.drive_wheels(*self._wheel_vel)
            # sleep
            r.sleep()
        # stop events on cozmo
        self._cozmo.stop_all_motors()
Ejemplo n.º 7
0
    def __init__(self,
                fixed_transforms_dict=None):
        self.pubs = {}
        self.models = {}
        self.pnp_solvers = {}
        self.pub_dimension = {}
        self.draw_colors = {}
        self.dimensions = {}
        self.class_ids = {}
        self.model_transforms = {}
        self.meshes = {}
        self.mesh_scales = {}
        self.cv_bridge = CvBridge()
        self.mesh_clouds = {}
        
        self.input_is_rectified = rospy.get_param('input_is_rectified', True)
        self.downscale_height = rospy.get_param('downscale_height', 500)

        self.config_detect = lambda: None
        self.config_detect.mask_edges = 1
        self.config_detect.mask_faces = 1
        self.config_detect.vertex = 1
        self.config_detect.threshold = 0.5
        self.config_detect.softmax = 1000
        self.config_detect.thresh_angle = rospy.get_param('thresh_angle', 0.5)
        self.config_detect.thresh_map = rospy.get_param('thresh_map', 0.01)
        self.config_detect.sigma = rospy.get_param('sigma', 3)
        self.config_detect.thresh_points = rospy.get_param("thresh_points", 0.1)
        self.downsampling_leaf_size = rospy.get_param('~downsampling_leaf_size', 0.02)
        self.fixed_transforms_dict = fixed_transforms_dict

        # For each object to detect, load network model, create PNP solver, and start ROS publishers
        for model, weights_url in rospy.get_param('weights').items():
            self.models[model] = \
                ModelData(
                    model,
                    resource_retriever.get_filename(weights_url, use_protocol=False)
                )
            self.models[model].load_net_model()

            try:
                M = np.array(rospy.get_param('model_transforms')[model], dtype='float64')
                self.model_transforms[model] = tf.transformations.quaternion_from_matrix(M)
            except KeyError:
                self.model_transforms[model] = np.array([0.0, 0.0, 0.0, 1.0], dtype='float64')

            try:
                self.meshes[model] = rospy.get_param('meshes')[model]
            except KeyError:
                pass

            try:
                self.mesh_scales[model] = rospy.get_param('mesh_scales')[model]
            except KeyError:
                self.mesh_scales[model] = 1.0

            try:
                cloud = PlyData.read(rospy.get_param('meshes_ply')[model]).elements[0].data
                cloud = np.transpose(np.vstack((cloud['x'], cloud['y'], cloud['z'])))
                if self.fixed_transforms_dict is None:
                    fixed_transform = np.eye(4)
                else:
                    fixed_transform = np.transpose(np.array(self.fixed_transforms_dict[model]))
                    fixed_transform[:3,3] = [i/100 for i in fixed_transform[:3,3]]
                # fixed_transform = np.linalg.inv(fixed_transform)
                if model == "coke_bottle" or model == "sprite_bottle":
                    fixed_transform[1,3] = 0.172

                print("Fixed transform : {}".format(fixed_transform))
                
                cloud_pose = pcl.PointCloud()
                cloud_pose.from_array(cloud)
                sor = cloud_pose.make_voxel_grid_filter()
                sor.set_leaf_size(self.downsampling_leaf_size, self.downsampling_leaf_size, self.downsampling_leaf_size)
                cloud_pose = sor.filter()

                self.mesh_clouds[model] = self.transform_cloud(np.asarray(cloud_pose), mat=fixed_transform)
                # self.mesh_clouds[model] = np.asarray(cloud_pose)
                # Points x 3 for dim of below
                rospy.logwarn("Loaded mesh cloud for : {} with size : {}, scaling : {}".format(model, cloud.shape[0], self.mesh_scales[model]))
                # scale_transform = tf.transformations.scale_matrix(self.mesh_scales[model])
                # cloud = np.hstack((cloud, np.ones((cloud.shape[0], 1))))
                # cloud = np.matmul(scale_transform, np.transpose(cloud))
                # self.mesh_clouds[model] = np.transpose(cloud)[:, :3]
            except KeyError:
                rospy.logwarn("Couldn't load mesh ply")
                pass

            self.draw_colors[model] = tuple(rospy.get_param("draw_colors")[model])
            self.dimensions[model] = tuple(rospy.get_param("dimensions")[model])
            self.class_ids[model] = rospy.get_param("class_ids")[model]

            self.pnp_solvers[model] = \
                CuboidPNPSolver(
                    model,
                    cuboid3d=Cuboid3d(rospy.get_param('dimensions')[model])
                )
            self.pubs[model] = \
                rospy.Publisher(
                    '{}/pose_{}'.format(rospy.get_param('topic_publishing'), model),
                    PoseStamped,
                    queue_size=10
                )
            self.pub_dimension[model] = \
                rospy.Publisher(
                    '{}/dimension_{}'.format(rospy.get_param('topic_publishing'), model),
                    String,
                    queue_size=10
                )

        # Start ROS publishers
        self.pub_rgb_dope_points = \
            rospy.Publisher(
                rospy.get_param('topic_publishing') + "/rgb_points",
                ImageSensor_msg,
                queue_size=10
            )
        self.pub_detections = \
            rospy.Publisher(
                'detected_objects',
                Detection3DArray,
                queue_size=10
            )
        self.pub_markers = \
            rospy.Publisher(
                'markers',
                MarkerArray,
                queue_size=10
            )
        
        self.pub_pose_cloud = \
            rospy.Publisher(
                rospy.get_param('topic_publishing') + "/pose_cloud",
                PointCloud2,
                queue_size=10
            )

        camera_ns = rospy.get_param('camera', 'dope/webcam')
        info_manager = CameraInfoManager(cname='dope_webcam_{}'.format(0),
                                        namespace=camera_ns)
        try:
            camera_info_url = rospy.get_param('camera_info_url')
            if not info_manager.setURL(camera_info_url):
                rospy.logwarn('Camera info URL invalid: %s', camera_info_url)
        except KeyError:
            # we don't have a camera_info_url, so we'll keep the
            # default ('file://${ROS_HOME}/camera_info/${NAME}.yaml')
            pass
        info_manager.loadCameraInfo()
        self.info_manager = info_manager
        self.camera_info = info_manager.getCameraInfo()

        # Start ROS subscriber
        # image_sub = message_filters.Subscriber(
        #     rospy.get_param('~topic_camera'),
        #     ImageSensor_msg
        # )
        # info_sub = message_filters.Subscriber(
        #     rospy.get_param('~topic_camera_info'),
        #     CameraInfo
        # )
        # ts = message_filters.TimeSynchronizer([image_sub, info_sub], 1)
        # ts.registerCallback(self.image_callback)

        print("Running DOPE...")