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