def get_manipulation_area_map_points(self, translation_vector):
manipulation_area_map_points = set()
manipulation_polygon = self._get_manipulation_polygon(
translation_vector)
if type(manipulation_polygon) is LineString:
coords = list(manipulation_polygon.coords)
# Check if manipulation causes to be out of map bounds
# If yes, return empty set
if (not Utils._is_in_matrix(
coords[0][0], coords[0][1], self.map_metadata.width,
self.map_metadata.height) or not Utils._is_in_matrix(
coords[1][0], coords[1][1], self.map_metadata.width,
self.map_metadata.height)):
return set()
manipulation_area_map_points = set(
bresenham(
int(coords[0][0] / self.map_metadata.resolution), # x1
int(coords[0][1] / self.map_metadata.resolution), # y1
int(coords[1][0] / self.map_metadata.resolution), # x2
int(coords[1][1] / self.map_metadata.resolution))) # y2
else:
# Utils.debug_publish_shapely_as_ros_polygon(manipulation_polygon.envelope, "/simulated/debug_polygon")
x, y = manipulation_polygon.envelope.exterior.xy
bb_top_left_corner = (int(min(x) / self.map_metadata.resolution),
int(min(y) / self.map_metadata.resolution))
bb_bottom_right_corner = (int(
max(x) / self.map_metadata.resolution),
int(
max(y) /
self.map_metadata.resolution))
# Utils.debug_publish_map_coords({bb_top_left_corner, bb_bottom_right_corner}, "/simulated/debug_map_coords")
# Check if manipulation causes to be out of map bounds
# If yes, return empty set
if (not Utils._is_in_matrix(
bb_top_left_corner[0], bb_top_left_corner[1],
self.map_metadata.width, self.map_metadata.height)
or not Utils._is_in_matrix(
bb_bottom_right_corner[0], bb_bottom_right_corner[1],
self.map_metadata.width, self.map_metadata.height)):
return set()
for i in range(bb_top_left_corner[0],
bb_bottom_right_corner[0] + 1):
for j in range(bb_top_left_corner[1],
bb_bottom_right_corner[1] + 1):
for corner in Utils.get_corners_world_coords(
i, j, self.map_metadata.resolution):
if manipulation_polygon.contains(Point(corner)):
manipulation_area_map_points.add((i, j))
# Utils.debug_publish_map_coords(manipulation_area_map_points, "/simulated/debug_map_coords")
return manipulation_area_map_points
def set_initial_robot_pose(self, pose, map_coord_x, map_coord_y):
if (Utils._is_in_matrix(map_coord_x, map_coord_y, self.simulated_map.info.width, self.simulated_map.info.height) and
not self.simulated_map.merged_occ_grid[map_coord_x][map_coord_y] >= Utils.ROS_COST_POSSIBLY_CIRCUMSCRIBED):
self.simulated_robot_state.set_pose(pose, self.simulated_map.info.resolution)
else:
raise RobotPlacementException
self.simulated_fov_pointcloud = self.simulated_map.get_point_cloud_in_fov(self.simulated_robot_state.pose)
def _local_goal_pose_callback(self, request):
local_pose_goal = request.local_pose_goal
is_manipulation = request.is_manipulation
response = LocalGoalPoseResponse()
goal_map_coords = Utils.map_coord_from_ros_pose(local_pose_goal, self.simulated_map.info.resolution)
expected_polygon_footprint = Point((local_pose_goal.pose.position.x, local_pose_goal.pose.position.y)).buffer(self.simulated_robot_state.metadata.radius)
# Check if expected robot polygon is going to intersect any obstacle's polygons
obstacles_colliding_with_robot = set()
for obstacle in self.simulated_map.obstacles.values():
if expected_polygon_footprint.intersects(obstacle.polygon):
# If the robot entered in collision with at least one unmovable obstacle, the pose is not updated
if obstacle.movability == Movability.unmovable:
response.real_pose = self.simulated_robot_state.pose
return response
obstacles_colliding_with_robot.add(obstacle)
# Check if robot is going to enter in contact with static obstacles
is_colliding_with_static_obstacles = (
Utils._is_in_matrix(goal_map_coords[0], goal_map_coords[1], self.simulated_map.info.width, self.simulated_map.info.height) and
self.simulated_map.inflated_static_occ_grid[goal_map_coords[0]][goal_map_coords[1]] >= Utils.ROS_COST_POSSIBLY_CIRCUMSCRIBED)
# If no obstacles collided with the robot, simply update the pose
if not (bool(obstacles_colliding_with_robot) or is_colliding_with_static_obstacles):
self.simulated_robot_state.set_pose(local_pose_goal, self.simulated_map.info.resolution)
self.simulated_fov_pointcloud = self.simulated_map.get_point_cloud_in_fov(self.simulated_robot_state.pose)
else:
# Else we must check if these obstacles collide with other when translation is applied
translation = (local_pose_goal.pose.position.x - self.simulated_robot_state.pose.pose.position.x,
local_pose_goal.pose.position.y - self.simulated_robot_state.pose.pose.position.y)
for colliding_obstacle in obstacles_colliding_with_robot:
colliding_obstacle_with_push = copy.deepcopy(colliding_obstacle)
colliding_obstacle_with_push.move(translation)
if colliding_obstacle_with_push.movability == Movability.movable:
# Check collision between obstacles
for other_obstacle_id, other_obstacle in self.simulated_map.obstacles.items():
# If the movable obstacle we are moving enters in collision with another, the pose is not updated
if (other_obstacle_id != colliding_obstacle_with_push.obstacle_id and
colliding_obstacle_with_push.polygon.intersects(other_obstacle.polygon)):
response.real_pose = self.simulated_robot_state.pose
return response
# Check collision between pushed obstacle and static obstacles
for map_point in colliding_obstacle_with_push.discretized_polygon:
if self.simulated_map.static_occ_grid[map_point[0]][map_point[1]] == Utils.ROS_COST_LETHAL:
response.real_pose = self.simulated_robot_state.pose
return response
# If no obstacle bothered us while pushing a movable obstacle, apply translation to it and update robot pose
self.simulated_robot_state.set_pose(local_pose_goal, self.simulated_map.info.resolution)
if is_manipulation:
for colliding_obstacle in obstacles_colliding_with_robot:
self.simulated_map.manually_move_obstacle(colliding_obstacle.obstacle_id, translation)
self.simulated_fov_pointcloud = self.simulated_map.get_point_cloud_in_fov(self.simulated_robot_state.pose)
response.real_pose = self.simulated_robot_state.pose
return response
def manually_add_obstacle(self, points_set, obstacle_id, movability,
robot_polygon):
obstacle = Obstacle.from_points_make_polygon(points_set, self.info,
self.frame_id,
self.robot_metadata,
obstacle_id, movability)
# Raise exception if obstacle in intersection with robot
if robot_polygon is not None and obstacle.polygon.intersects(
robot_polygon):
raise IntersectionWithRobotException
# Raise exception if any of the obstacle's points are out of map bounds
for map_point in obstacle.map_points_set:
if not Utils._is_in_matrix(map_point[0], map_point[1],
len(self.static_occ_grid),
len(self.static_occ_grid[0])):
raise ObstacleOutOfBoundsException
# Raise exception if obstacle intersects another obstacle or the static obstacles
if self.is_obstacle_intersecting_others(obstacle):
raise IntersectionWithObstaclesException
self.obstacles.update({obstacle.obstacle_id: obstacle})
self.compute_merged_occ_grid()
