def cb_pose(data):
    # get image with pose time
    t = data.header.stamp
    image = vf.get_latest(t, remove_older=True)
    if image is None:
        rospy.logwarn('No received images.')
        return

    h, w = image.shape[:2]
    if resize_ratio > 0:
        image = cv2.resize(image, (int(resize_ratio*w), int(resize_ratio*h)), interpolation=cv2.INTER_LINEAR)

    # ros topic to Person instance
    humans = []
    for p_idx, person in enumerate(data.persons):
        human = Human([])
        for body_part in person.body_part:
            part = BodyPart('', body_part.part_id, body_part.x, body_part.y, body_part.confidence)
            human.body_parts[body_part.part_id] = part

        humans.append(human)

    # draw
    image = TfPoseEstimator.draw_humans(image, humans, imgcopy=False)
    pub_img.publish(cv_bridge.cv2_to_imgmsg(image, 'bgr8')) 

def on_boxes_state_message(msg):
    #rospy.loginfo("received from test node: " + str(msg))

    str_msg = ""
    exit_flag = False
    for i, pose in enumerate(msg.pose):
        name = msg.name[i]    
        if "block" in name:
            str_msg +=  str(name) + ".y: "+ str(pose.position.y) +"\n"
            if pose.position.y >0.75:
                exit_flag = True
                rospy.logwarn("success, cube properly sent to tray. Ending!")
                break

    rospy.loginfo_throttle(1.0, "[Test Node] "+ str_msg)
    
    if exit_flag:
        exit_properly_runtime_test() 

def create_linear_motion_task_planner(self, target_pose, time=4.0, steps=400.0):
        """ An *incredibly simple* linearly-interpolated Cartesian move """

        # self.trajectory_planner.enable_collision_table1 = False
        # self.trajectory_planner.clear_parameters()
        # self.trajectory_planner.scene.remove_world_object("table1")
        # rospy.sleep(1.0)

        self.trajectory_planner.table1_z = -1.0
        rospy.logwarn("Targetpose:" + str(target_pose))
        jnts = self.sawyer_robot._limb.ik_request(target_pose, self.sawyer_robot._tip_name)
        rospy.logwarn("JNTS:" + str(jnts))
        success = self.safe_goto_joint_position(jnts).result()
        # success = self.trajectory_planner.move_to_joint_target(jnts.values(),attempts=300)
        self.trajectory_planner.table1_z = 0.0

        if not success:
            self.create_wait_forever_task().result()

        return True 

def create_decision_select_block_and_tray(self, blocks, target_block_index):
        """
        :return:
        """
        # An orientation for gripper fingers to be overhead and parallel to the obj
        rospy.logwarn("NEW TARGET BLOCK INDEX: %d" % target_block_index)

        target_block = None
        if blocks is not None and len(blocks) > 0:
            target_block = blocks[target_block_index]  # access first item , pose field
        else:
            rospy.logwarn("No block to pick from table!!")
            return False

        target_tray = self.environment_estimation.get_tray_by_color(target_block.get_color())
        target_tray.gazebo_pose = self.compute_tray_pick_offset_transform(target_tray.gazebo_pose)

        rospy.logwarn("TARGET TRAY POSE: " + str(target_tray))
        return target_block, target_tray 

def moveit_tray_place(self, target_block, target_tray):
        result = False
        while not result or result < 0:
            self.scheduler_yield()
            self.trajectory_planner.set_default_tables_z()
            self.trajectory_planner.table2_z = demo_constants.TABLE_HEIGHT
            self.trajectory_planner.update_table2_collision()
            self.trajectory_planner.update_table1_collision()
            target_block.tray = target_tray
            target_block.tray_place_pose = self.compute_grasp_pose_offset(target_tray.get_tray_place_block_pose())
            target_block.place_pose = target_block.tray_place_pose

            result = self.trajectory_planner.place_block(target_block)
            rospy.logwarn("place result: " + str(result))

        target_tray.notify_place_block(target_block, self.gripper_state) 

