Costmap

CMakeLists.txt

@@ -163,6 +163,9 @@ endif ()
 
 ## 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(streaming src/esw/streaming/*.cpp src/esw/streaming)
 target_compile_definitions(streaming PUBLIC BOOST_ASIO_NO_DEPRECATED)
 

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: 8
+
 object_search:
   coverage_radius: 10
   distance_between_spirals: 3

config/rviz/auton_sim.rviz

@@ -82,7 +82,7 @@ Visualization Manager:
           Value: true
         right_camera_link:
           Value: true
-        zed2i_left_camera_frame:
+        zed_left_camera_frame:
           Value: true
       Marker Alpha: 1
       Marker Scale: 1
@@ -112,7 +112,7 @@ Visualization Manager:
                 {}
               right_camera_link:
                 {}
-            zed2i_left_camera_frame:
+            zed_left_camera_frame:
               {}
       Update Interval: 0
       Value: true

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

package.xml

@@ -85,5 +85,6 @@
     <nodelet plugin="${prefix}/plugins/can.xml" />
     <nodelet plugin="${prefix}/plugins/simulator.xml" />
     <nodelet plugin="${prefix}/plugins/nv_gst_h265_enc.xml" />
+    <nodelet plugin="${prefix}/plugins/costmap.xml" />
   </export>
 </package>

plugins/costmap.xml

@@ -0,0 +1,6 @@
+<library path="lib/libcostmap_nodelet">
+    <class name="mrover/CostMapNodelet"
+           type="mrover::CostMapNodelet"
+           base_class_type="nodelet::Nodelet">
+    </class>
+</library>

pyproject.toml

@@ -26,6 +26,7 @@ dependencies = [
     "aenum==3.1.15",
     "daphne==4.0.0",
     "channels==4.0.0",
+    "scipy==1.12.0"
 ]
 
 [project.optional-dependencies]

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])),
+                # ),
             ]
         ]
     )

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

src/navigation/context.py

@@ -9,6 +9,7 @@
 import rospy
 import tf2_ros
 from geometry_msgs.msg import Twist
+from util.SE3 import SE3
 from mrover.msg import (
     Waypoint,
     GPSWaypoint,
@@ -119,7 +120,7 @@ def current_target_pos(self, odom_override: bool = True) -> Optional[np.ndarray]
             return self.get_target_pos(f"fiducial{current_waypoint.tag_id}", in_odom)
         elif current_waypoint.type.val == WaypointType.MALLET:
             return self.get_target_pos("hammer", in_odom)
-        elif current_waypoint.type == WaypointType.WATER_BOTTLE:
+        elif current_waypoint.type.val == WaypointType.WATER_BOTTLE:
             return self.get_target_pos("bottle", in_odom)
         else:
             return None

src/navigation/drive.py

@@ -141,6 +141,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
@@ -192,7 +193,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

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(),
@@ -62,6 +64,9 @@ def __init__(self, context: Context):
         self.state_machine.add_transitions(ApproachObjectState(), [DoneState(), SearchState(), RecoveryState()])
         self.state_machine.add_transitions(LongRangeState(), [ApproachPostState(), SearchState(), 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)
 

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