Avb/click ik

.gitignore

@@ -45,3 +45,7 @@ state_machine/state_log.txt
 
 data/*.png
 data/raw-pano-images
+
+# Dynamixel Ignores
+.catkin_tools/
+.dynamixel_sdk/

CMakeLists.txt

@@ -51,6 +51,7 @@ endif ()
 
 find_package(rclpy REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_python REQUIRED)
@@ -66,6 +67,7 @@ find_package(urdf REQUIRED)
 find_package(std_srvs REQUIRED)
 find_package(magic_enum REQUIRED)
 find_package(yaml_cpp_vendor REQUIRED)
+find_package(dynamixel_sdk REQUIRED)
 
 find_package(Assimp NAMES Assimp assimp REQUIRED)
 find_package(Bullet REQUIRED)
@@ -118,7 +120,7 @@ endif()
 find_package(PkgConfig REQUIRED)
 pkg_search_module(NetLink libnl-3.0 IMPORTED_TARGET QUIET)
 pkg_search_module(NetLinkRoute libnl-route-3.0 IMPORTED_TARGET QUIET)
-pkg_search_module(Gst gstreamer-1.0 IMPORTED_TARGET QUIET)
+pkg_search_module(Gst gstreamer-1.0 IMPORTED_TARGET QUIET) # if this is failing try: `sudo apt install libunwind-dev`
 pkg_search_module(GstApp gstreamer-app-1.0 IMPORTED_TARGET QUIET)
 pkg_search_module(LibUdev libudev IMPORTED_TARGET QUIET)
 
@@ -237,7 +239,7 @@ target_link_libraries(rover_gps_driver parameter_utils)
 
 mrover_add_node(heading_filter localization/heading_filter.cpp)
 ament_target_dependencies(heading_filter rclcpp rcutils sensor_msgs builtin_interfaces message_filters)
-target_link_libraries(heading_filter lie)
+target_link_libraries(heading_filter lie parameter_utils)
 
 mrover_add_node(pose_filter localization/pose_filter/*.cpp)
 ament_target_dependencies(pose_filter rclcpp tf2 tf2_ros geometry_msgs sensor_msgs)
@@ -323,18 +325,23 @@ if (EXISTS ${MROVER_EMBEDDED_ROOT_DIR} AND NOT APPLE)
     mrover_add_header_only_library(motor esw/motor)
     target_link_libraries(motor INTERFACE can_device units parameter_utils)
 
+    mrover_add_header_only_library(servo esw/servo)
+    target_link_libraries(servo INTERFACE units parameter_utils)
+
     mrover_add_header_only_library(science esw/science)
     target_link_libraries(science INTERFACE can_device units)
 
     macro(mrover_add_esw_bridge_node name sources)
         mrover_add_node(${name} ${sources})
-        ament_target_dependencies(${name} rclcpp std_srvs)
-        target_link_libraries(${name} can_device units motor science)
+        ament_target_dependencies(${name} rclcpp std_srvs dynamixel_sdk)
+        target_link_libraries(${name} can_device units motor science servo)
     endmacro()
 
+    # esw hardware bridge nodes
     mrover_add_esw_bridge_node(drive_hw_bridge esw/drive_hw_bridge.cpp)
     mrover_add_esw_bridge_node(arm_hw_bridge esw/arm_hw_bridge.cpp)
     mrover_add_esw_bridge_node(science_hw_bridge esw/science_hw_bridge.cpp)
+    mrover_add_esw_bridge_node(mast_gimbal_hw_bridge esw/mast_gimbal_hw_bridge.cpp)
 endif()
 
 mrover_add_node(differential_drive_controller esw/differential_drive_controller.cpp)
@@ -345,6 +352,13 @@ mrover_add_node(arm_controller navigation/arm_controller/arm_controller.cpp navi
 target_link_libraries(arm_controller lie)
 ament_target_dependencies(arm_controller rclcpp)
 
+# Click IK
+mrover_add_component(click_ik navigation/click_ik/*.cpp navigation/click_ik/pch.hpp ClickIK)
+mrover_ament_component(click_ik rclcpp rclcpp_components tf2 tf2_ros)
+mrover_link_component(click_ik lie parameter_utils)
+
+mrover_executable_from_component(click_ik navigation/click_ik/main.cpp click_ik_component)
+
 if (Gst_FOUND AND GstApp_FOUND)
     mrover_add_node(usb_camera esw/usb_camera/*.cpp esw/usb_camera/pch.hpp)
     target_link_libraries(usb_camera PkgConfig::Gst PkgConfig::GstApp opencv_core opencv_imgcodecs parameter_utils)
@@ -366,12 +380,12 @@ if (NetLink_FOUND AND NetLinkRoute_FOUND)
 endif ()
 
 # Teleop Qt5 Camera Client
-find_package(Qt5 5.15 REQUIRED COMPONENTS Widgets Multimedia MultimediaWidgets)
+find_package(Qt5 5.15 REQUIRED COMPONENTS Widgets)
 
 mrover_add_node(camera_client teleoperation/camera_client/*.*pp teleoperation/camera_client/include/pch.hpp)
 target_include_directories(camera_client PRIVATE teleoperation/camera_client/include)
-target_link_libraries(camera_client gst_utils Qt::Widgets Qt::Multimedia Qt::MultimediaWidgets opencv_core opencv_imgcodecs)
-ament_target_dependencies(camera_client rclcpp std_srvs sensor_msgs)
+target_link_libraries(camera_client gst_utils Qt::Widgets PkgConfig::Gst X11 opencv_core opencv_imgcodecs)
+ament_target_dependencies(camera_client rclcpp rclcpp_action std_srvs sensor_msgs)
 set_target_properties(camera_client PROPERTIES
     AUTOMOC ON
     AUTORCC ON

action/ClickIk.action

@@ -1,6 +1,6 @@
-uint32 point_in_image_x
-uint32 point_in_image_y
+float32 point_in_image_x
+float32 point_in_image_y
 ---
 bool success
 ---
-float32 distance
+float32 distance

action/IkImageSample.action

@@ -0,0 +1,8 @@
+uint32 w
+uint32 h
+float32 scale
+---
+bool[] success
+---
+uint32 completed
+uint32 total

ansible/roles/build/tasks/main.yml

@@ -127,6 +127,7 @@
       - ros-humble-xacro
       - ros-humble-magic-enum
       - ros-humble-rtcm-msgs
+      - ros-humble-dynamixel-sdk
       # TODO (ali): why does this need to be installed manually
       - libboost-dev
       - qtbase5-dev

ansible/roles/ci/tasks/main.yml

@@ -100,6 +100,7 @@
       - ros-humble-xacro
       - ros-humble-rtcm-msgs
       - ros-humble-magic-enum
+      - ros-humble-dynamixel-sdk
       # TODO (ali): why does this need to be installed manually
       - libboost-dev
       - qtmultimedia5-dev

ansible/roles/esw/files/rules/99-u2d2.rules

@@ -0,0 +1 @@
+SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6014", SYMLINK+="u2d2", GROUP="mrover"

config/cameras.yaml

@@ -3,13 +3,18 @@
     static_cam:
       address: 10.1.0.2 # if using RTP, should be set to IP of basestation
       port: 8081
+      # camera source can be one of three options: dev_node, dev_path, or image_topic
+      # if no camera source is specified, defaults to videotestsrc
       dev_node: "/dev/static_cam"
-      dev_path: ""
+      # crop_left: PIXELS
+      # crop_right: PIXELS
+      # crop_top: PIXELS
+      # crop_bottom: PIXELS
       stream:
-        pixel_format: MJPG # from v4l2-ctl -d DEV_NODE --list-formats-ext
+        pixel_format: YUYV # from v4l2-ctl -d DEV_NODE --list-formats-ext
         width: 640
         height: 480
-        framerate: 15
+        framerate: 30
         codec: H265
         bitrate: 1000000
       image_capture:
@@ -18,15 +23,14 @@
         height: 720
         framerate: 5
     boom_cam:
-      address: 10.1.0.2 # if using RTP, should be set to IP of basestation
+      address: 10.1.0.2
       port: 8082
       dev_node: "/dev/boomcam"
-      dev_path: ""
       stream:
-        pixel_format: MJPG # from v4l2-ctl -d DEV_NODE --list-formats-ext
+        pixel_format: YUYV
         width: 640
         height: 480
-        framerate: 15
+        framerate: 30
         codec: H265
         bitrate: 1000000
       image_capture:
@@ -37,13 +41,12 @@
     mob_cam_left:
       address: 10.1.0.2
       port: 8083
-      dev_node: "/dev/mobcam_left"
-      dev_path: ""
+      dev_node: "/dev/mob_cam_left"
       stream:
-        pixel_format: MJPG # from v4l2-ctl -d DEV_NODE --list-formats-ext
+        pixel_format: YUYV
         width: 640
         height: 480
-        framerate: 15
+        framerate: 30
         codec: H265
         bitrate: 1000000
       image_capture:
@@ -54,10 +57,9 @@
     mob_cam_right:
       address: 10.1.0.2
       port: 8084
-      dev_node: "/dev/mobcam_right"
-      dev_path: ""
+      dev_node: "/dev/mob_cam_right"
       stream:
-        pixel_format: MJPG # from v4l2-ctl -d DEV_NODE --list-formats-ext
+        pixel_format: MJPG
         width: 640
         height: 480
         framerate: 15
@@ -72,8 +74,6 @@
       address: 10.1.0.2
       port: 8085
       image_topic: "/long_range_cam/image"
-      dev_node: ""
-      dev_path: ""
       stream:
         pixel_format: ABGR32
         width: 640
@@ -90,10 +90,9 @@
       address: 10.1.0.2
       port: 8086
       dev_node: "/dev/zed_mini"
-      dev_path: ""
       crop_right: 672
       stream:
-        pixel_format: YUYV # from v4l2-ctl -d DEV_NODE --list-formats-ext
+        pixel_format: YUYV
         width: 1344
         height: 376
         framerate: 15
@@ -108,8 +107,6 @@
       address: 10.1.0.2
       port: 8087
       image_topic: "/zed/left/image"
-      dev_node: ""
-      dev_path: ""
       stream:
         pixel_format: ABGR32 # https://www.kernel.org/doc/html/v4.9/media/uapi/v4l/pixfmt-packed-rgb.html
         width: 1280

config/localization.yaml

@@ -26,8 +26,12 @@ heading_filter:
   ros__parameters:
     imu_watchdog_timeout: 1.0
     mag_heading_noise: 10.0
-    rtk_heading_noise: 0.01
+    rtk_heading_noise: 0.00001
+    drive_forward_heading_noise: 0.0001
     process_noise: 0.000001
+    minimum_linear_speed: 0.4
+    rover_heading_change_threshold: 0.05
+    use_mag: false
 
 pose_filter:
   ros__parameters:

config/mast_gimbal.yaml

@@ -1,30 +1,29 @@
 mast_gimbal_hw_bridge:
   ros__parameters:
-    mast_gimbal_pitch:
-      gear_ratio: 1.0
-      is_inverted: false
-      driver_voltage: 10.5
-      motor_max_voltage: 12.0
-      quad_present: false
-      abs_present: false
-    mast_gimbal_yaw:
-      gear_ratio: 1.0
-      is_inverted: true
-      driver_voltage: 10.5
-      motor_max_voltage: 12.0
-      quad_present: true
-      quad_ratio: 0.001013780932  # approx. 1000:1 per https://www.pololu.com/file/0J1487/pololu-micro-metal-gearmotors-rev-6-1.pdf
-      abs_present: false
-      calibration_throttle: 0.5
-      limit_switch_0_present: true
-      limit_switch_0_enabled: true
-      limit_switch_0_limits_forward: false
-      limit_switch_0_active_high: false
-      limit_switch_0_used_for_readjustment: true
-      limit_switch_0_readjust_position: 0.0
-      limit_switch_1_present: true
-      limit_switch_1_enabled: true
-      limit_switch_1_limits_forward: true
-      limit_switch_1_active_high: false
-      limit_switch_1_used_for_readjustment: true
-      limit_switch_1_readjust_position: 0.0
+    u2d2_device: "/dev/u2d2"
+    gimbal_pitch:
+      id: 3
+      position_multiplier: 1.0
+      forward_limit: 0.0
+      reverse_limit: 1.57
+      position_p: 5000.0
+      position_i: 0.0
+      position_d: 0.0
+      velocity_p: 0.0
+      velocity_i: 0.0
+      current_limit: 1750.0
+      profile_acceleration: 10.0
+      profile_velocity: 60.0
+    gimbal_yaw:
+      id: 4
+      position_multiplier: 6.25
+      forward_limit: 6.108
+      reverse_limit: 0.174
+      position_p: 4000.0
+      position_i: 0.0
+      position_d: 0.0
+      velocity_p: 0.0
+      velocity_i: 0.0
+      current_limit: 1000.0
+      profile_acceleration: 100.0
+      profile_velocity: 300.0

config/science.yaml

@@ -0,0 +1,16 @@
+science_hw_bridge:
+  ros__parameters:
+    u2d2_device: "/dev/u2d2"
+    funnel: # TODO(eric) validate
+      id: 5
+      position_multiplier: 5.859375
+      forward_limit: 6.28
+      reverse_limit: 0.0
+      position_p: 5000.0
+      position_i: 0.0
+      position_d: 0.0
+      velocity_p: 0.0
+      velocity_i: 0.0
+      current_limit: 1750.0
+      profile_acceleration: 60.0
+      profile_velocity: 500.0

esw/drive_hw_bridge.cpp

@@ -22,9 +22,9 @@ namespace mrover {
 
     using namespace std::chrono_literals;
 
-    class MotorTestBridge final : public rclcpp::Node {
+    class DriveHWBridge final : public rclcpp::Node {
     public:
-        MotorTestBridge() : Node{"drive_hw_bridge"} {
+        DriveHWBridge() : Node{"drive_hw_bridge"} {
             // all initialization is done in the init() function to allow for the usage of shared_from_this()
         }
 
@@ -147,7 +147,7 @@ namespace mrover {
 
 auto main(int const argc, char** argv) -> int {
     rclcpp::init(argc, argv);
-    auto const drive_bridge = std::make_shared<mrover::MotorTestBridge>();
+    auto const drive_bridge = std::make_shared<mrover::DriveHWBridge>();
     drive_bridge->init();
     rclcpp::spin(drive_bridge);
     rclcpp::shutdown();