diff --git "a/\001\360A@@-\274\002@8" "b/\001\360A@@-\274\002@8" new file mode 100644 index 00000000..e69de29b diff --git a/.gitignore b/.gitignore index 0bab88ce..f38ee457 100644 --- a/.gitignore +++ b/.gitignore @@ -45,3 +45,7 @@ state_machine/state_log.txt data/*.png data/raw-pano-images + +# Dynamixel Ignores +.catkin_tools/ +.dynamixel_sdk/ \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 4896fbfa..07743416 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/action/ClickIk.action b/action/ClickIk.action index 0763ad52..e375689c 100644 --- a/action/ClickIk.action +++ b/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 \ No newline at end of file diff --git a/action/IkImageSample.action b/action/IkImageSample.action new file mode 100644 index 00000000..639eefff --- /dev/null +++ b/action/IkImageSample.action @@ -0,0 +1,8 @@ +uint32 w +uint32 h +float32 scale +--- +bool[] success +--- +uint32 completed +uint32 total \ No newline at end of file diff --git a/ansible/roles/build/tasks/main.yml b/ansible/roles/build/tasks/main.yml index 4b97ef4b..877fc7ec 100644 --- a/ansible/roles/build/tasks/main.yml +++ b/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 diff --git a/ansible/roles/ci/tasks/main.yml b/ansible/roles/ci/tasks/main.yml index 79d7649e..31ac437e 100644 --- a/ansible/roles/ci/tasks/main.yml +++ b/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 diff --git a/ansible/roles/esw/files/rules/99-u2d2.rules b/ansible/roles/esw/files/rules/99-u2d2.rules new file mode 100644 index 00000000..f49f87a2 --- /dev/null +++ b/ansible/roles/esw/files/rules/99-u2d2.rules @@ -0,0 +1 @@ +SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6014", SYMLINK+="u2d2", GROUP="mrover" diff --git a/config/cameras.yaml b/config/cameras.yaml index 6611f669..0858509f 100644 --- a/config/cameras.yaml +++ b/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 diff --git a/config/localization.yaml b/config/localization.yaml index 73f0b0a6..b13f163e 100644 --- a/config/localization.yaml +++ b/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: diff --git a/config/mast_gimbal.yaml b/config/mast_gimbal.yaml index 4ec0b089..66956bf6 100644 --- a/config/mast_gimbal.yaml +++ b/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 diff --git a/config/science.yaml b/config/science.yaml new file mode 100644 index 00000000..cc262f83 --- /dev/null +++ b/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 diff --git a/esw/drive_hw_bridge.cpp b/esw/drive_hw_bridge.cpp index a32785e8..97581ea9 100644 --- a/esw/drive_hw_bridge.cpp +++ b/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(); + auto const drive_bridge = std::make_shared(); drive_bridge->init(); rclcpp::spin(drive_bridge); rclcpp::shutdown(); diff --git a/esw/gst_camera_server/gst_camera_server.cpp b/esw/gst_camera_server/gst_camera_server.cpp index b35edc77..23985b84 100644 --- a/esw/gst_camera_server/gst_camera_server.cpp +++ b/esw/gst_camera_server/gst_camera_server.cpp @@ -1,4 +1,5 @@ #include "gst_camera_server.hpp" +#include "gst_utils.hpp" namespace mrover { @@ -16,8 +17,6 @@ namespace mrover { auto gstBusMessage(GstBus*, GstMessage* message, gpointer data) -> gboolean; - auto isIpAddressReachable(std::string const& address, int port) -> bool; - auto GstCameraServer::deviceImageCallback(sensor_msgs::msg::Image::ConstSharedPtr const& msg) -> void { if (!mStreamPipelineWrapper.isPlaying() && !imageCaptureEnabled()) { return; @@ -68,7 +67,7 @@ namespace mrover { gst::addProperty("is-live", true), gst::addProperty("format", "time"), gst::addProperty("do-timestamp", true)); - pipeline.pushBack(std::format("video/x-raw,format={},width={},height={},framerate={}/1", gst::video::toString(gst::video::RawFormat::BGRA), imageWidth, imageHeight, imageFramerate)); + pipeline.pushBack(std::format("video/x-raw,format={},width={},height={},framerate={}/1", gst::video::v4l2::toStringGstType(pixelFormat), imageWidth, imageHeight, imageFramerate)); pipeline.pushBack("queue"); } else if (captureIsDev()) { pipeline.pushBack(gst::video::v4l2::createSrc(mDeviceNode, mStreamCaptureFormat, gst::addProperty("is-live", true))); @@ -78,69 +77,111 @@ namespace mrover { pipeline.pushBack(std::format("video/x-raw,format={},width={},height={},framerate={}/1", gst::video::v4l2::toStringGstType(pixelFormat), imageWidth, imageHeight, imageFramerate)); } - // Source decoder and H265 encoder - if (gst_element_factory_find("nvv4l2h265enc")) { - // Most likely on the Jetson - if (pixelFormat == gst::video::v4l2::PixelFormat::MJPG) { - // TODO(quintin): I had to apply this patch: https://forums.developer.nvidia.com/t/macrosilicon-usb/157777/4 - // nvv4l2camerasrc only supports UYUV by default, but our cameras are YUY2 (YUYV) - // Mostly used with USB cameras, MPEG capture uses way less USB bandwidth - pipeline.pushBack("nvv4l2decoder", gst::addProperty("mjpeg", 1)); // Hardware-accelerated JPEG decoding, output is apparently some unknown proprietary format - pipeline.pushBack("nvvidconv"); // Convert from proprietary format to NV12 so the encoder understands it - pipeline.pushBack("video/x-raw(memory:NVMM),format=NV12"); - - } else { - if (cropEnabled()) { - pipeline.pushBack(std::format("videocrop left={} right={} top={} bottom={}", mCropLeft, mCropRight, mCropTop, mCropBottom)); + if (nvHardwareAvailable()) { + switch (pixelFormat) { + case gst::video::v4l2::PixelFormat::YUYV: + case gst::video::v4l2::PixelFormat::ABGR32: { + if (cropEnabled()) { + pipeline.pushBack(std::format("videocrop left={} right={} top={} bottom={}", mCropLeft, mCropRight, mCropTop, mCropBottom)); + } + + pipeline.pushBack("nvvidconv"); + pipeline.pushBack("video/x-raw(memory:NVMM),format=NV12"); + break; + } + case gst::video::v4l2::PixelFormat::MJPG: { + pipeline.pushBack("nvv4l2decoder", gst::addProperty("mjpeg", 1)); // Hardware-accelerated JPEG decoding, output is apparently some unknown proprietary format + pipeline.pushBack("nvvidconv"); // Convert from proprietary format to NV12 so the encoder understands it + pipeline.pushBack("video/x-raw(memory:NVMM),format=NV12"); + // is not efficient to crop NV12 so we do not support it + break; } - - pipeline.pushBack("videoconvert"); - pipeline.pushBack("video/x-raw,format=I420"); // Convert to I420 for the encoder, note we are still on the CPU - pipeline.pushBack("nvvidconv"); // Upload to GPU memory for the encoder - pipeline.pushBack("video/x-raw(memory:NVMM),format=I420"); } - - pipeline.pushBack("nvv4l2h265enc", - gst::addProperty("bitrate", mBitrate), - gst::addProperty("iframeinterval", 300), - gst::addProperty("vbv-size", 33333), - gst::addProperty("insert-sps-pps", true), - gst::addProperty("control-rate", "constant_bitrate"), - gst::addProperty("profile", "Main"), - gst::addProperty("num-B-Frames", 0), - gst::addProperty("ratecontrol-enable", true), - gst::addProperty("preset-level", "UltraFastPreset"), - gst::addProperty("EnableTwopassCBR", false), - gst::addProperty("maxperf-enable", true)); - } else { - // For desktop/laptops with no hardware encoder - if (gst::video::v4l2::isCompressedPixelFormat(pixelFormat)) { - pipeline.pushBack(gst::video::createDefaultDecoder(toStringGstType(pixelFormat))); - } else { - pipeline.pushBack("videoconvert"); + switch (mCodec) { + // TODO (owen): ADD MORE FLEXIBILITY FOR ENCODING PROPERTIES + // https://docs.nvidia.com/metropolis/deepstream/7.1/text/DS_plugin_gst-nvvideo4linux2.html#encoder + case gst::video::Codec::H265: { + pipeline.pushBack("nvv4l2h265enc", + gst::addProperty("bitrate", mBitrate), + gst::addProperty("iframeinterval", 300), + gst::addProperty("vbv-size", 33333), + gst::addProperty("insert-sps-pps", true), + gst::addProperty("control-rate", "constant_bitrate"), + gst::addProperty("profile", "main"), + gst::addProperty("num-B-Frames", 0), + gst::addProperty("ratecontrol-enable", true), + gst::addProperty("preset-level", "UltraFastPreset"), + gst::addProperty("EnableTwopassCBR", false), + gst::addProperty("maxperf-enable", true)); + pipeline.pushBack("h265parse"); + break; + } + case gst::video::Codec::H264: { + pipeline.pushBack("nvv4l2h265enc", + gst::addProperty("bitrate", mBitrate)); + pipeline.pushBack("h264parse"); + break; + } + case gst::video::Codec::AV1: { + pipeline.pushBack("nvv4l2av1enc", + gst::addProperty("bitrate", mBitrate)); + break; + } + default: { + throw std::runtime_error{"Unsupported codec for NVENC hardware acceleration"}; + } } - - if (cropEnabled()) { - pipeline.pushBack(std::format("videocrop left={} right={} top={} bottom={}", mCropLeft, mCropRight, mCropTop, mCropBottom)); + } else { + switch (pixelFormat) { + case gst::video::v4l2::PixelFormat::YUYV: + case gst::video::v4l2::PixelFormat::ABGR32: { + if (cropEnabled()) { + pipeline.pushBack(std::format("videocrop left={} right={} top={} bottom={}", mCropLeft, mCropRight, mCropTop, mCropBottom)); + } + pipeline.pushBack("videoconvert"); + pipeline.pushBack(std::format("video/x-raw,format={}", gst::video::toString(gst::video::RawFormat::I420))); + break; + } + case gst::video::v4l2::PixelFormat::MJPG: { + pipeline.pushBack(gst::video::createDefaultDecoder(toStringGstType(pixelFormat))); + pipeline.pushBack("videoconvert"); + pipeline.pushBack(std::format("video/x-raw,format={}", gst::video::toString(gst::video::RawFormat::I420))); + if (cropEnabled()) { + pipeline.pushBack(std::format("videocrop left={} right={} top={} bottom={}", mCropLeft, mCropRight, mCropTop, mCropBottom)); + } + break; + } } - pipeline.pushBack(gst::video::createDefaultEncoder(mCodec)); - - if (mCodec == gst::video::Codec::H264) { + if (mCodec == gst::video::Codec::H265) { + pipeline.addPropsToElement(pipeline.size() - 1, + gst::addProperty("speed-preset", "ultrafast"), + gst::addProperty("tune", "zerolatency"), + gst::addProperty("bitrate", mBitrate / 1000), + gst::addProperty("name", "encoder")); + } else if (mCodec == gst::video::Codec::H264) { pipeline.addPropsToElement(pipeline.size() - 1, gst::addProperty("tune", "zerolatency"), + gst::addProperty("speed-preset", "ultrafast"), gst::addProperty("bitrate", mBitrate), gst::addProperty("name", "encoder")); } } + // ===== RTP PAYLOADER ===== + pipeline.pushBack(gst::video::getRtpPayloader(mCodec)); if (mCodec == gst::video::Codec::H265) { - pipeline.pushBack("h265parse"); - } else if (mCodec == gst::video::Codec::H264) { - pipeline.pushBack("h264parse"); + pipeline.addPropsToElement(pipeline.size() - 1, + gst::addProperty("config-interval", 1)); } + pipeline.pushBack("queue", + gst::addProperty("leaky", 2), // drop old packets if the network is slow + gst::addProperty("max-size-buffers", 3)); // try decreasing to 1 or 2 too reduce latency + pipeline.pushBack("udpsink", + gst::addProperty("host", mAddress), + gst::addProperty("port", mPort), + gst::addProperty("sync", false)); - pipeline.pushBack(gst::video::createRtpSink(mAddress, mPort, mCodec)); RCLCPP_INFO_STREAM(get_logger(), std::format("GStreamer stream launch string: {}", pipeline.str())); mStreamPipelineWrapper = gst::PipelineWrapper(pipeline.str()); @@ -335,6 +376,13 @@ namespace mrover { return mImageCaptureServer != nullptr; } + [[nodiscard]] auto GstCameraServer::nvHardwareAvailable() -> bool { + return (gst_element_factory_find("nvv4l2h265enc") != nullptr) && + (gst_element_factory_find("nvv4l2h264enc") != nullptr) && + (gst_element_factory_find("nvv4l2decoder") != nullptr) && + (gst_element_factory_find("nvvidconv") != nullptr); + } + GstCameraServer::GstCameraServer(rclcpp::NodeOptions const& options) : Node{"gst_camera_server", options} { try { declare_parameter("camera", rclcpp::ParameterType::PARAMETER_STRING); @@ -368,13 +416,13 @@ namespace mrover { {std::format("{}.crop_right", cameraName), mCropRight, 0}, {std::format("{}.crop_top", cameraName), mCropTop, 0}, {std::format("{}.crop_bottom", cameraName), mCropBottom, 0}, - {std::format("{}.stream.pixel_format", cameraName), streamPixelFormat, "MJPG"}, + {std::format("{}.stream.pixel_format", cameraName), streamPixelFormat, "YUYV"}, {std::format("{}.stream.width", cameraName), streamImageWidth, 0}, {std::format("{}.stream.height", cameraName), streamImageHeight, 0}, {std::format("{}.stream.framerate", cameraName), streamImageFramerate, 0}, {std::format("{}.stream.codec", cameraName), codec, "H265"}, {std::format("{}.stream.bitrate", cameraName), bitrate, 0}, - {std::format("{}.image_capture.pixel_format", cameraName), imageCapturePixelFormat, "MJPG"}, + {std::format("{}.image_capture.pixel_format", cameraName), imageCapturePixelFormat, "YUYV"}, {std::format("{}.image_capture.width", cameraName), imageCaptureImageWidth, 0}, {std::format("{}.image_capture.height", cameraName), imageCaptureImageHeight, 0}, {std::format("{}.image_capture.framerate", cameraName), imageCaptureImageFramerate, 0}, @@ -394,15 +442,7 @@ namespace mrover { } } - mPort = static_cast(port); - // TODO:(owen) none of this works - // if (mAddress != DEFAULT_IP_ADDRESS) { - // if (!isIpAddressReachable(mAddress, mPort)) { - // RCLCPP_ERROR_STREAM(get_logger(), std::format("IP address {} is not reachable. Using {} instead", mAddress, DEFAULT_IP_ADDRESS)); - // mAddress = DEFAULT_IP_ADDRESS; - // } - // } mStreamCaptureFormat = gst::video::v4l2::CaptureFormat{ .pixelFormat = gst::video::v4l2::getPixelFormatFromStringView(streamPixelFormat), @@ -499,24 +539,6 @@ namespace mrover { return TRUE; } - auto isIpAddressReachable(std::string const& address, int port) -> bool { - int sockfd = socket(AF_INET, SOCK_STREAM, 0); - - struct sockaddr_in sin {}; - sin.sin_family = AF_INET; - sin.sin_port = htons(port); - inet_pton(AF_INET, address.c_str(), &sin.sin_addr); - - bool isReachable; - if (connect(sockfd, (struct sockaddr*) &sin, sizeof(sin)) == -1) { - isReachable = false; - } else { - isReachable = true; - close(sockfd); - } - return isReachable; - } - } // namespace mrover #include "rclcpp_components/register_node_macro.hpp" diff --git a/esw/gst_camera_server/gst_camera_server.hpp b/esw/gst_camera_server/gst_camera_server.hpp index c82711f7..4fd19bd0 100644 --- a/esw/gst_camera_server/gst_camera_server.hpp +++ b/esw/gst_camera_server/gst_camera_server.hpp @@ -55,6 +55,8 @@ namespace mrover { auto mediaControlServerCallback(srv::MediaControl::Request::ConstSharedPtr const& req, srv::MediaControl::Response::SharedPtr const& res) -> void; auto imageCaptureServerCallback(std_srvs::srv::Trigger::Request::ConstSharedPtr const&, std_srvs::srv::Trigger::Response::SharedPtr const& res) -> void; + [[nodiscard]] static auto nvHardwareAvailable() -> bool; + public: explicit GstCameraServer(rclcpp::NodeOptions const& options = rclcpp::NodeOptions()); diff --git a/esw/led.cpp b/esw/led.cpp deleted file mode 100644 index 11f41869..00000000 --- a/esw/led.cpp +++ /dev/null @@ -1,94 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -enum class LEDMode { - Unknown = 0, - Off = 1, - Red = 2, - BlinkingGreen = 3, - Blue = 4 -}; - -static constexpr std::string_view DONE_STATE = "DoneState"; - -namespace mrover { - class LED final : public rclcpp::Node { - public: - LED() : Node{"led"} { - mStateSub = create_subscription( - "/nav_state", 10, [this](mrover::msg::StateMachineStateUpdate::ConstSharedPtr const& msg) { - stateMachineUpdateCallback(msg); - }); - - mLedPub = create_publisher("led", 10); - - mTeleopEnableServer = create_service( - "/enable_teleop", [this](std_srvs::srv::SetBool::Request::SharedPtr const& req, - std_srvs::srv::SetBool::Response::SharedPtr const& res) { - teleopEnabledCallback(req, res); - }); - } - - auto updateLED() -> void { - if (is_teleop_enabled) { - led_mode = LEDMode::Blue; - } else if (is_navigation_done) { - led_mode = LEDMode::BlinkingGreen; - } else { - led_mode = LEDMode::Red; - } - - mrover::msg::LED led_msg; - - if (led_mode == LEDMode::Red) { - led_msg.color = mrover::msg::LED::RED; - } else if (led_mode == LEDMode::Blue) { - led_msg.color = mrover::msg::LED::BLUE; - } else if (led_mode == LEDMode::BlinkingGreen) { - led_msg.color = mrover::msg::LED::BLINKING_GREEN; - } - - mLedPub->publish(led_msg); - } - - // When navigation reaches a waypoint it will publish "DoneState" to the "nav_state" topic - auto stateMachineUpdateCallback(mrover::msg::StateMachineStateUpdate::ConstSharedPtr const& msg) -> void { - is_navigation_done = msg->state == DONE_STATE; - - updateLED(); - } - - auto teleopEnabledCallback(std_srvs::srv::SetBool::Request::SharedPtr const& request, std_srvs::srv::SetBool::Response::SharedPtr const& response) -> bool { - is_teleop_enabled = request->data; - - updateLED(); - - return response->success = true; - } - - private: - bool is_navigation_done = false; - bool is_teleop_enabled = false; - LEDMode led_mode = LEDMode::Unknown; - - rclcpp::Publisher::SharedPtr mLedPub; - rclcpp::Subscription::SharedPtr mStateSub; - rclcpp::Service::SharedPtr mTeleopEnableServer; - }; -} // namespace mrover - -auto main(int argc, char** argv) -> int { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - - return 0; -} \ No newline at end of file diff --git a/esw/mast_gimbal_hw_bridge.cpp b/esw/mast_gimbal_hw_bridge.cpp index c2fdfd85..709b4931 100644 --- a/esw/mast_gimbal_hw_bridge.cpp +++ b/esw/mast_gimbal_hw_bridge.cpp @@ -9,102 +9,150 @@ #include #include -#include -#include -#include -#include -#include +#include -#include "motor_library/brushed.hpp" +#include +#include namespace mrover { class MastGimbalHWBridge : public rclcpp::Node { public: - MastGimbalHWBridge() : rclcpp::Node{"mast_gimbal_hw_bridge"} { + MastGimbalHWBridge() : Node{"mast_gimbal_hw_bridge"} { // all initialization is done in the init() function to allow for the usage of shared_from_this() } auto init() -> void { + // parse parameters + std::vector parameters = { + {"u2d2_device", mU2D2DeviceName, "/dev/u2d2"}, + }; + ParameterWrapper::declareParameters(this, parameters); + + mU2D2 = U2D2::getSharedInstance(); + if (mU2D2->init(mU2D2DeviceName) != U2D2::Status::Success) { + RCLCPP_FATAL(this->get_logger(), "failed to initialize U2D2 on %s", mU2D2DeviceName.c_str()); + rclcpp::shutdown(); + } - for (auto const& name: mMotorNames) { - mMotors[name] = std::make_shared(shared_from_this(), "jetson", name); + for (std::string const& servoName: mServoNames) { + auto servo = std::make_shared(shared_from_this(), servoName); + mServos.insert_or_assign(servoName, servo); + mControllerState.names.push_back(servoName); } - mThrottleSub = create_subscription("mast_gimbal_throttle_cmd", 1, [this](msg::Throttle::ConstSharedPtr const& msg) { processThrottleCmd(msg); }); + mTimerGroup = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + mServiceGroup = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + auto subOptions = rclcpp::SubscriptionOptions(); + subOptions.callback_group = mServiceGroup; - mPublishDataTimer = create_wall_timer( + mPositionService = this->create_service( + "gimbal_servo", + [this](srv::ServoPosition::Request::SharedPtr const& req, srv::ServoPosition::Response::SharedPtr const& res) { + servoPositionCallback(req, res); + }, + rmw_qos_profile_services_default, + mServiceGroup); + + mPublishTimer = this->create_wall_timer( std::chrono::milliseconds(100), - [this]() { publishDataCallback(); }); - mJointDataPub = create_publisher("mast_gimbal_joint_data", 1); - mControllerStatePub = create_publisher("mast_gimbal_controller_state", 1); - - mJointData.name = mMotorNames; - mJointData.position.resize(mMotorNames.size()); - mJointData.velocity.resize(mMotorNames.size()); - mJointData.effort.resize(mMotorNames.size()); - - mControllerState.names = mMotorNames; - mControllerState.states.resize(mMotorNames.size()); - mControllerState.errors.resize(mMotorNames.size()); - mControllerState.limits_hit.resize(mMotorNames.size()); + [this]() { publishDataCallback(); }, + mTimerGroup); + + mGimbalStatePub = this->create_publisher("gimbal_controller_state", 10); } private: - std::vector const mMotorNames = {"mast_gimbal_pitch", "mast_gimbal_yaw"}; - std::unordered_map> mMotors; + std::vector mServoNames = {"gimbal_pitch", "gimbal_yaw"}; + std::unordered_map> mServos; - rclcpp::Subscription::SharedPtr mThrottleSub; + std::shared_ptr mU2D2; + std::string mU2D2DeviceName; - rclcpp::TimerBase::SharedPtr mPublishDataTimer; - rclcpp::Publisher::SharedPtr mJointDataPub; - rclcpp::Publisher::SharedPtr mControllerStatePub; - sensor_msgs::msg::JointState mJointData; + rclcpp::CallbackGroup::SharedPtr mServiceGroup; + rclcpp::CallbackGroup::SharedPtr mTimerGroup; + + rclcpp::Service::SharedPtr mPositionService; + rclcpp::Publisher::SharedPtr mGimbalStatePub; + rclcpp::TimerBase::SharedPtr mPublishTimer; msg::ControllerState mControllerState; + auto servoPositionCallback(srv::ServoPosition::Request::SharedPtr const& req, srv::ServoPosition::Response::SharedPtr const& res) -> void { + size_t const n = req->names.size(); + res->at_tgts.resize(n, false); - auto processThrottleCmd(msg::Throttle::ConstSharedPtr const& msg) -> void { - if (msg->names.size() != msg->throttles.size()) { - RCLCPP_ERROR(get_logger(), "Name count and value count mismatched!"); - return; + for (size_t i = 0; i < n; ++i) { + if (auto const it = mServos.find(req->names[i]); it != mServos.end()) { + it->second->setPosition(req->positions[i], Servo::ServoMode::Limited); + } } - for (std::size_t i = 0; i < msg->names.size(); ++i) { - std::string const& name = msg->names[i]; - Dimensionless const& throttle = msg->throttles[i]; - mMotors[name]->setDesiredThrottle(throttle); - } - } + auto const start = this->now(); + auto const timeout = rclcpp::Duration::from_seconds(3); + rclcpp::Rate loop_rate(10); + while ((this->now() - start) < timeout) { + bool all_done = true; - auto publishDataCallback() -> void { - mJointData.header.stamp = get_clock()->now(); + for (size_t i = 0; i < n; ++i) { + auto it = mServos.find(req->names[i]); + if (it == mServos.end()) continue; - for (size_t i = 0; i < mMotorNames.size(); ++i) { - auto const& name = mMotorNames[i]; - auto const& motor = mMotors[name]; + auto const status = it->second->getTargetStatus(); + bool const reached = status == U2D2::Status::Success; + res->at_tgts[i] = reached; - mJointData.position[i] = {motor->getPosition().get()}; - mJointData.velocity[i] = {motor->getVelocity().get()}; - mJointData.effort[i] = {motor->getEffort()}; + if (!reached) all_done = false; + } - mControllerState.states[i] = {motor->getState()}; - mControllerState.errors[i] = {motor->getErrorState()}; - mControllerState.limits_hit[i] = {motor->getLimitsHitBits()}; + if (all_done) return; + loop_rate.sleep(); } - mJointDataPub->publish(mJointData); - mControllerStatePub->publish(mControllerState); + RCLCPP_WARN(this->get_logger(), "servo position timeout reached!"); + } + + auto publishDataCallback() -> void { + mControllerState.names.clear(); + mControllerState.positions.clear(); + mControllerState.velocities.clear(); + mControllerState.currents.clear(); + mControllerState.errors.clear(); + mControllerState.states.clear(); + mControllerState.limits_hit.clear(); + + for (auto const& [name, servo]: mServos) { + double pos, vel, cur; + U2D2::Status const status = servo->getPosition(pos); + servo->getVelocity(vel); + servo->getCurrent(cur); + + mControllerState.names.push_back(name); + mControllerState.positions.push_back(static_cast(pos)); + mControllerState.velocities.push_back(static_cast(vel)); + mControllerState.currents.push_back(static_cast(cur)); + mControllerState.errors.push_back(U2D2::stringifyStatus(status)); + mControllerState.states.push_back(U2D2::stringifyStatus(servo->getTargetStatus())); + mControllerState.limits_hit.push_back(servo->getLimitStatus()); + } + + mGimbalStatePub->publish(mControllerState); } }; } // namespace mrover -auto main(int argc, char** argv) -> int { + +auto main(int const argc, char** argv) -> int { rclcpp::init(argc, argv); - auto mast_gimbal_hw_bridge = std::make_shared(); + auto const mast_gimbal_hw_bridge = std::make_shared(); mast_gimbal_hw_bridge->init(); - rclcpp::spin(mast_gimbal_hw_bridge); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(mast_gimbal_hw_bridge); + executor.spin(); + rclcpp::shutdown(); return EXIT_SUCCESS; } diff --git a/esw/pdlb_hw_bridge.cpp b/esw/pdlb_hw_bridge.cpp deleted file mode 100644 index 9d9a9464..00000000 --- a/esw/pdlb_hw_bridge.cpp +++ /dev/null @@ -1,93 +0,0 @@ -#include "can_device.hpp" -#include "messaging.hpp" - -#include - -#include -#include -#include -#include - -namespace mrover { - - class PDLBBridge final : public rclcpp::Node { - public: - PDLBBridge() : rclcpp::Node("pdlb_hw_bridge") { - changeLEDSubscriber = create_subscription("led", 10, [this](mrover::msg::LED::ConstSharedPtr const& msg) { - PDLBBridge::changeLED(msg); - }); - PDLCANSubscriber = create_subscription("can/pdlb/in", 10, [this](msg::CAN::ConstSharedPtr const& msg) { - PDLBBridge::processCANMessage(msg); - }); - - enableArmLaserService = this->create_service("enable_arm_laser", [this]( - std_srvs::srv::SetBool::Request::SharedPtr const& request, - std_srvs::srv::SetBool::Response::SharedPtr response) { - PDLBBridge::handleEnableArmLaser(request, response); - }); - } - - void initialize() { - // Use this->shared_from_this() since rclcpp::Node already supports it - ledCanDevice = std::make_unique(this->shared_from_this(), "jetson", "pdlb"); - } - - void changeLED(mrover::msg::LED::ConstSharedPtr const& msg) { - if (!ledCanDevice) { - RCLCPP_ERROR(this->get_logger(), "ledCanDevice not initialized!"); - return; - } - mrover::LEDInfo ledInfo{}; - ledInfo.red = msg->color == mrover::msg::LED::RED; - ledInfo.green = msg->color == mrover::msg::LED::BLINKING_GREEN; - ledInfo.blue = msg->color == mrover::msg::LED::BLUE; - ledInfo.blinking = msg->color == mrover::msg::LED::BLINKING_GREEN; - ledCanDevice->publish_message(mrover::InBoundPDLBMessage{mrover::LEDCommand{.led_info = ledInfo}}); - } - - void handleEnableArmLaser( - std::shared_ptr const& request, - std::shared_ptr& response) { - if (request->data) { - // Code to enable the arm laser - response->message = "Arm laser enabled"; - ledCanDevice->publish_message(mrover::InBoundPDLBMessage{mrover::ArmLaserCommand{.enable = true}}); - response->success = true; - } else { - // Code to disable the arm laser - response->message = "Arm laser disabled"; - ledCanDevice->publish_message(mrover::InBoundPDLBMessage{mrover::ArmLaserCommand{.enable = false}}); - response->success = true; - } - } - - void processMessage(mrover::PDBData const msg) { - mrover::msg::PDLB msgToSend; - msgToSend.temperatures = msg.temperatures; - msgToSend.currents = msg.currents; - // Publish the message - pdlbPublisher->publish(msgToSend); - } - void processCANMessage(msg::CAN::ConstSharedPtr const& msg) { - OutBoundPDLBMessage const& message = *reinterpret_cast(msg->data.data()); - std::visit([this](auto&& arg) { processMessage(arg); }, message); - } - - private: - rclcpp::Subscription::SharedPtr changeLEDSubscriber; - rclcpp::Subscription::ConstSharedPtr PDLCANSubscriber; - std::unique_ptr ledCanDevice; - rclcpp::Publisher::SharedPtr pdlbPublisher; - rclcpp::Service::SharedPtr enableArmLaserService; - }; - -} // namespace mrover - -auto main(int argc, char** argv) -> int { - rclcpp::init(argc, argv); - auto led = std::make_shared(); - led->initialize(); - rclcpp::spin(led); - rclcpp::shutdown(); - return EXIT_SUCCESS; -} \ No newline at end of file diff --git a/esw/sa_hw_bridge.cpp b/esw/sa_hw_bridge.cpp deleted file mode 100644 index e8eeda44..00000000 --- a/esw/sa_hw_bridge.cpp +++ /dev/null @@ -1,278 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "motor_library/brushed.hpp" -#include - -namespace mrover { - -#pragma pack(push, 1) - static constexpr std::uint8_t HEADER_BYTE = 0xA6; - static constexpr std::uint8_t SERVO_SET_POSITION = 0x00; - - struct ServoSetPosition { - std::uint8_t header = HEADER_BYTE; - std::uint8_t messageID = SERVO_SET_POSITION; - std::uint8_t id{}; // internal id of servo - std::uint8_t isCounterClockwise{}; - float radians{}; - }; -#pragma pack(pop) - - class SAHWBridge : public rclcpp::Node { - static constexpr int SERIAL_INPUT_MSG_SIZE = 14; - - std::vector const mMotorNames = {"linear_actuator", "auger", "pump_0", "pump_1", "sensor_actuator"}; - std::unordered_map> mMotors; - - rclcpp::TimerBase::SharedPtr mPublishDataTimer; - - rclcpp::Subscription::SharedPtr mThrottleSub; - - rclcpp::Publisher::SharedPtr mJointDataPub; - rclcpp::Publisher::SharedPtr mControllerStatePub; - rclcpp::Publisher::SharedPtr mTemperatureDataPub; - rclcpp::Publisher::SharedPtr mHumidityDataPub; - rclcpp::Publisher::SharedPtr mServoPositionPub; - - sensor_msgs::msg::JointState mJointData; - msg::ControllerState mControllerState; - - - rclcpp::Service::SharedPtr mSetServoPositionSrv; - - boost::asio::serial_port mSerial; - boost::asio::io_service& io; - boost::asio::streambuf mInputBuffer; - std::vector mBuffer; - std::deque> mWriteQueue; - unsigned long mSerialBaudRate{}; - std::uint8_t mServoID{}; - std::string mSerialPort; - - - auto processThrottleCmd(msg::Throttle::ConstSharedPtr const& msg) -> void { - if (msg->names.size() != msg->throttles.size()) { - RCLCPP_ERROR(get_logger(), "Name count and value count mismatched!"); - return; - } - - for (std::size_t i = 0; i < msg->names.size(); ++i) { - std::string const& name = msg->names[i]; - Dimensionless const& throttle = msg->throttles[i]; - mMotors[name]->setDesiredThrottle(throttle); - } - } - - auto publishDataCallback() -> void { - mJointData.header.stamp = get_clock()->now(); - - for (size_t i = 0; i < mMotorNames.size(); ++i) { - auto const& name = mMotorNames[i]; - auto const& motor = mMotors[name]; - - mJointData.position[i] = {motor->getPosition().get()}; - mJointData.velocity[i] = {motor->getVelocity().get()}; - mJointData.effort[i] = {motor->getEffort()}; - - mControllerState.states[i] = {motor->getState()}; - mControllerState.errors[i] = {motor->getErrorState()}; - mControllerState.limits_hit[i] = {motor->getLimitsHitBits()}; - } - - mJointDataPub->publish(mJointData); - mControllerStatePub->publish(mControllerState); - } - - auto setServoPositionServiceCallback(srv::ServoSetPos::Request::ConstSharedPtr const& req, srv::ServoSetPos::Response::SharedPtr const& res) -> void { - ServoSetPosition set_pos{ - .id = mServoID, - .isCounterClockwise = req->is_counterclockwise, - .radians = req->position}; - - // parse set pos into a vector of bytes - std::vector bytes(sizeof(set_pos)); - std::memcpy(bytes.data(), &set_pos, sizeof(set_pos)); - - bool is_writing = !mWriteQueue.empty(); - mWriteQueue.push_back(bytes); - - if (!is_writing) { - asyncWriteSerial(); - } - - res->success = true; // TODO: what is fail condition here? service no longer performs serial write - } - - auto startAsyncReadThread() -> void { - std::thread([this]() { - asyncReadSerial(); - io.run(); - }).detach(); - } - - auto asyncReadSerial() -> void { - boost::asio::async_read(mSerial, boost::asio::buffer(mBuffer), boost::asio::transfer_exactly(SERIAL_INPUT_MSG_SIZE), - [this](boost::system::error_code const& ec, std::size_t len) { - if (ec) { - RCLCPP_ERROR(this->get_logger(), "Serial read failed: %s", ec.message().c_str()); - } else if (len != SERIAL_INPUT_MSG_SIZE) { - RCLCPP_ERROR(this->get_logger(), "Failed to read whole serial buffer"); - } else { - parseAndPublishBuffer(mBuffer); - } - // continue reads - asyncReadSerial(); - }); - } - - void asyncWriteSerial() { - boost::asio::async_write(mSerial, boost::asio::buffer(mWriteQueue.front()), [this](boost::system::error_code const& ec, std::size_t) { - if (ec) { - RCLCPP_ERROR(this->get_logger(), "Serial write failed: %s", ec.message().c_str()); - } - mWriteQueue.pop_front(); - if (!mWriteQueue.empty()) { - asyncWriteSerial(); - } - }); - } - - auto parseAndPublishBuffer(std::vector& buffer) const -> void { - // read from serial port and parse data - float raw_pos_rad{}; - float raw_temp{}; - float raw_hum{}; - - auto data = buffer.data(); - - if (*data == HEADER_BYTE) { - ++data; // points to DXL_ID - auto const dxl_id = static_cast(*data); - RCLCPP_DEBUG(this->get_logger(), "Using serial device %u", dxl_id); - ++data; - std::memcpy(&raw_pos_rad, data, sizeof(float)); - data += 4; - std::memcpy(&raw_temp, data, sizeof(float)); - data += 4; - std::memcpy(&raw_hum, data, sizeof(float)); - } else { - RCLCPP_WARN(this->get_logger(), "The first byte in serial message was not the header byte"); - } - - // send data to pubs - sensor_msgs::msg::Temperature temp_msg; - sensor_msgs::msg::RelativeHumidity humidity_msg; - msg::Position position_msg; - - temp_msg.temperature = raw_temp; - humidity_msg.relative_humidity = raw_hum; - position_msg.names = {"gear_diff"}; - position_msg.positions = {raw_pos_rad}; - - mServoPositionPub->publish(position_msg); - mTemperatureDataPub->publish(temp_msg); - mHumidityDataPub->publish(humidity_msg); - } - - public: - explicit SAHWBridge(boost::asio::io_service& io) : rclcpp::Node{"sa_hw_bridge"}, mSerial(io), io(io), mBuffer(SERIAL_INPUT_MSG_SIZE) { - // all initialization is done in the init() function to allow for the usage of shared_from_this() - } - - auto init() -> void { - mSerialPort = this->declare_parameter("port_unicore", "/dev/arduino"); - mSerialBaudRate = this->declare_parameter("baud_unicore", 115200); - mServoID = this->declare_parameter("servo_id", 0); - - for (auto const& name: mMotorNames) { - mMotors[name] = std::make_shared(shared_from_this(), "jetson", name); - } - - mThrottleSub = create_subscription("sa_throttle_cmd", 1, [this](msg::Throttle::ConstSharedPtr const& msg) { processThrottleCmd(msg); }); - - mJointDataPub = create_publisher("sa_joint_data", 1); - mControllerStatePub = create_publisher("sa_controller_state", 1); - mTemperatureDataPub = create_publisher("sa_temp_data", 1); - mHumidityDataPub = create_publisher("sa_humidity_data", 1); - mServoPositionPub = create_publisher("sa_gear_diff_position", 1); - - mPublishDataTimer = create_wall_timer( - std::chrono::milliseconds(100), - [this]() { publishDataCallback(); }); - - mJointData.name = mMotorNames; - mJointData.position.resize(mMotorNames.size()); - mJointData.velocity.resize(mMotorNames.size()); - mJointData.effort.resize(mMotorNames.size()); - - mControllerState.names = mMotorNames; - mControllerState.states.resize(mMotorNames.size()); - mControllerState.errors.resize(mMotorNames.size()); - mControllerState.limits_hit.resize(mMotorNames.size()); - - - mSetServoPositionSrv = create_service( - "sa_gear_diff_set_position", - [this](srv::ServoSetPos::Request::ConstSharedPtr const req, srv::ServoSetPos::Response::SharedPtr res) { - setServoPositionServiceCallback(req, res); - }); - - // configure serial io - boost::system::error_code ec; - mSerial.open(mSerialPort, ec); - if (ec) { - RCLCPP_FATAL(this->get_logger(), "Couldn't open serial port: %s", ec.message().c_str()); - } else { - mSerial.set_option(boost::asio::serial_port_base::baud_rate(mSerialBaudRate)); - // begin serial reads on a separate thread - startAsyncReadThread(); - } - } - - auto stop() -> void { - boost::system::error_code ec; - mSerial.close(ec); - if (ec) { - RCLCPP_WARN(this->get_logger(), "Failed to close serial port: %s", ec.message().c_str()); - } - } - }; -} // namespace mrover - - -auto main(int const argc, char** argv) -> int { - rclcpp::init(argc, argv); - - boost::asio::io_service io; - auto sa_hw_bridge = std::make_shared(io); - - sa_hw_bridge->init(); - rclcpp::spin(sa_hw_bridge); - - sa_hw_bridge->stop(); - rclcpp::shutdown(); - return EXIT_SUCCESS; -} diff --git a/esw/science_hw_bridge.cpp b/esw/science_hw_bridge.cpp index f5dd97e1..47efdee2 100644 --- a/esw/science_hw_bridge.cpp +++ b/esw/science_hw_bridge.cpp @@ -1,23 +1,18 @@ #include #include #include -#include #include #include -#include #include -#include -#include -#include -#include -#include #include -#include +#include #include #include +#include +#include namespace mrover { @@ -31,9 +26,35 @@ namespace mrover { ScienceHWBridge() : Node{"science_hw_bridge"} {} auto init() -> void { + // parse parameters + std::vector parameters = { + {"u2d2_device", mU2D2DeviceName, "/dev/u2d2"}, + }; + ParameterWrapper::declareParameters(this, parameters); + + mU2D2 = U2D2::getSharedInstance(); + if (mU2D2->init(mU2D2DeviceName) != U2D2::Status::Success) { + RCLCPP_FATAL(this->get_logger(), "failed to initialize U2D2 on %s", mU2D2DeviceName.c_str()); + rclcpp::shutdown(); + } + mScienceBoard = std::make_shared(shared_from_this(), "jetson", "science"); mAuger = std::make_shared>(shared_from_this(), "jetson", "auger"); mLinearActuator = std::make_shared>(shared_from_this(), "jetson", "linear_actuator"); + mFunnelServo = std::make_shared(shared_from_this(), "funnel"); + + mServiceGroup = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + auto subOptions = rclcpp::SubscriptionOptions(); + subOptions.callback_group = mServiceGroup; + + mFunnelPositionService = this->create_service( + "sp_funnel_servo", + [this](srv::ServoPosition::Request::SharedPtr const& req, srv::ServoPosition::Response::SharedPtr const& res) { + servoPositionCallback(req, res); + }, + rmw_qos_profile_services_default, + mServiceGroup); mSPThrottleSub = create_subscription("sp_thr_cmd", 1, [this](msg::Throttle::ConstSharedPtr const& msg) { processThrottleCmd(msg); }); @@ -59,18 +80,24 @@ namespace mrover { } private: - std::vector const mActuatorNames{"auger", "linear_actuator"}; + std::vector const mActuatorNames{"auger", "linear_actuator", "funnel"}; std::shared_ptr mScienceBoard; std::shared_ptr> mAuger; std::shared_ptr> mLinearActuator; + std::shared_ptr mFunnelServo; + std::shared_ptr mU2D2; + std::string mU2D2DeviceName; + + rclcpp::CallbackGroup::SharedPtr mServiceGroup; + rclcpp::Service::SharedPtr mFunnelPositionService; rclcpp::TimerBase::SharedPtr mPublishDataTimer; rclcpp::Subscription::SharedPtr mSPThrottleSub; rclcpp::Publisher::SharedPtr mControllerStatePub; msg::ControllerState mControllerState; - auto processThrottleCmd(msg::Throttle::ConstSharedPtr const& msg) -> void { + auto processThrottleCmd(msg::Throttle::ConstSharedPtr const& msg) const -> void { if (msg->names.size() != msg->throttles.size()) { RCLCPP_ERROR(get_logger(), "Name count and value count mismatched!"); return; @@ -92,6 +119,28 @@ namespace mrover { } } + auto servoPositionCallback(srv::ServoPosition::Request::SharedPtr const& req, srv::ServoPosition::Response::SharedPtr const& res) const -> void { + if (req->names.size() != 1 || req->names.at(0) != "funnel") return; + + mFunnelServo->setPosition(req->positions[0], Servo::ServoMode::Optimal); + res->at_tgts.resize(1); + + auto const start = this->now(); + auto const timeout = rclcpp::Duration::from_seconds(3); + rclcpp::Rate loop_rate(10); + + while ((this->now() - start) < timeout) { + auto const status = mFunnelServo->getTargetStatus(); + bool const reached = status == U2D2::Status::Success; + res->at_tgts[0] = reached; + + if (reached) return; + loop_rate.sleep(); + } + + RCLCPP_WARN(this->get_logger(), "servo position timeout reached!"); + } + auto publishDataCallback() -> void { mControllerState.header.stamp = now(); @@ -117,6 +166,20 @@ namespace mrover { mControllerState.currents[i] = mLinearActuator->getCurrent(); mControllerState.limits_hit[i] = mLinearActuator->getLimitsHitBits(); break; + case 'f' + 'o': { + double pos, vel, cur; + U2D2::Status const status = mFunnelServo->getPosition(pos); + mFunnelServo->getVelocity(vel); + mFunnelServo->getCurrent(cur); + mControllerState.names[i] = name; + mControllerState.states[i] = U2D2::stringifyStatus(mFunnelServo->getTargetStatus()); + mControllerState.errors[i] = U2D2::stringifyStatus(status); + mControllerState.positions[i] = static_cast(pos); + mControllerState.velocities[i] = static_cast(vel); + mControllerState.currents[i] = static_cast(cur); + mControllerState.limits_hit[i] = mFunnelServo->getLimitStatus(); + break; + } } } @@ -127,11 +190,15 @@ namespace mrover { } // namespace mrover -auto main(int argc, char** argv) -> int { +auto main(int const argc, char** argv) -> int { rclcpp::init(argc, argv); - auto scienceBridge = std::make_shared(); + auto const scienceBridge = std::make_shared(); scienceBridge->init(); - rclcpp::spin(scienceBridge); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(scienceBridge); + executor.spin(); + rclcpp::shutdown(); return EXIT_SUCCESS; } diff --git a/esw/servo/servo.hpp b/esw/servo/servo.hpp new file mode 100644 index 00000000..1dcfb438 --- /dev/null +++ b/esw/servo/servo.hpp @@ -0,0 +1,323 @@ +#pragma once + +#include +#include +#include +#include + +#include "u2d2.hpp" + +namespace mrover { + + class Servo { + using ServoID = uint8_t; + using ServoPosition = double; // Degrees + using ServoVelocity = double; // rot/sec + using ServoCurrent = double; // mA + using ServoAddr = uint16_t; + + static constexpr uint8_t ADDR_OPERATING_MODE = 11; + static constexpr uint8_t ADDR_TORQUE_ENABLE = 64; + static constexpr uint8_t ADDR_GOAL_POSITION = 116; + static constexpr uint8_t ADDR_PRESENT_POSITION = 132; + static constexpr uint8_t ADDR_PRESENT_VELOCITY = 128; + static constexpr uint8_t ADDR_PRESENT_CURRENT = 126; + + static constexpr int32_t SERVO_TICKS = 4096; + static constexpr uint8_t SERVO_POSITION_DEAD_ZONE = 5; + + static constexpr double TAU = 2 * M_PI; + + [[nodiscard]] constexpr auto getUpper(int64_t const val) const -> auto { return val == 0 ? SERVO_TICKS : val; } + + ServoID mServoID; + std::string mServoName; + int64_t mLimitAdjustment; + int64_t mAdjustedForwardLimit; + int64_t mAdjustedReverseLimit; + int64_t mGoalPosition; + uint32_t mPositionOffsetTicks; + double mPositionMultiplier; + + bool mAtLimit = false; + + rclcpp::Node::SharedPtr mNode; + + public: + enum class ServoProperty { + PositionPGain = 84, + PositionIGain = 82, + PositionDGain = 80, + VelocityPGain = 78, + VelocityIGain = 76, + CurrentLimit = 102, + ProfileVelocity = 112, + ProfileAcceleration = 108, + }; + + enum class ServoMode { + Optimal, + Clockwise, + CounterClockwise, + Limited, + }; + + Servo(rclcpp::Node::SharedPtr node, std::string servoName) : mServoName{std::move(servoName)}, mLimitAdjustment{0}, mAdjustedForwardLimit{0}, + mAdjustedReverseLimit{0}, mGoalPosition{0}, mPositionOffsetTicks{0}, mPositionMultiplier{1}, mNode{std::move(node)} { + + int id; + std::vector parameters = { + {std::format("{}.id", mServoName), id, 0}, + }; + ParameterWrapper::declareParameters(mNode.get(), parameters); + mServoID = static_cast(id); + + U2D2::getInstance()->registerServo(mServoID); + updateConfigFromParameters(); + + uint8_t hardwareStatus; + + // Use Position Control Mode + U2D2::getInstance()->write1Byte(ADDR_OPERATING_MODE, 4, mServoID, &hardwareStatus); + + // Enable torque + U2D2::getInstance()->write1Byte(ADDR_TORQUE_ENABLE, 1, mServoID, &hardwareStatus); + } + + + auto setPosition(ServoPosition const position, ServoMode const mode) -> U2D2::Status { + // Convert degrees to ticks (0.0 - 360.0) to (0 to SERVO_TICKS) + mGoalPosition = static_cast((position / TAU) * static_cast(SERVO_TICKS)); + + + auto currentPositionAndStatus = getCurrentServoPosition(); + + if (currentPositionAndStatus.second != U2D2::Status::Success) { + return currentPositionAndStatus.second; + } + + // Calculate the signed difference (accounting for overflow) + auto normalizedDifference = static_cast(mGoalPosition - currentPositionAndStatus.first); + + mAtLimit = false; + + switch (mode) { + case ServoMode::Optimal: + if (normalizedDifference > (SERVO_TICKS / 2)) { + mGoalPosition -= SERVO_TICKS; // Go the other (shorter) way around + } else if (normalizedDifference < -(SERVO_TICKS / 2)) { + mGoalPosition += SERVO_TICKS; // Go the other (shorter) way around + } + break; + case ServoMode::Clockwise: // clockwise + if (normalizedDifference < 0) { + mGoalPosition += SERVO_TICKS; + } + break; + case ServoMode::CounterClockwise: // counter clockwise + if (normalizedDifference > 0) { + mGoalPosition -= SERVO_TICKS; + } + break; + case ServoMode::Limited: { + + // Adjust target and current position + int64_t const adjustedCurrentPosition = (currentPositionAndStatus.first - mLimitAdjustment + SERVO_TICKS) % SERVO_TICKS; + int64_t const adjustedTargetPosition = (mGoalPosition - mLimitAdjustment + SERVO_TICKS) % SERVO_TICKS; + + // If the current path to the final position goes over the middle limit, go the other way + if (0 > adjustedCurrentPosition && 0 < adjustedTargetPosition) { + + if (normalizedDifference > 0) + mGoalPosition -= SERVO_TICKS; + else if (normalizedDifference < 0) + mGoalPosition += SERVO_TICKS; + } + + mAtLimit = false; + + // Limit destination if between mForwardLimit and middleLimit + if (getUpper(adjustedTargetPosition) > mAdjustedForwardLimit) { + mGoalPosition = (mAdjustedForwardLimit - adjustedCurrentPosition) % SERVO_TICKS; + if (normalizedDifference < 0 && !(getUpper(adjustedCurrentPosition) > mAdjustedForwardLimit && adjustedCurrentPosition < SERVO_TICKS)) mGoalPosition += SERVO_TICKS; + mAtLimit = true; + } + + // Limit destination if between mReverseLimit and middleLimit + else if (adjustedTargetPosition < getUpper(mAdjustedReverseLimit)) { + + mGoalPosition = (mAdjustedReverseLimit - adjustedCurrentPosition) % SERVO_TICKS; + if (normalizedDifference > 0 && !(getUpper(adjustedCurrentPosition) > 0 && adjustedCurrentPosition < getUpper(mAdjustedReverseLimit))) mGoalPosition -= SERVO_TICKS; + mAtLimit = true; + } + } + } + + // Write goal position + uint8_t hardwareStatus; + uint32_t rawGoal = offsetToRaw(mGoalPosition); + return U2D2::getInstance()->write4Byte(ADDR_GOAL_POSITION, rawGoal, mServoID, &hardwareStatus); + } + + + auto getPosition(ServoPosition& position) const -> U2D2::Status { + auto const positionTicks = getCurrentServoPosition(); + position = (static_cast(positionTicks.first) / static_cast(SERVO_TICKS)) * TAU; + return positionTicks.second; + } + + auto getVelocity(ServoVelocity& velocity) const -> U2D2::Status { + uint8_t hardwareStatus; + uint32_t velocity_int; + U2D2::Status const status = U2D2::getInstance()->read4Byte(ADDR_PRESENT_VELOCITY, velocity_int, mServoID, &hardwareStatus); + velocity = (static_cast(velocity_int) * 0.22888); // 0.22888f Conversion factor to get rot/sec (found in dynamixel wizard) + return status; + } + + auto getCurrent(ServoCurrent& current) const -> U2D2::Status { + uint8_t hardwareStatus; + uint16_t currentInt; + U2D2::Status const status = U2D2::getInstance()->read2Byte(ADDR_PRESENT_CURRENT, currentInt, mServoID, &hardwareStatus); + current = static_cast(currentInt) / 1000.0; + return status; + } + + [[nodiscard]] auto setProperty(ServoProperty prop, uint16_t const value) const -> U2D2::Status { + uint8_t hardwareStatus; + if (prop == ServoProperty::ProfileVelocity || prop == ServoProperty::ProfileAcceleration) { + return U2D2::getInstance()->write4Byte(static_cast(prop), value, mServoID, &hardwareStatus); + } + return U2D2::getInstance()->write2Byte(static_cast(prop), value, mServoID, &hardwareStatus); + } + + [[nodiscard]] auto getTargetStatus() const -> U2D2::Status { + auto const currentPositionAndStatus = getCurrentServoPosition(); + + if (currentPositionAndStatus.second != U2D2::Status::Success) return currentPositionAndStatus.second; + + if (std::abs(currentPositionAndStatus.first - mGoalPosition) < SERVO_POSITION_DEAD_ZONE) { + return U2D2::Status::Success; + } + + return U2D2::Status::Active; + } + + [[nodiscard]] auto getLimitStatus() const -> bool { + return mAtLimit; + } + + private: + auto check(bool const condition, std::string const& errMsg) const -> void { + if (!condition) RCLCPP_ERROR(mNode->get_logger(), "%s", errMsg.c_str()); + } + + auto updateConfigFromParameters() -> void { + double forwardLimit; + double reverseLimit; + double positionPGain; + double positionIGain; + double positionDGain; + double velocityPGain; + double velocityIGain; + double currentLimit; + double profileAcceleration; + double profileVelocity; + + std::vector parameters = { + {std::format("{}.position_multiplier", mServoName), mPositionMultiplier, 1.0}, + {std::format("{}.reverse_limit", mServoName), reverseLimit, 0.0}, + {std::format("{}.forward_limit", mServoName), forwardLimit, 340.0}, + {std::format("{}.position_p", mServoName), positionPGain, 400.0}, + {std::format("{}.position_i", mServoName), positionIGain, 0.0}, + {std::format("{}.position_d", mServoName), positionDGain, 0.0}, + {std::format("{}.velocity_p", mServoName), velocityPGain, 180.0}, + {std::format("{}.velocity_i", mServoName), velocityIGain, 90.0}, + {std::format("{}.current_limit", mServoName), currentLimit, 1750.0}, + {std::format("{}.profile_acceleration", mServoName), profileAcceleration, 100.0}, + {std::format("{}.profile_velocity", mServoName), profileVelocity, 100.0}}; + + ParameterWrapper::declareParameters(mNode.get(), parameters); + + check(setProperty(ServoProperty::PositionPGain, static_cast(positionPGain)) == U2D2::Status::Success, "pos p gain error"); + check(setProperty(ServoProperty::PositionIGain, static_cast(positionIGain)) == U2D2::Status::Success, "pos i gain error"); + check(setProperty(ServoProperty::PositionDGain, static_cast(positionDGain)) == U2D2::Status::Success, "pos d gain error"); + check(setProperty(ServoProperty::VelocityPGain, static_cast(velocityPGain)) == U2D2::Status::Success, "vel p gain error"); + check(setProperty(ServoProperty::VelocityIGain, static_cast(velocityIGain)) == U2D2::Status::Success, "vel i gain error"); + check(setProperty(ServoProperty::CurrentLimit, static_cast(currentLimit)) == U2D2::Status::Success, "current limit gain error"); + check(setProperty(ServoProperty::ProfileAcceleration, static_cast(profileAcceleration)) == U2D2::Status::Success, "profile accel gain error"); + check(setProperty(ServoProperty::ProfileVelocity, static_cast(profileVelocity)) == U2D2::Status::Success, "profile vel gain error"); + + ParameterWrapper::declareParameters(mNode.get(), parameters); + + int const reverseLimitTicks = static_cast((reverseLimit / TAU) * SERVO_TICKS); + int const forwardLimitTicks = static_cast((forwardLimit / TAU) * SERVO_TICKS); + + mLimitAdjustment = (forwardLimitTicks + reverseLimitTicks) / 2; + + if (forwardLimitTicks > reverseLimitTicks) { + mLimitAdjustment = (forwardLimitTicks + reverseLimitTicks + SERVO_TICKS) / 2; + } + mLimitAdjustment %= SERVO_TICKS; + + mAdjustedReverseLimit = (static_cast((reverseLimit / TAU) * SERVO_TICKS) - mLimitAdjustment + SERVO_TICKS) % SERVO_TICKS; + mAdjustedForwardLimit = (static_cast((forwardLimit / TAU) * static_cast(SERVO_TICKS)) - mLimitAdjustment + SERVO_TICKS) % SERVO_TICKS; + + setOffset(); + } + + auto setOffset() -> void { + // Read the servos current position + uint8_t hardwareStatus; + uint32_t presentPosition; + U2D2::getInstance()->read4Byte(ADDR_PRESENT_POSITION, presentPosition, mServoID, &hardwareStatus); + + // Update the offset of the servo + mPositionOffsetTicks = presentPosition; + } + + [[nodiscard]] auto rawToOffset(uint32_t position) const -> int64_t { + // apply the offset to the position of the servos + int64_t updatedPosition = static_cast(position / mPositionMultiplier) - mPositionOffsetTicks; + + // correct for an underflow + while (updatedPosition < 0) { + updatedPosition += SERVO_TICKS; + } + + // correct for an overflow + while (updatedPosition > std::numeric_limits::max()) { + updatedPosition -= SERVO_TICKS; + } + + return updatedPosition; + } + + [[nodiscard]] auto offsetToRaw(int64_t position) const -> uint32_t { + // apply the offset to the position of the servos + int64_t updatedPosition = static_cast(static_cast(position) * mPositionMultiplier) + mPositionOffsetTicks; + + // correct for an underflow + while (updatedPosition < 0) { + updatedPosition += SERVO_TICKS; + } + + // correct for an overflow + while (updatedPosition > std::numeric_limits::max()) { + updatedPosition -= SERVO_TICKS; + } + + return updatedPosition; + } + + [[nodiscard]] auto getCurrentServoPosition() const -> std::pair { + uint8_t hardwareStatus; + uint32_t presentPosition; + U2D2::Status const status = U2D2::getInstance()->read4Byte(ADDR_PRESENT_POSITION, presentPosition, mServoID, &hardwareStatus); + + int64_t offsetPosition = rawToOffset(presentPosition); + + + return std::make_pair(offsetPosition, status); + } + }; +} // namespace mrover diff --git a/esw/servo/u2d2.hpp b/esw/servo/u2d2.hpp new file mode 100644 index 00000000..f6eb533a --- /dev/null +++ b/esw/servo/u2d2.hpp @@ -0,0 +1,144 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace mrover { + + static constexpr float PROTOCOL_VERSION = 2.0; + static constexpr uint32_t SERVO_BAUDRATE = 57600; + + class U2D2 { + U2D2() : mPortHandler{nullptr}, mPacketHandler{nullptr}, mInitialized{false} {} + + dynamixel::PortHandler* mPortHandler; + dynamixel::PacketHandler* mPacketHandler; + std::unordered_set mServos; + mutable std::mutex mBusMutex; + bool mInitialized; + + public: + enum class Status : int32_t { + Active = 400, + HardwareFailure = 401, + Success = 0, + FailedToOpenPort = 1, + FailedToSetBaud = 2, + CommPortBusy = -1000, + CommTxFail = -1001, + CommRxFail = -1002, + CommTxError = -2000, + CommRxWaiting = -3000, + CommRxTimeout = -3001, + CommRxCorrupt = -3002, + CommNotAvailable = -9000, + }; + + + // delete copy ctor and assignment operator + U2D2(U2D2 const&) = delete; + auto operator=(U2D2 const&) -> U2D2& = delete; + + static auto getInstance() -> U2D2* { + static U2D2 inst; + return &inst; + } + + static auto getSharedInstance() -> std::shared_ptr { + static std::shared_ptr inst{getInstance(), [](U2D2*) {}}; + return inst; + } + + auto init(std::string const& deviceName) -> Status { + if (mInitialized) return Status::Success; + + mPortHandler = dynamixel::PortHandler::getPortHandler(deviceName.c_str()); + mPacketHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION); + + if (!mPortHandler->openPort()) return Status::FailedToOpenPort; + if (!mPortHandler->setBaudRate(SERVO_BAUDRATE)) return Status::FailedToSetBaud; + + mInitialized = true; + return Status::Success; + } + + auto registerServo(uint8_t const id) -> void { + if (mServos.contains(id)) { + RCLCPP_FATAL(rclcpp::get_logger("u2d2"), "duplicate servo id registered"); + rclcpp::shutdown(); + } + mServos.emplace(id); + } + + auto write1Byte(int const addr, uint8_t const data, uint8_t const id, uint8_t* hardwareStatus) const -> Status { + std::lock_guard lock(mBusMutex); + return static_cast(mPacketHandler->write1ByteTxRx(mPortHandler, id, addr, data, hardwareStatus)); + } + + auto write2Byte(int const addr, uint16_t const data, uint8_t const id, uint8_t* hardwareStatus) const -> Status { + std::lock_guard lock(mBusMutex); + return static_cast(mPacketHandler->write2ByteTxRx(mPortHandler, id, addr, data, hardwareStatus)); + } + + auto write4Byte(int const addr, uint32_t const data, uint8_t const id, uint8_t* hardwareStatus) const -> Status { + std::lock_guard lock(mBusMutex); + return static_cast(mPacketHandler->write4ByteTxRx(mPortHandler, id, addr, data, hardwareStatus)); + } + + auto read1Byte(int const addr, uint8_t& data, uint8_t const id, uint8_t* hardwareStatus) const -> Status { + std::lock_guard lock(mBusMutex); + return static_cast(mPacketHandler->read1ByteTxRx(mPortHandler, id, addr, &data, hardwareStatus)); + } + + auto read2Byte(int const addr, uint16_t& data, uint8_t const id, uint8_t* hardwareStatus) const -> Status { + std::lock_guard lock(mBusMutex); + return static_cast(mPacketHandler->read2ByteTxRx(mPortHandler, id, addr, &data, hardwareStatus)); + } + + auto read4Byte(int const addr, uint32_t& data, uint8_t const id, uint8_t* hardwareStatus) const -> Status { + std::lock_guard lock(mBusMutex); + return static_cast(mPacketHandler->read4ByteTxRx(mPortHandler, id, addr, &data, hardwareStatus)); + } + + static auto stringifyStatus(Status const status) -> std::string { + switch (status) { + case Status::Active: + return "Active"; + case Status::HardwareFailure: + return "HardwareFailure"; + case Status::Success: + return "Success"; + case Status::FailedToOpenPort: + return "FailedToOpenPort"; + case Status::FailedToSetBaud: + return "FailedToSetBaud"; + case Status::CommPortBusy: + return "CommPortBusy"; + case Status::CommTxFail: + return "CommTxFail"; + case Status::CommRxFail: + return "CommRxFail"; + case Status::CommTxError: + return "CommTxError"; + case Status::CommRxWaiting: + return "CommRxWaiting"; + case Status::CommRxTimeout: + return "CommRxTimeout"; + case Status::CommRxCorrupt: + return "CommRxCorrupt"; + case Status::CommNotAvailable: + return "CommNotAvailable"; + default: + return "UnknownStatus"; + } + } + }; + +} // namespace mrover diff --git a/esw/simple_bridge.cpp b/esw/simple_bridge.cpp deleted file mode 100644 index 913fc8bc..00000000 --- a/esw/simple_bridge.cpp +++ /dev/null @@ -1,19 +0,0 @@ -#include -#include - -#include - -auto main(int argc, char** argv) -> int { - ros::init(argc, argv, "simple_bridge"); - ros::NodeHandle nh; - ros::NodeHandle pnh{"~"}; - - auto groupName = pnh.param("group_name", {}); - if (groupName.empty()) throw std::runtime_error{"Group name is reuqired for a simple bridge!"}; - - mrover::MotorsGroup group{nh, groupName}; - - ros::spin(); - - return EXIT_SUCCESS; -} \ No newline at end of file diff --git a/gst_utils/gst_utils.hpp b/gst_utils/gst_utils.hpp index 2eea4728..7b95420a 100644 --- a/gst_utils/gst_utils.hpp +++ b/gst_utils/gst_utils.hpp @@ -219,14 +219,14 @@ namespace mrover::gst { return "video/x-raw"; } - // TODO:(owen) I don't like the config-interval=1 for H265. Should be configurable somehow -#define CODEC_ITER(_F) \ - _F(H264, "video/x-h264", "x264enc", "avdec_h264", "rtph264pay", "rtph264depay") \ - _F(H265, "video/x-h265", "x265enc", "avdec_h265", "rtph265pay config-interval=1", "rtph265depay") \ - _F(VP8, "video/x-vp8", "vp8enc", "vp8dec", "rtpvp8pay", "rtpvp8depay") \ - _F(VP9, "video/x-vp9", "vp9enc", "vp9dec", "rtpvp9pay", "rtpvp9depay") \ - _F(MPEG4, "video/mpeg4", "avenc_mpeg4", "avdec_mpeg4", "rtpmp4vpay", "rtpmp4vdepay") \ - _F(JPEG, "image/jpeg", "jpegenc", "jpegdec", "rtpjpegpay", "rtpjpegdepay") +#define CODEC_ITER(_F) \ + _F(H264, "video/x-h264", "x264enc", "avdec_h264", "rtph264pay", "rtph264depay") \ + _F(H265, "video/x-h265", "x265enc", "avdec_h265", "rtph265pay", "rtph265depay") \ + _F(VP8, "video/x-vp8", "vp8enc", "vp8dec", "rtpvp8pay", "rtpvp8depay") \ + _F(VP9, "video/x-vp9", "vp9enc", "vp9dec", "rtpvp9pay", "rtpvp9depay") \ + _F(MPEG4, "video/mpeg4", "avenc_mpeg4", "avdec_mpeg4", "rtpmp4vpay", "rtpmp4vdepay") \ + _F(JPEG, "image/jpeg", "jpegenc", "jpegdec", "rtpjpegpay", "rtpjpegdepay") \ + _F(AV1, "video/x-av1", "av1enc", "av1dec", "rtpav1pay", "rtpav1depay") enum class Codec : unsigned int { #define F(name, ...) name, @@ -335,22 +335,6 @@ namespace mrover::gst { return createDefaultDecoder(getCodecFromStringView(codec), std::forward(extraProps)...); } - inline auto createRtpSink(std::string const& host, std::uint16_t port, video::Codec codec) { - return std::format("{} ! queue ! udpsink host={} port={}", getRtpPayloader(codec), host, port); - } - - constexpr auto DEFAULT_RTP_JITTER = std::chrono::milliseconds(500); - - inline auto createRtpToRawSrc(std::uint16_t port, Codec codec, std::chrono::milliseconds rtpJitter = DEFAULT_RTP_JITTER) -> std::string { - std::string parser; - if (codec == Codec::H265) { - parser = "! h265parse"; - } else if (codec == Codec::H264) { - parser = "! h264parse"; - } - - return std::format("udpsrc port={} ! application/x-rtp,media=video ! rtpjitterbuffer latency={} ! {} {} ! decodebin", port, rtpJitter.count(), getRtpDepayloader(codec), parser); - } namespace v4l2 { diff --git a/launch/camera_client.launch.py b/launch/camera_client.launch.py index b56e0e71..bbf73dfe 100644 --- a/launch/camera_client.launch.py +++ b/launch/camera_client.launch.py @@ -3,18 +3,48 @@ from ament_index_python import get_package_share_directory from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import LaunchConfigurationEquals from launch_ros.actions import Node def generate_launch_description(): + cameras_yaml = Path(get_package_share_directory("mrover"), "config", "cameras.yaml") + + mode_arg = DeclareLaunchArgument("mode", default_value="real", description="Launch mode: real or sim") camera_client_node = Node( package="mrover", executable="camera_client", name="camera_client", - parameters=[ - Path(get_package_share_directory("mrover"), "config", "cameras.yaml"), - ], + parameters=[cameras_yaml], + ) + + # In sim mode, launch gst_camera_server nodes for cameras that use image_topic. + # Override address to 127.0.0.1 so streams go to localhost. + long_range_streamer_node = Node( + package="mrover", + executable="gst_camera_server", + name="long_range_streamer", + output="screen", + parameters=[cameras_yaml, {"long_range_cam.address": "127.0.0.1"}], + condition=LaunchConfigurationEquals("mode", "sim"), ) - return LaunchDescription([camera_client_node]) + zed_streamer_node = Node( + package="mrover", + executable="gst_camera_server", + name="zed_streamer", + output="screen", + parameters=[cameras_yaml, {"zed.address": "127.0.0.1"}], + condition=LaunchConfigurationEquals("mode", "sim"), + ) + + return LaunchDescription( + [ + mode_arg, + camera_client_node, + long_range_streamer_node, + zed_streamer_node, + ] + ) diff --git a/launch/jetson_base.launch.py b/launch/jetson_base.launch.py index 9d4057c9..348480c7 100644 --- a/launch/jetson_base.launch.py +++ b/launch/jetson_base.launch.py @@ -85,6 +85,16 @@ def generate_launch_description(): output="screen", ) + mast_gimbal_hw_bridge = Node( + package="mrover", + executable="mast_gimbal_hw_bridge", + name="mast_gimbal_hw_bridge", + output="screen", + parameters=[ + Path(get_package_share_directory("mrover"), "config", "mast_gimbal.yaml"), + ], + ) + return LaunchDescription( [ launch_include_can, diff --git a/launch/jetson_science.launch.py b/launch/jetson_science.launch.py index db71caf6..8be890df 100644 --- a/launch/jetson_science.launch.py +++ b/launch/jetson_science.launch.py @@ -24,6 +24,7 @@ def generate_launch_description(): name="science_hw_bridge", parameters=[ Path(get_package_share_directory("mrover"), "config", "esw.yaml"), + Path(get_package_share_directory("mrover"), "config", "science.yaml"), ], ) diff --git a/localization/heading_filter/heading_filter.cpp b/localization/heading_filter/heading_filter.cpp index a2a304ea..6f2206ac 100644 --- a/localization/heading_filter/heading_filter.cpp +++ b/localization/heading_filter/heading_filter.cpp @@ -4,29 +4,55 @@ namespace mrover { HeadingFilter::HeadingFilter() : Node("heading_filter") { - declare_parameter("world_frame", rclcpp::ParameterType::PARAMETER_STRING); - declare_parameter("gps_frame", rclcpp::ParameterType::PARAMETER_STRING); - declare_parameter("imu_watchdog_timeout", rclcpp::ParameterType::PARAMETER_DOUBLE); - declare_parameter("mag_heading_noise", rclcpp::ParameterType::PARAMETER_DOUBLE); - declare_parameter("rtk_heading_noise", rclcpp::ParameterType::PARAMETER_DOUBLE); - declare_parameter("process_noise", rclcpp::ParameterType::PARAMETER_DOUBLE); + std::vector params{ + {"world_frame", world_frame, std::string("map")}, + {"gps_frame", gps_frame, std::string("gps_frame")}, + {"imu_watchdog_timeout", imu_timeout, 1.0}, + {"mag_heading_noise", mag_noise, 10.0}, + {"rtk_heading_noise", rtk_noise, 0.01}, + {"drive_forward_heading_noise", drive_noise, 0.1}, + {"rover_heading_change_threshold", heading_delta_threshold, 0.05}, + {"minimum_linear_speed", min_speed, 0.4}, + {"process_noise", process_noise, 0.000001}, + {"use_mag", use_mag, false} + }; + + ParameterWrapper::declareParameters(this, params); // subscribers linearized_position_sub = this->create_subscription("/linearized_position", 1, [&](const geometry_msgs::msg::Vector3Stamped::ConstSharedPtr &position) { last_position = *position; + position_window.push_back(*position); + while (position_window.size() > DRIVE_FORWARD_CAP) { + position_window.pop_front(); + } }); + rtk_heading_sub.subscribe(this, "/heading/fix"); rtk_heading_status_sub.subscribe(this, "/heading_fix_status"); imu_sub.subscribe(this, "/zed_imu/data_raw"); mag_heading_sub.subscribe(this, "/zed_imu/mag_heading"); + cmd_vel_sub = this->create_subscription("/cmd_vel", 10, [&](const geometry_msgs::msg::Twist::ConstSharedPtr &twist_msg) { + twists.push_back(*twist_msg); + if (twists.size() > TWISTS_CAP) { + twists.erase(twists.begin(), twists.begin() + (twists.size() - TWISTS_CAP)); + } + }); + // imu data watchdog - const rclcpp::Duration IMU_AND_MAG_WATCHDOG_TIMEOUT = rclcpp::Duration::from_seconds(get_parameter("imu_watchdog_timeout").as_double()); + const rclcpp::Duration IMU_AND_MAG_WATCHDOG_TIMEOUT = rclcpp::Duration::from_seconds(imu_timeout); imu_and_mag_watchdog = this->create_wall_timer(IMU_AND_MAG_WATCHDOG_TIMEOUT.to_chrono(), [&]() { RCLCPP_WARN(get_logger(), "ZED IMU data watchdog expired"); last_imu.reset(); }); + // drive forward correction timer + drive_forward_timer = this->create_wall_timer( + std::chrono::milliseconds(static_cast(DRIVE_FORWARD_TIMER_S * 1000.0)), + [this]() -> void { drive_forward_callback(); } + ); + // synchronizers uint32_t queue_size = 10; @@ -105,10 +131,15 @@ namespace mrover { else if (measured_heading_deg > 180.) { measured_heading_deg -= 360.; } double const measured_heading = measured_heading_deg * (M_PI / 180.); + auto const previousX = X; + double heading_correction_delta = measured_heading - uncorrected_heading; heading_correction_delta = fmod((heading_correction_delta + 3 * M_PI), 2 * M_PI) - M_PI; - predict(get_parameter("process_noise").as_double()); - correct(heading_correction_delta, get_parameter("rtk_heading_noise").as_double()); + predict(process_noise); + correct(heading_correction_delta, rtk_noise); + + auto const correctionDelta = (X - previousX); + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000, "%s", std::format("RTK heading correction delta on X: {} rad", correctionDelta).c_str()); } @@ -116,7 +147,6 @@ namespace mrover { void HeadingFilter::sync_imu_and_mag_callback(const sensor_msgs::msg::Imu::ConstSharedPtr &imu, const mrover::msg::Heading::ConstSharedPtr &mag_heading) { imu_and_mag_watchdog.reset(); - last_imu = *imu; if (!last_position) { @@ -140,38 +170,47 @@ namespace mrover { } uncorrected_orientation.normalize(); SO3d uncorrected_orientation_rotm = uncorrected_orientation; - R2d uncorrected_forward = uncorrected_orientation_rotm.rotation().col(0).head(2); - if (!uncorrected_forward.array().isFinite().all()) { - RCLCPP_WARN(get_logger(), "Forward Vector not finite, skipping heading correction"); - return; - } - double uncorrected_heading = std::atan2(uncorrected_forward.y(), uncorrected_forward.x()); - if (!std::isfinite(uncorrected_heading)) { - RCLCPP_WARN(get_logger(), "Computed heading is not finite, skipping heading correction"); - return; - } - double measured_heading_deg = 90. - mag_heading->heading; - if (measured_heading_deg <= -180.) { measured_heading_deg += 360.; } - else if (measured_heading_deg > 180.) { measured_heading_deg -= 360.; } - double const measured_heading = measured_heading_deg * (M_PI / 180.); + if (use_mag) { + R2d uncorrected_forward = uncorrected_orientation_rotm.rotation().col(0).head(2); + if (!uncorrected_forward.array().isFinite().all()) { + RCLCPP_WARN(get_logger(), "Forward Vector not finite, skipping heading correction"); + return; + } + double uncorrected_heading = std::atan2(uncorrected_forward.y(), uncorrected_forward.x()); + if (!std::isfinite(uncorrected_heading)) { + RCLCPP_WARN(get_logger(), "Computed heading is not finite, skipping heading correction"); + return; + } + + double measured_heading_deg = 90. - mag_heading->heading; + if (measured_heading_deg <= -180.) { measured_heading_deg += 360.; } + else if (measured_heading_deg > 180.) { measured_heading_deg -= 360.; } + double const measured_heading = measured_heading_deg * (M_PI / 180.); - double heading_correction_delta = measured_heading - uncorrected_heading; - heading_correction_delta = fmod((heading_correction_delta + 3 * M_PI), 2 * M_PI) - M_PI; + double heading_correction_delta = measured_heading - uncorrected_heading; + heading_correction_delta = fmod((heading_correction_delta + 3 * M_PI), 2 * M_PI) - M_PI; + + auto const previousX = X; - auto previousX = X; + predict(process_noise); + correct(heading_correction_delta, mag_noise); - predict(get_parameter("process_noise").as_double()); - correct(heading_correction_delta, get_parameter("mag_heading_noise").as_double()); + if (!std::isfinite(X)) { + RCLCPP_WARN(get_logger(), "Kalman state X is not finite, skipping TF publish"); + return; + } + + auto const correctionDelta = (X - previousX); + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000, "%s", std::format("Mag heading correction delta on X: {} rad", correctionDelta).c_str()); + } if (!std::isfinite(X)) { RCLCPP_WARN(get_logger(), "Kalman state X is not finite, skipping TF publish"); return; } - - auto correctionDelta = (X - previousX); + SO3d curr_heading_correction = Eigen::AngleAxisd(X, R3d::UnitZ()); - RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000, "%s", std::format("Heading corrected by: {} rad", correctionDelta).c_str()); SO3d corrected_orientation = curr_heading_correction * uncorrected_orientation_rotm; Eigen::Quaterniond q = corrected_orientation.quat(); q.normalize(); @@ -181,7 +220,129 @@ namespace mrover { } pose_in_map.asSO3() = SO3d(q); - SE3Conversions::pushToTfTree(tf_broadcaster, get_parameter("gps_frame").as_string(), get_parameter("world_frame").as_string(), pose_in_map, get_clock()->now()); + SE3Conversions::pushToTfTree(tf_broadcaster, gps_frame, world_frame, pose_in_map, get_clock()->now()); + } + + void HeadingFilter::drive_forward_callback() { + + if (!last_imu) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "No IMU data for drive-forward correction"); + return; + } + + double const min_linear_speed = min_speed; + + // Gate on "commanded forward" + bool const had_cmd_vel_samples = !twists.empty(); + double mean_cmd_vel = 0.0; + if (had_cmd_vel_samples) { + for (auto const& twist : twists) { + mean_cmd_vel += twist.linear.x / static_cast(twists.size()); + } + } + twists.clear(); + if (mean_cmd_vel < min_linear_speed) { + RCLCPP_WARN_THROTTLE( + get_logger(), + *get_clock(), + 1000, + "Drive-forward skipped: mean cmd_vel.linear.x %.3f m/s below minimum_linear_speed %.3f m/s%s", + mean_cmd_vel, + min_linear_speed, + had_cmd_vel_samples ? "" : " (no cmd_vel samples in buffer)"); + return; + } + + if (position_window.size() < 2) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Insufficient position history for drive-forward correction"); + return; + } + + // Estimate mean velocity and ensure near-straight motion over the window. + R2d v_sum = R2d::Zero(); + double heading_change_accum = 0.0; + std::size_t readings = 0; + + auto wrapped_delta = [](double a) -> double { + return std::fmod(a + 3 * M_PI, 2 * M_PI) - M_PI; + }; + + auto prev_heading_opt = std::optional{}; + rcl_clock_type_t const clock_type = get_clock()->get_clock_type(); + for (std::size_t i = 1; i < position_window.size(); ++i) { + const auto& p0 = position_window[i - 1]; + const auto& p1 = position_window[i]; + + const rclcpp::Time t0(p0.header.stamp, clock_type); + const rclcpp::Time t1(p1.header.stamp, clock_type); + const double dt = (t1 - t0).seconds(); + if (!(dt > 1e-3) || !std::isfinite(dt)) continue; + + const R2d dp(p1.vector.x - p0.vector.x, p1.vector.y - p0.vector.y); + const R2d v = dp / dt; + if (!v.array().isFinite().all()) continue; + + v_sum += v; + const double h = std::atan2(v.y(), v.x()); + if (prev_heading_opt) heading_change_accum += std::fabs(wrapped_delta(h - *prev_heading_opt)); + prev_heading_opt = h; + ++readings; + } + + if (readings < 1) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Not enough valid velocity readings for drive-forward correction"); + return; + } + + const R2d mean_v = v_sum / static_cast(readings); + double const speed = mean_v.norm(); + if (speed < min_linear_speed) { + RCLCPP_WARN_THROTTLE( + get_logger(), + *get_clock(), + 1000, + "Drive-forward skipped: estimated planar speed %.3f m/s below minimum_linear_speed %.3f m/s", + speed, + min_linear_speed); + return; + } + + if (heading_change_accum > heading_delta_threshold) { + return; + } + + const double drive_forward_heading = std::atan2(mean_v.y(), mean_v.x()); + + // Compare against current IMU-derived heading and do a Kalman correct on delta. + auto const& qmsg = last_imu->orientation; + if (!std::isfinite(qmsg.w) || !std::isfinite(qmsg.x) || !std::isfinite(qmsg.y) || !std::isfinite(qmsg.z)) { + return; + } + Eigen::Quaterniond uncorrected_orientation(qmsg.w, qmsg.x, qmsg.y, qmsg.z); + const double norm2 = uncorrected_orientation.squaredNorm(); + if (!std::isfinite(norm2) || norm2 < 1e-12) { + return; + } + uncorrected_orientation.normalize(); + const R2d uncorrected_forward = uncorrected_orientation.toRotationMatrix().col(0).head(2); + if (!uncorrected_forward.array().isFinite().all()) { + return; + } + const double uncorrected_heading = std::atan2(uncorrected_forward.y(), uncorrected_forward.x()); + if (!std::isfinite(uncorrected_heading)) { + return; + } + + auto const previousX = X; + + double heading_correction_delta = drive_forward_heading - uncorrected_heading; + heading_correction_delta = wrapped_delta(heading_correction_delta); + + predict(process_noise); + correct(heading_correction_delta, drive_noise); + + auto const correctionDelta = (X - previousX); + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000, "%s", std::format("Drive forward correction delta on X: {} rad", correctionDelta).c_str()); } } @@ -194,4 +355,4 @@ int main(int argc, char**argv) { rclcpp::shutdown(); return 0; -} \ No newline at end of file +} diff --git a/localization/heading_filter/heading_filter.hpp b/localization/heading_filter/heading_filter.hpp index 379b08c5..afc135de 100644 --- a/localization/heading_filter/heading_filter.hpp +++ b/localization/heading_filter/heading_filter.hpp @@ -14,6 +14,7 @@ namespace mrover { // callbacks void sync_rtk_heading_callback(const mrover::msg::Heading::ConstSharedPtr &heading, const mrover::msg::FixStatus::ConstSharedPtr &heading_status); void sync_imu_and_mag_callback(const sensor_msgs::msg::Imu::ConstSharedPtr &imu, const mrover::msg::Heading::ConstSharedPtr &mag_heading); + void drive_forward_callback(); // subscribers and publishers rclcpp::Subscription::SharedPtr linearized_position_sub; @@ -21,6 +22,12 @@ namespace mrover { message_filters::Subscriber mag_heading_sub; message_filters::Subscriber rtk_heading_sub; message_filters::Subscriber rtk_heading_status_sub; + rclcpp::Subscription::ConstSharedPtr cmd_vel_sub; + + // params + std::string world_frame, gps_frame; + double imu_timeout, mag_noise, rtk_noise, drive_noise, heading_delta_threshold, min_speed, process_noise; + bool use_mag; // transform broadcaster tf2_ros::Buffer tf_buffer{get_clock()}; @@ -35,6 +42,7 @@ namespace mrover { // imu data watchdog rclcpp::TimerBase::SharedPtr imu_and_mag_watchdog; + rclcpp::TimerBase::SharedPtr drive_forward_timer; // 1D Kalman Filter state double X; @@ -43,6 +51,11 @@ namespace mrover { // data store std::optional last_imu; std::optional last_position; + std::vector twists; + std::deque position_window; + static constexpr std::size_t DRIVE_FORWARD_CAP = 3; + static constexpr std::size_t TWISTS_CAP = 50; + static constexpr double DRIVE_FORWARD_TIMER_S = 1.25; public: diff --git a/localization/heading_filter/pch.hpp b/localization/heading_filter/pch.hpp index 4509ad3f..543cc6af 100644 --- a/localization/heading_filter/pch.hpp +++ b/localization/heading_filter/pch.hpp @@ -1,28 +1,41 @@ #pragma once +// 1. Standard C++ libraries #include #include #include #include +// 2. Third-party geometry/math libraries +#include +#include +#include + +// 3. ROS 2 core systems #include #include #include +#include +// 4. ROS 2 utilities +#include +#include #include #include #include +// 5. Standard ROS 2 message types +#include #include #include #include + +// 6. MRover-specific custom messages #include #include #include -#include -#include -#include -#include -#include -#include +// 7. MRover-specific local headers +#include + + diff --git a/navigation/arm_controller/arm_controller.cpp b/navigation/arm_controller/arm_controller.cpp index 6ea54ae0..b30cbb3b 100644 --- a/navigation/arm_controller/arm_controller.cpp +++ b/navigation/arm_controller/arm_controller.cpp @@ -1,8 +1,6 @@ #include "arm_controller.hpp" -#include - namespace mrover { - const rclcpp::Duration ArmController::TIMEOUT = rclcpp::Duration(0, 0.3 * 1e9); // 0.3 seconds + rclcpp::Duration const ArmController::TIMEOUT = rclcpp::Duration(0, 0.3 * 1e9); // 0.3 seconds ArmController::ArmController() : Node{"arm_controller"}, mLastUpdate{get_clock()->now() - TIMEOUT} { mPosPub = create_publisher("arm_pos_cmd", 10); @@ -27,6 +25,10 @@ namespace mrover { mModeServ = create_service("ik_mode", [this](srv::IkMode::Request::ConstSharedPtr const& req, srv::IkMode::Response::SharedPtr const& resp) { modeCallback(req, resp); }); + + mSampleServ = create_service("ik_sample", [this](srv::IkSample::Request::ConstSharedPtr const& req, srv::IkSample::Response::SharedPtr const& resp) { + ikSampleCallback(req, resp); + }); } auto ArmController::ikPosCalc(ArmPos target) -> std::optional { @@ -58,11 +60,11 @@ namespace mrover { msg::Position positions; positions.names = {"joint_a", "joint_b", "joint_c", "joint_de_pitch", "joint_de_roll"}; positions.positions = { - static_cast(y), - static_cast(q1), - static_cast(q2), - static_cast(q3), - static_cast(target.roll), + static_cast(y), + static_cast(q1), + static_cast(q2), + static_cast(q3), + static_cast(target.roll), }; for (size_t i = 0; i < positions.names.size(); ++i) { @@ -78,7 +80,7 @@ namespace mrover { return std::nullopt; } } - + return positions; } @@ -121,11 +123,11 @@ namespace mrover { msg::Velocity velocities; velocities.names = {"joint_a", "joint_b", "joint_c", "joint_de_pitch", "joint_de_roll"}; velocities.velocities = { - static_cast(vel.linear.y), - static_cast(joint_b_vel), - static_cast(joint_c_vel), - static_cast(joint_de_pitch_vel), - static_cast(vel.angular.x), + static_cast(vel.linear.y), + static_cast(joint_b_vel), + static_cast(joint_c_vel), + static_cast(joint_de_pitch_vel), + static_cast(vel.angular.x), }; double scaleFactor = 1; @@ -147,7 +149,7 @@ namespace mrover { // scale down all velocities so that we don't exceed motor velocity limits if (scaleFactor > 1) RCLCPP_INFO_STREAM_THROTTLE(get_logger(), *get_clock(), 500, "Commanded velocity too high. Scaling down by factor of " << scaleFactor); - for (auto& v : velocities.velocities) + for (auto& v: velocities.velocities) v = static_cast(v / scaleFactor); return velocities; @@ -201,26 +203,24 @@ namespace mrover { mPosTarget = *ik_target; SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_target", "arm_base_link", mPosTarget.toSE3(), get_clock()->now()); if (mArmMode == ArmMode::POSITION_CONTROL || mArmMode == ArmMode::TYPING) - mLastUpdate = get_clock()->now(); + mLastUpdate = get_clock()->now(); else - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 100, "Received position command in velocity mode!"); + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 100, "Received position command in velocity mode!"); } auto ArmController::timerCallback() -> void { msg::Position mCurrPos; mCurrPos.names = {"joint_a", "joint_b", "joint_c", "joint_de_pitch", "joint_de_roll"}; mCurrPos.positions = { - static_cast(joints["joint_a"].pos), - static_cast(joints["joint_b"].pos), - static_cast(joints["joint_c"].pos), - static_cast(joints["joint_de_pitch"].pos), - static_cast(joints["joint_de_roll"].pos), + static_cast(joints["joint_a"].pos), + static_cast(joints["joint_b"].pos), + static_cast(joints["joint_c"].pos), + static_cast(joints["joint_de_pitch"].pos), + static_cast(joints["joint_de_roll"].pos), }; if (get_clock()->now() - mLastUpdate > TIMEOUT) { RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 100, "IK Timed Out"); - if(!mPosFallback) mPosFallback = mCurrPos; - mPosPub->publish(mPosFallback.value()); return; } @@ -229,37 +229,29 @@ namespace mrover { SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_target", "arm_base_link", mPosTarget.toSE3(), get_clock()->now()); if (positions) { mPosPub->publish(positions.value()); - mPosFallback = std::nullopt; } else { RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Position IK failed!"); - if(!mPosFallback) mPosFallback = mCurrPos; - mPosPub->publish(mPosFallback.value()); } } else if (mArmMode == ArmMode::VELOCITY_CONTROL) { // TODO: Determine joint velocities that cancels out arm sag auto velocities = ikVelCalc(mVelTarget); if (velocities && !( - velocities->velocities[0] == 0 && - velocities->velocities[1] == 0 && - velocities->velocities[2] == 0 && - velocities->velocities[3] == 0 && - velocities->velocities[4] == 0 - ) - ) { + velocities->velocities[0] == 0 && + velocities->velocities[1] == 0 && + velocities->velocities[2] == 0 && + velocities->velocities[3] == 0 && + velocities->velocities[4] == 0)) { mVelPub->publish(velocities.value()); - mPosFallback = std::nullopt; } else { - if(!velocities) RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Velocity IK failed!"); - if(!mPosFallback) mPosFallback = mCurrPos; - mPosPub->publish(mPosFallback.value()); + if (!velocities) RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Velocity IK failed!"); } } else { // typing mode msg::Position positions; positions.names = {"joint_a", "gripper"}; positions.positions = { - static_cast(mPosTarget.y + mTypingOrigin.y), - static_cast(mPosTarget.z + mTypingOrigin.gripper), + static_cast(mPosTarget.y + mTypingOrigin.y), + static_cast(mPosTarget.z + mTypingOrigin.gripper), }; // bounds checking and such @@ -277,7 +269,6 @@ namespace mrover { return; } } - mPosFallback = std::nullopt; mPosPub->publish(positions); } } @@ -297,6 +288,23 @@ namespace mrover { } resp->success = true; } + + void ArmController::ikSampleCallback(srv::IkSample::Request::ConstSharedPtr const& req, srv::IkSample::Response::SharedPtr const& resp) { + ArmPos testPose; + testPose.x = req->pos.x; + testPose.y = req->pos.y; + testPose.z = req->pos.z; + testPose.pitch = req->pitch; + testPose.roll = req->roll; + RCLCPP_INFO(this->get_logger(), "Checking Position Validity: x:%f, y:%f, z:%f\n", req->pos.x, req->pos.y, req->pos.z); + auto positions = ikPosCalc(testPose); + if (positions == std::nullopt) + resp->valid = false; + else + resp->valid = true; + RCLCPP_INFO(this->get_logger(), "Response identified as: %s", resp->valid ? "valid" : "invalid"); + } + } // namespace mrover auto main(int argc, char** argv) -> int { diff --git a/navigation/arm_controller/arm_controller.hpp b/navigation/arm_controller/arm_controller.hpp index f891d470..01a11784 100644 --- a/navigation/arm_controller/arm_controller.hpp +++ b/navigation/arm_controller/arm_controller.hpp @@ -7,7 +7,12 @@ namespace mrover { struct ArmPos { double x{0}, y{0}, z{0}, pitch{0}, roll{0}, gripper{0}; [[nodiscard]] auto toSE3() const -> SE3d { - return SE3d{{x, y, z,}, SO3d{Eigen::Quaterniond{Eigen::AngleAxisd{pitch, R3d::UnitY()} * Eigen::AngleAxisd{roll, R3d::UnitX()}}}}; + return SE3d{{ + x, + y, + z, + }, + SO3d{Eigen::Quaterniond{Eigen::AngleAxisd{pitch, R3d::UnitY()} * Eigen::AngleAxisd{roll, R3d::UnitX()}}}}; } auto operator+(R3d offset) const -> ArmPos { @@ -28,42 +33,29 @@ namespace mrover { struct JointWrapper { struct JointLimits { double minPos, maxPos, minVel, maxVel; - [[nodiscard]] auto posInBounds(double pos) const -> bool {return minPos <= pos && pos <= maxPos;} - [[nodiscard]] auto velInBounds(double vel) const -> bool {return minPos <= vel && vel <= maxPos;} + [[nodiscard]] auto posInBounds(double pos) const -> bool { return minPos <= pos && pos <= maxPos; } + [[nodiscard]] auto velInBounds(double vel) const -> bool { return minPos <= vel && vel <= maxPos; } }; - + JointLimits limits; double pos; }; - + // TODO: update velocity limits to make them real std::unordered_map joints = { - {"joint_a", { - .limits = {.minPos = 0, .maxPos = 0.35, .minVel = -0.05, .maxVel = 0.05}, - .pos = 0 - }}, - {"joint_b", { - .limits = {.minPos = -0.9, .maxPos = 0, .minVel = -0.05, .maxVel = 0.05}, - .pos = 0 - }}, - {"joint_c", { - .limits = {.minPos = -0.959931, .maxPos = 2.87979, .minVel = -0.05 * 2 * std::numbers::pi, .maxVel = 0.05 * 2 * std::numbers::pi}, - .pos = 0 - }}, - {"joint_de_pitch", { - .limits = {.minPos = -1.3, .maxPos = 1.2, .minVel = -0.2, .maxVel = 0.2}, // pretty conservative limits atm - .pos = 0 - }}, - {"joint_de_roll", { - .limits = {.minPos = -2.36, .maxPos = 1.44, .minVel = -1, .maxVel = 1}, - .pos = 0 - }}, - {"gripper", { - .limits = {.minPos = 0, .maxPos = 0.1, .minVel = -1, .maxVel = 1}, - .pos = 0 - }}, + {"joint_a", {.limits = {.minPos = 0, .maxPos = 0.35, .minVel = -0.05, .maxVel = 0.05}, .pos = 0}}, + {"joint_b", {.limits = {.minPos = -0.9, .maxPos = 0, .minVel = -0.05, .maxVel = 0.05}, .pos = 0}}, + {"joint_c", {.limits = {.minPos = -0.959931, .maxPos = 2.87979, .minVel = -0.05 * 2 * std::numbers::pi, .maxVel = 0.05 * 2 * std::numbers::pi}, .pos = 0}}, + {"joint_de_pitch", {.limits = {.minPos = -1.3, .maxPos = 1.2, .minVel = -0.2, .maxVel = 0.2}, // pretty conservative limits atm + .pos = 0}}, + {"joint_de_roll", {.limits = {.minPos = -2.36, .maxPos = 1.44, .minVel = -1, .maxVel = 1}, .pos = 0}}, + {"gripper", {.limits = {.minPos = 0, .maxPos = 0.1, .minVel = -1, .maxVel = 1}, .pos = 0}}, }; + // ClickIK Verification + void ikSampleCallback(srv::IkSample::Request::ConstSharedPtr const& req, srv::IkSample::Response::SharedPtr const& resp); + rclcpp::Service::SharedPtr mSampleServ; + [[maybe_unused]] rclcpp::Subscription::SharedPtr mIkSub; [[maybe_unused]] rclcpp::Subscription::SharedPtr mVelSub; [[maybe_unused]] rclcpp::Subscription::SharedPtr mJointSub; @@ -81,7 +73,6 @@ namespace mrover { auto timerCallback() -> void; ArmPos mArmPos, mTypingOrigin, mPosTarget; - std::optional mPosFallback; geometry_msgs::msg::Twist mVelTarget; rclcpp::Time mLastUpdate; @@ -91,7 +82,7 @@ namespace mrover { TYPING }; ArmMode mArmMode = ArmMode::POSITION_CONTROL; - static const rclcpp::Duration TIMEOUT; + static rclcpp::Duration const TIMEOUT; public: // TODO(quintin): Neven, please load these from config YAML files instead of hard coding. Ideally they would even be computed at runtime. This way you can change the xacro without worry. diff --git a/navigation/arm_controller/pch.hpp b/navigation/arm_controller/pch.hpp index ac5fc41c..227ce124 100644 --- a/navigation/arm_controller/pch.hpp +++ b/navigation/arm_controller/pch.hpp @@ -5,20 +5,22 @@ #include #include + #include +#include +#include +#include #include #include -#include -#include -#include #include -#include +#include +#include #include #include #include -#include +#include #include #include diff --git a/navigation/click_ik/click_ik.cpp b/navigation/click_ik/click_ik.cpp new file mode 100644 index 00000000..0f99ba48 --- /dev/null +++ b/navigation/click_ik/click_ik.cpp @@ -0,0 +1,328 @@ +#include "click_ik.hpp" +#include "mrover/srv/detail/ik_sample__struct.hpp" +#include +#include + +namespace mrover { + + ClickIkNode::ClickIkNode(rclcpp::NodeOptions const& options) : Node("click_ik", options) { + + auto handle_goal = [this](rclcpp_action::GoalUUID const& uuid, action::ClickIk_Goal::ConstSharedPtr const& goal) -> rclcpp_action::GoalResponse { + RCLCPP_INFO(this->get_logger(), "Click Ik request received for point: (%f, %f)", goal->point_in_image_x, goal->point_in_image_y); + (void) uuid; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + + auto handle_cancel = [this](std::shared_ptr> const& goal_handle) -> rclcpp_action::CancelResponse { + RCLCPP_INFO(this->get_logger(), "Cancelling ClickIk Action."); + (void) goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; + }; + + auto handle_accepted = [this](std::shared_ptr> const& goal_handle) -> void { + if (mCurrentGoalHandle) + abortClickIk(); + mCurrentGoalHandle = goal_handle; + auto thread_executor = [this, goal_handle]() { + return this->executeClickIk(goal_handle); + }; + std::thread{thread_executor}.detach(); + }; + + server = rclcpp_action::create_server( + this, + "click_ik", + handle_goal, + handle_cancel, + handle_accepted); + + auto iShandle_goal = [this](rclcpp_action::GoalUUID const& uuid, action::IkImageSample_Goal::ConstSharedPtr const& goal) -> rclcpp_action::GoalResponse { + RCLCPP_INFO(this->get_logger(), "IK Image Sample request received."); + (void) uuid; + (void) goal; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + + auto iShandle_cancel = [this](std::shared_ptr> const& goal_handle) -> rclcpp_action::CancelResponse { + RCLCPP_INFO(this->get_logger(), "Cancelling IK Image Sample Action."); + (void) goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; + }; + + auto iShandle_accepted = [this](std::shared_ptr> const& goal_handle) -> void { + if (iSCurrentGoalHandle) { + auto result = std::make_shared(); + iSCurrentGoalHandle->abort(result); + } + iSCurrentGoalHandle = goal_handle; + auto thread_executor = [this, goal_handle]() { + return this->executeIkImageSample(goal_handle); + }; + std::thread{thread_executor}.detach(); + }; + + iSServer = rclcpp_action::create_server( + this, + "ik_image_sample", + iShandle_goal, + iShandle_cancel, + iShandle_accepted); + + mIkSampleClient = create_client("ik_sample"); + + mPcSub = create_subscription("zed/left/points", 1, [this](sensor_msgs::msg::PointCloud2::ConstSharedPtr const& msg) { + pointCloudCallback(msg); + }); + + // IK Publisher + mIkPub = create_publisher("ik_pos_cmd", 1); + + // ArmStatus subscriber + // mStatusSub = create_subscription("arm_cmd_status", 1, [this](msg::ArmStatus const& msg) { + // statusCallback(msg); + // }); + + mCurrentGoalHandle = nullptr; + } + + void + ClickIkNode::executeClickIk(std::shared_ptr> const& goal_handle) { + RCLCPP_INFO(get_logger(), "Executing goal"); + if (!goal_handle) { + RCLCPP_WARN(get_logger(), "Invalid ClickIK goal handle"); + return; + } + + auto const goal = goal_handle->get_goal(); + auto feedback = std::make_shared(); + auto result = std::make_shared(); + auto target_point = spiralSearchInImg( + static_cast(goal->point_in_image_x * static_cast(mPointCloudWidth)), + static_cast(goal->point_in_image_y * static_cast(mPointCloudHeight))); + + if (!target_point.has_value()) { + RCLCPP_WARN(get_logger(), "Target point does not exist."); + auto result = std::make_shared(); + result->success = false; + mCurrentGoalHandle->abort(result); + mCurrentGoalHandle = nullptr; + return; // fix + } + + geometry_msgs::msg::Pose pose; + + double offset = 0.1; // make sure we don't collide by moving back a little from the target + pose.position.set__x(target_point.value().x - offset); + pose.position.set__y(target_point.value().y); + pose.position.set__z(target_point.value().z); + SE3d targetInZed = SE3Conversions::fromPose(pose); + target_point->x -= ArmController::END_EFFECTOR_LENGTH; + SE3d targetInArm = SE3Conversions::fromTfTree(*mTfBuffer, "zed_left_camera_frame", "arm_base_link"); + targetInArm *= targetInZed; + auto req = std::make_shared(); + req->pos.x = targetInArm.x(); + req->pos.y = targetInArm.y(); + req->pos.z = targetInArm.z(); + auto a = mIkSampleClient->async_send_request(req); + if (a.wait_for(std::chrono::milliseconds(1000)) != std::future_status::ready) { + RCLCPP_INFO(this->get_logger(), "Point Sample timed out."); + auto result = std::make_shared(); + result->success = false; + mCurrentGoalHandle->abort(result); + mCurrentGoalHandle = nullptr; + return; + } else if (!a.get()->valid) { + RCLCPP_INFO(this->get_logger(), "Aborted, Invalid point."); + auto result = std::make_shared(); + result->success = false; + mCurrentGoalHandle->abort(result); + mCurrentGoalHandle = nullptr; + return; + } + RCLCPP_INFO(this->get_logger(), "Point Verified"); + + timer = this->create_wall_timer(std::chrono::milliseconds(10), [this, targetInZed, targetInArm, goal_handle, result, feedback]() { + if (goal_handle->is_canceling()) { + auto result = std::make_shared(); + result->success = false; + timer->cancel(); + goal_handle->canceled(result); + if (mCurrentGoalHandle) + mCurrentGoalHandle = nullptr; + RCLCPP_INFO(get_logger(), "ClickIk Goal Cancelled Successfully."); + return; + } + if (!goal_handle->is_executing()) return; + + float const tolerance = 0.02; + try { + SE3d arm_position = SE3Conversions::fromTfTree(*mTfBuffer, "arm_base_link", "arm_fk"); + double distance = pow(pow(arm_position.x() + targetInArm.x(), 2) + pow(arm_position.y() + targetInArm.y(), 2) + pow(arm_position.z() + targetInArm.z(), 2), 0.5); + RCLCPP_INFO(this->get_logger(), "Arm Position: %f, %f, %f", arm_position.x(), arm_position.y(), arm_position.z()); + RCLCPP_INFO(this->get_logger(), "Arm Command: %f, %f, %f", targetInArm.x(), targetInArm.y(), targetInArm.z()); + + feedback->distance = static_cast(distance); + goal_handle->publish_feedback(feedback); + + if (distance < tolerance) { + timer->cancel(); + result->success = true; + goal_handle->succeed(result); + mCurrentGoalHandle = nullptr; + return; + } + msg::IK ik; + ik.pos.x = targetInArm.x(); + ik.pos.y = targetInArm.y(); + ik.pos.z = targetInArm.z(); + mIkPub->publish(ik); + } catch (tf2::ExtrapolationException& e) { + RCLCPP_WARN(this->get_logger(), "ExtrapolationException (due to lag?): %s", e.what()); + } + }); + } + + void ClickIkNode::abortClickIk() { + timer->cancel(); + if (mCurrentGoalHandle) { + auto result = std::make_shared(); + result->success = false; + mCurrentGoalHandle->abort(result); + mCurrentGoalHandle = nullptr; + } + RCLCPP_INFO(get_logger(), "ClickIk Goal Aborted Successfully."); + } + + void ClickIkNode::pointCloudCallback(sensor_msgs::msg::PointCloud2::ConstSharedPtr const& msg) { + // Update current pointer to pointcloud data + mPoints = reinterpret_cast(msg->data.data()); + mNumPoints = msg->width * msg->height; + mPointCloudWidth = msg->width; + mPointCloudHeight = msg->height; + } + + auto ClickIkNode::spiralSearchInImg(size_t xCenter, size_t yCenter) -> std::optional { + std::size_t currX = xCenter; + std::size_t currY = yCenter; + std::size_t radius = 0; + int t = 0; + constexpr int numPts = 16; + bool isPointInvalid = true; + Point point{}; + std::size_t temp = mNumPoints; + + // Find the smaller of the two box dimensions so we know the max spiral radius + std::size_t smallDim = std::min(mPointCloudWidth / 2, mPointCloudHeight / 2); + + while (isPointInvalid) { + // This is the parametric equation to spiral around the center pnt + currX = static_cast(static_cast(xCenter) + std::cos(t * 1.0 / numPts * 2 * M_PI) * static_cast(radius)); + currY = static_cast(static_cast(yCenter) + std::sin(t * 1.0 / numPts * 2 * M_PI) * static_cast(radius)); + + if (currX > mPointCloudWidth) + currX = mPointCloudWidth; + if (currY > mPointCloudHeight) + currY = mPointCloudHeight; + if (currX + currY * mPointCloudWidth >= temp) + return std::nullopt; + + // Grab the point from the pntCloud and determine if its a finite pnt + point = mPoints[currX + currY * mPointCloudWidth]; + + isPointInvalid = !std::isfinite(point.x) || !std::isfinite(point.y) || !std::isfinite(point.z); + if (isPointInvalid) + RCLCPP_WARN(this->get_logger(), "Tag center point not finite: [%f %f %f]", point.x, point.y, point.z); + + // After a full circle increase the radius + if (t % numPts == 0) { + radius++; + } + + // Increase the parameter + t++; + + // If we reach the edge of the box we stop spiraling + if (radius >= smallDim) { + return std::nullopt; + } + } + (void) temp; + return std::make_optional(point); + } + + // auto ClickIkNode::statusCallback(msg::ArmStatus const& msg) -> void { + // if (!msg.status && mCurrentGoalHandle) { + // RCLCPP_WARN(get_logger(), "Arm position unreachable"); + // abortClickIk(); + // } + // } + + void ClickIkNode::executeIkImageSample(std::shared_ptr> const& goal_handle) { + RCLCPP_INFO(get_logger(), "Executing goal"); + if (!goal_handle) { + RCLCPP_WARN(get_logger(), "Invalid IKImageSample goal handle"); + return; + } + + auto const goal = goal_handle->get_goal(); + RCLCPP_INFO(get_logger(), "Goal, w:%d, h:%d, scale:%f", goal->w, goal->h, goal->scale); + auto result = std::make_shared(); + result->success.resize(goal->w * goal->h); + SE3d target_transfer_function = SE3Conversions::fromTfTree(*mTfBuffer, "zed_left_camera_frame", "arm_base_link"); + for (size_t j = 0; j < goal->h; j++) { + for (size_t i = 0; i < goal->w; i++) { + if (goal_handle->is_canceling()) { + goal_handle->canceled(result); + if (iSCurrentGoalHandle) + iSCurrentGoalHandle = nullptr; + RCLCPP_INFO(get_logger(), "ClickIk Goal Cancelled Successfully."); + return; + } + auto target_point = spiralSearchInImg( + static_cast((static_cast(i) / static_cast(goal->w)) * static_cast(mPointCloudWidth)), + static_cast((static_cast(j) / static_cast(goal->h)) * static_cast(mPointCloudHeight))); + if (!target_point.has_value()) { + result->success[j * goal->w + i] = false; + continue; + } + + geometry_msgs::msg::Pose pose; + + double offset = 0.1; // make sure we don't collide by moving back a little from the target + pose.position.set__x(target_point.value().x - offset); + pose.position.set__y(target_point.value().y); + pose.position.set__z(target_point.value().z); + SE3d targetInZed = SE3Conversions::fromPose(pose); + target_point->x -= ArmController::END_EFFECTOR_LENGTH; + SE3d targetInArm = target_transfer_function; + targetInArm *= targetInZed; + srv::IkSample::Request::SharedPtr sendReq = std::make_shared(); + geometry_msgs::msg::Vector3 vec; + vec.set__x(targetInArm.x()); + vec.set__y(targetInArm.y()); + vec.set__z(targetInArm.z()); + sendReq->set__pos(vec); + sendReq->set__roll(0); + sendReq->set__pitch(0); + size_t index = j * goal->w + i; + auto future = mIkSampleClient->async_send_request(sendReq); + + if (future.wait_for(std::chrono::milliseconds(200)) == std::future_status::ready) { + result->success[index] = future.get()->valid; + RCLCPP_INFO(this->get_logger(), "Point: %zu resolved successfully.", index); + } else { + result->success[index] = false; + RCLCPP_INFO(this->get_logger(), "Point: %zu resolved unsuccessfully.", index); + } + } + } + iSCurrentGoalHandle = nullptr; + goal_handle->succeed(result); + } + +} // namespace mrover + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(mrover::ClickIkNode); \ No newline at end of file diff --git a/navigation/click_ik/click_ik.hpp b/navigation/click_ik/click_ik.hpp new file mode 100644 index 00000000..091f6a5f --- /dev/null +++ b/navigation/click_ik/click_ik.hpp @@ -0,0 +1,50 @@ +#pragma once + +#include "pch.hpp" +namespace mrover { + + class ClickIkNode final : public rclcpp::Node { + rclcpp::Subscription::ConstSharedPtr mPcSub; + rclcpp::Publisher::SharedPtr mIkPub; + + rclcpp_action::Server::SharedPtr server; + rclcpp_action::Server::SharedPtr iSServer; + + msg::IK message; + rclcpp::TimerBase::SharedPtr timer; + + std::shared_ptr> mCurrentGoalHandle; + std::shared_ptr> iSCurrentGoalHandle; + bool cancelIkImageSample = false; + + + rclcpp::Client::SharedPtr mIkSampleClient; + + Point const* mPoints{}; + std::size_t mNumPoints{}; + std::size_t mPointCloudWidth{}; + std::size_t mPointCloudHeight{}; + + std::unique_ptr mTfBuffer = std::make_unique(get_clock()); + std::shared_ptr mTfListener = std::make_shared(*mTfBuffer); + std::shared_ptr mTfBroadcaster = std::make_shared(this); + + + public: + void executeClickIk(std::shared_ptr> const& goal_handle); + void executeIkImageSample(std::shared_ptr> const& goal_handle); + + + explicit ClickIkNode(rclcpp::NodeOptions const& options = rclcpp::NodeOptions()); + + ~ClickIkNode() override = default; + + auto pointCloudCallback(sensor_msgs::msg::PointCloud2::ConstSharedPtr const& msg) -> void; + + auto abortClickIk() -> void; + + //Taken line for line from percep object detection code + auto spiralSearchInImg(size_t xCenter, size_t yCenter) -> std::optional; + }; + +} // namespace mrover diff --git a/navigation/click_ik/main.cpp b/navigation/click_ik/main.cpp new file mode 100644 index 00000000..fc5a993e --- /dev/null +++ b/navigation/click_ik/main.cpp @@ -0,0 +1,10 @@ +#include "click_ik.hpp" +#include +#include + +auto main(int argc, char** argv) -> int { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return EXIT_SUCCESS; +} \ No newline at end of file diff --git a/navigation/click_ik/pch.hpp b/navigation/click_ik/pch.hpp new file mode 100644 index 00000000..424400b9 --- /dev/null +++ b/navigation/click_ik/pch.hpp @@ -0,0 +1,52 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + + +#include "../navigation/arm_controller/arm_controller.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include diff --git a/package.xml b/package.xml index 54ecbfdf..82d460bc 100644 --- a/package.xml +++ b/package.xml @@ -16,12 +16,14 @@ nav_msgs sensor_msgs geometry_msgs + dynamixel_sdk magic_enum yaml_cpp_vendor ament_cmake ament_cmake_python rosidl_default_generators + rosidl_interface_packages ament_lint_auto ament_lint_common diff --git a/scripts/teleop/specific-scripts/mock_click_ik.py b/scripts/teleop/specific-scripts/mock_click_ik.py new file mode 100755 index 00000000..092147e8 --- /dev/null +++ b/scripts/teleop/specific-scripts/mock_click_ik.py @@ -0,0 +1,49 @@ +#!/usr/bin/env python3 + +import time + +import rclpy +from rclpy.node import Node +from rclpy.action import ActionServer + +from mrover.action import ClickIk + + +class MockClickIkServer(Node): + def __init__(self): + super().__init__("mock_click_ik_server") + self.action_server = ActionServer(self, ClickIk, "/click_ik", self.execute_callback) + self.get_logger().info("Mock ClickIk action server ready on /click_ik") + + def execute_callback(self, goal_handle): + x = goal_handle.request.point_in_image_x + y = goal_handle.request.point_in_image_y + self.get_logger().info(f"Received ClickIk goal: ({x}, {y})") + + feedback = ClickIk.Feedback() + for dist in range(100, 0, -1): + feedback.distance = float(dist) + goal_handle.publish_feedback(feedback) + time.sleep(0.05) + + goal_handle.succeed() + result = ClickIk.Result() + result.success = True + self.get_logger().info("ClickIk goal succeeded") + return result + + +def main(args=None): + rclpy.init(args=args) + try: + node = MockClickIkServer() + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/simulator/pch.hpp b/simulator/pch.hpp index 79f81e15..3df16518 100644 --- a/simulator/pch.hpp +++ b/simulator/pch.hpp @@ -90,3 +90,10 @@ #include #include #include + +#include +#include +#include +#include +#include +#include \ No newline at end of file diff --git a/simulator/simulator.controls.cpp b/simulator/simulator.controls.cpp index 22f61c79..f35ffba0 100644 --- a/simulator/simulator.controls.cpp +++ b/simulator/simulator.controls.cpp @@ -1,5 +1,4 @@ #include "simulator.hpp" - namespace mrover { auto Simulator::throttlesCallback(msg::Throttle::ConstSharedPtr const& msg) -> void { @@ -117,7 +116,7 @@ namespace mrover { } auto Simulator::userControls(Clock::duration dt) -> void { - if (mPublishIk && mIkMode) { + if (mPublishIk && mIkMode && !mClickIk) { msg::IK ik; ik.pos.x = mIkTarget.x(); ik.pos.y = mIkTarget.y(); @@ -125,7 +124,44 @@ namespace mrover { ik.pitch = mIkPitch; ik.roll = mIkRoll; mIkTargetPub->publish(ik); + } else if (mClickIk && mPublishClickIk) { + action::ClickIk::Goal goal; + goal.set__point_in_image_x(static_cast(mClickIkX) / static_cast(mStereoCameras.front().base.resolution.x())); + goal.set__point_in_image_y(static_cast(mClickIkY) / static_cast(mStereoCameras.front().base.resolution.y())); + rclcpp_action::Client::SendGoalOptions options; + options.result_callback = [this](rclcpp_action::ClientGoalHandle::WrappedResult const& result) { + this->mPublishClickIk = false; + this->mCancelClickIk = false; + RCLCPP_INFO(this->get_logger(), "Action finished with code %d", (int) result.code); + }; + mActionClient->async_send_goal(goal, options); + } else if (mCancelClickIk) { + mActionClient->async_cancel_all_goals(); + mCancelClickIk = false; + } else if (mSampleIk) { + mHasSampled = true; + size_t x = mStereoCameras.front().base.resolution.x() / IMAGE_SAMPLE_RESOLUTION; + size_t y = mStereoCameras.front().base.resolution.y() / IMAGE_SAMPLE_RESOLUTION; + action::IkImageSample::Goal goal; + goal.set__w(x); + goal.set__h(y); + goal.set__scale(IMAGE_SAMPLE_RESOLUTION); + rclcpp_action::Client::SendGoalOptions options; + options.result_callback = [this](rclcpp_action::ClientGoalHandle::WrappedResult const& future) { + if (future.code == rclcpp_action::ResultCode::ABORTED) + return; + this->mImageSample = future.result; + if (!mImageSample) { + RCLCPP_WARN(this->get_logger(), "ClickIK Image Sample failed"); + return; + } + RCLCPP_INFO(this->get_logger(), "Successfully received ClickIK Image Sample"); + }; + mImageSampleClient->async_send_goal(goal, options); + } else if (mClearIkSample) { + mImageSample = nullptr; } + if (!mHasFocus || mInGui) return; if (mCameraInRoverTarget) @@ -151,6 +187,54 @@ namespace mrover { twist.angular.z = 0; } + mIkVel.setZero(); + mIkPitchVel = 0; + mIkRollVel = 0; + if (glfwGetKey(mWindow.get(), mArmForwardKey) == GLFW_PRESS) { + mIkVel.x() = 1; + } + if (glfwGetKey(mWindow.get(), mArmBackwardKey) == GLFW_PRESS) { + mIkVel.x() = -1; + } + if (glfwGetKey(mWindow.get(), mArmLeftKey) == GLFW_PRESS) { + mIkVel.y() = 1; + } + if (glfwGetKey(mWindow.get(), mArmRightKey) == GLFW_PRESS) { + mIkVel.y() = -1; + } + if (glfwGetKey(mWindow.get(), mArmUpKey) == GLFW_PRESS) { + mIkVel.z() = 1; + } + if (glfwGetKey(mWindow.get(), mArmDownKey) == GLFW_PRESS) { + mIkVel.z() = -1; + } + if (glfwGetKey(mWindow.get(), mArmPitchUpKey) == GLFW_PRESS) { + mIkPitchVel = -1; + } + if (glfwGetKey(mWindow.get(), mArmPitchDownKey) == GLFW_PRESS) { + mIkPitchVel = 1; + } + if (glfwGetKey(mWindow.get(), mArmRollCWKey) == GLFW_PRESS) { + mIkRollVel = 1; + } + if (glfwGetKey(mWindow.get(), mArmRollCCWKey) == GLFW_PRESS) { + mIkRollVel = -1; + } + mIkVel.normalize(); + mIkVel *= mArmSpeed; + mIkPitchVel *= mArmSpeed; + mIkRollVel *= mArmSpeed; + if (mPublishIk && !mIkMode) { + geometry_msgs::msg::Twist vel; + vel.linear.x = mIkVel.x(); + vel.linear.y = mIkVel.y(); + vel.linear.z = mIkVel.z(); + vel.angular.x = mIkRollVel; + vel.angular.y = mIkPitchVel; + mIkVelPub->publish(vel); + } + + mIkVel.setZero(); mIkPitchVel = 0; mIkRollVel = 0; diff --git a/simulator/simulator.cpp b/simulator/simulator.cpp index 70c8a8d8..a7a5d585 100644 --- a/simulator/simulator.cpp +++ b/simulator/simulator.cpp @@ -38,6 +38,31 @@ namespace mrover { mIkModeClient = create_client("ik_mode"); + mActionClient = rclcpp_action::create_client(this->get_node_base_interface(), + this->get_node_graph_interface(), + this->get_node_logging_interface(), + this->get_node_waitables_interface(), + "click_ik"); + + mImageSampleClient = rclcpp_action::create_client(this->get_node_base_interface(), + this->get_node_graph_interface(), + this->get_node_logging_interface(), + this->get_node_waitables_interface(), + "ik_image_sample"); + + mMotorTimeoutMs = get_parameter("motor_timeout").as_int(); + + mIsHeadless = get_parameter("headless").as_bool(); + mEnablePhysics = mIsHeadless; + { + mGpsLinearizationReferencePoint = { + get_parameter("ref_lat").as_double(), + get_parameter("ref_lon").as_double(), + get_parameter("ref_alt").as_double(), + }; + mGpsLinerizationReferenceHeading = get_parameter("ref_heading").as_double(); + } + if (!mIsHeadless) initWindow(); initPhysics(); @@ -72,6 +97,7 @@ namespace mrover { {"joint_de_pitch", "arm_d_link"}, {"joint_de_roll", "arm_e_link"}, {"gripper", "arm_gripper_link"}, + {"gripper", "arm_gripper_link"}, {"front_left", "front_left_wheel_link"}, {"middle_left", "center_left_wheel_link"}, {"back_left", "back_left_wheel_link"}, diff --git a/simulator/simulator.gui.cpp b/simulator/simulator.gui.cpp index bd062235..fb65a763 100644 --- a/simulator/simulator.gui.cpp +++ b/simulator/simulator.gui.cpp @@ -1,4 +1,7 @@ #include "simulator.hpp" +#include +#include +#include namespace mrover { @@ -87,11 +90,23 @@ namespace mrover { mIkModeClient->async_send_request(req); } if (mIkMode) { - ImGui::SliderFloat("IK X Position", &mIkTarget.x(), 0, 1.5); - ImGui::SliderFloat("IK Y Position", &mIkTarget.y(), 0, .45); - ImGui::SliderFloat("IK Z Position", &mIkTarget.z(), -1.0, 1.0); - ImGui::SliderFloat("IK Pitch", &mIkPitch, -3.14f, 3.14f); - ImGui::SliderFloat("IK Roll", &mIkRoll, -3.14f, 3.14f); + ImGui::Checkbox("Click Ik Control", &mClickIk); + if (mClickIk) { + ImGui::SliderFloat("Click IK X", &mClickIkX, 0, mStereoCameras[0].base.resolution.x()); + ImGui::SliderFloat("Click IK Y", &mClickIkY, 0, mStereoCameras[0].base.resolution.x()); + mPublishClickIk = ImGui::Button("Send Action"); + ImGui::SameLine(); + mCancelClickIk = ImGui::Button("Cancel Action"); + mSampleIk = ImGui::Button("Sample Valid Points"); + ImGui::SameLine(); + mClearIkSample = ImGui::Button("Clear Sample"); + } else { + ImGui::SliderFloat("IK X Position", &mIkTarget.x(), 0, 1.5); + ImGui::SliderFloat("IK Y Position", &mIkTarget.y(), 0, .45); + ImGui::SliderFloat("IK Z Position", &mIkTarget.z(), -1.0, 1.0); + ImGui::SliderFloat("IK Pitch", &mIkPitch, -3.14f, 3.14f); + ImGui::SliderFloat("IK Roll", &mIkRoll, -3.14f, 3.14f); + } } else ImGui::SliderFloat("Arm Speed", &mArmSpeed, 0, 1); } @@ -177,6 +192,43 @@ namespace mrover { for (StereoCamera const& stereoCamera: mStereoCameras) { float aspect = static_cast(stereoCamera.base.resolution.x()) / static_cast(stereoCamera.base.resolution.y()); ImGui::Image(stereoCamera.base.colorTextureView, {320, 320 / aspect}, {0, 0}, {1, 1}); + if (mClickIk) { + ImVec2 p0 = ImGui::GetItemRectMin(); + ImVec2 p1 = ImGui::GetItemRectMax(); + + // Map pixel -> UI coords + float u = (mClickIkX + 0.5f) / static_cast(stereoCamera.base.resolution.x()); + float v = (mClickIkY + 0.5f) / static_cast(stereoCamera.base.resolution.y()); + + ImVec2 pos = {p0.x + u * (p1.x - p0.x), + p0.y + v * (p1.y - p0.y)}; + + float cell_w = (p1.x - p0.x) * (static_cast(IMAGE_SAMPLE_RESOLUTION) / static_cast(stereoCamera.base.resolution.x())); + float cell_h = (p1.y - p0.y) * (static_cast(IMAGE_SAMPLE_RESOLUTION) / static_cast((float) stereoCamera.base.resolution.y())); + float r_x = cell_w * 0.5f; + float r_y = cell_h * 0.5f; + // Draw a small 3x3 square so it's visible + ImU32 pink = IM_COL32(255, 0, 255, 255); + + if (mImageSample) { + ImU32 green = IM_COL32(0, 255, 0, 80); + ImU32 red = IM_COL32(255, 0, 0, 80); + for (int y = 0; y < stereoCamera.base.resolution.y() / IMAGE_SAMPLE_RESOLUTION; y += 1) { + for (int x = 0; x < stereoCamera.base.resolution.x() / IMAGE_SAMPLE_RESOLUTION; x += 1) { + float py = static_cast(y + 0.5f) * static_cast(cell_h); + float px = static_cast(x + 0.5f) * static_cast(cell_w); + ImGui::GetWindowDrawList()->AddRectFilled( + {p0.x + px - r_x, p0.y + py - r_y}, + {p0.x + px + r_x, p0.y + py + r_y}, + mImageSample->success[static_cast((static_cast(y) * (static_cast(stereoCamera.base.resolution.x()) / static_cast(IMAGE_SAMPLE_RESOLUTION))) + static_cast(x))] ? green : red); + } + } + } + ImGui::GetWindowDrawList()->AddRectFilled( + {pos.x - 2, pos.y - 2}, + {pos.x + 2, pos.y + 2}, + pink); + } } ImGui::End(); diff --git a/simulator/simulator.hpp b/simulator/simulator.hpp index dc8f2fae..9f479452 100644 --- a/simulator/simulator.hpp +++ b/simulator/simulator.hpp @@ -94,6 +94,7 @@ namespace mrover { boost::container::small_vector, 2> visualUniforms; boost::container::small_vector, 2> collisionUniforms; Clock::time_point lastUpdate = Clock::now(); + bool isHolding = false; }; // Bullet Resources @@ -269,6 +270,19 @@ namespace mrover { tf2_ros::TransformListener mTfListener{mTfBuffer}; tf2_ros::TransformBroadcaster mTfBroadcaster{this}; + bool mClickIk = false; + bool mPublishClickIk = false; + bool mCancelClickIk = false; + float mClickIkX{0}; + float mClickIkY{0}; + rclcpp_action::Client::SharedPtr mActionClient; + bool mSampleIk = false; + bool mHasSampled = false; + bool mClearIkSample = false; + action::IkImageSample::Result::SharedPtr mImageSample; + uint8_t const IMAGE_SAMPLE_RESOLUTION = 10; + rclcpp_action::Client::SharedPtr mImageSampleClient; + bool mPublishIk = true; bool mIkMode = true; // true = position control, false = velocity control Eigen::Vector3f mIkTarget{0.293, 0.0f, -0.331}; diff --git a/simulator/simulator.physics.cpp b/simulator/simulator.physics.cpp index 1696af7d..6fbf3c15 100644 --- a/simulator/simulator.physics.cpp +++ b/simulator/simulator.physics.cpp @@ -47,7 +47,7 @@ namespace mrover { // Make the rocker and bogie try to always return to their initial positions // They can still move, so they act as a suspension system if (auto it = mUrdfs.find("rover"); it != mUrdfs.end()) { - URDF const& rover = it->second; + URDF& rover = it->second; for (auto const& name: {"left_rocker_link", "right_rocker_link"}) { int linkIndex = rover.linkNameToMeta.at(name).index; @@ -59,13 +59,20 @@ namespace mrover { // TODO: fix hard-coded names? for (auto const& name: {"arm_a_link", "arm_b_link", "arm_c_link", "arm_d_link", "arm_e_link", "arm_gripper_link"}) { bool expired = std::chrono::duration_cast(Clock::now() - rover.linkNameToMeta.at(name).lastUpdate).count() > mMotorTimeoutMs; + auto& linkMeta = rover.linkNameToMeta.at(name); if (expired) { - int linkIndex = rover.linkNameToMeta.at(name).index; - auto* motor = std::bit_cast(rover.physics->getLink(linkIndex).m_userPtr); - assert(motor); - motor->setVelocityTarget(0, 1); - // set p gain to 0 to stop position control - motor->setPositionTarget(0, 0); + if (!linkMeta.isHolding) { + int linkIndex = linkMeta.index; + auto* motor = std::bit_cast(rover.physics->getLink(linkIndex).m_userPtr); + assert(motor); + linkMeta.isHolding = true; + + btScalar currPos = rover.physics->getJointPos(linkIndex); + motor->setVelocityTarget(0, 1); + motor->setPositionTarget(currPos, 1); + } + } else { + linkMeta.isHolding = false; } } } diff --git a/srv/IkSample.srv b/srv/IkSample.srv new file mode 100644 index 00000000..c8b5c424 --- /dev/null +++ b/srv/IkSample.srv @@ -0,0 +1,5 @@ +geometry_msgs/Vector3 pos +float32 pitch +float32 roll +--- +bool valid \ No newline at end of file diff --git a/style.sh b/style.sh index 36fe0387..8c182051 100755 --- a/style.sh +++ b/style.sh @@ -56,7 +56,7 @@ readonly MYPY_PATH=$(find_executable mypy 1.11.2) # Add new directories with C++ code here: readonly CPP_FILES=( - ./{perception,lie,esw,simulator,parameter_utils}/**/*.{cpp,hpp,h,cu,cuh} + ./{perception,lie,esw,simulator,parameter_utils,teleoperation}/**/*.{cpp,hpp,h,cu,cuh} ) echo "Style checking C++ ..." "${CLANG_FORMAT_PATH}" "${CLANG_FORMAT_ARGS[@]}" -i "${CPP_FILES[@]}" diff --git a/teleoperation/basestation_gui/backend/models_pydantic.py b/teleoperation/basestation_gui/backend/models_pydantic.py index 51d6a496..c1b1a1b9 100644 --- a/teleoperation/basestation_gui/backend/models_pydantic.py +++ b/teleoperation/basestation_gui/backend/models_pydantic.py @@ -69,7 +69,7 @@ class GimbalAdjustRequest(BaseModel): absolute: bool = False class GearDiffRequest(BaseModel): - position: float = Field(ge=-3.14159, le=3.14159) + position: float = Field(ge=0.0, le=6.28318) is_counterclockwise: bool = False class RecordingCreateRequest(BaseModel): diff --git a/teleoperation/basestation_gui/frontend/src/components/FunnelControls.vue b/teleoperation/basestation_gui/frontend/src/components/FunnelControls.vue index 5394a1e5..d33d6e5e 100644 --- a/teleoperation/basestation_gui/frontend/src/components/FunnelControls.vue +++ b/teleoperation/basestation_gui/frontend/src/components/FunnelControls.vue @@ -75,12 +75,12 @@ import { ref } from 'vue' import { scienceAPI } from '@/utils/api' const site_to_radians: Record = { - 0: 0.0, - 1: Math.PI / 3, - 2: (2 * Math.PI) / 3, - 3: Math.PI, - 4: (4 * Math.PI) / 3, - 5: (5 * Math.PI) / 3, + 0: 4.2586, // GRIESS B + 1: 3.1415, // SAMPLE + 2: 2.0071, // BURET A + 3: 5.1138, // BURET B + 4: 0.0, // TRASH + 5: 1.1693, // GRIESS A } const currentSite = ref(0) diff --git a/teleoperation/camera_client/include/CallbackCheckBox.hpp b/teleoperation/camera_client/include/CallbackCheckBox.hpp index c5481815..96b97201 100644 --- a/teleoperation/camera_client/include/CallbackCheckBox.hpp +++ b/teleoperation/camera_client/include/CallbackCheckBox.hpp @@ -1,13 +1,14 @@ #pragma once -#include +#include "pch.hpp" namespace mrover { + using RequestCallback = std::function; - constexpr auto DEFAULT_REQUEST_CALLBACK = []() { return true; }; class CallbackCheckBox : public QPushButton { Q_OBJECT + QIcon mUncheckedIcon; QIcon mCheckedIcon; QString mUncheckedText; @@ -19,79 +20,16 @@ namespace mrover { RequestCallback mOnCheckCallback; RequestCallback mOnUncheckCallback; - public: - explicit CallbackCheckBox(QString const& uncheckedText, QString const& checkedText, QWidget* parent = nullptr) - : QPushButton(parent), - mUncheckedText(uncheckedText), - mCheckedText(checkedText), - mUsingIcons(false) { - - setText(uncheckedText); - - mOnCheckCallback = DEFAULT_REQUEST_CALLBACK; - mOnUncheckCallback = DEFAULT_REQUEST_CALLBACK; + auto updateAppearance() -> void; + auto handleClick() -> void; - connect(this, &QPushButton::clicked, this, [this]() { - this->setDisabled(true); - if (mChecked) { - if (!mOnUncheckCallback()) { - goto end; - } - } else { - if (!mOnCheckCallback()) { - goto end; - } - } - mChecked = !mChecked; - setText(mChecked ? mCheckedText : mUncheckedText); - end: - this->setDisabled(false); - }); - } - explicit CallbackCheckBox(QIcon const& uncheckedIcon, QIcon const& checkedIcon, QWidget* parent = nullptr) - : QPushButton(parent), - mUncheckedIcon(uncheckedIcon), - mCheckedIcon(checkedIcon), - mUsingIcons(true) { - - setIcon(uncheckedIcon); - - mOnCheckCallback = DEFAULT_REQUEST_CALLBACK; - mOnUncheckCallback = DEFAULT_REQUEST_CALLBACK; - - connect(this, &QPushButton::clicked, this, [this]() { - this->setDisabled(true); - if (mChecked) { - if (!mOnUncheckCallback()) { - goto end; - } - } else { - if (!mOnCheckCallback()) { - goto end; - } - } - mChecked = !mChecked; - setIcon(mChecked ? mCheckedIcon : mUncheckedIcon); - end: - this->setDisabled(false); - }); - } - - void setChecked(bool checked) { - mChecked = checked; - if (mUsingIcons) { - setIcon(mChecked ? mCheckedIcon : mUncheckedIcon); - } else { - setText(mChecked ? mCheckedText : mUncheckedText); - } - } + public: + explicit CallbackCheckBox(QString uncheckedText, QString checkedText, QWidget* parent = nullptr); + explicit CallbackCheckBox(QIcon uncheckedIcon, QIcon checkedIcon, QWidget* parent = nullptr); - void setOnCheckCallback(RequestCallback callback) { - mOnCheckCallback = std::move(callback); - } - void setOnUncheckCallback(RequestCallback callback) { - mOnUncheckCallback = std::move(callback); - } + auto setChecked(bool checked) -> void; + auto setOnCheckCallback(RequestCallback callback) -> void; + auto setOnUncheckCallback(RequestCallback callback) -> void; }; } // namespace mrover diff --git a/teleoperation/camera_client/include/CameraClientMainWindow.hpp b/teleoperation/camera_client/include/CameraClientMainWindow.hpp index 507a2d50..f0f74a66 100644 --- a/teleoperation/camera_client/include/CameraClientMainWindow.hpp +++ b/teleoperation/camera_client/include/CameraClientMainWindow.hpp @@ -1,11 +1,13 @@ #pragma once -#include "CallbackCheckBox.hpp" +#include "ClickIkPanel.hpp" #include "GstRtpVideoCreatorWidget.hpp" #include "GstVideoWidgets.hpp" +#include "ImagePreview.hpp" #include "VideoSelectorWidget.hpp" namespace mrover { + class CameraClientMainWindow : public QMainWindow { Q_OBJECT @@ -18,17 +20,24 @@ namespace mrover { QDockWidget* mGstRtpVideoCreatorDock; GstRtpVideoCreatorWidget* mGstRtpVideoCreatorWidget; + QDockWidget* mClickIkDock; + ClickIkPanel* mClickIkPanel; + public: explicit CameraClientMainWindow(QWidget* parent = nullptr); - auto createCamera(std::string const& name, std::string const& pipeline) -> bool; - auto getCameraSelectorWidget() -> VideoSelectorWidget*; - static auto showImagePopup(QImage const& image) -> void; + auto createCamera(std::string const& name, std::string const& pipeline, CameraCallbacks callbacks) -> bool; + auto getCameraGridWidget() -> GstVideoGridWidget*; + auto getClickIkPanel() -> ClickIkPanel*; + + public slots: + void showImagePreview(QString const& cameraName, QImage const& image); signals: void closed(); protected: - void closeEvent(QCloseEvent* event) override; + auto closeEvent(QCloseEvent* event) -> void override; }; -}; // namespace mrover + +} // namespace mrover diff --git a/teleoperation/camera_client/include/CameraClientNode.hpp b/teleoperation/camera_client/include/CameraClientNode.hpp new file mode 100644 index 00000000..2bafb77f --- /dev/null +++ b/teleoperation/camera_client/include/CameraClientNode.hpp @@ -0,0 +1,57 @@ +#pragma once + +#include +#include + +#include +#include +#include +#include + +#include "GstRtpVideoCreatorWidget.hpp" + +namespace mrover { + + struct CameraInfo { + std::string name; + std::string pipeline; + }; + + // inherits from both QObject (for signals) and rclcpp::Node (for ROS) + class CameraClientNode : public QObject, public rclcpp::Node { + Q_OBJECT + + std::unordered_map::SharedPtr> mMediaControlClients; + std::unordered_map::SharedPtr> mImageCaptureClients; + std::unordered_map::SharedPtr> mImageCaptureSubscribers; + rclcpp_action::Client::SharedPtr mClickIkClient; + rclcpp_action::Client::SharedPtr mIkSampleClient; + action::IkImageSample::Result::SharedPtr mImageSample; + + auto imageCaptureCallback(std::string const& cameraName, sensor_msgs::msg::Image::ConstSharedPtr const& msg) -> void; + auto sendMediaControlRequest(std::string const& cameraName, std::uint8_t command) -> bool; + auto sendScreenshotRequest(std::string const& cameraName) -> bool; + + public: + explicit CameraClientNode(); + + // call after connecting signals to discover cameras from parameters + auto discoverCameras() -> void; + + signals: + void cameraDiscovered(CameraInfo info); + void imageCaptured(QString cameraName, QImage image); + void clickIkFeedback(float distance); + void clickIkResult(bool success); + void ikImageSampleResult(action::IkImageSample::Result::SharedPtr imageSample); + + public slots: + bool requestPause(std::string const& cameraName); + bool requestPlay(std::string const& cameraName); + bool requestStop(std::string const& cameraName); + bool requestScreenshot(std::string const& cameraName); + void sendClickIk(float x, float y); + void sampleClickIk(); + }; + +} // namespace mrover \ No newline at end of file diff --git a/teleoperation/camera_client/include/ClickIkPanel.hpp b/teleoperation/camera_client/include/ClickIkPanel.hpp new file mode 100644 index 00000000..d019cdc6 --- /dev/null +++ b/teleoperation/camera_client/include/ClickIkPanel.hpp @@ -0,0 +1,58 @@ +#pragma once + +#include "GstVideoWidgets.hpp" +#include "pch.hpp" + +namespace mrover { + + class ClickIkPanel : public QWidget { + Q_OBJECT + + QPushButton* mToggleButton; + QHBoxLayout* mInfoRow; + QLabel* mStatusLabel; + QLabel* mFeedbackLabel; + QLabel* mResultLabel; + QVBoxLayout* mLayout; + QWidget* mVideoContainer; + QVBoxLayout* mVideoContainerLayout; + mrover::GstVideoWidget* mVideoWidget; + QPushButton* mSampleButton; + QPushButton* mClearOverlayButton; + QLabel* mSampleOverlay; + QImage* mOverlayImage; + + + bool mEnabled = false; + bool mRunning = false; + bool mShowOverlay = false; + int mSuccessCount = 0; + int mFailCount = 0; + uint8_t const IMAGE_SAMPLE_RESOLUTION = 10; + + public: + explicit ClickIkPanel(QWidget* parent = nullptr); + + auto placeZedWidget(GstVideoWidget* widget) -> void; + [[nodiscard]] auto canSendClick() const -> bool { return mEnabled && !mRunning; } + + public slots: + void updateFeedback(float distance); + void updateResult(bool success); + void enableOverlayWidget(mrover::action::IkImageSample::Result::SharedPtr const& imageSample); + void markRunning(); + + signals: + void toggled(bool enabled); + void sample(); + + private: + void onToggle(); + void onSample(); + void onClearOverlay(); + void refreshStatus(); + void refreshResultLabel(bool lastSuccess); + void resizeEvent(QResizeEvent* event) override; + }; + +} // namespace mrover diff --git a/teleoperation/camera_client/include/GstRtpVideoCreatorWidget.hpp b/teleoperation/camera_client/include/GstRtpVideoCreatorWidget.hpp index 9fcae78d..449b9137 100644 --- a/teleoperation/camera_client/include/GstRtpVideoCreatorWidget.hpp +++ b/teleoperation/camera_client/include/GstRtpVideoCreatorWidget.hpp @@ -3,14 +3,30 @@ #include "pch.hpp" namespace mrover { + + namespace { + constexpr auto DEFAULT_RTP_JITTER = std::chrono::milliseconds(500); + } + + inline auto createRtpToRawSrc(std::uint16_t port, gst::video::Codec codec, std::chrono::milliseconds rtpJitter = DEFAULT_RTP_JITTER) -> std::string { + std::string parser; + if (codec == gst::video::Codec::H265) { + parser = "! h265parse"; + } else if (codec == gst::video::Codec::H264) { + parser = "! h264parse"; + } + + return std::format("udpsrc port={} ! application/x-rtp,media=video ! rtpjitterbuffer latency={} ! {} {} ! decodebin", port, rtpJitter.count(), getRtpDepayloader(codec), parser); + } + /** - * @class GstRtpVideoCreatorWidget - * @brief Widget to create a new RTP video source - * - * This widget allows the user to create a new RTP video source by specifying the name, port, and codec. - * It emits a createRequested signal when the user clicks the submit button, which provides the name they - * assigned the video source and the gstreamer pipeline which generally follows the structure (udpsrc --> rtpjitterbuffer --> rtpdepay --> decoder). - */ + * @class GstRtpVideoCreatorWidget + * @brief Widget to create a new RTP video source + * + * This widget allows the user to create a new RTP video source by specifying the name, port, and codec. + * It emits a createRequested signal when the user clicks the submit button, which provides the name they + * assigned the video source and the gstreamer pipeline which generally follows the structure (udpsrc --> rtpjitterbuffer --> rtpdepay --> decoder). + */ class GstRtpVideoCreatorWidget : public QWidget { Q_OBJECT @@ -39,6 +55,6 @@ namespace mrover { void onSubmitClicked(); private: - void setWaiting(bool waiting); + auto setWaiting(bool waiting) -> void; }; } // namespace mrover diff --git a/teleoperation/camera_client/include/GstVideoWidgets.hpp b/teleoperation/camera_client/include/GstVideoWidgets.hpp index 5879e7a3..13f15115 100644 --- a/teleoperation/camera_client/include/GstVideoWidgets.hpp +++ b/teleoperation/camera_client/include/GstVideoWidgets.hpp @@ -3,23 +3,58 @@ #include "pch.hpp" namespace mrover { - class GstVideoWidget : public QVideoWidget { + + class DraggableVideoFrame : public QFrame { Q_OBJECT - QMediaPlayer* mPlayer; + std::string mCameraName; + QPoint mDragStartPosition; + + protected: + void mousePressEvent(QMouseEvent* event) override; + void mouseMoveEvent(QMouseEvent* event) override; + + public: + explicit DraggableVideoFrame(std::string cameraName, QWidget* parent = nullptr); + + [[nodiscard]] auto cameraName() const -> std::string const& { return mCameraName; } + }; + + class GstVideoWidget : public QWidget { + Q_OBJECT + + GstElement* mPipeline = nullptr; + std::string mPipelineString; + bool mStarted = false; + bool mIsError = false; + QString mErrorString; + + int mImageWidth = 0; + int mImageHeight = 0; public: explicit GstVideoWidget(QWidget* parent = nullptr); + ~GstVideoWidget() override; auto setGstPipeline(std::string const& pipeline) -> void; + auto setImageSize(int w, int h) -> void; [[nodiscard]] auto errorString() const -> QString; - [[nodiscard]] auto error() const -> QMediaPlayer::Error; [[nodiscard]] auto isError() const -> bool; auto play() -> void; auto pause() -> void; auto stop() -> void; + + signals: + void clicked(std::uint32_t imageX, std::uint32_t imageY); + + protected: + void showEvent(QShowEvent* event) override; + void mousePressEvent(QMouseEvent* event) override; + + private: + void startPipeline(); }; class GstVideoGridWidget : public QWidget { @@ -34,7 +69,7 @@ namespace mrover { private: struct GstVideoBox { - QWidget* widget; + DraggableVideoFrame* widget; QVBoxLayout* layout; QLabel* label; @@ -43,12 +78,23 @@ namespace mrover { QGridLayout* mMainLayout; std::unordered_map mGstVideoBoxes; + std::vector mVisibleOrder; GstVideoGridWidget::Error mError; QString mErrorString; auto clearError() -> void; auto setError(Error error, std::string const& errorString) -> void; + auto findVideoBox(std::string const& name) -> GstVideoBox*; + auto rebuildGrid() -> void; + auto getDropTargetIndex(QPoint const& pos) const -> int; + auto calculateColumnCount() const -> int; + + protected: + void dragEnterEvent(QDragEnterEvent* event) override; + void dragMoveEvent(QDragMoveEvent* event) override; + void dropEvent(QDropEvent* event) override; + void resizeEvent(QResizeEvent* event) override; public: explicit GstVideoGridWidget(QWidget* parent = nullptr); @@ -60,9 +106,11 @@ namespace mrover { auto stopVideo(std::string const& name) -> bool; auto hideVideo(std::string const& name) -> bool; auto showVideo(std::string const& name) -> bool; + auto moveCamera(std::string const& name, int newIndex) -> bool; [[nodiscard]] auto error() const -> GstVideoGridWidget::Error; [[nodiscard]] auto errorString() const -> QString; [[nodiscard]] auto isError() const -> bool; }; + } // namespace mrover diff --git a/teleoperation/camera_client/include/ImagePreview.hpp b/teleoperation/camera_client/include/ImagePreview.hpp index 10cf7d16..ac261e6c 100644 --- a/teleoperation/camera_client/include/ImagePreview.hpp +++ b/teleoperation/camera_client/include/ImagePreview.hpp @@ -2,53 +2,22 @@ #include "pch.hpp" -class ImagePreview : public QMainWindow { - Q_OBJECT - - QLabel* label; - QImage image; - QString name; - -public: - explicit ImagePreview(QWidget* parent = nullptr) : QMainWindow(parent), label(new QLabel(this)) { - - label->setAlignment(Qt::AlignCenter); - label->setMinimumSize(640, 480); - setCentralWidget(label); - - auto* toolbar = addToolBar("Toolbar"); - - auto* saveAction = toolbar->addAction("Save"); - connect(saveAction, &QAction::triggered, this, &ImagePreview::saveImage); - - auto* saveAsAction = toolbar->addAction("Save As"); - connect(saveAsAction, &QAction::triggered, this, &ImagePreview::saveImageAs); - } - - auto updateImage(QImage const& newImage) -> void { - image = newImage.copy(); - label->setPixmap(QPixmap::fromImage(image)); - label->resize(image.size()); - } - -private slots: - void saveImage() { - QString const downloads = QStandardPaths::writableLocation(QStandardPaths::DownloadLocation); - QString const timestamp = QDateTime::currentDateTime().toString("yyyyMMdd_HHmmss"); - QString const filename = downloads + "/image_" + timestamp + ".png"; - - if (!image.save(filename)) { - qWarning("Failed to save image!"); - } - } - - void saveImageAs() { - QString const filename = QFileDialog::getSaveFileName( - this, "Save Image As", QStandardPaths::writableLocation(QStandardPaths::DownloadLocation) + "/image.png", - "All Files (*);;PNG Images (*.png);;JPEG Images (*.jpg *.jpeg)"); - - if (!filename.isEmpty() && !image.save(filename)) { - qWarning("Failed to save image."); - } - } -}; +namespace mrover { + + class ImagePreview : public QMainWindow { + Q_OBJECT + + QLabel* mLabel; + QImage mImage; + + public: + explicit ImagePreview(QWidget* parent = nullptr); + + auto updateImage(QImage const& newImage) -> void; + + private slots: + void saveImage(); + void saveImageAs(); + }; + +} // namespace mrover diff --git a/teleoperation/camera_client/include/VideoSelectorWidget.hpp b/teleoperation/camera_client/include/VideoSelectorWidget.hpp index d68ae3b0..46f7aefc 100644 --- a/teleoperation/camera_client/include/VideoSelectorWidget.hpp +++ b/teleoperation/camera_client/include/VideoSelectorWidget.hpp @@ -5,6 +5,16 @@ #include "CallbackCheckBox.hpp" namespace mrover { + + struct CameraCallbacks { + RequestCallback onHide; + RequestCallback onShow; + RequestCallback onPause; + RequestCallback onPlay; + RequestCallback onStop; + RequestCallback onScreenshot; + }; + class VideoSelectorWidget : public QWidget { Q_OBJECT @@ -23,16 +33,14 @@ namespace mrover { std::unordered_map mSelectors; QVBoxLayout* mSelectorsLayout; + auto createVisibilityCheckBox(Selector& selector, CameraCallbacks const& callbacks) -> void; + auto createMediaControls(Selector& selector, std::string const& cameraName, CameraCallbacks const& callbacks) -> void; + auto createScreenshotButton(Selector& selector, CameraCallbacks const& callbacks) -> void; + public: explicit VideoSelectorWidget(QWidget* parent = nullptr); - auto addCamera(std::string const& name) -> void; - auto addVisibilitySelector(std::string const& cameraName, RequestCallback hideRequest, RequestCallback showRequest) -> void; - auto addMediaControls(std::string const& cameraName, RequestCallback pauseRequest, RequestCallback playRequest, RequestCallback stopRequest) -> void; - auto addScreenshotButton(std::string const& cameraName, RequestCallback screenshotRequest) -> void; - - signals: - void stopClicked(std::string const& name); - void screenshotClicked(std::string const& name); + auto addCamera(std::string const& name, CameraCallbacks callbacks) -> void; }; + } // namespace mrover diff --git a/teleoperation/camera_client/include/pch.hpp b/teleoperation/camera_client/include/pch.hpp index 397e2d72..dd343ad5 100644 --- a/teleoperation/camera_client/include/pch.hpp +++ b/teleoperation/camera_client/include/pch.hpp @@ -1,7 +1,10 @@ #pragma once +#include #include #include +#include +#include #include #include #include @@ -10,7 +13,11 @@ #include #include #include +#include #include +#include +#include +#include #include #include #include @@ -18,24 +25,31 @@ #include #include #include -#include #include #include +#include +#include +#include #include #include +#include #include #include +#include #include #include -#include #include -#include +#include +#include #include +#include #include #include #include #include + +#include \ No newline at end of file diff --git a/teleoperation/camera_client/src/CallbackCheckBox.cpp b/teleoperation/camera_client/src/CallbackCheckBox.cpp new file mode 100644 index 00000000..3a84e123 --- /dev/null +++ b/teleoperation/camera_client/src/CallbackCheckBox.cpp @@ -0,0 +1,66 @@ +#include "CallbackCheckBox.hpp" + +namespace mrover { + + namespace { + constexpr auto DEFAULT_REQUEST_CALLBACK = []() { return true; }; + } // namespace + + CallbackCheckBox::CallbackCheckBox(QString uncheckedText, QString checkedText, QWidget* parent) + : QPushButton(parent), + mUncheckedText(std::move(uncheckedText)), + mCheckedText(std::move(checkedText)), + mUsingIcons(false), + mOnCheckCallback(DEFAULT_REQUEST_CALLBACK), + mOnUncheckCallback(DEFAULT_REQUEST_CALLBACK) { + + setText(mUncheckedText); + connect(this, &QPushButton::clicked, this, &CallbackCheckBox::handleClick); + } + + CallbackCheckBox::CallbackCheckBox(QIcon uncheckedIcon, QIcon checkedIcon, QWidget* parent) + : QPushButton(parent), + mUncheckedIcon(std::move(uncheckedIcon)), + mCheckedIcon(std::move(checkedIcon)), + mUsingIcons(true), + mOnCheckCallback(DEFAULT_REQUEST_CALLBACK), + mOnUncheckCallback(DEFAULT_REQUEST_CALLBACK) { + + setIcon(mUncheckedIcon); + connect(this, &QPushButton::clicked, this, &CallbackCheckBox::handleClick); + } + + auto CallbackCheckBox::handleClick() -> void { + setDisabled(true); + + bool const shouldToggle = mChecked ? mOnUncheckCallback() : mOnCheckCallback(); + if (shouldToggle) { + mChecked = !mChecked; + updateAppearance(); + } + + setDisabled(false); + } + + auto CallbackCheckBox::updateAppearance() -> void { + if (mUsingIcons) { + setIcon(mChecked ? mCheckedIcon : mUncheckedIcon); + } else { + setText(mChecked ? mCheckedText : mUncheckedText); + } + } + + auto CallbackCheckBox::setChecked(bool checked) -> void { + mChecked = checked; + updateAppearance(); + } + + auto CallbackCheckBox::setOnCheckCallback(RequestCallback callback) -> void { + mOnCheckCallback = std::move(callback); + } + + auto CallbackCheckBox::setOnUncheckCallback(RequestCallback callback) -> void { + mOnUncheckCallback = std::move(callback); + } + +} // namespace mrover diff --git a/teleoperation/camera_client/src/CameraClientMainWindow.cpp b/teleoperation/camera_client/src/CameraClientMainWindow.cpp index 95974810..e5eeb5d2 100644 --- a/teleoperation/camera_client/src/CameraClientMainWindow.cpp +++ b/teleoperation/camera_client/src/CameraClientMainWindow.cpp @@ -1,82 +1,100 @@ #include "CameraClientMainWindow.hpp" -using namespace mrover; - -CameraClientMainWindow::CameraClientMainWindow(QWidget* parent) : QMainWindow(parent) { - mCameraGridWidget = new GstVideoGridWidget(this); - mCentralScrollArea = new QScrollArea(this); - - mCameraSelectorDock = new QDockWidget("Camera Selector", this); - mCameraSelectorWidget = new VideoSelectorWidget(); - mCameraSelectorDock->setWidget(mCameraSelectorWidget); - - mGstRtpVideoCreatorDock = new QDockWidget("Add Camera", this); - mGstRtpVideoCreatorWidget = new GstRtpVideoCreatorWidget(); - mGstRtpVideoCreatorDock->setWidget(mGstRtpVideoCreatorWidget); - - mCentralScrollArea->setWidget(mCameraGridWidget); - mCentralScrollArea->setWidgetResizable(true); - setCentralWidget(mCentralScrollArea); - - addDockWidget(Qt::LeftDockWidgetArea, mCameraSelectorDock); - splitDockWidget(mCameraSelectorDock, mGstRtpVideoCreatorDock, Qt::Vertical); - - connect(mGstRtpVideoCreatorWidget, &GstRtpVideoCreatorWidget::createRequested, - this, [this](std::string const& name, std::string const& pipeline) { - qDebug() << "Creating camera with name:" << QString::fromStdString(name) << "and pipeline:" << QString::fromStdString(pipeline); - - if (bool success = createCamera(name, pipeline); !success) { - QMetaObject::invokeMethod(mGstRtpVideoCreatorWidget, "onCreateResult", - Qt::QueuedConnection, - Q_ARG(bool, false), - Q_ARG(QString, mCameraGridWidget->errorString())); - } else { - QMetaObject::invokeMethod(mGstRtpVideoCreatorWidget, "onCreateResult", - Qt::QueuedConnection, - Q_ARG(bool, true)); - } - }); -} - -auto CameraClientMainWindow::createCamera(std::string const& name, std::string const& pipeline) -> bool { - mCameraGridWidget->addGstVideoWidget(name, pipeline); - if (mCameraGridWidget->isError()) { - return false; +namespace mrover { + + CameraClientMainWindow::CameraClientMainWindow(QWidget* parent) : QMainWindow(parent) { + mCameraGridWidget = new GstVideoGridWidget(this); + mCentralScrollArea = new QScrollArea(this); + + mCameraSelectorDock = new QDockWidget("Camera Selector", this); + mCameraSelectorWidget = new VideoSelectorWidget(); + mCameraSelectorDock->setWidget(mCameraSelectorWidget); + + mGstRtpVideoCreatorDock = new QDockWidget("Add Camera", this); + mGstRtpVideoCreatorWidget = new GstRtpVideoCreatorWidget(); + mGstRtpVideoCreatorDock->setWidget(mGstRtpVideoCreatorWidget); + QPalette pal = QPalette(); + pal.setColor(QPalette::Window, QApplication::palette().color(QPalette::Window)); + mGstRtpVideoCreatorDock->setAutoFillBackground(true); + mGstRtpVideoCreatorDock->setPalette(pal); + + + mClickIkDock = new QDockWidget("ClickIK", this); + mClickIkPanel = new ClickIkPanel(); + mClickIkDock->setWidget(mClickIkPanel); + + mCentralScrollArea->setWidget(mCameraGridWidget); + mCentralScrollArea->setWidgetResizable(true); + setCentralWidget(mCentralScrollArea); + + addDockWidget(Qt::LeftDockWidgetArea, mCameraSelectorDock); + splitDockWidget(mCameraSelectorDock, mGstRtpVideoCreatorDock, Qt::Vertical); + addDockWidget(Qt::RightDockWidgetArea, mClickIkDock); + + connect(mGstRtpVideoCreatorWidget, &GstRtpVideoCreatorWidget::createRequested, + this, [this](std::string const& name, std::string const& pipeline) { + qDebug() << "Creating camera with name:" << QString::fromStdString(name) << "and pipeline:" << QString::fromStdString(pipeline); + + CameraCallbacks callbacks{ + .onHide = [this, name]() { + mCameraGridWidget->hideVideo(name); + if (auto* widget = mCameraGridWidget->getGstVideoWidget(name)) { + widget->stop(); + } + return true; }, + .onShow = [this, name]() { + mCameraGridWidget->showVideo(name); + if (auto* widget = mCameraGridWidget->getGstVideoWidget(name)) { + widget->play(); + } + return true; }, + .onPause = []() { return true; }, + .onPlay = []() { return true; }, + .onStop = []() { return true; }, + .onScreenshot = []() { return true; }, + }; + + if (bool success = createCamera(name, pipeline, std::move(callbacks)); !success) { + QMetaObject::invokeMethod(mGstRtpVideoCreatorWidget, "onCreateResult", + Qt::QueuedConnection, + Q_ARG(bool, false), + Q_ARG(QString, mCameraGridWidget->errorString())); + } else { + QMetaObject::invokeMethod(mGstRtpVideoCreatorWidget, "onCreateResult", + Qt::QueuedConnection, + Q_ARG(bool, true)); + } + }); } - mCameraSelectorWidget->addCamera(name); - - // clang-format off - mCameraSelectorWidget->addVisibilitySelector(name, - [this, name]() { - mCameraGridWidget->hideVideo(name); - mCameraGridWidget->getGstVideoWidget(name)->stop(); - return true; - }, - [this, name]() { - mCameraGridWidget->showVideo(name); - mCameraGridWidget->getGstVideoWidget(name)->play(); - return true; - }); - // clang-format on - - return true; -} - -auto CameraClientMainWindow::getCameraSelectorWidget() -> VideoSelectorWidget* { - return mCameraSelectorWidget; -} - -auto CameraClientMainWindow::showImagePopup(QImage const& image) -> void { - auto imagePopup = new QLabel(); - imagePopup->setWindowFlags(Qt::Window | Qt::WindowStaysOnTopHint); - imagePopup->setWindowTitle("Screenshot"); - imagePopup->setPixmap(QPixmap::fromImage(image)); - imagePopup->setScaledContents(true); - imagePopup->resize(image.width(), image.height()); - imagePopup->show(); -} - -void CameraClientMainWindow::closeEvent(QCloseEvent* event) { - emit closed(); - QMainWindow::closeEvent(event); -} + + auto CameraClientMainWindow::createCamera(std::string const& name, std::string const& pipeline, CameraCallbacks callbacks) -> bool { + mCameraGridWidget->addGstVideoWidget(name, pipeline); + if (mCameraGridWidget->isError()) { + return false; + } + mCameraSelectorWidget->addCamera(name, std::move(callbacks)); + return true; + } + + auto CameraClientMainWindow::getCameraGridWidget() -> GstVideoGridWidget* { + return mCameraGridWidget; + } + + auto CameraClientMainWindow::getClickIkPanel() -> ClickIkPanel* { + return mClickIkPanel; + } + + void CameraClientMainWindow::showImagePreview(QString const& cameraName, QImage const& image) { + auto* preview = new ImagePreview(); + preview->updateImage(image); + preview->setWindowTitle(cameraName + " - Screenshot"); + preview->setAttribute(Qt::WA_DeleteOnClose); + preview->show(); + } + + auto CameraClientMainWindow::closeEvent(QCloseEvent* event) -> void { + emit closed(); + QMainWindow::closeEvent(event); + } + +} // namespace mrover diff --git a/teleoperation/camera_client/src/CameraClientNode.cpp b/teleoperation/camera_client/src/CameraClientNode.cpp new file mode 100644 index 00000000..3c7b1c83 --- /dev/null +++ b/teleoperation/camera_client/src/CameraClientNode.cpp @@ -0,0 +1,158 @@ +#include "CameraClientNode.hpp" + +#include "GstRtpVideoCreatorWidget.hpp" + +namespace mrover { + + CameraClientNode::CameraClientNode() + : QObject(nullptr), + Node("camera_client") { + mClickIkClient = rclcpp_action::create_client(this, "/click_ik"); + mIkSampleClient = rclcpp_action::create_client(this, "/ik_image_sample"); + RCLCPP_INFO(get_logger(), "Camera client initialized"); + } + + auto CameraClientNode::discoverCameras() -> void { + declare_parameter("cameras", rclcpp::ParameterType::PARAMETER_STRING_ARRAY); + auto cameraNames = get_parameter("cameras").as_string_array(); + + declare_parameter("rtp_jitter_ms", 100); + auto rtpJitterMs = std::chrono::milliseconds(get_parameter("rtp_jitter_ms").as_int()); + + for (auto const& cameraName: cameraNames) { + RCLCPP_INFO(get_logger(), "cameraName: %s", cameraName.c_str()); + + if (mMediaControlClients.contains(cameraName)) { + RCLCPP_WARN(get_logger(), "Camera %s already exists, skipping", cameraName.c_str()); + continue; + } + + declare_parameter(std::format("{}.port", cameraName), rclcpp::ParameterType::PARAMETER_INTEGER); + auto const port = static_cast(get_parameter(std::format("{}.port", cameraName)).as_int()); + + declare_parameter(std::format("{}.stream.codec", cameraName), rclcpp::ParameterType::PARAMETER_STRING); + std::string const codec = get_parameter(std::format("{}.stream.codec", cameraName)).as_string(); + + std::string const pipeline = createRtpToRawSrc(port, gst::video::getCodecFromStringView(codec), rtpJitterMs); + + mMediaControlClients.emplace(cameraName, create_client(std::format("{}_media_control", cameraName))); + mImageCaptureClients.emplace(cameraName, create_client(std::format("{}_image_capture", cameraName))); + mImageCaptureSubscribers.emplace(cameraName, create_subscription( + std::format("{}_image", cameraName), 1, + [this, cameraName](sensor_msgs::msg::Image::ConstSharedPtr const& msg) { + imageCaptureCallback(cameraName, msg); + })); + + // emit signal for GUI to handle camera setup + emit cameraDiscovered(CameraInfo{.name = cameraName, .pipeline = pipeline}); + } + } + + auto CameraClientNode::sendMediaControlRequest(std::string const& cameraName, std::uint8_t command) -> bool { + auto it = mMediaControlClients.find(cameraName); + if (it == mMediaControlClients.end()) { + RCLCPP_ERROR(get_logger(), "Camera %s not found", cameraName.c_str()); + return false; + } + auto request = std::make_shared(); + request->command = command; + it->second->async_send_request(request); + return true; + } + + auto CameraClientNode::sendScreenshotRequest(std::string const& cameraName) -> bool { + auto it = mImageCaptureClients.find(cameraName); + if (it == mImageCaptureClients.end()) { + RCLCPP_ERROR(get_logger(), "Camera %s not found", cameraName.c_str()); + return false; + } + auto request = std::make_shared(); + it->second->async_send_request(request); + return true; + } + + bool CameraClientNode::requestPause(std::string const& cameraName) { + qDebug() << "Pause request for camera" << cameraName.c_str(); + return sendMediaControlRequest(cameraName, srv::MediaControl::Request::PAUSE); + } + + bool CameraClientNode::requestPlay(std::string const& cameraName) { + qDebug() << "Play request for camera" << cameraName.c_str(); + return sendMediaControlRequest(cameraName, srv::MediaControl::Request::PLAY); + } + + bool CameraClientNode::requestStop(std::string const& cameraName) { + qDebug() << "Stop request for camera" << cameraName.c_str(); + return sendMediaControlRequest(cameraName, srv::MediaControl::Request::STOP); + } + + bool CameraClientNode::requestScreenshot(std::string const& cameraName) { + qDebug() << "Screenshot request for camera" << cameraName.c_str(); + return sendScreenshotRequest(cameraName); + } + + auto CameraClientNode::imageCaptureCallback(std::string const& cameraName, sensor_msgs::msg::Image::ConstSharedPtr const& msg) -> void { + RCLCPP_INFO(get_logger(), "Received image from camera"); + if (msg->encoding != sensor_msgs::image_encodings::BGR8) { + RCLCPP_ERROR(get_logger(), "Unsupported encoding - image capture must be BGR8"); + return; + } + + cv::Size receivedSize{static_cast(msg->width), static_cast(msg->height)}; + cv::Mat bgrFrame{receivedSize, CV_8UC3, const_cast(msg->data.data()), msg->step}; + + // create a deep copy of the image data for the signal + QImage qImg(bgrFrame.data, bgrFrame.cols, bgrFrame.rows, static_cast(bgrFrame.step), QImage::Format_BGR888); + + // emit signal with a copy (since the original data will go out of scope) + emit imageCaptured(QString::fromStdString(cameraName), qImg.copy()); + } + + void CameraClientNode::sendClickIk(float x, float y) { + if (!mClickIkClient->action_server_is_ready()) { + RCLCPP_WARN(get_logger(), "ClickIk action server not ready"); + return; + } + auto goal = action::ClickIk::Goal{}; + goal.set__point_in_image_x(static_cast(x)); + goal.set__point_in_image_y(static_cast(y)); + RCLCPP_INFO(get_logger(), "Sending ClickIk goal: (%f, %f)", x, y); + + auto options = rclcpp_action::Client::SendGoalOptions{}; + options.feedback_callback = + [this](auto, auto feedback) { + emit clickIkFeedback(feedback->distance); + }; + options.result_callback = + [this](auto const& result) { + emit clickIkResult(result.result->success); + }; + mClickIkClient->async_send_goal(goal, options); + } + + void CameraClientNode::sampleClickIk() { + RCLCPP_INFO(this->get_logger(), "Sending ClickIK Image Sample Request."); + auto goal = action::IkImageSample::Goal(); + goal.set__w(128); + goal.set__h(72); + goal.set__scale(10); + auto options = rclcpp_action::Client::SendGoalOptions{}; + options.result_callback = + [this](auto const& result) { + if (result.code != rclcpp_action::ResultCode::SUCCEEDED) { + return; + } + + // 2. Ensure the result pointer actually exists + if (result.result) { + // Store it in a class member first to keep the memory alive + this->mImageSample = result.result; + + // 3. Emit the member or the pointer + emit ikImageSampleResult(this->mImageSample); + }; + }; + mIkSampleClient->async_send_goal(goal, options); + } + +} // namespace mrover diff --git a/teleoperation/camera_client/src/ClickIkPanel.cpp b/teleoperation/camera_client/src/ClickIkPanel.cpp new file mode 100644 index 00000000..31c4e535 --- /dev/null +++ b/teleoperation/camera_client/src/ClickIkPanel.cpp @@ -0,0 +1,179 @@ +#include "ClickIkPanel.hpp" + +namespace mrover { + + ClickIkPanel::ClickIkPanel(QWidget* parent) : QWidget(parent) { + mLayout = new QVBoxLayout(this); + + mInfoRow = new QHBoxLayout(); + mToggleButton = new QPushButton("Enable ClickIK", this); + mToggleButton->setCheckable(true); + connect(mToggleButton, &QPushButton::clicked, this, &ClickIkPanel::onToggle); + mStatusLabel = new QLabel(this); + mStatusLabel->setAlignment(Qt::AlignCenter); + mResultLabel = new QLabel(this); + mResultLabel->setAlignment(Qt::AlignCenter); + mFeedbackLabel = new QLabel(this); + mFeedbackLabel->setAlignment(Qt::AlignCenter); + mSampleButton = new QPushButton("Sample", this); + // mSampleButton->setCheckable(true); + connect(mSampleButton, &QPushButton::clicked, this, &ClickIkPanel::onSample); + mClearOverlayButton = new QPushButton("Clear Overlay", this); + // mClearOverlayButton->setCheckable(true); + connect(mClearOverlayButton, &QPushButton::clicked, this, &ClickIkPanel::onClearOverlay); + mInfoRow->addWidget(mToggleButton); + mInfoRow->addWidget(mStatusLabel, 1); + mInfoRow->addWidget(mResultLabel, 1); + mInfoRow->addWidget(mFeedbackLabel, 1); + mInfoRow->addWidget(mSampleButton, 1); + mInfoRow->addWidget(mClearOverlayButton, 1); + mLayout->addLayout(mInfoRow); + + mVideoContainer = new QWidget(this); + mVideoContainerLayout = new QVBoxLayout(mVideoContainer); + mVideoContainerLayout->setContentsMargins(0, 0, 0, 0); + mVideoContainer->setLayout(mVideoContainerLayout); + mLayout->addWidget(mVideoContainer, 1); + + mSampleOverlay = new QLabel(this); + mSampleOverlay->setAttribute(Qt::WA_TransparentForMouseEvents); + mSampleOverlay->setStyleSheet("background: transparent;"); + mSampleOverlay->setScaledContents(true); + + mOverlayImage = new QImage(1280, 720, QImage::Format_ARGB32_Premultiplied); + mSampleOverlay->setPixmap(QPixmap::fromImage(*mOverlayImage)); + mSampleOverlay->setVisible(false); + + setLayout(mLayout); + setMinimumWidth(640); + + refreshStatus(); + refreshResultLabel(false); + } + + auto ClickIkPanel::placeZedWidget(GstVideoWidget* widget) -> void { + mVideoWidget = widget; + mVideoWidget->setParent(mVideoContainer); + mVideoWidget->setGeometry(mVideoContainer->rect()); + mSampleOverlay->setParent(mVideoContainer); + mSampleOverlay->setGeometry(mVideoWidget->rect()); + mSampleOverlay->raise(); + mVideoWidget->show(); + } + + void ClickIkPanel::resizeEvent(QResizeEvent* event) { + QWidget::resizeEvent(event); // Let the base class handle layout first + + if (mVideoWidget && mVideoContainer) { + // Manually snap the video widget to the container's exact dimensions + mVideoWidget->setGeometry(mVideoContainer->rect()); + mSampleOverlay->setGeometry(mVideoWidget->rect()); + + // Synchronize the internal coordinate system to this new size + mVideoWidget->setImageSize(mVideoWidget->width(), mVideoWidget->height()); + mSampleOverlay->setPixmap(QPixmap::fromImage(*mOverlayImage)); + } + } + + void ClickIkPanel::onToggle() { + mEnabled = mToggleButton->isChecked(); + mToggleButton->setText(mEnabled ? "Disable ClickIK" : "Enable ClickIK"); + if (!mEnabled) { + mSuccessCount = 0; + mFailCount = 0; + mRunning = false; + } + refreshStatus(); + refreshResultLabel(false); + emit toggled(mEnabled); + } + + void ClickIkPanel::onSample() { + emit sample(); + }; + + void ClickIkPanel::onClearOverlay() { + mSampleOverlay->setVisible(false); + }; + + void ClickIkPanel::enableOverlayWidget(mrover::action::IkImageSample::Result::SharedPtr const& imageSample) { + if (imageSample) { + QColor green(0, 255, 0, 80); + QColor red(255, 0, 0, 80); + + int resX = 1280; + int resY = 720; + + int gridW = 128; + int gridH = 72; + int expected = gridW * gridH; + + if (static_cast(imageSample->success.size()) < expected) { + // bail / warn + return; + } + + int cellW = resX / gridW; // should be 10 + int cellH = resY / gridH; // should be 10 + + for (int y = 0; y < gridH; ++y) { + for (int x = 0; x < gridW; ++x) { + int idx = y * gridW + x; // row-major + QColor cellColor = imageSample->success[idx] || (x == 0 && y == 0) || (x == 127 && y == 71) ? green : red; + + int startX = x * cellW; + int startY = y * cellH; + + for (int py = 0; py < cellH; ++py) { + for (int px = 0; px < cellW; ++px) { + mOverlayImage->setPixelColor(startX + px, startY + py, cellColor); + } + } + } + } + } + mSampleOverlay->setPixmap(QPixmap::fromImage(*mOverlayImage)); + mSampleOverlay->setVisible(true); + } + + void ClickIkPanel::markRunning() { + mRunning = true; + refreshStatus(); + } + + void ClickIkPanel::updateFeedback(float distance) { + mFeedbackLabel->setText(QString("Distance: %1").arg(static_cast(distance), 0, 'f', 3)); + } + + void ClickIkPanel::updateResult(bool success) { + mRunning = false; + if (success) { + ++mSuccessCount; + } else { + ++mFailCount; + } + refreshStatus(); + refreshResultLabel(success); + } + + void ClickIkPanel::refreshStatus() { + if (!mEnabled) { + mStatusLabel->setText("Status: OFF"); + mFeedbackLabel->setText("Distance: --"); + } else if (mRunning) { + mStatusLabel->setText("Status: WAITING"); + } else { + mStatusLabel->setText("Status: READY"); + } + } + + void ClickIkPanel::refreshResultLabel(bool lastSuccess) { + if (mSuccessCount == 0 && mFailCount == 0) { + mResultLabel->setText("Last: -- | OK: 0 | Failed: 0"); + } else { + QString last = lastSuccess ? "Last: OK" : "Last: FAILED"; + mResultLabel->setText(QString("%1 | OK: %2 | Failed: %3").arg(last).arg(mSuccessCount).arg(mFailCount)); + } + } + +} // namespace mrover diff --git a/teleoperation/camera_client/src/GstRtpVideoCreatorWidget.cpp b/teleoperation/camera_client/src/GstRtpVideoCreatorWidget.cpp index cf240a4c..e06a6402 100644 --- a/teleoperation/camera_client/src/GstRtpVideoCreatorWidget.cpp +++ b/teleoperation/camera_client/src/GstRtpVideoCreatorWidget.cpp @@ -9,7 +9,6 @@ inline auto fromStringView(std::string_view view) -> QString { GstRtpVideoCreatorWidget::GstRtpVideoCreatorWidget(QWidget* parent) : QWidget(parent) { mMainLayout = new QVBoxLayout(this); - // Form mFormWidget = new QWidget(this); mFormLayout = new QFormLayout(mFormWidget); mNameLineEdit = new QLineEdit(mFormWidget); @@ -23,13 +22,11 @@ GstRtpVideoCreatorWidget::GstRtpVideoCreatorWidget(QWidget* parent) : QWidget(pa mFormLayout->addRow(tr("&Codec*"), mVideoCodecComboBox); mMainLayout->addWidget(mFormWidget); - // Error mErrorLabel = new QLabel(this); mErrorLabel->setStyleSheet("QLabel { color : red; }"); mErrorLabel->setVisible(false); mMainLayout->addWidget(mErrorLabel); - // Submit mSubmitButton = new QPushButton(tr("Submit"), this); connect(mSubmitButton, &QPushButton::clicked, this, &GstRtpVideoCreatorWidget::onSubmitClicked); mMainLayout->addWidget(mSubmitButton); @@ -50,7 +47,7 @@ void GstRtpVideoCreatorWidget::onCreateResult(bool success, QString const& error setWaiting(false); } -void GstRtpVideoCreatorWidget::setWaiting(bool waiting) { +auto GstRtpVideoCreatorWidget::setWaiting(bool waiting) -> void { mSubmitButton->setEnabled(!waiting); mNameLineEdit->setEnabled(!waiting); mPortLineEdit->setEnabled(!waiting); @@ -73,7 +70,7 @@ void GstRtpVideoCreatorWidget::onSubmitClicked() { } gst::video::Codec const codec = gst::video::getCodecFromStringView(mVideoCodecComboBox->currentText().toStdString()); - std::string const pipeline = gst::video::createRtpToRawSrc(static_cast(port), codec); + std::string const pipeline = createRtpToRawSrc(static_cast(port), codec); std::string name; if (mNameLineEdit->text().isEmpty()) { diff --git a/teleoperation/camera_client/src/GstVideoWidgets.cpp b/teleoperation/camera_client/src/GstVideoWidgets.cpp index bd016659..cf3176d2 100644 --- a/teleoperation/camera_client/src/GstVideoWidgets.cpp +++ b/teleoperation/camera_client/src/GstVideoWidgets.cpp @@ -2,48 +2,176 @@ using namespace mrover; -GstVideoWidget::GstVideoWidget(QWidget* parent) : QVideoWidget(parent) { - mPlayer = new QMediaPlayer(this); - mPlayer->setVideoOutput(this); +// -------------------------------------------- +// DraggableVideoFrame +// -------------------------------------------- + +DraggableVideoFrame::DraggableVideoFrame(std::string cameraName, QWidget* parent) + : QFrame(parent), mCameraName(std::move(cameraName)) { + setFrameShape(QFrame::StyledPanel); + setFrameShadow(QFrame::Raised); + setCursor(Qt::OpenHandCursor); +} + +void DraggableVideoFrame::mousePressEvent(QMouseEvent* event) { + if (event->button() == Qt::LeftButton) { + mDragStartPosition = event->pos(); + setCursor(Qt::ClosedHandCursor); + } + QFrame::mousePressEvent(event); +} + +void DraggableVideoFrame::mouseMoveEvent(QMouseEvent* event) { + if (!(event->buttons() & Qt::LeftButton)) { + return; + } + + // check if we've moved far enough to start a drag + if ((event->pos() - mDragStartPosition).manhattanLength() < QApplication::startDragDistance()) { + return; + } + + auto* drag = new QDrag(this); + auto* mimeData = new QMimeData(); + mimeData->setText(QString::fromStdString(mCameraName)); + drag->setMimeData(mimeData); + + // visual feedback + QPixmap pixmap = grab(); + pixmap = pixmap.scaled(pixmap.size() / 2, Qt::KeepAspectRatio, Qt::SmoothTransformation); + drag->setPixmap(pixmap); + drag->setHotSpot(QPoint(pixmap.width() / 2, pixmap.height() / 2)); + + setCursor(Qt::OpenHandCursor); + drag->exec(Qt::MoveAction); +} + +// -------------------------------------------- +// GstVideoWidget +// -------------------------------------------- + +GstVideoWidget::GstVideoWidget(QWidget* parent) : QWidget(parent) { +} + +GstVideoWidget::~GstVideoWidget() { + if (mPipeline) { + gst_element_set_state(mPipeline, GST_STATE_NULL); + gst_object_unref(mPipeline); + } } auto GstVideoWidget::setGstPipeline(std::string const& pipeline) -> void { - mPlayer->setMedia(QUrl(std::format("gst-pipeline: {} ! videoconvert ! xvimagesink name=\"qtvideosink\" sync=false", pipeline).c_str())); + if (mPipeline) { + gst_element_set_state(mPipeline, GST_STATE_NULL); + gst_object_unref(mPipeline); + mPipeline = nullptr; + } + mPipelineString = pipeline + " ! videoconvert ! qwidget5videosink name=videosink"; + mStarted = false; + mIsError = false; + mErrorString.clear(); +} + +void GstVideoWidget::showEvent(QShowEvent* event) { + QWidget::showEvent(event); + if (mStarted || mPipelineString.empty()) return; + mStarted = true; + + startPipeline(); +} + +void GstVideoWidget::startPipeline() { + GError* err = nullptr; + mPipeline = gst_parse_launch(mPipelineString.c_str(), &err); + if (!mPipeline) { + mIsError = true; + mErrorString = err ? QString::fromUtf8(err->message) : "Failed to create pipeline"; + if (err) g_error_free(err); + return; + } + if (err) g_error_free(err); + + GstElement* sink = gst_bin_get_by_name(GST_BIN(mPipeline), "videosink"); + if (sink) { + g_object_set(sink, "widget", this, nullptr); + gst_object_unref(sink); + } + play(); } auto GstVideoWidget::errorString() const -> QString { - return mPlayer->errorString(); + return mErrorString; +} + +auto GstVideoWidget::isError() const -> bool { + return mIsError; } -auto GstVideoWidget::error() const -> QMediaPlayer::Error { - return mPlayer->error(); +auto GstVideoWidget::setImageSize(int w, int h) -> void { + mImageWidth = w; + mImageHeight = h; } -auto GstVideoWidget::isError() const -> bool { - return mPlayer->error() != QMediaPlayer::NoError; +void GstVideoWidget::mousePressEvent(QMouseEvent* event) { + if (event->button() == Qt::LeftButton && mImageWidth > 0 && mImageHeight > 0) { + int widgetW = width(); + int widgetH = height(); + + double scaleX = static_cast(widgetW) / mImageWidth; + double scaleY = static_cast(widgetH) / mImageHeight; + double scale = std::min(scaleX, scaleY); + + double renderW = mImageWidth * scale; + double renderH = mImageHeight * scale; + + double offsetX = (widgetW - renderW) / 2.0; + double offsetY = (widgetH - renderH) / 2.0; + + double imgX = (event->pos().x() - offsetX) / scale; + double imgY = (event->pos().y() - offsetY) / scale; + + if (imgX >= 0 && imgX < mImageWidth && imgY >= 0 && imgY < mImageHeight) { + emit clicked(static_cast(imgX), static_cast(imgY)); + } + } + QWidget::mousePressEvent(event); } auto GstVideoWidget::play() -> void { - mPlayer->play(); + if (mPipeline) gst_element_set_state(mPipeline, GST_STATE_PLAYING); } auto GstVideoWidget::pause() -> void { - mPlayer->pause(); + if (mPipeline) gst_element_set_state(mPipeline, GST_STATE_PAUSED); } auto GstVideoWidget::stop() -> void { - mPlayer->stop(); + if (mPipeline) gst_element_set_state(mPipeline, GST_STATE_NULL); } +// -------------------------------------------- +// GstVideoGridWidget // -------------------------------------------- +namespace { + constexpr int MIN_VIDEO_WIDTH = 640; +} + GstVideoGridWidget::GstVideoGridWidget(QWidget* parent) : QWidget(parent), mError(NoError) { mMainLayout = new QGridLayout(this); - mMainLayout->setContentsMargins(5, -1, 5, -1); + mMainLayout->setContentsMargins(5, 5, 5, 5); mMainLayout->setSpacing(10); setLayout(mMainLayout); + setAcceptDrops(true); +} + +auto GstVideoGridWidget::calculateColumnCount() const -> int { + int const availableWidth = width() - mMainLayout->contentsMargins().left() - mMainLayout->contentsMargins().right(); + int const cellWidth = MIN_VIDEO_WIDTH + mMainLayout->spacing(); + int const columns = std::max(1, availableWidth / cellWidth); + return columns; } auto GstVideoGridWidget::addGstVideoWidget(std::string const& name, std::string const& pipeline) -> bool { @@ -53,12 +181,12 @@ auto GstVideoGridWidget::addGstVideoWidget(std::string const& name, std::string return false; } - auto* gstVideoBoxWidget = new QWidget(this); + auto* gstVideoBoxWidget = new DraggableVideoFrame(name, this); auto* gstVideoBoxLayout = new QVBoxLayout(gstVideoBoxWidget); auto* gstVideoBoxLabel = new QLabel(QString::fromStdString(name), gstVideoBoxWidget); auto* gstVideoBoxGstVideoWidget = new GstVideoWidget(gstVideoBoxWidget); - gstVideoBoxWidget->setMinimumSize(640, 360); + gstVideoBoxWidget->setMinimumSize(640, 480); gstVideoBoxGstVideoWidget->setGstPipeline(pipeline); if (gstVideoBoxGstVideoWidget->isError()) { @@ -72,76 +200,120 @@ auto GstVideoGridWidget::addGstVideoWidget(std::string const& name, std::string gstVideoBoxLayout->addWidget(gstVideoBoxGstVideoWidget, 1); gstVideoBoxWidget->setLayout(gstVideoBoxLayout); - - int const index = static_cast(mGstVideoBoxes.size()); - mMainLayout->addWidget(gstVideoBoxWidget, index / 2, index % 2, Qt::AlignCenter); - mGstVideoBoxes.emplace(name, GstVideoBox{.widget = gstVideoBoxWidget, .layout = gstVideoBoxLayout, .label = gstVideoBoxLabel, .gstVideoWidget = gstVideoBoxGstVideoWidget}); + + mVisibleOrder.push_back(name); + rebuildGrid(); + clearError(); return true; } -auto GstVideoGridWidget::getGstVideoWidget(std::string const& name) -> GstVideoWidget* { - if (!mGstVideoBoxes.contains(name)) { +auto GstVideoGridWidget::findVideoBox(std::string const& name) -> GstVideoBox* { + auto it = mGstVideoBoxes.find(name); + if (it == mGstVideoBoxes.end()) { + setError(NonExistsError, "Camera name does not exist"); return nullptr; } - return mGstVideoBoxes.at(name).gstVideoWidget; + clearError(); + return &it->second; } -auto GstVideoGridWidget::playVideo(std::string const& name) -> bool { - if (!mGstVideoBoxes.contains(name)) { - setError(NonExistsError, "Camera name does not exist"); - return false; +auto GstVideoGridWidget::rebuildGrid() -> void { + // remove all widgets from the grid (without deleting them) + for (auto& [name, box]: mGstVideoBoxes) { + mMainLayout->removeWidget(box.widget); } - mGstVideoBoxes.at(name).gstVideoWidget->play(); - clearError(); + int const columns = calculateColumnCount(); + + // re-add only visible widgets in order + for (std::size_t idx = 0; idx < mVisibleOrder.size(); ++idx) { + auto* box = findVideoBox(mVisibleOrder[idx]); + if (box) { + int const row = static_cast(idx) / columns; + int const col = static_cast(idx) % columns; + mMainLayout->addWidget(box->widget, row, col, Qt::AlignCenter); + box->widget->setVisible(true); + } + } +} + +void GstVideoGridWidget::resizeEvent(QResizeEvent* event) { + QWidget::resizeEvent(event); + rebuildGrid(); +} + +auto GstVideoGridWidget::getGstVideoWidget(std::string const& name) -> GstVideoWidget* { + auto* box = findVideoBox(name); + return box ? box->gstVideoWidget : nullptr; +} + +auto GstVideoGridWidget::playVideo(std::string const& name) -> bool { + auto* box = findVideoBox(name); + if (!box) return false; + box->gstVideoWidget->play(); return true; } auto GstVideoGridWidget::pauseVideo(std::string const& name) -> bool { - if (!mGstVideoBoxes.contains(name)) { - setError(NonExistsError, "Camera name does not exist"); - return false; - } - mGstVideoBoxes.at(name).gstVideoWidget->pause(); - - clearError(); + auto* box = findVideoBox(name); + if (!box) return false; + box->gstVideoWidget->pause(); return true; } auto GstVideoGridWidget::stopVideo(std::string const& name) -> bool { - if (!mGstVideoBoxes.contains(name)) { - setError(NonExistsError, "Camera name does not exist"); - return false; - } - mGstVideoBoxes.at(name).gstVideoWidget->stop(); - - clearError(); + auto* box = findVideoBox(name); + if (!box) return false; + box->gstVideoWidget->stop(); return true; } auto GstVideoGridWidget::hideVideo(std::string const& name) -> bool { - if (!mGstVideoBoxes.contains(name)) { - setError(NonExistsError, "Camera name does not exist"); - return false; + auto* box = findVideoBox(name); + if (!box) return false; + + auto it = std::find(mVisibleOrder.begin(), mVisibleOrder.end(), name); + if (it != mVisibleOrder.end()) { + mVisibleOrder.erase(it); } - mGstVideoBoxes.at(name).widget->setVisible(false); - clearError(); + box->widget->setVisible(false); + rebuildGrid(); return true; } auto GstVideoGridWidget::showVideo(std::string const& name) -> bool { - if (!mGstVideoBoxes.contains(name)) { - setError(NonExistsError, "Camera name does not exist"); + auto* box = findVideoBox(name); + if (!box) return false; + + auto it = std::find(mVisibleOrder.begin(), mVisibleOrder.end(), name); + if (it == mVisibleOrder.end()) { + mVisibleOrder.push_back(name); + } + + rebuildGrid(); + return true; +} + +auto GstVideoGridWidget::moveCamera(std::string const& name, int newIndex) -> bool { + auto it = std::find(mVisibleOrder.begin(), mVisibleOrder.end(), name); + if (it == mVisibleOrder.end()) { + setError(NonExistsError, "Camera is not visible"); return false; } - mGstVideoBoxes.at(name).widget->setVisible(true); + int const maxIndex = static_cast(mVisibleOrder.size()) - 1; + int const clampedIndex = std::clamp(newIndex, 0, maxIndex); + + mVisibleOrder.erase(it); + mVisibleOrder.insert(mVisibleOrder.begin() + clampedIndex, name); + + rebuildGrid(); clearError(); return true; } @@ -167,3 +339,57 @@ auto GstVideoGridWidget::setError(Error error, std::string const& errorString) - mError = error; mErrorString = QString::fromStdString(errorString); } + +auto GstVideoGridWidget::getDropTargetIndex(QPoint const& pos) const -> int { + if (mVisibleOrder.empty()) { + return 0; + } + + auto it = mGstVideoBoxes.find(mVisibleOrder.front()); + if (it == mGstVideoBoxes.end()) { + return static_cast(mVisibleOrder.size()); + } + + QSize const cellSize = it->second.widget->size(); + int const spacing = mMainLayout->spacing(); + int const columns = calculateColumnCount(); + + int const col = pos.x() / (cellSize.width() + spacing); + int const row = pos.y() / (cellSize.height() + spacing); + int const index = row * columns + col; + + return std::clamp(index, 0, static_cast(mVisibleOrder.size())); +} + +void GstVideoGridWidget::dragEnterEvent(QDragEnterEvent* event) { + if (event->mimeData()->hasText()) { + std::string const cameraName = event->mimeData()->text().toStdString(); + if (mGstVideoBoxes.contains(cameraName)) { + event->acceptProposedAction(); + return; + } + } + event->ignore(); +} + +void GstVideoGridWidget::dragMoveEvent(QDragMoveEvent* event) { + if (event->mimeData()->hasText()) { + event->acceptProposedAction(); + } +} + +void GstVideoGridWidget::dropEvent(QDropEvent* event) { + if (!event->mimeData()->hasText()) { + event->ignore(); + return; + } + + std::string const cameraName = event->mimeData()->text().toStdString(); + int const targetIndex = getDropTargetIndex(event->pos()); + + if (moveCamera(cameraName, targetIndex)) { + event->acceptProposedAction(); + } else { + event->ignore(); + } +} diff --git a/teleoperation/camera_client/src/ImagePreview.cpp b/teleoperation/camera_client/src/ImagePreview.cpp new file mode 100644 index 00000000..f5ef92f3 --- /dev/null +++ b/teleoperation/camera_client/src/ImagePreview.cpp @@ -0,0 +1,49 @@ +#include "ImagePreview.hpp" + +namespace mrover { + + ImagePreview::ImagePreview(QWidget* parent) + : QMainWindow(parent), + mLabel(new QLabel(this)) { + + mLabel->setAlignment(Qt::AlignCenter); + mLabel->setMinimumSize(640, 480); + setCentralWidget(mLabel); + + auto* toolbar = addToolBar("Toolbar"); + + auto* saveAction = toolbar->addAction("Save"); + connect(saveAction, &QAction::triggered, this, &ImagePreview::saveImage); + + auto* saveAsAction = toolbar->addAction("Save As"); + connect(saveAsAction, &QAction::triggered, this, &ImagePreview::saveImageAs); + } + + auto ImagePreview::updateImage(QImage const& newImage) -> void { + mImage = newImage.copy(); + mLabel->setPixmap(QPixmap::fromImage(mImage)); + mLabel->resize(mImage.size()); + } + + void ImagePreview::saveImage() { + QString const downloads = QStandardPaths::writableLocation(QStandardPaths::DownloadLocation); + QString const timestamp = QDateTime::currentDateTime().toString("yyyyMMdd_HHmmss"); + QString const filename = downloads + "/image_" + timestamp + ".png"; + + if (!mImage.save(filename)) { + qWarning("Failed to save image!"); + } + } + + void ImagePreview::saveImageAs() { + QString const filename = QFileDialog::getSaveFileName( + this, "Save Image As", + QStandardPaths::writableLocation(QStandardPaths::DownloadLocation) + "/image.png", + "All Files (*);;PNG Images (*.png);;JPEG Images (*.jpg *.jpeg)"); + + if (!filename.isEmpty() && !mImage.save(filename)) { + qWarning("Failed to save image."); + } + } + +} // namespace mrover diff --git a/teleoperation/camera_client/src/VideoSelectorWidget.cpp b/teleoperation/camera_client/src/VideoSelectorWidget.cpp index a24388e8..3f4dbcee 100644 --- a/teleoperation/camera_client/src/VideoSelectorWidget.cpp +++ b/teleoperation/camera_client/src/VideoSelectorWidget.cpp @@ -1,109 +1,105 @@ #include "VideoSelectorWidget.hpp" -using namespace mrover; - -VideoSelectorWidget::VideoSelectorWidget(QWidget* parent) - : QWidget(parent) { - mSelectorsLayout = new QVBoxLayout(this); - mSelectorsLayout->setSpacing(10); - mSelectorsLayout->addStretch(); - setLayout(mSelectorsLayout); - - mUsingIcons = (QIcon::hasThemeIcon("pause-symbolic") && - QIcon::hasThemeIcon("play-symbolic") && - QIcon::hasThemeIcon("stop-symbolic") && - QIcon::hasThemeIcon("camera-photo-symbolic")); -} - -auto VideoSelectorWidget::addCamera(std::string const& name) -> void { - if (mSelectors.contains(name)) { - qDebug() << "Selector with name" << QString::fromStdString(name) << "already exists."; - return; +namespace mrover { + + VideoSelectorWidget::VideoSelectorWidget(QWidget* parent) + : QWidget(parent) { + mSelectorsLayout = new QVBoxLayout(this); + mSelectorsLayout->setSpacing(10); + mSelectorsLayout->addStretch(); + setLayout(mSelectorsLayout); + + mUsingIcons = (QIcon::hasThemeIcon("pause-symbolic") && + QIcon::hasThemeIcon("play-symbolic") && + QIcon::hasThemeIcon("stop-symbolic") && + QIcon::hasThemeIcon("camera-photo-symbolic")); } - Selector selector{}; - selector.widget = new QWidget(this); - selector.layout = new QHBoxLayout(selector.widget); - selector.widget->setLayout(selector.layout); - - selector.nameLabel = new QLabel(QString::fromStdString(name), selector.widget); - selector.layout->addWidget(selector.nameLabel); - selector.layout->addStretch(); - - mSelectors.emplace(name, selector); - - // Maintain bottom stretch to keep checkboxes top-aligned - mSelectorsLayout->takeAt(mSelectorsLayout->count() - 1); - mSelectorsLayout->addWidget(selector.widget); - mSelectorsLayout->addStretch(); -} - -auto VideoSelectorWidget::addVisibilitySelector(std::string const& cameraName, RequestCallback hideRequest, RequestCallback showRequest) -> void { - if (!mSelectors.contains(cameraName)) { - qDebug() << "Selector with name" << QString::fromStdString(cameraName) << "does not exist."; - return; - } - Selector& selector = mSelectors.at(cameraName); - if (mUsingIcons) { - selector.visibilityCheckBox = new CallbackCheckBox(QIcon::fromTheme("view-reveal-symbolic"), QIcon::fromTheme("view-conceal-symbolic"), selector.widget); - } else { - selector.visibilityCheckBox = new CallbackCheckBox("Show", "Hide", selector.widget); - } + auto VideoSelectorWidget::addCamera(std::string const& name, CameraCallbacks callbacks) -> void { + if (mSelectors.contains(name)) { + qDebug() << "Selector with name" << QString::fromStdString(name) << "already exists."; + return; + } - selector.visibilityCheckBox->setOnCheckCallback(std::move(showRequest)); - selector.visibilityCheckBox->setOnUncheckCallback(std::move(hideRequest)); - selector.visibilityCheckBox->setChecked(true); + Selector selector{}; + selector.widget = new QWidget(this); + selector.layout = new QHBoxLayout(selector.widget); + selector.widget->setLayout(selector.layout); - selector.layout->insertWidget(2, selector.visibilityCheckBox); -} + selector.nameLabel = new QLabel(QString::fromStdString(name), selector.widget); + selector.layout->addWidget(selector.nameLabel); + selector.layout->addStretch(); -auto VideoSelectorWidget::addMediaControls(std::string const& cameraName, RequestCallback pauseRequest, RequestCallback playRequest, RequestCallback stopRequest) -> void { - if (!mSelectors.contains(cameraName)) { - qDebug() << "Selector with name" << QString::fromStdString(cameraName) << "does not exist."; - return; - } - Selector& selector = mSelectors.at(cameraName); - - if (mUsingIcons) { - selector.playPauseCheckBox = new CallbackCheckBox(QIcon::fromTheme("media-playback-start-symbolic"), QIcon::fromTheme("media-playback-pause-symbolic"), selector.widget); - selector.stopButton = new QPushButton(QIcon{QIcon::fromTheme("stop-symbolic")}, QString(), selector.widget); - } else { - selector.playPauseCheckBox = new CallbackCheckBox("Play", "Pause", selector.widget); - selector.stopButton = new QPushButton("Stop", selector.widget); + createVisibilityCheckBox(selector, callbacks); + createMediaControls(selector, name, callbacks); + createScreenshotButton(selector, callbacks); + + mSelectors.emplace(name, selector); + + // bottom stretch to keep selectors top-aligned + mSelectorsLayout->takeAt(mSelectorsLayout->count() - 1); + mSelectorsLayout->addWidget(selector.widget); + mSelectorsLayout->addStretch(); } - selector.playPauseCheckBox->setOnCheckCallback(std::move(playRequest)); - selector.playPauseCheckBox->setOnUncheckCallback(std::move(pauseRequest)); - selector.playPauseCheckBox->setChecked(true); - - connect(selector.stopButton, &QPushButton::clicked, this, [this, cameraName, stopRequest]() { - if (this->mSelectors.contains(cameraName)) { - mSelectors.at(cameraName).playPauseCheckBox->setChecked(false); - stopRequest(); + + auto VideoSelectorWidget::createVisibilityCheckBox(Selector& selector, CameraCallbacks const& callbacks) -> void { + if (mUsingIcons) { + selector.visibilityCheckBox = new CallbackCheckBox( + QIcon::fromTheme("view-reveal-symbolic"), + QIcon::fromTheme("view-conceal-symbolic"), + selector.widget); + } else { + selector.visibilityCheckBox = new CallbackCheckBox("Show", "Hide", selector.widget); } - }); - selector.layout->addWidget(selector.playPauseCheckBox); - selector.layout->addWidget(selector.stopButton); -} + selector.visibilityCheckBox->setOnCheckCallback(callbacks.onShow); + selector.visibilityCheckBox->setOnUncheckCallback(callbacks.onHide); + selector.visibilityCheckBox->setChecked(true); -auto VideoSelectorWidget::addScreenshotButton(std::string const& cameraName, RequestCallback screenshotRequest) -> void { - if (!mSelectors.contains(cameraName)) { - qDebug() << "Selector with name" << QString::fromStdString(cameraName) << "does not exist."; - return; + selector.layout->addWidget(selector.visibilityCheckBox); } - Selector& selector = mSelectors.at(cameraName); - if (mUsingIcons) { - selector.screenshotButton = new QPushButton(QIcon{QIcon::fromTheme("camera-photo-symbolic")}, QString(), selector.widget); - } else { - selector.screenshotButton = new QPushButton("Screenshot", selector.widget); + auto VideoSelectorWidget::createMediaControls(Selector& selector, std::string const& cameraName, CameraCallbacks const& callbacks) -> void { + if (mUsingIcons) { + selector.playPauseCheckBox = new CallbackCheckBox( + QIcon::fromTheme("media-playback-start-symbolic"), + QIcon::fromTheme("media-playback-pause-symbolic"), + selector.widget); + selector.stopButton = new QPushButton(QIcon::fromTheme("stop-symbolic"), QString(), selector.widget); + } else { + selector.playPauseCheckBox = new CallbackCheckBox("Play", "Pause", selector.widget); + selector.stopButton = new QPushButton("Stop", selector.widget); + } + + selector.playPauseCheckBox->setOnCheckCallback(callbacks.onPlay); + selector.playPauseCheckBox->setOnUncheckCallback(callbacks.onPause); + selector.playPauseCheckBox->setChecked(true); + + auto stopCallback = callbacks.onStop; + connect(selector.stopButton, &QPushButton::clicked, this, [this, cameraName, stopCallback]() { + if (mSelectors.contains(cameraName)) { + mSelectors.at(cameraName).playPauseCheckBox->setChecked(false); + stopCallback(); + } + }); + + selector.layout->addWidget(selector.playPauseCheckBox); + selector.layout->addWidget(selector.stopButton); } - connect(selector.screenshotButton, &QPushButton::clicked, this, [this, cameraName, screenshotRequest]() { - if (this->mSelectors.contains(cameraName)) { - screenshotRequest(); + auto VideoSelectorWidget::createScreenshotButton(Selector& selector, CameraCallbacks const& callbacks) -> void { + if (mUsingIcons) { + selector.screenshotButton = new QPushButton(QIcon::fromTheme("camera-photo-symbolic"), QString(), selector.widget); + } else { + selector.screenshotButton = new QPushButton("Screenshot", selector.widget); } - }); - selector.layout->addWidget(selector.screenshotButton); -} + auto screenshotCallback = callbacks.onScreenshot; + connect(selector.screenshotButton, &QPushButton::clicked, this, [screenshotCallback]() { + screenshotCallback(); + }); + + selector.layout->addWidget(selector.screenshotButton); + } + +} // namespace mrover diff --git a/teleoperation/camera_client/src/main.cpp b/teleoperation/camera_client/src/main.cpp index dfd5e892..e48736d8 100644 --- a/teleoperation/camera_client/src/main.cpp +++ b/teleoperation/camera_client/src/main.cpp @@ -1,159 +1,130 @@ -#include - #include "pch.hpp" -#include "CameraClientMainWindow.hpp" -#include "ImagePreview.hpp" - -namespace mrover { - class CameraClientNode : public rclcpp::Node { - - std::shared_ptr mQtGui; - std::unordered_map::SharedPtr> mMediaControlClients; - std::unordered_map::SharedPtr> mImageCaptureClients; - std::unordered_map::SharedPtr> mImageCaptureSubscribers; - - auto imageCaptureCallback(std::string const& cameraName, sensor_msgs::msg::Image::ConstSharedPtr const& msg) { - RCLCPP_INFO(get_logger(), "Received image from camera"); - if (msg->encoding != sensor_msgs::image_encodings::BGR8) { - RCLCPP_ERROR(this->get_logger(), "Unsupported encoding - image capture must be BGR8"); - return; - } +#include + +#include +#undef Bool +#undef Status +#undef CursorShape +#undef None +#undef KeyPress +#undef KeyRelease +#undef FocusIn +#undef FocusOut +#undef FontChange +#undef Expose +#undef Unsorted - cv::Size receivedSize{static_cast(msg->width), static_cast(msg->height)}; - cv::Mat bgrFrame{receivedSize, CV_8UC3, const_cast(msg->data.data()), msg->step}; - - QImage qImg(bgrFrame.data, bgrFrame.cols, bgrFrame.rows, static_cast(bgrFrame.step), QImage::Format_BGR888); +#include "CameraClientMainWindow.hpp" +#include "CameraClientNode.hpp" - auto* imagePreview = new ImagePreview(); - imagePreview->updateImage(qImg); - imagePreview->setWindowTitle(QString::fromStdString(cameraName) + " - Screenshot"); - imagePreview->setAttribute(Qt::WA_DeleteOnClose); - imagePreview->show(); - } +namespace { + XErrorHandler gPreviousHandler = nullptr; - public: - explicit CameraClientNode(std::shared_ptr qtGui) : Node("camera_client"), mQtGui(std::move(qtGui)) { - RCLCPP_INFO(get_logger(), "Camera client initialized"); - - declare_parameter("cameras", rclcpp::ParameterType::PARAMETER_STRING_ARRAY); - auto cameraNames = get_parameter("cameras").as_string_array(); - - declare_parameter("rtp_jitter_ms", 100); - auto rtpJitterMs = std::chrono::milliseconds(get_parameter("rtp_jitter_ms").as_int()); - - for (auto const& cameraName: cameraNames) { - RCLCPP_INFO(get_logger(), "cameraName: %s", cameraName.c_str()); - - if (mMediaControlClients.contains(cameraName)) { - RCLCPP_WARN(get_logger(), "Camera %s already exists, skipping", cameraName.c_str()); - continue; - } - - declare_parameter(std::format("{}.port", cameraName), rclcpp::ParameterType::PARAMETER_INTEGER); - std::uint16_t const port = static_cast(this->get_parameter(std::format("{}.port", cameraName)).as_int()); - - declare_parameter(std::format("{}.stream.codec", cameraName), rclcpp::ParameterType::PARAMETER_STRING); - std::string const codec = this->get_parameter(std::format("{}.stream.codec", cameraName)).as_string(); - - std::string const pipeline = gst::video::createRtpToRawSrc(port, gst::video::getCodecFromStringView(codec), rtpJitterMs); - - mMediaControlClients.emplace(cameraName, create_client(std::format("{}_media_control", cameraName))); - mImageCaptureClients.emplace(cameraName, create_client(std::format("{}_image_capture", cameraName))); - mImageCaptureSubscribers.emplace(cameraName, create_subscription(std::format("{}_image", cameraName), 1, [this, cameraName](sensor_msgs::msg::Image::ConstSharedPtr const& msg) { - imageCaptureCallback(cameraName, msg); - })); - - RequestCallback pipelinePauseRequest = [this, cameraName]() { - qDebug() << "Pause request for camera" << cameraName.c_str(); - auto client = mMediaControlClients.find(cameraName); - if (client == mMediaControlClients.end()) { - RCLCPP_ERROR(get_logger(), "Camera %s not found", cameraName.c_str()); - return false; - } - auto request = std::make_shared(); - request->command = srv::MediaControl::Request::PAUSE; - auto result = client->second->async_send_request(request); - - // TODO:(owen) check result success - return true; - }; - - RequestCallback pipelinePlayRequest = [this, cameraName]() { - qDebug() << "Play request for camera" << cameraName.c_str(); - auto client = mMediaControlClients.find(cameraName); - if (client == mMediaControlClients.end()) { - RCLCPP_ERROR(get_logger(), "Camera %s not found", cameraName.c_str()); - return false; - } - auto request = std::make_shared(); - request->command = srv::MediaControl::Request::PLAY; - auto result = client->second->async_send_request(request); - - // TODO:(owen) check result success - return true; - }; - - RequestCallback pipelineStopRequest = [this, cameraName]() { - qDebug() << "Stop request for camera" << cameraName.c_str(); - auto client = mMediaControlClients.find(cameraName); - if (client == mMediaControlClients.end()) { - RCLCPP_ERROR(get_logger(), "Camera %s not found", cameraName.c_str()); - return false; - } - auto request = std::make_shared(); - request->command = srv::MediaControl::Request::STOP; - auto result = client->second->async_send_request(request); - - // TODO:(owen) check result success - return true; - }; - - RequestCallback screenshotRequest = [this, cameraName]() { - qDebug() << "Screenshot request for camera" << cameraName.c_str(); - auto client = mImageCaptureClients.find(cameraName); - if (client == mImageCaptureClients.end()) { - RCLCPP_ERROR(get_logger(), "Camera %s not found", cameraName.c_str()); - return false; - } - auto request = std::make_shared(); - auto result = client->second->async_send_request(request); - - // TODO:(owen) check result success - return true; - }; - - mQtGui->createCamera(cameraName, pipeline); - mQtGui->getCameraSelectorWidget()->addMediaControls(cameraName, std::move(pipelinePauseRequest), std::move(pipelinePlayRequest), std::move(pipelineStopRequest)); - mQtGui->getCameraSelectorWidget()->addScreenshotButton(cameraName, std::move(screenshotRequest)); - } - } - }; -} // namespace mrover + int filterXErrors(Display* dpy, XErrorEvent* event) { + if (event->error_code == BadWindow || event->error_code == BadDrawable) return 0; + if (gPreviousHandler) return gPreviousHandler(dpy, event); + return 0; + } +} // namespace auto main(int argc, char** argv) -> int { QApplication app(argc, argv); - auto qtGui = std::make_shared(); - qtGui->setWindowTitle("MRover Cameras"); - qtGui->setMinimumSize(1280, 720); - qtGui->show(); + + gPreviousHandler = XSetErrorHandler(filterXErrors); + gst_init(nullptr, nullptr); rclcpp::init(argc, argv); - auto node = std::make_shared(qtGui); + + auto mainWindow = std::make_shared(); + mainWindow->setWindowTitle("MRover Cameras"); + mainWindow->setMinimumSize(1280, 720); + + auto node = std::make_shared(); + + QObject::connect(node.get(), &mrover::CameraClientNode::cameraDiscovered, + mainWindow.get(), [mainWindow, node](mrover::CameraInfo const& info) { + std::string const& name = info.name; + + mrover::CameraCallbacks callbacks{ + .onHide = [mainWindow, name]() { + mainWindow->getCameraGridWidget()->hideVideo(name); + if (auto* widget = mainWindow->getCameraGridWidget()->getGstVideoWidget(name)) { + widget->stop(); + } + return true; }, + .onShow = [mainWindow, name]() { + mainWindow->getCameraGridWidget()->showVideo(name); + if (auto* widget = mainWindow->getCameraGridWidget()->getGstVideoWidget(name)) { + widget->play(); + } + return true; }, + .onPause = [node, name]() { return node->requestPause(name); }, + .onPlay = [node, name]() { return node->requestPlay(name); }, + .onStop = [node, name]() { return node->requestStop(name); }, + .onScreenshot = [node, name]() { return node->requestScreenshot(name); }, + }; + + if (name == "zed") { + auto* panel = mainWindow->getClickIkPanel(); + auto* videoWidget = new mrover::GstVideoWidget(); + videoWidget->setGstPipeline(info.pipeline); + videoWidget->setImageSize(1280, 720); + panel->placeZedWidget(videoWidget); + QObject::connect(panel, &mrover::ClickIkPanel::sample, panel, [nodePtr = node.get()]() { + nodePtr->sampleClickIk(); + }); + QObject::connect(videoWidget, &mrover::GstVideoWidget::clicked, + panel, [panel, videoWidget, nodePtr = node.get()](std::uint32_t x, std::uint32_t y) { + if (panel->canSendClick()) { + auto uiW = static_cast(videoWidget->width()); + auto uiH = static_cast(videoWidget->height()); + + float streamX = static_cast(x) / uiW; + float streamY = static_cast(y) / uiH; + panel->markRunning(); + nodePtr->sendClickIk(streamX, streamY); + } + }); + } else { + mainWindow->createCamera(name, info.pipeline, std::move(callbacks)); + } + }); + + QObject::connect(node.get(), &mrover::CameraClientNode::imageCaptured, + mainWindow.get(), &mrover::CameraClientMainWindow::showImagePreview); + + QObject::connect(node.get(), &mrover::CameraClientNode::clickIkFeedback, + mainWindow->getClickIkPanel(), &mrover::ClickIkPanel::updateFeedback); + QObject::connect(node.get(), &mrover::CameraClientNode::clickIkResult, + mainWindow->getClickIkPanel(), &mrover::ClickIkPanel::updateResult); + QObject::connect(node.get(), &mrover::CameraClientNode::ikImageSampleResult, + mainWindow->getClickIkPanel(), &mrover::ClickIkPanel::enableOverlayWidget); + + node->discoverCameras(); + + QObject::connect(mainWindow.get(), &mrover::CameraClientMainWindow::closed, []() { + rclcpp::shutdown(); + }); rclcpp::executors::MultiThreadedExecutor exec; exec.add_node(node); - QObject::connect(qtGui.get(), &mrover::CameraClientMainWindow::closed, [&]() { - rclcpp::shutdown(); + QTimer spinTimer; + QObject::connect(&spinTimer, &QTimer::timeout, [&exec]() { + if (rclcpp::ok()) { + exec.spin_some(); + } else { + QApplication::quit(); + } }); + spinTimer.start(10); // 10ms - while (rclcpp::ok()) { - exec.spin_some(); - app.processEvents(); - } + mainWindow->show(); + + int const result = QApplication::exec(); exec.remove_node(node); - return EXIT_SUCCESS; + return result; } diff --git a/x265_3.5-2_arm64.deb b/x265_3.5-2_arm64.deb new file mode 100644 index 00000000..e238cd69 --- /dev/null +++ b/x265_3.5-2_arm64.deb @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1ac0579846d0ef75ad110bb840711b14ccdddd30e0d85b2fea524bab89a4b58a +size 55966 diff --git "a/\264dv\314\330\005\375\323@\017\314\006\021\254\001\0224\003\022!\002\022\247\004\022e\003\022\307\022X\003\022,\003\022\273\004\022s\006\022\021" "b/\264dv\314\330\005\375\323@\017\314\006\021\254\001\0224\003\022!\002\022\247\004\022e\003\022\307\022X\003\022,\003\022\273\004\022s\006\022\021" new file mode 100644 index 00000000..e69de29b