def moveit_tabletop_place(self, target_block):
        result = False
        while not result or result < 0:
            self.scheduler_yield()
            self.trajectory_planner.set_default_tables_z()
            self.trajectory_planner.table1_z = demo_constants.TABLE_HEIGHT
            self.trajectory_planner.update_table2_collision()
            self.trajectory_planner.update_table1_collision()
            target_block.table_place_pose = self.compute_grasp_pose_offset(
                target_block.tabletop_arm_view_estimated_pose)
            target_block.place_pose = target_block.table_place_pose

            result = self.trajectory_planner.place_block(target_block)
            rospy.logwarn("place result: " + str(result))

        self.environment_estimation.table.notify_gripper_place(target_block,self.gripper_state) 

def place_block_to_tray(self,tray_index):
        tray_index = int(tray_index)
        trayslen = len(self.environment_estimation.trays)
        rospy.logwarn("request place to tray: %d", tray_index)

        if self.gripper_state.holding_block is not None:
            rospy.logwarn("gripper holding block:"+ str(self.gripper_state.holding_block.get_state()))

            if tray_index  <trayslen  and tray_index >= 0:
                target_tray = self.environment_estimation.trays[tray_index]
                self.moveit_tray_place(self.gripper_state.holding_block, target_tray).result()
            else:
                rospy.logwarn(
                    "Cannot place, incorrect tray id")

        else:
            rospy.logwarn(
                "Cannot place block, robot is not holding any block") 

def check_success(self, position, close_goal):

        rospy.logwarn("gripping force: " + str(self._gripper.get_force()))
        rospy.logwarn("gripper position: " + str(self._gripper.get_position()))
        rospy.logwarn("gripper position deadzone: " + str(self._gripper.get_dead_zone()))

        if not self._gripper.is_moving():
            success = True
        else:
            success = False

        # success = fabs(self._gripper.get_position() - position) < self._gripper.get_dead_zone()


        rospy.logwarn("gripping success: " + str(success))

        return success 

def _HandleVelocityCommand(self, twistCommand):
		""" Handle movement requests. """
		v = twistCommand.linear.x        # m/s
		omega = twistCommand.angular.z      # rad/s
		rospy.logwarn("Handling twist command: " + str(v) + "," + str(omega))

		v= v*1000
		omega= omega * 1000

		

#		message = 's %.3f %.3f\r' % (v, omega)
		message = 's %d %d\r' % (v, omega)
		rospy.logwarn(str(v)+str(omega))
		rospy.logwarn("Sending speed command message: " + message)
		self._WriteSerial(message) 

def _BroadcastBatteryInfo(self, lineParts):
		partsCount = len(lineParts)
		#rospy.logwarn(partsCount)

		if (partsCount  < 1):
			pass
		
		try:
			batteryVoltage = float(lineParts[1])
			batteryState = BatteryState()
			batteryState.voltage = batteryVoltage
			
			if (batteryVoltage <= self._VoltageLowlimit):
				batteryState.isLow = 1
			if (batteryVoltage <= self._VoltageLowLowlimit):
				batteryState.isLowLow = 1;

#			self._BatteryStatePublisher.publish(batteryState)
			
			#rospy.loginfo(batteryState)
		
		except:
			rospy.logwarn("Unexpected error:" + str(sys.exc_info()[0])) 

def wheelCallback(self, msg):
    ######################################################
        enc = long(msg.data)
#	rospy.logwarn(enc)
        if (enc < self.encoder_low_wrap and self.prev_encoder > self.encoder_high_wrap) :
            self.wheel_mult = self.wheel_mult + 1
            
        if (enc > self.encoder_high_wrap and self.prev_encoder < self.encoder_low_wrap) :
            self.wheel_mult = self.wheel_mult - 1
           
         
        self.wheel_latest = 1.0 * (enc + self.wheel_mult * (self.encoder_max - self.encoder_min)) / self.ticks_per_meter 
        self.prev_encoder = enc
        
        
#        rospy.logdebug("-D- %s wheelCallback msg.data= %0.3f wheel_latest = %0.3f mult=%0.3f" % (self.nodename, enc, self.wheel_latest, self.wheel_mult))
    
    ###################################################### 

def wheelCallback(self, msg):
    ######################################################
        enc = long(msg.data)
