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