diff --git a/CMakeLists.txt b/CMakeLists.txt index fa781871d..6ece9a7c3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -182,6 +182,9 @@ mrover_add_esw_bridge_node(science_bridge src/esw/science_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 4f58e028b..263fa1a28 100644 --- a/config/navigation.yaml +++ b/config/navigation.yaml @@ -14,6 +14,13 @@ search: segments_per_rotation: 8 distance_between_spirals: 3 +water_bottle_search: + stop_threshold: 0.5 + drive_forward_threshold: 0.34 + coverage_radius: 10 + segments_per_rotation: 10 + distance_between_spirals: 3 + 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 940662d26..fa4efcf13 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -15,13 +15,13 @@ dependencies = [ "defusedxml==0.7.1", # MRover dependencies "Django==4.2.11", - "empy==3.3.4", "numpy==1.26.4", + "scipy==1.13.0", "pandas==2.2.0", "pyarrow==15.0.0", "shapely==2.0.1", "pyserial==3.5", - "moteus==0.3.67", + "moteus==0.3.69", "pymap3d==3.0.1", "daphne==4.0.0", "channels==4.0.0", diff --git a/scripts/debug_course_publisher.py b/scripts/debug_course_publisher.py index ff5b34384..540991a31 100755 --- a/scripts/debug_course_publisher.py +++ b/scripts/debug_course_publisher.py @@ -20,6 +20,10 @@ [ convert_waypoint_to_gps(waypoint) for waypoint in [ + # ( + # 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.NO_SEARCH)), SE3(position=np.array([4, 4, 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..b85760dc2 --- /dev/null +++ b/src/navigation/astar.py @@ -0,0 +1,256 @@ +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 # Holds the initial 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 + :return: reversed path except the starting point (we are already there) + """ + costmap_2d = 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: + costmap_2d[step[0]][step[1]] = 2 # path (.) + costmap_2d[reversed_path[0][0]][reversed_path[0][1]] = 3 # start + costmap_2d[reversed_path[-1][0]][reversed_path[-1][1]] = 4 # end + + for row in costmap_2d: + line = [] + for col in row: + if col == 1.0: + line.append("\u2588") + elif 1.0 > col >= 0.8: + line.append("\u2593") + elif 0.8 > col >= 0.5: + line.append("\u2592") + elif 0.5 > 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 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 terrain (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() diff --git a/src/navigation/context.py b/src/navigation/context.py index bc566b15f..9b90ec538 100644 --- a/src/navigation/context.py +++ b/src/navigation/context.py @@ -1,10 +1,11 @@ from __future__ import annotations from dataclasses import dataclass -from typing import ClassVar, Optional, List, Tuple +from typing import Optional, List, Tuple import numpy as np import pymap3d +from scipy.signal import convolve2d import rospy import tf2_ros @@ -19,6 +20,7 @@ LongRangeTags, ) from mrover.srv import EnableAuton, EnableAutonRequest, EnableAutonResponse +from nav_msgs.msg import OccupancyGrid from navigation import approach_post, long_range, approach_object from navigation.drive import DriveController from std_msgs.msg import Time, Bool @@ -38,7 +40,6 @@ tf_broadcaster: tf2_ros.StaticTransformBroadcaster = tf2_ros.StaticTransformBroadcaster() - @dataclass class Rover: ctx: Context @@ -66,10 +67,12 @@ class Environment: Information such as locations of tags or obstacles """ - NO_TAG: ClassVar[int] = -1 - ctx: Context long_range_tags: LongRangeTagStore + cost_map: CostMap + + NO_TAG: int = -1 + arrived_at_target: bool = False arrived_at_waypoint: bool = False last_target_location: Optional[np.ndarray] = None @@ -185,6 +188,17 @@ def query(self, tag_id: int) -> Optional[TagData]: return self._data[tag_id] +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 @@ -302,6 +316,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] @@ -322,11 +337,12 @@ def __init__(self) -> None: 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.world_frame = rospy.get_param("world_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: @@ -340,3 +356,22 @@ def stuck_callback(self, msg: Bool) -> None: 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 degrees 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 44f0608a2..fb909e853 100644 --- a/src/navigation/drive.py +++ b/src/navigation/drive.py @@ -122,6 +122,7 @@ def compute_lookahead_point( :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 diff --git a/src/navigation/failure_identification/failure_identification.py b/src/navigation/failure_identification/failure_identification.py index b1508955b..d8cb20ba1 100755 --- a/src/navigation/failure_identification/failure_identification.py +++ b/src/navigation/failure_identification/failure_identification.py @@ -12,12 +12,13 @@ from mrover.msg import StateMachineStateUpdate from nav_msgs.msg import Odometry from navigation.failure_identification.watchdog import WatchDog -from sensor_msgs.msg import JointState from std_msgs.msg import Bool -from util.ros_utils import get_rosparam -DATAFRAME_MAX_SIZE = get_rosparam("failure_identification/dataframe_max_size", 200) -POST_RECOVERY_GRACE_PERIOD = get_rosparam("failure_identification/post_recovery_grace_period", 5.0) +DATAFRAME_MAX_SIZE = rospy.get_param("failure_identification/dataframe_max_size") +POST_RECOVERY_GRACE_PERIOD = rospy.get_param("failure_identification/post_recovery_grace_period") + +# test recovery state using the stuck button on the GUI rather than analyzing data +TEST_RECOVERY_STATE = rospy.get_param("failure_identification/test_recovery_state", False) class FailureIdentifier: @@ -39,7 +40,6 @@ class FailureIdentifier: def __init__(self): nav_status_sub = message_filters.Subscriber("nav_state", StateMachineStateUpdate) - drive_status_sub = message_filters.Subscriber("drive_joint_data", JointState) odometry_sub = message_filters.Subscriber("/odometry", Odometry) ts = message_filters.ApproximateTimeSynchronizer([nav_status_sub, odometry_sub], 10, 1.0, allow_headerless=True) @@ -101,14 +101,11 @@ def update(self, nav_status: StateMachineStateUpdate, odometry: Odometry) -> Non Updates the current row of the data frame with the latest data from the rover then appends the row to the data frame @param nav_status: the current state of the rover, used to determine if the rover is already recovering - @param drive_status: the current status of the rovers motors, has velocity and effort data @param odometry: the current odometry of the rover, has position and velocity data publishes a message to the /nav_stuck topic indicating if the rover is stuck """ - # test recovery state using the stuck button on the GUI rather than analyzing data - TEST_RECOVERY_STATE = get_rosparam("failure_identification/test_recovery_state", False) # if the state is 'done' or 'off', write the data frame to a csv file if we were collecting if nav_status.state == "DoneState" or nav_status.state == "OffState": self.write_to_csv() diff --git a/src/navigation/failure_identification/watchdog.py b/src/navigation/failure_identification/watchdog.py index ead04e74f..00f95eb75 100644 --- a/src/navigation/failure_identification/watchdog.py +++ b/src/navigation/failure_identification/watchdog.py @@ -1,15 +1,14 @@ from typing import Tuple -import rospy import numpy as np import pandas as pd +import rospy from util.SO3 import SO3 -from util.ros_utils import get_rosparam -WINDOW_SIZE = get_rosparam("watchdog/window_size", 100) -ANGULAR_THRESHOLD = get_rosparam("watchdog/angular_threshold", 0.001) -LINEAR_THRESHOLD = get_rosparam("watchdog/linear_threshold", 0.55) +WINDOW_SIZE = rospy.get_param("watchdog/window_size") +ANGULAR_THRESHOLD = rospy.get_param("watchdog/angular_threshold") +LINEAR_THRESHOLD = rospy.get_param("watchdog/linear_threshold") class WatchDog: diff --git a/src/navigation/long_range.py b/src/navigation/long_range.py index 555fa3b24..ca30e5c16 100644 --- a/src/navigation/long_range.py +++ b/src/navigation/long_range.py @@ -1,11 +1,11 @@ from typing import Optional import numpy as np +from scipy.spatial.transform import Rotation import rospy from navigation import approach_post, recovery from navigation.approach_target_base import ApproachTargetBaseState -from util.np_utils import normalized from util.state_lib.state import State DISTANCE_AHEAD = rospy.get_param("long_range/distance_ahead") @@ -46,14 +46,10 @@ def get_target_position(self, context) -> Optional[np.ndarray]: if tag.hit_count <= 0: bearing_to_tag = 0 - # rover_direction rotated by bearing to tag - bearing_rotation_mat = np.array( - [[np.cos(bearing_to_tag), -np.sin(bearing_to_tag)], [np.sin(bearing_to_tag), np.cos(bearing_to_tag)]] - ) + bearing_rotation_mat = Rotation.from_rotvec([0, 0, bearing_to_tag]).as_matrix() - direction_to_tag = bearing_rotation_mat @ rover_direction[:2] + direction_to_tag = bearing_rotation_mat[:2, :2] @ rover_direction[:2] - direction_to_tag = normalized(direction_to_tag) distance = DISTANCE_AHEAD direction_to_tag = np.array([direction_to_tag[0], direction_to_tag[1], 0.0]) tag_position = rover_position + direction_to_tag * distance diff --git a/src/navigation/navigation.py b/src/navigation/navigation.py index 4488a30d4..c28983d4a 100755 --- a/src/navigation/navigation.py +++ b/src/navigation/navigation.py @@ -12,6 +12,7 @@ from navigation.recovery import RecoveryState from navigation.search import SearchState from navigation.state import DoneState, OffState, off_check +from navigation.water_bottle_search import WaterBottleSearchState from navigation.waypoint import WaypointState @@ -50,6 +51,7 @@ def __init__(self, context: Context): PostBackupState(), ApproachPostState(), ApproachObjectState(), + WaterBottleSearchState(), LongRangeState(), SearchState(), RecoveryState(), @@ -61,6 +63,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 55ecbe430..53eb795f5 100644 --- a/src/navigation/search.py +++ b/src/navigation/search.py @@ -1,6 +1,3 @@ -from __future__ import annotations - -from dataclasses import dataclass from typing import Optional import numpy as np @@ -9,76 +6,10 @@ from mrover.msg import GPSPointList, WaypointType from navigation import recovery, waypoint from navigation.context import convert_cartesian_to_gps -from navigation.trajectory import Trajectory +from navigation.trajectory import SearchTrajectory from util.state_lib.state import State -@dataclass -class SearchTrajectory(Trajectory): - # Associated tag for this trajectory - tag_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, - tag_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 tag_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, - tag_id, - ) - - class SearchState(State): traj: SearchTrajectory prev_target: Optional[np.ndarray] = None @@ -104,6 +35,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( @@ -112,6 +44,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..a9d849c4e 100644 --- a/src/navigation/trajectory.py +++ b/src/navigation/trajectory.py @@ -20,3 +20,95 @@ 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 tag for this trajectory + tag_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, + tag_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 tag_id: tag id to associate with this trajectory (int) + :param insert_extra: + :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, + tag_id, + ) diff --git a/src/navigation/water_bottle_search.py b/src/navigation/water_bottle_search.py new file mode 100644 index 000000000..93ec48cff --- /dev/null +++ b/src/navigation/water_bottle_search.py @@ -0,0 +1,180 @@ +from typing import Optional + +import numpy as np + +import rospy +from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion +from mrover.msg import GPSPointList +from nav_msgs.msg import Path +from navigation import approach_object, recovery, waypoint +from navigation.astar import AStar, SpiralEnd, NoPath +from navigation.context import convert_cartesian_to_gps, Context +from navigation.trajectory import Trajectory, SearchTrajectory +from std_msgs.msg import Header +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 = rospy.get_param("water_bottle_search/stop_threshold") + DRIVE_FWD_THRESH = rospy.get_param("water_bottle_search/drive_forward_threshold") + SPIRAL_COVERAGE_RADIUS = rospy.get_param("water_bottle_search/coverage_radius") + SEGMENTS_PER_ROTATION = rospy.get_param( + "water_bottle_search/segments_per_rotation", + ) # TODO: after testing, might need to change + DISTANCE_BETWEEN_SPIRALS = rospy.get_param( + "water_bottle_search/distance_between_spirals" + ) # 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 + """ + costmap_2d = self.context.env.cost_map.data + # convert end to occupancy grid coordinates then node + end_ij = self.astar.cartesian_to_ij(end) + end_node = self.astar.Node(None, (end_ij[0], end_ij[1])) + + # check if end node is within range, if it is, check if it has a high cost + if (costmap_2d.shape[0] - 1) >= end_node.position[0] >= 0 and (costmap_2d.shape[1] - 1) >= end_node.position[ + 1 + ] >= 0: + while costmap_2d[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 + end_ij = self.astar.cartesian_to_ij(self.traj.get_cur_pt()) + end_node = self.astar.Node(None, (end_ij[0], end_ij[1])) + print(f"End has high cost! new end: {end_ij}") + 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.Time.now() + 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.Time.now() - self.time_last_updated > rospy.Duration(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: + # TODO: what to do in this case + self.traj.reset() + occupancy_list = None + except NoPath: + # 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.Time.now() + + # 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 spiral 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 72d7ee373..7ea425113 100644 --- a/src/navigation/waypoint.py +++ b/src/navigation/waypoint.py @@ -1,18 +1,32 @@ -from typing import ClassVar - import rospy -from navigation import search, recovery, post_backup, state -from navigation.context import Context +from mrover.msg import WaypointType +from mrover.srv import MoveCostMap +from navigation import ( + search, + recovery, + post_backup, + state, + water_bottle_search, +) from util.state_lib.state import State class WaypointState(State): - STOP_THRESHOLD = rospy.get_param("waypoint/stop_threshold") - DRIVE_FORWARD_THRESHOLD = rospy.get_param("waypoint/drive_forward_threshold") - NO_TAG: ClassVar[int] = -1 + STOP_THRESHOLD: float = rospy.get_param("waypoint/stop_threshold") + DRIVE_FORWARD_THRESHOLD: float = rospy.get_param("waypoint/drive_forward_threshold") + NO_TAG: int = -1 + + def on_enter(self, context) -> None: + # TODO: In context find the access for type of waypoint - def on_enter(self, context: Context) -> None: - context.env.arrived_at_waypoint = False + 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: + 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 @@ -52,6 +66,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/src/util/state_lib/state.py b/src/util/state_lib/state.py index 1ba0c1675..8f7abc796 100644 --- a/src/util/state_lib/state.py +++ b/src/util/state_lib/state.py @@ -24,7 +24,7 @@ def on_exit(self, context) -> None: """ Called exactly once when the state is exited. No cleanup of internal state is necessary since this state will be destroyed. - An example usecase of this may be to write to an external log or send a service call to an external system. + An example use case of this may be to write to an external log or send a service call to an external system. :param context: The context object that is passed to the state machine. """ raise NotImplementedError @@ -54,8 +54,11 @@ def __ne__(self, other) -> bool: return not self.__eq__(other) -##state to be returned to signfy that the state machine should exit class ExitState(State): + """ + State to be returned to signify that the state machine should exit. + """ + def on_enter(self, ctx): pass 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