#	rospy.logwarn(enc)
        if (enc < self.encoder_low_wrap and self.prev_encoder > self.encoder_high_wrap) :
            self.wheel_mult = self.wheel_mult + 1
            
        if (enc > self.encoder_high_wrap and self.prev_encoder < self.encoder_low_wrap) :
            self.wheel_mult = self.wheel_mult - 1
           
         
        self.wheel_latest = 1.0 * (enc + self.wheel_mult * (self.encoder_max - self.encoder_min)) / self.ticks_per_meter 
        self.prev_encoder = enc
        
        
#        rospy.logdebug("-D- %s wheelCallback msg.data= %0.3f wheel_latest = %0.3f mult=%0.3f" % (self.nodename, enc, self.wheel_latest, self.wheel_mult))
    
    ###################################################### 

def _HandleVelocityCommand(self, twistCommand):
		""" Handle movement requests. """
		v = twistCommand.linear.x        # m/s
		omega = twistCommand.angular.z      # rad/s
		rospy.logwarn("Handling twist command: " + str(v) + "," + str(omega))

		v= v*1000
		omega= omega * 1000

		

#		message = 's %.3f %.3f\r' % (v, omega)
		message = 's %d %d\r' % (v, omega)
		rospy.logwarn(str(v)+str(omega))
		rospy.logwarn("Sending speed command message: " + message)
		self._WriteSerial(message) 

def _HandleVelocityCommand(self, twistCommand):
		""" Handle movement requests. """
		v = twistCommand.linear.x        # m/s
		omega = twistCommand.angular.z      # rad/s
		rospy.logwarn("Handling twist command: " + str(v) + "," + str(omega))

		v= v*1000
		omega= omega * 1000

		

#		message = 's %.3f %.3f\r' % (v, omega)
		message = 's %d %d\r' % (v, omega)
		rospy.logwarn(str(v)+str(omega))
		rospy.logwarn("Sending speed command message: " + message)
		self._WriteSerial(message) 

def wheelCallback(self, msg):
    ######################################################
        enc = long(msg.data)
#	rospy.logwarn(enc)
        if (enc < self.encoder_low_wrap and self.prev_encoder > self.encoder_high_wrap) :
            self.wheel_mult = self.wheel_mult + 1
            
        if (enc > self.encoder_high_wrap and self.prev_encoder < self.encoder_low_wrap) :
            self.wheel_mult = self.wheel_mult - 1
           
         
        self.wheel_latest = 1.0 * (enc + self.wheel_mult * (self.encoder_max - self.encoder_min)) / self.ticks_per_meter 
        self.prev_encoder = enc
        
        
#        rospy.logdebug("-D- %s wheelCallback msg.data= %0.3f wheel_latest = %0.3f mult=%0.3f" % (self.nodename, enc, self.wheel_latest, self.wheel_mult))
    
    ###################################################### 

def read_parameter(name, default):
  """
  Get a parameter from the ROS parameter server. If it's not found, a 
  warn is printed.
  @type name: string
  @param name: Parameter name
  @param default: Default value for the parameter. The type should be 
  the same as the one expected for the parameter.
  @return: The restulting parameter
  """
  if not rospy.has_param(name):
    rospy.logwarn('Parameter [%s] not found, using default: %s' % (name, default))
  return rospy.get_param(name, default) 

def get_ros_param(param, default=None):
    try:
        key = rospy.search_param(param)
        return default if key is None else rospy.get_param(key, default)
    except Exception as e:
        rospy.logwarn('Failed to get ros param {}, will use default {}. Exception: '.format(param, default, e))
        return default 

