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
