From fb38bf0edf8594196073d64fbeceb429ee663478 Mon Sep 17 00:00:00 2001 From: Quintin Date: Sat, 14 Oct 2023 10:54:05 -0700 Subject: [PATCH 1/6] Simplify cmake list, add a bunch of utility macros, glob automatically for msgs and services (#591) --- CMakeLists.txt | 377 +++++++++++++++++++------------------------------ 1 file changed, 147 insertions(+), 230 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 96322401d..587e1f174 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,94 +3,69 @@ project(mrover VERSION 2024.0.0 LANGUAGES CXX) set(CMAKE_CXX_STANDARD 23) set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) # Generate compile_commands.json for clangd if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang") - add_link_options(-fuse-ld=lld-16) + add_link_options(-fuse-ld=lld-16) # LLVM lld is faster than GNU ld endif () if (CMAKE_BUILD_TYPE STREQUAL "Release") set(MROVER_CPP_COMPILE_OPTIONS -Wall -Wextra -Werror -pedantic) endif () -# Generate compile_commands.json for clangd -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - # ROS packages list -set(MROVER_PACKAGES +set(MROVER_ROS_DEPENDENCIES rospy roscpp + rostest + nodelet std_msgs + sensor_msgs message_generation dynamic_reconfigure - rostest - sensor_msgs + tf2 + tf2_ros + tf2_geometry_msgs + gazebo_ros ) -# Message files -set(MROVER_MESSAGE_FILES - CalibrationStatus.msg - CameraCmd.msg - CAN.msg - Chassis.msg - ControllerGroupState.msg - ControllerState.msg - Course.msg - EnableAuton.msg - GPSPointList.msg - GPSWaypoint.msg - HeaterData.msg - ImuAndMag.msg - LED.msg - MoteusState.msg - MotorsStatus.msg - NavMetadata.msg - NavState.msg - NetworkBandwidth.msg - PDB.msg - Position.msg - ScienceThermistors.msg - Spectral.msg - SpectralGroup.msg - Throttle.msg - Velocity.msg - Waypoint.msg - WaypointType.msg - GPSPointList.msg -) +# Search a path for all files matching a glob pattern and extract the filenames +macro(extract_filenames directory_path out_file_names) + file(GLOB_RECURSE full_paths ${directory_path}) + set(${out_file_names} "") + foreach (FULL_PATH ${full_paths}) + get_filename_component(FILE_NAME ${FULL_PATH} NAME) + list(APPEND ${out_file_names} ${FILE_NAME}) + endforeach () +endmacro() -# Service files -set(MROVER_SERVICE_FILES - AdjustMotor.srv - ChangeArmMode.srv - ChangeCameras.srv - FetchMessageFromPackage.srv - FetchPackages.srv - PublishCourse.srv - PublishEnableAuton.srv -) +extract_filenames(msg/*.msg MROVER_MESSAGE_FILES) -# Generate messages list -set(MROVER_ROS_MESSAGES - sensor_msgs +extract_filenames(srv/*.srv MROVER_SERVICE_FILES) + +set(MROVER_MESSAGE_DEPENDENCIES std_msgs sensor_msgs ) -# Dynamic reconfigure parameter file list set(MROVER_PARAMETERS config/DetectorParams.cfg ) -# catkin packages list -set(MROVER_CATKIN_PACKAGES - roscpp rospy std_msgs message_runtime +set(MROVER_CMAKE_INCLUDES + starter_project/autonomy/AutonomyStarterProject.cmake ) -macro(rosify_cpp_target_macro target) +### ====== ### +### Macros ### +### ====== ### + +macro(target_rosify target) target_link_libraries(${target} PRIVATE ${catkin_LIBRARIES}) target_include_directories(${target} SYSTEM PRIVATE ${catkin_INCLUDE_DIRS} src/util) add_dependencies(${target} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_compile_options(${target} PRIVATE $<$:${MROVER_CPP_COMPILE_OPTIONS}>) + # Installing is necessary for roslaunch to find the node install(TARGETS ${target} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} @@ -98,194 +73,97 @@ macro(rosify_cpp_target_macro target) ) endmacro() -macro(add_cpp_library_macro name sources includes) +macro(mrover_add_library name sources includes) file(GLOB_RECURSE CPP_LIB_SOURCES ${sources}) add_library(${name} ${CPP_LIB_SOURCES}) target_include_directories(${name} PUBLIC ${includes}) - rosify_cpp_target_macro(${name}) + target_rosify(${name}) endmacro() -macro(add_cpp_interface_library_macro name includes) +macro(mrover_add_header_only_library name includes) add_library(${name} INTERFACE) target_include_directories(${name} INTERFACE ${includes}) endmacro() -macro(add_cpp_node_macro name sources) - file(GLOB_RECURSE CPP_NODE_SOURCES ${sources}) - add_executable(${name} ${CPP_NODE_SOURCES}) - rosify_cpp_target_macro(${name}) +macro(mrover_add_node name sources) + file(GLOB_RECURSE NODE_SOURCES ${sources}) + add_executable(${name} ${NODE_SOURCES}) + target_rosify(${name}) endmacro() -macro(add_cpp_nodelet_macro name sources includes) +macro(mrover_add_nodelet name sources includes) # A nodelet runs inside another process so it is a library - add_cpp_library_macro(${name} ${sources} ${includes}) + mrover_add_library(${name}_node ${sources} ${includes}) + # Also add a node for quick debugging + mrover_add_node(${name}_nodelet ${sources}) + target_compile_definitions(${name}_nodelet PRIVATE MROVER_IS_NODELET) + # Optional PCH + if (ARGV3) + target_precompile_headers(${name}_node PRIVATE ${ARGV3}) + target_precompile_headers(${name}_nodelet PRIVATE ${ARGV3}) + endif () endmacro() -macro(add_gazebo_plugin_macro name sources includes) - add_cpp_library_macro(${name} ${sources} ${includes}) - - # TODO: find a proper variable name that points to /opt/ros/noetic/lib - target_link_directories(${name} PRIVATE ${GAZEBO_LIBRARY_DIRS} /opt/ros/noetic/lib) - target_link_libraries(${name} PRIVATE ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES}) - target_include_directories(${name} SYSTEM PRIVATE ${GAZEBO_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) - set_target_properties(${name} PROPERTIES CXX_STANDARD 17) +macro(mrover_nodelet_link_libraries name) + target_link_libraries(${name}_node PRIVATE ${ARGN}) + target_link_libraries(${name}_nodelet PRIVATE ${ARGN}) endmacro() -# launch install macro -macro(install_launch_macro) - install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch - ) +macro(mrover_nodelet_include_directories name) + target_include_directories(${name}_node SYSTEM PRIVATE ${ARGN}) + target_include_directories(${name}_nodelet SYSTEM PRIVATE ${ARGN}) endmacro() -macro(add_tests_macro) - # Add C++ unit tests - catkin_add_gtest(example-cpp-test test/example/cpp_test.cpp) - - # Python unit tests - catkin_add_nosetests(test/navigation/drive_test.py) - catkin_add_nosetests(test/teleop/teleop_test.py) - catkin_add_nosetests(test/util/SE3_test.py) - catkin_add_nosetests(test/util/SO3_test.py) - - # Integration tests (python and c++) - find_package(rostest REQUIRED) - add_rostest(test/example/basic_integration_test.test) - add_rostest(test/integration/integration.test) - add_rostest(test/util/SE3_tf_test.test) +macro(mrover_nodelet_defines name) + target_compile_definitions(${name}_node PRIVATE ${ARGN}) + target_compile_definitions(${name}_nodelet PRIVATE ${ARGN}) endmacro() -# Subdirectories before message declarations -set(MROVER_CMAKE_INCLUDES "") - - -# -=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- -# Specify build details by appending to lists -# and implementing the some extra macros -# Add new devices as elseif blocks -# Select device with --cmake-flags -D DEVICE= -# -=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- - -if (${DEVICE} MATCHES "raspi4") - # -=-=-=-=- - # Lists - # -=-=-=-=- - - # Add any raspi4 specific packages here - # list(APPEND _PACKAGES ) - - # -=-=-=-=- - # Macros - # -=-=-=-=- - - macro(include_directories_macro) - include_directories( - ${catkin_INCLUDE_DIRS} - ) - endmacro() - - # define an add and link macro - # Put items here to build - macro(add_and_link_macro) - # add_cpp_node_macro(arm_bridge "src/esw/arm_bridge/*") - endmacro() -else () - # -=-=-=-=- - # Lists - # -=-=-=-=- - - # Add any laptop specific packages here - list(APPEND MROVER_PACKAGES - tf2_geometry_msgs - tf2_ros - tf2 - gazebo_ros - ) - - # append subdirectories - list(APPEND MROVER_CMAKE_INCLUDES - starter_project/autonomy/AutonomyStarterProject.cmake - ) +macro(mrover_add_gazebo_plugin name sources includes) + mrover_add_library(${name} ${sources} ${includes}) + # TODO: find a proper variable name that points to /opt/ros/noetic/lib + target_link_directories(${name} PRIVATE ${GAZEBO_LIBRARY_DIRS} /opt/ros/noetic/lib) + target_link_libraries(${name} PRIVATE ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES}) + target_include_directories(${name} SYSTEM PRIVATE ${GAZEBO_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) + set_target_properties(${name} PROPERTIES CXX_STANDARD 17) +endmacro() - # -=-=-=-=- - # Macros - # -=-=-=-=- - - # These packages need to be found individually - macro(add_packages_macro) - find_package(OpenCV REQUIRED) - find_package(gazebo REQUIRED) - find_package(Eigen3 REQUIRED) - find_package(ZED 2 QUIET) - if (ZED_FOUND) - # Anything newer than C++17 combined with libstdc++13 is not supported just yet by NVCC (the CUDA compiler) - set(CMAKE_CUDA_STANDARD 17) - set(CMAKE_CUDA_STANDARD_REQUIRED ON) - set(CMAKE_CUDA_SEPARABLE_COMPILATION ON) - # Jetson Xavier NX is Volta 7.2 architecture - # Perception Laptop (A4000, Quadro version of RTX 3080) is Ampere 8.6 architecture - set(CMAKE_CUDA_ARCHITECTURES 72 86) - enable_language(CUDA) - endif () - endmacro() - - # define an add and link macro - # Put items here to build - macro(add_and_link_macro) - add_cpp_nodelet_macro(tag_detector_nodelet src/perception/tag_detector/*.cpp src/perception/tag_detector) - target_precompile_headers(tag_detector_nodelet PRIVATE src/perception/tag_detector/pch.hpp) - target_link_libraries(tag_detector_nodelet PRIVATE opencv_core opencv_objdetect opencv_aruco opencv_imgproc lie) - - add_cpp_library_macro(lie src/util/lie/*.cpp src/util/lie) - - add_cpp_interface_library_macro(moteus deps/moteus/lib/cpp/mjbots) - - add_gazebo_plugin_macro(differential_drive_plugin_6w src/simulator/differential_drive_6w.cpp src) - - add_gazebo_plugin_macro(kinect_plugin src/simulator/gazebo_ros_openni_kinect.cpp src/simulator) - target_link_libraries(kinect_plugin PRIVATE gazebo_ros_camera_utils DepthCameraPlugin Eigen3::Eigen) - set_target_properties(kinect_plugin PROPERTIES CXX_CLANG_TIDY "") - - add_cpp_node_macro(sim_arm_bridge src/simulator/arm_bridge/*.cpp) - - if (ZED_FOUND) - add_cpp_nodelet_macro(zed_nodelet src/perception/zed_wrapper/*.c* src/perception/zed_wrapper) - target_precompile_headers(zed_nodelet PRIVATE src/perception/zed_wrapper/pch.hpp) - target_include_directories(zed_nodelet SYSTEM PRIVATE ${ZED_INCLUDE_DIRS} ${CUDA_INCLUDE_DIRS}) - target_link_libraries(zed_nodelet PRIVATE ${ZED_LIBRARIES} ${OpenCV_LIBRARIES} ${SPECIAL_OS_LIBS} lie) - target_compile_definitions(zed_nodelet PRIVATE - MROVER_IS_NODELET # Generate code relevant to nodelet - ALLOW_BUILD_DEBUG # Ignore ZED warnings about Debug mode - __CUDA_INCLUDE_COMPILER_INTERNAL_HEADERS__ # Eigen includes some files it should not, ignore - ) - endif () - endmacro() +### ============ ### +### Dependencies ### +### ============ ### + +find_package(OpenCV REQUIRED) +find_package(ZED 2 QUIET) +find_package(gazebo REQUIRED) +find_package(Eigen3 REQUIRED) +if (ZED_FOUND) + # Anything newer than C++17 combined with libstdc++13 is not supported just yet by NVCC (the CUDA compiler) + set(CMAKE_CUDA_STANDARD 17) + set(CMAKE_CUDA_STANDARD_REQUIRED ON) + set(CMAKE_CUDA_SEPARABLE_COMPILATION ON) + set(CMAKE_CUDA_FLAGS "--diag-suppress 108,68") + # Jetson Xavier NX is Volta 7.2 architecture + # Perception Laptop (A4000, Quadro version of RTX 3080) is Ampere 8.6 architecture + set(CMAKE_CUDA_ARCHITECTURES 72 86) + enable_language(CUDA) endif () - -# 3. Find Packages find_package( catkin REQUIRED COMPONENTS - ${MROVER_PACKAGES} + ${MROVER_ROS_DEPENDENCIES} ) -if (COMMAND add_packages_macro) - add_packages_macro() -endif () - - -# 4. Python module support catkin_python_setup() - -# 4.5. CMake includes before message declarations -foreach (CMAKE_INCLUDE ${MROVER_CMAKE_INCLUDES}) - include(${CMAKE_INCLUDE}) +foreach (MROVER_CMAKE_INCLUDE ${MROVER_CMAKE_INCLUDES}) + include(${MROVER_CMAKE_INCLUDE}) endforeach () +### ======== ### +### Messages ### +### ======== ### -# 5. Message Generators (add_xxx) add_message_files( FILES ${MROVER_MESSAGE_FILES} @@ -296,38 +174,77 @@ add_service_files( ${MROVER_SERVICE_FILES} ) - -# 6. Invoke messages (generate_messages) generate_messages( DEPENDENCIES - ${MROVER_ROS_MESSAGES} + ${MROVER_MESSAGE_DEPENDENCIES} ) generate_dynamic_reconfigure_options( ${MROVER_PARAMETERS} ) +catkin_package() -# 7. Specify package build info export (catkin_package) -catkin_package( - CATKIN_DEPENDS - ${MROVER_CATKIN_PACKAGES} -) +### ======= ### +### Targets ### +### ======= ### +# Please browse the "Macros" section before adding anything here +# Lots of custom macros have been added to make adding new targets easier -if (COMMAND add_and_link_macro) - add_and_link_macro() -endif () +## Libraries +mrover_add_header_only_library(moteus deps/moteus/lib/cpp/mjbots) +mrover_add_library(lie src/util/lie/*.cpp src/util/lie) -# 9. Tests to build -if (COMMAND add_tests_macro) - add_tests_macro() -endif () +## ESW + +mrover_add_node(sim_arm_bridge src/simulator/arm_bridge/*.cpp) +## Perception -# 10. Install rules -install_launch_macro() -if (COMMAND additional_install_macro) - additional_install_macro() +mrover_add_nodelet(tag_detector src/perception/tag_detector/*.cpp src/perception/tag_detector src/perception/tag_detector/pch.hpp) +mrover_nodelet_link_libraries(tag_detector opencv_core opencv_objdetect opencv_aruco opencv_imgproc tbb lie) + +if (ZED_FOUND) + mrover_add_nodelet(zed src/perception/zed_wrapper/*.c* src/perception/zed_wrapper src/perception/zed_wrapper/pch.hpp) + mrover_nodelet_include_directories(zed ${ZED_INCLUDE_DIRS} ${CUDA_INCLUDE_DIRS}) + mrover_nodelet_link_libraries(zed ${ZED_LIBRARIES} ${SPECIAL_OS_LIBS} lie) + mrover_nodelet_defines(zed + ALLOW_BUILD_DEBUG # Ignore ZED warnings about Debug mode + __CUDA_INCLUDE_COMPILER_INTERNAL_HEADERS__ # Eigen includes some files it should not, ignore + ) endif () + +## Simulator + +mrover_add_gazebo_plugin(differential_drive_plugin_6w src/simulator/differential_drive_6w.cpp src) + +mrover_add_gazebo_plugin(kinect_plugin src/simulator/gazebo_ros_openni_kinect.cpp src/simulator) +target_link_libraries(kinect_plugin PRIVATE gazebo_ros_camera_utils DepthCameraPlugin Eigen3::Eigen) +set_target_properties(kinect_plugin PROPERTIES CXX_CLANG_TIDY "") + +### ======= ### +### Testing ### +### ======= ### + +# Add C++ unit tests +catkin_add_gtest(example-cpp-test test/example/cpp_test.cpp) + +# Python unit tests +catkin_add_nosetests(test/navigation/drive_test.py) +catkin_add_nosetests(test/teleop/teleop_test.py) +catkin_add_nosetests(test/util/SE3_test.py) +catkin_add_nosetests(test/util/SO3_test.py) + +# Integration tests (python and c++) +find_package(rostest REQUIRED) +add_rostest(test/example/basic_integration_test.test) +add_rostest(test/integration/integration.test) +add_rostest(test/util/SE3_tf_test.test) + +## Install + +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch +) From fc36ce9d64a2f75cc749a80175348061f33780eb Mon Sep 17 00:00:00 2001 From: Quintin Date: Sat, 14 Oct 2023 10:58:35 -0700 Subject: [PATCH 2/6] Gate removal from navigation (#592) * Deletion changes * Last gate deletes * Fix formatting --------- Co-authored-by: Prabhleen Pawar --- config/navigation.yaml | 6 - msg/WaypointType.msg | 1 - src/navigation/context.py | 65 -------- src/navigation/gate.py | 276 --------------------------------- src/navigation/navigation.py | 12 -- src/navigation/partial_gate.py | 115 -------------- src/navigation/recovery.py | 2 - src/navigation/search.py | 13 +- src/navigation/waypoint.py | 11 +- 9 files changed, 4 insertions(+), 497 deletions(-) delete mode 100644 src/navigation/gate.py delete mode 100644 src/navigation/partial_gate.py diff --git a/config/navigation.yaml b/config/navigation.yaml index 1fd36fefb..2994bca36 100644 --- a/config/navigation.yaml +++ b/config/navigation.yaml @@ -15,12 +15,6 @@ drive: driving_p: 10.0 lookahead_distance: 1.0 -gate: - stop_thresh: 0.2 - drive_fwd_thresh: 0.34 - approach_distance: 2.0 - post_radius: 0.7 - search: stop_thresh: 0.5 drive_fwd_thresh: 0.34 diff --git a/msg/WaypointType.msg b/msg/WaypointType.msg index a0a015f10..df2088fbd 100644 --- a/msg/WaypointType.msg +++ b/msg/WaypointType.msg @@ -1,4 +1,3 @@ int32 NO_SEARCH = 0 int32 POST = 1 -int32 GATE = 2 int32 val \ No newline at end of file diff --git a/src/navigation/context.py b/src/navigation/context.py index aa55d29a0..d4fe20407 100644 --- a/src/navigation/context.py +++ b/src/navigation/context.py @@ -26,26 +26,6 @@ tf_broadcaster: tf2_ros.StaticTransformBroadcaster = tf2_ros.StaticTransformBroadcaster() -POST_RADIUS = get_rosparam("gate/post_radius", 0.7) - - -@dataclass -class Gate: - post1: np.ndarray - post2: np.ndarray - - def get_post_shapes(self) -> tuple[Point, Point]: - """ - Creates a circular path of RADIUS around each post for checking intersection with our path - :return: tuple of the two shapely Point objects representing the posts - """ - - # Find circle of both posts - post1_shape = Point(self.post1[:2]).buffer(POST_RADIUS) - post2_shape = Point(self.post2[:2]).buffer(POST_RADIUS) - - return post1_shape, post2_shape - @dataclass class Rover: @@ -123,36 +103,6 @@ def current_fid_pos(self, odom_override: bool = True) -> Optional[np.ndarray]: return self.get_fid_pos(current_waypoint.fiducial_id, in_odom) - def other_gate_fid_pos(self) -> Optional[np.ndarray]: - """ - retrieves the position of the other gate post (which is 1 + current id) if we are looking for a gate - """ - assert self.ctx.course - current_waypoint = self.ctx.course.current_waypoint() - if self.ctx.course.look_for_gate() and current_waypoint is not None: - return self.get_fid_pos(current_waypoint.fiducial_id + 1, self.ctx.use_odom) - else: - return None - - def current_gate(self, odom_override: bool = True) -> Optional[Gate]: - """ - retrieves the position of the gate (if we know where it is, and we are looking for one) - :param: odom_override if false will force it to be in the map frame, true will mean use odom if we are using it (set by rosparam) - """ - - if self.ctx.course: - current_waypoint = self.ctx.course.current_waypoint() - if current_waypoint is None or not self.ctx.course.look_for_gate(): - return None - - post1 = self.get_fid_pos(current_waypoint.fiducial_id, self.ctx.use_odom and odom_override) - post2 = self.get_fid_pos(current_waypoint.fiducial_id + 1, self.ctx.use_odom and odom_override) - if post1 is None or post2 is None: - return None - return Gate(post1[:2], post2[:2]) - else: - return None - @dataclass class Course: @@ -188,17 +138,6 @@ def current_waypoint(self) -> Optional[mrover.msg.Waypoint]: return None return self.course_data.waypoints[self.waypoint_index] - def look_for_gate(self) -> bool: - """ - Returns whether the currently active waypoint (if it exists) indicates - that we should go to a gate - """ - waypoint = self.current_waypoint() - if waypoint is not None: - return waypoint.type.val == mrover.msg.WaypointType.GATE - else: - return False - def look_for_post(self) -> bool: """ Returns whether the currently active waypoint (if it exists) indicates @@ -261,8 +200,6 @@ class Context: tf_listener: tf2_ros.TransformListener vel_cmd_publisher: rospy.Publisher search_point_publisher: rospy.Publisher - gate_point_publisher: rospy.Publisher - gate_path_publisher: rospy.Publisher vis_publisher: rospy.Publisher course_listener: rospy.Subscriber stuck_listener: rospy.Subscriber @@ -285,8 +222,6 @@ def __init__(self): self.vel_cmd_publisher = rospy.Publisher("cmd_vel", Twist, queue_size=1) self.vis_publisher = rospy.Publisher("nav_vis", Marker, queue_size=1) self.search_point_publisher = rospy.Publisher("search_path", GPSPointList, queue_size=1) - self.gate_path_publisher = rospy.Publisher("gate_path", GPSPointList, queue_size=1) - self.gate_point_publisher = rospy.Publisher("estimated_gate_location", GPSPointList, queue_size=1) self.enable_auton_service = rospy.Service("enable_auton", mrover.srv.PublishEnableAuton, self.recv_enable_auton) self.stuck_listener = rospy.Subscriber("nav_stuck", Bool, self.stuck_callback) self.course = None diff --git a/src/navigation/gate.py b/src/navigation/gate.py deleted file mode 100644 index 280a386cd..000000000 --- a/src/navigation/gate.py +++ /dev/null @@ -1,276 +0,0 @@ -from __future__ import annotations - -from dataclasses import dataclass -from typing import Optional - -import numpy as np -from aenum import Enum, NoAlias -from mrover.msg import GPSPointList -from shapely.geometry import LineString, Polygon, Point -from util.np_utils import normalized, perpendicular_2d -from util.ros_utils import get_rosparam - -from context import Context, Rover, convert_cartesian_to_gps -from context import Gate -from state import BaseState - -STOP_THRESH = get_rosparam("gate/stop_thresh", 0.2) -DRIVE_FWD_THRESH = get_rosparam("gate/drive_fwd_thresh", 0.34) # 20 degrees - -APPROACH_DISTANCE = get_rosparam("gate/approach_distance", 2.0) - - -@dataclass -class GatePath: - """ - Represents a path through a gate. The reason this is not subclassing Trajectory is because it needs to dynamically - update the path every time we have a new gate estimate. Trajectory subclasses are meant to be immutable and regenerating - them leads to problems with reseting the cur_pt variable. This class maintains minimal state (direction through the gate) - through keeping track of what approach and prep points it should use and also keeps track of whether we've passed through - gate or not. - The Client can update the values of the actual points in the trajectory upon calling the update() method - Use this class by first constructing the object GatePath(rover_pos, gate) and then calling update every time you have a new - gate estimate. Call get_cur_pt() to get the current path that you should be following (which will be None if we are done) - """ - - rover_pos: np.ndarray - gate: Gate - prep_idx: int - approach_idx: int - victory_idx: int - prep_pts: np.ndarray - approach_pts: np.ndarray - center: np.ndarray - passed_center: bool = False - center_idx: int = 2 - path_index: int = 0 - - def __init__(self, rover_pos: np.ndarray, gate: Gate): - self.rover_pos = rover_pos[:2] - self.gate = gate - self.__update_pts() - self.prep_idx = int(np.argmin(np.linalg.norm(self.prep_pts - self.rover_pos, axis=1))) - self.approach_idx = int(np.argmin(np.linalg.norm(self.approach_pts - self.rover_pos, axis=1))) - self.victory_idx = int(np.argmax(np.linalg.norm(self.approach_pts - self.rover_pos, axis=1))) - self.update(rover_pos, gate) - self.path_index = self.__optimize_path() - - def __update_center(self) -> None: - self.center = (self.gate.post1 + self.gate.post2) / 2 - - def __update_approach_pts(self) -> None: - """ - Updates the approach points based on the current gate estimate - """ - post1 = self.gate.post1 - post2 = self.gate.post2 - - # the direction of the post is just the normalized vector from post1 to post2 - post_direction = normalized(post2 - post1) - perpendicular = perpendicular_2d(post_direction) - - # approach points are the points that are directly out from the center (a straight line) of - # the gate "approach_distance" away - self.approach_pts = np.array( - [ - APPROACH_DISTANCE * perpendicular + self.center, - -APPROACH_DISTANCE * perpendicular + self.center, - ] - ) - - def __update_prep_pts(self) -> None: - """ - Updates the prep points based on the current gate estimate - """ - post1 = self.gate.post1 - post2 = self.gate.post2 - - # the direction of the post is just the normalized vector from post1 to post2 - post_direction = normalized(post2 - post1) - perpendicular = perpendicular_2d(post_direction) - - # prep points are the points that are perpendicular to the center of the gate - # "approach_distance" away - # prep points are points that are directly out in either direction from the posts - # the idea here is that if we go to the closest prep point then an approach point, - # we will never collide with the post - self.prep_pts = np.array( - [ - (2 * APPROACH_DISTANCE * perpendicular) + post1, - (2 * APPROACH_DISTANCE * perpendicular) + post2, - (-2 * APPROACH_DISTANCE * perpendicular) + post1, - (-2 * APPROACH_DISTANCE * perpendicular) + post2, - ] - ) - - def __update_pts(self) -> None: - """ - Updates the prep points, approach points, and center of the gate - """ - self.__update_center() - self.__update_approach_pts() - self.__update_prep_pts() - - def __get_full_path(self) -> np.ndarray: - """ - :returns: np.array which represents the coordinates of the path. - """ - return np.array( - [ - self.prep_pts[self.prep_idx], - self.approach_pts[self.approach_idx], - self.center, - self.approach_pts[self.victory_idx], - ] - ) - - def __optimize_path(self) -> int: - """ - Here is a reference for the points mentioned below: - https://github.com/umrover/mrover-ros/wiki/Navigation#searchtrajectory - :returns: an integer representing the farthest along point on the path that we can - drive to without intersecting the gate (while still driving through it) - """ - if not self.__should_optimize(): - return 0 - # Get the shapes of both the posts - post_one_shape, post_two_shape = self.gate.get_post_shapes() - - rover = self.rover_pos[:2] - - # try paths with successively more points until we have one that won't intersect - all_pts = self.__get_full_path() - num_pts_included = ( - 2 # require that we include the victory point and the center point in the path to ensure traversal - ) - path = self.__make_shapely_path(rover, all_pts[-num_pts_included:, :]) - while path.intersects(post_one_shape) or path.intersects(post_two_shape): - num_pts_included += 1 - if num_pts_included == all_pts.shape[0]: - break - path = self.__make_shapely_path(rover, all_pts[-num_pts_included:]) - - return all_pts.shape[0] - num_pts_included - - def __get_paint(self) -> Polygon: - """ - Generates the 'paint' as a shapely polygon. The 'paint' is the region that the rover is allowed to optimize its path within - """ - return Polygon(self.prep_pts) - - def __should_optimize(self) -> bool: - """ - :returns: True if the rover should optimize its path, False otherwise. - the rover should optimize its path if its position is within the paint - """ - paint = self.__get_paint() - return paint.contains(Point(self.rover_pos)) - - def __make_shapely_path(self, rover: Rover, path_pts: np.ndarray) -> LineString: - """ - :param rover: position vector of the rover - :param pathPts: This is a np.array that has the coordinates of the path - :returns: Returns a Shapeley (geometry limake_shabrary) object of LineString which put the path points given into - one cohesive line segments. - """ - path_list = np.vstack((rover, path_pts)) - return LineString(path_list) - - def update(self, rover_pos: np.ndarray, gate: Gate) -> None: - """ - Updates the calculated prep, approach, and center points based on the current gate estimate - """ - self.rover_pos = rover_pos[:2] - self.gate = gate - self.__update_pts() - - def get_cur_pt(self) -> Optional[np.ndarray]: - """ - :returns: The point on the path that the rover should drive to next, or None if the rover has completed the path - """ - # first get the full path with all the key points. - full_path = self.__get_full_path() - - pt = full_path[self.path_index] - # if we are close enough to the point, move on to the next one - if np.linalg.norm(pt - self.rover_pos) < STOP_THRESH: - self.path_index += 1 - # if we have reached the end of the path, return None - if self.path_index >= len(full_path): - return None - pt = full_path[self.path_index] - # append a 0.0 to the end so the point is in R^3 - return np.append(pt, 0.0) - - -class GateTraverseStateTransitions(Enum): - _settings_ = NoAlias - - no_gate = "SearchState" - finished_gate = "DoneState" - continue_gate_traverse = "GateTraverseState" - recovery_state = "RecoveryState" - - -class GateTraverseState(BaseState): - STOP_THRESH = get_rosparam("gate/stop_thresh", 0.2) - DRIVE_FWD_THRESH = get_rosparam("gate/drive_fwd_thresh", 0.34) # 20 degrees - APPROACH_DISTANCE = get_rosparam("gate/approach_distance", 2.0) - - traj: Optional[GatePath] = None - - def __init__( - self, - context: Context, - ): - own_transitions = [GateTraverseStateTransitions.continue_gate_traverse.name] # type: ignore - super().__init__( - context, - own_transitions=own_transitions, # type: ignore - add_outcomes=[transition.name for transition in GateTraverseStateTransitions], # type: ignore - ) - - def reset(self) -> None: - self.traj = None - - def evaluate(self, ud): - # Check if a path has been generated and its associated with the same - # waypoint as the previous one. Generate one if not - gate = self.context.env.current_gate() - if gate is None: - return GateTraverseStateTransitions.no_gate.name # type: ignore - - rover_position = self.context.rover.get_pose(in_odom_frame=True).position - if self.traj is None: - self.traj = GatePath(rover_position, gate) - else: - self.traj.update(rover_position, gate) - - if self.traj is None: - return GateTraverseState.finished_gate.name # type: ignore - - # continue executing this path from wherever it left off - target_pos = self.traj.get_cur_pt() - if target_pos is None: - self.context.course.increment_waypoint() - return GateTraverseStateTransitions.finished_gate.name # type: ignore - - cmd_vel, _ = self.context.rover.driver.get_drive_command( - target_pos, - self.context.rover.get_pose(in_odom_frame=True), - self.STOP_THRESH, - self.DRIVE_FWD_THRESH, - in_odom=self.context.use_odom, - ) - - if self.context.rover.stuck: - self.context.rover.previous_state = GateTraverseStateTransitions.continue_gate_traverse.name # type: ignore - return GateTraverseStateTransitions.recovery_state.name # type: ignore - - map_gate = self.context.env.current_gate(odom_override=False) - if map_gate is not None: - self.context.gate_point_publisher.publish( - GPSPointList([convert_cartesian_to_gps(p) for p in [map_gate.post1, map_gate.post2]]) - ) - self.context.rover.send_drive_command(cmd_vel) - return GateTraverseStateTransitions.continue_gate_traverse.name # type: ignore diff --git a/src/navigation/navigation.py b/src/navigation/navigation.py index 2416c605a..cb82b177b 100755 --- a/src/navigation/navigation.py +++ b/src/navigation/navigation.py @@ -13,8 +13,6 @@ from approach_post import ApproachPostState, ApproachPostStateTransitions from context import Context -from gate import GateTraverseState, GateTraverseStateTransitions -from partial_gate import PartialGateState, PartialGateStateTransitions from post_backup import PostBackupState, PostBackupTransitions from recovery import RecoveryState, RecoveryStateTransitions from search import SearchState, SearchStateTransitions @@ -55,19 +53,9 @@ def __init__(self, context: Context): self.state_machine.add( "SearchState", SearchState(self.context), transitions=self.get_transitions(SearchStateTransitions) ) - self.state_machine.add( - "GateTraverseState", - GateTraverseState(self.context), - transitions=self.get_transitions(GateTraverseStateTransitions), - ) self.state_machine.add( "RecoveryState", RecoveryState(self.context), transitions=self.get_transitions(RecoveryStateTransitions) ) - self.state_machine.add( - "PartialGateState", - PartialGateState(self.context), - transitions=self.get_transitions(PartialGateStateTransitions), - ) self.state_machine.add( "PostBackupState", PostBackupState(self.context), diff --git a/src/navigation/partial_gate.py b/src/navigation/partial_gate.py deleted file mode 100644 index 722484a10..000000000 --- a/src/navigation/partial_gate.py +++ /dev/null @@ -1,115 +0,0 @@ -from dataclasses import dataclass -from typing import Optional - -import numpy as np -from aenum import Enum, NoAlias -from util import np_utils - -from context import Context -from state import BaseState -from trajectory import Trajectory - -STOP_THRESH = 0.2 -DRIVE_FWD_THRESH = 0.95 -POST_SEPARATION = 2 # meters - - -@dataclass -class PartialGateTrajectory(Trajectory): - def partial_gate_traj(post_pos, rover_pos): - """ - Generates a trajectory to find second of two posts - :param post_pos: position of the post (np.ndarray) - :param rover_pos: position of the rover (np.ndarray). Assumes that the rover is facing the post - :return: list of positions for the rover to traverse List(np.ndarray) - """ - # If the post is at (a, b) and rover is at (a + x, b + y), - # Where c is the distance between posts (2m) - # we'd calculate rover-to-post as c * (-x, -y).Then, - # Point 1 (Left Perp): (a + cy, b - cx) - # Point 2 (Opposite): (a - cx, b - cy) - # Point 3 (Right Perp): (a - cy, b + cx) - # Point 4 (Finish): (a + cx, b + cy) - # See documentation on the Wiki for visual. - - # Converting to 2d arrays - post_pos = post_pos[:2] - rover_pos = rover_pos[:2] - - rover_to_post = post_pos - rover_pos - rover_to_post = POST_SEPARATION * np_utils.normalized(rover_to_post) - # scale vector to have magnitude == POST_SEPARATION - - left_perp = np_utils.perpendicular_2d(rover_to_post) # (-y,x) - right_perp = -left_perp # (y,-x) - - # This is just making our trajectory points into an array that we can read in - coords = np.vstack( - (post_pos + left_perp, post_pos + rover_to_post, post_pos + right_perp, post_pos - rover_to_post) - ) - - # adding z coordinates to coords, all 0 - coords = np.hstack((coords, np.zeros((4, 1)))) # 4 because there are 4 points in the path - - return PartialGateTrajectory(coords) - - -class PartialGateStateTransitions(Enum): - _settings_ = NoAlias - # State Transitions - no_fiducial = "SearchState" - partial_gate = "PartialGateState" - found_gate = "GateTraverseState" - done = "DoneState" - recovery_state = "RecoveryState" - - -class PartialGateState(BaseState): - traj: Optional[PartialGateTrajectory] = None - - def __init__( - self, - context: Context, - ): - own_transitions = [PartialGateStateTransitions.partial_gate.name] # type: ignore - super().__init__(context, own_transitions, add_outcomes=[transition.name for transition in PartialGateStateTransitions]) # type: ignore - - def reset(self) -> None: - self.traj = None - - def evaluate(self, ud): - post_pos = self.context.env.current_fid_pos() - if post_pos is None: - post_pos = self.context.env.other_gate_fid_pos() - gate = self.context.env.current_gate() - - if gate is not None: # If we have a gate, we are done - return PartialGateStateTransitions.found_gate.name # type: ignore - elif post_pos is not None: # Searching for second post - if self.traj is None: - self.traj = PartialGateTrajectory.partial_gate_traj( - post_pos, self.context.rover.get_pose(in_odom_frame=True).position - ) - else: - return PartialGateStateTransitions.no_fiducial.name # type: ignore - - target_pos = self.traj.get_cur_pt() - cmd_vel, arrived = self.context.rover.driver.get_drive_command( - target_pos, - self.context.rover.get_pose(in_odom_frame=True), - STOP_THRESH, - DRIVE_FWD_THRESH, - in_odom=self.context.use_odom, - ) - if arrived: - # if we finish the gate path, we're done (or continue search) CHECK THIS*** - if self.traj.increment_point(): - self.context.course.increment_waypoint() - return PartialGateStateTransitions.done.name # type: ignore - - if self.context.rover.stuck: - self.context.rover.previous_state = PartialGateStateTransitions.partial_gate.name # type: ignore - return PartialGateStateTransitions.recovery_state.name # type: ignore - - self.context.rover.send_drive_command(cmd_vel) - return PartialGateStateTransitions.partial_gate.name # type: ignore diff --git a/src/navigation/recovery.py b/src/navigation/recovery.py index 7f0e38d68..d8ab217e8 100644 --- a/src/navigation/recovery.py +++ b/src/navigation/recovery.py @@ -19,12 +19,10 @@ class RecoveryStateTransitions(Enum): _settings_ = NoAlias continue_waypoint_traverse = "WaypointState" - continue_gate_traverse = "GateTraverseState" continue_search = "SearchState" continue_recovery = "RecoveryState" continue_post_backup = "PostBackupState" recovery_state = "RecoveryState" - partial_gate = "PartialGateState" class JTurnAction(Enum): diff --git a/src/navigation/search.py b/src/navigation/search.py index 964380688..86531e66f 100644 --- a/src/navigation/search.py +++ b/src/navigation/search.py @@ -85,8 +85,6 @@ class SearchStateTransitions(Enum): no_fiducial = "WaypointState" continue_search = "SearchState" found_fiducial_post = "ApproachPostState" - found_fiducial_gate = "PartialGateState" - found_gate = "GateTraverseState" recovery_state = "RecoveryState" @@ -156,13 +154,8 @@ def evaluate(self, ud): ) self.context.rover.send_drive_command(cmd_vel) - # if we see the fiduicial or gate, go to either fiducial or gate state - if self.context.env.current_gate() is not None: - return SearchStateTransitions.found_gate.name # type: ignore - elif self.context.env.current_fid_pos() is not None and self.context.course.look_for_post(): + # if we see the fiduicial go to either fiducial + if self.context.env.current_fid_pos() is not None and self.context.course.look_for_post(): return SearchStateTransitions.found_fiducial_post.name # type: ignore - elif ( - self.context.env.current_fid_pos() is not None or self.context.env.other_gate_fid_pos() is not None - ) and self.context.course.look_for_gate(): - return SearchStateTransitions.found_fiducial_gate.name # type: ignore + return SearchStateTransitions.continue_search.name # type: ignore diff --git a/src/navigation/waypoint.py b/src/navigation/waypoint.py index 9300660a7..47e89ce77 100644 --- a/src/navigation/waypoint.py +++ b/src/navigation/waypoint.py @@ -16,9 +16,7 @@ class WaypointStateTransitions(Enum): search_at_waypoint = "SearchState" no_waypoint = "DoneState" find_approach_post = "ApproachPostState" - go_to_gate = "GateTraverseState" recovery_state = "RecoveryState" - partial_gate = "PartialGateState" backup_from_post = "PostBackupState" @@ -65,13 +63,6 @@ def evaluate(self, ud) -> str: if self.context.env.arrived_at_post: self.context.env.arrived_at_post = False return WaypointStateTransitions.backup_from_post.name # type: ignore - - # Go into either gate or search if we see them early (and are looking) - if self.context.course.look_for_gate(): - if self.context.env.current_gate() is not None: - return WaypointStateTransitions.go_to_gate.name # type: ignore - if self.context.env.current_fid_pos() is not None or self.context.env.other_gate_fid_pos() is not None: - return WaypointStateTransitions.partial_gate.name # type: ignore if self.context.course.look_for_post(): if self.context.env.current_fid_pos() is not None: return WaypointStateTransitions.find_approach_post.name # type: ignore @@ -86,7 +77,7 @@ def evaluate(self, ud) -> str: self.DRIVE_FWD_THRESH, ) if arrived: - if not self.context.course.look_for_gate() and not self.context.course.look_for_post(): + if not self.context.course.look_for_post(): # We finished a regular waypoint, go onto the next one self.context.course.increment_waypoint() else: From d15510a53cb3a57ef5c058ee59d08a925f88be67 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sun, 15 Oct 2023 19:12:18 -0400 Subject: [PATCH 3/6] Fix gitignore being too exclusive, add cuda host compiler to ci config, fix node and nodelet mixup in cmake, fix tag detector node compile --- .gitignore | 15 --------------- CMakeLists.txt | 4 ++-- ansible/roles/build/files/profiles/ci/config.yaml | 1 + src/perception/tag_detector/tag_detector.cpp | 2 ++ 4 files changed, 5 insertions(+), 17 deletions(-) diff --git a/.gitignore b/.gitignore index 0f4fb421a..6ef1aee6b 100644 --- a/.gitignore +++ b/.gitignore @@ -29,18 +29,3 @@ moteus-cal* # CSV *.csv - -# STM32 workspace -.metadata - -Debug/ -Release/ -.metadata/ -.settings/ -RemoteSystemsTempFiles/ - -*.chm -*.settings.xml - -*.xml -*.launch diff --git a/CMakeLists.txt b/CMakeLists.txt index 587e1f174..cfeaa755d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -93,9 +93,9 @@ endmacro() macro(mrover_add_nodelet name sources includes) # A nodelet runs inside another process so it is a library - mrover_add_library(${name}_node ${sources} ${includes}) + mrover_add_library(${name}_nodelet ${sources} ${includes}) # Also add a node for quick debugging - mrover_add_node(${name}_nodelet ${sources}) + mrover_add_node(${name}_node ${sources}) target_compile_definitions(${name}_nodelet PRIVATE MROVER_IS_NODELET) # Optional PCH if (ARGV3) diff --git a/ansible/roles/build/files/profiles/ci/config.yaml b/ansible/roles/build/files/profiles/ci/config.yaml index 98808b8db..b6232849c 100644 --- a/ansible/roles/build/files/profiles/ci/config.yaml +++ b/ansible/roles/build/files/profiles/ci/config.yaml @@ -8,6 +8,7 @@ cmake_args: - -DCMAKE_CXX_FLAGS=-pipe - -DCMAKE_CXX_COMPILER=clang++-16 - -DCMAKE_CXX_CLANG_TIDY=clang-tidy-16 + - -DCMAKE_CUDA_HOST_COMPILER=clang++-16 - -Wno-dev devel_layout: linked devel_space: devel diff --git a/src/perception/tag_detector/tag_detector.cpp b/src/perception/tag_detector/tag_detector.cpp index 73698714d..f1524a5b6 100644 --- a/src/perception/tag_detector/tag_detector.cpp +++ b/src/perception/tag_detector/tag_detector.cpp @@ -155,5 +155,7 @@ int main(int argc, char** argv) { return EXIT_SUCCESS; } +#ifdef MROVER_IS_NODELET #include PLUGINLIB_EXPORT_CLASS(mrover::TagDetectorNodelet, nodelet::Nodelet) +#endif From 665b5976e983d8f9d688ad727d4eec4eb450867b Mon Sep 17 00:00:00 2001 From: qhdwight Date: Mon, 23 Oct 2023 12:10:55 -0400 Subject: [PATCH 4/6] Explicitly tell CMake that nodes depend on their nodelet --- CMakeLists.txt | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index cfeaa755d..98c9465f3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -96,8 +96,12 @@ macro(mrover_add_nodelet name sources includes) mrover_add_library(${name}_nodelet ${sources} ${includes}) # Also add a node for quick debugging mrover_add_node(${name}_node ${sources}) + # Explicitly tell CMake to re-build the nodelet when the node is built + # CMake cannot tell these are dependent since a node dynamically (at runtime) loads the nodelet as a shared library + add_dependencies(${name}_node ${name}_nodelet) + # Allows the source code to split based on whether it is a node or a nodelet target_compile_definitions(${name}_nodelet PRIVATE MROVER_IS_NODELET) - # Optional PCH + # Optional pre-compiled header (PCH) support if (ARGV3) target_precompile_headers(${name}_node PRIVATE ${ARGV3}) target_precompile_headers(${name}_nodelet PRIVATE ${ARGV3}) From 06ecd147ecaacca941a7bb54b668fd02d2917fb4 Mon Sep 17 00:00:00 2001 From: Quintin Date: Fri, 27 Oct 2023 10:24:25 -0400 Subject: [PATCH 5/6] atomic is not needed here since always modified in critical section, wrong assert that works in release but not debug (#594) --- src/perception/zed_wrapper/zed_wrapper.bridge.cu | 2 +- src/perception/zed_wrapper/zed_wrapper.cpp | 2 +- src/perception/zed_wrapper/zed_wrapper.hpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/perception/zed_wrapper/zed_wrapper.bridge.cu b/src/perception/zed_wrapper/zed_wrapper.bridge.cu index 62335d6e9..5ebab9f35 100644 --- a/src/perception/zed_wrapper/zed_wrapper.bridge.cu +++ b/src/perception/zed_wrapper/zed_wrapper.bridge.cu @@ -37,7 +37,7 @@ namespace mrover { assert(bgraGpu.getWidth() >= xyzGpu.getWidth()); assert(bgraGpu.getHeight() >= xyzGpu.getHeight()); assert(bgraGpu.getChannels() == 4); - assert(xyzGpu.getChannels() == 3); + assert(xyzGpu.getChannels() == 4); // Last channel is unused assert(msg); auto* bgraGpuPtr = bgraGpu.getPtr(sl::MEM::GPU); diff --git a/src/perception/zed_wrapper/zed_wrapper.cpp b/src/perception/zed_wrapper/zed_wrapper.cpp index c19353c8f..f69ef66ba 100644 --- a/src/perception/zed_wrapper/zed_wrapper.cpp +++ b/src/perception/zed_wrapper/zed_wrapper.cpp @@ -137,7 +137,7 @@ namespace mrover { { std::unique_lock lock{mSwapMutex}; // Waiting on the condition variable will drop the lock and reacquire it when the condition is met - mSwapCv.wait(lock, [this] { return mIsSwapReady.load(); }); + mSwapCv.wait(lock, [this] { return mIsSwapReady; }); mIsSwapReady = false; mPcThreadProfiler.measureEvent("Wait"); diff --git a/src/perception/zed_wrapper/zed_wrapper.hpp b/src/perception/zed_wrapper/zed_wrapper.hpp index d252851bb..cde5d0aaa 100644 --- a/src/perception/zed_wrapper/zed_wrapper.hpp +++ b/src/perception/zed_wrapper/zed_wrapper.hpp @@ -53,7 +53,7 @@ namespace mrover { std::thread mPointCloudThread, mGrabThread; std::mutex mSwapMutex; std::condition_variable mSwapCv; - std::atomic_bool mIsSwapReady = false; + bool mIsSwapReady = false; LoopProfiler mPcThreadProfiler{"Zed Wrapper Point Cloud"}, mGrabThreadProfiler{"Zed Wrapper Grab"}; From c19fae353c9a2a5bcb8db197914fa6f1d381d298 Mon Sep 17 00:00:00 2001 From: Ali Ryckman <51866496+alisonryckman@users.noreply.github.com> Date: Sun, 29 Oct 2023 13:25:35 -0400 Subject: [PATCH 6/6] add obj detect msg (#598) * add obj detect msg * add enum for objects * bruh --- msg/DetectedObject.msg | 4 ++++ msg/DetectedObjectType.msg | 3 +++ 2 files changed, 7 insertions(+) create mode 100644 msg/DetectedObject.msg create mode 100644 msg/DetectedObjectType.msg diff --git a/msg/DetectedObject.msg b/msg/DetectedObject.msg new file mode 100644 index 000000000..fcc2d45ca --- /dev/null +++ b/msg/DetectedObject.msg @@ -0,0 +1,4 @@ +DetectedObjectType type +float32 detection_confidence +float32 bearing +float32 distance \ No newline at end of file diff --git a/msg/DetectedObjectType.msg b/msg/DetectedObjectType.msg new file mode 100644 index 000000000..28c2319ff --- /dev/null +++ b/msg/DetectedObjectType.msg @@ -0,0 +1,3 @@ +uint8 NO_OBJ=0 +uint8 MALLET=1 +uint8 WATER_BOTTLE=2 \ No newline at end of file