def retrieve_credentials(self):
        try:
            cert_file = self.get_param('certfile')
            key_file = self.get_param('keyfile')
            endpoint = self.get_param('endpoint')
            role_alias = self.get_param('role')
            connect_timeout = self.get_param('connect_timeout_ms', self.DEFAULT_AUTH_CONNECT_TIMEOUT_MS)
            total_timeout = self.get_param('total_timeout_ms', self.DEFAULT_AUTH_TOTAL_TIMEOUT_MS)
            thing_name = self.get_param('thing_name', '')

            if any(v is None for v in (cert_file, key_file, endpoint, role_alias, thing_name)):
                return None

            headers = {'x-amzn-iot-thingname': thing_name} if len(thing_name) > 0 else None
            url = 'https://{}/role-aliases/{}/credentials'.format(endpoint, role_alias)
            timeout = (connect_timeout, total_timeout - connect_timeout)  # see also: urllib3/util/timeout.py

            response = requests.get(url, cert=(cert_file, key_file), headers=headers, timeout=timeout)
            d = response.json()['credentials']

            rospy.loginfo('Credentials expiry time: {}'.format(d['expiration']))

            return {
                'access_key': d['accessKeyId'],
                'secret_key': d['secretAccessKey'],
                'token': d['sessionToken'],
                'expiry_time': d['expiration'],
            }
        except Exception as e:
            rospy.logwarn('Failed to fetch credentials from AWS IoT: {}'.format(e))
            return None 

def get_tray_place_block_pose(self):
        #return self.gazebo_pose
        copygazebopose = copy.deepcopy(self.gazebo_pose)

        yoffset = -0.08 + 0.075 * len(self.blocks)
        copygazebopose.position.y -= yoffset
        copygazebopose.position.z += self.TRAY_SURFACE_THICKNESS

        zrot = tf.transformations.quaternion_from_euler(0, 0, -math.pi/2.0)

        trayorientatin = [copygazebopose.orientation.x, copygazebopose.orientation.y, copygazebopose.orientation.z, copygazebopose.orientation.w]
        # oorient = [overhead_orientation.x,overhead_orientation.y,overhead_orientation.z,overhead_orientation.w]

        # resultingorient = tf.transformations.quaternion_multiply(cubeorientation, tf.transformations.quaternion_conjugate(oorient))
        resultingorient = tf.transformations.quaternion_multiply(trayorientatin, zrot)

        # resultingorient = cubeorientation


        copygazebopose.orientation = Quaternion(x=resultingorient[0], y=resultingorient[1], z=resultingorient[2],
                                      w=resultingorient[3])


        rospy.logwarn("Tray Place location (objects %d, %lf): " % (len(self.blocks), yoffset) + str(copygazebopose))

        return copygazebopose 

def force_joint_limits(jntspos):
    joint_limits = [(-3.0503, 3.0503), (-3.8095, 2.2736), (-3.0426, 3.0426), (-3.0439, 3.0439)]

    rospy.logwarn(jntspos)
    for i in xrange(len(joint_limits)):
        lower = joint_limits[i][0]
        upper = joint_limits[i][1]
        if jntspos[i] < lower and (jntspos[i] + 2.0 * math.pi) < upper:
            jntspos[i] = jntspos[i] + 2 * math.pi

        if jntspos[i] > upper and (jntspos[i] - 2.0 * math.pi) > lower:
            jntspos[i] = jntspos[i] - 2 * math.pi

    return jntspos 

def gripper_open(self):
        """
        :return:
        """
        rospy.logwarn("OPENING GRIPPER")
        self._gripper.open()
        while self._gripper.is_moving() and not rospy.is_shutdown():
            rospy.sleep(0.4) 

def gripper_close(self):
        """
        :return:
        """
        rospy.logwarn("CLOSING GRIPPER")
        self._gripper.close()

        while self._gripper.is_moving() and not rospy.is_shutdown():
            rospy.sleep(0.1) 

def scheduler_yield(self):
        rospy.logwarn("scheduler yield")
        if self.cancel_signal:
            raise Exception("scheduler cancel") 

def add_task(self, task):
        """
        :return:
        """
        rospy.logwarn("adding task: " + task.name)
        self.print_tasks()
        try:
            self.mutex.acquire()
            rospy.logwarn("adding task mutex: " + task.name)
            self.tasks.append(task)
        finally:
            self.mutex.release() 

def robot_sayt2s(self, text):
        """
        :param text:
        :return:
        """
        rospy.logwarn("ROBOT SAYS: " + text) 

