Python rospy.logwarn方法代碼示例

# 需要導入模塊: import rospy [as 別名]
# 或者: from rospy import logwarn [as 別名]
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')) 

# 需要導入模塊: import rospy [as 別名]
# 或者: from rospy import logwarn [as 別名]
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() 

# 需要導入模塊: import rospy [as 別名]
# 或者: from rospy import logwarn [as 別名]
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 

# 需要導入模塊: import rospy [as 別名]
# 或者: from rospy import logwarn [as 別名]
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 

# 需要導入模塊: import rospy [as 別名]
# 或者: from rospy import logwarn [as 別名]
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) 

# 需要導入模塊: import rospy [as 別名]
# 或者: from rospy import logwarn [as 別名]
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) 

# 需要導入模塊: import rospy [as 別名]
# 或者: from rospy import logwarn [as 別名]
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) 

# 需要導入模塊: import rospy [as 別名]
# 或者: from rospy import logwarn [as 別名]
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) 

# 需要導入模塊: import rospy [as 別名]
# 或者: from rospy import logwarn [as 別名]
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") 

# 需要導入模塊: import rospy [as 別名]
# 或者: from rospy import logwarn [as 別名]
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 

# 需要導入模塊: import rospy [as 別名]
# 或者: from rospy import logwarn [as 別名]
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) 

# 需要導入模塊: import rospy [as 別名]
# 或者: from rospy import logwarn [as 別名]
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])) 

# 需要導入模塊: import rospy [as 別名]
# 或者: from rospy import logwarn [as 別名]
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))
    
    ###################################################### 

# 需要導入模塊: import rospy [as 別名]
# 或者: from rospy import logwarn [as 別名]
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) 

# 需要導入模塊: import rospy [as 別名]
# 或者: from rospy import logwarn [as 別名]
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