Python rospy.logwarn() Examples

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 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 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 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 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 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 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 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 _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 _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 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 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 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 _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 _place(self, group, target, place):
        """
        Place a target using the planning group
        """

        # Obtain possible places:
        places = self._generate_places(place)

        # Create and send Place goal:
        goal = self._create_place_goal(group, target, places)

        state = self._place_ac.send_goal_and_wait(goal)
        if state != GoalStatus.SUCCEEDED:
            rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text())
            return None

        result = self._place_ac.get_result()

        # Check for error:
        err = result.error_code.val
        if err != MoveItErrorCodes.SUCCESS:
            rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err])))

            return False

        return True 

def _signal_handler(self, signal, frame):
        rospy.logwarn("\nDF_CLIENT: SIGINT caught!")
        self.exit()

    # ----------------- #
    #  Audio Utilities  #
    # ----------------- # 

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

		if (partsCount  < 1):
			pass
		
		try:



			line_parts_float = float(lineParts[1])
			rospy.logwarn("Bat 1")
			lineparts_int = round(line_parts_float)
			rospy.logwarn("Bat 2")
			rospy.logwarn(lineparts_int)
			batteryVoltage =  12



			rospy.logwarn("Bat 3")



			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]))
			rospy.logwarn("Error from battery") 

def _place(self, group, target, place):
        """
        Place a target using the planning group
        """

        # Obtain possible places:
        places = self._generate_places(place)

        # Create and send Place goal:
        goal = self._create_place_goal(group, target, places)

        state = self._place_ac.send_goal_and_wait(goal)
        if state != GoalStatus.SUCCEEDED:
            rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text())
            return None

        result = self._place_ac.get_result()

        # Check for error:
        err = result.error_code.val
        if err != MoveItErrorCodes.SUCCESS:
            rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err])))

            return False

        return True 

def _pickup(self, group, target, width):
        """
        Pick up a target using the planning group
        """

        # Obtain possible grasps from the grasp generator server:
        grasps = self._generate_grasps(self._pose_coke_can, width)

        # Create and send Pickup goal:
        goal = self._create_pickup_goal(group, target, grasps)

        state = self._pickup_ac.send_goal_and_wait(goal)
        if state != GoalStatus.SUCCEEDED:
            rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text())
            return None

        result = self._pickup_ac.get_result()

        # Check for error:
        err = result.error_code.val
        if err != MoveItErrorCodes.SUCCESS:
            rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err])))

            return False

        return True 

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))

		message = 's %.2f %.2f\r' % (v, omega)
		rospy.logdebug("Sending speed command message: " + message)
		self._WriteSerial(message) 

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

		if (partsCount  < 1):
			pass
		
		try:



			line_parts_float = float(lineParts[1])
			rospy.logwarn("Bat 1")
			lineparts_int = round(line_parts_float)
			rospy.logwarn("Bat 2")
			rospy.logwarn(lineparts_int)
			batteryVoltage =  12



			rospy.logwarn("Bat 3")



			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]))
			rospy.logwarn("Error from battery") 

def update_block(self, block):
        rospy.logwarn("updating block collision info")
        if not block in self.registered_blocks:
            raise Exception("block does not exist")

        rospy.logwarn("current blocks: " + str(self.registered_blocks))
        block_pose = geometry_msgs.msg.PoseStamped()
        block_pose.pose = block.grasp_pose
        block_pose.header.stamp = rospy.Time.now()
        block_pose.header.frame_id = self.robot.get_planning_frame()
        block_index = self.registered_blocks.index(block)
        self.scene.add_box("block" + str(block_index), block_pose, size=(0.04, 0.04, 0.04))
        rospy.sleep(0.5)
        return block_index 

def move_to_cartesian_target(self, pose_target, attempts=100):
        """
        
        :param pose_target: geometry_msgs/Pose
        :return: 
        """
        self.clear_parameters()

        self.update_environment_obstacles()

        self.group.set_pose_target(pose_target)
        self.group.set_num_planning_attempts(attempts)

        rospy.sleep(0.3)

        constraints = self.path_constraints(self.group.get_current_pose().pose)
        self.group.set_path_constraints(constraints)

        plan1 = self.group.plan()
        rospy.logwarn(plan1)
        rospy.sleep(0.1)

        self.publish_planning_scene()

        # rospy.logwarn("PLAAAAN!!! "+ str(plan1))

        if plan1.joint_trajectory.header.frame_id != "":
            display_trajectory = moveit_msgs.msg.DisplayTrajectory()

            display_trajectory.trajectory_start = self.robot.get_current_state()
            display_trajectory.trajectory.append(plan1)

            self.display_trajectory_publisher.publish(display_trajectory)

            success = self.group.go(wait=True)
            rospy.logwarn("result of the motion: " + str(success))
            return success
        else:
            return False 

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 _command_gripper_update(self, position, is_close_goal):
        if is_close_goal:
            self._gripper.close()
            rospy.logwarn("cmd: close")
        else:
            self._gripper.open()
            rospy.logwarn("cmd: open")

            # self._gripper.set_position(position) 

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))

		message = 's %.2f %.2f\r' % (v, omega)
		rospy.logdebug("Sending speed command message: " + message)
		self._WriteSerial(message) 

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

		if (partsCount  < 1):
			pass
		
		try:



			line_parts_float = float(lineParts[1])
			rospy.logwarn("Bat 1")
			lineparts_int = round(line_parts_float)
			rospy.logwarn("Bat 2")
			rospy.logwarn(lineparts_int)
			batteryVoltage =  12



			rospy.logwarn("Bat 3")



			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]))
			rospy.logwarn("Error from battery") 

def _signal_handler(self, signal, frame):
        rospy.logwarn("SIGINT Caught!")
        self.exit() 

def detect_intent_text(self, msg):
        """Use the Dialogflow API to detect a user's intent. Goto the Dialogflow
        console to define intents and params.
        :param msg: DialogflowRequest msg
        :return query_result: Dialogflow's query_result with action parameters
        :rtype: DialogflowResult
        """
        # Create the Query Input
        text_input = TextInput(text=msg.query_text, language_code=self._language_code)
        query_input = QueryInput(text=text_input)
        # Create QueryParameters
        user_contexts = utils.converters.contexts_msg_to_struct(msg.contexts)
        self.last_contexts = utils.converters.contexts_msg_to_struct(self.last_contexts)
        contexts = self.last_contexts + user_contexts
        params = QueryParameters(contexts=contexts)
        try:
            response = self._session_cli.detect_intent(
                    session=self._session,
                    query_input=query_input,
                    query_params=params,
                    output_audio_config=self._output_audio_config
            )
        except google.api_core.exceptions.ServiceUnavailable:
            rospy.logwarn("DF_CLIENT: Deadline exceeded exception caught. The response "
                          "took too long or you aren't connected to the internet!")
        else:
            # Store context for future use
            self.last_contexts = utils.converters.contexts_struct_to_msg(
                    response.query_result.output_contexts
            )
            df_msg = utils.converters.result_struct_to_msg(
                    response.query_result)
            rospy.loginfo(utils.output.print_result(response.query_result))
            # Play audio
            if self.PLAY_AUDIO:
                self._play_stream(response.output_audio)
            self._results_pub.publish(df_msg)
            return df_msg