diff --git a/ReadMe.md b/ReadMe.md index 409b440f..fc426111 100755 --- a/ReadMe.md +++ b/ReadMe.md @@ -436,6 +436,33 @@ __Reminder 4: The described below is replaced with the actual one, the x ros2 launch xarm_moveit_servo uf850_moveit_servo_realmove.launch.py robot_ip:=192.168.1.181 joystick_type:=1 ``` + - Controlling with __PS4__ joystick: + - left stick for X and Y direction. + - right stick for ROLL and PITCH adjustment. + - left and right trigger (LT/RT) for Z direction. + - left and right bumper (LB/RB) for YAW adjustment. + - D-PAD for controlling joint1 and joint2. + - buttons SQUARE and CIRCLE for controlling last joint. + - buttons TRIANGLE and CROSS for controlling second last joint. + + ```bash + cd ~/dev_ws/ + # For controlling simulated xArm: + ros2 launch xarm_moveit_servo xarm_moveit_servo_fake.launch.py joystick_type:=4 + # Or controlling simulated Lite6: + ros2 launch xarm_moveit_servo lite6_moveit_servo_fake.launch.py joystick_type:=4 + # Or controlling simulated UFACTORY850: + ros2 launch xarm_moveit_servo uf850_moveit_servo_fake.launch.py joystick_type:=4 + + + # For controlling real xArm: (use xArm 5 as example) + ros2 launch xarm_moveit_servo xarm_moveit_servo_realmove.launch.py robot_ip:=192.168.1.123 dof:=5 joystick_type:=4 + # Or controlling real Lite6: + ros2 launch xarm_moveit_servo lite6_moveit_servo_realmove.launch.py robot_ip:=192.168.1.123 joystick_type:=4 + # Or controlling real UFACTORY850: + ros2 launch xarm_moveit_servo uf850_moveit_servo_realmove.launch.py robot_ip:=192.168.1.181 joystick_type:=4 + ``` + - Controlling with __3Dconnexion SpaceMouse Wireless__: - 6 DOFs of the mouse are mapped for controlling X/Y/Z/ROLL/PITCH/YAW - Left button clicked for just X/Y/Z adjustment diff --git a/xarm_moveit_servo/include/xarm_moveit_servo/xarm_joystick_input.h b/xarm_moveit_servo/include/xarm_moveit_servo/xarm_joystick_input.h index 46bc6df4..2dc11fb2 100644 --- a/xarm_moveit_servo/include/xarm_moveit_servo/xarm_joystick_input.h +++ b/xarm_moveit_servo/include/xarm_moveit_servo/xarm_joystick_input.h @@ -30,6 +30,10 @@ class JoyToServoPub : public rclcpp::Node const std::vector& axes, const std::vector& buttons, std::unique_ptr& twist, std::unique_ptr& joint); + bool _convert_ps4_joy_to_cmd( + const std::vector& axes, const std::vector& buttons, + std::unique_ptr& twist, + std::unique_ptr& joint); bool _convert_spacemouse_wireless_joy_to_cmd(const std::vector& axes, const std::vector& buttons, std::unique_ptr& twist); diff --git a/xarm_moveit_servo/src/xarm_joystick_input.cpp b/xarm_moveit_servo/src/xarm_joystick_input.cpp index e9bb162f..cef7ad70 100644 --- a/xarm_moveit_servo/src/xarm_joystick_input.cpp +++ b/xarm_moveit_servo/src/xarm_joystick_input.cpp @@ -15,7 +15,8 @@ enum JOYSTICK_TYPE { JOYSTICK_XBOX360_WIRED = 1, JOYSTICK_XBOX360_WIRELESS = 2, - JOYSTICK_SPACEMOUSE_WIRELESS = 3 + JOYSTICK_SPACEMOUSE_WIRELESS = 3, + JOYSTICK_PS4 = 4 }; enum XBOX360_WIRED_CONTROLLER_AXIS @@ -42,6 +43,18 @@ enum XBOX360_WIRELESS_CONTROLLER_AXIS XBOX360_WIRELESS_CROSS_KEY_FB = 7 }; +enum PS4_CONTROLLER_AXIS +{ + PS4_LEFT_STICK_LR = 0, + PS4_LEFT_STICK_FB = 1, + PS4_LEFT_TRIGGER = 2, + PS4_RIGHT_STICK_LR = 3, + PS4_RIGHT_STICK_FB = 4, + PS4_RIGHT_TRIGGER = 5, + PS4_CROSS_KEY_LR = 6, + PS4_CROSS_KEY_FB = 7 +}; + enum XBOX360_CONTROLLER_BUTTON { XBOX360_BTN_A = 0, @@ -57,6 +70,21 @@ enum XBOX360_CONTROLLER_BUTTON XBOX360_BTN_STICK_RIGHT = 10 }; +enum PS4_CONTROLLER_BUTTON +{ + PS4_BTN_A = 0, // Cross + PS4_BTN_B = 1, // Cicrle + PS4_BTN_Y = 2, // Triangle + PS4_BTN_X = 3, // Square + PS4_BTN_LB = 4, + PS4_BTN_RB = 5, + PS4_BTN_BACK = 8, // Share + PS4_BTN_START = 9, // Options + PS4_BTN_POWER = 10, // Home + PS4_BTN_STICK_LEFT = 11, + PS4_BTN_STICK_RIGHT = 12 +}; + enum SPACEMOUSE_WIRELESS_AXIS { SPM_STICK_Y = 0, @@ -223,6 +251,57 @@ bool JoyToServoPub::_convert_xbox360_joy_to_cmd( return true; } +bool JoyToServoPub::_convert_ps4_joy_to_cmd( + const std::vector& axes, const std::vector& buttons, + std::unique_ptr& twist, + std::unique_ptr& joint) +{ + // PS4 axis + int left_stick_lr = PS4_LEFT_STICK_LR; + int left_stick_fb = PS4_LEFT_STICK_FB; + int right_stick_lr = PS4_RIGHT_STICK_LR; + int right_stick_fb = PS4_RIGHT_STICK_FB; + int left_trigger = PS4_LEFT_TRIGGER; + int right_trigger = PS4_RIGHT_TRIGGER; + int cross_key_lr = PS4_CROSS_KEY_LR; + int cross_key_fb = PS4_CROSS_KEY_FB; + + if (buttons[PS4_BTN_BACK] && planning_frame_ == ee_frame_name_) { + planning_frame_ = robot_link_command_frame_; + } + else if (buttons[PS4_BTN_START] && planning_frame_ == robot_link_command_frame_) { + planning_frame_ = ee_frame_name_; + } + + if (buttons[PS4_BTN_A] || buttons[PS4_BTN_B] + || buttons[PS4_BTN_X] || buttons[PS4_BTN_Y] + || axes[cross_key_lr] || axes[cross_key_fb]) + { + // Map the D_PAD to the proximal joints + joint->joint_names.push_back("joint1"); + joint->velocities.push_back(axes[cross_key_lr] * 1); + joint->joint_names.push_back("joint2"); + joint->velocities.push_back(axes[cross_key_fb] * 1); + + // Map the diamond to the distal joints + joint->joint_names.push_back("joint" + std::to_string(dof_)); + joint->velocities.push_back((buttons[PS4_BTN_B] - buttons[PS4_BTN_X]) * 1); + joint->joint_names.push_back("joint" + std::to_string(dof_ - 1)); + joint->velocities.push_back((buttons[PS4_BTN_Y] - buttons[PS4_BTN_A]) * 1); + return false; + } + + // The bread and butter: map buttons to twist commands + twist->twist.linear.x = axes[left_stick_fb]; + twist->twist.linear.y = axes[left_stick_lr]; + twist->twist.linear.z = -1 * (axes[left_trigger] - axes[right_trigger]); + twist->twist.angular.y = axes[right_stick_fb]; + twist->twist.angular.x = axes[right_stick_lr]; + twist->twist.angular.z = buttons[PS4_BTN_LB] - buttons[PS4_BTN_RB]; + + return true; +} + bool JoyToServoPub::_convert_spacemouse_wireless_joy_to_cmd(const std::vector& axes, const std::vector& buttons, std::unique_ptr& twist) { @@ -288,6 +367,11 @@ void JoyToServoPub::_joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg) return; pub_twist = _convert_spacemouse_wireless_joy_to_cmd(msg->axes, msg->buttons, twist_msg); break; + case JOYSTICK_PS4: // PS4 controller + if (msg->axes.size() != 8 || msg->buttons.size() != 13) + return; + pub_twist = _convert_ps4_joy_to_cmd(msg->axes, msg->buttons, twist_msg, joint_msg); + break; default: return; }