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: 10
+
 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")