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
