diff --git a/.gitignore b/.gitignore index c5d292aed..03f0191c5 100644 --- a/.gitignore +++ b/.gitignore @@ -50,9 +50,12 @@ state_machine/state_log.txt data/*.png data/raw-pano-images +# blender backup files +*.blend1 + # Dynamixel Ignores .catkin_tools/ .dynamixel_sdk/ # Persistent esw mrover_can FetchContent cache (survives a clean build, not tracked) -/.cache/esw/ \ No newline at end of file +/.cache/esw/ diff --git a/.vscode/extensions.json b/.vscode/extensions.json index be55320a7..378e89762 100644 --- a/.vscode/extensions.json +++ b/.vscode/extensions.json @@ -9,6 +9,6 @@ "redhat.vscode-yaml", "llvm-vs-code-extensions.vscode-clangd", "eamodio.gitlens", - "streetsidesoftware.code-spell-checker", + "streetsidesoftware.code-spell-checker" ], } \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 12834dcd4..4ec9102f0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -344,6 +344,13 @@ mrover_link_component(tag_detector lie opencv_core opencv_aruco opencv_imgproc l mrover_executable_from_component(stereo_tag_detector perception/executables/stereo_tag_detector.cpp tag_detector_component) mrover_executable_from_component(image_tag_detector perception/executables/image_tag_detector.cpp tag_detector_component) +# Keyboard Typing +mrover_add_component(keyboard_typing perception/keyboard_typing/keyboard_typing.cpp perception/keyboard_typing/pch.hpp KeyboardTypingNode perception/keyboard_typing/constants.hpp) +mrover_ament_component(keyboard_typing rclcpp rclcpp_components tf2 tf2_ros geometry_msgs sensor_msgs) +mrover_link_component(keyboard_typing lie parameter_utils opencv_core opencv_aruco opencv_imgproc opencv_highgui opencv_dnn opencv_calib3d opencv_imgcodecs opencv_video loop_profiler) + +mrover_executable_from_component(keyboard_typing perception/executables/keyboard_typing.cpp keyboard_typing_component) + # Cost Map mrover_add_component(cost_map perception/cost_map/*.cpp perception/cost_map/pch.hpp CostMapNode) mrover_ament_component(cost_map rclcpp rclcpp_components tf2 tf2_ros) diff --git a/config/keyboard_typing.yaml b/config/keyboard_typing.yaml new file mode 100644 index 000000000..02ae3d862 --- /dev/null +++ b/config/keyboard_typing.yaml @@ -0,0 +1,37 @@ +/**: + ros__parameters: + tag_size: 0.02 + # Format: [id, x, y, z] + + # Distance of each tag to bottom left tag, probably wont be needed but change if necessary + # Will need to change tag id on day of comp + tag_offsets: [ + 4.0, 0.0, 0.0, 0.0, # Bottom-left + 5.0, 0.41407, 0.0, 0.0, # Bottom-right + 3.0, 0.41407, 0.183444, 0.0, # Top-left + 2.0, 0.0, 0.183444, 0.0 # Top-right + ] + min_code_length: 3 + max_code_length: 6 + + camera_matrix: [297.5423204165929, 0.0, 306.51784066632075, + 0.0, 295.1006235094907, 247.4880266546329, + 0.0, 0.0, 1.0] + + # camera_matrix: [294.8447664894051, 0.0, 320.781111263661, + # 0.0, 292.9945007031336, 241.50291271027783, + # 0.0, 0.0, 1.0] + + distortion_coefficients: [-0.002013779189917317, + -0.05320314517636387, + -0.0013949346108894896, + 0.00023247660995265573, + 0.018284177139663432] + + # Add vertical and horizontal dist from tag center to keyboard as needed + # vertical z key from keyboard corner = 0.031575 + # horizontal z key from keyboard corner 0.05495 + z_key_transform: [0.041575, 0.06495, 0.0303784] + + + diff --git a/config/simulator/mars_map.yaml b/config/simulator/mars_map.yaml index 63b70d97c..590d351a7 100644 --- a/config/simulator/mars_map.yaml +++ b/config/simulator/mars_map.yaml @@ -12,3 +12,7 @@ objects: mars_world: type: urdf uri: package://mrover/urdf/world/mars.urdf.xacro + lander: + type: urdf + uri: package://mrover/urdf/world/lander.urdf.xacro + position: [ -5, 2, -0.75 ] diff --git a/launch/jetson_arm.launch.py b/launch/jetson_arm.launch.py index 54379a9d5..9fdddd852 100644 --- a/launch/jetson_arm.launch.py +++ b/launch/jetson_arm.launch.py @@ -57,10 +57,25 @@ def generate_launch_description(): output="screen", ) + # keyboard typing node + keyboard_typing_node = Node( + package="mrover", + executable="keyboard_typing", + name="keyboard_typing", + parameters=[Path(get_package_share_directory("mrover"), "config", "keyboard_typing.yaml")], + respawn=False, + ) + + arm_e_link_to_cam = Node( + package="tf2_ros", + executable="static_transform_publisher", + arguments=["0.02948991", "0", "-0.0456159", "0", "-1.0996", "0", "arm_fk_de", "finger_camera_frame"], + ) + launch_localization = IncludeLaunchDescription( PythonLaunchDescriptionSource( Path(get_package_share_directory("mrover"), "launch/localization.launch.py").__str__() ) ) - return LaunchDescription([launch_include_jetson_base, arm_hw_bridge_node, cam_container, launch_localization]) + return LaunchDescription([launch_include_jetson_base, arm_hw_bridge_node, cam_container, launch_localization, keyboard_typing_node, arm_e_link_to_cam]) diff --git a/launch/keyboardtypingtest.launch.py b/launch/keyboardtypingtest.launch.py new file mode 100644 index 000000000..e6cc78e7a --- /dev/null +++ b/launch/keyboardtypingtest.launch.py @@ -0,0 +1,36 @@ +from pathlib import Path + +from ament_index_python import get_package_share_directory + +import launch +from launch_ros.actions import Node, LoadComposableNodes +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + # keyboard typing node + keyboard_typing = Node( + package="mrover", + executable="keyboard_typing", + name="keyboard_typing", + parameters=[Path(get_package_share_directory("mrover"), "config", "keyboard_typing.yaml")], + respawn=False, + ) + + # gripper: [0.16533, 0, 0.059547] # forward, horizontal, vertical + # gripper_pitch: 1.134 + + # base_link_to_zed = Node( + # package="tf2_ros", + # executable="static_transform_publisher", + # arguments=["0", "0", "1", "0", "0", "0", "1", "base_link", "zed_left_camera_frame"], + # ) + + # joint DE to camera: [0, 0.03713988, 0.0945642] roll: 0, pitch: 1.134, yaw: 0 + arm_e_link_to_cam = Node( + package="tf2_ros", + executable="static_transform_publisher", + arguments=["0.02948991", "0", "-0.0456159", "0", "-1.0996", "0", "arm_fk_de", "finger_camera_frame"], + ) + + return launch.LaunchDescription([keyboard_typing, arm_e_link_to_cam]) diff --git a/perception/executables/keyboard_typing.cpp b/perception/executables/keyboard_typing.cpp new file mode 100644 index 000000000..98ff5e3b7 --- /dev/null +++ b/perception/executables/keyboard_typing.cpp @@ -0,0 +1,12 @@ +#include + +auto main(int argc, char** argv) -> int { + rclcpp::init(argc, argv); + auto keyboardT = std::make_shared(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(keyboardT); + executor.spin(); + + rclcpp::shutdown(); + return EXIT_SUCCESS; +} diff --git a/perception/keyboard_typing/keyboard_typing.cpp b/perception/keyboard_typing/keyboard_typing.cpp new file mode 100644 index 000000000..bbbfaf071 --- /dev/null +++ b/perception/keyboard_typing/keyboard_typing.cpp @@ -0,0 +1,624 @@ +#include "keyboard_typing.hpp" + + +namespace mrover { + KeyboardTypingNode::KeyboardTypingNode(rclcpp::NodeOptions const& options) : rclcpp::Node("keyboard_typing_node", options), mLoopProfiler{get_logger()} { + RCLCPP_INFO_STREAM(get_logger(), "KeyBoardTypingNode starting up"); + + std::vector params{ + {"min_code_length", mMinCodeLength, 3}, + {"max_code_length", mMaxCodeLength, 6}, + {"tag_size", mTagSize, 0.02}, + }; + + ParameterWrapper::declareParameters(this, params); + + // Declare vector params manually since param wrapper doesnt support vector + std::map vector_params; + vector_params["camera_matrix"] = rclcpp::ParameterValue(std::vector(9, 0.0)); + vector_params["distortion_coefficients"] = rclcpp::ParameterValue(std::vector(5, 0.0)); + vector_params["tag_offsets"] = rclcpp::ParameterValue(std::vector{}); + vector_params["z_key_transform"] = rclcpp::ParameterValue(std::vector{}); + + this->declare_parameters("", vector_params); + std::vector cam_raw = this->get_parameter("camera_matrix").as_double_array(); + std::vector dist_raw = this->get_parameter("distortion_coefficients").as_double_array(); + std::vector zTransformRaw = this->get_parameter("z_key_transform").as_double_array(); + + // Read in Camera intrinsics + mCameraMatrix = cv::Mat(3, 3, CV_64F, cam_raw.data()).clone(); + mDistCoeffs = cv::Mat(1, (int) dist_raw.size(), CV_64F, dist_raw.data()).clone(); + mZKeyTransform = Eigen::Vector3d::Map(zTransformRaw.data()); + + // Grab tag offsets + std::vector offset_raw = this->get_parameter("tag_offsets").as_double_array(); + + for (size_t i = 0; i + 3 < offset_raw.size(); i += 4) { + int id = static_cast(offset_raw[i]); + float x = static_cast(offset_raw[i + 1]); + float y = static_cast(offset_raw[i + 2]); + float z = static_cast(offset_raw[i + 3]); + + mTagLayout[id] = cv::Vec3d(x, y, z); + } + + // Set up action server + mTypingClient = rclcpp_action::create_client(this, "typing_pos"); + + mTypingServer = rclcpp_action::create_server( + this, + "es_typing_code", + std::bind(&KeyboardTypingNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&KeyboardTypingNode::handle_cancel, this, std::placeholders::_1), + std::bind(&KeyboardTypingNode::handle_accepted, this, std::placeholders::_1)); + + // subscribe to image stream + // mImageSub = create_subscription("/video4/image", rclcpp::QoS(1), [this](sensor_msgs::msg::Image::ConstSharedPtr const& msg) { + // yawCallback(msg); + // }); + + mImageSub = create_subscription("/finger_camera/image", rclcpp::QoS(1), [this](sensor_msgs::msg::Image::ConstSharedPtr const& msg) { + yawCallback(msg); + }); + + // grab transform from finger_cam to gripper + // wait until the transformation is acquired + while (true) { + try { + cam_to_gripper = SE3Conversions::fromTfTree(tf_buffer, "finger_camera_frame", "arm_fk_de"); + break; + } catch (tf2::TransformException const& e) { + RCLCPP_WARN_STREAM_THROTTLE(get_logger(), *get_clock(), 1000, std::format("TF tree error processing keyboard typing: {}", e.what())); + } + } + + // Create Ik mode client + mIkModeClient = create_client("ik_mode"); + + current_key = ""; + + // create publisher + mYawPub = this->create_publisher("/keyboard/yaw", rclcpp::QoS(1)); + + mIKPub = this->create_publisher("ik_pos_cmd", rclcpp::QoS(1)); + } + + auto KeyboardTypingNode::yawCallback(sensor_msgs::msg::Image::ConstSharedPtr const& msg) -> void { + // If we are still updating pose estimate (i.e. no launch code sent, send IK and update pose) + std::optional output = std::nullopt; + if (mUpdatePoseEstimate) { + // sendIKCommand(1.100, .253, .367, 0, 0); + output = estimatePose(msg); + } + + // Publish yaw & se3d + if (output.has_value()) { + mrover::msg::KeyboardYaw msg; + msg.yaw = output->yaw; + mYawPub->publish(msg); + + // Output debug + // RCLCPP_INFO_STREAM(get_logger(), "X: " << output->pose.position.x); + // RCLCPP_INFO_STREAM(get_logger(), "Y: " << output->pose.position.y); + // RCLCPP_INFO_STREAM(get_logger(), "Z: " << output->pose.position.z); + + // Transform from camera frame to arm_fk frame: + // Camera X (horizontal) -> arm_fk Y + // Camera Y (vertical) -> arm_fk Z + // Camera Z (forward) -> arm_fk X + + // open cv coordinates + // +X is right + // -Y is up + // +Z is forward + + // robot coordinates + // -Y is right + // +Z is up + // +X is forwards + + // Convert pose to se3d + SE3d cam_to_tag = SE3Conversions::fromPose(output->pose); + + // Map to tf2 axis + Eigen::Matrix3d opencv_to_ros_rot; + + opencv_to_ros_rot << 0, 0, 1, // tf2 X comes from opencv z + -1, 0, 0, // tf2 Y comes from opencv x + 0, -1, 0; // tf2 Z comes from opencv Y + + SE3d opencv_to_ros(Eigen::Vector3d::Zero(), Eigen::Quaterniond(opencv_to_ros_rot)); + + // Fix orientation + Eigen::Matrix3d R_tag_fix; + R_tag_fix << 0, 1, 0, + 0, 0, 1, + 1, 0, 0; + SE3d orientation_fix(Eigen::Vector3d::Zero(), Eigen::Quaterniond(R_tag_fix)); + + // Grab final transform + SE3d arm_fk_to_tag = cam_to_gripper * opencv_to_ros * cam_to_tag * orientation_fix; + + // Publish tag to tf tree + SE3Conversions::pushToTfTree(tf_broadcaster, "keyboard_tag", "arm_fk_de", arm_fk_to_tag, get_clock()->now()); + + // Publish z key to tf tree + SE3d z_to_tag{mZKeyTransform, Eigen::Quaterniond::Identity()}; + SE3Conversions::pushToTfTree(tf_broadcaster, "keyboard_z", "keyboard_tag", z_to_tag, get_clock()->now()); + } + } + + auto KeyboardTypingNode::estimatePose(sensor_msgs::msg::Image::ConstSharedPtr const& msg) -> std::optional { + // Read in image + cv::Mat bgraImage{static_cast(msg->height), static_cast(msg->width), CV_8UC4, const_cast(msg->data.data())}; + + // For debugging image + // cv::Mat bgrImage; + // cv::cvtColor(bgraImage, bgrImage, cv::COLOR_BGRA2BGR); + + // convert to gray for estimation + cv::Mat grayImage; + cv::cvtColor(bgraImage, grayImage, cv::COLOR_BGRA2GRAY); + + // Optional: enhance contrast + // cv::equalizeHist(grayImage, grayImage); + + // Optional: apply Gaussian blur to reduce noise + // cv::GaussianBlur(grayImage, grayImage, cv::Size(5,5), 0); + + // Adaptive contrasting to improve detection reliability + cv::Ptr clahe = cv::createCLAHE(); + clahe->setClipLimit(2.0); + clahe->setTilesGridSize(cv::Size(8, 8)); + + clahe->apply(grayImage, grayImage); + + std::vector> markerCorners, rejectedCandidates; + std::vector ids; + cv::Ptr detectorParams = cv::aruco::DetectorParameters::create(); + + detectorParams->cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX; + + // Detect Markers + cv::aruco::detectMarkers(grayImage, dictionary, markerCorners, ids, detectorParams, rejectedCandidates); + + // Estimate pose + std::optional combined_tvec, combined_rvec; + if (!ids.empty()) { + std::vector valid_indices; + for (size_t i = 0; i < ids.size(); ++i) { + if (mTagLayout.count(ids[i]) > 0) { + valid_indices.push_back(i); + } + } + + int validcnt = valid_indices.size(); + + float half_size = mTagSize / 2.0f; + + std::vector all_objPoints; + std::vector all_imgPoints; + if (validcnt > 0) { + for (int idx: valid_indices) { + int id = ids[idx]; + + cv::Point3f tag_center(mTagLayout[id][0], mTagLayout[id][1], mTagLayout[id][2]); + + all_objPoints.push_back(tag_center + cv::Point3f(-half_size, half_size, 0)); // Top-Left + all_objPoints.push_back(tag_center + cv::Point3f(half_size, half_size, 0)); // Top-Right + all_objPoints.push_back(tag_center + cv::Point3f(half_size, -half_size, 0)); // Bottom-Right + all_objPoints.push_back(tag_center + cv::Point3f(-half_size, -half_size, 0)); // Bottom-Left + + for (int i = 0; i < 4; i++) { + all_imgPoints.push_back(markerCorners.at(idx)[i]); + } + } + cv::Vec3d rvec, tvec; + cv::solvePnP(all_objPoints, all_imgPoints, mCameraMatrix, mDistCoeffs, rvec, tvec, false, cv::SOLVEPNP_ITERATIVE); + combined_rvec = rvec; + combined_tvec = tvec; + } + } + + // Grab rvecs and tvecs for each individual marker for debugging + // std::vector rvecs(nMarkers), tvecs(nMarkers); + // // populate rvecs and tvecs + // if (!ids.empty()) { + // for (size_t i = 0; i < nMarkers; ++i) { + // cv::solvePnP(objPoints, markerCorners.at(i), mCameraMatrix, mDistCoeffs, rvecs.at(i), tvecs.at(i), false, cv::SOLVEPNP_IPPE_SQUARE); + // } + // } + + // draw results for debugging + // if (!ids.empty()) { + // cv::aruco::drawDetectedMarkers(bgrImage, markerCorners, ids); + // for (size_t i = 0; i < ids.size(); ++i) { + // cv::drawFrameAxes(bgrImage, mCameraMatrix, mDistCoeffs, rvecs[i], tvecs[i], markerLength * 1.5f, 2); + // } + // } + + if (combined_tvec.has_value() && combined_rvec.has_value()) { + geometry_msgs::msg::Pose finalestimation; + + std::tie(combined_tvec, combined_rvec) = vectorMedianFilter(*combined_tvec, *combined_rvec); + + // Debugging statements + // RCLCPP_INFO_STREAM(get_logger(), "X: " << (*combined_tvec)[0]); + // RCLCPP_INFO_STREAM(get_logger(), "Y: " << (*combined_tvec)[1]); + // RCLCPP_INFO_STREAM(get_logger(), "Z: " << (*combined_tvec)[2]); + + Eigen::Vector3d rvec( + (*combined_rvec)[0], + (*combined_rvec)[1], + (*combined_rvec)[2]); + + double angle = rvec.norm(); + Eigen::Quaterniond q; + + if (angle < 1e-12) { + q.setIdentity(); + } else { + Eigen::Vector3d axis = rvec / angle; + q = Eigen::AngleAxisd(angle, axis); + } + + finalestimation.orientation.x = q.x(); + finalestimation.orientation.y = q.y(); + finalestimation.orientation.z = q.z(); + finalestimation.orientation.w = q.w(); + + finalestimation.position.x = (*combined_tvec)[0]; + finalestimation.position.y = (*combined_tvec)[1]; + finalestimation.position.z = (*combined_tvec)[2]; + + Eigen::Vector3d ypr = q.toRotationMatrix().eulerAngles(2, 1, 0); + + double yaw_deg = ypr[0] * 180.0 / M_PI; + double pitch_deg = ypr[1] * 180.0 / M_PI; + + // Draw axes for debugging + // cv::drawFrameAxes(bgrImage, mCameraMatrix, mDistCoeffs, combined_rvec, combined_tvec, markerLength * 1.5f, 2); + + // Draw circle for debugging + // int cx = grayImage.cols / 2; + // int cy = grayImage.rows / 2; + + // cv::circle( + // bgrImage, + // cv::Point(cx, cy), + // 2, // radius + // cv::Scalar(0, 0, 255), // BGR: red + // cv::FILLED); + + // Draw axes for debugging + // cv::drawFrameAxes(grayImage, camMatrix, distCoeffs, final_rvec, final_tvec, markerLength * 1.5f, 2); + // cv::imshow("out1", bgrImage); + // cv::imshow("out", grayImage); + // int key = cv::waitKey(1) & 0xFF; + // if (key == 'r') { + // // logPose = !logPose; + // // RCLCPP_INFO_STREAM(get_logger(), "Toggled logPose: " << (logPose ? "ON" : "OFF")); + // align_arm(); + // // send_z_key_command(); + // } + // if (key == 't') { + // align_to_z(); + // } + + return pose_output{finalestimation, yaw_deg}; + } else { + // debug image + // cv::imshow("out1", bgraImage); + // cv::waitKey(1); + return std::nullopt; + } + } + + auto KeyboardTypingNode::vectorMedianFilter(cv::Vec3d tvec, cv::Vec3d rvec) -> std::pair { + if (tvec_window.size() > 4) { + tvec_window.pop_front(); + rvec_window.pop_front(); + } + tvec_window.push_back(tvec); + rvec_window.push_back(rvec); + + cv::Vec3d medianTvec = tvec_window.back(); + double min_sum = std::numeric_limits::max(); + + // Take the median + int filtered_idx = 0; + + // Find median rvec + for (size_t i = 0; i < rvec_window.size(); ++i) { + double angle_diff_radians = 0; + double currentTvecSum = 0; + for (size_t j = 0; j < rvec_window.size(); ++j) { + if (i == j) { + continue; + } + cv::Mat R_i; + cv::Rodrigues(rvec_window[i], R_i); + cv::Mat R_j; + cv::Rodrigues(rvec_window[j], R_j); + + // Compute diff and then convert back to rodrigues to get magnitude + cv::Mat R_diff = R_i * R_j.t(); // transpose is inverse b/c orthogonal, so computes difference + cv::Mat axis_angle_vec; + cv::Rodrigues(R_diff, axis_angle_vec); + + angle_diff_radians += cv::norm(axis_angle_vec); + currentTvecSum += cv::norm(tvec_window[i] - tvec_window[j]); + } + double min_sum_current = angle_diff_radians + currentTvecSum; + if (min_sum_current < min_sum) { + min_sum = min_sum_current; + filtered_idx = i; + } + } + + return {tvec_window[filtered_idx], rvec_window[filtered_idx]}; + } + + auto KeyboardTypingNode::align_arm() -> void { + // Grab gripper_to_tag and then calculate deltas + SE3d gripper_to_tag; + SE3d armbase_to_armfk; + + try { + gripper_to_tag = SE3Conversions::fromTfTree(tf_buffer, "keyboard_tag", "arm_base_link"); + armbase_to_armfk = SE3Conversions::fromTfTree(tf_buffer, "arm_fk_ee", "arm_base_link"); + + // Grab pitch from tag transform + double r00 = gripper_to_tag.transform()(0, 0); + double r10 = gripper_to_tag.transform()(1, 0); + double r20 = gripper_to_tag.transform()(2, 0); + + double pitch_rad = -std::atan2(-r20, std::hypot(r00, r10)); + + RCLCPP_INFO_STREAM(this->get_logger(), "pitch = " << pitch_rad); + + sendIKCommand(armbase_to_armfk.translation().x(), gripper_to_tag.translation().y(), gripper_to_tag.translation().z(), pitch_rad, -1.5708); + + } catch (tf2::TransformException const& e) { + RCLCPP_WARN_STREAM_THROTTLE(get_logger(), *get_clock(), 1000, std::format("TF tree error processing keyboard typing: {}", e.what())); + } + } + + auto KeyboardTypingNode::align_to_z() -> void { + // Grab gripper_to_tag and then calculate deltas + SE3d armbase_to_z; + SE3d armbase_to_armfk; + + try { + armbase_to_z = SE3Conversions::fromTfTree(tf_buffer, "keyboard_z", "arm_base_link"); + armbase_to_armfk = SE3Conversions::fromTfTree(tf_buffer, "arm_fk_ee", "arm_base_link"); + + // Grab pitch from tag transform + double r00 = armbase_to_z.transform()(0, 0); + double r10 = armbase_to_z.transform()(1, 0); + double r20 = armbase_to_z.transform()(2, 0); + + double pitch_rad = -std::atan2(-r20, std::hypot(r00, r10)); + + double r21 = armbase_to_z.transform()(2, 1); + double r22 = armbase_to_z.transform()(2, 2); + + keyboard_roll = std::atan2(r21, r22); + + sendIKCommand(armbase_to_armfk.translation().x(), armbase_to_z.translation().y(), armbase_to_z.translation().z(), pitch_rad, keyboard_roll); + + RCLCPP_INFO_STREAM(get_logger(), "y_delta = " << (armbase_to_armfk.translation().y() - armbase_to_z.translation().y())); + + } catch (tf2::TransformException const& e) { + RCLCPP_WARN_STREAM_THROTTLE(get_logger(), *get_clock(), 1000, std::format("TF tree error processing keyboard typing: {}", e.what())); + } + + current_key = 'z'; + } + + auto KeyboardTypingNode::sendIKCommand(float x, float y, float z, float pitch, float roll) -> void { + if (!mIKPub) { + RCLCPP_ERROR(get_logger(), "IK publisher not initialized"); + } + // shift over to arm_gripper_link + float z_offset_local = 0.073; + + float dx = z_offset_local * (std::sin(pitch) * std::cos(roll)); + float dy = z_offset_local * -std::sin(roll); + float dz = z_offset_local * (std::cos(pitch) * std::cos(roll)); + + msg::IK message; + + message.pos.x = x + dx; + message.pos.y = y + dy; + message.pos.z = z + dz; + + message.pitch = pitch; + message.roll = -1.5708; + + using clock = std::chrono::steady_clock; + auto start = clock::now(); + auto duration = std::chrono::duration(10); + + SE3d curarmpos = SE3Conversions::fromTfTree(tf_buffer, "arm_fk_ee", "arm_base_link"); + + double dist = pow(curarmpos.translation().y() - y, 2) + pow(curarmpos.translation().z() - z, 2); + while (clock::now() - start < duration && dist > 0.0001) { + mIKPub->publish(message); + curarmpos = SE3Conversions::fromTfTree(tf_buffer, "arm_fk_ee", "arm_base_link"); + dist = pow(curarmpos.translation().y() - y, 2) + pow(curarmpos.translation().z() - y, 2); + RCLCPP_INFO_STREAM(get_logger(), "remaining distance = " << dist); + } + + RCLCPP_INFO(get_logger(), "Published IK Command {x=%.3f, y=%.3f, z=%.3f, p=%.3f, r=%.3f}", x, message.pos.y, message.pos.z, message.pitch, message.roll); + } + + // ----------------------- ACTION SERVER/CLIENT --------------------------- + // Typing IK action client functions (communication with Nav) + auto KeyboardTypingNode::send_goal(float x_delta, float y_delta) -> bool { + // Wait at most 5 seconds for nav action server before timing out + if (!mTypingClient->wait_for_action_server(std::chrono::seconds(5))) { + RCLCPP_INFO_STREAM(this->get_logger(), "Typing Deltas Action Server not available"); + return false; + } + + RCLCPP_INFO_STREAM(this->get_logger(), "Sending Goal"); + + auto goal_msg = action::TypingPosition::Goal(); + goal_msg.x = x_delta; + goal_msg.y = y_delta; + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + // send_goal_options.goal_response_callback = std::bind(&KeyboardTypingNode::goal_response_callback, this, std::placeholders::_1); + send_goal_options.feedback_callback = std::bind(&KeyboardTypingNode::feedback_callback, this, std::placeholders::_1, std::placeholders::_2); + // send_goal_options.result_callback = std::bind(&KeyboardTypingNode::result_callback, this, std::placeholders::_1); + + std::shared_future future_goal_handle = mTypingClient->async_send_goal(goal_msg, send_goal_options); + RCLCPP_INFO_STREAM(this->get_logger(), "Sent first goal, waiting for response"); + future_goal_handle.wait(); + + if (future_goal_handle.get() == nullptr) + return false; + + std::shared_future future_result = mTypingClient->async_get_result(future_goal_handle.get()); + future_result.wait(); + return (future_result.get().code == rclcpp_action::ResultCode::SUCCEEDED); + } + + void KeyboardTypingNode::feedback_callback(GoalHandleTypingPosition::SharedPtr, std::shared_ptr const feedback) { + // RCLCPP_INFO_STREAM(get_logger(), std::format("Feedback: {}", feedback->dist_remaining)); + } + + // void KeyboardTypingNode::result_callback(const GoalHandleTypingPosition::WrappedResult & result) { + // mGoalReached = (result.code == rclcpp_action::ResultCode::SUCCEEDED); + // } + + // Typing IK action server functions (communication with teleop) + auto KeyboardTypingNode::handle_goal(rclcpp_action::GoalUUID const& uuid, std::shared_ptr goal) -> rclcpp_action::GoalResponse { + // Reject goal if code length is incorrect, code already active, or not letters + if (goal->launch_code.length() < mMinCodeLength || goal->launch_code.length() > mMaxCodeLength) { + RCLCPP_WARN(this->get_logger(), "Launch code length must be in between %d and %d!", mMinCodeLength, mMaxCodeLength); + return rclcpp_action::GoalResponse::REJECT; + } + + if (mAcceptedGoalHandle && mAcceptedGoalHandle->is_active()) { + RCLCPP_WARN(this->get_logger(), "Launch code already active!"); + return rclcpp_action::GoalResponse::REJECT; + } + + // Check that each passed character is in the keyboard map (i.e. it's a valid char) + std::string temp = goal->launch_code; + if (!std::all_of(temp.begin(), temp.end(), [](unsigned char c) { + return keyboard_offset.contains(static_cast(std::toupper(static_cast(c)))); + })) { + RCLCPP_WARN(this->get_logger(), "All keys must be letters or - (backspace)!"); + return rclcpp_action::GoalResponse::REJECT; + } + + mUpdatePoseEstimate = false; // Once we accept a goal, we no longer want to be updating the pose estimate + + RCLCPP_INFO_STREAM(get_logger(), std::format("Goal Accepted: {}", temp)); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + auto KeyboardTypingNode::handle_cancel(std::shared_ptr const goal_handle) -> rclcpp_action::CancelResponse { + if (goal_handle != mAcceptedGoalHandle) { + RCLCPP_WARN(this->get_logger(), "Invalid Cancel ID"); + return rclcpp_action::CancelResponse::REJECT; + } + + RCLCPP_INFO_STREAM(get_logger(), std::format("Cacelling goal {}", goal_handle->get_goal()->launch_code)); + mUpdatePoseEstimate = true; + mAcceptedGoalHandle = nullptr; + mTypingClient->async_cancel_all_goals(); // Cancel goal send to nav + return rclcpp_action::CancelResponse::ACCEPT; + } + + void KeyboardTypingNode::handle_accepted(std::shared_ptr const goal_handle) { + mAcceptedGoalHandle = goal_handle; // Set goal handle in the executor thread, not a new one + + std::thread([this, goal_handle]() { + auto result = std::make_shared(); + auto feedback = std::make_shared(); + + result->success = false; + + std::string launchCode = goal_handle->get_goal()->launch_code; + std::transform(launchCode.begin(), launchCode.end(), launchCode.begin(), ::toupper); + + // Align arm over z key + RCLCPP_INFO_STREAM(this->get_logger(), "Aligning to z key"); + + if (mAlignArm) { + align_to_z(); + } else { + // grab roll of keyboard + try { + SE3d armbase_to_z = SE3Conversions::fromTfTree(tf_buffer, "keyboard_z", "arm_base_link"); + + double r21 = armbase_to_z.transform()(2, 1); + double r22 = armbase_to_z.transform()(2, 2); + + keyboard_roll = std::atan2(r21, r22); + + } catch (tf2::TransformException const& e) { + RCLCPP_WARN_STREAM_THROTTLE(get_logger(), *get_clock(), 1000, std::format("TF tree error processing keyboard typing: {}", e.what())); + } + } + + // Activate typing mode + RCLCPP_INFO_STREAM(this->get_logger(), "Sending ik mode request"); + auto ik_req = std::make_shared(); + ik_req->mode = srv::IkMode::Request::TYPING; + mIkModeClient->async_send_request(ik_req); + + RCLCPP_INFO_STREAM(this->get_logger(), "Typing Sequence starting"); + // Start typing sequence + for (size_t i = 0; i < launchCode.length(); i++) { + feedback->current_index = static_cast(i); + goal_handle->publish_feedback(feedback); + + RCLCPP_WARN(this->get_logger(), "Sending Goal"); + + float x_pos = 0; + float y_pos = 0; + + x_pos = keyboard_offset.at(launchCode[i])[0]; + y_pos = keyboard_offset.at(launchCode[i])[1]; + + double rolled_x_pos = (x_pos * std::cos(keyboard_roll)) - (y_pos * std::sin(keyboard_roll)); + + double rolled_y_pos = (x_pos * std::sin(keyboard_roll)) + (y_pos * std::cos(keyboard_roll)); + + RCLCPP_WARN(this->get_logger(), "X_pos", mMinCodeLength, mMaxCodeLength); + + RCLCPP_INFO_STREAM(this->get_logger(), "current letter = " << launchCode[i]); + RCLCPP_INFO_STREAM(this->get_logger(), "x_pos = " << x_pos); + RCLCPP_INFO_STREAM(this->get_logger(), "y_pos = " << y_pos); + RCLCPP_INFO_STREAM(this->get_logger(), "rolled_x_pos = " << rolled_x_pos); + RCLCPP_INFO_STREAM(this->get_logger(), "rolled_y_pos = " << rolled_y_pos); + bool status = send_goal(-rolled_x_pos, rolled_y_pos); + + // Check goal cancelled + if (goal_handle->is_canceling()) { + result->success = false; + goal_handle->canceled(result); + return; + } + + // If goal fails and we're on the first key, stop + if (!status && i == 0) { + result->success = false; + goal_handle->abort(result); + return; + } + + current_key = launchCode[i]; + } + + result->success = true; + mUpdatePoseEstimate = true; + goal_handle->succeed(result); + }).detach(); + } +} // namespace mrover \ No newline at end of file diff --git a/perception/keyboard_typing/keyboard_typing.hpp b/perception/keyboard_typing/keyboard_typing.hpp new file mode 100644 index 000000000..3cc5447fd --- /dev/null +++ b/perception/keyboard_typing/keyboard_typing.hpp @@ -0,0 +1,146 @@ +#pragma once + +#include "pch.hpp" + +constexpr double key_length = 0.01905; +constexpr double secondRowX = -0.0095; +constexpr double thirdRowX = -0.01345; +constexpr double fourthRowX = -0.009525; +constexpr double secondRowZ = 0.0007112; +constexpr double thirdRowZ = 0.0017272; +constexpr double fourthRowZ = 0.0048514; + +inline std::unordered_map const keyboard_offset = { + {'Z', cv::Vec3d{0, 0, 0}}, + {'X', cv::Vec3d{key_length, 0, 0}}, + {'C', cv::Vec3d{2 * key_length, 0, 0}}, + {'V', cv::Vec3d{3 * key_length, 0, 0}}, + {'B', cv::Vec3d{4 * key_length, 0, 0}}, + {'N', cv::Vec3d{5 * key_length, 0, 0}}, + {'M', cv::Vec3d{6 * key_length, 0, 0}}, + + {'A', cv::Vec3d{secondRowX, key_length, secondRowZ}}, + {'S', cv::Vec3d{secondRowX + key_length, key_length, secondRowZ}}, + {'D', cv::Vec3d{secondRowX + 2 * key_length, key_length, secondRowZ}}, + {'F', cv::Vec3d{secondRowX + 3 * key_length, key_length, secondRowZ}}, + {'G', cv::Vec3d{secondRowX + 4 * key_length, key_length, secondRowZ}}, + {'H', cv::Vec3d{secondRowX + 5 * key_length, key_length, secondRowZ}}, + {'J', cv::Vec3d{secondRowX + 6 * key_length, key_length, secondRowZ}}, + {'K', cv::Vec3d{secondRowX + 7 * key_length, key_length, secondRowZ}}, + {'L', cv::Vec3d{secondRowX + 8 * key_length, key_length, secondRowZ}}, + + {'Q', cv::Vec3d{thirdRowX, 2 * key_length, thirdRowZ}}, + {'W', cv::Vec3d{thirdRowX + key_length, 2 * key_length, thirdRowZ}}, + {'E', cv::Vec3d{thirdRowX + 2 * key_length, 2 * key_length, thirdRowZ}}, + {'R', cv::Vec3d{thirdRowX + 3 * key_length, 2 * key_length, thirdRowZ}}, + {'T', cv::Vec3d{thirdRowX + 4 * key_length, 2 * key_length, thirdRowZ}}, + {'Y', cv::Vec3d{thirdRowX + 5 * key_length, 2 * key_length, thirdRowZ}}, + {'U', cv::Vec3d{thirdRowX + 6 * key_length, 2 * key_length, thirdRowZ}}, + {'I', cv::Vec3d{thirdRowX + 7 * key_length, 2 * key_length, thirdRowZ}}, + {'O', cv::Vec3d{thirdRowX + 8 * key_length, 2 * key_length, thirdRowZ}}, + {'P', cv::Vec3d{thirdRowX + 9 * key_length, 2 * key_length, thirdRowZ}}, + + // backspace + {'-', cv::Vec3d{fourthRowX + 11 * key_length + 0.028575, 3 * key_length, fourthRowZ}}}; + +// #include "constants.h" +namespace mrover { + class KeyboardTypingNode : public rclcpp::Node { + private: + // TypingPosition action client - this communicates with Nav + using TypingPosition = mrover::action::TypingPosition; + using GoalHandleTypingPosition = rclcpp_action::ClientGoalHandle; + + rclcpp_action::Client::SharedPtr mTypingClient; + + // Ik mode client + rclcpp::Client::SharedPtr mIkModeClient; + + void feedback_callback(GoalHandleTypingPosition::SharedPtr, std::shared_ptr const feedback); + + auto send_goal(float x_delta, float y_delta) -> bool; + + // TypingCode action server - this communicates with teleop + using TypingCode = mrover::action::TypingCode; + using GoalHandleTypingCode = rclcpp_action::ServerGoalHandle; + + rclcpp_action::Server::SharedPtr mTypingServer; + + auto handle_goal(rclcpp_action::GoalUUID const& uuid, std::shared_ptr goal) -> rclcpp_action::GoalResponse; + auto handle_cancel(std::shared_ptr const goal_handle) -> rclcpp_action::CancelResponse; + void handle_accepted(std::shared_ptr const goal_handle); + + std::shared_ptr mAcceptedGoalHandle = nullptr; + + // Store information for pose estimation + struct pose_output { + geometry_msgs::msg::Pose pose; + double yaw; + }; + + std::string current_key; + + double keyboard_roll; + + bool mAlignArm = false; + + // Params + int mMinCodeLength{}, mMaxCodeLength{}; + cv::Mat mCameraMatrix; + cv::Mat mDistCoeffs; + double mTagSize; + Eigen::Vector3d mZKeyTransform; + + // transform broadcaster + tf2_ros::Buffer tf_buffer{get_clock()}; + tf2_ros::TransformListener tf_listener{tf_buffer}; + tf2_ros::TransformBroadcaster tf_broadcaster{this}; + + // transform from camera to end effector + SE3d cam_to_gripper; + + // Define a board + cv::Ptr rover_board; + + // Sub to /finger_camera/image topic + rclcpp::Subscription::SharedPtr mImageSub; + + LoopProfiler mLoopProfiler; + + // Can pub to any topic just make the name make sense + rclcpp::Publisher::SharedPtr mYawPub; + rclcpp::Publisher::SharedPtr mIKPub; + rclcpp::Publisher::SharedPtr mIKVelPub; + + // First position is rotation vector, second is translation vector + // std::vector current_estimate; + std::optional mCameraToKey = std::nullopt; + bool mUpdatePoseEstimate = true; + + cv::Ptr dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); + + // Layout map (ID -> Bottom-Left Corner Position) + std::map mTagLayout; + + auto sendIKCommand(float x, float y, float z, float pitch, float roll) -> void; + + auto yawCallback(sensor_msgs::msg::Image::ConstSharedPtr const& msg) -> void; + + auto estimatePose(sensor_msgs::msg::Image::ConstSharedPtr const& msg) -> std::optional; + + auto align_arm() -> void; + + auto align_to_z() -> void; + + // Median Filter + + auto vectorMedianFilter(cv::Vec3d tvec, cv::Vec3d rvec) -> std::pair; + + std::deque tvec_window; + + std::deque rvec_window; + + public: + explicit KeyboardTypingNode(rclcpp::NodeOptions const& options = rclcpp::NodeOptions()); + }; +} // namespace mrover \ No newline at end of file diff --git a/perception/keyboard_typing/pch.hpp b/perception/keyboard_typing/pch.hpp new file mode 100644 index 000000000..b141efd29 --- /dev/null +++ b/perception/keyboard_typing/pch.hpp @@ -0,0 +1,54 @@ +#pragma once + +// Precompiled headers should generally be included first +#include "keyboard_typing/pch.hpp" + +// Standard Library +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "lie.hpp" +#include +#include \ No newline at end of file diff --git a/urdf/meshes/lander.fbx b/urdf/meshes/lander.fbx index 82e3947b9..9eaeb7a45 100644 --- a/urdf/meshes/lander.fbx +++ b/urdf/meshes/lander.fbx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:418af84e0a09e6fc7fbb70cbbb4e285129589edb03fb72040ca85aa6003bedfc -size 1401404 +oid sha256:c27dd8469ae85f5410d142d0c986b02075c170634b9a3d83472ba9b12c1baad5 +size 1401388 diff --git a/urdf/rover/rover.urdf.xacro b/urdf/rover/rover.urdf.xacro index 348a1556f..b747cf93e 100644 --- a/urdf/rover/rover.urdf.xacro +++ b/urdf/rover/rover.urdf.xacro @@ -479,8 +479,8 @@ gg - - + + diff --git a/urdf/staging/lander.blend b/urdf/staging/lander.blend index c0ba0b6c6..03b60cb3e 100644 --- a/urdf/staging/lander.blend +++ b/urdf/staging/lander.blend @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:2d25815f6156944d341c1e184a37613d995481e8a056997821aa38e063d70f74 -size 3615364 +oid sha256:aa2e5865f3349972fb88214f37c788d098fa28e9fae635fc07367978eddaeb59 +size 3616260 diff --git a/urdf/staging/lander_decimated.blend b/urdf/staging/lander_decimated.blend index ce93d0dfc..690b1f997 100644 --- a/urdf/staging/lander_decimated.blend +++ b/urdf/staging/lander_decimated.blend @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:6222f1799e6976c043b0f372e1930548913181af914a291f151078d6f15ef429 -size 600167 +oid sha256:e27cf3948b82a424bde712a607262a3a435f7cf8e64525164b974821a4f59b8d +size 1615744 diff --git a/urdf/world/lander.urdf.xacro b/urdf/world/lander.urdf.xacro index 6e7e801a4..4649245e7 100644 --- a/urdf/world/lander.urdf.xacro +++ b/urdf/world/lander.urdf.xacro @@ -2,8 +2,8 @@ - - + +