diff --git a/CMakeLists.txt b/CMakeLists.txt index 7c68974f1..e02e2f0a7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -67,15 +67,19 @@ set(MROVER_ROS_DEPENDENCIES tf2 tf2_ros tf2_geometry_msgs + actionlib_msgs ) extract_filenames(msg/*.msg MROVER_MESSAGE_FILES) extract_filenames(srv/*.srv MROVER_SERVICE_FILES) +extract_filenames(action/*.action MROVER_ACTION_FILES) + set(MROVER_MESSAGE_DEPENDENCIES std_msgs sensor_msgs + actionlib_msgs ) set(MROVER_PARAMETERS @@ -98,7 +102,7 @@ if (ZED_FOUND) set(CMAKE_CUDA_STANDARD 17) set(CMAKE_CUDA_STANDARD_REQUIRED ON) set(CMAKE_CUDA_SEPARABLE_COMPILATION ON) - set(CMAKE_CUDA_FLAGS "--diag-suppress 108,68") + set(CMAKE_CUDA_FLAGS "--diag-suppress 20012") # Jetson Xavier NX/AGX has Volta 7.2 architecture # Perception Laptop (A4000, Quadro version of RTX 3080) has Ampere 8.6 architecture set(CMAKE_CUDA_ARCHITECTURES 72 86) @@ -123,6 +127,8 @@ add_message_files(FILES ${MROVER_MESSAGE_FILES}) add_service_files(FILES ${MROVER_SERVICE_FILES}) +add_action_files(DIRECTORY action FILES ${MROVER_ACTION_FILES}) + generate_messages(DEPENDENCIES ${MROVER_MESSAGE_DEPENDENCIES}) generate_dynamic_reconfigure_options(${MROVER_PARAMETERS}) @@ -197,9 +203,11 @@ if (CUDA_FOUND) # target_include_directories(nv_vid_codec_h265_enc SYSTEM PUBLIC deps/nvenc) endif () -mrover_add_nodelet(gst_websocket_streamer src/esw/gst_websocket_streamer/*.c* src/esw/gst_websocket_streamer src/esw/gst_websocket_streamer/pch.hpp) -mrover_nodelet_link_libraries(gst_websocket_streamer PRIVATE websocket_server ${Gst_LIBRARIES} ${GstApp_LIBRARIES}) -mrover_nodelet_include_directories(gst_websocket_streamer ${Gst_INCLUDE_DIRS} ${GstApp_INCLUDE_DIRS}) +if (Gst_FOUND AND GstApp_FOUND AND LibUsb_FOUND AND LibUdev_FOUND) + mrover_add_nodelet(gst_websocket_streamer src/esw/gst_websocket_streamer/*.c* src/esw/gst_websocket_streamer src/esw/gst_websocket_streamer/pch.hpp) + mrover_nodelet_link_libraries(gst_websocket_streamer PRIVATE websocket_server ${Gst_LIBRARIES} ${GstApp_LIBRARIES} ${LibUsb_LIBRARIES} ${LibUdev_LIBRARIES} opencv_core opencv_imgproc) + mrover_nodelet_include_directories(gst_websocket_streamer ${Gst_INCLUDE_DIRS} ${GstApp_INCLUDE_DIRS} ${LibUsb_INCLUDE_DIRS} ${LibUdev_INCLUDE_DIRS}) +endif () if (ZED_FOUND) mrover_add_nodelet(object_detector src/perception/object_detector/*.c* src/perception/object_detector src/perception/object_detector/pch.hpp) diff --git a/action/ArmAction.action b/action/ArmAction.action new file mode 100644 index 000000000..9cc4f5321 --- /dev/null +++ b/action/ArmAction.action @@ -0,0 +1,3 @@ +string name +--- +--- diff --git a/ansible/roles/build/files/profiles/ci/config.yaml b/ansible/roles/build/files/profiles/ci/config.yaml index d14216f40..c0346527b 100644 --- a/ansible/roles/build/files/profiles/ci/config.yaml +++ b/ansible/roles/build/files/profiles/ci/config.yaml @@ -5,9 +5,9 @@ catkin_make_args: [ ] cmake_args: - -DCMAKE_BUILD_TYPE=Release - -DCMAKE_C_COMPILER=clang-16 - - -DCMAKE_CXX_FLAGS=-pipe + - -DCMAKE_CXX_FLAGS=-pipe -fgnuc-version=13.1.0 - -DCMAKE_CXX_COMPILER=clang++-16 - - -DCMAKE_CUDA_HOST_COMPILER=clang++-16 + - -DCMAKE_CUDA_HOST_COMPILER=g++-9 - -DMROVER_IS_CI=ON - -Wno-dev devel_layout: linked diff --git a/ansible/roles/build/files/profiles/debug/config.yaml b/ansible/roles/build/files/profiles/debug/config.yaml index 71005790a..434ae5ae9 100644 --- a/ansible/roles/build/files/profiles/debug/config.yaml +++ b/ansible/roles/build/files/profiles/debug/config.yaml @@ -5,11 +5,11 @@ catkin_make_args: [ ] cmake_args: - -DCMAKE_BUILD_TYPE=Debug - -DCMAKE_C_COMPILER=clang-16 - - -DCMAKE_CXX_FLAGS=-pipe + - -DCMAKE_CXX_FLAGS=-pipe -fgnuc-version=13.1.0 - -DCMAKE_CXX_COMPILER=clang++-16 - -DCMAKE_CXX_COMPILER_LAUNCHER=ccache - -DCMAKE_CUDA_COMPILER_LAUNCHER=ccache - - -DCMAKE_CUDA_HOST_COMPILER=clang++-16 + - -DCMAKE_CUDA_HOST_COMPILER=g++-9 - -Wno-dev devel_layout: linked devel_space: devel diff --git a/ansible/roles/build/files/profiles/release/config.yaml b/ansible/roles/build/files/profiles/release/config.yaml index f0c29e538..ef35c5cb7 100644 --- a/ansible/roles/build/files/profiles/release/config.yaml +++ b/ansible/roles/build/files/profiles/release/config.yaml @@ -5,11 +5,11 @@ catkin_make_args: [ ] cmake_args: - -DCMAKE_BUILD_TYPE=Release - -DCMAKE_C_COMPILER=clang-16 - - -DCMAKE_CXX_FLAGS=-march=native -pipe + - -DCMAKE_CXX_FLAGS=-march=native -pipe -fgnuc-version=13.1.0 - -DCMAKE_CXX_COMPILER=clang++-16 - -DCMAKE_CXX_COMPILER_LAUNCHER=ccache - -DCMAKE_CUDA_COMPILER_LAUNCHER=ccache - - -DCMAKE_CUDA_HOST_COMPILER=clang++-16 + - -DCMAKE_CUDA_HOST_COMPILER=g++-9 - -DCMAKE_INTERPROCEDURAL_OPTIMIZATION=ON - -Wno-dev devel_layout: linked diff --git a/ansible/roles/build/tasks/main.yml b/ansible/roles/build/tasks/main.yml index 6d91a9651..c2c6e022f 100644 --- a/ansible/roles/build/tasks/main.yml +++ b/ansible/roles/build/tasks/main.yml @@ -97,6 +97,7 @@ - libgstreamer1.0-dev - libgstreamer-plugins-base1.0-dev - vainfo + - x264 - name: Install Local APT Packages become: True diff --git a/ansible/roles/esw/tasks/main.yml b/ansible/roles/esw/tasks/main.yml index f9a81d7a5..9ae41142b 100644 --- a/ansible/roles/esw/tasks/main.yml +++ b/ansible/roles/esw/tasks/main.yml @@ -22,4 +22,4 @@ - name: Install Moteus GUI pip: - name: moteus_gui + name: moteus-gui diff --git a/ansible/roles/jetson_services/files/rules/99-usb-serial.rules b/ansible/roles/jetson_services/files/rules/99-usb-serial.rules new file mode 100644 index 000000000..99fcb6258 --- /dev/null +++ b/ansible/roles/jetson_services/files/rules/99-usb-serial.rules @@ -0,0 +1,3 @@ +SUBSYSTEM=="tty", ATTRS{idVendor}=="1546", ATTRS{idProduct}=="01a9", SYMLINK+="gps_%n" +SUBSYSTEM=="tty", ATTRS{idVendor}=="2341", ATTRS{idProduct}=="0058", SYMLINK+="imu" +SUBSYSTEM=="video4linux", KERNEL=="video*", ATTRS{idVendor}=="1bcf", ATTRS{idProduct}=="0b12", ATTR{index}=="0", GROUP="video", SYMLINK+="arducam" diff --git a/ansible/roles/jetson_services/tasks/main.yml b/ansible/roles/jetson_services/tasks/main.yml index e69de29bb..74c048ea2 100644 --- a/ansible/roles/jetson_services/tasks/main.yml +++ b/ansible/roles/jetson_services/tasks/main.yml @@ -0,0 +1,14 @@ +- name: USB Rules + become: true + synchronize: + src: files/rules/ + dest: /etc/udev/rules.d + recursive: true + +- name: udev Reload Rules + become: true + command: udevadm control --reload-rules + +- name: udev TTY Trigger + become: true + command: udevadm trigger --subsystem-match=tty diff --git a/cmake/deps.cmake b/cmake/deps.cmake index b73637ce8..b3bb4a520 100644 --- a/cmake/deps.cmake +++ b/cmake/deps.cmake @@ -69,3 +69,5 @@ pkg_search_module(NetLink libnl-3.0 QUIET) pkg_search_module(NetLinkRoute libnl-route-3.0 QUIET) pkg_search_module(Gst gstreamer-1.0 QUIET) pkg_search_module(GstApp gstreamer-app-1.0 QUIET) +pkg_search_module(LibUsb libusb-1.0 QUIET) +pkg_search_module(LibUdev libudev QUIET) diff --git a/config/esw.yaml b/config/esw.yaml index f69c17812..14bbfb5ad 100644 --- a/config/esw.yaml +++ b/config/esw.yaml @@ -1,10 +1,10 @@ right_gps_driver: - port: "/dev/gps" + port: "/dev/ttyACM0" baud: 38400 frame_id: "base_link" left_gps_driver: - port: "/dev/ttyACM0" + port: "/dev/gps_0" baud: 38400 frame_id: "base_link" @@ -52,7 +52,7 @@ can: id: 0x15 - name: "joint_a" bus: 1 - id: 0x20 + id: 0x20 - name: "joint_b" bus: 1 id: 0x21 @@ -73,10 +73,10 @@ can: id: 0x26 - name: "mast_gimbal_y" bus: 3 - id: 0x28 + id: 0x27 - name: "mast_gimbal_z" bus: 3 - id: 0x27 + id: 0x28 - name: "sa_x" bus: 1 id: 0x29 @@ -131,10 +131,10 @@ brushless_motors: max_velocity: 20.0 max_torque: 5.0 joint_a: - # 1236.8475 rad/meter, or 196.850393476 rev/meter, and we want a max vel of 40 meter/s. + # 1236.8475 rad/meter, or 196.850393476 rev/meter, and we want a max vel of 40 meter/s. # gear ratio is currently at 0.005080 revolutions = 1 meter - min_velocity: -0.10 # this means -0.10 meters per second. - max_velocity: 0.10 + min_velocity: -0.05 # this means -0.10 meters per second. + max_velocity: 0.05 limit_0_present: true limit_1_present: true limit_0_enabled: true @@ -153,11 +153,11 @@ brushless_motors: max_backward_pos: 0.0 max_torque: 20.0 joint_c: - min_velocity: -0.08 # in terms of output - max_velocity: 0.08 # max output shaft speed: 5 rpm (for now) + min_velocity: -0.03 # in terms of output + max_velocity: 0.03 # max output shaft speed: 5 rpm (for now) min_position: -0.125 max_position: 0.30 # 220 degrees of motion is the entire range - max_torque: 20.0 + max_torque: 200.0 joint_de_0: min_velocity: -5.0 max_velocity: 5.0 @@ -604,4 +604,4 @@ auton_led_driver: baud: 115200 science: - shutoff_temp: 50.0f \ No newline at end of file + shutoff_temp: 50.0f diff --git a/config/localization.yaml b/config/localization.yaml index 55dc1b47a..698a69056 100644 --- a/config/localization.yaml +++ b/config/localization.yaml @@ -8,4 +8,4 @@ gps_linearization: reference_point_longitude: -83.7096706 reference_point_altitude: 234.1 reference_heading: 90.0 - use_both_gps: true + use_both_gps: false diff --git a/config/moteus/joint_c.cfg b/config/moteus/joint_c.cfg index c475f8708..761535fc1 100644 --- a/config/moteus/joint_c.cfg +++ b/config/moteus/joint_c.cfg @@ -1363,9 +1363,9 @@ servo.adc_cur_cycles 2 servo.adc_aux_cycles 47 servo.pid_dq.kp 0.060606 servo.pid_dq.ki 102.321388 -servo.pid_position.kp 14.000000 +servo.pid_position.kp 800.000000 servo.pid_position.ki 0.000000 -servo.pid_position.kd 1.450000 +servo.pid_position.kd 50.000000 servo.pid_position.iratelimit -1.000000 servo.pid_position.ilimit 0.000000 servo.pid_position.max_desired_rate 0.000000 diff --git a/config/teleop.yaml b/config/teleop.yaml index 8c05ad09a..1b8c68952 100644 --- a/config/teleop.yaml +++ b/config/teleop.yaml @@ -6,34 +6,24 @@ teleop: joint_a: multiplier: -1 slow_mode_multiplier: 0.5 - invert: True - xbox_index: 0 joint_b: multiplier: 1 slow_mode_multiplier: 1 - invert: False - xbox_index: 1 joint_c: - multiplier: -1 + multiplier: 1 slow_mode_multiplier: 0.6 - invert: False - xbox_index: 3 joint_de_pitch: - multiplier: 0.5 + multiplier: -1 slow_mode_multiplier: 1 - invert: True joint_de_roll: - multiplier: -0.5 + multiplier: 1 slow_mode_multiplier: 1 - invert: True allen_key: multiplier: 1 slow_mode_multiplier: 1 - invert: False gripper: multiplier: 1 slow_mode_multiplier: 1 - invert: True sa_controls: sa_x: @@ -67,21 +57,32 @@ teleop: enabled: true multiplier: 1 twist: - multiplier: 0.7 + multiplier: 0.6 + tilt: + multiplier: 1 xbox_mappings: - left_js_x: 0 - left_js_y: 1 - right_js_x: 2 - right_js_y: 3 - left_trigger: 6 - right_trigger: 7 + left_x: 0 + left_y: 1 + right_x: 2 + right_y: 3 a: 0 b: 1 x: 2 y: 3 left_bumper: 4 right_bumper: 5 + left_trigger: 6 + right_trigger: 7 + back: 8 + forward: 9 + left_stick_click: 10 + right_stick_click: 11 + dpad_up: 12 + dpad_down: 13 + dpad_left: 14 + dpad_right: 15 + home: 16 joystick_mappings: left_right: 0 @@ -119,6 +120,6 @@ teleop: bar: ["/tf_static", "/rosout", "/tf"] ik_multipliers: - x: 1 - y: 1 - z: 1 \ No newline at end of file + x: 0.1 + y: 0.1 + z: 0.1 \ No newline at end of file diff --git a/launch/basestation.launch b/launch/basestation.launch index b5039bcbe..10679cc71 100644 --- a/launch/basestation.launch +++ b/launch/basestation.launch @@ -4,14 +4,18 @@ + + + - - - + + + + diff --git a/launch/esw_base.launch b/launch/esw_base.launch index e2ccf7d2d..623a5f7cf 100644 --- a/launch/esw_base.launch +++ b/launch/esw_base.launch @@ -1,77 +1,67 @@ - + - - - - - - - - - + + + + + + + + + - + + + + + + - - - - - - + + + + + + - - - - - - + + + + + + - + + + + + + - + - - - - - + + + + + - + diff --git a/launch/localization.launch b/launch/localization.launch index 33acfa8ff..45803a5a9 100644 --- a/launch/localization.launch +++ b/launch/localization.launch @@ -2,7 +2,7 @@ - + diff --git a/launch/simulator.launch b/launch/simulator.launch index 23230736f..6c3e96e33 100644 --- a/launch/simulator.launch +++ b/launch/simulator.launch @@ -17,6 +17,11 @@ + + + + diff --git a/package.xml b/package.xml index 5a42eb6a6..9ab172f38 100644 --- a/package.xml +++ b/package.xml @@ -64,6 +64,7 @@ rviz_imu_plugin robot_localization + rtcm_msgs diff --git a/scripts/debug_arm_ik.py b/scripts/debug_arm_ik.py index 4757343cc..8fb2e95e4 100755 --- a/scripts/debug_arm_ik.py +++ b/scripts/debug_arm_ik.py @@ -1,7 +1,8 @@ #!/usr/bin/env python3 import rospy -from geometry_msgs.msg import Pose, Quaternion, Point, PointStamped +from geometry_msgs.msg import Pose, PoseStamped, Quaternion, Point, PointStamped +from std_msgs.msg import Header from mrover.msg import IK if __name__ == "__main__": @@ -13,18 +14,24 @@ pub.publish( IK( - pose=Pose( - position=Point(x=0.5, y=0.5, z=0.5), - orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + target=PoseStamped( + header=Header(stamp=rospy.Time.now(), frame_id="base_link"), + pose=Pose( + position=Point(x=0.8, y=1.0, z=0.5), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) ) ) def on_clicked_point(clicked_point: PointStamped): ik = IK( - pose=Pose( - position=clicked_point.point, - orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + target=PoseStamped( + header=Header(stamp=rospy.Time.now(), frame_id="base_link"), + pose=Pose( + position=clicked_point.point, + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) ) pub.publish(ik) diff --git a/setup.py b/setup.py index 48ba751d1..e5807a321 100644 --- a/setup.py +++ b/setup.py @@ -13,6 +13,7 @@ "navigation.failure_identification", "esw", "teleoperation.teleoperation", + "teleoperation.arm_controller", ], package_dir={"": "src"}, ) diff --git a/src/esw/can_driver/can_driver.cpp b/src/esw/can_driver/can_driver.cpp index 7b9aa81c2..6355105f8 100644 --- a/src/esw/can_driver/can_driver.cpp +++ b/src/esw/can_driver/can_driver.cpp @@ -69,7 +69,7 @@ namespace mrover { } } else { NODELET_WARN("No CAN devices specified or config was invalid. Did you forget to load the correct ROS parameters?"); - NODELET_WARN("For example before testing the devboard run: \"rosparam load config/esw_devboard.yml\""); + NODELET_WARN("For example before testing the devboard run: \"rosparam load config/esw.yml\""); } mCanNetLink = CanNetLink{mInterface}; @@ -201,7 +201,7 @@ namespace mrover { } catch (boost::system::system_error const& error) { // check if ran out of buffer space if (error.code() == boost::asio::error::no_buffer_space) { - NODELET_WARN_STREAM_THROTTLE(1, "No buffer space available to send CAN message"); + NODELET_WARN_STREAM_THROTTLE(1, "No buffer space available to send CAN message. This usually indicates an electrical problem with the bus. CAN will avoid sending out messages if it can not see other devices."); return; } ros::shutdown(); diff --git a/src/esw/drive_bridge/main.cpp b/src/esw/drive_bridge/main.cpp index 9e836e462..afa2092ec 100644 --- a/src/esw/drive_bridge/main.cpp +++ b/src/esw/drive_bridge/main.cpp @@ -43,7 +43,8 @@ auto main(int argc, char** argv) -> int { } void moveDrive(geometry_msgs::Twist::ConstPtr const& msg) { - // See 11.5.1 in "Controls Engineering in the FIRST Robotics Competition" for the math + // See 13.3.1.4 in "Modern Robotics" for the math + // Link: https://hades.mech.northwestern.edu/images/7/7f/MR.pdf auto forward = MetersPerSecond{msg->linear.x}; auto turn = RadiansPerSecond{msg->angular.z}; // TODO(quintin) Don't ask me to explain perfectly why we need to cancel out a meters unit in the numerator diff --git a/src/esw/gst_websocket_streamer/gst_websocket_streamer.cpp b/src/esw/gst_websocket_streamer/gst_websocket_streamer.cpp index a1430549b..1033a1cc6 100644 --- a/src/esw/gst_websocket_streamer/gst_websocket_streamer.cpp +++ b/src/esw/gst_websocket_streamer/gst_websocket_streamer.cpp @@ -1,13 +1,9 @@ #include "gst_websocket_streamer.hpp" -#if defined(MROVER_IS_JETSON) -constexpr bool IS_JETSON = true; -#else -constexpr bool IS_JETSON = false; -#endif - namespace mrover { + using namespace std::string_view_literals; + template auto gstCheck(T* t) -> T* { if (!t) throw std::runtime_error{"Failed to create"}; @@ -26,7 +22,13 @@ namespace mrover { GstBuffer* buffer = gst_sample_get_buffer(sample); GstMapInfo map; gst_buffer_map(buffer, &map, GST_MAP_READ); - mStreamServer->broadcast({reinterpret_cast(map.data), map.size}); + + // Prefix the encoded chunk with metadata for the decoder on the browser side + std::vector chunk(sizeof(ChunkHeader) + map.size); + std::memcpy(chunk.data(), &mChunkHeader, sizeof(ChunkHeader)); + std::memcpy(chunk.data() + sizeof(ChunkHeader), map.data, map.size); + + mStreamServer->broadcast(chunk); gst_buffer_unmap(buffer, &map); gst_sample_unref(sample); @@ -38,70 +40,68 @@ namespace mrover { mMainLoop = gstCheck(g_main_loop_new(nullptr, FALSE)); + // Source std::string launch; - if (mCaptureDevice.empty()) { - if constexpr (IS_JETSON) { - // ReSharper disable once CppDFAUnreachableCode - launch = std::format( - // App source is pushed to when we get a ROS BGRA image message - // is-live prevents frames from being pushed when the pipeline is in READY - "appsrc name=imageSource is-live=true " - "! video/x-raw,format=BGRA,width={},height={},framerate=30/1 " - "! videoconvert " // Convert BGRA => I420 (YUV) for the encoder, note we are still on the CPU - "! video/x-raw,format=I420 " - "! nvvidconv " // Upload to GPU memory for the encoder - "! video/x-raw(memory:NVMM),format=I420 " - "! nvv4l2h265enc name=encoder bitrate={} iframeinterval=300 vbv-size=33333 insert-sps-pps=true control-rate=constant_bitrate profile=Main num-B-Frames=0 ratecontrol-enable=true preset-level=UltraFastPreset EnableTwopassCBR=false maxperf-enable=true " - // App sink is pulled from (getting H265 chunks) on another thread and sent to the stream server - // sync=false is needed to avoid weirdness, going from playing => ready => playing will not work otherwise - "! appsink name=streamSink sync=false", - mImageWidth, mImageHeight, mBitrate); - } else { - // ReSharper disable once CppDFAUnreachableCode - launch = std::format( - "appsrc name=imageSource is-live=true " - "! video/x-raw,format=BGRA,width={},height={},framerate=30/1 " - "! nvh265enc name=encoder " - "! appsink name=streamSink sync=false", - mImageWidth, mImageHeight); - } + if (mDeviceNode.empty()) { + // App source is pushed to when we get a ROS BGRA image message + // is-live prevents frames from being pushed when the pipeline is in READY + launch += "appsrc name=imageSource is-live=true "; } else { - if constexpr (IS_JETSON) { - // ReSharper disable once CppDFAUnreachableCode - - // 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) - - launch = std::format( + launch += std::format("v4l2src device={} ", mDeviceNode); + } + // Source format + std::string captureFormat = mDeviceNode.empty() ? "video/x-raw,format=BGRA" : mDecodeJpegFromDevice ? "image/jpeg" + : "video/x-raw,format=YUY2"; + launch += std::format("! {},width={},height={},framerate={}/1 ", + captureFormat, mImageWidth, mImageHeight, mImageFramerate); + // Source decoder and H265 encoder + if (gst_element_factory_find("nvv4l2h265enc")) { + // Most likely on the Jetson + if (captureFormat.contains("jpeg")) { + // Mostly used with USB cameras, MPEG capture uses way less USB bandwidth + launch += + // 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) // "nvv4l2camerasrc device={} " - - "v4l2src device={} " - // "! video/x-raw(memory:NVMM),format=YUY2,width={},height={},framerate={}/1 " - "! image/jpeg,width={},height={},framerate={}/1 " - "! nvv4l2decoder mjpeg=1 " - - "! nvvidconv " - "! video/x-raw(memory:NVMM),format=NV12 " - "! nvv4l2h265enc name=encoder bitrate={} iframeinterval=300 vbv-size=33333 insert-sps-pps=true control-rate=constant_bitrate profile=Main num-B-Frames=0 ratecontrol-enable=true preset-level=UltraFastPreset EnableTwopassCBR=false maxperf-enable=true " - "! appsink name=streamSink sync=false", - mCaptureDevice, mImageWidth, mImageHeight, mImageFramerate, mBitrate); + + "! nvv4l2decoder mjpeg=1 " // Hardware-accelerated JPEG decoding, output is apparently some unknown proprietary format + "! nvvidconv " // Convert from proprietary format to NV12 so the encoder understands it + "! video/x-raw(memory:NVMM),format=NV12 "; } else { - // ReSharper disable once CppDFAUnreachableCode - launch = std::format( - "v4l2src device={} " - "! video/x-raw,format=YUY2,width={},height={},framerate=30/1 " - "! videoconvert " - "! nvh265enc name=encoder " - "! appsink name=streamSink sync=false", - mCaptureDevice, mImageWidth, mImageHeight); + launch += "! videoconvert " // Convert to I420 for the encoder, note we are still on the CPU + "! video/x-raw,format=I420 " + "! nvvidconv " // Upload to GPU memory for the encoder + "! video/x-raw(memory:NVMM),format=I420 "; } + launch += std::format("! nvv4l2h265enc name=encoder bitrate={} iframeinterval=300 vbv-size=33333 insert-sps-pps=true control-rate=constant_bitrate profile=Main num-B-Frames=0 ratecontrol-enable=true preset-level=UltraFastPreset EnableTwopassCBR=false maxperf-enable=true ", + mBitrate); + mChunkHeader.codec = ChunkHeader::Codec::H265; + } else if (gst_element_factory_find("nvh265enc")) { + // For desktop/laptops with the custom NVIDIA bad gstreamer plugins built (a massive pain to do!) + if (captureFormat.contains("jpeg")) + launch += "! jpegdec "; + else + launch += "! videoconvert "; + launch += "! nvh265enc name=encoder "; + mChunkHeader.codec = ChunkHeader::Codec::H265; + } else { + // For desktop/laptops with no hardware encoder + if (captureFormat.contains("jpeg")) + launch += "! jpegdec "; + else + launch += "! videoconvert "; + launch += std::format("! x264enc tune=zerolatency bitrate={} name=encoder ", mBitrate); + mChunkHeader.codec = ChunkHeader::Codec::H264; } + // App sink is pulled from (getting H265 chunks) on another thread and sent to the stream server + // sync=false is needed to avoid weirdness, going from playing => ready => playing will not work otherwise + launch += "! appsink name=streamSink sync=false"; ROS_INFO_STREAM(std::format("GStreamer launch: {}", launch)); mPipeline = gstCheck(gst_parse_launch(launch.c_str(), nullptr)); - if (mCaptureDevice.empty()) mImageSource = gstCheck(gst_bin_get_by_name(GST_BIN(mPipeline), "imageSource")); + if (mDeviceNode.empty()) mImageSource = gstCheck(gst_bin_get_by_name(GST_BIN(mPipeline), "imageSource")); mStreamSink = gstCheck(gst_bin_get_by_name(GST_BIN(mPipeline), "streamSink")); if (gst_element_set_state(mPipeline, GST_STATE_PAUSED) == GST_STATE_CHANGE_FAILURE) @@ -127,14 +127,22 @@ namespace mrover { if (mStreamServer->clientCount() == 0) return; if (msg->encoding != sensor_msgs::image_encodings::BGRA8) throw std::runtime_error{"Unsupported encoding"}; - if (msg->width != mImageWidth || msg->height != mImageHeight) throw std::runtime_error{"Unsupported resolution"}; + + cv::Size receivedSize{static_cast(msg->width), static_cast(msg->height)}; + cv::Mat bgraFrame{receivedSize, CV_8UC4, const_cast(msg->data.data()), msg->step}; + + if (cv::Size targetSize{static_cast(mImageWidth), static_cast(mImageHeight)}; + receivedSize != targetSize) { + ROS_WARN_ONCE("Image size does not match pipeline app source size, will resize"); + resize(bgraFrame, bgraFrame, targetSize); + } // "step" is the number of bytes (NOT pixels) in an image row - std::size_t size = msg->step * msg->height; + std::size_t size = bgraFrame.step * bgraFrame.rows; GstBuffer* buffer = gstCheck(gst_buffer_new_allocate(nullptr, size, nullptr)); GstMapInfo info; gst_buffer_map(buffer, &info, GST_MAP_WRITE); - std::memcpy(info.data, msg->data.data(), size); + std::memcpy(info.data, bgraFrame.data, size); gst_buffer_unmap(buffer, &info); gst_app_src_push_buffer(GST_APP_SRC(mImageSource), buffer); @@ -145,12 +153,56 @@ namespace mrover { } } + auto widthAndHeightToResolution(std::uint32_t width, std::uint32_t height) -> ChunkHeader::Resolution { + if (width == 640 && height == 480) return ChunkHeader::Resolution::EGA; + if (width == 1280 && height == 720) return ChunkHeader::Resolution::HD; + if (width == 1920 && height == 1080) return ChunkHeader::Resolution::FHD; + throw std::runtime_error{"Unsupported resolution"}; + } + + auto findDeviceNode(std::string_view devicePath) -> std::string { + udev* udevContext = udev_new(); + if (!udevContext) throw std::runtime_error{"Failed to initialize udev"}; + + udev_enumerate* enumerate = udev_enumerate_new(udevContext); + if (!enumerate) throw std::runtime_error{"Failed to create udev enumeration"}; + + udev_enumerate_add_match_subsystem(enumerate, "video4linux"); + udev_enumerate_scan_devices(enumerate); + + udev_list_entry* devices = udev_enumerate_get_list_entry(enumerate); + if (!devices) throw std::runtime_error{"Failed to get udev device list"}; + + udev_device* device{}; + udev_list_entry* entry; + udev_list_entry_foreach(entry, devices) { + device = udev_device_new_from_syspath(udevContext, udev_list_entry_get_name(entry)); + + if (udev_device_get_devpath(device) != devicePath) continue; + + if (!device) throw std::runtime_error{"Failed to get udev device"}; + + break; + } + if (!device) throw std::runtime_error{"Failed to find udev device"}; + + std::string deviceNode = udev_device_get_devnode(device); + + udev_device_unref(device); + udev_enumerate_unref(enumerate); + udev_unref(udevContext); + + return deviceNode; + } + auto GstWebsocketStreamerNodelet::onInit() -> void { try { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); - mCaptureDevice = mPnh.param("device", ""); + mDeviceNode = mPnh.param("dev_node", ""); + mDevicePath = mPnh.param("dev_path", ""); + mDecodeJpegFromDevice = mPnh.param("decode_jpeg_from_device", true); mImageTopic = mPnh.param("image_topic", "image"); mImageWidth = mPnh.param("width", 640); mImageHeight = mPnh.param("height", 480); @@ -159,6 +211,10 @@ namespace mrover { auto port = mPnh.param("port", 8081); mBitrate = mPnh.param("bitrate", 2000000); + mChunkHeader.resolution = widthAndHeightToResolution(mImageWidth, mImageHeight); + + if (!mDevicePath.empty()) mDeviceNode = findDeviceNode(mDevicePath); + gst_init(nullptr, nullptr); initPipeline(); @@ -193,12 +249,12 @@ namespace mrover { ROS_INFO("Stopped GStreamer pipeline"); }); - if (mCaptureDevice.empty()) { + if (mDeviceNode.empty()) { mImageSubscriber = mNh.subscribe(mImageTopic, 1, &GstWebsocketStreamerNodelet::imageCallback, this); } } catch (std::exception const& e) { - ROS_ERROR_STREAM(std::format("Exception initializing NVIDIA GST H265 Encoder: {}", e.what())); + ROS_ERROR_STREAM(std::format("Exception initializing gstreamer websocket streamer: {}", e.what())); ros::requestShutdown(); } } diff --git a/src/esw/gst_websocket_streamer/gst_websocket_streamer.hpp b/src/esw/gst_websocket_streamer/gst_websocket_streamer.hpp index c4950f62c..69520febe 100644 --- a/src/esw/gst_websocket_streamer/gst_websocket_streamer.hpp +++ b/src/esw/gst_websocket_streamer/gst_websocket_streamer.hpp @@ -2,14 +2,42 @@ #include "pch.hpp" +// Uses gstreamer to encode and stream video over a websocket +// The input can be a ROS BGRA image topic or a USB device +// Hardware accelerated is used when possible (with the Jetson or NVIDIA GPUs) +// Run "export GST_DEBUG=2" to debug gstreamer issues + namespace mrover { + struct ChunkHeader { + enum struct Resolution : std::uint8_t { + EGA, // 640x480 + HD, // 1280x720 + FHD, // 1920x1080 + } resolution; + enum struct Codec : std::uint8_t { + H265, + H264, + } codec; + }; + class GstWebsocketStreamerNodelet final : public nodelet::Nodelet { ros::NodeHandle mNh, mPnh; - std::string mCaptureDevice; + // For example, /dev/video0 + // These device paths are not garunteed to stay the same between reboots + // Prefer sys path for non-debugging purposes + std::string mDeviceNode; + bool mDecodeJpegFromDevice{}; // Uses less USB hub bandwidth, which is limited since we are using 2.0 std::string mImageTopic; + // To find the sys path: + // 1) Disconnect all cameras + // 2) Confirm there are no /dev/video* devices + // 2) Connect the camera you want to use + // 3) Run "ls /dev/video*" to verify the device is connected + // 4) Run "udevadm info -q path -n /dev/video0" to get the sys path + std::string mDevicePath; std::uint64_t mBitrate{}; std::uint32_t mImageWidth{}, mImageHeight{}, mImageFramerate{}; @@ -22,6 +50,8 @@ namespace mrover { std::thread mMainLoopThread; std::thread mStreamSinkThread; + ChunkHeader mChunkHeader{}; + auto onInit() -> void override; auto pullStreamSamplesLoop() -> void; diff --git a/src/esw/gst_websocket_streamer/pch.hpp b/src/esw/gst_websocket_streamer/pch.hpp index 2c224a3ec..2769438ad 100644 --- a/src/esw/gst_websocket_streamer/pch.hpp +++ b/src/esw/gst_websocket_streamer/pch.hpp @@ -1,20 +1,26 @@ #pragma once -#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/src/localization/gps_driver.py b/src/localization/gps_driver.py old mode 100755 new mode 100644 diff --git a/src/navigation/approach_object.py b/src/navigation/approach_object.py index 694840cdf..b4ab42742 100644 --- a/src/navigation/approach_object.py +++ b/src/navigation/approach_object.py @@ -25,6 +25,7 @@ def on_exit(self, context): pass def get_target_pos(self, context) -> Optional[np.ndarray]: + # either position or None object_pos = context.env.current_target_pos() return object_pos diff --git a/src/navigation/approach_target_base.py b/src/navigation/approach_target_base.py index 5c763149b..74959d765 100644 --- a/src/navigation/approach_target_base.py +++ b/src/navigation/approach_target_base.py @@ -6,11 +6,12 @@ from typing import Optional import numpy as np -from navigation import search +from navigation import search, waypoint class ApproachTargetBaseState(State): STOP_THRESH = get_rosparam("single_fiducial/stop_thresh", 0.7) + STOP_THRESH_WAYPOINT = get_rosparam("waypoint/stop_thresh", 0.5) FIDUCIAL_STOP_THRESHOLD = get_rosparam("single_fiducial/fiducial_stop_threshold", 1.75) DRIVE_FWD_THRESH = get_rosparam("waypoint/drive_fwd_thresh", 0.34) # 20 degrees @@ -37,6 +38,8 @@ def on_loop(self, context) -> State: """ target_pos = self.get_target_pos(context) if target_pos is None: + if self.__class__.__name__ == "LongRangeState" and not context.env.arrived_at_waypoint: + return waypoint.WaypointState() return search.SearchState() try: diff --git a/src/navigation/context.py b/src/navigation/context.py index e04f226a4..3face5797 100644 --- a/src/navigation/context.py +++ b/src/navigation/context.py @@ -79,6 +79,7 @@ class Environment: long_range_tags: LongRangeTagStore NO_FIDUCIAL: ClassVar[int] = -1 arrived_at_target: bool = False + arrived_at_waypoint: bool = False last_target_location: Optional[np.ndarray] = None def get_target_pos(self, id: str, in_odom_frame: bool = True) -> Optional[np.ndarray]: @@ -106,7 +107,7 @@ def get_target_pos(self, id: str, in_odom_frame: bool = True) -> Optional[np.nda def current_target_pos(self, odom_override: bool = True) -> Optional[np.ndarray]: """ Retrieves the position of the current fiducial or object (and we are looking for it) - :param: odom_override if false will force it to be in the map frame + :param odom_override: if false will force it to be in the map frame """ assert self.ctx.course in_odom = self.ctx.use_odom and odom_override @@ -119,13 +120,17 @@ def current_target_pos(self, odom_override: bool = True) -> Optional[np.ndarray] return self.get_target_pos(f"fiducial{current_waypoint.tag_id}", in_odom) elif current_waypoint.type.val == WaypointType.MALLET: return self.get_target_pos("hammer", in_odom) - elif current_waypoint.type == WaypointType.WATER_BOTTLE: + elif current_waypoint.type.val == WaypointType.WATER_BOTTLE: return self.get_target_pos("bottle", in_odom) else: return None class LongRangeTagStore: + """ + Context class to represent the tags seen in the long range camera + """ + @dataclass class TagData: hit_count: int @@ -144,32 +149,54 @@ def __init__(self, ctx: Context, min_hits: int = MIN_HITS, max_hits: int = MAX_H self.max_hits = max_hits def push_frame(self, tags: List[LongRangeTag]) -> None: + """ + Loops through our current list of our stored tags and checks if the new message includes each tag or doesn't. + If it does include it, we will increment our hit count for that tag id, store the new tag information, and reset the time we saw it. + If it does not include it, we will decrement our hit count for that tag id, and if the hit count becomes zero, then we remove it from our stored list. + If there are tag ids in the new message that we don't have stored, we will add it to our stored list. + :param tags: a list of LongRangeTags sent by perception, which includes an id and bearing for each tag in the list + """ + # Update our current tags + tags_ids = [tag.id for tag in tags] for _, cur_tag in list(self.__data.items()): - tags_ids = [tag.id for tag in tags] + # if we don't see one of our tags in the new message, decrement its hit count if cur_tag.tag.id not in tags_ids: cur_tag.hit_count -= DECREMENT_WEIGHT if cur_tag.hit_count <= 0: - del self.__data[cur_tag.tag.id] + cur_tag.hit_count = 0 + # if we haven't seen the tag in a while, remove it from our list + time_difference = rospy.get_time() - cur_tag.time + if time_difference > LONG_RANGE_TAG_EXPIRATION_TIME_SECONDS: + del self.__data[cur_tag.tag.id] + # if we do see one of our tags in the new message, increment its hit count else: cur_tag.hit_count += INCREMENT_WEIGHT - cur_tag.time = rospy.get_time() if cur_tag.hit_count > self.max_hits: cur_tag.hit_count = self.max_hits + # Add newly seen tags to our data structure and update current tag's information for tag in tags: if tag.id not in self.__data: self.__data[tag.id] = self.TagData(hit_count=INCREMENT_WEIGHT, tag=tag, time=rospy.get_time()) + else: + self.__data[tag.id].tag = tag + self.__data[tag.id].time = rospy.get_time() def get(self, tag_id: int) -> Optional[LongRangeTag]: + """ + Returns the corresponding tag if the tag has been seen by the long range camera enough times recently + :param tag_id: id corresponding to the tag we want to return + :return: LongRangeTag if we have seen the tag enough times recently in the long range camera, otherwise return None + """ if len(self.__data) == 0: return None if tag_id not in self.__data: return None time_difference = rospy.get_time() - self.__data[tag_id].time if ( - tag_id in self.__data - and self.__data[tag_id].hit_count >= self.min_hits - and time_difference <= LONG_RANGE_TAG_EXPIRATION_TIME_SECONDS + # self.__data[tag_id].hit_count >= self.min_hits and + time_difference + <= LONG_RANGE_TAG_EXPIRATION_TIME_SECONDS ): return self.__data[tag_id].tag else: @@ -203,7 +230,7 @@ def current_waypoint(self) -> Optional[Waypoint]: """ Returns the currently active waypoint - :return: Next waypoint to reach if we have an active course + :return: Next waypoint to reach if we have an active course """ if self.course_data is None or self.waypoint_index >= len(self.course_data.waypoints): return None @@ -234,18 +261,18 @@ def look_for_object(self) -> bool: def is_complete(self) -> bool: return self.waypoint_index == len(self.course_data.waypoints) - def check_approach(self) -> Optional[State]: + def get_approach_target_state(self) -> Optional[State]: """ Returns one of the approach states (ApproachPostState, LongRangeState, or ApproachObjectState) if we are looking for a post or object and we see it in one of the cameras (ZED or long range) """ + current_waypoint = self.current_waypoint() if self.look_for_post(): - current_waypoint = self.current_waypoint() - assert current_waypoint is not None # if we see the tag in the ZED, go to ApproachPostState if self.ctx.env.current_target_pos() is not None: return approach_post.ApproachPostState() # if we see the tag in the long range camera, go to LongRangeState + assert current_waypoint is not None if self.ctx.env.long_range_tags.get(current_waypoint.tag_id) is not None: return long_range.LongRangeState() elif self.look_for_object(): diff --git a/src/navigation/long_range.py b/src/navigation/long_range.py index 554c47490..669acde75 100644 --- a/src/navigation/long_range.py +++ b/src/navigation/long_range.py @@ -42,6 +42,10 @@ def get_target_pos(self, context) -> Optional[np.ndarray]: rover_direction = pose.rotation.direction_vector() bearing_to_tag = np.radians(tag.bearing) + # If you have not seen the tag in a while but are waiting until the expiration time is up, + # keep going towards where it was last seen (the direction you are heading), don't use an old bearing value + if context.env.long_range_tags._LongRangeTagStore__data[tag_id].hit_count <= 0: + bearing_to_tag = 0 # rover_direction rotated by bearing to tag bearing_rotation_mat = np.array( diff --git a/src/navigation/navigation.py b/src/navigation/navigation.py index 5cb574e71..a97cb1ec5 100755 --- a/src/navigation/navigation.py +++ b/src/navigation/navigation.py @@ -60,7 +60,9 @@ def __init__(self, context: Context): ], ) self.state_machine.add_transitions(ApproachObjectState(), [DoneState(), SearchState(), RecoveryState()]) - self.state_machine.add_transitions(LongRangeState(), [ApproachPostState(), SearchState(), RecoveryState()]) + self.state_machine.add_transitions( + LongRangeState(), [ApproachPostState(), SearchState(), WaypointState(), RecoveryState()] + ) self.state_machine.add_transitions(OffState(), [WaypointState(), DoneState()]) self.state_machine.configure_off_switch(OffState(), off_check) self.state_machine_server = StatePublisher(self.state_machine, "nav_structure", 1, "nav_state", 10) diff --git a/src/navigation/search.py b/src/navigation/search.py index ad6a23ee6..bec75bced 100644 --- a/src/navigation/search.py +++ b/src/navigation/search.py @@ -149,7 +149,8 @@ def on_loop(self, context) -> State: context.rover.send_drive_command(cmd_vel) # returns either ApproachPostState, LongRangeState, ApproachObjectState, or None - if context.course.check_approach() is not None: - return context.course.check_approach() + approach_state = context.course.get_approach_target_state() + if approach_state is not None: + return approach_state return self diff --git a/src/navigation/waypoint.py b/src/navigation/waypoint.py index 9de2d9217..493c23d7a 100644 --- a/src/navigation/waypoint.py +++ b/src/navigation/waypoint.py @@ -12,7 +12,7 @@ class WaypointState(State): NO_FIDUCIAL = get_rosparam("waypoint/no_fiducial", -1) def on_enter(self, context) -> None: - pass + context.env.arrived_at_waypoint = False def on_exit(self, context) -> None: pass @@ -35,8 +35,9 @@ def on_loop(self, context) -> State: return post_backup.PostBackupState() # returns either ApproachPostState, LongRangeState, ApproachObjectState, or None - if context.course.check_approach() is not None: - return context.course.check_approach() + approach_state = context.course.get_approach_target_state() + if approach_state is not None: + return approach_state # Attempt to find the waypoint in the TF tree and drive to it try: @@ -48,10 +49,11 @@ def on_loop(self, context) -> State: self.DRIVE_FWD_THRESH, ) if arrived: + context.env.arrived_at_waypoint = True if not context.course.look_for_post() and not context.course.look_for_object(): # We finished a regular waypoint, go onto the next one context.course.increment_waypoint() - elif context.course.look_for_post() or context.course.look_for_object(): + else: # We finished a waypoint associated with a post or mallet, but we have not seen it yet. return search.SearchState() diff --git a/src/perception/usb_camera/usb_camera.cpp b/src/perception/usb_camera/usb_camera.cpp index e83d6cac2..2452b638c 100644 --- a/src/perception/usb_camera/usb_camera.cpp +++ b/src/perception/usb_camera/usb_camera.cpp @@ -42,8 +42,7 @@ namespace mrover { mCamInfoPub = mNh.advertise(cameraInfoTopicName, 1); std::string captureFormat = std::format("video/x-raw,format=YUY2,width={},height={},framerate={}/1", width, height, framerate); - std::string convertFormat = "video/x-raw,format=BGRA"; - std::string gstString = std::format("v4l2src device={} ! {} ! videoconvert ! {} ! appsink", device, captureFormat, convertFormat); + std::string gstString = std::format("v4l2src device={} ! {} ! appsink", device, captureFormat); NODELET_INFO_STREAM(std::format("GStreamer string: {}", gstString)); cv::VideoCapture capture{gstString, cv::CAP_GSTREAMER}; if (!capture.isOpened()) throw std::runtime_error{"USB camera failed to open"}; @@ -57,7 +56,9 @@ namespace mrover { if (mImgPub.getNumSubscribers()) { auto imageMessage = boost::make_shared(); - fillImageMessage(frame, imageMessage); + cv::Mat bgra; + cvtColor(frame, bgra, cv::COLOR_YUV2BGRA_YUY2); + fillImageMessage(bgra, imageMessage); imageMessage->header.frame_id = "long_range_cam_frame"; imageMessage->header.stamp = ros::Time::now(); mImgPub.publish(imageMessage); diff --git a/src/preload/bits/allocator.h b/src/preload/bits/allocator.h index a58775c45..c33fb23b3 100644 --- a/src/preload/bits/allocator.h +++ b/src/preload/bits/allocator.h @@ -6,7 +6,7 @@ // C++ should have never broke this backwards compatibility! // How silly! -#if defined(__linux__) && defined(__GNUC__) +#if defined(__linux__) && defined(__GLIBCXX__) && !defined(__CUDACC__) // Allocators -*- C++ -*- @@ -285,4 +285,8 @@ namespace std _GLIBCXX_VISIBILITY(default) { #endif +#else + +#include_next + #endif diff --git a/src/simulator/simulator.cpp b/src/simulator/simulator.cpp index 499e0acb9..0fbc4639a 100644 --- a/src/simulator/simulator.cpp +++ b/src/simulator/simulator.cpp @@ -1,4 +1,5 @@ #include "simulator.hpp" +#include namespace mrover { @@ -23,7 +24,7 @@ namespace mrover { mMotorStatusPub = mNh.advertise("/drive_status", 1); mDriveControllerStatePub = mNh.advertise("/drive_controller_data", 1); mArmControllerStatePub = mNh.advertise("/arm_controller_data", 1); - mArmJointStatePub = mNh.advertise("/arm_joint_states", 1); + mArmJointStatePub = mNh.advertise("/arm_joint_data", 1); mIkTargetPub = mNh.advertise("/arm_ik", 1); diff --git a/src/simulator/simulator.hpp b/src/simulator/simulator.hpp index 3e12b9485..ddd4f0f86 100644 --- a/src/simulator/simulator.hpp +++ b/src/simulator/simulator.hpp @@ -4,6 +4,7 @@ #include "glfw_pointer.hpp" #include "wgpu_objects.hpp" +#include using namespace std::literals; diff --git a/src/simulator/simulator.render.cpp b/src/simulator/simulator.render.cpp index ce2bf2247..9f725cc11 100644 --- a/src/simulator/simulator.render.cpp +++ b/src/simulator/simulator.render.cpp @@ -208,6 +208,19 @@ namespace mrover { mWindow = GlfwPointer{w, h, WINDOW_NAME, nullptr, nullptr}; NODELET_INFO_STREAM(std::format("Created window of size: {}x{}", w, h)); + if (cv::Mat logo = imread(std::filesystem::path{std::source_location::current().file_name()}.parent_path() / "mrover_logo.png", cv::IMREAD_UNCHANGED); + logo.type() == CV_8UC4) { + cvtColor(logo, logo, cv::COLOR_BGRA2RGBA); + GLFWimage logoImage{ + .width = logo.cols, + .height = logo.rows, + .pixels = logo.data, + }; + glfwSetWindowIcon(mWindow.get(), 1, &logoImage); + } else { + ROS_WARN_STREAM("Failed to load logo image"); + } + if (glfwRawMouseMotionSupported()) glfwSetInputMode(mWindow.get(), GLFW_RAW_MOUSE_MOTION, GLFW_TRUE); glfwSetWindowUserPointer(mWindow.get(), this); diff --git a/src/simulator/simulator.sensors.cpp b/src/simulator/simulator.sensors.cpp index caf0bbf53..a6b42f703 100644 --- a/src/simulator/simulator.sensors.cpp +++ b/src/simulator/simulator.sensors.cpp @@ -1,4 +1,5 @@ #include "simulator.hpp" +#include namespace mrover { @@ -249,11 +250,19 @@ namespace mrover { mDriveControllerStatePub.publish(driveControllerState); ControllerState armControllerState; + sensor_msgs::JointState armJointState; + std::vector zeros = {0, 0, 0, 0, 0, 0}; + armJointState.header.stamp = ros::Time::now(); for (auto& linkName: {"arm_a_link", "arm_b_link", "arm_c_link", "arm_d_link", "arm_e_link"}) { armControllerState.name.emplace_back(armMsgToUrdf.backward(linkName).value()); armControllerState.state.emplace_back("Armed"); armControllerState.error.emplace_back("None"); + armJointState.name.emplace_back(armMsgToUrdf.backward(linkName).value()); + armJointState.position.emplace_back(rover.physics->getJointPos(rover.linkNameToMeta.at(linkName).index)); + armJointState.velocity.emplace_back(rover.physics->getJointVel(rover.linkNameToMeta.at(linkName).index)); + armJointState.effort.emplace_back(rover.physics->getJointTorque(rover.linkNameToMeta.at(linkName).index)); + std::uint8_t limitSwitches = 0b000; if (auto limits = rover.model.getLink(linkName)->parent_joint->limits) { double joinPosition = rover.physics->getJointPos(rover.linkNameToMeta.at(linkName).index); @@ -264,16 +273,7 @@ namespace mrover { armControllerState.limit_hit.push_back(limitSwitches); } mArmControllerStatePub.publish(armControllerState); - - sensor_msgs::JointState jointState; - jointState.header.stamp = ros::Time::now(); - for (auto& linkName: {"arm_a_link", "arm_b_link", "arm_c_link", "arm_d_link", "arm_e_link"}) { - jointState.name.push_back(armMsgToUrdf.backward(linkName).value()); - jointState.position.push_back(rover.physics->getJointPos(rover.linkNameToMeta.at(linkName).index)); - jointState.velocity.push_back(rover.physics->getJointVel(rover.linkNameToMeta.at(linkName).index)); - jointState.effort.push_back(rover.physics->getJointTorque(rover.linkNameToMeta.at(linkName).index)); - } - mArmJointStatePub.publish(jointState); + mArmJointStatePub.publish(armJointState); } } diff --git a/src/teleoperation/arm_automation/__init__.py b/src/teleoperation/arm_automation/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/teleoperation/arm_automation/arm_automation.py b/src/teleoperation/arm_automation/arm_automation.py new file mode 100755 index 000000000..4ce34b92a --- /dev/null +++ b/src/teleoperation/arm_automation/arm_automation.py @@ -0,0 +1,76 @@ +#!/usr/bin/env python3 + +import rospy + +import actionlib + +from mrover.msg import ArmActionAction, ArmActionGoal, ArmActionResult, Position + +from sensor_msgs.msg import JointState + +import numpy as np + + +def arm_automation() -> None: + rospy.init_node("arm_automation") + + server = None + joint_state = None + + pos_pub = rospy.Publisher("arm_position_cmd", Position, queue_size=1) + + def joint_state_callback(msg: JointState): + nonlocal joint_state + joint_state = msg + + rospy.Subscriber("arm_joint_data", JointState, joint_state_callback) + + def execute_callback(goal: ArmActionGoal): + if joint_state is None: + rospy.logerr("No joint state data available") + server.set_aborted(ArmActionResult()) + return + + if goal.name == "de_home": + target_names = ["joint_de_pitch", "joint_de_roll"] + target_positions = [joint_state.position[joint_state.name.index("joint_de_pitch")], np.pi / 8] + rospy.loginfo(f"Moving to {target_positions} for {target_names}") + else: + rospy.logerr("Invalid goal name") + server.set_aborted(ArmActionResult()) + return + + rate = rospy.Rate(20) + while not rospy.is_shutdown(): + if server.is_preempt_requested(): + server.set_preempted() + rospy.loginfo("Preempted") + server.set_aborted(ArmActionResult()) + return + + pos_pub.publish(Position(names=target_names, positions=target_positions)) + + feedback = [ + joint_state.position[joint_state.name.index("joint_de_pitch")], + joint_state.position[joint_state.name.index("joint_de_roll")], + ] + + if np.allclose(target_positions, feedback, atol=0.1): + rospy.loginfo("Reached target") + break + + rate.sleep() + + server.set_succeeded(ArmActionResult()) + + rospy.sleep(1) + + server = actionlib.SimpleActionServer("arm_action", ArmActionAction, execute_cb=execute_callback, auto_start=False) + server.start() + + +if __name__ == "__main__": + try: + arm_automation() + except rospy.ROSInterruptException: + pass diff --git a/src/teleoperation/arm_controller/arm_controller.cpp b/src/teleoperation/arm_controller/arm_controller.cpp index b0d7d23af..dbbe35771 100644 --- a/src/teleoperation/arm_controller/arm_controller.cpp +++ b/src/teleoperation/arm_controller/arm_controller.cpp @@ -16,19 +16,19 @@ namespace mrover { Position positions; positions.names = {"joint_a", "joint_b", "joint_c", "joint_de_pitch", "joint_de_roll"}; positions.positions.resize(positions.names.size()); - SE3d target_frame_to_arm_b_static; + SE3d targetFrameToArmBaseLink; try { - target_frame_to_arm_b_static = SE3Conversions::fromTfTree(mTfBuffer, ik_target.target.header.frame_id, "arm_base_link"); - } catch (...) { - ROS_WARN_THROTTLE(1, "Failed to resolve information about target frame"); + targetFrameToArmBaseLink = SE3Conversions::fromTfTree(mTfBuffer, ik_target.target.header.frame_id, "arm_base_link"); + } catch (tf2::TransformException const& exception) { + ROS_WARN_STREAM_THROTTLE(1, std::format("Failed to get transform from {} to arm_base_link: {}", ik_target.target.header.frame_id, exception.what())); return; } - Eigen::Vector4d target{ik_target.target.pose.position.x, ik_target.target.pose.position.y, ik_target.target.pose.position.z, 1}; - Eigen::Vector4d target_in_arm_b_static = target_frame_to_arm_b_static.transform() * target; - double x = target_in_arm_b_static.x() - END_EFFECTOR_LENGTH; // shift back by the length of the end effector - double z = target_in_arm_b_static.z(); - double y = target_in_arm_b_static.y(); - SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_target", "arm_base_link", target_frame_to_arm_b_static); + SE3d endEffectorInTarget{{ik_target.target.pose.position.x, ik_target.target.pose.position.y, ik_target.target.pose.position.z}, SO3d::Identity()}; + SE3d endEffectorInArmBaseLink = targetFrameToArmBaseLink * endEffectorInTarget; + double x = endEffectorInArmBaseLink.translation().x() - END_EFFECTOR_LENGTH; // shift back by the length of the end effector + double y = endEffectorInArmBaseLink.translation().y(); + double z = endEffectorInArmBaseLink.translation().z(); + SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_target", "arm_base_link", endEffectorInArmBaseLink); double gamma = 0; double x3 = x - LINK_DE * std::cos(gamma); diff --git a/src/teleoperation/arm_controller/pch.hpp b/src/teleoperation/arm_controller/pch.hpp index 2f68bad08..104f5832c 100644 --- a/src/teleoperation/arm_controller/pch.hpp +++ b/src/teleoperation/arm_controller/pch.hpp @@ -2,6 +2,7 @@ #include #include +#include #include diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py index 1b83fdea9..cf6b24347 100644 --- a/src/teleoperation/backend/consumers.py +++ b/src/teleoperation/backend/consumers.py @@ -3,7 +3,7 @@ import os import threading from datetime import datetime -from math import copysign, pi +from math import copysign, pi, isfinite import pytz from channels.generic.websocket import JsonWebsocketConsumer @@ -11,10 +11,14 @@ import rospy import tf2_ros from backend.models import AutonWaypoint, BasicWaypoint -from geometry_msgs.msg import Twist, Pose, Point, Quaternion, Vector3 +from geometry_msgs.msg import Twist, Pose, Point, Quaternion, Vector3, PoseStamped + +import actionlib # from cv_bridge import CvBridge from mrover.msg import ( + ArmActionAction, + ArmActionGoal, PDLB, ControllerState, GPSWaypoint, @@ -33,8 +37,8 @@ HeaterData, ) from mrover.srv import EnableAuton, AdjustMotor, ChangeCameras, CapturePanorama -from sensor_msgs.msg import NavSatFix, Temperature, RelativeHumidity -from std_msgs.msg import String +from sensor_msgs.msg import NavSatFix, Temperature, RelativeHumidity, JointState +from std_msgs.msg import String, Header from std_srvs.srv import SetBool, Trigger from tf.transformations import euler_from_quaternion from util.SE3 import SE3 @@ -64,6 +68,20 @@ class GUIConsumer(JsonWebsocketConsumer): def connect(self): self.accept() try: + # ROS Parameters + self.mappings = rospy.get_param("teleop/joystick_mappings") + self.drive_config = rospy.get_param("teleop/drive_controls") + self.max_wheel_speed = rospy.get_param("rover/max_speed") + self.wheel_radius = rospy.get_param("wheel/radius") + self.max_angular_speed = self.max_wheel_speed / self.wheel_radius + self.ra_config = rospy.get_param("teleop/ra_controls") + self.ik_names = rospy.get_param("teleop/ik_multipliers") + self.RA_NAMES = rospy.get_param("teleop/ra_names") + self.brushless_motors = rospy.get_param("brushless_motors/controllers") + self.brushed_motors = rospy.get_param("brushed_motors/controllers") + self.xbox_mappings = rospy.get_param("teleop/xbox_mappings") + self.sa_config = rospy.get_param("teleop/sa_controls") + # Publishers self.twist_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1) self.led_pub = rospy.Publisher("/auton_led_cmd", String, queue_size=1) @@ -84,10 +102,12 @@ def connect(self): self.arm_moteus_sub = rospy.Subscriber( "/arm_controller_data", ControllerState, self.arm_controller_callback ) + self.arm_joint_sub = rospy.Subscriber("/arm_joint_data", JointState, self.arm_joint_callback) + self.sa_joint_sub = rospy.Subscriber("/sa_joint_data", JointState, self.sa_joint_callback) self.drive_moteus_sub = rospy.Subscriber( "/drive_controller_data", ControllerState, self.drive_controller_callback ) - self.gps_fix = rospy.Subscriber("/gps/fix", NavSatFix, self.gps_fix_callback) + self.gps_fix = rospy.Subscriber("/left_gps_driver/fix", NavSatFix, self.gps_fix_callback) self.drive_status_sub = rospy.Subscriber("/drive_status", MotorsStatus, self.drive_status_callback) self.led_sub = rospy.Subscriber("/led", LED, self.led_callback) self.nav_state_sub = rospy.Subscriber("/nav_state", StateMachineStateUpdate, self.nav_state_callback) @@ -116,27 +136,11 @@ def connect(self): self.capture_panorama_srv = rospy.ServiceProxy("capture_panorama", CapturePanorama) self.heater_auto_shutoff_srv = rospy.ServiceProxy("science_change_heater_auto_shutoff_state", SetBool) - # ROS Parameters - self.mappings = rospy.get_param("teleop/joystick_mappings") - self.drive_config = rospy.get_param("teleop/drive_controls") - self.max_wheel_speed = rospy.get_param("rover/max_speed") - self.wheel_radius = rospy.get_param("wheel/radius") - self.rover_width = rospy.get_param("rover/width") - self.max_angular_speed = self.max_wheel_speed / (self.rover_width / 2) - self.ra_config = rospy.get_param("teleop/ra_controls") - self.ik_names = rospy.get_param("teleop/ik_multipliers") - self.RA_NAMES = rospy.get_param("teleop/ra_names") - self.brushless_motors = rospy.get_param("brushless_motors/controllers") - self.brushed_motors = rospy.get_param("brushed_motors/controllers") - self.xbox_mappings = rospy.get_param("teleop/xbox_mappings") - self.sa_config = rospy.get_param("teleop/sa_controls") - self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) self.flight_thread = threading.Thread(target=self.flight_attitude_listener) self.flight_thread.start() - - except rospy.ROSException as e: + except Exception as e: rospy.logerr(e) def disconnect(self, close_code): @@ -183,8 +187,8 @@ def receive(self, text_data): self.send_auton_command(message) elif message["type"] == "teleop_enabled": self.send_teleop_enabled(message) - elif message["type"] == "auton_tfclient": - self.auton_bearing() + elif message["type"] == "bearing": + self.bearing() elif message["type"] == "mast_gimbal": self.mast_gimbal(message) elif message["type"] == "max_streams": @@ -216,16 +220,18 @@ def receive(self, text_data): except Exception as e: rospy.logerr(e) + @staticmethod def filter_xbox_axis( - self, - value: int, + value: float, deadzone_threshold: float = DEFAULT_ARM_DEADZONE, quad_control: bool = False, ) -> float: - deadzoned_val = deadzone(value, deadzone_threshold) - return quadratic(deadzoned_val) if quad_control else deadzoned_val + value = deadzone(value, deadzone_threshold) + if quad_control: + value = quadratic(value) + return value - def filter_xbox_button(self, button_array: "List[int]", pos_button: str, neg_button: str) -> float: + def filter_xbox_button(self, button_array: list[float], pos_button: str, neg_button: str) -> float: """ Applies various filtering functions to an axis for controlling the arm :return: Return -1, 0, or 1 depending on what buttons are being pressed @@ -254,158 +260,156 @@ def to_velocity(self, input: int, joint_name: str, brushless: bool = True) -> fl / 2 ) - def handle_controls_message(self, msg): - CACHE = ["cache"] - SA_NAMES = ["sa_x", "sa_y", "sa_z", "sampler", "sensor_actuator"] - RA_NAMES = self.RA_NAMES - ra_slow_mode = False - raw_left_trigger = msg["buttons"][self.xbox_mappings["left_trigger"]] - left_trigger = raw_left_trigger if raw_left_trigger > 0 else 0 - raw_right_trigger = msg["buttons"][self.xbox_mappings["right_trigger"]] - right_trigger = raw_right_trigger if raw_right_trigger > 0 else 0 - arm_pubs = [self.arm_position_cmd_pub, self.arm_velocity_cmd_pub, self.arm_throttle_cmd_pub, self.arm_ik_pub] - sa_pubs = [self.sa_position_cmd_pub, self.sa_velocity_cmd_pub, self.sa_throttle_cmd_pub] - cache_pubs = [self.cache_position_cmd_pub, self.cache_velocity_cmd_pub, self.cache_throttle_cmd_pub] - publishers = [] - controls_names = [] - if msg["type"] == "cache_values": - controls_names = CACHE - publishers = cache_pubs - elif msg["type"] == "arm_values": - controls_names = RA_NAMES - publishers = arm_pubs - elif msg["type"] == "sa_arm_values": - controls_names = SA_NAMES - publishers = sa_pubs - - if msg["arm_mode"] == "ik": - ee_in_map = SE3.from_tf_tree(self.tf_buffer, "base_link", "arm_e_link") - - left_trigger = self.filter_xbox_axis(msg["axes"][self.xbox_mappings["left_trigger"]]) - if left_trigger < 0: - left_trigger = 0 - - right_trigger = self.filter_xbox_axis(msg["axes"][self.xbox_mappings["right_trigger"]]) - if right_trigger < 0: - right_trigger = 0 - ee_in_map.position[0] += ( - self.ik_names["x"] * self.filter_xbox_axis(msg["axes"][self.xbox_mappings["left_js_x"]]), - ) - ee_in_map.position[1] += ( - self.ik_names["y"] * self.filter_xbox_axis(msg["axes"][self.xbox_mappings["left_js_y"]]), + def publish_ik(self, axes: list[float], buttons: list[float]) -> None: + ee_in_map = SE3.from_tf_tree(self.tf_buffer, "base_link", "arm_d_link") + + ee_in_map.position[0] += self.ik_names["x"] * self.filter_xbox_axis(axes[self.xbox_mappings["left_js_x"]]) + ee_in_map.position[1] += self.ik_names["y"] * self.filter_xbox_axis(axes[self.xbox_mappings["left_js_y"]]) + ee_in_map.position[2] += self.ik_names["z"] * self.filter_xbox_button(buttons, "right_trigger", "left_trigger") + + self.send( + text_data=json.dumps( + { + "type": "ik", + "target": { + "position": ee_in_map.position.tolist(), + "quaternion": ee_in_map.rotation.quaternion.tolist(), + }, + } ) - ee_in_map.position[2] -= self.ik_names["z"] * left_trigger + self.ik_names["z"] * right_trigger + ) - arm_ik_cmd = IK( - pose=Pose( - position=Point(*ee_in_map.position), - orientation=Quaternion(*ee_in_map.rotation.quaternion), + self.arm_ik_pub.publish( + IK( + target=PoseStamped( + header=Header(stamp=rospy.Time.now(), frame_id="base_link"), + pose=Pose( + position=Point(*ee_in_map.position), + orientation=Quaternion(*ee_in_map.rotation.quaternion), + ), ) ) - publishers[3].publish(arm_ik_cmd) - - elif msg["arm_mode"] == "position": - position_names = controls_names - if msg["type"] == "arm_values": - position_names = ["joint_b", "joint_c", "joint_de_pitch", "joint_de_roll"] - position_cmd = Position( - names=position_names, - positions=msg["positions"], - ) - publishers[0].publish(position_cmd) - - elif msg["arm_mode"] == "velocity": - velocity_cmd = Velocity() - velocity_cmd.names = controls_names - if msg["type"] == "cache_values": - velocity_cmd.velocities = [ - self.sa_config["cache"]["multiplier"] - * self.filter_xbox_button(msg["buttons"], "right_bumper", "left_bumper") - ] - elif msg["type"] == "sa_arm_values": - velocity_cmd.velocities = [ - self.to_velocity( - self.filter_xbox_axis(msg["axes"][self.sa_config["sa_x"]["xbox_index"]]), "sa_x", False - ), - self.to_velocity( - self.filter_xbox_axis(msg["axes"][self.sa_config["sa_y"]["xbox_index"]]), "sa_y", False - ), - self.to_velocity( - self.filter_xbox_axis(msg["axes"][self.sa_config["sa_z"]["xbox_index"]]), "sa_z", True - ), - self.sa_config["sampler"]["multiplier"] * (right_trigger - left_trigger), - self.sa_config["sensor_actuator"]["multiplier"] - * self.filter_xbox_button(msg["buttons"], "right_bumper", "left_bumper"), - ] - elif msg["type"] == "arm_values": - velocity_cmd.velocities = [ - self.to_velocity( - self.filter_xbox_axis(msg["axes"][self.ra_config["joint_a"]["xbox_index"]]), "joint_a" - ), - self.to_velocity( - self.filter_xbox_axis(msg["axes"][self.ra_config["joint_b"]["xbox_index"]]), "joint_b", False - ), - self.to_velocity( - self.filter_xbox_axis(msg["axes"][self.ra_config["joint_c"]["xbox_index"]]), "joint_c" - ), - self.to_velocity( - self.filter_xbox_axis(msg["axes"][self.ra_config["joint_de_pitch"]["xbox_index"]]), "joint_de_0" - ), - self.to_velocity( - self.filter_xbox_axis(msg["axes"][self.ra_config["joint_de_roll"]["xbox_index"]]), "joint_de_1" - ), - self.ra_config["allen_key"]["multiplier"] * self.filter_xbox_button(msg["buttons"], "y", "a"), - self.ra_config["gripper"]["multiplier"] * self.filter_xbox_button(msg["buttons"], "b", "x"), - ] - publishers[1].publish(velocity_cmd) - - elif msg["arm_mode"] == "throttle": - throttle_cmd = Throttle() - throttle_cmd.names = controls_names - if msg["type"] == "cache_values": - throttle_cmd.throttles = [ - self.sa_config["cache"]["multiplier"] - * self.filter_xbox_button(msg["buttons"], "right_bumper", "left_bumper") - ] - elif msg["type"] == "arm_values": - # d_pad_x = msg["axes"][self.xbox_mappings["d_pad_x"]] - # if d_pad_x > 0.5: - # ra_slow_mode = True - # elif d_pad_x < -0.5: - # ra_slow_mode = False - - throttle_cmd.throttles = [ - self.filter_xbox_axis(msg["axes"][self.ra_config["joint_a"]["xbox_index"]]), - self.filter_xbox_axis(msg["axes"][self.ra_config["joint_b"]["xbox_index"]]), - self.filter_xbox_axis(msg["axes"][self.ra_config["joint_c"]["xbox_index"]]), - self.filter_xbox_button(msg["buttons"], "right_trigger", "left_trigger"), - self.filter_xbox_button(msg["buttons"], "left_bumper", "right_bumper"), - self.ra_config["allen_key"]["multiplier"] * self.filter_xbox_button(msg["buttons"], "y", "a"), - self.ra_config["gripper"]["multiplier"] * self.filter_xbox_button(msg["buttons"], "x", "b"), - ] - - for i, name in enumerate(self.RA_NAMES): - if ra_slow_mode: - throttle_cmd.throttles[i] *= self.ra_config[name]["slow_mode_multiplier"] - if self.ra_config[name]["invert"]: - throttle_cmd.throttles[i] *= -1 - elif msg["type"] == "sa_arm_values": - throttle_cmd.throttles = [ - self.filter_xbox_axis(msg["axes"][self.sa_config["sa_x"]["xbox_index"]]), - self.filter_xbox_axis(msg["axes"][self.sa_config["sa_y"]["xbox_index"]]), - self.filter_xbox_axis(msg["axes"][self.sa_config["sa_z"]["xbox_index"]]), - self.sa_config["sampler"]["multiplier"] * (right_trigger - left_trigger), - self.sa_config["sensor_actuator"]["multiplier"] - * self.filter_xbox_button(msg["buttons"], "right_bumper", "left_bumper"), - ] - - fast_mode_activated = msg["buttons"][self.xbox_mappings["a"]] or msg["buttons"][self.xbox_mappings["b"]] - if not fast_mode_activated: - for i, name in enumerate(SA_NAMES): - # When going up (vel < 0) with SA joint 2, we DON'T want slow mode. - if not (name == "sa_z" and throttle_cmd.throttles[i] < 0): - throttle_cmd.throttles[i] *= self.sa_config[name]["slow_mode_multiplier"] - publishers[2].publish(throttle_cmd) + ) + + def publish_position(self, type, names, positions): + position_cmd = Position( + names=names, + positions=positions, + ) + if type == "arm_values": + self.arm_position_cmd_pub.publish(position_cmd) + elif type == "sa_arm_values": + self.sa_position_cmd_pub.publish(position_cmd) + elif type == "cache_values": + self.cache_position_cmd_pub.publish(position_cmd) + + def publish_velocity(self, type, names, axes, buttons): + left_trigger = self.filter_xbox_axis(axes[self.xbox_mappings["left_trigger"]]) + right_trigger = self.filter_xbox_axis(axes[self.xbox_mappings["right_trigger"]]) + velocity_cmd = Velocity() + velocity_cmd.names = names + if type == "cache_values": + velocity_cmd.velocities = [ + self.sa_config["cache"]["multiplier"] * self.filter_xbox_button(buttons, "right_bumper", "left_bumper") + ] + self.cache_velocity_cmd_pub.publish(velocity_cmd) + elif type == "sa_arm_values": + velocity_cmd.velocities = [ + self.to_velocity(self.filter_xbox_axis(axes[self.sa_config["sa_x"]["xbox_index"]]), "sa_x", False), + self.to_velocity(self.filter_xbox_axis(axes[self.sa_config["sa_y"]["xbox_index"]]), "sa_y", False), + self.to_velocity(self.filter_xbox_axis(axes[self.sa_config["sa_z"]["xbox_index"]]), "sa_z", True), + self.sa_config["sampler"]["multiplier"] * (right_trigger - left_trigger), + self.sa_config["sensor_actuator"]["multiplier"] + * self.filter_xbox_button(buttons, "right_bumper", "left_bumper"), + ] + self.sa_velocity_cmd_pub.publish(velocity_cmd) + elif type == "arm_values": + velocity_cmd.velocities = [ + self.to_velocity(self.filter_xbox_axis(axes[self.ra_config["joint_a"]["xbox_index"]]), "joint_a"), + self.to_velocity( + self.filter_xbox_axis(axes[self.ra_config["joint_b"]["xbox_index"]]), "joint_b", False + ), + self.to_velocity(self.filter_xbox_axis(axes[self.ra_config["joint_c"]["xbox_index"]]), "joint_c"), + self.to_velocity( + self.filter_xbox_axis(axes[self.ra_config["joint_de_pitch"]["xbox_index"]]), "joint_de_0" + ), + self.to_velocity( + self.filter_xbox_axis(axes[self.ra_config["joint_de_roll"]["xbox_index"]]), "joint_de_1" + ), + self.ra_config["allen_key"]["multiplier"] * self.filter_xbox_button(buttons, "y", "a"), + self.ra_config["gripper"]["multiplier"] * self.filter_xbox_button(buttons, "b", "x"), + ] + self.arm_velocity_cmd_pub.publish(velocity_cmd) + + def publish_throttle(self, type, names, axes, buttons): + left_trigger = self.filter_xbox_axis(buttons[self.xbox_mappings["left_trigger"]]) + right_trigger = self.filter_xbox_axis(buttons[self.xbox_mappings["right_trigger"]]) + throttle_cmd = Throttle() + throttle_cmd.names = names + if type == "cache_values": + throttle_cmd.throttles = [ + self.sa_config["cache"]["multiplier"] * self.filter_xbox_button(buttons, "right_bumper", "left_bumper") + ] + self.cache_throttle_cmd_pub.publish(throttle_cmd) + elif type == "arm_values": + throttle_cmd.throttles = [ + self.ra_config["joint_a"]["multiplier"] * self.filter_xbox_axis(axes[self.xbox_mappings["left_x"]]), + self.ra_config["joint_b"]["multiplier"] * self.filter_xbox_axis(axes[self.xbox_mappings["left_y"]]), + self.ra_config["joint_c"]["multiplier"] + * quadratic(self.filter_xbox_axis(axes[self.xbox_mappings["right_y"]])), + self.ra_config["joint_de_pitch"]["multiplier"] + * self.filter_xbox_axis( + buttons[self.xbox_mappings["right_trigger"]] - buttons[self.xbox_mappings["left_trigger"]] + ), + self.ra_config["joint_de_roll"]["multiplier"] + * self.filter_xbox_axis( + buttons[self.xbox_mappings["right_bumper"]] - buttons[self.xbox_mappings["left_bumper"]] + ), + self.ra_config["allen_key"]["multiplier"] * self.filter_xbox_button(buttons, "y", "a"), + self.ra_config["gripper"]["multiplier"] * self.filter_xbox_button(buttons, "b", "x"), + ] + self.arm_throttle_cmd_pub.publish(throttle_cmd) + elif type == "sa_arm_values": + throttle_cmd.throttles = [ + self.filter_xbox_axis(axes[self.sa_config["sa_x"]["xbox_index"]]), + self.filter_xbox_axis(axes[self.sa_config["sa_y"]["xbox_index"]]), + self.filter_xbox_axis(axes[self.sa_config["sa_z"]["xbox_index"]]), + self.sa_config["sampler"]["multiplier"] * (right_trigger - left_trigger), + self.sa_config["sensor_actuator"]["multiplier"] + * self.filter_xbox_button(buttons, "right_bumper", "left_bumper"), + ] + self.sa_throttle_cmd_pub.publish(throttle_cmd) + + def handle_controls_message(self, msg): + names = ["joint_a", "joint_b", "joint_c", "joint_de_pitch", "joint_de_roll", "allen_key", "gripper"] + if msg["type"] == "sa_arm_values": + names = ["sa_x", "sa_y", "sa_z", "sampler", "sensor_actuator"] + elif msg["type"] == "cache_values": + names = ["cache"] + + if msg["buttons"][self.xbox_mappings["home"]] > 0.5: + rospy.loginfo("Homing DE") + + client = actionlib.SimpleActionClient("arm_action", ArmActionAction) + if client.wait_for_server(timeout=rospy.Duration(1)): + goal = ArmActionGoal(name="de_home") + client.send_goal(goal) + + client.wait_for_result() + else: + rospy.logwarn("Arm action server not available") + else: + if msg["arm_mode"] == "ik": + self.publish_ik(msg["axes"], msg["buttons"]) + + elif msg["arm_mode"] == "position": + self.publish_position(type=msg["type"], names=names, positions=msg["positions"]) + + elif msg["arm_mode"] == "velocity": + self.publish_velocity(type=msg["type"], names=names, axes=msg["axes"], buttons=msg["buttons"]) + + elif msg["arm_mode"] == "throttle": + self.publish_throttle(type=msg["type"], names=names, axes=msg["axes"], buttons=msg["buttons"]) def handle_joystick_message(self, msg): # Tiny deadzone so we can safely e-stop with dampen switch @@ -432,6 +436,8 @@ def get_axes_input( # angular_from_lateral = get_axes_input("left_right", 0.4, True) angular = get_axes_input("twist", 0.03, True, self.max_angular_speed * dampen) + linear += get_axes_input("tilt", 0.5, scale=0.1) + self.twist_pub.publish( Twist( linear=Vector3(x=linear), @@ -469,6 +475,11 @@ def arm_controller_callback(self, msg): ) ) + def sa_joint_callback(self, msg): + names = msg.name + z = msg.position[names.index("sa_z")] + self.send(text_data=json.dumps({"type": "sa_z", "sa_z": z})) + def drive_controller_callback(self, msg): hits = [] for n in msg.limit_hit: @@ -643,12 +654,12 @@ def ish_thermistor_data_callback(self, msg): temps = [x.temperature for x in msg.temps] self.send(text_data=json.dumps({"type": "thermistor", "temps": temps})) - def auton_bearing(self): + def bearing(self): base_link_in_map = SE3.from_tf_tree(self.tf_buffer, "map", "base_link") self.send( text_data=json.dumps( { - "type": "auton_tfclient", + "type": "bearing", "rotation": base_link_in_map.rotation.quaternion.tolist(), } ) @@ -800,6 +811,11 @@ def flight_attitude_listener(self): rate.sleep() + def arm_joint_callback(self, msg: JointState) -> None: + # Set non-finite values to zero + msg.position = [x if isfinite(x) else 0 for x in msg.position] + self.send(text_data=json.dumps({"type": "fk", "positions": msg.position})) + def science_spectral_callback(self, msg): self.send( text_data=json.dumps({"type": "spectral_data", "site": msg.site, "data": msg.data, "error": msg.error}) diff --git a/src/teleoperation/frontend/index.html b/src/teleoperation/frontend/index.html index a88854489..e3f02c525 100644 --- a/src/teleoperation/frontend/index.html +++ b/src/teleoperation/frontend/index.html @@ -4,7 +4,7 @@ - Vite App + MRover
diff --git a/src/teleoperation/frontend/package.json b/src/teleoperation/frontend/package.json index 8638e8b32..76d04eba6 100644 --- a/src/teleoperation/frontend/package.json +++ b/src/teleoperation/frontend/package.json @@ -20,6 +20,7 @@ "html2canvas": "^1.4.1", "leaflet": "^1.9.4", "quaternion-to-euler": "^0.5.0", + "three": "^0.163.0", "vue": "^3.3.4", "vue-flight-indicators": "^0.1.1", "vue-router": "^4.2.4", diff --git a/src/teleoperation/frontend/public/meshes/arm_a.fbx b/src/teleoperation/frontend/public/meshes/arm_a.fbx new file mode 100644 index 000000000..8f7c71b41 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/arm_a.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d72481ca801642e73fea847317225514516d10667d777c0287021a90c77befba +size 869948 diff --git a/src/teleoperation/frontend/public/meshes/arm_b.fbx b/src/teleoperation/frontend/public/meshes/arm_b.fbx new file mode 100644 index 000000000..e9360724a --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/arm_b.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0578a7fe0557e7bc86e3949cc748275386e5a472d564367a00f722f0060d47c1 +size 1127196 diff --git a/src/teleoperation/frontend/public/meshes/arm_c.fbx b/src/teleoperation/frontend/public/meshes/arm_c.fbx new file mode 100644 index 000000000..5fbded9a7 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/arm_c.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e3bad4fb02574c6faf02cac83665d7a48972640301d0ff3089d1a7200f9b81ff +size 883884 diff --git a/src/teleoperation/frontend/public/meshes/arm_d.fbx b/src/teleoperation/frontend/public/meshes/arm_d.fbx new file mode 100644 index 000000000..be192a925 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/arm_d.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d270eeab807b552603478d239cbe1d5dc0cc374c11cfd8aff8d49b78340df9d3 +size 316844 diff --git a/src/teleoperation/frontend/public/meshes/arm_e.fbx b/src/teleoperation/frontend/public/meshes/arm_e.fbx new file mode 100644 index 000000000..5630b17d9 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/arm_e.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b42cb29f991ff0dd2752e613388cef21e9fa5030b50bb023d70f8f9f891a0bea +size 560812 diff --git a/src/teleoperation/frontend/public/meshes/bottle.fbx b/src/teleoperation/frontend/public/meshes/bottle.fbx new file mode 100644 index 000000000..25d7a8eed --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/bottle.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:df9dfe054c07005d820d7c81906136bd1bf621ebf4e4dcf3fbfb52f7e0fb4716 +size 17596 diff --git a/src/teleoperation/frontend/public/meshes/ground.fbx b/src/teleoperation/frontend/public/meshes/ground.fbx new file mode 100644 index 000000000..05b93f260 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/ground.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8323c262d0e44f0f1d128a3c68752ef95b70a9b4602de2ff06f43cef106d4b31 +size 43772 diff --git a/src/teleoperation/frontend/public/meshes/hammer.fbx b/src/teleoperation/frontend/public/meshes/hammer.fbx new file mode 100644 index 000000000..9f8b6b9e4 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/hammer.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4b5e36777f78ac597bcd49076a0cf2b674d886c315b50b5d1ff75c7bcea3123a +size 16620 diff --git a/src/teleoperation/frontend/public/meshes/primitives/cube.fbx b/src/teleoperation/frontend/public/meshes/primitives/cube.fbx new file mode 100644 index 000000000..9326aa8ca --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/primitives/cube.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3fc4c443d8ae8ca4cca8d98e6aa37a811db6fc23ce4eb169abc3314625a5f27a +size 11756 diff --git a/src/teleoperation/frontend/public/meshes/primitives/cylinder.fbx b/src/teleoperation/frontend/public/meshes/primitives/cylinder.fbx new file mode 100644 index 000000000..02941d306 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/primitives/cylinder.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4678acc34ec93779fa178d326f321b8cc67976c7e8df754c88fea29b34ee0239 +size 12460 diff --git a/src/teleoperation/frontend/public/meshes/primitives/sphere.fbx b/src/teleoperation/frontend/public/meshes/primitives/sphere.fbx new file mode 100644 index 000000000..07b9cab65 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/primitives/sphere.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b5eb2df5136ad610d75ba102a17a9bee8fbb0cb253a45cc6e8bad1c527559c25 +size 13948 diff --git a/src/teleoperation/frontend/public/meshes/rock.fbx b/src/teleoperation/frontend/public/meshes/rock.fbx new file mode 100644 index 000000000..0bd101bbe --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/rock.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8098184699873dfc775269d78e3a2d069344d89b69fe67d38d06a3dba6f92c4d +size 18940 diff --git a/src/teleoperation/frontend/public/meshes/rover_chassis.fbx b/src/teleoperation/frontend/public/meshes/rover_chassis.fbx new file mode 100644 index 000000000..9e4fe6a3d --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/rover_chassis.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:497810bf42e67701d823becb224787bd04121c788a59303bfd6d485d1268fa01 +size 1966956 diff --git a/src/teleoperation/frontend/public/meshes/rover_left_bogie.fbx b/src/teleoperation/frontend/public/meshes/rover_left_bogie.fbx new file mode 100644 index 000000000..4d3a9e550 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/rover_left_bogie.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ad295bd7c739fd5b8e4d05f02db3494db37253b4315c59b60e06942fb93e3a8f +size 1569308 diff --git a/src/teleoperation/frontend/public/meshes/rover_left_rocker.fbx b/src/teleoperation/frontend/public/meshes/rover_left_rocker.fbx new file mode 100644 index 000000000..dcfad7ae4 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/rover_left_rocker.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:70a8a1a16c7223473ea44751b1a42d2f5e2cbfe41e312cdf1172ccfa0f8c8bcc +size 1062556 diff --git a/src/teleoperation/frontend/public/meshes/rover_left_wheel.fbx b/src/teleoperation/frontend/public/meshes/rover_left_wheel.fbx new file mode 100644 index 000000000..ab6089917 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/rover_left_wheel.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:13129b734d971654de759255f9a6e65c2571bab60ad668a2c325f1a92b377612 +size 556396 diff --git a/src/teleoperation/frontend/public/meshes/rover_right_bogie.fbx b/src/teleoperation/frontend/public/meshes/rover_right_bogie.fbx new file mode 100644 index 000000000..93b721e5a --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/rover_right_bogie.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:607783822c09899857fe0e556d6f8241130f3fbae96d12356ba4beb8f79bad06 +size 1567468 diff --git a/src/teleoperation/frontend/public/meshes/rover_right_rocker.fbx b/src/teleoperation/frontend/public/meshes/rover_right_rocker.fbx new file mode 100644 index 000000000..73f8b8f0e --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/rover_right_rocker.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f90f0f99210eea2963f2ac4a257ff8f908f6906b82dc4bd57fa61f590786da24 +size 1062780 diff --git a/src/teleoperation/frontend/public/meshes/rover_right_wheel.fbx b/src/teleoperation/frontend/public/meshes/rover_right_wheel.fbx new file mode 100644 index 000000000..bb5022926 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/rover_right_wheel.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:201aaec237d10542c6a97ee8afd7b7bca7c01b6463952872e22a3369b31fa3cb +size 550204 diff --git a/src/teleoperation/frontend/src/components/ArmControls.vue b/src/teleoperation/frontend/src/components/ArmControls.vue index 5a0bf132f..f9520a8b4 100644 --- a/src/teleoperation/frontend/src/components/ArmControls.vue +++ b/src/teleoperation/frontend/src/components/ArmControls.vue @@ -86,6 +86,7 @@ ]" /> + @@ -96,6 +97,7 @@ import ToggleButton from './ToggleButton.vue' import CalibrationCheckbox from './CalibrationCheckbox.vue' import MotorAdjust from './MotorAdjust.vue' import LimitSwitch from './LimitSwitch.vue' +import Rover3D from './Rover3D.vue' // In seconds const updateRate = 0.05 @@ -106,7 +108,8 @@ export default defineComponent({ ToggleButton, CalibrationCheckbox, MotorAdjust, - LimitSwitch + LimitSwitch, + Rover3D }, data() { return { @@ -137,6 +140,7 @@ export default defineComponent({ } }, positions: [], + send_positions: false // Only send after submit is clicked for the first time } }, @@ -152,11 +156,17 @@ export default defineComponent({ alert('Toggling Arm Laser failed.') } } + }, + arm_mode(newMode) { + if (newMode !== 'position') { + this.positions = [] + this.send_positions = false + } } }, - mounted: function() { - document.addEventListener('keydown', this.keyDown); + mounted: function () { + document.addEventListener('keydown', this.keyDown) }, beforeUnmount: function () { @@ -166,22 +176,26 @@ export default defineComponent({ created: function () { interval = window.setInterval(() => { - const gamepads = navigator.getGamepads() - for (let i = 0; i < 4; i++) { - const gamepad = gamepads[i] - if (gamepad) { - // Microsoft and Xbox for old Xbox 360 controllers - // X-Box for new PowerA Xbox One controllers - if ( - gamepad.id.includes('Microsoft') || - gamepad.id.includes('Xbox') || - gamepad.id.includes('X-Box') - ) { - let buttons = gamepad.buttons.map((button) => { - return button.value - }) + if (this.send_positions) { + this.publishJoystickMessage([], [], this.arm_mode, this.positions) + } else if (this.arm_mode !== "position") { + const gamepads = navigator.getGamepads() + for (let i = 0; i < 4; i++) { + const gamepad = gamepads[i] + if (gamepad) { + // Microsoft and Xbox for old Xbox 360 controllers + // X-Box for new PowerA Xbox One controllers + if ( + gamepad.id.includes('Microsoft') || + gamepad.id.includes('Xbox') || + gamepad.id.includes('X-Box') + ) { + let buttons = gamepad.buttons.map((button) => { + return button.value + }) - this.publishJoystickMessage(gamepad.axes, buttons, this.arm_mode, this.positions) + this.publishJoystickMessage(gamepad.axes, buttons, this.arm_mode, []) + } } } } @@ -221,13 +235,15 @@ export default defineComponent({ this.positions = Object.values(this.temp_positions).map( (obj) => (Number(obj.value) * Math.PI) / 180 ) + this.send_positions = true + this.publishJoystickMessage([], [], this.arm_mode, this.positions) }, - keyDown: function(event: { key: string }) { + keyDown: function (event: { key: string }) { if (event.key == ' ') { - this.arm_mode = 'arm_disabled'; + this.arm_mode = 'arm_disabled' } - }, + } } }) diff --git a/src/teleoperation/frontend/src/components/AutonTask.vue b/src/teleoperation/frontend/src/components/AutonTask.vue index 35018caa2..f0da85182 100644 --- a/src/teleoperation/frontend/src/components/AutonTask.vue +++ b/src/teleoperation/frontend/src/components/AutonTask.vue @@ -20,11 +20,11 @@

Joystick Values

-
- -
+
+ +
@@ -47,7 +47,7 @@
- +
@@ -156,7 +156,7 @@ export default defineComponent({ this.odom.latitude_deg = msg.latitude this.odom.longitude_deg = msg.longitude this.odom.altitude = msg.altitude - } else if (msg.type == 'auton_tfclient') { + } else if (msg.type == 'bearing') { this.odom.bearing_deg = quaternionToMapAngle(msg.rotation) } else if (msg.type == "center_map") { this.odom.latitude_deg = msg.latitude @@ -179,7 +179,7 @@ export default defineComponent({ this.sendMessage({ "type": "center_map" }); }, 250) interval = setInterval(() => { - this.sendMessage({ type: 'auton_tfclient' }) + this.sendMessage({ type: 'bearing' }) }, 1000) }, @@ -190,15 +190,15 @@ export default defineComponent({ .wrapper { display: grid; grid-gap: 10px; - grid-template-columns: 60% 40%; + grid-template-columns: auto 30% 30%; grid-template-rows: repeat(6, auto); grid-template-areas: - 'header header' - 'map waypoints' - 'data waypoints' - 'data conditions' - 'moteus moteus' - 'cameras cameras'; + 'header header header' + 'feed map waypoints' + 'data data waypoints' + 'data data conditions' + 'moteus moteus moteus' + 'cameras cameras cameras'; font-family: sans-serif; height: auto; @@ -308,4 +308,8 @@ h2 { .data { grid-area: data; } + +.feed { + grid-area: feed; +} diff --git a/src/teleoperation/frontend/src/components/CameraFeed.vue b/src/teleoperation/frontend/src/components/CameraFeed.vue index efbce30af..d29247716 100644 --- a/src/teleoperation/frontend/src/components/CameraFeed.vue +++ b/src/teleoperation/frontend/src/components/CameraFeed.vue @@ -3,27 +3,14 @@

{{ name }} • ID: {{ id }}

-
- - -
diff --git a/src/teleoperation/frontend/src/components/ISHTask.vue b/src/teleoperation/frontend/src/components/ISHTask.vue index 8be0a6a1c..08e46c76a 100644 --- a/src/teleoperation/frontend/src/components/ISHTask.vue +++ b/src/teleoperation/frontend/src/components/ISHTask.vue @@ -25,7 +25,7 @@
- +
diff --git a/src/teleoperation/frontend/src/components/Menu.vue b/src/teleoperation/frontend/src/components/Menu.vue index cfea3f72e..cea3a9804 100644 --- a/src/teleoperation/frontend/src/components/Menu.vue +++ b/src/teleoperation/frontend/src/components/Menu.vue @@ -13,8 +13,7 @@ - - +
diff --git a/src/teleoperation/frontend/src/components/MenuButton.vue b/src/teleoperation/frontend/src/components/MenuButton.vue index 6218a81f8..5b0c47f68 100644 --- a/src/teleoperation/frontend/src/components/MenuButton.vue +++ b/src/teleoperation/frontend/src/components/MenuButton.vue @@ -16,7 +16,7 @@ export default defineComponent({ type: String, required: true } - } + }, }) diff --git a/src/teleoperation/frontend/src/components/Rover3D.vue b/src/teleoperation/frontend/src/components/Rover3D.vue new file mode 100644 index 000000000..3c53d78ea --- /dev/null +++ b/src/teleoperation/frontend/src/components/Rover3D.vue @@ -0,0 +1,57 @@ + + + + + \ No newline at end of file diff --git a/src/teleoperation/frontend/src/components/SAArmControls.vue b/src/teleoperation/frontend/src/components/SAArmControls.vue index d624b8036..5047bd2db 100644 --- a/src/teleoperation/frontend/src/components/SAArmControls.vue +++ b/src/teleoperation/frontend/src/components/SAArmControls.vue @@ -45,6 +45,8 @@
+ +

SA Z Position: {{ z_position }} inches

{ + return button.value + }) + + this.publishJoystickMessage(gamepad.axes, buttons, this.arm_mode, []) + } } } } @@ -146,6 +155,15 @@ export default defineComponent({ alert('Toggling Arm Laser failed.') } } + else if (msg.type == 'sa_z') { + this.z_position = msg.sa_z * metersToInches + } + }, + arm_mode(newMode) { + if (newMode !== 'position') { + this.positions = [] + this.send_positions = false + } } }, @@ -178,6 +196,12 @@ export default defineComponent({ this.positions = Object.values(this.temp_positions).map( (obj) => (Number(obj.value) * Math.PI) / 180 ) + this.send_positions = true + this.publishJoystickMessage([], [], this.arm_mode, this.positions) + }, + + zero: function() { + this.sendMessage({type: "arm_adjust", name: "sa_z", value: 0}) } } }) diff --git a/src/teleoperation/frontend/src/components/SATask.vue b/src/teleoperation/frontend/src/components/SATask.vue index 45af1e6fa..f381f72d4 100644 --- a/src/teleoperation/frontend/src/components/SATask.vue +++ b/src/teleoperation/frontend/src/components/SATask.vue @@ -28,7 +28,7 @@
- +
@@ -78,13 +78,6 @@ :topic_name="'sa_calibrate_sensor_actuator'" />
-
@@ -113,7 +106,9 @@ import MotorAdjust from './MotorAdjust.vue' import OdometryReading from './OdometryReading.vue' import SAArmControls from './SAArmControls.vue' import { disableAutonLED, quaternionToMapAngle } from '../utils.js' -import { mapState } from 'vuex' +import { mapState, mapActions } from 'vuex' + +let interval: number export default { components: { @@ -182,10 +177,26 @@ export default { this.moteusState.error = msg.error this.moteusState.limit_hit = msg.limit_hit } + else if (msg.type == 'nav_sat_fix') { + this.odom.latitude_deg = msg.latitude + this.odom.longitude_deg = msg.longitude + this.odom.altitude = msg.altitude + } + else if (msg.type == 'bearing') { + this.odom.bearing_deg = quaternionToMapAngle(msg.rotation) + } } }, - created: function () {} + methods: { + ...mapActions('websocket', ['sendMessage']), + }, + + created: function () { + interval = setInterval(() => { + this.sendMessage({ type: 'bearing' }) + }, 1000) + } } diff --git a/src/teleoperation/frontend/src/router/index.ts b/src/teleoperation/frontend/src/router/index.ts index f34e022f1..9d4faac4b 100644 --- a/src/teleoperation/frontend/src/router/index.ts +++ b/src/teleoperation/frontend/src/router/index.ts @@ -4,6 +4,8 @@ import DMESTask from '../components/DMESTask.vue' import AutonTask from '../components/AutonTask.vue' import ISHTask from '../components/ISHTask.vue' import SATask from '../components/SATask.vue' +import Cameras from '../components/Cameras.vue' +import Rover3D from '../components/Rover3D.vue' const routes = [ { @@ -41,7 +43,21 @@ const routes = [ path: '/ISHTask', name: 'ISHTask', component: ISHTask - } + }, + { + path: '/Cameras', + name: 'Cameras', + component: Cameras, + props: { + isSA: false, + mission: 'other' + } + }, + { + path: "/Control", + name: "Control", + component: Rover3D, + }, ] const router = createRouter({ history: createWebHistory(), diff --git a/src/teleoperation/frontend/src/rover_three.js b/src/teleoperation/frontend/src/rover_three.js new file mode 100644 index 000000000..ac83a7d02 --- /dev/null +++ b/src/teleoperation/frontend/src/rover_three.js @@ -0,0 +1,162 @@ +import * as THREE from 'three'; +import { FBXLoader } from 'three/examples/jsm/loaders/FBXLoader' +import { TrackballControls } from 'three/addons/controls/TrackballControls.js'; + +export function threeSetup(containerId) { + // Get the container where the scene should be placed + const container = document.getElementById(containerId); + + const canvas = { + width: container.clientWidth, + height: container.clientHeight + } + + const scene = new THREE.Scene(); + const camera = new THREE.PerspectiveCamera(75, canvas.width / canvas.height, 0.1, 1000); + camera.up.set(0, 0, 1); + + const directionalLight = new THREE.DirectionalLight(0xffffff, 0.5); + directionalLight.position.set(1, 2, 3); + scene.add(directionalLight); + + const renderer = new THREE.WebGLRenderer(); + renderer.setSize(canvas.width, canvas.height); + container.appendChild(renderer.domElement); // append it to your specific HTML element + + scene.background = new THREE.Color(0xcccccc); + + //controls to orbit around model + var controls = new TrackballControls(camera, renderer.domElement); + controls.rotateSpeed = 1.0; + controls.zoomSpeed = 1.2; + controls.panSpeed = 0.8; + controls.noZoom = false; + controls.noPan = false; + controls.staticMoving = true; + controls.dynamicDampingFactor = 0.3; + + const joints = [ + { + name: "chassis", + file: "rover_chassis.fbx", + translation: [0.164882, -0.200235, 0.051497], + rotation: [0, 0, 0], + }, + { + name: "a", + file: "arm_a.fbx", + translation: [0.034346, 0, 0.049024], + rotation: [0, 0, 0], + }, + { + name: "b", + file: "arm_b.fbx", + translation: [0.534365, 0, 0.009056], + rotation: [0, 0, 0], + }, + { + name: "c", + file: "arm_c.fbx", + translation: [0.546033, 0, 0.088594], + rotation: [0, 0, 0], + }, + { + name: "d", + file: "arm_d.fbx", + translation: [0.044886, 0, 0], + rotation: [0, 0, 0], + }, + { + name: "e", + file: "arm_e.fbx", + translation: [0.044886, 0, 0], + rotation: [0, 0, 0], + } + ] + + const loader = new FBXLoader(); + for (let i = 0; i < joints.length; ++i) { + loader.load('/meshes/' + joints[i].file, (fbx) => { + // Traverse the loaded scene to find meshes or objects + fbx.traverse((child)=> { + if (child.isMesh) { + if(joints[i].name == "d" || joints[i].name == "e") { + child.material = new THREE.MeshStandardMaterial({color: 0x00ff00}); + } + else child.material = new THREE.MeshStandardMaterial({color: 0xffffff}); + scene.add(child); + child.scale.set(1, 1, 1); + child.position.set(0, 0, 0); + } + }); + }, + (xhr) => { + console.log((xhr.loaded / xhr.total) * 100 + '% loaded') + }, + (error) => { + console.log(error) + }); + } + + camera.position.x = 2; + camera.position.y = 2; + camera.position.z = 2; + camera.lookAt(new THREE.Vector3(0, 0, 0)); + + const targetCube = new THREE.Mesh( + new THREE.BoxGeometry( 0.1, 0.1, 0.1 ), + new THREE.MeshBasicMaterial({color: 0x00ff00}) + ); + scene.add( targetCube ); + + function animate() { + requestAnimationFrame(animate); + controls.update(); + renderer.render(scene, camera); + } + + animate(); + + function fk(positions) { + + let cumulativeMatrix = new THREE.Matrix4(); + + cumulativeMatrix.makeTranslation(new THREE.Vector3(0, 0, 0.439675)); //makes meshes relative to base_link + + for(let i = 0; i < joints.length; ++i){ + let mesh = scene.getObjectByName(joints[i].name); + + let localMatrix = new THREE.Matrix4(); + + let rotationAngle = positions[i]; // Angle for this joint + if(joints[i].name == "chassis") { + localMatrix.makeTranslation(0,rotationAngle,0); + } + else { + localMatrix.makeRotationY(rotationAngle); + } + + let offset = new THREE.Vector3().fromArray(joints[i].translation); + + localMatrix.setPosition( + new THREE.Vector3().setFromMatrixPosition(localMatrix).add(offset) + ); + + mesh.matrixAutoUpdate = false; + mesh.matrix = cumulativeMatrix.clone(); + + cumulativeMatrix.multiply(localMatrix); + } + } + + function ik(target) { + let quaternion = new THREE.Quaternion(...target.quaternion); + targetCube.position.set(...target.position); + targetCube.setRotationFromQuaternion(quaternion); + } + + // Return an object with the updateJointAngles() function + return { + fk, ik + } +}; diff --git a/src/teleoperation/gui_chromium.sh b/src/teleoperation/gui_chromium.sh index 3f5e50061..e83d1c054 100755 --- a/src/teleoperation/gui_chromium.sh +++ b/src/teleoperation/gui_chromium.sh @@ -4,4 +4,6 @@ # Chromium currently only supports this when using VA-API hardware acceleration # It uses the WebCodecs API to decode # You can easily test if your setup works with this URL: https://w3c.github.io/webcodecs/samples/video-decode-display/ -chromium --enable-features=VaapiVideoDecodeLinuxGL --app=http://localhost:8080 +readonly FLAGS="--enable-features=VaapiVideoDecodeLinuxGL" +readonly ADDRESS="http://localhost:8080" +chromium ${FLAGS} --app=${ADDRESS}/$1 diff --git a/src/teleoperation/gui_chromium_cameras.sh b/src/teleoperation/gui_chromium_cameras.sh new file mode 100755 index 000000000..ef6bade5b --- /dev/null +++ b/src/teleoperation/gui_chromium_cameras.sh @@ -0,0 +1,3 @@ +#!/bin/bash + +$(dirname $0)/gui_chromium.sh Cameras diff --git a/src/teleoperation/gui_chromium_menu.sh b/src/teleoperation/gui_chromium_menu.sh new file mode 100755 index 000000000..a259fcd1c --- /dev/null +++ b/src/teleoperation/gui_chromium_menu.sh @@ -0,0 +1,3 @@ +#!/bin/bash + +$(dirname $0)/gui_chromium.sh diff --git a/urdf/rover/rover.urdf.xacro b/urdf/rover/rover.urdf.xacro index 3aedbbc29..f79b800bb 100644 --- a/urdf/rover/rover.urdf.xacro +++ b/urdf/rover/rover.urdf.xacro @@ -204,7 +204,7 @@ - +