diff --git a/CMakeLists.txt b/CMakeLists.txt index 7c68974f1..e03b97b8e 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 @@ -115,14 +119,16 @@ endforeach () set(BUILD_SHARED_LIBS ON) -### ======== ### -### Messages ### -### ======== ### +### ================================ ### +### Messages & Actions & Reconfigure ### +### ================================ ### 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}) @@ -191,6 +197,9 @@ mrover_nodelet_link_libraries(long_range_tag_detector opencv_core opencv_objdete mrover_add_nodelet(usb_camera src/perception/usb_camera/*.cpp src/perception/usb_camera src/perception/usb_camera/pch.hpp) mrover_nodelet_link_libraries(usb_camera opencv_core opencv_objdetect opencv_aruco opencv_imgproc opencv_highgui) +mrover_add_nodelet(click_ik src/perception/click_ik/*.cpp src/perception/click_ik src/perception/click_ik/pch.hpp) +mrover_nodelet_link_libraries(click_ik lie) + if (CUDA_FOUND) # mrover_add_node(nv_vid_codec_h265_enc src/esw/nv_vid_codec_h265_enc/*.c*) # target_link_libraries(nv_vid_codec_h265_enc PUBLIC cuda nvidia-encode opencv_core opencv_imgproc streaming) diff --git a/action/ClickIk.action b/action/ClickIk.action new file mode 100644 index 000000000..d17ea39ff --- /dev/null +++ b/action/ClickIk.action @@ -0,0 +1,6 @@ +uint32 pointInImageX +uint32 pointInImageY +--- +bool success +--- +float32 distance diff --git a/deps/manif b/deps/manif index 03c497df3..c8a9fcbfe 160000 --- a/deps/manif +++ b/deps/manif @@ -1 +1 @@ -Subproject commit 03c497df3364e999cec46fe4f974eae1959a34e1 +Subproject commit c8a9fcbfe157b55a7a860577febdc5350043a722 diff --git a/launch/simulator.launch b/launch/simulator.launch index 23230736f..c318c1088 100644 --- a/launch/simulator.launch +++ b/launch/simulator.launch @@ -1,26 +1,33 @@ - - + + - + - - + + + args="manager" output="screen" /> - + args="load mrover/SimulatorNodelet perception_nodelet_manager" output="screen"> + - + + + + + - - + + - + \ No newline at end of file diff --git a/msg/ArmStatus.msg b/msg/ArmStatus.msg new file mode 100644 index 000000000..4f47d908d --- /dev/null +++ b/msg/ArmStatus.msg @@ -0,0 +1 @@ +bool status \ No newline at end of file diff --git a/package.xml b/package.xml index 7208ccec5..02932aa8a 100644 --- a/package.xml +++ b/package.xml @@ -85,5 +85,6 @@ + diff --git a/plugins/click_ik.xml b/plugins/click_ik.xml new file mode 100644 index 000000000..ffed8d6b4 --- /dev/null +++ b/plugins/click_ik.xml @@ -0,0 +1,6 @@ + + + + diff --git a/scripts/debug_click_ik.py b/scripts/debug_click_ik.py new file mode 100644 index 000000000..c84e9a7f6 --- /dev/null +++ b/scripts/debug_click_ik.py @@ -0,0 +1,40 @@ +#!/usr/bin/env python3 + +import rospy +from mrover.msg import ClickIkAction, ClickIkGoal +import actionlib +import sys + + +def click_ik_client(): + # Creates the SimpleActionClient, passing the type of the action + client = actionlib.SimpleActionClient("do_click_ik", ClickIkAction) + print("created click ik client") + # Waits until the action server has started up and started + # listening for goals. + client.wait_for_server() + print("Found server") + # Creates a goal to send to the action server. + goal = ClickIkGoal() + # Sends the goal to the action server. + goal.pointInImageX = 320 + goal.pointInImageY = 240 + client.send_goal(goal, feedback_cb=feedback) + client.wait_for_result() + result = client.get_result() + return result + + +def feedback(msg): + print(msg) + + +if __name__ == "__main__": + try: + # Initializes a rospy node so that the SimpleActionClient can + # publish and subscribe over ROS. + rospy.init_node("debug_click_ik") + result = click_ik_client() + print("result: ", result) + except rospy.ROSInterruptException: + print("program interrupted before completion", file=sys.stderr) diff --git a/scripts/debug_service.py b/scripts/debug_service.py index ce2b7d761..775f688d4 100755 --- a/scripts/debug_service.py +++ b/scripts/debug_service.py @@ -6,16 +6,16 @@ from typing import Any import rospy -from std_srvs.srv import SetBool, SetBoolResponse +from mrover.srv import ChangeCameras, ChangeCamerasResponse # Change these values for the service name and type definition to test different values -SERVICE_NAME = "science_enable_heater_b1" -SERVICE_TYPE = SetBool +SERVICE_NAME = "change_cameras" +SERVICE_TYPE = ChangeCameras def print_service_request(service_request: Any): rospy.loginfo(service_request) - return SetBoolResponse(success=True) + return ChangeCamerasResponse(success=True) def main(): diff --git a/src/esw/fw/pdlb_bare/CMakeLists.txt b/src/esw/fw/pdlb_bare/CMakeLists.txt new file mode 100644 index 000000000..437c91213 --- /dev/null +++ b/src/esw/fw/pdlb_bare/CMakeLists.txt @@ -0,0 +1,73 @@ +#THIS FILE IS AUTO GENERATED FROM THE TEMPLATE! DO NOT CHANGE! +set(CMAKE_SYSTEM_NAME Generic) +set(CMAKE_SYSTEM_VERSION 1) +cmake_minimum_required(VERSION 3.28) + +# specify cross-compilers and tools +set(CMAKE_C_COMPILER arm-none-eabi-gcc) +set(CMAKE_CXX_COMPILER arm-none-eabi-g++) +set(CMAKE_ASM_COMPILER arm-none-eabi-gcc) +set(CMAKE_AR arm-none-eabi-ar) +set(CMAKE_OBJCOPY arm-none-eabi-objcopy) +set(CMAKE_OBJDUMP arm-none-eabi-objdump) +set(SIZE arm-none-eabi-size) +set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY) + +# project settings +project(pdlb_bare C CXX ASM) +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_C_STANDARD 11) + +#Uncomment for hardware floating point +#add_compile_definitions(ARM_MATH_CM4;ARM_MATH_MATRIX_CHECK;ARM_MATH_ROUNDING) +#add_compile_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16) +#add_link_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16) + +#Uncomment for software floating point +#add_compile_options(-mfloat-abi=soft) + +add_compile_options(-mcpu=cortex-m4 -mthumb -mthumb-interwork) +add_compile_options(-ffunction-sections -fdata-sections -fno-common -fmessage-length=0) + +# uncomment to mitigate c++17 absolute addresses warnings +#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-register") + +# Enable assembler files preprocessing +add_compile_options($<$:-x$assembler-with-cpp>) + +if ("${CMAKE_BUILD_TYPE}" STREQUAL "Release") + message(STATUS "Maximum optimization for speed") + add_compile_options(-Ofast) +elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "RelWithDebInfo") + message(STATUS "Maximum optimization for speed, debug info included") + add_compile_options(-Ofast -g) +elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "MinSizeRel") + message(STATUS "Maximum optimization for size") + add_compile_options(-Os) +else () + message(STATUS "Minimal optimization, debug info included") + add_compile_options(-Og -g) +endif () + +include_directories(Core/Inc Drivers/STM32G4xx_HAL_Driver/Inc Drivers/STM32G4xx_HAL_Driver/Inc/Legacy Drivers/CMSIS/Device/ST/STM32G4xx/Include Drivers/CMSIS/Include) + +add_definitions(-DDEBUG -DUSE_HAL_DRIVER -DSTM32G431xx) + +file(GLOB_RECURSE SOURCES "Core/*.*" "Drivers/*.*") + +set(LINKER_SCRIPT ${CMAKE_SOURCE_DIR}/) + +add_link_options(-Wl,-gc-sections,--print-memory-usage,-Map=${PROJECT_BINARY_DIR}/${PROJECT_NAME}.map) +add_link_options(-mcpu=cortex-m4 -mthumb -mthumb-interwork) +add_link_options(-T ${LINKER_SCRIPT}) + +add_executable(${PROJECT_NAME}.elf ${SOURCES} ${LINKER_SCRIPT}) + +set(HEX_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.hex) +set(BIN_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.bin) + +add_custom_command(TARGET ${PROJECT_NAME}.elf POST_BUILD + COMMAND ${CMAKE_OBJCOPY} -Oihex $ ${HEX_FILE} + COMMAND ${CMAKE_OBJCOPY} -Obinary $ ${BIN_FILE} + COMMENT "Building ${HEX_FILE} +Building ${BIN_FILE}") diff --git a/src/esw/fw/pdlb_bare/CMakeLists_template.txt b/src/esw/fw/pdlb_bare/CMakeLists_template.txt new file mode 100644 index 000000000..3b7fc4493 --- /dev/null +++ b/src/esw/fw/pdlb_bare/CMakeLists_template.txt @@ -0,0 +1,72 @@ +#${templateWarning} +set(CMAKE_SYSTEM_NAME Generic) +set(CMAKE_SYSTEM_VERSION 1) +${cmakeRequiredVersion} +# specify cross-compilers and tools +set(CMAKE_C_COMPILER arm-none-eabi-gcc) +set(CMAKE_CXX_COMPILER arm-none-eabi-g++) +set(CMAKE_ASM_COMPILER arm-none-eabi-gcc) +set(CMAKE_AR arm-none-eabi-ar) +set(CMAKE_OBJCOPY arm-none-eabi-objcopy) +set(CMAKE_OBJDUMP arm-none-eabi-objdump) +set(SIZE arm-none-eabi-size) +set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY) + +# project settings +project(${projectName} C CXX ASM) +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_C_STANDARD 11) + +#Uncomment for hardware floating point +#add_compile_definitions(ARM_MATH_CM4;ARM_MATH_MATRIX_CHECK;ARM_MATH_ROUNDING) +#add_compile_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16) +#add_link_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16) + +#Uncomment for software floating point +#add_compile_options(-mfloat-abi=soft) + +add_compile_options(-mcpu=${mcpu} -mthumb -mthumb-interwork) +add_compile_options(-ffunction-sections -fdata-sections -fno-common -fmessage-length=0) + +# uncomment to mitigate c++17 absolute addresses warnings +#set(CMAKE_CXX_FLAGS "$${CMAKE_CXX_FLAGS} -Wno-register") + +# Enable assembler files preprocessing +add_compile_options($<$:-x$assembler-with-cpp>) + +if ("$${CMAKE_BUILD_TYPE}" STREQUAL "Release") + message(STATUS "Maximum optimization for speed") + add_compile_options(-Ofast) +elseif ("$${CMAKE_BUILD_TYPE}" STREQUAL "RelWithDebInfo") + message(STATUS "Maximum optimization for speed, debug info included") + add_compile_options(-Ofast -g) +elseif ("$${CMAKE_BUILD_TYPE}" STREQUAL "MinSizeRel") + message(STATUS "Maximum optimization for size") + add_compile_options(-Os) +else () + message(STATUS "Minimal optimization, debug info included") + add_compile_options(-Og -g) +endif () + +include_directories(${includes}) + +add_definitions(${defines}) + +file(GLOB_RECURSE SOURCES ${sources}) + +set(LINKER_SCRIPT $${CMAKE_SOURCE_DIR}/${linkerScript}) + +add_link_options(-Wl,-gc-sections,--print-memory-usage,-Map=$${PROJECT_BINARY_DIR}/$${PROJECT_NAME}.map) +add_link_options(-mcpu=${mcpu} -mthumb -mthumb-interwork) +add_link_options(-T $${LINKER_SCRIPT}) + +add_executable($${PROJECT_NAME}.elf $${SOURCES} $${LINKER_SCRIPT}) + +set(HEX_FILE $${PROJECT_BINARY_DIR}/$${PROJECT_NAME}.hex) +set(BIN_FILE $${PROJECT_BINARY_DIR}/$${PROJECT_NAME}.bin) + +add_custom_command(TARGET $${PROJECT_NAME}.elf POST_BUILD + COMMAND $${CMAKE_OBJCOPY} -Oihex $ $${HEX_FILE} + COMMAND $${CMAKE_OBJCOPY} -Obinary $ $${BIN_FILE} + COMMENT "Building $${HEX_FILE} +Building $${BIN_FILE}") diff --git a/src/esw/fw/pdlb_bare/st_nucleo_g4.cfg b/src/esw/fw/pdlb_bare/st_nucleo_g4.cfg new file mode 100644 index 000000000..8e583e77d --- /dev/null +++ b/src/esw/fw/pdlb_bare/st_nucleo_g4.cfg @@ -0,0 +1,19 @@ +# SPDX-License-Identifier: GPL-2.0-or-later + +# This is for all ST NUCLEO with any STM32G4. Known boards at the moment: +# NUCLEO-G431KB +# https://www.st.com/en/evaluation-tools/nucleo-g431kb.html +# NUCLEO-G431RB +# https://www.st.com/en/evaluation-tools/nucleo-g431rb.html +# NUCLEO-G474RE +# https://www.st.com/en/evaluation-tools/nucleo-g474re.html +# NUCLEO-G491RE +# https://www.st.com/en/evaluation-tools/nucleo-g491re.html + +source [find interface/stlink.cfg] + +transport select hla_swd + +source [find target/stm32g4x.cfg] + +reset_config srst_only diff --git a/src/perception/click_ik/click_ik.cpp b/src/perception/click_ik/click_ik.cpp new file mode 100644 index 000000000..f8324b54b --- /dev/null +++ b/src/perception/click_ik/click_ik.cpp @@ -0,0 +1,141 @@ +#include "click_ik.hpp" +#include + +namespace mrover { + + void ClickIkNodelet::startClickIk() { + ROS_INFO("Executing goal"); + + mrover::ClickIkGoalConstPtr const& goal = server.acceptNewGoal(); + auto target_point = spiralSearchInImg(static_cast(goal->pointInImageX), static_cast(goal->pointInImageY)); + + //Check if optional has value + if (!target_point.has_value()) { + //Handle gracefully + ROS_WARN("Target point does not exist."); + return; + } + + geometry_msgs::Pose pose; + + double offset = 0.1; // make sure we don't collide by moving back a little from the target + pose.position.x = target_point.value().x - offset; + pose.position.y = target_point.value().y; + pose.position.z = target_point.value().z; + + message.target.pose = pose; + message.target.header.frame_id = "zed_left_camera_frame"; + timer = mNh.createTimer(ros::Duration(0.010), [&, pose](ros::TimerEvent const) { + if (server.isPreemptRequested()) { + timer.stop(); + return; + } + // Check if done + const float tolerance = 0.02; // 2 cm + try { + SE3d arm_position = SE3Conversions::fromTfTree(mTfBuffer, "arm_e_link", "zed_left_camera_frame"); + double distance = pow(pow(arm_position.x() + ArmController::END_EFFECTOR_LENGTH - pose.position.x, 2) + pow(arm_position.y() - pose.position.y, 2) + pow(arm_position.z() - pose.position.z, 2), 0.5); + ROS_INFO("Distance to target: %f", distance); + mrover::ClickIkFeedback feedback; + feedback.distance = static_cast(distance); + server.publishFeedback(feedback); + + if (distance < tolerance) { + timer.stop(); + mrover::ClickIkResult result; + result.success = true; + server.setSucceeded(result); + return; + } + // Otherwise publish message + mIkPub.publish(message); + } catch (tf2::ExtrapolationException& e) { + ROS_WARN("ExtrapolationException (due to lag?): %s", e.what()); + } + }); + } + + void ClickIkNodelet::cancelClickIk() { + timer.stop(); + } + + void ClickIkNodelet::onInit() { + mNh = getMTNodeHandle(); + mPnh = getMTPrivateNodeHandle(); + + mPcSub = mNh.subscribe("camera/left/points", 1, &ClickIkNodelet::pointCloudCallback, this); + // IK Publisher + mIkPub = mNh.advertise("/arm_ik", 1); + + // ArmStatus subscriber + mStatusSub = mNh.subscribe("arm_status", 1, &ClickIkNodelet::statusCallback, this); + + ROS_INFO("Starting action server"); + server.registerGoalCallback([this]() { startClickIk(); }); + server.registerPreemptCallback([this] { cancelClickIk(); }); + server.start(); + ROS_INFO("Action server started"); + } + + void ClickIkNodelet::pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg) { + // Update current pointer to pointcloud data + mPoints = reinterpret_cast(msg->data.data()); + mNumPoints = msg->width * msg->height; + mPointCloudWidth = msg->width; + mPointCloudHeight = msg->height; + } + + auto ClickIkNodelet::spiralSearchInImg(size_t xCenter, size_t yCenter) -> std::optional { + std::size_t currX = xCenter; + std::size_t currY = yCenter; + std::size_t radius = 0; + int t = 0; + constexpr int numPts = 16; + bool isPointInvalid = true; + Point point{}; + + // Find the smaller of the two box dimensions so we know the max spiral radius + std::size_t smallDim = std::min(mPointCloudWidth / 2, mPointCloudHeight / 2); + + while (isPointInvalid) { + // This is the parametric equation to spiral around the center pnt + currX = static_cast(static_cast(xCenter) + std::cos(t * 1.0 / numPts * 2 * M_PI) * static_cast(radius)); + currY = static_cast(static_cast(yCenter) + std::sin(t * 1.0 / numPts * 2 * M_PI) * static_cast(radius)); + + // Grab the point from the pntCloud and determine if its a finite pnt + point = mPoints[currX + currY * mPointCloudWidth]; + + isPointInvalid = !std::isfinite(point.x) || !std::isfinite(point.y) || !std::isfinite(point.z); + if (isPointInvalid) + NODELET_WARN("Tag center point not finite: [%f %f %f]", point.x, point.y, point.z); + + // After a full circle increase the radius + if (t % numPts == 0) { + radius++; + } + + // Increase the parameter + t++; + + // If we reach the edge of the box we stop spiraling + if (radius >= smallDim) { + return std::nullopt; + } + } + + return std::make_optional(point); + } + + void ClickIkNodelet::statusCallback(ArmStatus const& status) { + if (!status.status) { + cancelClickIk(); + if (server.isActive()) { + mrover::ClickIkResult result; + result.success = false; + server.setAborted(result, "Arm position unreachable"); + NODELET_WARN("Arm position unreachable"); + } + } + } + +} // namespace mrover diff --git a/src/perception/click_ik/click_ik.hpp b/src/perception/click_ik/click_ik.hpp new file mode 100644 index 000000000..dc0e79a18 --- /dev/null +++ b/src/perception/click_ik/click_ik.hpp @@ -0,0 +1,43 @@ +#pragma once + +#include "pch.hpp" + +namespace mrover { + + class ClickIkNodelet final : public nodelet::Nodelet { + ros::NodeHandle mNh, mPnh; + ros::Subscriber mPcSub; + ros::Publisher mIkPub; + ros::Subscriber mStatusSub; + actionlib::SimpleActionServer server = actionlib::SimpleActionServer(mNh, "do_click_ik", false); + + IK message; + ros::Timer timer; + + Point const* mPoints{}; + std::size_t mNumPoints{}; + std::size_t mPointCloudWidth{}; + std::size_t mPointCloudHeight{}; + + tf2_ros::Buffer mTfBuffer{}; + tf2_ros::TransformListener mTfListener{mTfBuffer}; + tf2_ros::TransformBroadcaster mTfBroadcaster; + + public: + ClickIkNodelet() = default; + + ~ClickIkNodelet() override = default; + + void onInit() override; + + auto pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg) -> void; + void statusCallback(ArmStatus const&); + + void startClickIk(); + void cancelClickIk(); + + //Taken line for line from percep object detection code + auto spiralSearchInImg(size_t xCenter, size_t yCenter) -> std::optional; + }; + +} // namespace mrover diff --git a/src/perception/click_ik/main.cpp b/src/perception/click_ik/main.cpp new file mode 100644 index 000000000..176cd6250 --- /dev/null +++ b/src/perception/click_ik/main.cpp @@ -0,0 +1,21 @@ +#include "click_ik.hpp" + +#ifdef MROVER_IS_NODELET + +#include +PLUGINLIB_EXPORT_CLASS(mrover::ClickIkNodelet, nodelet::Nodelet) + +#else + +auto main(int argc, char** argv) -> int { + ros::init(argc, argv, "click_ik"); + + // Start the ClickIK Nodelet + nodelet::Loader nodelet; + nodelet.load(ros::this_node::getName(), "mrover/ClickIkNodelet", ros::names::getRemappings(), {}); + ros::spin(); + + return EXIT_SUCCESS; +} + +#endif diff --git a/src/perception/click_ik/pch.hpp b/src/perception/click_ik/pch.hpp new file mode 100644 index 000000000..523a761d7 --- /dev/null +++ b/src/perception/click_ik/pch.hpp @@ -0,0 +1,35 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + + +#include "../teleoperation/arm_controller/arm_controller.hpp" +#include "mrover/ClickIkAction.h" +#include "mrover/ClickIkGoal.h" +#include "mrover/IK.h" diff --git a/src/perception/tag_detector/zed/tag_detector.hpp b/src/perception/tag_detector/zed/tag_detector.hpp index 395958424..981f53098 100644 --- a/src/perception/tag_detector/zed/tag_detector.hpp +++ b/src/perception/tag_detector/zed/tag_detector.hpp @@ -12,7 +12,6 @@ namespace mrover { }; class TagDetectorNodelet : public nodelet::Nodelet { - ros::NodeHandle mNh, mPnh; ros::Publisher mImgPub; @@ -45,8 +44,8 @@ namespace mrover { std::vector> mImmediateCorners; std::vector mImmediateIds; std::unordered_map mTags; - dynamic_reconfigure::Server mConfigServer; - dynamic_reconfigure::Server::CallbackType mCallbackType; + dynamic_reconfigure::Server mConfigServer; + dynamic_reconfigure::Server::CallbackType mCallbackType; LoopProfiler mProfiler{"Tag Detector"}; diff --git a/src/simulator/simulator.controls.cpp b/src/simulator/simulator.controls.cpp index f46cff1df..8fec9cb1a 100644 --- a/src/simulator/simulator.controls.cpp +++ b/src/simulator/simulator.controls.cpp @@ -154,11 +154,10 @@ namespace mrover { auto SimulatorNodelet::userControls(Clock::duration dt) -> void { if (mPublishIk) { IK ik; - ik.target.header.stamp = ros::Time::now(); - ik.target.header.frame_id = "arm_base_link"; ik.target.pose.position.x = mIkTarget.x(); ik.target.pose.position.y = mIkTarget.y(); ik.target.pose.position.z = mIkTarget.z(); + ik.target.header.frame_id = "arm_base_link"; mIkTargetPub.publish(ik); } diff --git a/src/simulator/simulator.hpp b/src/simulator/simulator.hpp index 9565c14db..8e10be866 100644 --- a/src/simulator/simulator.hpp +++ b/src/simulator/simulator.hpp @@ -84,6 +84,7 @@ namespace mrover { int index{}; boost::container::static_vector, 2> visualUniforms; boost::container::static_vector, 2> collisionUniforms; + Clock::time_point lastUpdate = Clock::now(); }; std::string name; @@ -222,7 +223,7 @@ namespace mrover { tf2_ros::TransformBroadcaster mTfBroadcaster; bool mPublishIk = true; - Eigen::Vector3f mIkTarget{0.382, 0.01, -0.217}; + Eigen::Vector3f mIkTarget{0.35, 0, -0.25}; ros::Publisher mIkTargetPub; R3 mGpsLinearizationReferencePoint{}; @@ -421,7 +422,7 @@ namespace mrover { template auto forEachWithMotor(N const& names, V const& values, F&& function) -> void { if (auto it = mUrdfs.find("rover"); it != mUrdfs.end()) { - URDF const& rover = it->second; + URDF& rover = it->second; for (auto const& combined: boost::combine(names, values)) { std::string const& name = boost::get<0>(combined); @@ -431,6 +432,7 @@ namespace mrover { std::string const& name = urdfName.value(); int linkIndex = rover.linkNameToMeta.at(name).index; + rover.linkNameToMeta.at(name).lastUpdate = Clock::now(); auto* motor = std::bit_cast(rover.physics->getLink(linkIndex).m_userPtr); assert(motor); diff --git a/src/simulator/simulator.physics.cpp b/src/simulator/simulator.physics.cpp index 0cc1acdca..4a583d295 100644 --- a/src/simulator/simulator.physics.cpp +++ b/src/simulator/simulator.physics.cpp @@ -50,6 +50,19 @@ namespace mrover { motor->setMaxAppliedImpulse(0.5); motor->setPositionTarget(0); } + // check if arm motor commands have expired + // TODO(quintin): fix hard-coded names? + for (auto const& name: {"arm_a_link", "arm_b_link", "arm_c_link", "arm_d_link", "arm_e_link"}) { + bool expired = std::chrono::duration_cast(Clock::now() - rover.linkNameToMeta.at(name).lastUpdate).count() > 20; + if (expired) { + int linkIndex = rover.linkNameToMeta.at(name).index; + auto* motor = std::bit_cast(rover.physics->getLink(linkIndex).m_userPtr); + assert(motor); + motor->setVelocityTarget(0, 1); + // set p gain to 0 to stop position control + motor->setPositionTarget(0, 0); + } + } } float updateDuration = std::clamp(std::chrono::duration_cast>(dt).count(), 0.0f, 0.1f); diff --git a/src/teleoperation/arm_controller/arm_controller.cpp b/src/teleoperation/arm_controller/arm_controller.cpp index b0d7d23af..1357c4899 100644 --- a/src/teleoperation/arm_controller/arm_controller.cpp +++ b/src/teleoperation/arm_controller/arm_controller.cpp @@ -1,10 +1,18 @@ #include "arm_controller.hpp" +#include "lie.hpp" +#include +#include +#include namespace mrover { - ArmController::ArmController() { - mIkSubscriber = mNh.subscribe("arm_ik", 1, &ArmController::ik_callback, this); - mPositionPublisher = mNh.advertise("arm_position_cmd", 1); + ros::NodeHandle nh; + // sleep(2); + double frequency{}; + mNh.param("/frequency", frequency, 100); + mIkSubscriber = nh.subscribe("arm_ik", 1, &ArmController::ik_callback, this); + mPositionPublisher = nh.advertise("arm_position_cmd", 1); + mStatusPublisher = nh.advertise("arm_status", 1); } auto yawSo3(double r) -> SO3d { @@ -16,21 +24,23 @@ 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 target_frame_to_arm_base_link; try { - target_frame_to_arm_b_static = SE3Conversions::fromTfTree(mTfBuffer, ik_target.target.header.frame_id, "arm_base_link"); + target_frame_to_arm_base_link = SE3Conversions::fromTfTree(mTfBuffer, ik_target.target.header.frame_id, "arm_base_link"); } catch (...) { ROS_WARN_THROTTLE(1, "Failed to resolve information about target frame"); 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); + Eigen::Vector4d target_in_arm_base_link = target_frame_to_arm_base_link.transform() * target; + double x = target_in_arm_base_link.x() - END_EFFECTOR_LENGTH; // shift back by the length of the end effector + double z = target_in_arm_base_link.z(); + double y = target_in_arm_base_link.y(); + SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_target", "arm_base_link", target_frame_to_arm_base_link); + // final angle of end effector double gamma = 0; + double x3 = x - LINK_DE * std::cos(gamma); double z3 = z - LINK_DE * std::sin(gamma); @@ -64,11 +74,20 @@ namespace mrover { positions.positions[2] = static_cast(q2); positions.positions[3] = static_cast(q3); mPositionPublisher.publish(positions); + ArmStatus status; + status.status = true; + mStatusPublisher.publish(status); } else { ROS_WARN_THROTTLE(1, "Can not reach target within arm limits!"); + ArmStatus status; + status.status = false; + mStatusPublisher.publish(status); } } else { ROS_WARN_THROTTLE(1, "Can not solve for arm target!"); + ArmStatus status; + status.status = false; + mStatusPublisher.publish(status); } } @@ -80,4 +99,4 @@ auto main(int argc, char** argv) -> int { mrover::ArmController armController; ros::spin(); -} \ No newline at end of file +} diff --git a/src/teleoperation/arm_controller/arm_controller.hpp b/src/teleoperation/arm_controller/arm_controller.hpp index 98d989232..2d42868d0 100644 --- a/src/teleoperation/arm_controller/arm_controller.hpp +++ b/src/teleoperation/arm_controller/arm_controller.hpp @@ -9,6 +9,7 @@ namespace mrover { [[maybe_unused]] ros::Subscriber mIkSubscriber; ros::NodeHandle mNh; ros::Publisher mPositionPublisher; + ros::Publisher mStatusPublisher; tf2_ros::TransformBroadcaster mTfBroadcaster; tf2_ros::Buffer mTfBuffer{}; tf2_ros::TransformListener mTfListener{mTfBuffer}; diff --git a/src/teleoperation/arm_controller/pch.hpp b/src/teleoperation/arm_controller/pch.hpp index 2f68bad08..fb01e862a 100644 --- a/src/teleoperation/arm_controller/pch.hpp +++ b/src/teleoperation/arm_controller/pch.hpp @@ -4,6 +4,7 @@ #include #include +#include #include #include @@ -11,5 +12,5 @@ #include #include - -#include +#include +#include diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py index 1b83fdea9..fe2fe0eda 100644 --- a/src/teleoperation/backend/consumers.py +++ b/src/teleoperation/backend/consumers.py @@ -31,11 +31,16 @@ Spectral, ScienceThermistors, HeaterData, + ClickIkAction, + ClickIkGoal, + ClickIkFeedback, ) +import actionlib from mrover.srv import EnableAuton, AdjustMotor, ChangeCameras, CapturePanorama from sensor_msgs.msg import NavSatFix, Temperature, RelativeHumidity from std_msgs.msg import String from std_srvs.srv import SetBool, Trigger + from tf.transformations import euler_from_quaternion from util.SE3 import SE3 @@ -105,6 +110,9 @@ def connect(self): self.science_spectral = rospy.Subscriber("/science_spectral", Spectral, self.science_spectral_callback) self.cmd_vel = rospy.Subscriber("/cmd_vel", Twist, self.cmd_vel_callback) + # Action clients + self.click_ik_client = actionlib.SimpleActionClient("do_click_ik", ClickIkAction) + # Services self.laser_service = rospy.ServiceProxy("enable_arm_laser", SetBool) self.enable_auton = rospy.ServiceProxy("enable_auton", EnableAuton) @@ -211,6 +219,10 @@ def receive(self, text_data): self.save_basic_waypoint_list(message) elif message["type"] == "get_basic_waypoint_list": self.get_basic_waypoint_list(message) + elif message["type"] == "start_click_ik": + self.start_click_ik(message) + elif message["type"] == "cancel_click_ik": + self.cancel_click_ik(message) elif message["type"] == "download_csv": self.download_csv(message) except Exception as e: @@ -681,14 +693,6 @@ def capture_panorama(self) -> None: except rospy.ServiceException as e: print(f"Service call failed: {e}") - # def capture_photo(self): - # try: - # response = self.capture_photo_srv() - # image = response.photo - # self.image_callback(image) - # except rospy.ServiceException as e: - # print(f"Service call failed: {e}") - # def image_callback(self, msg): # bridge = CvBridge() # try: @@ -800,6 +804,19 @@ def flight_attitude_listener(self): rate.sleep() + def start_click_ik(self, msg) -> None: + goal = ClickIkGoal() + goal.pointInImageX = msg["data"]["x"] + goal.pointInImageY = msg["data"]["y"] + + def feedback_cb(feedback: ClickIkFeedback) -> None: + self.send(text_data=json.dumps({"type": "click_ik_feedback", "distance": feedback.distance})) + + self.click_ik_client.send_goal(goal, feedback_cb=feedback_cb) + + def cancel_click_ik(self, msg) -> None: + self.click_ik_client.cancel_all_goals() + 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/src/components/CameraFeed.vue b/src/teleoperation/frontend/src/components/CameraFeed.vue index efbce30af..dcf49acd3 100644 --- a/src/teleoperation/frontend/src/components/CameraFeed.vue +++ b/src/teleoperation/frontend/src/components/CameraFeed.vue @@ -53,6 +53,8 @@ export default defineComponent({ // IK Mode IKCam: false, + // SA/ISH Mode + site: 'A', quality: 2 } }, @@ -64,19 +66,42 @@ export default defineComponent({ mounted: function () { this.startStream(this.id) - // this.$nextTick(() => { - // const canvas: HTMLCanvasElement = document.getElementById( - // 'stream-' + this.id - // ) as HTMLCanvasElement - // const context = canvas.getContext('2d') ?? new CanvasRenderingContext2D() - // context.fillStyle = 'black' - // context.fillRect(0, 0, canvas.width, canvas.height) - // }) + this.$nextTick(() => { + const canvas: HTMLCanvasElement = document.getElementById( + 'stream-' + this.id + ) as HTMLCanvasElement + const context = canvas.getContext('2d') ?? new CanvasRenderingContext2D() + context.fillStyle = 'black' + context.fillRect(0, 0, canvas.width, canvas.height) + }) }, methods: { ...mapActions('websocket', ['sendMessage']), + downloadScreenshot: function () { + const currentdate = new Date() + const dateString = + currentdate.getMonth() + + 1 + + '-' + + currentdate.getDate() + + '-' + + currentdate.getFullYear() + + ' @ ' + + currentdate.getHours() + + ':' + + currentdate.getMinutes() + + ':' + + currentdate.getSeconds() + var link = document.createElement('a') + console.log('dateString', dateString) + link.download = 'Site_' + this.site + ' ' + dateString + '.png' + let canvas = document.getElementById('stream-' + this.id) as HTMLCanvasElement + link.href = canvas.toDataURL() + link.click() + link.remove() + }, handleClick: function (event: MouseEvent) { if (this.IKCam && this.mission === 'ik') { this.sendMessage({ type: 'start_click_ik', data: { x: event.offsetX, y: event.offsetY } }) @@ -90,11 +115,11 @@ export default defineComponent({ resolution: parseInt(this.quality) }) }, - + toggleIKMode: function () { this.IKCam = !this.IKCam }, - + startStream(number: Number) { // This function is called as a retry when the websocket closes // If our component goes away (unmounts) we should stop trying to reconnect diff --git a/src/teleoperation/streaming/embuild/stream_client.js b/src/teleoperation/streaming/embuild/stream_client.js new file mode 100644 index 000000000..6737bc774 --- /dev/null +++ b/src/teleoperation/streaming/embuild/stream_client.js @@ -0,0 +1 @@ +var Module=typeof Module!="undefined"?Module:{};var moduleOverrides=Object.assign({},Module);var arguments_=[];var thisProgram="./this.program";var quit_=(status,toThrow)=>{throw toThrow};var ENVIRONMENT_IS_WEB=typeof window=="object";var ENVIRONMENT_IS_WORKER=typeof importScripts=="function";var ENVIRONMENT_IS_NODE=typeof process=="object"&&typeof process.versions=="object"&&typeof process.versions.node=="string";var scriptDirectory="";function locateFile(path){if(Module["locateFile"]){return Module["locateFile"](path,scriptDirectory)}return scriptDirectory+path}var read_,readAsync,readBinary;if(ENVIRONMENT_IS_NODE){var fs=require("fs");var nodePath=require("path");if(ENVIRONMENT_IS_WORKER){scriptDirectory=nodePath.dirname(scriptDirectory)+"/"}else{scriptDirectory=__dirname+"/"}read_=(filename,binary)=>{filename=isFileURI(filename)?new URL(filename):nodePath.normalize(filename);return fs.readFileSync(filename,binary?undefined:"utf8")};readBinary=filename=>{var ret=read_(filename,true);if(!ret.buffer){ret=new Uint8Array(ret)}return ret};readAsync=(filename,onload,onerror,binary=true)=>{filename=isFileURI(filename)?new URL(filename):nodePath.normalize(filename);fs.readFile(filename,binary?undefined:"utf8",(err,data)=>{if(err)onerror(err);else onload(binary?data.buffer:data)})};if(!Module["thisProgram"]&&process.argv.length>1){thisProgram=process.argv[1].replace(/\\/g,"/")}arguments_=process.argv.slice(2);if(typeof module!="undefined"){module["exports"]=Module}process.on("uncaughtException",ex=>{if(ex!=="unwind"&&!(ex instanceof ExitStatus)&&!(ex.context instanceof ExitStatus)){throw ex}});quit_=(status,toThrow)=>{process.exitCode=status;throw toThrow};Module["inspect"]=()=>"[Emscripten Module object]"}else if(ENVIRONMENT_IS_WEB||ENVIRONMENT_IS_WORKER){if(ENVIRONMENT_IS_WORKER){scriptDirectory=self.location.href}else if(typeof document!="undefined"&&document.currentScript){scriptDirectory=document.currentScript.src}if(scriptDirectory.indexOf("blob:")!==0){scriptDirectory=scriptDirectory.substr(0,scriptDirectory.replace(/[?#].*/,"").lastIndexOf("/")+1)}else{scriptDirectory=""}{read_=url=>{var xhr=new XMLHttpRequest;xhr.open("GET",url,false);xhr.send(null);return xhr.responseText};if(ENVIRONMENT_IS_WORKER){readBinary=url=>{var xhr=new XMLHttpRequest;xhr.open("GET",url,false);xhr.responseType="arraybuffer";xhr.send(null);return new Uint8Array(xhr.response)}}readAsync=(url,onload,onerror)=>{var xhr=new XMLHttpRequest;xhr.open("GET",url,true);xhr.responseType="arraybuffer";xhr.onload=()=>{if(xhr.status==200||xhr.status==0&&xhr.response){onload(xhr.response);return}onerror()};xhr.onerror=onerror;xhr.send(null)}}}else{}var out=Module["print"]||console.log.bind(console);var err=Module["printErr"]||console.error.bind(console);Object.assign(Module,moduleOverrides);moduleOverrides=null;if(Module["arguments"])arguments_=Module["arguments"];if(Module["thisProgram"])thisProgram=Module["thisProgram"];if(Module["quit"])quit_=Module["quit"];var wasmBinary;if(Module["wasmBinary"])wasmBinary=Module["wasmBinary"];if(typeof WebAssembly!="object"){abort("no native wasm support detected")}var wasmMemory;var ABORT=false;var EXITSTATUS;var HEAP8,HEAPU8,HEAP16,HEAPU16,HEAP32,HEAPU32,HEAPF32,HEAPF64;function updateMemoryViews(){var b=wasmMemory.buffer;Module["HEAP8"]=HEAP8=new Int8Array(b);Module["HEAP16"]=HEAP16=new Int16Array(b);Module["HEAPU8"]=HEAPU8=new Uint8Array(b);Module["HEAPU16"]=HEAPU16=new Uint16Array(b);Module["HEAP32"]=HEAP32=new Int32Array(b);Module["HEAPU32"]=HEAPU32=new Uint32Array(b);Module["HEAPF32"]=HEAPF32=new Float32Array(b);Module["HEAPF64"]=HEAPF64=new Float64Array(b)}var __ATPRERUN__=[];var __ATINIT__=[];var __ATMAIN__=[];var __ATPOSTRUN__=[];var runtimeInitialized=false;function preRun(){if(Module["preRun"]){if(typeof Module["preRun"]=="function")Module["preRun"]=[Module["preRun"]];while(Module["preRun"].length){addOnPreRun(Module["preRun"].shift())}}callRuntimeCallbacks(__ATPRERUN__)}function initRuntime(){runtimeInitialized=true;callRuntimeCallbacks(__ATINIT__)}function preMain(){callRuntimeCallbacks(__ATMAIN__)}function postRun(){if(Module["postRun"]){if(typeof Module["postRun"]=="function")Module["postRun"]=[Module["postRun"]];while(Module["postRun"].length){addOnPostRun(Module["postRun"].shift())}}callRuntimeCallbacks(__ATPOSTRUN__)}function addOnPreRun(cb){__ATPRERUN__.unshift(cb)}function addOnInit(cb){__ATINIT__.unshift(cb)}function addOnPostRun(cb){__ATPOSTRUN__.unshift(cb)}var runDependencies=0;var runDependencyWatcher=null;var dependenciesFulfilled=null;function addRunDependency(id){runDependencies++;if(Module["monitorRunDependencies"]){Module["monitorRunDependencies"](runDependencies)}}function removeRunDependency(id){runDependencies--;if(Module["monitorRunDependencies"]){Module["monitorRunDependencies"](runDependencies)}if(runDependencies==0){if(runDependencyWatcher!==null){clearInterval(runDependencyWatcher);runDependencyWatcher=null}if(dependenciesFulfilled){var callback=dependenciesFulfilled;dependenciesFulfilled=null;callback()}}}function abort(what){if(Module["onAbort"]){Module["onAbort"](what)}what="Aborted("+what+")";err(what);ABORT=true;EXITSTATUS=1;what+=". Build with -sASSERTIONS for more info.";var e=new WebAssembly.RuntimeError(what);throw e}var dataURIPrefix="data:application/octet-stream;base64,";var isDataURI=filename=>filename.startsWith(dataURIPrefix);var isFileURI=filename=>filename.startsWith("file://");var wasmBinaryFile;wasmBinaryFile="stream_client.wasm";if(!isDataURI(wasmBinaryFile)){wasmBinaryFile=locateFile(wasmBinaryFile)}function getBinarySync(file){if(file==wasmBinaryFile&&wasmBinary){return new Uint8Array(wasmBinary)}if(readBinary){return readBinary(file)}throw"both async and sync fetching of the wasm failed"}function getBinaryPromise(binaryFile){if(!wasmBinary&&(ENVIRONMENT_IS_WEB||ENVIRONMENT_IS_WORKER)){if(typeof fetch=="function"&&!isFileURI(binaryFile)){return fetch(binaryFile,{credentials:"same-origin"}).then(response=>{if(!response["ok"]){throw"failed to load wasm binary file at '"+binaryFile+"'"}return response["arrayBuffer"]()}).catch(()=>getBinarySync(binaryFile))}else if(readAsync){return new Promise((resolve,reject)=>{readAsync(binaryFile,response=>resolve(new Uint8Array(response)),reject)})}}return Promise.resolve().then(()=>getBinarySync(binaryFile))}function instantiateArrayBuffer(binaryFile,imports,receiver){return getBinaryPromise(binaryFile).then(binary=>WebAssembly.instantiate(binary,imports)).then(instance=>instance).then(receiver,reason=>{err(`failed to asynchronously prepare wasm: ${reason}`);abort(reason)})}function instantiateAsync(binary,binaryFile,imports,callback){if(!binary&&typeof WebAssembly.instantiateStreaming=="function"&&!isDataURI(binaryFile)&&!isFileURI(binaryFile)&&!ENVIRONMENT_IS_NODE&&typeof fetch=="function"){return fetch(binaryFile,{credentials:"same-origin"}).then(response=>{var result=WebAssembly.instantiateStreaming(response,imports);return result.then(callback,function(reason){err(`wasm streaming compile failed: ${reason}`);err("falling back to ArrayBuffer instantiation");return instantiateArrayBuffer(binaryFile,imports,callback)})})}return instantiateArrayBuffer(binaryFile,imports,callback)}function createWasm(){var info={"a":wasmImports};function receiveInstance(instance,module){wasmExports=instance.exports;wasmMemory=wasmExports["G"];updateMemoryViews();wasmTable=wasmExports["N"];addOnInit(wasmExports["H"]);removeRunDependency("wasm-instantiate");return wasmExports}addRunDependency("wasm-instantiate");function receiveInstantiationResult(result){receiveInstance(result["instance"])}if(Module["instantiateWasm"]){try{return Module["instantiateWasm"](info,receiveInstance)}catch(e){err(`Module.instantiateWasm callback failed with error: ${e}`);return false}}instantiateAsync(wasmBinary,wasmBinaryFile,info,receiveInstantiationResult);return{}}var ASM_CONSTS={19476:($0,$1,$2,$3,$4,$5,$6,$7)=>{const width=$0;const height=$1;const y=HEAPU8.subarray($2,$2+width*height);const u=HEAPU8.subarray($3,$3+width*height/4);const v=HEAPU8.subarray($4,$4+width*height/4);const yStride=$5;const uStride=$6;const vStride=$7;const canvas=document.getElementById("canvas");const ctx=canvas.getContext("2d");if(Module.imageBuffer===undefined){Module.imageBuffer=ctx.createImageData(width,height);canvas.width=width;canvas.height=height}const imageBuffer=Module.imageBuffer;const imageData=imageBuffer.data;for(let i=0;i{while(callbacks.length>0){callbacks.shift()(Module)}};var noExitRuntime=Module["noExitRuntime"]||true;function ExceptionInfo(excPtr){this.excPtr=excPtr;this.ptr=excPtr-24;this.set_type=function(type){HEAPU32[this.ptr+4>>2]=type};this.get_type=function(){return HEAPU32[this.ptr+4>>2]};this.set_destructor=function(destructor){HEAPU32[this.ptr+8>>2]=destructor};this.get_destructor=function(){return HEAPU32[this.ptr+8>>2]};this.set_caught=function(caught){caught=caught?1:0;HEAP8[this.ptr+12>>0]=caught};this.get_caught=function(){return HEAP8[this.ptr+12>>0]!=0};this.set_rethrown=function(rethrown){rethrown=rethrown?1:0;HEAP8[this.ptr+13>>0]=rethrown};this.get_rethrown=function(){return HEAP8[this.ptr+13>>0]!=0};this.init=function(type,destructor){this.set_adjusted_ptr(0);this.set_type(type);this.set_destructor(destructor)};this.set_adjusted_ptr=function(adjustedPtr){HEAPU32[this.ptr+16>>2]=adjustedPtr};this.get_adjusted_ptr=function(){return HEAPU32[this.ptr+16>>2]};this.get_exception_ptr=function(){var isPointer=___cxa_is_pointer_type(this.get_type());if(isPointer){return HEAPU32[this.excPtr>>2]}var adjusted=this.get_adjusted_ptr();if(adjusted!==0)return adjusted;return this.excPtr}}var exceptionLast=0;var uncaughtExceptionCount=0;var ___cxa_throw=(ptr,type,destructor)=>{var info=new ExceptionInfo(ptr);info.init(type,destructor);exceptionLast=ptr;uncaughtExceptionCount++;throw exceptionLast};var __embind_register_bigint=(primitiveType,name,size,minRange,maxRange)=>{};var embind_init_charCodes=()=>{var codes=new Array(256);for(var i=0;i<256;++i){codes[i]=String.fromCharCode(i)}embind_charCodes=codes};var embind_charCodes;var readLatin1String=ptr=>{var ret="";var c=ptr;while(HEAPU8[c]){ret+=embind_charCodes[HEAPU8[c++]]}return ret};var awaitingDependencies={};var registeredTypes={};var typeDependencies={};var BindingError;var throwBindingError=message=>{throw new BindingError(message)};var InternalError;function sharedRegisterType(rawType,registeredInstance,options={}){var name=registeredInstance.name;if(!rawType){throwBindingError(`type "${name}" must have a positive integer typeid pointer`)}if(registeredTypes.hasOwnProperty(rawType)){if(options.ignoreDuplicateRegistrations){return}else{throwBindingError(`Cannot register type '${name}' twice`)}}registeredTypes[rawType]=registeredInstance;delete typeDependencies[rawType];if(awaitingDependencies.hasOwnProperty(rawType)){var callbacks=awaitingDependencies[rawType];delete awaitingDependencies[rawType];callbacks.forEach(cb=>cb())}}function registerType(rawType,registeredInstance,options={}){if(!("argPackAdvance"in registeredInstance)){throw new TypeError("registerType registeredInstance requires argPackAdvance")}return sharedRegisterType(rawType,registeredInstance,options)}var GenericWireTypeSize=8;var __embind_register_bool=(rawType,name,trueValue,falseValue)=>{name=readLatin1String(name);registerType(rawType,{name:name,"fromWireType":function(wt){return!!wt},"toWireType":function(destructors,o){return o?trueValue:falseValue},"argPackAdvance":GenericWireTypeSize,"readValueFromPointer":function(pointer){return this["fromWireType"](HEAPU8[pointer])},destructorFunction:null})};function handleAllocatorInit(){Object.assign(HandleAllocator.prototype,{get(id){return this.allocated[id]},has(id){return this.allocated[id]!==undefined},allocate(handle){var id=this.freelist.pop()||this.allocated.length;this.allocated[id]=handle;return id},free(id){this.allocated[id]=undefined;this.freelist.push(id)}})}function HandleAllocator(){this.allocated=[undefined];this.freelist=[]}var emval_handles=new HandleAllocator;var __emval_decref=handle=>{if(handle>=emval_handles.reserved&&0===--emval_handles.get(handle).refcount){emval_handles.free(handle)}};var count_emval_handles=()=>{var count=0;for(var i=emval_handles.reserved;i{emval_handles.allocated.push({value:undefined},{value:null},{value:true},{value:false});emval_handles.reserved=emval_handles.allocated.length;Module["count_emval_handles"]=count_emval_handles};var Emval={toValue:handle=>{if(!handle){throwBindingError("Cannot use deleted val. handle = "+handle)}return emval_handles.get(handle).value},toHandle:value=>{switch(value){case undefined:return 1;case null:return 2;case true:return 3;case false:return 4;default:{return emval_handles.allocate({refcount:1,value:value})}}}};function simpleReadValueFromPointer(pointer){return this["fromWireType"](HEAP32[pointer>>2])}var __embind_register_emval=(rawType,name)=>{name=readLatin1String(name);registerType(rawType,{name:name,"fromWireType":handle=>{var rv=Emval.toValue(handle);__emval_decref(handle);return rv},"toWireType":(destructors,value)=>Emval.toHandle(value),"argPackAdvance":GenericWireTypeSize,"readValueFromPointer":simpleReadValueFromPointer,destructorFunction:null})};var floatReadValueFromPointer=(name,width)=>{switch(width){case 4:return function(pointer){return this["fromWireType"](HEAPF32[pointer>>2])};case 8:return function(pointer){return this["fromWireType"](HEAPF64[pointer>>3])};default:throw new TypeError(`invalid float width (${width}): ${name}`)}};var __embind_register_float=(rawType,name,size)=>{name=readLatin1String(name);registerType(rawType,{name:name,"fromWireType":value=>value,"toWireType":(destructors,value)=>value,"argPackAdvance":GenericWireTypeSize,"readValueFromPointer":floatReadValueFromPointer(name,size),destructorFunction:null})};var integerReadValueFromPointer=(name,width,signed)=>{switch(width){case 1:return signed?pointer=>HEAP8[pointer>>0]:pointer=>HEAPU8[pointer>>0];case 2:return signed?pointer=>HEAP16[pointer>>1]:pointer=>HEAPU16[pointer>>1];case 4:return signed?pointer=>HEAP32[pointer>>2]:pointer=>HEAPU32[pointer>>2];default:throw new TypeError(`invalid integer width (${width}): ${name}`)}};var __embind_register_integer=(primitiveType,name,size,minRange,maxRange)=>{name=readLatin1String(name);if(maxRange===-1){maxRange=4294967295}var fromWireType=value=>value;if(minRange===0){var bitshift=32-8*size;fromWireType=value=>value<>>bitshift}var isUnsignedType=name.includes("unsigned");var checkAssertions=(value,toTypeName)=>{};var toWireType;if(isUnsignedType){toWireType=function(destructors,value){checkAssertions(value,this.name);return value>>>0}}else{toWireType=function(destructors,value){checkAssertions(value,this.name);return value}}registerType(primitiveType,{name:name,"fromWireType":fromWireType,"toWireType":toWireType,"argPackAdvance":GenericWireTypeSize,"readValueFromPointer":integerReadValueFromPointer(name,size,minRange!==0),destructorFunction:null})};var __embind_register_memory_view=(rawType,dataTypeIndex,name)=>{var typeMapping=[Int8Array,Uint8Array,Int16Array,Uint16Array,Int32Array,Uint32Array,Float32Array,Float64Array];var TA=typeMapping[dataTypeIndex];function decodeMemoryView(handle){var size=HEAPU32[handle>>2];var data=HEAPU32[handle+4>>2];return new TA(HEAP8.buffer,data,size)}name=readLatin1String(name);registerType(rawType,{name:name,"fromWireType":decodeMemoryView,"argPackAdvance":GenericWireTypeSize,"readValueFromPointer":decodeMemoryView},{ignoreDuplicateRegistrations:true})};function readPointer(pointer){return this["fromWireType"](HEAPU32[pointer>>2])}var stringToUTF8Array=(str,heap,outIdx,maxBytesToWrite)=>{if(!(maxBytesToWrite>0))return 0;var startIdx=outIdx;var endIdx=outIdx+maxBytesToWrite-1;for(var i=0;i=55296&&u<=57343){var u1=str.charCodeAt(++i);u=65536+((u&1023)<<10)|u1&1023}if(u<=127){if(outIdx>=endIdx)break;heap[outIdx++]=u}else if(u<=2047){if(outIdx+1>=endIdx)break;heap[outIdx++]=192|u>>6;heap[outIdx++]=128|u&63}else if(u<=65535){if(outIdx+2>=endIdx)break;heap[outIdx++]=224|u>>12;heap[outIdx++]=128|u>>6&63;heap[outIdx++]=128|u&63}else{if(outIdx+3>=endIdx)break;heap[outIdx++]=240|u>>18;heap[outIdx++]=128|u>>12&63;heap[outIdx++]=128|u>>6&63;heap[outIdx++]=128|u&63}}heap[outIdx]=0;return outIdx-startIdx};var stringToUTF8=(str,outPtr,maxBytesToWrite)=>stringToUTF8Array(str,HEAPU8,outPtr,maxBytesToWrite);var lengthBytesUTF8=str=>{var len=0;for(var i=0;i=55296&&c<=57343){len+=4;++i}else{len+=3}}return len};var UTF8Decoder=typeof TextDecoder!="undefined"?new TextDecoder("utf8"):undefined;var UTF8ArrayToString=(heapOrArray,idx,maxBytesToRead)=>{var endIdx=idx+maxBytesToRead;var endPtr=idx;while(heapOrArray[endPtr]&&!(endPtr>=endIdx))++endPtr;if(endPtr-idx>16&&heapOrArray.buffer&&UTF8Decoder){return UTF8Decoder.decode(heapOrArray.subarray(idx,endPtr))}var str="";while(idx>10,56320|ch&1023)}}return str};var UTF8ToString=(ptr,maxBytesToRead)=>ptr?UTF8ArrayToString(HEAPU8,ptr,maxBytesToRead):"";var __embind_register_std_string=(rawType,name)=>{name=readLatin1String(name);var stdStringIsUTF8=name==="std::string";registerType(rawType,{name:name,"fromWireType"(value){var length=HEAPU32[value>>2];var payload=value+4;var str;if(stdStringIsUTF8){var decodeStartPtr=payload;for(var i=0;i<=length;++i){var currentBytePtr=payload+i;if(i==length||HEAPU8[currentBytePtr]==0){var maxRead=currentBytePtr-decodeStartPtr;var stringSegment=UTF8ToString(decodeStartPtr,maxRead);if(str===undefined){str=stringSegment}else{str+=String.fromCharCode(0);str+=stringSegment}decodeStartPtr=currentBytePtr+1}}}else{var a=new Array(length);for(var i=0;i>2]=length;if(stdStringIsUTF8&&valueIsOfTypeString){stringToUTF8(value,ptr,length+1)}else{if(valueIsOfTypeString){for(var i=0;i255){_free(ptr);throwBindingError("String has UTF-16 code units that do not fit in 8 bits")}HEAPU8[ptr+i]=charCode}}else{for(var i=0;i{var endPtr=ptr;var idx=endPtr>>1;var maxIdx=idx+maxBytesToRead/2;while(!(idx>=maxIdx)&&HEAPU16[idx])++idx;endPtr=idx<<1;if(endPtr-ptr>32&&UTF16Decoder)return UTF16Decoder.decode(HEAPU8.subarray(ptr,endPtr));var str="";for(var i=0;!(i>=maxBytesToRead/2);++i){var codeUnit=HEAP16[ptr+i*2>>1];if(codeUnit==0)break;str+=String.fromCharCode(codeUnit)}return str};var stringToUTF16=(str,outPtr,maxBytesToWrite)=>{if(maxBytesToWrite===undefined){maxBytesToWrite=2147483647}if(maxBytesToWrite<2)return 0;maxBytesToWrite-=2;var startPtr=outPtr;var numCharsToWrite=maxBytesToWrite>1]=codeUnit;outPtr+=2}HEAP16[outPtr>>1]=0;return outPtr-startPtr};var lengthBytesUTF16=str=>str.length*2;var UTF32ToString=(ptr,maxBytesToRead)=>{var i=0;var str="";while(!(i>=maxBytesToRead/4)){var utf32=HEAP32[ptr+i*4>>2];if(utf32==0)break;++i;if(utf32>=65536){var ch=utf32-65536;str+=String.fromCharCode(55296|ch>>10,56320|ch&1023)}else{str+=String.fromCharCode(utf32)}}return str};var stringToUTF32=(str,outPtr,maxBytesToWrite)=>{if(maxBytesToWrite===undefined){maxBytesToWrite=2147483647}if(maxBytesToWrite<4)return 0;var startPtr=outPtr;var endPtr=startPtr+maxBytesToWrite-4;for(var i=0;i=55296&&codeUnit<=57343){var trailSurrogate=str.charCodeAt(++i);codeUnit=65536+((codeUnit&1023)<<10)|trailSurrogate&1023}HEAP32[outPtr>>2]=codeUnit;outPtr+=4;if(outPtr+4>endPtr)break}HEAP32[outPtr>>2]=0;return outPtr-startPtr};var lengthBytesUTF32=str=>{var len=0;for(var i=0;i=55296&&codeUnit<=57343)++i;len+=4}return len};var __embind_register_std_wstring=(rawType,charSize,name)=>{name=readLatin1String(name);var decodeString,encodeString,getHeap,lengthBytesUTF,shift;if(charSize===2){decodeString=UTF16ToString;encodeString=stringToUTF16;lengthBytesUTF=lengthBytesUTF16;getHeap=()=>HEAPU16;shift=1}else if(charSize===4){decodeString=UTF32ToString;encodeString=stringToUTF32;lengthBytesUTF=lengthBytesUTF32;getHeap=()=>HEAPU32;shift=2}registerType(rawType,{name:name,"fromWireType":value=>{var length=HEAPU32[value>>2];var HEAP=getHeap();var str;var decodeStartPtr=value+4;for(var i=0;i<=length;++i){var currentBytePtr=value+4+i*charSize;if(i==length||HEAP[currentBytePtr>>shift]==0){var maxReadBytes=currentBytePtr-decodeStartPtr;var stringSegment=decodeString(decodeStartPtr,maxReadBytes);if(str===undefined){str=stringSegment}else{str+=String.fromCharCode(0);str+=stringSegment}decodeStartPtr=currentBytePtr+charSize}}_free(value);return str},"toWireType":(destructors,value)=>{if(!(typeof value=="string")){throwBindingError(`Cannot pass non-string to C++ string type ${name}`)}var length=lengthBytesUTF(value);var ptr=_malloc(4+length+charSize);HEAPU32[ptr>>2]=length>>shift;encodeString(value,ptr+4,length+charSize);if(destructors!==null){destructors.push(_free,ptr)}return ptr},"argPackAdvance":GenericWireTypeSize,"readValueFromPointer":simpleReadValueFromPointer,destructorFunction(ptr){_free(ptr)}})};var __embind_register_void=(rawType,name)=>{name=readLatin1String(name);registerType(rawType,{isVoid:true,name:name,"argPackAdvance":0,"fromWireType":()=>undefined,"toWireType":(destructors,o)=>undefined})};var getTypeName=type=>{var ptr=___getTypeName(type);var rv=readLatin1String(ptr);_free(ptr);return rv};var requireRegisteredType=(rawType,humanName)=>{var impl=registeredTypes[rawType];if(undefined===impl){throwBindingError(humanName+" has unknown type "+getTypeName(rawType))}return impl};var __emval_as=(handle,returnType,destructorsRef)=>{handle=Emval.toValue(handle);returnType=requireRegisteredType(returnType,"emval::as");var destructors=[];var rd=Emval.toHandle(destructors);HEAPU32[destructorsRef>>2]=rd;return returnType["toWireType"](destructors,handle)};var emval_symbols={};var getStringOrSymbol=address=>{var symbol=emval_symbols[address];if(symbol===undefined){return readLatin1String(address)}return symbol};var emval_get_global=()=>{if(typeof globalThis=="object"){return globalThis}return function(){return Function}()("return this")()};var __emval_get_global=name=>{if(name===0){return Emval.toHandle(emval_get_global())}else{name=getStringOrSymbol(name);return Emval.toHandle(emval_get_global()[name])}};var __emval_get_property=(handle,key)=>{handle=Emval.toValue(handle);key=Emval.toValue(key);return Emval.toHandle(handle[key])};var __emval_new_cstring=v=>Emval.toHandle(getStringOrSymbol(v));var runDestructors=destructors=>{while(destructors.length){var ptr=destructors.pop();var del=destructors.pop();del(ptr)}};var __emval_run_destructors=handle=>{var destructors=Emval.toValue(handle);runDestructors(destructors);__emval_decref(handle)};var _abort=()=>{abort("")};var readEmAsmArgsArray=[];var readEmAsmArgs=(sigPtr,buf)=>{readEmAsmArgsArray.length=0;var ch;while(ch=HEAPU8[sigPtr++]){var wide=ch!=105;wide&=ch!=112;buf+=wide&&buf%8?4:0;readEmAsmArgsArray.push(ch==112?HEAPU32[buf>>2]:ch==105?HEAP32[buf>>2]:HEAPF64[buf>>3]);buf+=wide?8:4}return readEmAsmArgsArray};var runEmAsmFunction=(code,sigPtr,argbuf)=>{var args=readEmAsmArgs(sigPtr,argbuf);return ASM_CONSTS[code].apply(null,args)};var _emscripten_asm_const_int=(code,sigPtr,argbuf)=>runEmAsmFunction(code,sigPtr,argbuf);var _emscripten_date_now=()=>Date.now();var _emscripten_memcpy_js=(dest,src,num)=>HEAPU8.copyWithin(dest,src,src+num);var abortOnCannotGrowMemory=requestedSize=>{abort("OOM")};var _emscripten_resize_heap=requestedSize=>{var oldSize=HEAPU8.length;requestedSize>>>=0;abortOnCannotGrowMemory(requestedSize)};var _emscripten_websocket_is_supported=()=>typeof WebSocket!="undefined";var WS={sockets:[null],socketEvent:null};var _emscripten_websocket_new=createAttributes=>{if(typeof WebSocket=="undefined"){return-1}if(!createAttributes){return-5}var createAttrs=createAttributes>>2;var url=UTF8ToString(HEAP32[createAttrs]);var protocols=HEAP32[createAttrs+1];var socket=protocols?new WebSocket(url,UTF8ToString(protocols).split(",")):new WebSocket(url);socket.binaryType="arraybuffer";var socketId=WS.sockets.length;WS.sockets[socketId]=socket;return socketId};var wasmTableMirror=[];var wasmTable;var getWasmTableEntry=funcPtr=>{var func=wasmTableMirror[funcPtr];if(!func){if(funcPtr>=wasmTableMirror.length)wasmTableMirror.length=funcPtr+1;wasmTableMirror[funcPtr]=func=wasmTable.get(funcPtr)}return func};var _emscripten_websocket_set_onclose_callback_on_thread=(socketId,userData,callbackFunc,thread)=>{if(!WS.socketEvent)WS.socketEvent=_malloc(1024);var socket=WS.sockets[socketId];if(!socket){return-3}socket.onclose=function(e){HEAPU32[WS.socketEvent>>2]=socketId;HEAPU32[WS.socketEvent+4>>2]=e.wasClean;HEAPU32[WS.socketEvent+8>>2]=e.code;stringToUTF8(e.reason,WS.socketEvent+10,512);getWasmTableEntry(callbackFunc)(0,WS.socketEvent,userData)};return 0};var _emscripten_websocket_set_onerror_callback_on_thread=(socketId,userData,callbackFunc,thread)=>{if(!WS.socketEvent)WS.socketEvent=_malloc(1024);var socket=WS.sockets[socketId];if(!socket){return-3}socket.onerror=function(e){HEAPU32[WS.socketEvent>>2]=socketId;getWasmTableEntry(callbackFunc)(0,WS.socketEvent,userData)};return 0};var stringToNewUTF8=str=>{var size=lengthBytesUTF8(str)+1;var ret=_malloc(size);if(ret)stringToUTF8(str,ret,size);return ret};var _emscripten_websocket_set_onmessage_callback_on_thread=(socketId,userData,callbackFunc,thread)=>{if(!WS.socketEvent)WS.socketEvent=_malloc(1024);var socket=WS.sockets[socketId];if(!socket){return-3}socket.onmessage=function(e){HEAPU32[WS.socketEvent>>2]=socketId;if(typeof e.data=="string"){var buf=stringToNewUTF8(e.data);var len=lengthBytesUTF8(e.data)+1;HEAPU32[WS.socketEvent+12>>2]=1}else{var len=e.data.byteLength;var buf=_malloc(len);HEAP8.set(new Uint8Array(e.data),buf);HEAPU32[WS.socketEvent+12>>2]=0}HEAPU32[WS.socketEvent+4>>2]=buf;HEAPU32[WS.socketEvent+8>>2]=len;getWasmTableEntry(callbackFunc)(0,WS.socketEvent,userData);_free(buf)};return 0};var _emscripten_websocket_set_onopen_callback_on_thread=(socketId,userData,callbackFunc,thread)=>{if(!WS.socketEvent)WS.socketEvent=_malloc(1024);var socket=WS.sockets[socketId];if(!socket){return-3}socket.onopen=function(e){HEAPU32[WS.socketEvent>>2]=socketId;getWasmTableEntry(callbackFunc)(0,WS.socketEvent,userData)};return 0};var ENV={};var getExecutableName=()=>thisProgram||"./this.program";var getEnvStrings=()=>{if(!getEnvStrings.strings){var lang=(typeof navigator=="object"&&navigator.languages&&navigator.languages[0]||"C").replace("-","_")+".UTF-8";var env={"USER":"web_user","LOGNAME":"web_user","PATH":"/","PWD":"/","HOME":"/home/web_user","LANG":lang,"_":getExecutableName()};for(var x in ENV){if(ENV[x]===undefined)delete env[x];else env[x]=ENV[x]}var strings=[];for(var x in env){strings.push(`${x}=${env[x]}`)}getEnvStrings.strings=strings}return getEnvStrings.strings};var stringToAscii=(str,buffer)=>{for(var i=0;i>0]=str.charCodeAt(i)}HEAP8[buffer>>0]=0};var SYSCALLS={varargs:undefined,get(){var ret=HEAP32[+SYSCALLS.varargs>>2];SYSCALLS.varargs+=4;return ret},getp(){return SYSCALLS.get()},getStr(ptr){var ret=UTF8ToString(ptr);return ret}};var _environ_get=(__environ,environ_buf)=>{var bufSize=0;getEnvStrings().forEach((string,i)=>{var ptr=environ_buf+bufSize;HEAPU32[__environ+i*4>>2]=ptr;stringToAscii(string,ptr);bufSize+=string.length+1});return 0};var _environ_sizes_get=(penviron_count,penviron_buf_size)=>{var strings=getEnvStrings();HEAPU32[penviron_count>>2]=strings.length;var bufSize=0;strings.forEach(string=>bufSize+=string.length+1);HEAPU32[penviron_buf_size>>2]=bufSize;return 0};var _fd_close=fd=>52;var convertI32PairToI53Checked=(lo,hi)=>hi+2097152>>>0<4194305-!!lo?(lo>>>0)+hi*4294967296:NaN;function _fd_seek(fd,offset_low,offset_high,whence,newOffset){var offset=convertI32PairToI53Checked(offset_low,offset_high);return 70}var printCharBuffers=[null,[],[]];var printChar=(stream,curr)=>{var buffer=printCharBuffers[stream];if(curr===0||curr===10){(stream===1?out:err)(UTF8ArrayToString(buffer,0));buffer.length=0}else{buffer.push(curr)}};var _fd_write=(fd,iov,iovcnt,pnum)=>{var num=0;for(var i=0;i>2];var len=HEAPU32[iov+4>>2];iov+=8;for(var j=0;j>2]=num;return 0};var runtimeKeepaliveCounter=0;var keepRuntimeAlive=()=>noExitRuntime||runtimeKeepaliveCounter>0;var _proc_exit=code=>{EXITSTATUS=code;if(!keepRuntimeAlive()){if(Module["onExit"])Module["onExit"](code);ABORT=true}quit_(code,new ExitStatus(code))};var exitJS=(status,implicit)=>{EXITSTATUS=status;_proc_exit(status)};var handleException=e=>{if(e instanceof ExitStatus||e=="unwind"){return EXITSTATUS}quit_(1,e)};embind_init_charCodes();BindingError=Module["BindingError"]=class BindingError extends Error{constructor(message){super(message);this.name="BindingError"}};InternalError=Module["InternalError"]=class InternalError extends Error{constructor(message){super(message);this.name="InternalError"}};handleAllocatorInit();init_emval();var wasmImports={i:___cxa_throw,p:__embind_register_bigint,z:__embind_register_bool,y:__embind_register_emval,g:__embind_register_float,b:__embind_register_integer,a:__embind_register_memory_view,h:__embind_register_std_string,d:__embind_register_std_wstring,A:__embind_register_void,C:__emval_as,c:__emval_decref,k:__emval_get_global,j:__emval_get_property,D:__emval_new_cstring,B:__emval_run_destructors,e:_abort,F:_emscripten_asm_const_int,f:_emscripten_date_now,w:_emscripten_memcpy_js,t:_emscripten_resize_heap,E:_emscripten_websocket_is_supported,x:_emscripten_websocket_new,m:_emscripten_websocket_set_onclose_callback_on_thread,n:_emscripten_websocket_set_onerror_callback_on_thread,l:_emscripten_websocket_set_onmessage_callback_on_thread,q:_emscripten_websocket_set_onopen_callback_on_thread,r:_environ_get,s:_environ_sizes_get,v:_fd_close,o:_fd_seek,u:_fd_write};var wasmExports=createWasm();var ___wasm_call_ctors=()=>(___wasm_call_ctors=wasmExports["H"])();var _main=Module["_main"]=(a0,a1)=>(_main=Module["_main"]=wasmExports["I"])(a0,a1);var _free=a0=>(_free=wasmExports["J"])(a0);var _malloc=a0=>(_malloc=wasmExports["K"])(a0);var ___getTypeName=a0=>(___getTypeName=wasmExports["L"])(a0);var __embind_initialize_bindings=Module["__embind_initialize_bindings"]=()=>(__embind_initialize_bindings=Module["__embind_initialize_bindings"]=wasmExports["M"])();var ___errno_location=()=>(___errno_location=wasmExports["__errno_location"])();var ___cxa_increment_exception_refcount=a0=>(___cxa_increment_exception_refcount=wasmExports["__cxa_increment_exception_refcount"])(a0);var ___cxa_is_pointer_type=a0=>(___cxa_is_pointer_type=wasmExports["O"])(a0);var calledRun;dependenciesFulfilled=function runCaller(){if(!calledRun)run();if(!calledRun)dependenciesFulfilled=runCaller};function callMain(){var entryFunction=_main;var argc=0;var argv=0;try{var ret=entryFunction(argc,argv);exitJS(ret,true);return ret}catch(e){return handleException(e)}}function run(){if(runDependencies>0){return}preRun();if(runDependencies>0){return}function doRun(){if(calledRun)return;calledRun=true;Module["calledRun"]=true;if(ABORT)return;initRuntime();preMain();if(Module["onRuntimeInitialized"])Module["onRuntimeInitialized"]();if(shouldRunNow)callMain();postRun()}if(Module["setStatus"]){Module["setStatus"]("Running...");setTimeout(function(){setTimeout(function(){Module["setStatus"]("")},1);doRun()},1)}else{doRun()}}if(Module["preInit"]){if(typeof Module["preInit"]=="function")Module["preInit"]=[Module["preInit"]];while(Module["preInit"].length>0){Module["preInit"].pop()()}}var shouldRunNow=true;if(Module["noInitialRun"])shouldRunNow=false;run(); diff --git a/src/teleoperation/streaming/embuild/stream_client.wasm b/src/teleoperation/streaming/embuild/stream_client.wasm new file mode 100644 index 000000000..a78ceed0b Binary files /dev/null and b/src/teleoperation/streaming/embuild/stream_client.wasm differ diff --git a/urdf/rover/rover.urdf.xacro b/urdf/rover/rover.urdf.xacro index 3aedbbc29..1e92b05d1 100644 --- a/urdf/rover/rover.urdf.xacro +++ b/urdf/rover/rover.urdf.xacro @@ -201,18 +201,18 @@ - + - - - + + + - - - + + +