From 7ce6aaa70730f899c9453bb98c33d2819c9f594c Mon Sep 17 00:00:00 2001 From: ligerfotis Date: Fri, 12 Apr 2024 17:28:51 +0200 Subject: [PATCH] keyboard servo for ur and gripper --- .idea/.gitignore | 2 + src/__init__.py | 0 src/servo_keyboard/CMakeLists.txt | 52 +++ src/servo_keyboard/LICENSE | 202 +++++++++ src/servo_keyboard/package.xml | 26 ++ .../src/servo_keyboard_input.cpp | 389 ++++++++++++++++++ .../launch/ur_robotiq_control.launch.py | 38 +- src/ur_robotiq_servo/__init__.py | 0 src/ur_robotiq_servo/config/joy-params.yaml | 3 +- src/ur_robotiq_servo/config/kb-params.yaml | 6 + .../launch/kb_servo.launch.py | 46 +++ src/ur_robotiq_servo/package.xml | 1 + src/ur_robotiq_servo/servo_keyboard_input.py | 196 +++++++++ src/ur_robotiq_servo/setup.py | 4 +- .../ur_robotiq_servo/kb_control.py | 142 +++++++ 15 files changed, 1104 insertions(+), 3 deletions(-) create mode 100644 src/__init__.py create mode 100644 src/servo_keyboard/CMakeLists.txt create mode 100644 src/servo_keyboard/LICENSE create mode 100644 src/servo_keyboard/package.xml create mode 100644 src/servo_keyboard/src/servo_keyboard_input.cpp create mode 100644 src/ur_robotiq_servo/__init__.py create mode 100644 src/ur_robotiq_servo/config/kb-params.yaml create mode 100644 src/ur_robotiq_servo/launch/kb_servo.launch.py create mode 100644 src/ur_robotiq_servo/servo_keyboard_input.py create mode 100644 src/ur_robotiq_servo/ur_robotiq_servo/kb_control.py diff --git a/.idea/.gitignore b/.idea/.gitignore index 13566b8..a9d7db9 100644 --- a/.idea/.gitignore +++ b/.idea/.gitignore @@ -6,3 +6,5 @@ # Datasource local storage ignored files /dataSources/ /dataSources.local.xml +# GitHub Copilot persisted chat sessions +/copilot/chatSessions diff --git a/src/__init__.py b/src/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/servo_keyboard/CMakeLists.txt b/src/servo_keyboard/CMakeLists.txt new file mode 100644 index 0000000..5fba1ea --- /dev/null +++ b/src/servo_keyboard/CMakeLists.txt @@ -0,0 +1,52 @@ +cmake_minimum_required(VERSION 3.8) +project(servo_keyboard) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(control_msgs REQUIRED) +find_package(controller_manager_msgs REQUIRED) + +add_executable(servo_keyboard_input src/servo_keyboard_input.cpp) +target_include_directories(servo_keyboard_input PUBLIC + $ + $) +target_compile_features(servo_keyboard_input PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +# Include directories +target_include_directories(servo_keyboard_input PRIVATE + ${rclcpp_INCLUDE_DIRS} + ${geometry_msgs_INCLUDE_DIRS} + ${control_msgs_INCLUDE_DIRS} + ${controller_manager_msgs_INCLUDE_DIRS} +) + +# Link the executable with required libraries +ament_target_dependencies(servo_keyboard_input + rclcpp + geometry_msgs + control_msgs + controller_manager_msgs +) + +install(TARGETS servo_keyboard_input + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/servo_keyboard/LICENSE b/src/servo_keyboard/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/src/servo_keyboard/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/src/servo_keyboard/package.xml b/src/servo_keyboard/package.xml new file mode 100644 index 0000000..9014e35 --- /dev/null +++ b/src/servo_keyboard/package.xml @@ -0,0 +1,26 @@ + + + + servo_keyboard + 0.0.0 + TODO: Package description + Fotios Lyegrakis + Apache-2.0 + + ament_cmake + + + rclcpp + control_msgs + geometry_msgs + controller_manager_msgs + controller_manager_msgs + + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/servo_keyboard/src/servo_keyboard_input.cpp b/src/servo_keyboard/src/servo_keyboard_input.cpp new file mode 100644 index 0000000..e29dc4a --- /dev/null +++ b/src/servo_keyboard/src/servo_keyboard_input.cpp @@ -0,0 +1,389 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik LLC nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Title : servo_keyboard_input.cpp + * Project : moveit2_tutorials + * Created : 05/31/2021 + * Author : Adam Pettinger + */ + +#include +#include +#include +#include // Add necessary includes +#include // Add necessary includes + +#include +#include +#include +#include + +// Define used keys +#define KEYCODE_LEFT 0x44 +#define KEYCODE_RIGHT 0x43 +#define KEYCODE_UP 0x41 +#define KEYCODE_DOWN 0x42 +#define KEYCODE_PERIOD 0x2E +#define KEYCODE_SEMICOLON 0x3B +#define KEYCODE_E 0x65 +#define KEYCODE_W 0x77 +#define KEYCODE_1 0x31 +#define KEYCODE_2 0x32 +#define KEYCODE_3 0x33 +#define KEYCODE_4 0x34 +#define KEYCODE_5 0x35 +#define KEYCODE_6 0x36 +#define KEYCODE_R 0x72 +#define KEYCODE_Q 0x71 +#define KEYCODE_PLUS 0x2B // Keycode for the plus sign (+) +#define KEYCODE_MINUS 0x2D // Keycode for the minus sign (-) +#define KEYCODE_GRIPPER 0x67 // Keycode for the gripper control button (g) + +// Some constants used in the Servo Teleop demo +const std::string TWIST_TOPIC = "/servo_node/delta_twist_cmds"; +const std::string JOINT_TOPIC = "/servo_node/delta_joint_cmds"; +const std::string GRIPPER_TOPIC = "/forward_gripper_position_controller/commands"; +const size_t ROS_QUEUE_SIZE = 10; +const std::string EEF_FRAME_ID = "world"; +const std::string BASE_FRAME_ID = "tool0"; + +// A class for reading the key inputs from the terminal +class KeyboardReader +{ +public: + KeyboardReader() : kfd(0) + { + // get the console in raw mode + tcgetattr(kfd, &cooked); + struct termios raw; + memcpy(&raw, &cooked, sizeof(struct termios)); + raw.c_lflag &= ~(ICANON | ECHO); + // Setting a new line, then end of file + raw.c_cc[VEOL] = 1; + raw.c_cc[VEOF] = 2; + tcsetattr(kfd, TCSANOW, &raw); + } + void readOne(char* c) + { + int rc = read(kfd, c, 1); + if (rc < 0) + { + throw std::runtime_error("read failed"); + } + } + void shutdown() + { + tcsetattr(kfd, TCSANOW, &cooked); + } + +private: + int kfd; + struct termios cooked; +}; + +// Converts key-presses to Twist or Jog commands for Servo, in lieu of a controller +class KeyboardServo +{ +public: + KeyboardServo(); + int keyLoop(); + +private: + void spin(); + + void handlePlusPress(); + void handleMinusPress(); + + void publishGripperCommand(double finger_joint_angle); + + rclcpp::Node::SharedPtr nh_; + + rclcpp::Publisher::SharedPtr twist_pub_; + rclcpp::Publisher::SharedPtr joint_pub_; + rclcpp::Publisher::SharedPtr gripper_cmd_pub_; + + std::string frame_to_publish_; + double joint_vel_cmd_; + double gripper_speed_multiplier_; + double last_finger_joint_angle_; + double gripper_lower_limit_; + double gripper_upper_limit_; +}; + +KeyboardServo::KeyboardServo() + : frame_to_publish_(BASE_FRAME_ID), + joint_vel_cmd_(1.0), + gripper_speed_multiplier_(0.01), // Example value, adjust as needed + last_finger_joint_angle_(0.0), // Example value, adjust as needed + gripper_lower_limit_(0.0), // Example value, adjust as needed + gripper_upper_limit_(0.70) // Example value, adjust as needed +{ + nh_ = rclcpp::Node::make_shared("servo_keyboard_input"); + + twist_pub_ = nh_->create_publisher(TWIST_TOPIC, ROS_QUEUE_SIZE); + joint_pub_ = nh_->create_publisher(JOINT_TOPIC, ROS_QUEUE_SIZE); + gripper_cmd_pub_ = nh_->create_publisher(GRIPPER_TOPIC, ROS_QUEUE_SIZE); +} + +KeyboardReader input; + +void KeyboardServo::publishGripperCommand(double finger_joint_angle) +{ + auto msg = std::make_unique(); + msg->data.push_back(finger_joint_angle); + gripper_cmd_pub_->publish(std::move(msg)); +} + +void quit(int sig) +{ + (void)sig; + input.shutdown(); + rclcpp::shutdown(); + exit(0); +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + KeyboardServo keyboard_servo; + + signal(SIGINT, quit); + + int rc = keyboard_servo.keyLoop(); + input.shutdown(); + rclcpp::shutdown(); + + return rc; +} + +void KeyboardServo::spin() +{ + while (rclcpp::ok()) + { + rclcpp::spin_some(nh_); + } +} + +void KeyboardServo::handlePlusPress() +{ + // Calculate the new finger joint angle + double delta_angle = 1.0 * gripper_speed_multiplier_; + double new_finger_joint_angle = last_finger_joint_angle_ + delta_angle; +// printf("New finger joint angle: %f\n", new_finger_joint_angle); + // Check if the new angle is within the limits + if (new_finger_joint_angle <= gripper_upper_limit_) + { + // Update the finger joint angle + last_finger_joint_angle_ = new_finger_joint_angle; +// printf("New finger joint angle: %f\n", last_finger_joint_angle_); + // Publish the gripper command with the new angle + publishGripperCommand(last_finger_joint_angle_); + } +} + +void KeyboardServo::handleMinusPress() +{ + // Calculate the new finger joint angle + double delta_angle = -1.0 * gripper_speed_multiplier_; + double new_finger_joint_angle = last_finger_joint_angle_ + delta_angle; + + // Check if the new angle is within the limits + if (new_finger_joint_angle >= gripper_lower_limit_) + { + // Update the finger joint angle + last_finger_joint_angle_ = new_finger_joint_angle; + + // Publish the gripper command with the new angle + publishGripperCommand(last_finger_joint_angle_); + } +} + + + +int KeyboardServo::keyLoop() +{ + char c; + bool publish_twist = false; + bool publish_joint = false; + + std::thread{ std::bind(&KeyboardServo::spin, this) }.detach(); + + puts("Reading from keyboard"); + puts("---------------------------"); + puts("Use arrow keys and the '.' and ';' keys to Cartesian jog"); + puts("Use 'W' to Cartesian jog in the world frame, and 'E' for the End-Effector frame"); + puts("Use 1|2|3|4|5|6|7 keys to joint jog. 'R' to reverse the direction of jogging."); + puts("Use '+' to open the gripper, '-' to close it."); + puts("'Q' to quit."); + + + for (;;) + { + // get the next event from the keyboard + try + { + input.readOne(&c); + } + catch (const std::runtime_error&) + { + perror("read():"); + return -1; + } + + RCLCPP_DEBUG(nh_->get_logger(), "value: 0x%02X\n", c); + + // // Create the messages we might publish + auto twist_msg = std::make_unique(); + auto joint_msg = std::make_unique(); + + // Use read key-press + switch (c) + { + case KEYCODE_LEFT: + RCLCPP_DEBUG(nh_->get_logger(), "LEFT"); + twist_msg->twist.linear.y = -1.0; + publish_twist = true; + break; + case KEYCODE_RIGHT: + RCLCPP_DEBUG(nh_->get_logger(), "RIGHT"); + twist_msg->twist.linear.y = 1.0; + publish_twist = true; + break; + case KEYCODE_UP: + RCLCPP_DEBUG(nh_->get_logger(), "UP"); + twist_msg->twist.linear.x = 1.0; + publish_twist = true; + break; + case KEYCODE_DOWN: + RCLCPP_DEBUG(nh_->get_logger(), "DOWN"); + twist_msg->twist.linear.x = -1.0; + publish_twist = true; + break; + case KEYCODE_PERIOD: + RCLCPP_DEBUG(nh_->get_logger(), "PERIOD"); + twist_msg->twist.linear.z = -1.0; + publish_twist = true; + break; + case KEYCODE_SEMICOLON: + RCLCPP_DEBUG(nh_->get_logger(), "SEMICOLON"); + twist_msg->twist.linear.z = 1.0; + publish_twist = true; + break; + case KEYCODE_E: + RCLCPP_DEBUG(nh_->get_logger(), "E"); + frame_to_publish_ = EEF_FRAME_ID; + break; + case KEYCODE_W: + RCLCPP_DEBUG(nh_->get_logger(), "W"); + frame_to_publish_ = BASE_FRAME_ID; + break; + case KEYCODE_1: + RCLCPP_DEBUG(nh_->get_logger(), "1"); + joint_msg->joint_names.push_back("shoulder_lift_joint"); + joint_msg->velocities.push_back(joint_vel_cmd_); + publish_joint = true; + break; + case KEYCODE_2: + RCLCPP_DEBUG(nh_->get_logger(), "2"); + joint_msg->joint_names.push_back("shoulder_pan_joint"); + joint_msg->velocities.push_back(joint_vel_cmd_); + publish_joint = true; + break; + case KEYCODE_3: + RCLCPP_DEBUG(nh_->get_logger(), "3"); + joint_msg->joint_names.push_back("elbow_joint"); + joint_msg->velocities.push_back(joint_vel_cmd_); + publish_joint = true; + break; + case KEYCODE_4: + RCLCPP_DEBUG(nh_->get_logger(), "4"); + joint_msg->joint_names.push_back("wrist_1_joint"); + joint_msg->velocities.push_back(joint_vel_cmd_); + publish_joint = true; + break; + case KEYCODE_5: + RCLCPP_DEBUG(nh_->get_logger(), "5"); + joint_msg->joint_names.push_back("wrist_2_joint"); + joint_msg->velocities.push_back(joint_vel_cmd_); + publish_joint = true; + break; + case KEYCODE_6: + RCLCPP_DEBUG(nh_->get_logger(), "6"); + joint_msg->joint_names.push_back("wrist_3_joint"); + joint_msg->velocities.push_back(joint_vel_cmd_); + publish_joint = true; + break; +// case KEYCODE_7: +// RCLCPP_DEBUG(nh_->get_logger(), "7"); +// joint_msg->joint_names.push_back("finger_joint"); +// joint_msg->velocities.push_back(joint_vel_cmd_); +// publish_joint = true; +// break; + case KEYCODE_R: + RCLCPP_DEBUG(nh_->get_logger(), "R"); + joint_vel_cmd_ *= -1; + break; + case KEYCODE_Q: + RCLCPP_DEBUG(nh_->get_logger(), "quit"); + return 0; + // Add cases for other keys as needed + case KEYCODE_PLUS: + RCLCPP_DEBUG(nh_->get_logger(), "PLUS"); + handlePlusPress(); + break; + case KEYCODE_MINUS: + RCLCPP_DEBUG(nh_->get_logger(), "MINUS"); + handleMinusPress(); + break; + } + + // If a key requiring a publish was pressed, publish the message now + if (publish_twist) + { + twist_msg->header.stamp = nh_->now(); + twist_msg->header.frame_id = frame_to_publish_; + twist_pub_->publish(std::move(twist_msg)); + publish_twist = false; + } + else if (publish_joint) + { + joint_msg->header.stamp = nh_->now(); + joint_msg->header.frame_id = BASE_FRAME_ID; + joint_pub_->publish(std::move(joint_msg)); + publish_joint = false; + } + } + + return 0; +} \ No newline at end of file diff --git a/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py b/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py index 776bc8a..616a956 100644 --- a/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py +++ b/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py @@ -6,7 +6,8 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, OpaqueFunction from launch.conditions import IfCondition, UnlessCondition from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution, TextSubstitution - +# from moveit_configs_utils import MoveItConfigsBuilder +# from launch_param_builder import ParameterBuilder def launch_setup(context, *args, **kwargs): # Initialize Arguments @@ -170,6 +171,7 @@ def launch_setup(context, *args, **kwargs): robot_description = {"robot_description": robot_description_content} + initial_joint_controllers = PathJoinSubstitution( [FindPackageShare(description_package), "config", controllers_file] ) @@ -275,6 +277,38 @@ def launch_setup(context, *args, **kwargs): }, ], ) + ############################################################################################################ + # # Get parameters for the Servo node + # servo_params = ( + # ParameterBuilder("ur_robotiq_servo") + # .yaml( + # parameter_namespace="moveit_servo", + # file_path="config/ur_robotiq_simulated_config.yaml", + # ) + # .to_dict() + # ) + # print(servo_params) + # + # # A node to publish world -> panda_link0 transform + # static_tf = Node( + # package="tf2_ros", + # executable="static_transform_publisher", + # name="static_transform_publisher", + # output="log", + # arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"], + # ) + # + # # The servo cpp interface demo + # # Creates the Servo node and publishes commands to it + # servo_node = Node( + # package="moveit2_tutorials", + # executable="servo_cpp_interface_demo", + # output="screen", + # parameters=[ + # servo_params, + # robot_description, + # ], + # ) robot_state_publisher_node = Node( package="robot_state_publisher", @@ -353,6 +387,8 @@ def launch_setup(context, *args, **kwargs): ur_control_node, dashboard_client_node, controller_stopper_node, + # static_tf, + # servo_node, robot_state_publisher_node, rviz_node, initial_joint_controller_spawner_stopped, diff --git a/src/ur_robotiq_servo/__init__.py b/src/ur_robotiq_servo/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/ur_robotiq_servo/config/joy-params.yaml b/src/ur_robotiq_servo/config/joy-params.yaml index 8ad2d13..f8747b6 100644 --- a/src/ur_robotiq_servo/config/joy-params.yaml +++ b/src/ur_robotiq_servo/config/joy-params.yaml @@ -1,7 +1,8 @@ joy_node: ros__parameters: device_id: 0 - device_name: "PS5 Controller" +# device_name: "PS5 Controller" + device_name: "Keyboard" deadzone: 0.5 autorepeat_rate: 20.0 sticky_buttons: false diff --git a/src/ur_robotiq_servo/config/kb-params.yaml b/src/ur_robotiq_servo/config/kb-params.yaml new file mode 100644 index 0000000..acc2b1d --- /dev/null +++ b/src/ur_robotiq_servo/config/kb-params.yaml @@ -0,0 +1,6 @@ +keyboard_servo_node: + ros__parameters: + linear_speed_multiplier: 0.2 + gripper_speed_multiplier: 0.02 + gripper_lower_limit: 0.0 + gripper_upper_limit: 0.70 \ No newline at end of file diff --git a/src/ur_robotiq_servo/launch/kb_servo.launch.py b/src/ur_robotiq_servo/launch/kb_servo.launch.py new file mode 100644 index 0000000..14e3a40 --- /dev/null +++ b/src/ur_robotiq_servo/launch/kb_servo.launch.py @@ -0,0 +1,46 @@ +import os + +from launch import LaunchDescription +from launch_ros.actions import Node +import ament_index_python.packages +from launch_param_builder import ParameterBuilder +from moveit_configs_utils import MoveItConfigsBuilder + +def generate_launch_description(): + config_directory = os.path.join( + ament_index_python.packages.get_package_share_directory('ur_robotiq_servo'), + 'config') + joy_params = os.path.join(config_directory, 'joy-params.yaml') + ps5_params = os.path.join(config_directory, 'ps5-params.yaml') + + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .to_moveit_configs() + ) + # Get parameters for the Servo node + servo_params = ( + ParameterBuilder("moveit_servo") + .yaml( + parameter_namespace="moveit_servo", + file_path="config/panda_simulated_config.yaml", + ) + .to_dict() + ) + + # The servo cpp interface demo + # Creates the Servo node and publishes commands to it + servo_node = Node( + package="moveit2_tutorials", + executable="servo_cpp_interface_demo", + output="screen", + parameters=[ + servo_params, + moveit_config.robot_description, + moveit_config.robot_description_semantic, + ], + ) + + return LaunchDescription([ + servo_node + ]) \ No newline at end of file diff --git a/src/ur_robotiq_servo/package.xml b/src/ur_robotiq_servo/package.xml index 3099325..88eaedd 100644 --- a/src/ur_robotiq_servo/package.xml +++ b/src/ur_robotiq_servo/package.xml @@ -14,6 +14,7 @@ std_srvs joy moveit_servo + keyboard ament_copyright ament_flake8 diff --git a/src/ur_robotiq_servo/servo_keyboard_input.py b/src/ur_robotiq_servo/servo_keyboard_input.py new file mode 100644 index 0000000..4e3dc49 --- /dev/null +++ b/src/ur_robotiq_servo/servo_keyboard_input.py @@ -0,0 +1,196 @@ +#!/usr/bin/env python3 + +import sys +import tty +import termios +import threading +import signal +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import TwistStamped +from control_msgs.msg import JointJog + +# Define key codes +KEYCODE_RIGHT = 0x43 +KEYCODE_LEFT = 0x44 +KEYCODE_UP = 0x41 +KEYCODE_DOWN = 0x42 +KEYCODE_PERIOD = 0x2E +KEYCODE_SEMICOLON = 0x3B +KEYCODE_1 = 0x31 +KEYCODE_2 = 0x32 +KEYCODE_3 = 0x33 +KEYCODE_4 = 0x34 +KEYCODE_5 = 0x35 +KEYCODE_6 = 0x36 +KEYCODE_7 = 0x37 +KEYCODE_Q = 0x71 +KEYCODE_R = 0x72 +KEYCODE_J = 0x6A +KEYCODE_T = 0x74 +KEYCODE_W = 0x77 +KEYCODE_E = 0x65 + +# Constants used in the Servo Teleop demo +TWIST_TOPIC = "/servo_node/delta_twist_cmds" +JOINT_TOPIC = "/servo_node/delta_joint_cmds" +ROS_QUEUE_SIZE = 10 +PLANNING_FRAME_ID = "world" +EE_FRAME_ID = "tool0" + + +class KeyboardReader: + def __init__(self): + self.file_descriptor = sys.stdin.fileno() + self.old_settings = termios.tcgetattr(self.file_descriptor) + tty.setraw(self.file_descriptor) + + def read_one(self): + return sys.stdin.read(1) + + def shutdown(self): + termios.tcsetattr(self.file_descriptor, termios.TCSADRAIN, self.old_settings) + + +class KeyboardServo: + def __init__(self): + self.joint_vel_cmd = 1.0 + self.command_frame_id = PLANNING_FRAME_ID + self.node = rclpy.create_node("servo_keyboard_input") + self.twist_pub = self.node.create_publisher(TwistStamped, TWIST_TOPIC, ROS_QUEUE_SIZE) + self.joint_pub = self.node.create_publisher(JointJog, JOINT_TOPIC, ROS_QUEUE_SIZE) + self.switch_input = self.node.create_client(ServoCommandType, "servo_node/switch_command_type") + self.request = ServoCommandType.Request() + + def spin(self): + rclpy.spin(self.node) + + def key_loop(self): + publish_twist = False + publish_joint = False + print("Reading from keyboard") + print("---------------------------") + print("All commands are in the planning frame") + print("Use arrow keys and the '.' and ';' keys to Cartesian jog") + print("Use 1|2|3|4|5|6|7 keys to joint jog. 'r' to reverse the direction of jogging.") + print("Use 'j' to select joint jog.") + print("Use 't' to select twist") + print("Use 'w' and 'e' to switch between sending command in planning frame or end effector frame") + print("'Q' to quit.") + + while True: + c = input() + + twist_msg = TwistStamped() + joint_msg = JointJog() + joint_msg.joint_names = [ + "shoulder_lift_joint", + "shoulder_pan_joint", + "elbow_joint", + "wrist_1_joint", + "wrist_2_joint", + "wrist_2_joint", + "finger_joint", + ] + joint_msg.velocities = [0.0] * 7 + + # Use read key-press + if c == '\x1b': # ANSI escape sequence + c = input() + if c == '[': + c = input() + if c == 'A': + twist_msg.twist.linear.x = 0.5 # UP + publish_twist = True + elif c == 'B': + twist_msg.twist.linear.x = -0.5 # DOWN + publish_twist = True + elif c == 'C': + twist_msg.twist.linear.y = 0.5 # RIGHT + publish_twist = True + elif c == 'D': + twist_msg.twist.linear.y = -0.5 # LEFT + publish_twist = True + elif ord(c) == KEYCODE_PERIOD: + twist_msg.twist.linear.z = -0.5 # '.' + publish_twist = True + elif ord(c) == KEYCODE_SEMICOLON: + twist_msg.twist.linear.z = 0.5 # ';' + publish_twist = True + elif ord(c) == KEYCODE_1: + joint_msg.velocities[0] = self.joint_vel_cmd # '1' + publish_joint = True + elif ord(c) == KEYCODE_2: + joint_msg.velocities[1] = self.joint_vel_cmd # '2' + publish_joint = True + elif ord(c) == KEYCODE_3: + joint_msg.velocities[2] = self.joint_vel_cmd # '3' + publish_joint = True + elif ord(c) == KEYCODE_4: + joint_msg.velocities[3] = self.joint_vel_cmd # '4' + publish_joint = True + elif ord(c) == KEYCODE_5: + joint_msg.velocities[4] = self.joint_vel_cmd # '5' + publish_joint = True + elif ord(c) == KEYCODE_6: + joint_msg.velocities[5] = self.joint_vel_cmd # '6' + publish_joint = True + elif ord(c) == KEYCODE_7: + joint_msg.velocities[6] = self.joint_vel_cmd # '7' + publish_joint = True + elif ord(c) == KEYCODE_R: + self.joint_vel_cmd *= -1 # 'r' + elif ord(c) == KEYCODE_J: + self.request.command_type = ServoCommandType.Request.JOINT_JOG # 'j' + if self.switch_input.wait_for_service(timeout_sec=1): + result = self.switch_input.call(self.request) + if result.success: + print("Switched to input type: JointJog") + else: + print("Could not switch input to: JointJog") + elif ord(c) == KEYCODE_T: + self.request.command_type = ServoCommandType.Request.TWIST # 't' + if self.switch_input.wait_for_service(timeout_sec=1): + result = self.switch_input.call(self.request) + if result.success: + print("Switched to input type: Twist") + else: + print("Could not switch input to: Twist") + elif ord(c) == KEYCODE_W: + print(f"Command frame set to: {PLANNING_FRAME_ID}") # 'w' + self.command_frame_id = PLANNING_FRAME_ID + elif ord(c) == KEYCODE_E: + print(f"Command frame set to: {EE_FRAME_ID}") # 'e' + self.command_frame_id = EE_FRAME_ID + elif ord(c) == KEYCODE_Q: + print("Quit") # 'Q' + return 0 + + # If a key requiring a publish was pressed, publish the message now + if publish_twist: + twist_msg.header.stamp = self.node.get_clock().now().to_msg() + twist_msg.header.frame_id = self.command_frame_id + self.twist_pub.publish(twist_msg) + publish_twist = False + elif publish_joint: + joint_msg.header.stamp = self.node.get_clock().now().to_msg() + joint_msg.header.frame_id = PLANNING_FRAME_ID + self.joint_pub.publish(joint_msg) + publish_joint = False + + return 0 + + +def quit_handler(sig, frame): + input_reader.shutdown() + rclpy.shutdown() + sys.exit(0) + + +if __name__ == "__main__": + signal.signal(signal.SIGINT, quit_handler) + rclpy.init(args=sys.argv) + input_reader = KeyboardReader() + servo_keyboard = KeyboardServo() + threading.Thread(target=servo_keyboard.spin).start() + servo_keyboard.key_loop() diff --git a/src/ur_robotiq_servo/setup.py b/src/ur_robotiq_servo/setup.py index d91e499..1df7211 100644 --- a/src/ur_robotiq_servo/setup.py +++ b/src/ur_robotiq_servo/setup.py @@ -24,7 +24,9 @@ setup( tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'ps5_servo = ur_robotiq_servo.ps5_control:main' + 'ps5_servo = ur_robotiq_servo.ps5_control:main', + 'kb_servo = ur_robotiq_servo.kb_control:main', + # 'servo_kb_input = ur_robotiq_servo.servo_keyboard_input:main', ], }, ) diff --git a/src/ur_robotiq_servo/ur_robotiq_servo/kb_control.py b/src/ur_robotiq_servo/ur_robotiq_servo/kb_control.py new file mode 100644 index 0000000..bb7c843 --- /dev/null +++ b/src/ur_robotiq_servo/ur_robotiq_servo/kb_control.py @@ -0,0 +1,142 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Joy, JointState +from geometry_msgs.msg import TwistStamped +from control_msgs.msg import JointJog +from std_srvs.srv import Trigger +from std_msgs.msg import Float64MultiArray, MultiArrayDimension, MultiArrayLayout +import keyboard + +class KBControllerNode(Node): + def __init__(self): + super().__init__('kb_controller_node') + # States + self.mode = 'twist' # Initialize mode to 'twist'. Alternatives: 'twist', 'joint' + self.last_button_state = 0 # Track the last state of the toggle button to detect presses + self.last_finger_joint_angle = 0.0 + + # Parameters + self.linear_speed_multiplier = self.declare_parameter('linear_speed_multiplier', 1.0) + self.linear_speed_multiplier = self.get_parameter('linear_speed_multiplier').get_parameter_value().double_value + self.get_logger().info(f"Linear speed multiplier: {self.linear_speed_multiplier}") + + self.use_fake_hardware = self.declare_parameter('use_fake_hardware', False) + self.use_fake_hardware = self.get_parameter('use_fake_hardware').get_parameter_value().bool_value + self.get_logger().info(f"Use fake hardware: {self.use_fake_hardware}") + + self.gripper_speed_multiplier = self.declare_parameter('gripper_speed_multiplier', 1.0) + self.gripper_speed_multiplier = (self.get_parameter('gripper_speed_multiplier') + .get_parameter_value().double_value) + self.get_logger().info(f"Gripper speed multiplier: {self.gripper_speed_multiplier}") + + self.gripper_lower_limit = self.declare_parameter('gripper_lower_limit', 1.0) + self.gripper_lower_limit = (self.get_parameter('gripper_lower_limit') + .get_parameter_value().double_value) + self.get_logger().info(f"Gripper lower limit: {self.gripper_lower_limit}") + + self.gripper_upper_limit = self.declare_parameter('gripper_upper_limit', 1.0) + self.gripper_upper_limit = (self.get_parameter('gripper_upper_limit') + .get_parameter_value().double_value) + self.get_logger().info(f"Gripper upper limit: {self.gripper_upper_limit}") + # Subscriber and Publisher + self.joint_state_sub = self.create_subscription( + JointState, + '/joint_states', + self.joint_state_callback, + 10) + + self.joy_sub = self.create_subscription( + Joy, + '/joy', + self.keyboard_callback, + 10) + + self.twist_pub = self.create_publisher( + TwistStamped, + '/servo_node/delta_twist_cmds', + 10) + + self.joint_pub = self.create_publisher( + JointJog, + '/servo_node/delta_joint_cmds', + 10) + + self.gripper_cmd_pub = self.create_publisher( + Float64MultiArray, + '/forward_gripper_position_controller/commands', + 10) + + # Services + self.servo_client = self.create_client(Trigger, '/servo_node/start_servo') + + srv_msg = Trigger.Request() + while not self.servo_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('/servo_node/start_servo service not available, waiting again...') + + self.call_start_servo() + + self.get_logger().info('kb_servo_node started!') + + def call_start_servo(self): + request = Trigger.Request() + future = self.servo_client.call_async(request) + rclpy.spin_until_future_complete(self, future) + response = future.result() + if response.success: + self.get_logger().info(f'Successfully called start_servo: {response.message}') + else: + self.get_logger().info('Failed to call start_servo') + + def joint_state_callback(self, msg): + try: + index = msg.name.index('finger_joint') + self.last_finger_joint_angle = msg.position[index] + except ValueError: + self.get_logger().error('finger_joint not found in /joint_states msg') + + def keyboard_callback(self, event): + # Process keyboard events + print("Key pressed") + if event.event_type == keyboard.KEY_DOWN: + # Handle key presses + if event.name == 't': + # Toggle mode between twist and joint control + self.mode = 'joint' if self.mode == 'twist' else 'twist' + self.get_logger().info(f'Mode switched to: {self.mode}') + elif event.name == 'w': + # Move forward + self.publish_twist(1.0 * self.linear_speed_multiplier) # Adjust speed as needed + self.get_logger().info('Moving forward') + elif event.name == 's': + # Move backward + self.publish_twist(-1.0 * self.linear_speed_multiplier) # Adjust speed as needed + self.get_logger().info('Moving backward') + elif event.event_type == keyboard.KEY_UP: + # Handle key releases + if event.name == 'w' or event.name == 's': + # Stop moving + self.publish_twist(0.0) + self.get_logger().info('Stopped moving') + + def publish_twist(self, linear_speed): + twist_msg = TwistStamped() + twist_msg.header.frame_id = 'tool0' + twist_msg.header.stamp = self.get_clock().now().to_msg() + twist_msg.twist.linear.x = linear_speed + twist_msg.twist.linear.y = 0.0 # Adjust as needed + twist_msg.twist.linear.z = 0.0 # Adjust as needed + twist_msg.twist.angular.x = 0.0 + twist_msg.twist.angular.y = 0.0 + twist_msg.twist.angular.z = 0.0 + self.twist_pub.publish(twist_msg) + +def main(args=None): + rclpy.init(args=args) + node = KBControllerNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file