def send_place_pallet_msg(self, x,y,z, returnEvent=False):
msg_l = pallet_manipulation_command_list_t()
msg_l.num_commands = 1
msg_l.utime = self.bot_time_now()
msg = pallet_manipulation_command_t()
msg.task_id = self.get_unique_task_id()
msg.utime = msg_l.utime
msg.pickup = False
msg.pallet_id = -1
msg.destination_id = -1
msg.roi = pointlist2d_t()
msg.pose = pose_t()
msg.pose.pos = (x, y, z)
msg.pose.vel = [0,0,0]
msg.pose.orientation = [0,0,0,0]
msg.pose.rotation_rate = [0,0,0]
msg.pose.accel = [0,0,0]
msg_l.commands = [msg]
if returnEvent:
self.event_num += 1
return lcm.Event(self.event_num, msg_l.utime, "PALLET_MANIPULATION_COMMAND_LIST", msg_l.encode())
else:
self.lc.publish("PALLET_MANIPULATION_COMMAND_LIST", msg_l.encode())
def send_checkpoint(self, chk_pt, returnEvent=False):
msg = task_destination_checkpoint_t()
msg.utime = self.bot_time_now()
msg.clear = 0
msg.checkpoint = chk_pt
msg.task_id = self.get_unique_task_id()
if returnEvent:
self.event_num += 1
return lcm.Event(self.event_num, msg.utime, "TASK_DESTINATION_CHECKPOINT", msg.encode())
else:
self.lc.publish("TASK_DESTINATION_CHECKPOINT", msg.encode())
def send_latlon(self, lat, lon, returnEvent=False):
msg = task_destination_pose_t()
msg.utime = self.bot_time_now()
msg.pos = [lat, lon]
#TODO fix me
#msg.heading = 1.79+3.14/2-1
#msg.heading_matters = True
msg.task_id = self.get_unique_task_id()
if returnEvent:
self.event_num += 1
return lcm.Event(self.event_num, msg.utime, "TASK_DESTINATION_POSE_GPS", msg.encode())
else:
self.lc.publish("TASK_DESTINATION_POSE_GPS", msg.encode())
def send_transport_msg(self, lat, lon, orientation, returnEvent=False):
msg = pose_t()
msg.utime = self.bot_time_now()
msg.pos = (lat, lon, orientation)
msg.vel = (0, 0, 0)
msg.orientation = (orientation, 0, 0, 0)
msg.rotation_rate = (0, 0, 0)
msg.accel = (0, 0, 0)
if returnEvent:
self.event_num += 1
return lcm.Event(self.event_num, msg.utime, "SIM_TELEPORT_GLOBAL_FRAME", msg.encode())
else:
self.lc.publish("SIM_TELEPORT_GLOBAL_FRAME", msg.encode())
def send_xy(self, xy, returnEvent=False):
if self.rndf == None:
raise Error("Can't call send xy without supplying an RNDF")
x,y,z = ru.rndf_pose_to_robot((xy[0], xy[1], 0), self.trans_xyz, self.trans_theta, self.trans_latlon, self.rndf)
print x,y
msg = task_destination_pose_t()
msg.utime = self.bot_time_now()
msg.pos = [x, y]
#TODO fix me
#msg.heading = 1.79+3.14/2-1
#msg.heading_matters = True
msg.task_id = self.get_unique_task_id()
if returnEvent:
self.event_num += 1
return lcm.Event(self.event_num, msg.utime, "TASK_DESTINATION_POSE", msg.encode())
else:
self.lc.publish("TASK_DESTINATION_POSE", msg.encode())