def action_handler(self, data): if not self.command_flags[data.action]: self.command_flags[data.action] = True if data.action == 'lift': if not self.carry: operation_area = shapely.geometry.Polygon([[0.05, 0], [0.05, self.robot_description['lift_range']], [-0.05, self.robot_description['lift_range']], [-0.05, 0]]) operation_area = shapely.ops.transform(lambda x, y, z=None: (x+self.pos.x, y+self.pos.y), operation_area) operation_area = shapely.affinity.rotate(operation_area, self.orientation, use_radians=True) for item in self.warehouse.items: if Utils.approx_eq(item.orientation, self.orientation, 5.0*math.pi/180.0) or Utils.approx_eq(math.pi-item.orientation, self.orientation, 5.0*math.pi/180.0): if item.pos.within(operation_area): self.carry = item self.warehouse.items.remove(item) item.footprint = shapely.ops.transform(lambda x, y, z=None: (x-item.pos.x, y-item.pos.y), item.footprint) item.footprint = shapely.affinity.rotate(item.footprint, -item.orientation, use_radians=True) item.orientation = 0 item.pos = shapely.geometry.Point(0, 0) break if not self.carry: height = float(data.param) for shelf in self.warehouse.shelves: if Utils.approx_eq(shelf.orientation, self.orientation, 5.0*math.pi/180.0) or Utils.approx_eq(math.pi-shelf.orientation, self.orientation, 5.0*math.pi/180.0): if shelf.pos.within(operation_area): grid = height * shelf.pile / shelf.height self.carry = shelf.carry[grid] shelf.carry[grid] = None break elif data.action == 'put': if self.carry: operation_area = shapely.geometry.Polygon([[0.05, 0], [0.05, self.robot_description['lift_range']], [-0.05, self.robot_description['lift_range']], [-0.05, 0]]) operation_area = shapely.ops.transform(lambda x, y, z=None: (x+self.pos.x, y+self.pos.y), operation_area) operation_area = shapely.affinity.rotate(operation_area, self.orientation, use_radians=True) height = float(data.param) for shelf in self.warehouse.shelves: if shelf.pos.within(operation_area): if Utils.approx_eq(shelf.orientation, self.orientation, 5.0*math.pi/180.0) or Utils.approx_eq(math.pi-shelf.orientation, self.orientation, 5.0*math.pi/180.0): grid = height * shelf.pile / shelf.height if not shelf.carry[grid]: shelf.carry[grid] = self.carry self.carry = None else: #TODO pass else: #TODO pass break if self.carry: item = self.carry item.orientation = self.orientation item.pos = shapely.ops.transform(lambda x, y, z=None: (x+self.robot_description['lift_range']*math.cos(self.orientation), y+self.robot_description['lift_range']*math.sin(self.orientation)), self.pos) item.footprint = shapely.ops.transform(lambda x, y, z=None: (x+item.pos.x, y+item.pos.y), item.footprint) item.footprint = shapely.affinity.rotate(item.footprint, item.orientation, use_radians=True) if not self.warehouse.scene.intersect(item.footprint): self.warehouse.items.add(item) self.carry = None else: #TODO pass
def run(self): linear_accel = min(max(self.robot_description['linear_accel_min']/self.warehouse.simulator_step, self.vel_target.linear.x - self.vel_odom.linear.x), self.robot_description['linear_accel_max']/self.warehouse.simulator_step) self.vel_odom.linear.x += linear_accel angular_accel = min(max(-self.robot_description['angular_accel_max']/self.warehouse.simulator_step, self.vel_target.angular.z - self.vel_odom.angular.z), self.robot_description['angular_accel_max']/self.warehouse.simulator_step) self.vel_odom.angular.z += angular_accel self.orientation += self.vel_odom.angular.z/self.warehouse.simulator_step self.battery.quantity -= abs(self.vel_odom.angular.z / self.robot_description['angular_speed_max']) * self.robot_description['battery_cost_vel_angular'] / self.warehouse.simulator_step self.vel.linear.x = self.vel_odom.linear.x*math.cos(self.orientation) self.vel.linear.y = self.vel_odom.linear.x*math.sin(self.orientation) self.vel.angular.z = self.vel_odom.angular.z self.footprint = shapely.affinity.rotate(self.footprint, self.vel.angular.z/self.warehouse.simulator_step, use_radians=True) if not Utils.approx_eq(self.vel_odom.linear.x, 0): self.battery.quantity -= abs(self.vel_odom.linear.x / self.robot_description['linear_speed_max']) * self.robot_description['battery_cost_vel_linear'] / self.warehouse.simulator_step new_ft = shapely.ops.transform(lambda x, y, z=None: (x+self.vel.linear.x/self.warehouse.simulator_step, y+self.vel.linear.y/self.warehouse.simulator_step), self.footprint) trail = self.footprint.union(new_ft).convex_hull if self.warehouse.floor.collide_with_obstacle(trail) : self.broken = True for robot in self.warehouse.robots: if robot is self: continue else: if robot.footprint.intersects(trail): robot.broken = True self.broken = True for item in self.warehouse.items: if item.footprint.intersects(trail): self.broken = True for shelf in self.warehouse.shelves: if shelf.footprint.intersects(trail): self.broken = True for gate in self.warehouse.gates: if (not gate.open) and gate.intersects(trail): self.broken = True if not self.broken: self.pos = shapely.ops.transform(lambda x, y, z=None: (x+self.vel.linear.x/self.warehouse.simulator_step, y+self.vel_odom.linear.y/self.warehouse.simulator_step), self.pos) self.footprint = new_ft