def publish_loop(): rospy.init_node('sortingdummy', anonymous=True) msg_status = mes_sorting_status() msg_status.header.stamp = rospy.Time.now() msg_status.version_id = 3 msg_status.state = msg_status.STATE_SORTING msg_status.done_pct = 0 msg_status.status = 'alora' msg_command = mes_sorting_command() msg_command.header.stamp = rospy.Time.now() msg_command.command = msg_command.COMMAND_SORTBRICKS msg_command.order.order_id = 1 msg_command.order.bricks.append(lego_brick(color=lego_brick.COLOR_RED, size=4, count=2)) msg_command.order.bricks.append(lego_brick(color=lego_brick.COLOR_BLUE, size=6, count=5)) msg_command.order.bricks.append(lego_brick(color=lego_brick.COLOR_YELLOW,size=8, count =2)) pub2 = rospy.Publisher('/mes/command', mes_sorting_command,queue_size=1) #pub = rospy.Publisher('/mes/incoming', mes_sorting_command, queue_size=1) while not rospy.is_shutdown(): #pub.publish(1) pub2.publish(msg_command) response = raw_input('Input something') print response res = rospy.Subscriber('/mes/status') res2 = rospy.Subscriber('/mes/command')
def setCommand(self,command): self.logDataToTopic(1,2,"New command received") gnrl=general() gnrl.general=self.convertCommand(command['command']) self.mes_outgoing_publisher.publish(gnrl) self.logDataToTopic(1,3,"New command published to topic") rospy.loginfo("command "+str(self.convertCommand(command['command']))) self.ros_msg_command.command = self.convertCommand(command['command']) if (command.has_key("order")): self.logDataToTopic(1,4,"New order received") self.msg_command.order.order_id = command['order']['order_id'] self.msg=command['order'] # added self.mes_order_publisher.publish(self.msg) self.logDataToTopic(1,5,"New order published to Topic") legos = command['order']['bricks'] self.msg_command.order.bricks = [] for i in range(len(legos)): self.msg_command.order.bricks.append(lego_brick()) self.msg_command.order.bricks[i].color = legos[i]['color'] self.msg_command.order.bricks[i].size = legos[i]['size'] self.msg_command.order.bricks[i].count = legos[i]['count'] else: self.logDataToTopic(1,6,"No order received initaialize default") self.ros_msg_command.order.order_id = 0 self.ros_msg_command.order.bricks = [] return
def setCommand(self, command): self.logDataToTopic(1, 2, "New command received") gnrl = general() gnrl.general = self.convertCommand(command['command']) self.mes_outgoing_publisher.publish(gnrl) self.logDataToTopic(1, 3, "New command published to topic") rospy.loginfo("command " + str(self.convertCommand(command['command']))) self.ros_msg_command.command = self.convertCommand(command['command']) if (command.has_key("order")): self.logDataToTopic(1, 4, "New order received") self.msg_command.order.order_id = command['order']['order_id'] self.msg = command['order'] # added self.mes_order_publisher.publish(self.msg) self.logDataToTopic(1, 5, "New order published to Topic") legos = command['order']['bricks'] self.msg_command.order.bricks = [] for i in range(len(legos)): self.msg_command.order.bricks.append(lego_brick()) self.msg_command.order.bricks[i].color = legos[i]['color'] self.msg_command.order.bricks[i].size = legos[i]['size'] self.msg_command.order.bricks[i].count = legos[i]['count'] else: self.logDataToTopic(1, 6, "No order received initaialize default") self.ros_msg_command.order.order_id = 0 self.ros_msg_command.order.bricks = [] return