diff --git a/CMakeLists.txt b/CMakeLists.txt index e02e2f0a7..a6b7a91d4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -185,6 +185,9 @@ mrover_add_esw_bridge_node(pdb_test_bridge src/esw/pdb_test_bridge/*.cpp) ## Perception +mrover_add_nodelet(costmap src/perception/cost_map/*.cpp src/perception/cost_map src/perception/cost_map/pch.hpp) +mrover_nodelet_link_libraries(costmap lie) + mrover_add_library(websocket_server src/esw/websocket_server/*.cpp src/esw/websocket_server) target_compile_definitions(websocket_server PUBLIC BOOST_ASIO_NO_DEPRECATED) diff --git a/config/navigation.yaml b/config/navigation.yaml index 4483d8c56..c1ca5e300 100644 --- a/config/navigation.yaml +++ b/config/navigation.yaml @@ -22,6 +22,12 @@ search: segments_per_rotation: 8 distance_between_spirals: 3 +water_bottle_search: + stop_thresh: 0.5 + drive_fwd_thresh: 0.34 + coverage_radius: 10 + segments_per_rotation: 10 + object_search: coverage_radius: 10 distance_between_spirals: 3 diff --git a/config/rviz/zed_test.rviz b/config/rviz/zed_test.rviz index 104b8410d..03d5807a4 100644 --- a/config/rviz/zed_test.rviz +++ b/config/rviz/zed_test.rviz @@ -72,7 +72,7 @@ Visualization Manager: Value: false zed2i_camera_center: Value: false - zed2i_left_camera_frame: + zed_left_camera_frame: Value: false zed2i_left_camera_optical_frame: Value: false @@ -102,7 +102,7 @@ Visualization Manager: zed2i_camera_center: zed2i_baro_link: {} - zed2i_left_camera_frame: + zed_left_camera_frame: zed2i_left_camera_optical_frame: {} zed2i_temp_left_link: @@ -145,7 +145,7 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - zed2i_left_camera_frame: + zed_left_camera_frame: Alpha: 1 Show Axes: false Show Trail: false diff --git a/package.xml b/package.xml index 9ab172f38..84d90b794 100644 --- a/package.xml +++ b/package.xml @@ -82,6 +82,7 @@ + diff --git a/plugins/costmap.xml b/plugins/costmap.xml new file mode 100644 index 000000000..2873e5822 --- /dev/null +++ b/plugins/costmap.xml @@ -0,0 +1,6 @@ + + + + diff --git a/pyproject.toml b/pyproject.toml index b1580f196..9b9829a90 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -26,6 +26,7 @@ dependencies = [ "aenum==3.1.15", "daphne==4.0.0", "channels==4.0.0", + "scipy==1.12.0", "pyubx2==1.2.35", "opencv-python==4.9.0.80", ] diff --git a/scripts/debug_course_publisher.py b/scripts/debug_course_publisher.py index ff5b34384..734b953f4 100755 --- a/scripts/debug_course_publisher.py +++ b/scripts/debug_course_publisher.py @@ -21,17 +21,17 @@ convert_waypoint_to_gps(waypoint) for waypoint in [ ( - Waypoint(tag_id=0, type=WaypointType(val=WaypointType.NO_SEARCH)), - SE3(position=np.array([4, 4, 0])), - ), - ( - Waypoint(tag_id=0, type=WaypointType(val=WaypointType.POST)), - SE3(position=np.array([-2, -2, 0])), - ), - ( - Waypoint(tag_id=1, type=WaypointType(val=WaypointType.POST)), - SE3(position=np.array([11, -10, 0])), - ), + Waypoint(tag_id=-1, type=WaypointType(val=WaypointType.WATER_BOTTLE)), + SE3(position=np.array([0, 0, 0])), + )#, + # ( + # Waypoint(tag_id=0, type=WaypointType(val=WaypointType.POST)), + # SE3(position=np.array([-2, -2, 0])), + # ), + # ( + # Waypoint(tag_id=1, type=WaypointType(val=WaypointType.POST)), + # SE3(position=np.array([11, -10, 0])), + # ), ] ] ) diff --git a/scripts/debug_water_bottle_search.py b/scripts/debug_water_bottle_search.py new file mode 100755 index 000000000..942fb927e --- /dev/null +++ b/scripts/debug_water_bottle_search.py @@ -0,0 +1,47 @@ +#!/usr/bin/env python3 + +import numpy as np + +import rospy +import time +from nav_msgs.msg import OccupancyGrid, MapMetaData +from geometry_msgs.msg import Pose +from std_msgs.msg import Header +from util.course_publish_helpers import publish_waypoints + + +""" +The purpose of this file is for testing the water bottle search state. +Specifically the occupancy grid message. +""" + +if __name__ == "__main__": + rospy.init_node("debug_water_bottle") + try: + # int8[] data + test_grid = OccupancyGrid() + test_grid.data = np.array([1, 2, 3, 4, 5, 6, 7, 8, 9], dtype=np.int8) + + # nav_msgs/MapMetaData info + metadata = MapMetaData() + metadata.map_load_time = rospy.Time.now() + metadata.resolution = 3 + metadata.width = 3 + metadata.height = 3 + metadata.origin = Pose() + test_grid.info = metadata + + # std_msgs/Header header + header = Header() + test_grid.header = header + + # rospy.loginfo(f"Before publish") + # #costpub = rospy.Publisher("costmap", OccupancyGrid, queue_size=1) + # for i in range(10): + # costpub.publish(test_grid) + # time.sleep(1) + # rospy.loginfo(f"After publish") + # rospy.spin() + + except rospy.ROSInterruptException as e: + print(f"Didn't work to publish or retrieve message from ros") diff --git a/src/navigation/astar.py b/src/navigation/astar.py new file mode 100644 index 000000000..fefce2159 --- /dev/null +++ b/src/navigation/astar.py @@ -0,0 +1,245 @@ +from __future__ import annotations + +import heapq +import random +from threading import Lock + +import numpy as np + +from navigation.context import Context + +class SpiralEnd(Exception): + """ + Raise when there are no more points left in the search spiral + """ + pass + +class NoPath(Exception): + """ + Raise when an A* path could not be found + """ + pass + +class AStar: + origin: np.ndarray = np.array([]) # holds the inital rover pose (waypoint of water bottle) + context: Context + costmap_lock: Lock + + def __init__(self, origin: np.ndarray, context: Context)-> None: + self.origin = origin + self.context = context + self.costmap_lock = Lock() + + class Node: + """ + Node class for astar pathfinding + """ + def __init__(self, parent=None, position=None): + self.parent = parent + self.position = position + self.g = 0 + self.h = 0 + self.f = 0 + + def __eq__(self, other): + return self.position == other.position + + # defining less than for purposes of heap queue + def __lt__(self, other): + return self.f < other.f + + # defining greater than for purposes of heap queue + def __gt__(self, other): + return self.f > other.f + + def cartesian_to_ij(self, cart_coord: np.ndarray) -> np.ndarray: + """ + Convert real world cartesian coordinates (x, y) to coordinates in the occupancy grid (i, j) + using formula floor(v - (WP + [-W/2, H/2]) / r) * [1, -1] + v: (x,y) coordinate + WP: origin + W, H: grid width, height (meters) + r: resolution (meters/cell) + :param cart_coord: array of x and y cartesian coordinates + :return: array of i and j coordinates for the occupancy grid + """ + width = self.context.env.cost_map.width * self.context.env.cost_map.resolution + height = self.context.env.cost_map.height * self.context.env.cost_map.resolution + width_height = np.array([-1 * width / 2, height / 2]) # [-W/2, H/2] + converted_coord = np.floor((cart_coord[0:2] - (self.origin + width_height)) / self.context.env.cost_map.resolution) + return converted_coord.astype(np.int8) * np.array([1, -1]) + + def ij_to_cartesian(self, ij_coords: np.ndarray) -> np.ndarray: + """ + Convert coordinates in the occupancy grid (i, j) to real world cartesian coordinates (x, y) + using formula (WP - [W/2, H/2]) + [j * r, i * r] + [r/2, -r/2] * [1, -1] + WP: origin + W, H: grid width, height (meters) + r: resolution (meters/cell) + :param ij_coords: array of i and j occupancy grid coordinates + :return: array of x and y coordinates in the real world + """ + width = self.context.env.cost_map.width * self.context.env.cost_map.resolution + height = self.context.env.cost_map.height * self.context.env.cost_map.resolution + width_height = np.array([width / 2, height / 2]) # [W/2, H/2] + resolution_conversion = ij_coords * [self.context.env.cost_map.resolution, self.context.env.cost_map.resolution] # [j * r, i * r] + half_res = np.array([self.context.env.cost_map.resolution / 2, -1 * self.context.env.cost_map.resolution / 2]) # [r/2, -r/2] + return ((self.origin - width_height) + resolution_conversion + half_res) * np.array([1, -1]) + + def return_path(self, current_node: Node) -> list: + """ + Return the path given from A-Star in reverse through current node's parents + :param current_node: end point of path which contains parents to retrieve path + :param costmap2D: current costmap + :return: reversed path except the starting point (we are already there) + """ + costmap2D = np.copy(self.context.env.cost_map.data) + path = [] + current = current_node + while current is not None: + path.append(current.position) + current = current.parent + reversed_path = path[::-1] + print("ij:", reversed_path[1:]) + + # Print visual of costmap with path and start (S) and end (E) points + # The lighter the block, the more costly it is + for step in reversed_path: + costmap2D[step[0]][step[1]] = 2 # path (.) + costmap2D[reversed_path[0][0]][reversed_path[0][1]] = 3 # start + costmap2D[reversed_path[-1][0]][reversed_path[-1][1]] = 4 # end + + for row in costmap2D: + line = [] + for col in row: + if col == 1.0: + line.append("\u2588") + elif col < 1.0 and col >= 0.8: + line.append("\u2593") + elif col < 0.8 and col >= 0.5: + line.append("\u2592") + elif col < 0.5 and col >= 0.2: + line.append("\u2591") + elif col < 0.2: + line.append(" ") + elif col == 2: + line.append(".") + elif col == 3: + line.append("S") + elif col == 4: + line.append("E") + print("".join(line)) + + return reversed_path[1:] + + def a_star(self, start: np.ndarray, end: np.ndarray) -> list | None: + """ + A-STAR Algorithm: f(n) = g(n) + h(n) to find a path from the given start to the given end in the given costmap + :param costmap2d: occupancy grid in the form of an 2D array of floats + :param start: rover pose in cartesian coordinates + :param end: next point in the spiral from traj in cartesian coordinates + :return: list of A-STAR coordinates in the occupancy grid coordinates (i,j) + """ + with self.costmap_lock: + costmap2d = self.context.env.cost_map.data + # convert start and end to occupancy grid coordinates + startij = self.cartesian_to_ij(start) + endij = self.cartesian_to_ij(end) + + # initialize start and end nodes + start_node = self.Node(None, (startij[0], startij[1])) + end_node = self.Node(None, (endij[0], endij[1])) + + if start_node == end_node: + return None + + print(f"start: {start}, end: {end}") + print(f"startij: {startij}, endij: {endij}") + + # initialize both open and closed list + open_list = [] + closed_list = [] + + # heapify the open_list and add the start node + heapq.heapify(open_list) + heapq.heappush(open_list, start_node) + + # add a stop condition + outer_iterations = 0 + max_iterations = costmap2d.shape[0] * costmap2d.shape[1] // 2 + + # movements/squares we can search + adjacent_squares = ((0, -1), (0, 1), (-1, 0), (1, 0), (-1, -1), (-1, 1), (1, -1), (1, 1)) + adjacent_square_pick_index = [0, 1, 2, 3, 4, 5, 6, 7] + + # loop until you find the end + while len(open_list) > 0: + # randomize the order of the adjacent_squares_pick_index to avoid a decision making bias + random.shuffle(adjacent_square_pick_index) + + outer_iterations += 1 + + if outer_iterations > max_iterations: + # if we hit this point return the path such as it is. It will not contain the destination + print("giving up on pathfinding too many iterations") + return self.return_path(current_node) + + # get the current node + current_node = heapq.heappop(open_list) + closed_list.append(current_node) + + # found the goal + if current_node == end_node: + return self.return_path(current_node) + + # generate children + children = [] + for pick_index in adjacent_square_pick_index: + new_position = adjacent_squares[pick_index] + node_position = (current_node.position[0] + new_position[0], current_node.position[1] + new_position[1]) + # make sure within range + if ( + node_position[0] > (costmap2d.shape[0] - 1) + or node_position[0] < 0 + or node_position[1] > (costmap2d.shape[1] - 1) + or node_position[1] < 0 + ): + continue + + # make sure it is traversable terrian (not too high of a cost) + if costmap2d[node_position[0]][node_position[1]] >= 0.2: # TODO: find optimal value + continue + + # create new node and append it + new_node = self.Node(current_node, node_position) + children.append(new_node) + + # loop through children + for child in children: + # child is on the closed list + if len([closed_child for closed_child in closed_list if closed_child == child]) > 0: + continue + # create the f (total), g (cost in map), and h (euclidean distance) values + child.g = current_node.g + costmap2d[child.position[0], child.position[1]] + child.h = ((child.position[0] - end_node.position[0]) ** 2) + ( + (child.position[1] - end_node.position[1]) ** 2 + ) + child.f = child.g + child.h + # child is already in the open list + if ( + len( + [ + open_node + for open_node in open_list + if child.position == open_node.position and child.g > open_node.g + ] + ) + > 0 + ): + continue + # add the child to the open list + heapq.heappush(open_list, child) + + print("Couldn't find a path to destination") + raise NoPath() + \ No newline at end of file diff --git a/src/navigation/context.py b/src/navigation/context.py index 3face5797..d5f56f28e 100644 --- a/src/navigation/context.py +++ b/src/navigation/context.py @@ -2,6 +2,7 @@ from dataclasses import dataclass from typing import ClassVar, Optional, List, Tuple +from scipy.signal import convolve2d import numpy as np import pymap3d @@ -9,6 +10,7 @@ import rospy import tf2_ros from geometry_msgs.msg import Twist +from util.SE3 import SE3 from mrover.msg import ( Waypoint, GPSWaypoint, @@ -21,6 +23,7 @@ from mrover.srv import EnableAuton, EnableAutonRequest, EnableAutonResponse from navigation.drive import DriveController from navigation import approach_post, long_range, approach_object +from nav_msgs.msg import OccupancyGrid from std_msgs.msg import Time, Bool from util.SE3 import SE3 from visualization_msgs.msg import Marker @@ -77,6 +80,7 @@ class Environment: ctx: Context long_range_tags: LongRangeTagStore + cost_map: CostMap NO_FIDUCIAL: ClassVar[int] = -1 arrived_at_target: bool = False arrived_at_waypoint: bool = False @@ -203,6 +207,15 @@ def get(self, tag_id: int) -> Optional[LongRangeTag]: return None +class CostMap: + """ + Context class to represent the costmap generated around the water bottle waypoint + """ + data: np.ndarray + resolution: int + height: int + width: int + @dataclass class Course: ctx: Context @@ -332,6 +345,7 @@ class Context: course_listener: rospy.Subscriber stuck_listener: rospy.Subscriber tag_data_listener: rospy.Subscriber + costmap_listener: rospy.Subscriber # Use these as the primary interfaces in states course: Optional[Course] @@ -355,13 +369,14 @@ def __init__(self): self.stuck_listener = rospy.Subscriber("nav_stuck", Bool, self.stuck_callback) self.course = None self.rover = Rover(self, False, "") - self.env = Environment(self, long_range_tags=LongRangeTagStore(self)) + self.env = Environment(self, long_range_tags=LongRangeTagStore(self), cost_map=CostMap()) self.disable_requested = False self.use_odom = rospy.get_param("use_odom_frame") self.world_frame = rospy.get_param("world_frame") self.odom_frame = rospy.get_param("odom_frame") self.rover_frame = rospy.get_param("rover_frame") self.tag_data_listener = rospy.Subscriber("tags", LongRangeTags, self.tag_data_callback) + self.costmap_listener = rospy.Subscriber("costmap", OccupancyGrid, self.costmap_callback) def recv_enable_auton(self, req: EnableAutonRequest) -> EnableAutonResponse: if req.enable: @@ -375,3 +390,23 @@ def stuck_callback(self, msg: Bool): def tag_data_callback(self, tags: LongRangeTags) -> None: self.env.long_range_tags.push_frame(tags.longRangeTags) + + def costmap_callback(self, msg: OccupancyGrid): + """ + Callback function for the occupancy grid perception sends + :param msg: Occupancy Grid representative of a 30 x 30m square area with origin at GNSS waypoint. Values are 0, 1, -1 + """ + self.env.cost_map.resolution = msg.info.resolution # meters/cell + self.env.cost_map.height = msg.info.height # cells + self.env.cost_map.width = msg.info.width # cells + self.env.cost_map.data = np.array(msg.data).reshape((int(self.env.cost_map.height), int(self.env.cost_map.width))).astype(np.float32) + + # change all unidentified points to have a slight cost + self.env.cost_map.data[self.env.cost_map.data == -1.0] = 0.1 # TODO: find optimal value + self.env.cost_map.data = np.rot90(self.env.cost_map.data, k=3, axes=(0,1)) # rotate 90 degress clockwise + + # apply kernel to average the map with zero padding + kernel_shape = (7,7) # TODO: find optimal kernel size + kernel = np.ones(kernel_shape, dtype = np.float32) / (kernel_shape[0]*kernel_shape[1]) + self.env.cost_map.data = convolve2d(self.env.cost_map.data, kernel, mode = "same") + diff --git a/src/navigation/drive.py b/src/navigation/drive.py index 0cfe2e758..3b651f652 100644 --- a/src/navigation/drive.py +++ b/src/navigation/drive.py @@ -143,6 +143,7 @@ def get_lookahead_pt( :return: the lookahead point """ # compute the vector from the previous target position to the current target position + path_vec = path_end - path_start # compute the vector from the previous target position to the rover position rover_vec = rover_pos - path_start @@ -194,7 +195,7 @@ def get_drive_command( self.reset() return Twist(), True - if path_start is not None: + if path_start is not None and np.linalg.norm(path_start - target_pos) > LOOKAHEAD_DISTANCE: target_pos = self.get_lookahead_pt(path_start, target_pos, rover_pos, LOOKAHEAD_DISTANCE) target_dir = target_pos - rover_pos diff --git a/src/navigation/navigation.py b/src/navigation/navigation.py index a97cb1ec5..8e843eb08 100755 --- a/src/navigation/navigation.py +++ b/src/navigation/navigation.py @@ -14,6 +14,7 @@ from navigation.search import SearchState from navigation.state import DoneState, OffState, off_check from navigation.waypoint import WaypointState +from navigation.water_bottle_search import WaterBottleSearchState from navigation.approach_object import ApproachObjectState from navigation.long_range import LongRangeState @@ -53,6 +54,7 @@ def __init__(self, context: Context): PostBackupState(), ApproachPostState(), ApproachObjectState(), + WaterBottleSearchState(), LongRangeState(), SearchState(), RecoveryState(), @@ -64,6 +66,9 @@ def __init__(self, context: Context): LongRangeState(), [ApproachPostState(), SearchState(), WaypointState(), RecoveryState()] ) self.state_machine.add_transitions(OffState(), [WaypointState(), DoneState()]) + self.state_machine.add_transitions( + WaterBottleSearchState(), [WaypointState(), RecoveryState(), ApproachObjectState()] + ) self.state_machine.configure_off_switch(OffState(), off_check) self.state_machine_server = StatePublisher(self.state_machine, "nav_structure", 1, "nav_state", 10) diff --git a/src/navigation/search.py b/src/navigation/search.py index bec75bced..864ffcf1a 100644 --- a/src/navigation/search.py +++ b/src/navigation/search.py @@ -12,73 +12,7 @@ from navigation import approach_post, approach_object, recovery, waypoint, long_range from navigation.context import convert_cartesian_to_gps -from navigation.trajectory import Trajectory - - -@dataclass -class SearchTrajectory(Trajectory): - # Associated fiducial for this trajectory - fid_id: int - - @classmethod - def gen_spiral_coordinates( - cls, coverage_radius: float, distance_between_spirals: float, num_segments_per_rotation: int - ) -> np.ndarray: - """ - Generates a set of coordinates for a spiral search pattern centered at the origin - :param coverage_radius: radius of the spiral search pattern (float) - :param distance_between_spirals: distance between each spiralradii = angles * (distance_between_spirals / (2*np.pi)) (float) - :param num_segments_per_rotation: number of segments that the spiral has per rotation (int) - :return: np.ndarray of coordinates - """ - # the number of spirals should ensure coverage of the entire radius. - # We add 1 to ensure that the last spiral covers the radius along the entire rotation, - # as otherwise we will just make the outermost point touch the radius - num_spirals = np.ceil(coverage_radius / distance_between_spirals).astype("int") + 1 - # the angles are evenly spaced between 0 and 2pi*num_segments_per_rotation (add one to the number of points because N+1 points make N segments) - angles = np.linspace(0, 2 * np.pi * num_spirals, num_segments_per_rotation * num_spirals + 1) - # radii are computed via following polar formula. - # This is correct because you want the radius to increase by 'distance_between_spirals' every 2pi radians (one rotation) - radii = angles * (distance_between_spirals / (2 * np.pi)) - # convert to cartesian coordinates - xcoords = np.cos(angles) * radii - ycoords = np.sin(angles) * radii - # we want to return as a 2D matrix where each row is a coordinate pair - # so we reshape x and y coordinates to be (n, 1) matricies then stack horizontally to get (n, 2) matrix - return np.hstack((xcoords.reshape(-1, 1), ycoords.reshape(-1, 1))) - - @classmethod - def spiral_traj( - cls, - center: np.ndarray, - coverage_radius: float, - distance_between_spirals: float, - segments_per_rotation: int, - fid_id: int, - ) -> SearchTrajectory: - """ - Generates a square spiral search pattern around a center position, assumes rover is at the center position - :param center: position to center spiral on (np.ndarray) - :param coverage_radius: radius of the spiral search pattern (float) - :param distance_between_spirals: distance between each spiral (float) - :param segments_per_rotation: number of segments per spiral (int), for example, 4 segments per rotation would be a square spiral, 8 segments per rotation would be an octagonal spiral - :param fid_id: tag id to associate with this trajectory (int) - :return: SearchTrajectory object - """ - zero_centered_spiral_r2 = cls.gen_spiral_coordinates( - coverage_radius, distance_between_spirals, segments_per_rotation - ) - - # numpy broadcasting magic to add center to each row of the spiral coordinates - spiral_coordinates_r2 = zero_centered_spiral_r2 + center - # add a column of zeros to make it 3D - spiral_coordinates_r3 = np.hstack( - (spiral_coordinates_r2, np.zeros(spiral_coordinates_r2.shape[0]).reshape(-1, 1)) - ) - return SearchTrajectory( - spiral_coordinates_r3, - fid_id, - ) +from navigation.trajectory import SearchTrajectory class SearchState(State): @@ -106,6 +40,7 @@ def on_enter(self, context) -> None: self.DISTANCE_BETWEEN_SPIRALS, self.SEGMENTS_PER_ROTATION, search_center.tag_id, + False ) else: # water bottle or mallet self.traj = SearchTrajectory.spiral_traj( @@ -114,6 +49,7 @@ def on_enter(self, context) -> None: self.OBJECT_DISTANCE_BETWEEN_SPIRALS, self.SEGMENTS_PER_ROTATION, search_center.tag_id, + False ) self.prev_target = None diff --git a/src/navigation/trajectory.py b/src/navigation/trajectory.py index a44dceb3a..15c627bff 100644 --- a/src/navigation/trajectory.py +++ b/src/navigation/trajectory.py @@ -20,3 +20,94 @@ def increment_point(self) -> bool: """ self.cur_pt += 1 return self.cur_pt >= len(self.coordinates) + + def reset(self)-> None: + """ + Resets the trajectory back to its start + """ + self.cur_pt = 0 + + +@dataclass +class SearchTrajectory(Trajectory): + # Associated fiducial for this trajectory + fid_id: int + + @classmethod + def gen_spiral_coordinates( + cls, coverage_radius: float, distance_between_spirals: float, num_segments_per_rotation: int, insert_extra: bool + ) -> np.ndarray: + """ + Generates a set of coordinates for a spiral search pattern centered at the origin + :param coverage_radius: radius of the spiral search pattern (float) + :param distance_between_spirals: distance between each spiralradii = angles * (distance_between_spirals / (2*np.pi)) (float) + :param num_segments_per_rotation: number of segments that the spiral has per rotation (int) + :return: np.ndarray of coordinates + """ + # the number of spirals should ensure coverage of the entire radius. + # We add 1 to ensure that the last spiral covers the radius along the entire rotation, + # as otherwise we will just make the outermost point touch the radius + num_spirals = np.ceil(coverage_radius / distance_between_spirals).astype("int") + 1 + # the angles are evenly spaced between 0 and 2pi*num_segments_per_rotation (add one to the number of points because N+1 points make N segments) + angles = np.linspace(0, 2 * np.pi * num_spirals, num_segments_per_rotation * num_spirals + 1) + # radii are computed via following polar formula. + # This is correct because you want the radius to increase by 'distance_between_spirals' every 2pi radians (one rotation) + radii = angles * (distance_between_spirals / (2 * np.pi)) + # convert to cartesian coordinates + xcoords = np.cos(angles) * radii + ycoords = np.sin(angles) * radii + # we want to return as a 2D matrix where each row is a coordinate pair + # so we reshape x and y coordinates to be (n, 1) matricies then stack horizontally to get (n, 2) matrix + vertices = np.hstack((xcoords.reshape(-1, 1), ycoords.reshape(-1, 1))) + all_points = [] + print("VERTICES") + print(repr(vertices)) + if insert_extra: + for i in range(len(vertices) - 1): + all_points.append(vertices[i]) + vector = vertices[i+1] - vertices[i] + magnitude = np.linalg.norm(vector) + unit_vector = vector / magnitude + count = 0.0 + while(count < magnitude - 3.5): + all_points.append(all_points[-1]+(unit_vector*2.5)) + count += 2.5 + print("ALL POINTS:") + print(repr(np.array(all_points))) + return np.array(all_points) + + return vertices + + @classmethod + def spiral_traj( + cls, + center: np.ndarray, + coverage_radius: float, + distance_between_spirals: float, + segments_per_rotation: int, + fid_id: int, + insert_extra: bool + ): + """ + Generates a square spiral search pattern around a center position, assumes rover is at the center position + :param center: position to center spiral on (np.ndarray) + :param coverage_radius: radius of the spiral search pattern (float) + :param distance_between_spirals: distance between each spiral (float) + :param segments_per_rotation: number of segments per spiral (int), for example, 4 segments per rotation would be a square spiral, 8 segments per rotation would be an octagonal spiral + :param fid_id: tag id to associate with this trajectory (int) + :return: SearchTrajectory object + """ + zero_centered_spiral_r2 = cls.gen_spiral_coordinates( + coverage_radius, distance_between_spirals, segments_per_rotation, insert_extra + ) + + # numpy broadcasting magic to add center to each row of the spiral coordinates + spiral_coordinates_r2 = zero_centered_spiral_r2 + center + # add a column of zeros to make it 3D + spiral_coordinates_r3 = np.hstack( + (spiral_coordinates_r2, np.zeros(spiral_coordinates_r2.shape[0]).reshape(-1, 1)) + ) + return SearchTrajectory( + spiral_coordinates_r3, + fid_id, + ) diff --git a/src/navigation/water_bottle_search.py b/src/navigation/water_bottle_search.py new file mode 100644 index 000000000..e011cc30f --- /dev/null +++ b/src/navigation/water_bottle_search.py @@ -0,0 +1,185 @@ +from __future__ import annotations + +from typing import Optional + +import numpy as np + +import rospy +from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion +from std_msgs.msg import Header +from mrover.msg import GPSPointList +from nav_msgs.msg import Path +from navigation import approach_object, recovery, waypoint +from navigation.context import convert_cartesian_to_gps, Context +from navigation.trajectory import Trajectory, SearchTrajectory +from navigation.astar import AStar, SpiralEnd, NoPath +from util.ros_utils import get_rosparam +from util.state_lib.state import State + + +# REFERENCE: https://docs.google.com/document/d/18GjDWxIu5f5-N5t5UgbrZGdEyaDj9ZMEUuXex8-NKrA/edit +class WaterBottleSearchState(State): + """ + State when searching for the water bottle + Follows a search spiral but uses A* to avoid obstacles + """ + traj: SearchTrajectory # spiral + star_traj: Trajectory # returned by astar + prev_target: Optional[np.ndarray] = None + is_recovering: bool = False + context: Context + time_last_updated: rospy.Time + path_pub: rospy.Publisher + astar: AStar + + STOP_THRESH = get_rosparam("water_bottle_search/stop_thresh", 0.5) + DRIVE_FWD_THRESH = get_rosparam("water_bottle_search/drive_fwd_thresh", 0.34) + SPIRAL_COVERAGE_RADIUS = get_rosparam("water_bottle_search/coverage_radius", 10) + SEGMENTS_PER_ROTATION = get_rosparam( + "water_bottle_search/segments_per_rotation", 10 + ) # TODO: after testing, might need to change + DISTANCE_BETWEEN_SPIRALS = get_rosparam( + "water_bottle_search/distance_between_spirals", 3 + ) # TODO: after testing, might need to change + + def find_endpoint(self, end: np.ndarray) -> np.ndarray: + """ + A-STAR Algorithm: f(n) = g(n) + h(n) to find a path from the given start to the given end in the given costmap + :param end: next point in the spiral from traj in cartesian coordinates + :return: the end point in cartesian coordinates + """ + costmap2d = self.context.env.cost_map.data + # convert end to occupancy grid coordinates then node + endij = self.astar.cartesian_to_ij(end) + end_node = self.astar.Node(None, (endij[0], endij[1])) + + # check if end node is within range, if it is, check if it has a high cost + if ( + end_node.position[0] <= (costmap2d.shape[0] - 1) + and end_node.position[0] >= 0 + and end_node.position[1] <= (costmap2d.shape[1] - 1) + and end_node.position[1] >= 0 + ): + while(costmap2d[end_node.position[0], end_node.position[1]] >= 0.2): # TODO: find optimal value + # True if the trajectory is finished + if self.traj.increment_point(): + raise SpiralEnd() + # update end point to be the next point in the search spiral + endij = self.astar.cartesian_to_ij(self.traj.get_cur_pt()) + end_node = self.astar.Node(None, (endij[0], endij[1])) + print(f"End has high cost! new end: {endij}") + return self.traj.get_cur_pt() + + def on_enter(self, context) -> None: + self.context = context + search_center = context.course.current_waypoint() + if not self.is_recovering: + self.traj = SearchTrajectory.spiral_traj( + context.rover.get_pose().position[0:2], + self.SPIRAL_COVERAGE_RADIUS, + self.DISTANCE_BETWEEN_SPIRALS, + self.SEGMENTS_PER_ROTATION, + search_center.tag_id, + True + ) + origin = context.rover.get_pose().position[0:2] + self.astar = AStar(origin, context) + print(f"ORIGIN: {origin}") + self.prev_target = None + self.star_traj = Trajectory(np.array([])) + self.time_last_updated = rospy.get_time() + self.path_pub = rospy.Publisher("path", Path, queue_size=10) + + def on_exit(self, context) -> None: + self.context.costmap_listener.unregister() + + def on_loop(self, context) -> State: + # only update our costmap every 1 second + if rospy.get_time() - self.time_last_updated > 1: + cur_rover_pose = self.context.rover.get_pose().position[0:2] + end_point = self.find_endpoint(self.traj.get_cur_pt()[0:2]) + + # call A-STAR + print("RUN ASTAR") + try: + occupancy_list = self.astar.a_star(cur_rover_pose, end_point[0:2]) + except SpiralEnd as spiral_error: + # TODO: what to do in this case + self.traj.reset() + occupancy_list = None + except NoPath as path_error: + # increment end point + if self.traj.increment_point(): + # TODO: what to do in this case + self.traj.reset() + occupancy_list = None + if occupancy_list is None: + self.star_traj = Trajectory(np.array([])) + else: + cartesian_coords = self.astar.ij_to_cartesian(np.array(occupancy_list)) + print(f"{cartesian_coords}, shape: {cartesian_coords.shape}") + self.star_traj = Trajectory( + np.hstack((cartesian_coords, np.zeros((cartesian_coords.shape[0], 1)))) + ) # current point gets set back to 0 + + # create path type to publish planned path segments to see in rviz + path = Path() + poses = [] + path.header = Header() + path.header.frame_id = "map" + for coord in cartesian_coords: + pose_stamped = PoseStamped() + pose_stamped.header = Header() + pose_stamped.header.frame_id = "map" + point = Point(coord[0], coord[1], 0) + quat = Quaternion(0,0,0,1) + pose_stamped.pose = Pose(point, quat) + poses.append(pose_stamped) + path.poses = poses + self.path_pub.publish(path) + + self.time_last_updated = rospy.get_time() + + # continue executing the path from wherever it left off + target_pos = self.traj.get_cur_pt() + traj_target = True + # if there is an alternate path we need to take to avoid the obstacle, use that trajectory + if len(self.star_traj.coordinates) != 0: + target_pos = self.star_traj.get_cur_pt() + traj_target = False + cmd_vel, arrived = context.rover.driver.get_drive_command( + target_pos, + context.rover.get_pose(), + self.STOP_THRESH, + self.DRIVE_FWD_THRESH, + path_start=self.prev_target, + ) + if arrived: + self.prev_target = target_pos + # if our target was the search spiral point, only increment the spiral path + if traj_target: + print("arrived at sprial point") + # if we finish the spiral without seeing the object, move on with course + if self.traj.increment_point(): + return waypoint.WaypointState() + else: # otherwise, increment the astar path + # if we finish the astar path, then reset astar and increment the spiral path + if self.star_traj.increment_point(): + print("arrived at end of astar") + self.star_traj = Trajectory(np.array([])) + if self.traj.increment_point(): + return waypoint.WaypointState() + if context.rover.stuck: + context.rover.previous_state = self + self.is_recovering = True + return recovery.RecoveryState() + else: + self.is_recovering = False + context.search_point_publisher.publish( + GPSPointList([convert_cartesian_to_gps(pt) for pt in self.traj.coordinates]) + ) + context.rover.send_drive_command(cmd_vel) + + if context.env.current_target_pos() is not None and context.course.look_for_object(): + return approach_object.ApproachObjectState() + return self diff --git a/src/navigation/waypoint.py b/src/navigation/waypoint.py index 493c23d7a..d89695e4c 100644 --- a/src/navigation/waypoint.py +++ b/src/navigation/waypoint.py @@ -1,9 +1,11 @@ import tf2_ros - +import rospy from util.ros_utils import get_rosparam from util.state_lib.state import State -from navigation import search, recovery, approach_post, post_backup, state, approach_object, long_range +from navigation import search, recovery, approach_post, post_backup, state, approach_object, long_range, water_bottle_search +from mrover.msg import WaypointType +from mrover.srv import MoveCostMap class WaypointState(State): @@ -12,7 +14,16 @@ class WaypointState(State): NO_FIDUCIAL = get_rosparam("waypoint/no_fiducial", -1) def on_enter(self, context) -> None: - context.env.arrived_at_waypoint = False + #TODO: In context find the access for type of waypoint + + current_waypoint = context.course.current_waypoint() + if current_waypoint.type.val == WaypointType.WATER_BOTTLE: + rospy.wait_for_service("move_cost_map") + move_cost_map = rospy.ServiceProxy("move_cost_map", MoveCostMap) + try: + resp = move_cost_map(f"course{context.course.waypoint_index}") + except rospy.ServiceException as exc: + rospy.logerr(f"Service call failed: {exc}") def on_exit(self, context) -> None: pass @@ -53,6 +64,9 @@ def on_loop(self, context) -> State: if not context.course.look_for_post() and not context.course.look_for_object(): # We finished a regular waypoint, go onto the next one context.course.increment_waypoint() + elif current_waypoint.type.val == WaypointType.WATER_BOTTLE: + # We finished a waypoint associated with the water bottle, but we have not seen it yet + return water_bottle_search.WaterBottleSearchState() else: # We finished a waypoint associated with a post or mallet, but we have not seen it yet. return search.SearchState() diff --git a/src/perception/cost_map/README.md b/src/perception/cost_map/README.md new file mode 100644 index 000000000..b5046ea95 --- /dev/null +++ b/src/perception/cost_map/README.md @@ -0,0 +1,21 @@ +(WIP) + +### Code Layout + +- [cost_map.cpp](./cost_map.cpp) Mainly ROS node setup (topics, parameters, etc.) +- [cost_map.processing.cpp](./cost_map.processing.cpp) Processes ~~pointclouds~~ occupancy grids to continuously update the global terrain occupancy grid + +### Explanation + +The goal of this code is to take in a local occupancy grid / costmap (using http://docs.ros.org/en/melodic/api/nav_msgs/html/msg/OccupancyGrid.html) and output a global occupancy grid (again using http://docs.ros.org/en/melodic/api/nav_msgs/html/msg/OccupancyGrid.html). + +~~The goal of this code is to take in pointcloud data from the zed and output a cost map / occupancy grid that represents the difficulty of passing terrain.~~ + +When a new local occupancy grid is received, the nodelet transforms it into the global frame using tf_tree information about the rover's frame relative to a chosen origin. The resolution (size of each grid space in physical units), grid width (in physical units), and grid height (in physical units) is used to determine the size of the costmap and to which grid space a given cost point is used to update. For simplicity, the new grid space's cost is equal to the maximum cost among the point cloud points that fall within the grid space. The cost map is then published for use by navigation nodelets. + +~~When a new pointcloud is received, the nodelet first calculates the costs associated with each point in the pointcloud. The cost at a particular location is 100 if the curvature estimate is above our threshold, 0 if it is not, or -1 if there is no data for that location. This discreteness is helpful for the navigation functions using this costmap, as it minimizes erratic movement to account for new data.~~ + +~~After costs are calculated, each point's location is transformed from the local basis (relative to the rover) to the global basis (relative to the starting location).~~ + +~~Processed cost point data is then used to update the rover's costmap. The resolution (size of each grid space in physical units), grid width (in physical units), and grid height (in physical units) is used to determine the size of the costmap and to which grid space a given cost point is used to update. For simplicity, the new grid space's cost is equal to the maximum cost among the point cloud points that fall within the grid space. The cost map is then published for use by navigation nodelets.~~ + diff --git a/src/perception/cost_map/cost_map.cpp b/src/perception/cost_map/cost_map.cpp new file mode 100644 index 000000000..81f39f1b1 --- /dev/null +++ b/src/perception/cost_map/cost_map.cpp @@ -0,0 +1,35 @@ +#include "cost_map.hpp" + +namespace mrover { + + auto CostMapNodelet::onInit() -> void { + mNh = getMTNodeHandle(); + mPnh = getMTPrivateNodeHandle(); // Unused for now + mNh.param("publish_cost_map", mPublishCostMap, true); + mNh.param("resolution", mResolution, 0.5); + mNh.param("map_width", mDimension, 30); + + mCostMapPub = mCmt.advertise("costmap", 1); // We publish our results to "costmap" + + mServer = mNh.advertiseService("move_cost_map", &CostMapNodelet::moveCostMapCallback, this); + + mPcSub = mNh.subscribe("camera/left/points", 1, &CostMapNodelet::pointCloudCallback, this); + + mGlobalGridMsg.info.resolution = mResolution; + mGlobalGridMsg.info.width = static_cast(mDimension / mResolution); + mGlobalGridMsg.info.height = static_cast(mDimension / mResolution); + // make center of map at (0, 0) + mGlobalGridMsg.info.origin.position.x = -1 * mDimension / 2; + mGlobalGridMsg.info.origin.position.y = -1 * mDimension / 2; + + mGlobalGridMsg.data.resize(mGlobalGridMsg.info.width * mGlobalGridMsg.info.height, UNKNOWN_COST); + } + + auto CostMapNodelet::moveCostMapCallback(MoveCostMap::Request& req, MoveCostMap::Response& res) -> bool { + SE3d waypointPos = SE3Conversions::fromTfTree(mTfBuffer, req.course, "map"); + mGlobalGridMsg.info.origin.position.x = waypointPos.x() - 1 * mDimension / 2; + mGlobalGridMsg.info.origin.position.y = waypointPos.y() - 1 * mDimension / 2; + res.success = true; + return true; + } +} // namespace mrover \ No newline at end of file diff --git a/src/perception/cost_map/cost_map.hpp b/src/perception/cost_map/cost_map.hpp new file mode 100644 index 000000000..c2b67486c --- /dev/null +++ b/src/perception/cost_map/cost_map.hpp @@ -0,0 +1,43 @@ +#pragma once + +#include "pch.hpp" + +namespace mrover { + + class CostMapNodelet final : public nodelet::Nodelet { + + constexpr static std::int8_t UNKNOWN_COST = -1, FREE_COST = 0, OCCUPIED_COST = 1; + + ros::NodeHandle mNh, mPnh, mCmt; + ros::Publisher mCostMapPub; + ros::Subscriber mPcSub; + + bool mPublishCostMap{}; // If set, publish the global costmap + float mResolution{}; // Meters per cell + float mDimension{}; // Dimensions of the square costmap in meters + double mNormalThreshold = 0.9; + int mDownSamplingFactor = 4; + std::uint32_t mNumPoints = 640 * 480 / mDownSamplingFactor / mDownSamplingFactor; + Eigen::MatrixXf mPointsInMap{4, mNumPoints}; + Eigen::MatrixXf mNormalsInMap{4, mNumPoints}; + + tf2_ros::Buffer mTfBuffer; + tf2_ros::TransformListener mTfListener{mTfBuffer}; + + std::optional mPreviousPose; + nav_msgs::OccupancyGrid mGlobalGridMsg; + + void onInit() override; + + ros::ServiceServer mServer; + + public: + CostMapNodelet() = default; + + ~CostMapNodelet() override = default; + + auto pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg) -> void; + auto moveCostMapCallback(MoveCostMap::Request& req, MoveCostMap::Response& res) -> bool; + }; + +} // namespace mrover diff --git a/src/perception/cost_map/cost_map.processing.cpp b/src/perception/cost_map/cost_map.processing.cpp new file mode 100644 index 000000000..7f411dfb8 --- /dev/null +++ b/src/perception/cost_map/cost_map.processing.cpp @@ -0,0 +1,53 @@ +#include + +#include "cost_map.hpp" + +namespace mrover { + + auto CostMapNodelet::pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg) -> void { + assert(msg); + assert(msg->height > 0); + assert(msg->width > 0); + + if (!mPublishCostMap) return; + + try { + SE3f cameraToMap = SE3Conversions::fromTfTree(mTfBuffer, "zed_left_camera_frame", "map").cast(); + auto* points = reinterpret_cast(msg->data.data()); + for (std::size_t i = 0, r = 0; r < msg->height; r += mDownSamplingFactor) { + for (std::size_t c = 0; c < msg->width; c += mDownSamplingFactor) { + auto* point = points + r * msg->width + c; + Eigen::Vector4f pointInCamera{point->x, point->y, point->z, 1}; + Eigen::Vector4f normalInCamera{point->normal_x, point->normal_y, point->normal_z, 0}; + + mPointsInMap.col(static_cast(i)) = cameraToMap.transform() * pointInCamera; + mNormalsInMap.col(static_cast(i)) = cameraToMap.transform() * normalInCamera; + i++; + } + } + + for (Eigen::Index i = 0; i < mNumPoints; i++) { + double xInMap = mPointsInMap.col(i).x(); + double yInMap = mPointsInMap.col(i).y(); + Eigen::Vector3f normalInMap = mNormalsInMap.col(i).head<3>(); + + if (xInMap >= mGlobalGridMsg.info.origin.position.x && xInMap <= mGlobalGridMsg.info.origin.position.x + mDimension && + yInMap >= mGlobalGridMsg.info.origin.position.y && yInMap <= mGlobalGridMsg.info.origin.position.y + mDimension) { + int xIndex = std::floor((xInMap - mGlobalGridMsg.info.origin.position.x) / mGlobalGridMsg.info.resolution); + int yIndex = std::floor((yInMap - mGlobalGridMsg.info.origin.position.y) / mGlobalGridMsg.info.resolution); + auto costMapIndex = mGlobalGridMsg.info.width * yIndex + xIndex; + + // Z is the vertical component of the normal + // A small Z component indicates largely horizontal normal (surface is vertical) + std::int8_t cost = normalInMap.z() < mNormalThreshold ? OCCUPIED_COST : FREE_COST; + + mGlobalGridMsg.data[costMapIndex] = std::max(mGlobalGridMsg.data[costMapIndex], cost); + } + } + mCostMapPub.publish(mGlobalGridMsg); + } catch (...) { + // TODO(neven): Catch TF exceptions only, and log warn them + } + } + +} // namespace mrover \ No newline at end of file diff --git a/src/perception/cost_map/main.cpp b/src/perception/cost_map/main.cpp new file mode 100644 index 000000000..1850df82a --- /dev/null +++ b/src/perception/cost_map/main.cpp @@ -0,0 +1,22 @@ +#include "cost_map.hpp" + +#ifdef MROVER_IS_NODELET + +#include +PLUGINLIB_EXPORT_CLASS(mrover::CostMapNodelet, nodelet::Nodelet) + +#else + +int main(int argc, char** argv) { + ros::init(argc, argv, "costmap"); + + // Start Costmap Nodelet + nodelet::Loader nodelet; + nodelet.load(ros::this_node::getName(), "mrover/CostMapNodelet", ros::names::getRemappings(), {}); + + ros::spin(); + + return EXIT_SUCCESS; +} + +#endif diff --git a/src/perception/cost_map/pch.hpp b/src/perception/cost_map/pch.hpp new file mode 100644 index 000000000..63ff6fe5f --- /dev/null +++ b/src/perception/cost_map/pch.hpp @@ -0,0 +1,21 @@ +#pragma once + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include diff --git a/src/perception/zed_wrapper/zed_wrapper.bridge.cu b/src/perception/zed_wrapper/zed_wrapper.bridge.cu index cacb63f77..0e9088e11 100644 --- a/src/perception/zed_wrapper/zed_wrapper.bridge.cu +++ b/src/perception/zed_wrapper/zed_wrapper.bridge.cu @@ -10,7 +10,7 @@ namespace mrover { /** * @brief Runs on the GPU, interleaving the XYZ and BGRA buffers into a single buffer of #Point structs. */ - __global__ void fillPointCloudMessageKernel(sl::float4* xyzGpuPtr, sl::uchar4* bgraGpuPtr, Point* pcGpuPtr, size_t size) { + __global__ void fillPointCloudMessageKernel(sl::float4* xyzGpuPtr, sl::uchar4* bgraGpuPtr, sl::float4* normalsGpuPtr, Point* pcGpuPtr, size_t size) { // This function is invoked once per element at index #i in the point cloud size_t i = blockIdx.x * blockDim.x + threadIdx.x; if (i >= size) return; @@ -22,6 +22,10 @@ namespace mrover { pcGpuPtr[i].g = bgraGpuPtr[i].g; pcGpuPtr[i].r = bgraGpuPtr[i].b; pcGpuPtr[i].a = bgraGpuPtr[i].a; + pcGpuPtr[i].normal_x = normalsGpuPtr[i].x; + pcGpuPtr[i].normal_x = normalsGpuPtr[i].y; + pcGpuPtr[i].normal_x = normalsGpuPtr[i].z; + // ROS_WARN(xyzGpuPtr[i].x); } /** @@ -32,7 +36,7 @@ namespace mrover { * @param pcGpu Point cloud buffer on the GPU (@see Point) * @param msg Point cloud message with buffer on the CPU */ - void fillPointCloudMessageFromGpu(sl::Mat& xyzGpu, sl::Mat& bgraGpu, PointCloudGpu& pcGpu, sensor_msgs::PointCloud2Ptr const& msg) { + void fillPointCloudMessageFromGpu(sl::Mat& xyzGpu, sl::Mat& bgraGpu, sl::Mat& normalsGpu, PointCloudGpu& pcGpu, sensor_msgs::PointCloud2Ptr const& msg) { assert(bgraGpu.getWidth() >= xyzGpu.getWidth()); assert(bgraGpu.getHeight() >= xyzGpu.getHeight()); assert(bgraGpu.getChannels() == 4); @@ -41,6 +45,7 @@ namespace mrover { auto* bgraGpuPtr = bgraGpu.getPtr(sl::MEM::GPU); auto* xyzGpuPtr = xyzGpu.getPtr(sl::MEM::GPU); + auto* normalsGpuPtr = normalsGpu.getPtr(sl::MEM::GPU); msg->is_bigendian = __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__; msg->is_dense = true; msg->height = bgraGpu.getHeight(); @@ -52,7 +57,7 @@ namespace mrover { Point* pcGpuPtr = pcGpu.data().get(); dim3 threadsPerBlock{BLOCK_SIZE}; dim3 numBlocks{static_cast(std::ceil(static_cast(size) / BLOCK_SIZE))}; - fillPointCloudMessageKernel<<>>(xyzGpuPtr, bgraGpuPtr, pcGpuPtr, size); + fillPointCloudMessageKernel<<>>(xyzGpuPtr, bgraGpuPtr, normalsGpuPtr, pcGpuPtr, size); checkCudaError(cudaPeekAtLastError()); checkCudaError(cudaMemcpy(msg->data.data(), pcGpuPtr, size * sizeof(Point), cudaMemcpyDeviceToHost)); } diff --git a/src/perception/zed_wrapper/zed_wrapper.cpp b/src/perception/zed_wrapper/zed_wrapper.cpp index cdeb3a5dd..c59817229 100644 --- a/src/perception/zed_wrapper/zed_wrapper.cpp +++ b/src/perception/zed_wrapper/zed_wrapper.cpp @@ -71,6 +71,7 @@ namespace mrover { mImageResolution = sl::Resolution(imageWidth, imageHeight); mPointResolution = sl::Resolution(imageWidth, imageHeight); + mNormalsResolution = sl::Resolution(imageWidth, imageHeight); NODELET_INFO("Resolution: %s image: %zux%zu points: %zux%zu", grabResolutionString.c_str(), mImageResolution.width, mImageResolution.height, mPointResolution.width, mPointResolution.height); @@ -142,7 +143,7 @@ namespace mrover { mIsSwapReady = false; mPcThreadProfiler.measureEvent("Wait"); - fillPointCloudMessageFromGpu(mPcMeasures.leftPoints, mPcMeasures.leftImage, mPointCloudGpu, pointCloudMsg); + fillPointCloudMessageFromGpu(mPcMeasures.leftPoints, mPcMeasures.leftImage, mPcMeasures.leftNormals, mPointCloudGpu, pointCloudMsg); pointCloudMsg->header.stamp = mPcMeasures.time; pointCloudMsg->header.frame_id = "zed_left_camera_frame"; mPcThreadProfiler.measureEvent("Fill Message"); @@ -223,6 +224,8 @@ namespace mrover { throw std::runtime_error("ZED failed to retrieve left image"); if (mZed.retrieveMeasure(mGrabMeasures.leftPoints, sl::MEASURE::XYZ, sl::MEM::GPU, mPointResolution) != sl::ERROR_CODE::SUCCESS) throw std::runtime_error("ZED failed to retrieve point cloud"); + if (mZed.retrieveMeasure(mGrabMeasures.leftNormals, sl::MEASURE::NORMALS, sl::MEM::GPU, mNormalsResolution) != sl::ERROR_CODE::SUCCESS) + throw std::runtime_error("ZED failed to retrieve point cloud normals"); assert(mGrabMeasures.leftImage.timestamp == mGrabMeasures.leftPoints.timestamp); @@ -307,6 +310,7 @@ namespace mrover { sl::Mat::swap(other.leftImage, leftImage); sl::Mat::swap(other.rightImage, rightImage); sl::Mat::swap(other.leftPoints, leftPoints); + sl::Mat::swap(other.leftNormals, leftNormals); std::swap(time, other.time); return *this; } diff --git a/src/perception/zed_wrapper/zed_wrapper.hpp b/src/perception/zed_wrapper/zed_wrapper.hpp index 4b16ee7c8..eb242402a 100644 --- a/src/perception/zed_wrapper/zed_wrapper.hpp +++ b/src/perception/zed_wrapper/zed_wrapper.hpp @@ -12,6 +12,7 @@ namespace mrover { sl::Mat leftImage; sl::Mat rightImage; sl::Mat leftPoints; + sl::Mat leftNormals; Measures() = default; @@ -31,7 +32,7 @@ namespace mrover { PointCloudGpu mPointCloudGpu; - sl::Resolution mImageResolution, mPointResolution; + sl::Resolution mImageResolution, mPointResolution, mNormalsResolution; sl::String mSvoPath; int mGrabTargetFps{}; int mDepthConfidence{}; @@ -68,7 +69,7 @@ namespace mrover { auto slTime2Ros(sl::Timestamp t) -> ros::Time; - auto fillPointCloudMessageFromGpu(sl::Mat& xyzGpu, sl::Mat& bgraGpu, PointCloudGpu& pcGpu, sensor_msgs::PointCloud2Ptr const& msg) -> void; + void fillPointCloudMessageFromGpu(sl::Mat& xyzGpu, sl::Mat& bgraGpu, sl::Mat& normalsGpu, PointCloudGpu& pcGpu, sensor_msgs::PointCloud2Ptr const& msg); auto fillCameraInfoMessages(sl::CalibrationParameters& calibration, sl::Resolution const& resolution, sensor_msgs::CameraInfoPtr const& leftInfoMsg, sensor_msgs::CameraInfoPtr const& rightInfoMsg) -> void; diff --git a/srv/MoveCostMap.srv b/srv/MoveCostMap.srv new file mode 100644 index 000000000..f4b1b49d8 --- /dev/null +++ b/srv/MoveCostMap.srv @@ -0,0 +1,3 @@ +string course +--- +bool success \ No newline at end of file diff --git a/urdf/meshes/bottle.fbx b/urdf/meshes/bottle.fbx index 25d7a8eed..5602ef9c2 100644 --- a/urdf/meshes/bottle.fbx +++ b/urdf/meshes/bottle.fbx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:df9dfe054c07005d820d7c81906136bd1bf621ebf4e4dcf3fbfb52f7e0fb4716 +oid sha256:a6eda3d1f92892e01fcf2ad3dff4421d59ef1f4688fbe8e7e700151065a5b6be size 17596 diff --git a/urdf/meshes/ground.fbx b/urdf/meshes/ground.fbx index 05b93f260..1e06dd797 100644 --- a/urdf/meshes/ground.fbx +++ b/urdf/meshes/ground.fbx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:8323c262d0e44f0f1d128a3c68752ef95b70a9b4602de2ff06f43cef106d4b31 -size 43772 +oid sha256:eb9df0b23202bdbaf6319b67e2e723197edec84fba09b53ee2c18ffbdf454626 +size 39708 diff --git a/urdf/meshes/ground4.fbx b/urdf/meshes/ground4.fbx new file mode 100644 index 000000000..536474571 --- /dev/null +++ b/urdf/meshes/ground4.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:aa35ccd62d8ed61e394b19c204f09bb708d9fbe41877a2069a9550034bc54bc6 +size 36396 diff --git a/urdf/meshes/ground5.fbx b/urdf/meshes/ground5.fbx new file mode 100644 index 000000000..ba78f106c --- /dev/null +++ b/urdf/meshes/ground5.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:48d228d1446e1daff8c87564fb6db10961cb629866f98f31855283460ff7af2f +size 39180 diff --git a/urdf/meshes/groundka.fbx b/urdf/meshes/groundka.fbx new file mode 100644 index 000000000..ae7048eb3 --- /dev/null +++ b/urdf/meshes/groundka.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4f9c96351ab4c6198aee86fa5387078f9e24b2829581a8a14dc41e889b04a467 +size 43468 diff --git a/urdf/meshes/groundkay.fbx b/urdf/meshes/groundkay.fbx new file mode 100644 index 000000000..ebe4d5e8c --- /dev/null +++ b/urdf/meshes/groundkay.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8838030f2b4c57a71b66c9ffcc29487d4bb8fee2f546decb61dd4100621dac1a +size 40380 diff --git a/urdf/staging/ground4.blend b/urdf/staging/ground4.blend new file mode 100644 index 000000000..1e812cf51 --- /dev/null +++ b/urdf/staging/ground4.blend @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ab0ee4bf7fef788a2b7773fec19bb1c561bc79592b19bd5b7b21a2f8f9c984c8 +size 1007984 diff --git a/urdf/staging/ground5.blend b/urdf/staging/ground5.blend new file mode 100644 index 000000000..a398c12b1 --- /dev/null +++ b/urdf/staging/ground5.blend @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4a50e943e8573b690a57d3ba0894b31c8f050b1465d909f05da19241bbff49c2 +size 999228 diff --git a/urdf/staging/groundka.blend b/urdf/staging/groundka.blend new file mode 100644 index 000000000..f324660d2 --- /dev/null +++ b/urdf/staging/groundka.blend @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8df22a59b1326ec66b3f3dd8f45d144368ec30f6b2ed5fef11abcc1577eeff8e +size 999012 diff --git a/urdf/staging/groundkay.blend b/urdf/staging/groundkay.blend new file mode 100644 index 000000000..f07b5a190 --- /dev/null +++ b/urdf/staging/groundkay.blend @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:56f0bcf481f686ba191e57787b5f0a43ccc5824d93f62558dbe0d44bcbb5d832 +size 993092