def create_go_vision_head_pose_task(self):
        """
        :return:
        """

        self.sawyer_robot.gripper_open()

        self.trajectory_planner.ceilheight = 0.95
        self.trajectory_planner.update_ceiling_obstacle()

        # joint_angles = self.sawyer_robot._limb.joint_angles()

        oldceil = self.trajectory_planner.ceilheight

        joint_angles = self.head_pose_joints

        self.trajectory_planner.ceilheight = 2.0

        # self.sawyer_robot._limb.move_to_joint_positions(joint_angles)
        found = self.safe_goto_joint_position(joint_angles).result()
        # self.delay_task(0.2).result()
        rospy.sleep(0.5)
        self.trajectory_planner.ceilheight = oldceil
        """
        rospy.logwarn(self.sawyer_robot._limb.endpoint_pose())

        targetpose = Pose()
        targetpose.position.x = 0.4595219280890743
        targetpose.position.y = 0.1473752184292072
        targetpose.position.z = 0.019578584407653032

        targetpose.orientation.x = -0.011325648436031916
        targetpose.orientation.y = 0.9998115142702567
        targetpose.orientation.z = -0.006101035043221461
        targetpose.orientation.w = 0.014541079448283218

        found = self.trajectory_planner.move_to_cartesian_target(targetpose)
        """

        if not found:
            self.create_wait_forever_task().result() 

def create_pick_task(self, target_pose, approach_speed, approach_time, meet_time, retract_time,
                         hover_distance):
        """
        :param target_pose:
        :param approach_speed:
        :return:
        """
        rospy.logwarn("PICKING")
        return self.create_pick_loop_task(target_pose, approach_speed, approach_time, meet_time, retract_time,
                                          hover_distance).result() 

def approach_hover(self, pose, time, hover_distance, approach_speed=0.001):
        """
        :param pose:
        :param time:
        :param approach_speed:
        :return:
        """
        approach_pose = copy.deepcopy(pose)
        rospy.logwarn("approach pose:" + str(approach_pose))
        rospy.logwarn("hover distance:" + str(hover_distance))
        # approach with a pose the hover-distance above the requested pose

        rospy.logwarn("approach prev z :" + str(approach_pose.position.z))
        approach_pose.position.z = approach_pose.position.z + hover_distance
        rospy.logwarn("approach pos z :" + str(approach_pose.position.z))
        # joint_angles = self._limb.ik_request(approach, self._tip_name)

        # self._limb.set_joint_position_speed(0.0001)
        # self._guarded_move_to_joint_position(joint_angles)
        success = self.create_linear_motion_task(approach_pose, time=time).result()

        if not success:
            self.create_wait_forever_task().result()

        rospy.sleep(0.1)
        # self._limb.set_joint_position_speed(0.0001) 

def moveit_tabletop_pick(self, target_block):
        # self.sawyer_robot.gripper_open()
        self.trajectory_planner.ceilheight = 0.7
        self.trajectory_planner.register_box(target_block)

        result = False
        while not result or result < 0:
            self.scheduler_yield()
            rospy.logwarn("target block: " + str(target_block))

            target_block.grasp_pose = copy.deepcopy(
                self.compute_grasp_pose_offset(target_block.tabletop_arm_view_estimated_pose))
            rospy.logwarn("target block pose : " + str(target_block.grasp_pose))
            self.trajectory_planner.set_default_tables_z()
            self.trajectory_planner.table1_z = demo_constants.TABLE_HEIGHT
            self.trajectory_planner.update_table1_collision()
            result = self.trajectory_planner.pick_block(target_block, "table1")
            rospy.logwarn("pick result: " + str(result))

        self.environment_estimation.table.notify_gripper_pick(target_block, self.gripper_state) 

def moveit_traytop_pick(self, target_block):
        # self.sawyer_robot.gripper_open()
        result = False
        while not result or result < 0:
            self.scheduler_yield()
            rospy.logwarn("target block: " + str(target_block))
            target_block.grasp_pose = self.compute_grasp_pose_offset(target_block.traytop_arm_view_estimated_pose)

            rospy.logwarn("target block pose : " + str(target_block.grasp_pose))
            self.trajectory_planner.set_default_tables_z()
            self.trajectory_planner.table2_z = demo_constants.TABLE_HEIGHT
            self.trajectory_planner.update_table2_collision()

            result = self.trajectory_planner.pick_block(target_block, "table2")
            rospy.logwarn("pick result: " + str(result))

        target_block.tray.notify_pick_block(target_block,self.gripper_state)