From 082616eb7e955595deb40cce16b0f54f62d477f1 Mon Sep 17 00:00:00 2001 From: Niko Date: Wed, 3 Apr 2024 16:50:26 +0200 Subject: [PATCH] added ps5_control.py --- README.md | 2 +- docker-compose.yaml | 4 +- .../config/ur_robotiq_controllers.yaml | 31 ++++ .../launch/ur_robotiq_control.launch.py | 17 +- .../config/moveit_controllers.yaml | 14 ++ .../config/ur_servo.yaml | 2 +- .../launch/ur_robotiq_moveit.launch.py | 2 - ....xacro => old_ur_robotiq_macro.srdf.xacro} | 16 +- .../srdf/ur_manipulator_macro.srdf.xacro | 172 ++++++++++++++++++ src/ur_robotiq_servo/config/joy-params.yaml | 8 + .../launch/ps5_servo.launch.py | 31 ++++ src/ur_robotiq_servo/package.xml | 1 + src/ur_robotiq_servo/setup.py | 5 + .../ur_robotiq_servo/ps5_control.py | 99 ++++++++++ 14 files changed, 391 insertions(+), 13 deletions(-) rename src/ur_robotiq_moveit_config/srdf/{ur_robotiq_macro.srdf.xacro => old_ur_robotiq_macro.srdf.xacro} (88%) create mode 100644 src/ur_robotiq_moveit_config/srdf/ur_manipulator_macro.srdf.xacro create mode 100644 src/ur_robotiq_servo/config/joy-params.yaml create mode 100644 src/ur_robotiq_servo/launch/ps5_servo.launch.py diff --git a/README.md b/README.md index d2f87a2..61d3799 100644 --- a/README.md +++ b/README.md @@ -64,7 +64,7 @@ ros2 launch ur_robotiq_moveit_config ur_robotiq_moveit.launch.py use_fake_hardwa ### use_fake_hardware:=true Terminal 1.: ```bash -ros2 launch ur_robotiq_description ur_robotiq_control.launch.py robot_ip:=aaa.bbb.ccc.ddd use_fake_hardware:=true launch_rviz:=false +ros2 launch ur_robotiq_description ur_robotiq_control.launch.py robot_ip:=aaa.bbb.ccc.ddd use_fake_hardware:=true launch_rviz:=false initial_joint_controller:=joint_trajectory_controller ``` Terminal 2.: ```bash diff --git a/docker-compose.yaml b/docker-compose.yaml index 0292553..c85f16f 100644 --- a/docker-compose.yaml +++ b/docker-compose.yaml @@ -2,6 +2,7 @@ version: '3.8' services: ros2: build: . + privileged: true volumes: - ./src:/ros2_ws/src - /tmp/.X11-unix:/tmp/.X11-unix @@ -13,7 +14,8 @@ services: - DISPLAY=$DISPLAY devices: - /dev/dri:/dev/dri - - /dev/input:/dev/input + - /dev/input/js0:/dev/input/js0 + - /dev/input/js1:/dev/input/js1 networks: ros_network: diff --git a/src/ur_robotiq_description/config/ur_robotiq_controllers.yaml b/src/ur_robotiq_description/config/ur_robotiq_controllers.yaml index 9a50b20..6ecfce1 100644 --- a/src/ur_robotiq_description/config/ur_robotiq_controllers.yaml +++ b/src/ur_robotiq_description/config/ur_robotiq_controllers.yaml @@ -33,6 +33,9 @@ controller_manager: robotiq_activation_controller: type: robotiq_controllers/RobotiqActivationController + combined_joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + speed_scaling_state_broadcaster: ros__parameters: @@ -162,3 +165,31 @@ robotiq_activation_controller: ros__parameters: default: true +combined_trajectory_controller: + ros__parameters: + joints: + - $(var tf_prefix)shoulder_pan_joint + - $(var tf_prefix)shoulder_lift_joint + - $(var tf_prefix)elbow_joint + - $(var tf_prefix)wrist_1_joint + - $(var tf_prefix)wrist_2_joint + - $(var tf_prefix)wrist_3_joint + - $(var tf_prefix)finger_joint + command_interfaces: + - position + state_interfaces: + - position + - velocity + state_publish_rate: 100.0 + action_monitor_rate: 20.0 + allow_partial_joints_goal: false + constraints: + stopped_velocity_tolerance: 0.2 + goal_time: 0.0 + $(var tf_prefix)shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)elbow_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)wrist_1_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)wrist_2_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)wrist_3_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)finger_joint: { trajectory: 0.2, goal: 0.1 } \ 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 b91cee9..c391a22 100644 --- a/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py +++ b/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py @@ -25,6 +25,7 @@ def launch_setup(context, *args, **kwargs): use_fake_hardware = LaunchConfiguration("use_fake_hardware") fake_sensor_commands = LaunchConfiguration("fake_sensor_commands") controller_spawner_timeout = LaunchConfiguration("controller_spawner_timeout") + initial_joint_controller = LaunchConfiguration("initial_joint_controller") activate_joint_controller = LaunchConfiguration("activate_joint_controller") launch_rviz = LaunchConfiguration("launch_rviz") headless_mode = LaunchConfiguration("headless_mode") @@ -169,15 +170,11 @@ def launch_setup(context, *args, **kwargs): robot_description = {"robot_description": robot_description_content} - if use_fake_hardware.perform(context): - initial_joint_controller = "joint_trajectory_controller" - else: - initial_joint_controller = "scaled_joint_trajectory_controller" - initial_joint_controllers = PathJoinSubstitution( [FindPackageShare(description_package), "config", controllers_file] ) + rviz_config_file = PathJoinSubstitution( [FindPackageShare(description_package), "rviz", "view_robot.rviz"] ) @@ -459,7 +456,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "use_fake_hardware", - default_value="true", + default_value="false", description="Start robot with fake hardware mirroring command to its states.", ) ) @@ -485,7 +482,13 @@ def generate_launch_description(): description="Timeout used when spawning controllers.", ) ) - + declared_arguments.append( + DeclareLaunchArgument( + "initial_joint_controller", + default_value="scaled_joint_trajectory_controller", + description="Initially loaded robot controller.", + ) + ) declared_arguments.append( DeclareLaunchArgument( "activate_joint_controller", diff --git a/src/ur_robotiq_moveit_config/config/moveit_controllers.yaml b/src/ur_robotiq_moveit_config/config/moveit_controllers.yaml index 158f762..accd281 100644 --- a/src/ur_robotiq_moveit_config/config/moveit_controllers.yaml +++ b/src/ur_robotiq_moveit_config/config/moveit_controllers.yaml @@ -4,6 +4,7 @@ controller_names: - joint_trajectory_controller - robotiq_gripper_controller - robotiq_gripper_joint_trajectory_controller + - combined_joint_trajectory_controller scaled_joint_trajectory_controller: @@ -45,3 +46,16 @@ robotiq_gripper_joint_trajectory_controller: joints: - finger_joint +combined_joint_trajectory_controller: + type: FollowJointTrajectory + action_ns: follow_joint_trajectory + default: true + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + - finger_joint + diff --git a/src/ur_robotiq_moveit_config/config/ur_servo.yaml b/src/ur_robotiq_moveit_config/config/ur_servo.yaml index 9802702..b4362dc 100644 --- a/src/ur_robotiq_moveit_config/config/ur_servo.yaml +++ b/src/ur_robotiq_moveit_config/config/ur_servo.yaml @@ -2,7 +2,7 @@ # Modify all parameters related to servoing here ############################################### use_gazebo: false # Whether the robot is started in a Gazebo simulation environment - +use_intra_process_comms : true ## Properties of incoming commands command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s scale: diff --git a/src/ur_robotiq_moveit_config/launch/ur_robotiq_moveit.launch.py b/src/ur_robotiq_moveit_config/launch/ur_robotiq_moveit.launch.py index 61d39db..9018cf2 100644 --- a/src/ur_robotiq_moveit_config/launch/ur_robotiq_moveit.launch.py +++ b/src/ur_robotiq_moveit_config/launch/ur_robotiq_moveit.launch.py @@ -148,10 +148,8 @@ def launch_setup(context, *args, **kwargs): # the scaled_joint_trajectory_controller does not work on fake hardware use_fake_hw = use_fake_hardware.perform(context) controllers_yaml = load_yaml(moveit_config_package, "config/moveit_controllers.yaml") - print(controllers_yaml) if use_fake_hw == "true": - print("Using fake hardware") controllers_yaml["scaled_joint_trajectory_controller"]["default"] = False controllers_yaml["joint_trajectory_controller"]["default"] = True controllers_yaml["robotiq_gripper_controller"]["default"] = False diff --git a/src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro b/src/ur_robotiq_moveit_config/srdf/old_ur_robotiq_macro.srdf.xacro similarity index 88% rename from src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro rename to src/ur_robotiq_moveit_config/srdf/old_ur_robotiq_macro.srdf.xacro index 0f20db3..034a71d 100644 --- a/src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro +++ b/src/ur_robotiq_moveit_config/srdf/old_ur_robotiq_macro.srdf.xacro @@ -64,6 +64,7 @@ + @@ -102,6 +103,19 @@ + + + + + + + + + + + + + @@ -117,4 +131,4 @@ - \ No newline at end of file + diff --git a/src/ur_robotiq_moveit_config/srdf/ur_manipulator_macro.srdf.xacro b/src/ur_robotiq_moveit_config/srdf/ur_manipulator_macro.srdf.xacro new file mode 100644 index 0000000..92985df --- /dev/null +++ b/src/ur_robotiq_moveit_config/srdf/ur_manipulator_macro.srdf.xacro @@ -0,0 +1,172 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/ur_robotiq_servo/config/joy-params.yaml b/src/ur_robotiq_servo/config/joy-params.yaml new file mode 100644 index 0000000..8ad2d13 --- /dev/null +++ b/src/ur_robotiq_servo/config/joy-params.yaml @@ -0,0 +1,8 @@ +joy_node: + ros__parameters: + device_id: 0 + device_name: "PS5 Controller" + deadzone: 0.5 + autorepeat_rate: 20.0 + sticky_buttons: false + coalesce_interval_ms: 1 \ No newline at end of file diff --git a/src/ur_robotiq_servo/launch/ps5_servo.launch.py b/src/ur_robotiq_servo/launch/ps5_servo.launch.py new file mode 100644 index 0000000..4ece31f --- /dev/null +++ b/src/ur_robotiq_servo/launch/ps5_servo.launch.py @@ -0,0 +1,31 @@ +import os + +from launch import LaunchDescription +from launch_ros.actions import Node +import ament_index_python.packages + +def generate_launch_description(): + config_directory = os.path.join( + ament_index_python.packages.get_package_share_directory('ur_robotiq_servo'), + 'config') + params = os.path.join(config_directory, 'joy-params.yaml') + return LaunchDescription([ + Node( + package='joy', + executable='joy_node', + name='joy_node', + output='screen', + parameters=[{ + params, + }], + ), + Node( + package='ur_robotiq_servo', + executable='ps5_servo', + name='ps5_servo_node', + output='screen', + parameters=[{ + }], + arguments=[], + ), + ]) \ No newline at end of file diff --git a/src/ur_robotiq_servo/package.xml b/src/ur_robotiq_servo/package.xml index f936ee1..3099325 100644 --- a/src/ur_robotiq_servo/package.xml +++ b/src/ur_robotiq_servo/package.xml @@ -11,6 +11,7 @@ sensor_msgs geometry_msgs std_msgs + std_srvs joy moveit_servo diff --git a/src/ur_robotiq_servo/setup.py b/src/ur_robotiq_servo/setup.py index 1114fd8..d91e499 100644 --- a/src/ur_robotiq_servo/setup.py +++ b/src/ur_robotiq_servo/setup.py @@ -1,4 +1,6 @@ from setuptools import find_packages, setup +import os +from glob import glob package_name = 'ur_robotiq_servo' @@ -10,6 +12,8 @@ setup( ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), ], install_requires=['setuptools'], zip_safe=True, @@ -20,6 +24,7 @@ setup( tests_require=['pytest'], entry_points={ 'console_scripts': [ + 'ps5_servo = ur_robotiq_servo.ps5_control:main' ], }, ) diff --git a/src/ur_robotiq_servo/ur_robotiq_servo/ps5_control.py b/src/ur_robotiq_servo/ur_robotiq_servo/ps5_control.py index e69de29..a19c3e7 100644 --- a/src/ur_robotiq_servo/ur_robotiq_servo/ps5_control.py +++ b/src/ur_robotiq_servo/ur_robotiq_servo/ps5_control.py @@ -0,0 +1,99 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Joy +from geometry_msgs.msg import TwistStamped +from control_msgs.msg import JointJog +from std_srvs.srv import Trigger + + +class PS5ControllerNode(Node): + def __init__(self): + super().__init__('ps5_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 + + # Subscriber and Publisher + self.joy_sub = self.create_subscription( + Joy, + '/joy', + self.joy_callback, + 1) + self.twist_pub = self.create_publisher( + TwistStamped, + '/servo_node/delta_twist_cmds', + 1) + self.joint_pub = self.create_publisher( + JointJog, + '/servo_node/delta_joint_cmds', + 1) + + # 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('ps5_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 joy_callback(self, msg): + # Check for button press to toggle mode + # Assuming button 2 (e.g., Triangle on PS5) for toggling mode + current_button_state = msg.buttons[16] + if current_button_state == 1 and self.last_button_state == 0: + self.mode = 'joint' if self.mode == 'twist' else 'twist' + self.get_logger().info(f'Mode switched to: {self.mode}') + self.last_button_state = current_button_state + + left_trigger = (msg.axes[2] - 1) / - 2.0 + right_trigger = (msg.axes[5] - 1) / - 2.0 + + # Process control input based on current mode + if self.mode == 'twist': + twist_msg = TwistStamped() + twist_msg.header.frame_id = 'tool0' + twist_msg.header.stamp = self.get_clock().now().to_msg() + twist_msg.twist.linear.x = msg.axes[0] + twist_msg.twist.linear.y = msg.axes[1] + twist_msg.twist.linear.z = left_trigger - right_trigger + self.twist_pub.publish(twist_msg) + elif self.mode == 'joint': + joint_msg = JointJog() + joint_msg.header.frame_id = 'tool0' + joint_msg.header.stamp = self.get_clock().now().to_msg() + joint_msg.joint_names = ["shoulder_pan_joint", + "shoulder_lift_joint", + "elbow_joint", + "wrist_1_joint", + "wrist_2_joint", + "wrist_3_joint"] + joint_msg.velocities = [msg.axes[0], + msg.axes[1], + msg.axes[2], + msg.axes[3], + (msg.buttons[11] - msg.buttons[12]) * 1.0, + (msg.buttons[13] - msg.buttons[14]) * 1.0] + self.joint_pub.publish(joint_msg) + +def main(args=None): + rclpy.init(args=args) + node = PS5ControllerNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file