diff --git a/turtlebot3_manipulation/package.xml b/turtlebot3_manipulation/package.xml index 3ea7aed..ff8f8ff 100644 --- a/turtlebot3_manipulation/package.xml +++ b/turtlebot3_manipulation/package.xml @@ -6,7 +6,7 @@ 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_bringup/launch/__pycache__/base.launch.cpython-310.pyc b/turtlebot3_manipulation_bringup/launch/__pycache__/base.launch.cpython-310.pyc new file mode 100644 index 0000000..dcc9350 Binary files /dev/null and b/turtlebot3_manipulation_bringup/launch/__pycache__/base.launch.cpython-310.pyc differ diff --git a/turtlebot3_manipulation_bringup/launch/__pycache__/gazebo.launch.cpython-310.pyc b/turtlebot3_manipulation_bringup/launch/__pycache__/gazebo.launch.cpython-310.pyc new file mode 100644 index 0000000..2fe3e4f Binary files /dev/null and b/turtlebot3_manipulation_bringup/launch/__pycache__/gazebo.launch.cpython-310.pyc differ diff --git a/turtlebot3_manipulation_bringup/package.xml b/turtlebot3_manipulation_bringup/package.xml index f78dab9..f5f3c93 100644 --- a/turtlebot3_manipulation_bringup/package.xml +++ b/turtlebot3_manipulation_bringup/package.xml @@ -6,7 +6,7 @@ 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/package.xml b/turtlebot3_manipulation_cartographer/package.xml index 580f7a5..634841a 100644 --- a/turtlebot3_manipulation_cartographer/package.xml +++ b/turtlebot3_manipulation_cartographer/package.xml @@ -6,7 +6,7 @@ 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/package.xml b/turtlebot3_manipulation_description/package.xml index ffdfc4c..18257b4 100644 --- a/turtlebot3_manipulation_description/package.xml +++ b/turtlebot3_manipulation_description/package.xml @@ -6,7 +6,7 @@ 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 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_example/CMakeLists.txt b/turtlebot3_manipulation_example/CMakeLists.txt new file mode 100644 index 0000000..b38d1f9 --- /dev/null +++ b/turtlebot3_manipulation_example/CMakeLists.txt @@ -0,0 +1,39 @@ +cmake_minimum_required(VERSION 3.8) +project(turtlebot3_manipulation_example) + +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(moveit_ros_planning_interface REQUIRED) +find_package(rclcpp REQUIRED) + +add_executable(hello_moveit src/hello_moveit.cpp) +target_include_directories(hello_moveit PUBLIC + $ + $) +target_compile_features(hello_moveit PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +ament_target_dependencies( + hello_moveit + "moveit_ros_planning_interface" + "rclcpp" +) + +install(TARGETS hello_moveit + 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/turtlebot3_manipulation_example/package.xml b/turtlebot3_manipulation_example/package.xml new file mode 100644 index 0000000..1495c6c --- /dev/null +++ b/turtlebot3_manipulation_example/package.xml @@ -0,0 +1,21 @@ + + + + turtlebot3_manipulation_example + 0.0.0 + TODO: Package description + robotis + TODO: License declaration + + ament_cmake + + moveit_ros_planning_interface + rclcpp + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/turtlebot3_manipulation_example/src/hello_moveit.cpp b/turtlebot3_manipulation_example/src/hello_moveit.cpp new file mode 100644 index 0000000..f25e109 --- /dev/null +++ b/turtlebot3_manipulation_example/src/hello_moveit.cpp @@ -0,0 +1,54 @@ +// Modify the MoveIt tutorial as much as you like using the basic example. +// Author: Sungho Woo + +#include + +#include +#include + +int main(int argc, char * argv[]) +{ + // Initialize ROS and create the Node + rclcpp::init(argc, argv); + auto const node = std::make_shared( + "hello_moveit", + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true) + ); + + // Create a ROS logger + auto const logger = rclcpp::get_logger("hello_moveit"); + +// Create the MoveIt MoveGroup Interface +using moveit::planning_interface::MoveGroupInterface; +auto move_group_interface = MoveGroupInterface(node, "arm"); + +// Set a target Pose + auto const target_pose = []{ + geometry_msgs::msg::Pose msg; + msg.orientation.w = 1.0; + msg.position.x = 0.2; + msg.position.y = 0.0; + msg.position.z = 0.2; + return msg; + }(); + move_group_interface.setPoseTarget(target_pose); + move_group_interface.setGoalJointTolerance(0.01); + move_group_interface.setGoalOrientationTolerance(0.01); + + // Create a plan to that target pose + auto const [success, plan] = [&move_group_interface]{ + moveit::planning_interface::MoveGroupInterface::Plan msg; + auto const ok = static_cast(move_group_interface.plan(msg)); + return std::make_pair(ok, msg); + }(); + + // Execute the plan + if(success) { + move_group_interface.execute(plan); + } else { + RCLCPP_ERROR(logger, "Planing failed!"); + } + // Shutdown ROS + rclcpp::shutdown(); + return 0; +} diff --git a/turtlebot3_manipulation_hardware/CHANGELOG.rst b/turtlebot3_manipulation_hardware/CHANGELOG.rst index 8f5975a..e7b11b0 100644 --- a/turtlebot3_manipulation_hardware/CHANGELOG.rst +++ b/turtlebot3_manipulation_hardware/CHANGELOG.rst @@ -8,4 +8,4 @@ Changelog for package turtlebot3_manipulation_hardware * 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..56ed06b 100644 --- a/turtlebot3_manipulation_hardware/package.xml +++ b/turtlebot3_manipulation_hardware/package.xml @@ -6,7 +6,7 @@ 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 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..68393f1 100644 --- a/turtlebot3_manipulation_moveit_config/CHANGELOG.rst +++ b/turtlebot3_manipulation_moveit_config/CHANGELOG.rst @@ -8,4 +8,4 @@ Changelog for package turtlebot3_manipulation_moveit_config * 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_servo.yaml b/turtlebot3_manipulation_moveit_config/config/moveit_servo.yaml index 3e34f09..0aca033 100644 --- a/turtlebot3_manipulation_moveit_config/config/moveit_servo.yaml +++ b/turtlebot3_manipulation_moveit_config/config/moveit_servo.yaml @@ -66,4 +66,4 @@ self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collis scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m] # Parameters for "stop_distance"-type collision checking collision_distance_safety_factor: 1000.0 # Must be >= 1. A large safety factor is recommended to account for latency -min_allowable_collision_distance: 0.01 # Stop if a collision is closer than this [m] +min_allowable_collision_distance: 0.01 # Stop if a collision is closer than this [m] \ No newline at end of file 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..bdaafb3 100644 --- a/turtlebot3_manipulation_moveit_config/package.xml +++ b/turtlebot3_manipulation_moveit_config/package.xml @@ -6,7 +6,7 @@ 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 diff --git a/turtlebot3_manipulation_navigation2/package.xml b/turtlebot3_manipulation_navigation2/package.xml index 1bf2615..e40d29b 100644 --- a/turtlebot3_manipulation_navigation2/package.xml +++ b/turtlebot3_manipulation_navigation2/package.xml @@ -6,7 +6,7 @@ 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..caf828f 100644 --- a/turtlebot3_manipulation_teleop/CHANGELOG.rst +++ b/turtlebot3_manipulation_teleop/CHANGELOG.rst @@ -8,4 +8,4 @@ Changelog for package turtlebot3_manipulation_teleop * 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..79630a9 100644 --- a/turtlebot3_manipulation_teleop/package.xml +++ b/turtlebot3_manipulation_teleop/package.xml @@ -4,7 +4,7 @@ turtlebot3_manipulation_teleop 2.1.1 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 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)); }