diff --git a/.github/workflows/ros-ci.yml b/.github/workflows/ros-ci.yml index 6d5a950..2a187c2 100644 --- a/.github/workflows/ros-ci.yml +++ b/.github/workflows/ros-ci.yml @@ -15,20 +15,10 @@ jobs: fail-fast: false matrix: ros_distribution: - # - foxy - # - galactic - humble # - rolling include: - # # Foxy Fitzroy (June 2020 - May 2023) - # - docker_image: rostooling/setup-ros-docker:ubuntu-focal-ros-foxy-ros-base-latest - # ros_distribution: foxy - # ros_version: 2 - # # Galactic Geochelone (May 2021) - # - docker_image: rostooling/setup-ros-docker:ubuntu-focal-ros-galactic-ros-base-latest - # ros_distribution: galactic - # ros_version: 2 - # Humble Hawksbill (May 2027) + # Humble Hawksbill (May 2027) - docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-humble-ros-base-latest ros_distribution: humble ros_version: 2 @@ -50,4 +40,4 @@ jobs: with: package-name: turtlebot3_manipulation target-ros2-distro: ${{ matrix.ros_distribution }} - vcs-repo-file-url: "" \ No newline at end of file + vcs-repo-file-url: "" diff --git a/README.md b/README.md index ea35e56..6e9ea02 100644 --- a/README.md +++ b/README.md @@ -2,11 +2,6 @@ -## ROS Packages for OpenManipulator with TurtleBot3 -|Version|Kinetic + Ubuntu Xenial|Melodic + Ubuntu Bionic| -|:---:|:---:|:---:| -|[![GitHub version](https://badge.fury.io/gh/ROBOTIS-GIT%2Fopen_manipulator_with_tb3.svg)](https://badge.fury.io/gh/ROBOTIS-GIT%2Fopen_manipulator_with_tb3)|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/open_manipulator_with_tb3.svg?branch=kinetic-devel)](https://travis-ci.org/ROBOTIS-GIT/open_manipulator_with_tb3)|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/open_manipulator_with_tb3.svg?branch=melodic-devel)](https://travis-ci.org/ROBOTIS-GIT/open_manipulator_with_tb3)| - ## ROBOTIS e-Manual for OpenManipulator with TurtleBot3 - [ROBOTIS e-Manual for OpenManipulator with TurtleBot3](http://emanual.robotis.com/docs/en/platform/turtlebot3/manipulation/#manipulation) @@ -45,7 +40,6 @@ ## Documents and Videos related to OpenManipulator with TurtleBot3 - [ROBOTIS e-Manual for OpenManipulator](http://emanual.robotis.com/docs/en/platform/openmanipulator/) - [ROBOTIS e-Manual for TurtleBot3](http://turtlebot3.robotis.com/) -- [ROBOTIS e-Manual for ROBOTIS MANIPULATOR-H](http://emanual.robotis.com/docs/en/platform/manipulator_h/introduction/) - [ROBOTIS e-Manual for Dynamixel SDK](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/overview/) - [ROBOTIS e-Manual for Dynamixel Workbench](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_workbench/) - [e-Book for TurtleBot3 and OpenManipulator](https://community.robotsource.org/t/download-the-ros-robot-programming-book-for-free/51/) diff --git a/turtlebot3_manipulation/CHANGELOG.rst b/turtlebot3_manipulation/CHANGELOG.rst index 836fe5f..5fbc20f 100644 --- a/turtlebot3_manipulation/CHANGELOG.rst +++ b/turtlebot3_manipulation/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package turtlebot3_manipulation ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.0 (2024-12-02) +------------------ +* Added gripper control in teleoperation. +* Resolved the issue where teleoperation was not functioning on the actual robot. +* Fixed the intermittent issue of Gazebo not launching. +* Fixed the error log related to the mimic joint. +* Contributors: Sungho Woo + 2.1.1 (2022-10-14) ------------------ * Support ROS2 Humble diff --git a/turtlebot3_manipulation/package.xml b/turtlebot3_manipulation/package.xml index 3ea7aed..a074bf6 100644 --- a/turtlebot3_manipulation/package.xml +++ b/turtlebot3_manipulation/package.xml @@ -2,17 +2,18 @@ turtlebot3_manipulation - 2.1.1 + 2.2.0 ROS 2 package for turtlebot3_manipulation - Will Son + Pyo Apache 2.0 https://emanual.robotis.com/docs/en/platform/turtlebot3/manipulation/ https://github.com/ROBOTIS-GIT/turtlebot3_manipulation https://github.com/ROBOTIS-GIT/turtlebot3_manipulation/issues Darby Lim Hye-jong KIM + Sungho Woo ament_cmake turtlebot3_manipulation_bringup turtlebot3_manipulation_cartographer diff --git a/turtlebot3_manipulation_bringup/CHANGELOG.rst b/turtlebot3_manipulation_bringup/CHANGELOG.rst index db74d6f..117892c 100644 --- a/turtlebot3_manipulation_bringup/CHANGELOG.rst +++ b/turtlebot3_manipulation_bringup/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package turtlebot3_manipulation_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.0 (2024-12-02) +------------------ +* Updated maintainer information +* Contributors: Sungho Woo + 2.1.1 (2022-10-14) ------------------ * Support ROS2 Humble diff --git a/turtlebot3_manipulation_bringup/package.xml b/turtlebot3_manipulation_bringup/package.xml index f78dab9..bc7a968 100644 --- a/turtlebot3_manipulation_bringup/package.xml +++ b/turtlebot3_manipulation_bringup/package.xml @@ -2,11 +2,11 @@ turtlebot3_manipulation_bringup - 2.1.1 + 2.2.0 ROS 2 package for turtlebot3_manipulation - Will Son + Pyo Apache 2.0 https://emanual.robotis.com/docs/en/platform/turtlebot3/manipulation/ https://github.com/ROBOTIS-GIT/turtlebot3_manipulation diff --git a/turtlebot3_manipulation_cartographer/CHANGELOG.rst b/turtlebot3_manipulation_cartographer/CHANGELOG.rst index 25092c4..dcc5d2c 100644 --- a/turtlebot3_manipulation_cartographer/CHANGELOG.rst +++ b/turtlebot3_manipulation_cartographer/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package turtlebot3_manipulation_cartographer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.0 (2024-12-02) +------------------ +* Updated maintainer information +* Contributors: Sungho Woo + 2.1.1 (2022-10-14) ------------------ * Support ROS2 Humble diff --git a/turtlebot3_manipulation_cartographer/package.xml b/turtlebot3_manipulation_cartographer/package.xml index 580f7a5..d07627f 100644 --- a/turtlebot3_manipulation_cartographer/package.xml +++ b/turtlebot3_manipulation_cartographer/package.xml @@ -2,11 +2,11 @@ turtlebot3_manipulation_cartographer - 2.1.1 + 2.2.0 ROS 2 launch scripts for cartographer - Will Son + Pyo Apache 2.0 https://emanual.robotis.com/docs/en/platform/turtlebot3/manipulation/ https://github.com/ROBOTIS-GIT/turtlebot3_manipulation diff --git a/turtlebot3_manipulation_description/CHANGELOG.rst b/turtlebot3_manipulation_description/CHANGELOG.rst index af0b908..05489f9 100644 --- a/turtlebot3_manipulation_description/CHANGELOG.rst +++ b/turtlebot3_manipulation_description/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package turtlebot3_manipulation_description ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.0 (2024-12-02) +------------------ +* Fixed the intermittent issue of Gazebo not launching +* Fixed the error log related to the mimic joint. +* Contributors: Sungho Woo + 2.1.1 (2022-10-14) ------------------ * Support ROS2 Humble diff --git a/turtlebot3_manipulation_description/package.xml b/turtlebot3_manipulation_description/package.xml index ffdfc4c..e56b4bb 100644 --- a/turtlebot3_manipulation_description/package.xml +++ b/turtlebot3_manipulation_description/package.xml @@ -2,17 +2,18 @@ turtlebot3_manipulation_description - 2.1.1 + 2.2.0 ROS 2 package for turtlebot3_manipulation_description - Will Son + Pyo Apache 2.0 https://emanual.robotis.com/docs/en/platform/turtlebot3/manipulation/ https://github.com/ROBOTIS-GIT/turtlebot3_manipulation https://github.com/ROBOTIS-GIT/turtlebot3_manipulation/issues Darby Lim Hye-jong KIM + Sungho Woo ament_cmake joint_state_publisher joint_state_publisher_gui diff --git a/turtlebot3_manipulation_description/urdf/open_manipulator_x.urdf.xacro b/turtlebot3_manipulation_description/urdf/open_manipulator_x.urdf.xacro index 0ad10ea..936b164 100644 --- a/turtlebot3_manipulation_description/urdf/open_manipulator_x.urdf.xacro +++ b/turtlebot3_manipulation_description/urdf/open_manipulator_x.urdf.xacro @@ -24,7 +24,7 @@ https://github.com/ROBOTIS-GIT/open_manipulator/blob/kinetic-devel/open_manipula - + @@ -207,6 +207,12 @@ https://github.com/ROBOTIS-GIT/open_manipulator/blob/kinetic-devel/open_manipula + + + + + + diff --git a/turtlebot3_manipulation_hardware/CHANGELOG.rst b/turtlebot3_manipulation_hardware/CHANGELOG.rst index 8f5975a..a02d626 100644 --- a/turtlebot3_manipulation_hardware/CHANGELOG.rst +++ b/turtlebot3_manipulation_hardware/CHANGELOG.rst @@ -2,10 +2,15 @@ Changelog for package turtlebot3_manipulation_hardware ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.0 (2024-12-02) +------------------ +* Resolved the issue where teleoperation was not functioning on the actual robot +* Contributors: Sungho Woo + 2.1.1 (2022-10-14) ------------------ * Support ROS2 Humble * MoveIt environment configured * use ros2_control framework instead of ROBOTIS custom library * removed dependency to `turtlebot3_*`` and `open_manipulator` packages -* Contributors: Hye-Jong KIM, Darby Lim, Will Son \ No newline at end of file +* Contributors: Hye-Jong KIM, Darby Lim, Will Son diff --git a/turtlebot3_manipulation_hardware/package.xml b/turtlebot3_manipulation_hardware/package.xml index 4e75eb8..0aee74e 100644 --- a/turtlebot3_manipulation_hardware/package.xml +++ b/turtlebot3_manipulation_hardware/package.xml @@ -2,16 +2,17 @@ turtlebot3_manipulation_hardware - 2.1.1 + 2.2.0 ROS 2 package for turtlebot3_manipulation_hardware - Will Son + Pyo Apache 2.0 https://emanual.robotis.com/docs/en/platform/turtlebot3/manipulation/ https://github.com/ROBOTIS-GIT/turtlebot3_manipulation https://github.com/ROBOTIS-GIT/turtlebot3_manipulation/issues Darby Lim + Sungho Woo ament_cmake dynamixel_sdk hardware_interface diff --git a/turtlebot3_manipulation_hardware/src/turtlebot3_manipulation_system.cpp b/turtlebot3_manipulation_hardware/src/turtlebot3_manipulation_system.cpp index 3fa6140..46f03dc 100644 --- a/turtlebot3_manipulation_hardware/src/turtlebot3_manipulation_system.cpp +++ b/turtlebot3_manipulation_hardware/src/turtlebot3_manipulation_system.cpp @@ -94,9 +94,9 @@ hardware_interface::CallbackReturn TurtleBot3ManipulationSystemHardware::on_init dxl_joint_commands_.resize(4, 0.0); dxl_joint_commands_[0] = 0.0; - dxl_joint_commands_[1] = -1.57; - dxl_joint_commands_[2] = 1.37; - dxl_joint_commands_[3] = 0.26; + dxl_joint_commands_[1] = -1.05; + dxl_joint_commands_[2] = 1.05; + dxl_joint_commands_[3] = 0.0; dxl_gripper_commands_.resize(2, 0.0); diff --git a/turtlebot3_manipulation_moveit_config/.setup_assistant b/turtlebot3_manipulation_moveit_config/.setup_assistant index 0d5e671..73f22be 100644 --- a/turtlebot3_manipulation_moveit_config/.setup_assistant +++ b/turtlebot3_manipulation_moveit_config/.setup_assistant @@ -22,4 +22,4 @@ moveit_setup_assistant_config: - position state: - position - - velocity \ No newline at end of file + - velocity diff --git a/turtlebot3_manipulation_moveit_config/CHANGELOG.rst b/turtlebot3_manipulation_moveit_config/CHANGELOG.rst index 373d5d6..b3ed566 100644 --- a/turtlebot3_manipulation_moveit_config/CHANGELOG.rst +++ b/turtlebot3_manipulation_moveit_config/CHANGELOG.rst @@ -2,10 +2,15 @@ Changelog for package turtlebot3_manipulation_moveit_config ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.0 (2024-12-02) +------------------ +* Resolved the issue where teleoperation was not functioning on the actual robot +* Contributors: Sungho Woo + 2.1.1 (2022-10-14) ------------------ * Support ROS2 Humble * MoveIt environment configured * use ros2_control framework instead of ROBOTIS custom library * removed dependency to `turtlebot3_*`` and `open_manipulator` packages -* Contributors: Hye-Jong KIM, Darby Lim, Will Son \ No newline at end of file +* Contributors: Hye-Jong KIM, Darby Lim, Will Son diff --git a/turtlebot3_manipulation_moveit_config/config/kinematics.yaml b/turtlebot3_manipulation_moveit_config/config/kinematics.yaml index 603c9ad..b901127 100644 --- a/turtlebot3_manipulation_moveit_config/config/kinematics.yaml +++ b/turtlebot3_manipulation_moveit_config/config/kinematics.yaml @@ -2,4 +2,4 @@ arm: kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.05 - position_only_ik: false + position_only_ik: True diff --git a/turtlebot3_manipulation_moveit_config/config/moveit_controllers.yaml b/turtlebot3_manipulation_moveit_config/config/moveit_controllers.yaml index 2a56c01..a9e747c 100644 --- a/turtlebot3_manipulation_moveit_config/config/moveit_controllers.yaml +++ b/turtlebot3_manipulation_moveit_config/config/moveit_controllers.yaml @@ -17,4 +17,3 @@ gripper_controller: default: true joints: - gripper_left_joint - diff --git a/turtlebot3_manipulation_moveit_config/config/turtlebot3_manipulation.srdf b/turtlebot3_manipulation_moveit_config/config/turtlebot3_manipulation.srdf index 886fefd..2dc59c0 100644 --- a/turtlebot3_manipulation_moveit_config/config/turtlebot3_manipulation.srdf +++ b/turtlebot3_manipulation_moveit_config/config/turtlebot3_manipulation.srdf @@ -94,9 +94,11 @@ + + diff --git a/turtlebot3_manipulation_moveit_config/launch/move_group.launch.py b/turtlebot3_manipulation_moveit_config/launch/move_group.launch.py index 9877d34..a945983 100644 --- a/turtlebot3_manipulation_moveit_config/launch/move_group.launch.py +++ b/turtlebot3_manipulation_moveit_config/launch/move_group.launch.py @@ -96,7 +96,7 @@ def generate_launch_description(): "moveit_manage_controllers": True, "trajectory_execution.allowed_execution_duration_scaling": 1.2, "trajectory_execution.allowed_goal_duration_margin": 0.5, - "trajectory_execution.allowed_start_tolerance": 0.01, + "trajectory_execution.allowed_start_tolerance": 0.05, } # Moveit Controllers @@ -120,13 +120,15 @@ def generate_launch_description(): "publish_geometry_updates": True, "publish_state_updates": True, "publish_transforms_updates": True, + "publish_robot_description": True, + "publish_robot_description_semantic": True } ld = LaunchDescription() use_sim = LaunchConfiguration('use_sim') declare_use_sim = DeclareLaunchArgument( 'use_sim', - default_value='false', + default_value='true', description='Start robot in Gazebo simulation.') ld.add_action(declare_use_sim) diff --git a/turtlebot3_manipulation_moveit_config/launch/setup_assistant.launch.py b/turtlebot3_manipulation_moveit_config/launch/setup_assistant.launch.py index 7621045..fd169ea 100644 --- a/turtlebot3_manipulation_moveit_config/launch/setup_assistant.launch.py +++ b/turtlebot3_manipulation_moveit_config/launch/setup_assistant.launch.py @@ -23,4 +23,4 @@ def generate_launch_description(): moveit_config = MoveItConfigsBuilder("turtlebot3_manipulation", package_name="turtlebot3_manipulation_moveit_config").to_moveit_configs() - return generate_setup_assistant_launch(moveit_config) \ No newline at end of file + return generate_setup_assistant_launch(moveit_config) diff --git a/turtlebot3_manipulation_moveit_config/package.xml b/turtlebot3_manipulation_moveit_config/package.xml index a698ab2..72ae8f0 100644 --- a/turtlebot3_manipulation_moveit_config/package.xml +++ b/turtlebot3_manipulation_moveit_config/package.xml @@ -2,16 +2,17 @@ turtlebot3_manipulation_moveit_config - 2.1.1 + 2.2.0 An automatically generated package with all the configuration and launch files for using the turtlebot3_manipulation with the MoveIt Motion Planning Framework - Will Son + Pyo BSD http://moveit.ros.org/ https://github.com/ros-planning/moveit2/issues https://github.com/ros-planning/moveit2 Hye-jong KIM + Sungho Woo ament_cmake moveit_ros_move_group diff --git a/turtlebot3_manipulation_navigation2/CHANGELOG.rst b/turtlebot3_manipulation_navigation2/CHANGELOG.rst index 4ef152a..d67a019 100644 --- a/turtlebot3_manipulation_navigation2/CHANGELOG.rst +++ b/turtlebot3_manipulation_navigation2/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package turtlebot3_manipulation_navigation2 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.0 (2024-12-02) +------------------ +* Updated maintainer information +* Contributors: Sungho Woo + 2.1.1 (2022-10-14) ------------------ * Support ROS2 Humble diff --git a/turtlebot3_manipulation_navigation2/package.xml b/turtlebot3_manipulation_navigation2/package.xml index 1bf2615..fee99bf 100644 --- a/turtlebot3_manipulation_navigation2/package.xml +++ b/turtlebot3_manipulation_navigation2/package.xml @@ -2,11 +2,11 @@ turtlebot3_manipulation_navigation2 - 2.1.1 + 2.2.0 ROS 2 launch scripts for navigation2 - Will Son + Pyo Apache 2.0 Darby Lim Pyo diff --git a/turtlebot3_manipulation_teleop/CHANGELOG.rst b/turtlebot3_manipulation_teleop/CHANGELOG.rst index 5aabf11..6d02b2b 100644 --- a/turtlebot3_manipulation_teleop/CHANGELOG.rst +++ b/turtlebot3_manipulation_teleop/CHANGELOG.rst @@ -2,10 +2,15 @@ Changelog for package turtlebot3_manipulation_teleop ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.0 (2024-12-02) +------------------ +* Added gripper control in teleoperation +* Contributors: Sungho Woo + 2.1.1 (2022-10-14) ------------------ * Support ROS2 Humble * MoveIt environment configured * use ros2_control framework instead of ROBOTIS custom library * removed dependency to `turtlebot3_*`` and `open_manipulator` packages -* Contributors: Hye-Jong KIM, Darby Lim, Will Son \ No newline at end of file +* Contributors: Hye-Jong KIM, Darby Lim, Will Son diff --git a/turtlebot3_manipulation_teleop/CMakeLists.txt b/turtlebot3_manipulation_teleop/CMakeLists.txt index ff62162..d49ac4d 100644 --- a/turtlebot3_manipulation_teleop/CMakeLists.txt +++ b/turtlebot3_manipulation_teleop/CMakeLists.txt @@ -21,6 +21,7 @@ find_package(nav_msgs REQUIRED) find_package(control_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_srvs REQUIRED) +find_package(rclcpp_action REQUIRED) add_executable(turtlebot3_manipulation_teleop src/turtlebot3_manipulation_teleop.cpp) target_include_directories(turtlebot3_manipulation_teleop PUBLIC @@ -37,6 +38,7 @@ ament_target_dependencies( "control_msgs" "sensor_msgs" "std_srvs" + "rclcpp_action" ) install(TARGETS turtlebot3_manipulation_teleop diff --git a/turtlebot3_manipulation_teleop/include/turtlebot3_manipulation_teleop/turtlebot3_manipulation_teleop.hpp b/turtlebot3_manipulation_teleop/include/turtlebot3_manipulation_teleop/turtlebot3_manipulation_teleop.hpp index 7fdab7a..1669c4c 100644 --- a/turtlebot3_manipulation_teleop/include/turtlebot3_manipulation_teleop/turtlebot3_manipulation_teleop.hpp +++ b/turtlebot3_manipulation_teleop/include/turtlebot3_manipulation_teleop/turtlebot3_manipulation_teleop.hpp @@ -1,19 +1,3 @@ -// Copyright 2022 ROBOTIS CO., LTD. -// -// 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. -// -// Author: Hye-jong KIM - #ifndef TURTLEBOT3_MANIPULATION_TELEOP__TURTLEBOT3_MANIPULATION_TELEOP_HPP_ #define TURTLEBOT3_MANIPULATION_TELEOP__TURTLEBOT3_MANIPULATION_TELEOP_HPP_ @@ -21,6 +5,9 @@ #include #include #include +#include +#include +#include #include #include @@ -30,26 +17,6 @@ #include // Define used keys -#define KEYCODE_UP 0x41 // Base Front -#define KEYCODE_DOWN 0x42 // Base Back -#define KEYCODE_RIGHT 0x43 // Base Right -#define KEYCODE_LEFT 0x44 // Base Left -#define KEYCODE_SPACE 0x20 // Base Stop - -#define KEYCODE_A 0x61 -#define KEYCODE_D 0x64 -#define KEYCODE_S 0x73 -#define KEYCODE_X 0x78 -#define KEYCODE_Z 0x7A -#define KEYCODE_C 0x63 -#define KEYCODE_F 0x66 -#define KEYCODE_V 0x76 - -#define KEYCODE_1 0x31 -#define KEYCODE_2 0x32 -#define KEYCODE_3 0x33 -#define KEYCODE_4 0x34 - #define KEYCODE_1 0x31 #define KEYCODE_2 0x32 #define KEYCODE_3 0x33 @@ -59,35 +26,30 @@ #define KEYCODE_E 0x65 #define KEYCODE_R 0x72 -#define KEYCODE_ESC 0x1B - -#define KEYCODE_5 0x35 -#define KEYCODE_6 0x36 -#define KEYCODE_7 0x37 #define KEYCODE_O 0x6F +#define KEYCODE_P 0x70 + +#define KEYCODE_J 0x6A #define KEYCODE_K 0x6B #define KEYCODE_L 0x6C -#define KEYCODE_P 0x70 -#define KEYCODE_SEMICOLON 0x3B -#define KEYCODE_PERIOD 0x2E +#define KEYCODE_I 0x69 +#define KEYCODE_SPACE 0x20 +#define KEYCODE_ESC 0x1B // Some constants used in the Servo Teleop demo const char BASE_TWIST_TOPIC[] = "cmd_vel"; -const char ARM_TWIST_TOPIC[] = "/servo_node/delta_twist_cmds"; const char ARM_JOINT_TOPIC[] = "/servo_node/delta_joint_cmds"; +const char GRIPPER_TOPIC[] = "/gripper_controller/gripper_cmd"; const size_t ROS_QUEUE_SIZE = 10; - const double BASE_LINEAR_VEL_MAX = 0.26; // m/s const double BASE_LINEAR_VEL_STEP = 0.01; // m/s const double BASE_ANGULAR_VEL_MAX = 1.8; // rad/s const double BASE_ANGULAR_VEL_STEP = 0.1; // rad/s +const char BASE_FRAME_ID[] = "link0"; -const char BASE_FRAME_ID[] = "link2"; - -const double ARM_TWIST_VEL = 0.2; // m/s -const double ARM_JOINT_VEL = 1.0; // rad/s +const double ARM_JOINT_VEL = 10.0; // rad/s // A class for reading the key inputs from the terminal class KeyboardReader @@ -113,8 +75,10 @@ class KeyboardServo void connect_moveit_servo(); void start_moveit_servo(); void stop_moveit_servo(); - + void send_goal(float position); private: + rclcpp_action::Client::SharedPtr client_; + void spin(); void pub(); @@ -122,17 +86,29 @@ class KeyboardServo rclcpp::Client::SharedPtr servo_start_client_; rclcpp::Client::SharedPtr servo_stop_client_; - rclcpp::Publisher::SharedPtr base_twist_pub_; - rclcpp::Publisher::SharedPtr arm_twist_pub_; rclcpp::Publisher::SharedPtr joint_pub_; geometry_msgs::msg::Twist cmd_vel_; - geometry_msgs::msg::TwistStamped task_msg_; control_msgs::msg::JointJog joint_msg_; + control_msgs::msg::GripperCommand gripper_cmd_; - bool publish_task_; bool publish_joint_; + + void goal_result_callback(const rclcpp_action::ClientGoalHandle::WrappedResult& result) + { + switch (result.code) + { + case rclcpp_action::ResultCode::SUCCEEDED: + break; + case rclcpp_action::ResultCode::ABORTED: + break; + case rclcpp_action::ResultCode::CANCELED: + break; + default: + break; + } + } }; KeyboardReader input; diff --git a/turtlebot3_manipulation_teleop/package.xml b/turtlebot3_manipulation_teleop/package.xml index d5a2e2f..e14a3fd 100644 --- a/turtlebot3_manipulation_teleop/package.xml +++ b/turtlebot3_manipulation_teleop/package.xml @@ -2,14 +2,15 @@ turtlebot3_manipulation_teleop - 2.1.1 + 2.2.0 Ros2 Package of the turtlebot3_manipulation_teleop - Will Son + Pyo Apache 2.0 https://emanual.robotis.com/docs/en/platform/turtlebot3/manipulation/ https://github.com/ROBOTIS-GIT/turtlebot3_manipulation https://github.com/ROBOTIS-GIT/turtlebot3_manipulation/issues Hye-jong KIM + Sungho Woo ament_cmake diff --git a/turtlebot3_manipulation_teleop/src/turtlebot3_manipulation_teleop.cpp b/turtlebot3_manipulation_teleop/src/turtlebot3_manipulation_teleop.cpp index aba2693..2c814db 100644 --- a/turtlebot3_manipulation_teleop/src/turtlebot3_manipulation_teleop.cpp +++ b/turtlebot3_manipulation_teleop/src/turtlebot3_manipulation_teleop.cpp @@ -12,11 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. // -// Author: Hye-jong KIM +// Author: Hye-jong KIM, Sungho Woo #include #include - #include "turtlebot3_manipulation_teleop/turtlebot3_manipulation_teleop.hpp" // KeyboardReader @@ -50,7 +49,7 @@ void KeyboardReader::shutdown() // KeyboardServo KeyboardServo::KeyboardServo() -: publish_task_(false), publish_joint_(false) +: publish_joint_(false) { nh_ = rclcpp::Node::make_shared("servo_keyboard_input"); @@ -61,9 +60,8 @@ KeyboardServo::KeyboardServo() base_twist_pub_ = nh_->create_publisher(BASE_TWIST_TOPIC, ROS_QUEUE_SIZE); - arm_twist_pub_ = - nh_->create_publisher(ARM_TWIST_TOPIC, ROS_QUEUE_SIZE); joint_pub_ = nh_->create_publisher(ARM_JOINT_TOPIC, ROS_QUEUE_SIZE); + client_ = rclcpp_action::create_client(nh_, "gripper_controller/gripper_cmd"); cmd_vel_ = geometry_msgs::msg::Twist(); } @@ -84,9 +82,20 @@ int KeyboardServo::keyLoop() puts("Reading from keyboard"); puts("---------------------------"); - puts("Use o|k|l|; keys to move turtlebot base and use 'space' key to stop the base"); - puts("Use s|x|z|c|a|d|f|v keys to Cartesian jog"); - puts("Use 1|2|3|4|q|w|e|r keys to joint jog."); + puts("Joint Control Keys:"); + puts(" 1/q: Joint1 +/-"); + puts(" 2/w: Joint2 +/-"); + puts(" 3/e: Joint3 +/-"); + puts(" 4/r: Joint4 +/-"); + puts("Use o|p to open/close the gripper."); + puts(""); + puts("Command Control Keys:"); + puts(" i: Move up"); + puts(" k: Move down"); + puts(" l: Move right"); + puts(" j: Move left"); + puts(" space bar: Move stop"); + puts("---------------------------"); puts("'ESC' to quit."); std::thread{std::bind(&KeyboardServo::pub, this)}.detach(); @@ -104,29 +113,33 @@ int KeyboardServo::keyLoop() RCLCPP_INFO(nh_->get_logger(), "value: 0x%02X", c); // Use read key-press + joint_msg_.joint_names.clear(); + joint_msg_.velocities.clear(); + switch (c) { - case KEYCODE_O: // KEYCODE_UP: + // Command Control Keys + case KEYCODE_I: cmd_vel_.linear.x = std::min(cmd_vel_.linear.x + BASE_LINEAR_VEL_STEP, BASE_LINEAR_VEL_MAX); cmd_vel_.linear.y = 0.0; cmd_vel_.linear.z = 0.0; RCLCPP_INFO_STREAM(nh_->get_logger(), "LINEAR VEL : " << cmd_vel_.linear.x); break; - case KEYCODE_L: // KEYCODE_DOWN: + case KEYCODE_K: cmd_vel_.linear.x = std::max(cmd_vel_.linear.x - BASE_LINEAR_VEL_STEP, -BASE_LINEAR_VEL_MAX); cmd_vel_.linear.y = 0.0; cmd_vel_.linear.z = 0.0; RCLCPP_INFO_STREAM(nh_->get_logger(), "LINEAR VEL : " << cmd_vel_.linear.x); break; - case KEYCODE_K: // KEYCODE_LEFT: + case KEYCODE_J: cmd_vel_.angular.x = 0.0; cmd_vel_.angular.y = 0.0; cmd_vel_.angular.z = std::min(cmd_vel_.angular.z + BASE_ANGULAR_VEL_STEP, BASE_ANGULAR_VEL_MAX); RCLCPP_INFO_STREAM(nh_->get_logger(), "ANGULAR VEL : " << cmd_vel_.angular.z); break; - case KEYCODE_SEMICOLON: // KEYCODE_RIGHT: + case KEYCODE_L: cmd_vel_.angular.x = 0.0; cmd_vel_.angular.y = 0.0; cmd_vel_.angular.z = @@ -137,50 +150,8 @@ int KeyboardServo::keyLoop() cmd_vel_ = geometry_msgs::msg::Twist(); RCLCPP_INFO_STREAM(nh_->get_logger(), "STOP base"); break; - case KEYCODE_A: - task_msg_.twist.linear.z = ARM_TWIST_VEL; - publish_task_ = true; - RCLCPP_INFO_STREAM(nh_->get_logger(), "Arm z UP"); - break; - case KEYCODE_D: - task_msg_.twist.linear.z = -ARM_TWIST_VEL; - publish_task_ = true; - RCLCPP_INFO_STREAM(nh_->get_logger(), "Arm z DOWN"); - break; - case KEYCODE_S: - task_msg_.twist.linear.x = ARM_TWIST_VEL; - publish_task_ = true; - RCLCPP_INFO_STREAM(nh_->get_logger(), "Arm x Front"); - break; - case KEYCODE_X: - task_msg_.twist.linear.x = -ARM_TWIST_VEL; - publish_task_ = true; - RCLCPP_INFO_STREAM(nh_->get_logger(), "Arm x Back"); - break; - case KEYCODE_Z: - joint_msg_.joint_names.push_back("joint1"); - joint_msg_.velocities.push_back(ARM_JOINT_VEL); - publish_joint_ = true; - RCLCPP_INFO_STREAM(nh_->get_logger(), "Arm Turn left."); - break; - case KEYCODE_C: - joint_msg_.joint_names.push_back("joint1"); - joint_msg_.velocities.push_back(-ARM_JOINT_VEL); - publish_joint_ = true; - RCLCPP_INFO_STREAM(nh_->get_logger(), "Arm Turn right."); - break; - case KEYCODE_F: - joint_msg_.joint_names.push_back("joint4"); - joint_msg_.velocities.push_back(ARM_JOINT_VEL); - publish_joint_ = true; - RCLCPP_INFO_STREAM(nh_->get_logger(), "Gripper Down."); - break; - case KEYCODE_V: - joint_msg_.joint_names.push_back("joint4"); - joint_msg_.velocities.push_back(-ARM_JOINT_VEL); - publish_joint_ = true; - RCLCPP_INFO_STREAM(nh_->get_logger(), "Gripper Up."); - break; + + // Joint Control Keys case KEYCODE_1: joint_msg_.joint_names.push_back("joint1"); joint_msg_.velocities.push_back(ARM_JOINT_VEL); @@ -229,6 +200,14 @@ int KeyboardServo::keyLoop() publish_joint_ = true; RCLCPP_INFO_STREAM(nh_->get_logger(), "Joint4 -"); break; + case KEYCODE_O: + send_goal(0.025); + RCLCPP_INFO_STREAM(nh_->get_logger(), "Gripper Open"); + break; + case KEYCODE_P: + send_goal(-0.015); + RCLCPP_INFO_STREAM(nh_->get_logger(), "Gripper Close"); + break; case KEYCODE_ESC: RCLCPP_INFO_STREAM(nh_->get_logger(), "quit"); servoing = false; @@ -242,14 +221,27 @@ int KeyboardServo::keyLoop() return 0; } +void KeyboardServo::send_goal(float position) +{ + auto goal_msg = control_msgs::action::GripperCommand::Goal(); + goal_msg.command.position = position; // Set position + goal_msg.command.max_effort = -1.0; // Set max effort + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.result_callback = std::bind(&KeyboardServo::goal_result_callback, this, std::placeholders::_1); + + RCLCPP_INFO(nh_->get_logger(), "Sending goal"); + client_->async_send_goal(goal_msg, send_goal_options); +} + void KeyboardServo::connect_moveit_servo() { for (int i = 0; i < 10; i++) { if (servo_start_client_->wait_for_service(std::chrono::seconds(1))) { - RCLCPP_INFO_STREAM(nh_->get_logger(), "SUCCESS TO CONNNET SERVO START SERVER"); + RCLCPP_INFO_STREAM(nh_->get_logger(), "SUCCESS TO CONNECT SERVO START SERVER"); break; } - RCLCPP_WARN_STREAM(nh_->get_logger(), "WAIT TO CONNNET SERVO START SERVER"); + RCLCPP_WARN_STREAM(nh_->get_logger(), "WAIT TO CONNECT SERVO START SERVER"); if (i == 9) { RCLCPP_ERROR_STREAM( nh_->get_logger(), @@ -259,10 +251,10 @@ void KeyboardServo::connect_moveit_servo() } for (int i = 0; i < 10; i++) { if (servo_stop_client_->wait_for_service(std::chrono::seconds(1))) { - RCLCPP_INFO_STREAM(nh_->get_logger(), "SUCCESS TO CONNNET SERVO STOP SERVER"); + RCLCPP_INFO_STREAM(nh_->get_logger(), "SUCCESS TO CONNECT SERVO STOP SERVER"); break; } - RCLCPP_WARN_STREAM(nh_->get_logger(), "WAIT TO CONNNET SERVO STOP SERVER"); + RCLCPP_WARN_STREAM(nh_->get_logger(), "WAIT TO CONNECT SERVO STOP SERVER"); if (i == 9) { RCLCPP_ERROR_STREAM( nh_->get_logger(), @@ -283,7 +275,7 @@ void KeyboardServo::start_moveit_servo() future.get(); } else { RCLCPP_ERROR_STREAM( - nh_->get_logger(), "FAIL to start 'moveit_servo', excute without 'moveit_servo'"); + nh_->get_logger(), "FAIL to start 'moveit_servo', execute without 'moveit_servo'"); } } @@ -303,20 +295,13 @@ void KeyboardServo::pub() { while (rclcpp::ok()) { // If a key requiring a publish was pressed, publish the message now - if (publish_task_) { - task_msg_.header.stamp = nh_->now(); - task_msg_.header.frame_id = BASE_FRAME_ID; - arm_twist_pub_->publish(task_msg_); - publish_task_ = false; - RCLCPP_INFO_STREAM(nh_->get_logger(), "TASK PUB"); - } else if (publish_joint_) { + if (publish_joint_) { joint_msg_.header.stamp = nh_->now(); joint_msg_.header.frame_id = BASE_FRAME_ID; joint_pub_->publish(joint_msg_); publish_joint_ = false; RCLCPP_INFO_STREAM(nh_->get_logger(), "Joint PUB"); } - // Base pub base_twist_pub_->publish(cmd_vel_); rclcpp::sleep_for(std::chrono::milliseconds(10)); }