diff --git a/README.md b/README.md index 425f4d8..17ea74a 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,7 @@ ROS2 Hardware Interface and Description for the ADA Robot ## Dependencies 1. Install the Kinova SDK for your robot by searching [here](https://www.kinovarobotics.com/resources?r=79301&s). PRL currently uses the [Gen2 SDK v1.5.1](https://drive.google.com/file/d/1UEQAow0XLcVcPCeQfHK9ERBihOCclkJ9/view). Note that although the latest version of that SDK is for Ubuntu 16.04, it still works on Ubuntu 22.04 (only for x86 systems, not ARM system). -2. Install required ROS binaries. Note that some or all of these may already be installed. `sudo apt install ros-humble-ros2-control ros-humble-ros2-controllers ros-humble-kinematics-interface-kdl ros-humble-ament-cmake-clang-format ros-humble-rviz2 ros-humble-moveit ros-humble-moveit-ros-perception ros-humble-ackermann-msgs ros-humble-control-toolbox ros-humble-generate-parameter-library ros-humble-generate-parameter-library-py` +2. Install required ROS binaries. Note that some or all of these may already be installed. `sudo apt install ros-humble-ros2-control ros-humble-ros2-controllers ros-humble-kinematics-interface-kdl ros-humble-ament-cmake-clang-format ros-humble-rviz2 ros-humble-moveit ros-humble-moveit-ros-perception ros-humble-ackermann-msgs ros-humble-control-toolbox ros-humble-generate-parameter-library ros-humble-generate-parameter-library-py ros-humble-moveit-ros-control-interface` 3. Install required python packages: `python3 -m pip install trimesh` 4. Configure and build your workspace: 1. Git clone [this repo (ada_ros2)](https://github.com/personalrobotics/ada_ros2) and [`pr_ros_controllers` (branch: `main`)](https://github.com/personalrobotics/pr_ros_controllers) into your ROS2 workspace's `src` folder. @@ -17,6 +17,31 @@ ROS2 Hardware Interface and Description for the ADA Robot ### Real 1. Run `ros2 launch ada_moveit demo.launch.py` command from your ROS2 workspace. If running for feeding specifically (i.e., where the watchdog dying kills the controllers) run `ros2 launch ada_moveit demo_feeding.launch.py`. Make sure the watchdog is running before you launch this node. +### MoveIt Servo + +MoveIt Servo allows [real-time arm servoing](https://moveit.picknik.ai/humble/doc/examples/realtime_servo/realtime_servo_tutorial.html) in cartesian space by sending twist commands to the end effector. + +To use Servo with keyboard teleop: +1. Launch the force-torque sensor: + 1. Sim: `ros2 run ada_feeding dummy_ft_sensor.py` + 2. Real: `ros2 run forque_sensor_hardware forque_sensor_hardware` +2. Launch MoveIt: + 1. Sim:`ros2 launch ada_moveit demo.launch.py sim:=mock` + 2. Real: `ros2 launch ada_moveit demo.launch.py` +3. Re-tare the F/T sensor: `ros2 service call /wireless_ft/set_bias std_srvs/srv/SetBool "{data: true}"` +4. Enable MoveIt Servo: + 1. Switch Controllers: `ros2 service call /controller_manager/switch_controller controller_manager_msgs/srv/SwitchController "{activate_controllers: [\"jaco_arm_servo_controller\"], deactivate_controllers: [\"jaco_arm_controller\"], start_controllers: [], stop_controllers: [], strictness: 0, start_asap: false, activate_asap: false, timeout: {sec: 0, nanosec: 0}}"` + 2. Toggle Servo On: `ros2 service call /servo_node/start_servo std_srvs/srv/Trigger "{}"` +5. Run the keyboard teleop script: `ros2 run ada_moveit ada_keyboard_teleop.py` +6. Follow the on-screen instructions to teleoperate the robot. +7. Toggle Servo Off: `ros2 service call /servo_node/stop_servo std_srvs/srv/Trigger "{}"` + +To create your own Servo client: +1. Follow steps 1-4 above. +2. Have your client publish Twist commands to `/servo_node/delta_twist_cmds`. Note the following: + 1. For reliable cartesian control when sending angular velocities on the real robot and `lovelace`, ensure the angular velocity is <= 0.3 rad/s in magnitude. Greater angular velocities might change the end effector's position in addition to its orientation. We believe this is because of latencies with MoveIt Servo getting the robot's joint states via the joint state publisher. + 2. Be sure to send 0-velocity Twist messages at the end to stop the robot. + ## Camera Calibration See the [README for the `default` calibration](./ada_moveit/calib/default/README.md) for details about our extrinsics calibration methdology. diff --git a/ada_moveit/CMakeLists.txt b/ada_moveit/CMakeLists.txt index f3f87e9..85d3a21 100644 --- a/ada_moveit/CMakeLists.txt +++ b/ada_moveit/CMakeLists.txt @@ -2,11 +2,20 @@ cmake_minimum_required(VERSION 3.22) project(ada_moveit) find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) -ament_package() +ament_python_install_package(${PROJECT_NAME}) + +# Install Python executables +install(PROGRAMS + scripts/ada_keyboard_teleop.py + DESTINATION lib/${PROJECT_NAME} +) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME} PATTERN "setup_assistant.launch" EXCLUDE) install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) install(DIRECTORY calib DESTINATION share/${PROJECT_NAME}) install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/ada_moveit/ada_moveit/__init__.py b/ada_moveit/ada_moveit/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/ada_moveit/config/mock_controllers.yaml b/ada_moveit/config/mock_controllers.yaml index 48e2a9a..ad757ac 100644 --- a/ada_moveit/config/mock_controllers.yaml +++ b/ada_moveit/config/mock_controllers.yaml @@ -7,6 +7,10 @@ controller_manager: type: force_gate_controller/ForceGateController + jaco_arm_servo_controller: + type: force_gate_controller/ForceGatePositionController + + hand_controller: type: joint_trajectory_controller/JointTrajectoryController @@ -50,6 +54,16 @@ jaco_arm_controller: goal: 0.02 trajectory: 0.05 +jaco_arm_servo_controller: + ros__parameters: + joints: + - j2n6s200_joint_1 + - j2n6s200_joint_2 + - j2n6s200_joint_3 + - j2n6s200_joint_4 + - j2n6s200_joint_5 + - j2n6s200_joint_6 + hand_controller: ros__parameters: joints: diff --git a/ada_moveit/config/mock_servo.yaml b/ada_moveit/config/mock_servo.yaml new file mode 100644 index 0000000..e988419 --- /dev/null +++ b/ada_moveit/config/mock_servo.yaml @@ -0,0 +1,75 @@ +# Adapted from https://github.com/ros-planning/moveit2/blob/humble/moveit_ros/moveit_servo/config/panda_simulated_config.yaml +############################################### +# Modify all parameters related to servoing here +############################################### +servo_node: + ros__parameters: + moveit_servo: + use_gazebo: false # Whether the robot is started in a Gazebo simulation environment + + ## 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: + # Scale parameters are only used if command_in_type=="unitless" + linear: 0.1 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. + rotational: 0.3 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands. + # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic. + joint: 0.5 + + # Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling) + # override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0] + + ## Properties of outgoing commands + publish_period: 0.034 # 1/Nominal publish rate [seconds] + low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) + + # What type of topic does your robot driver expect? + # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory + command_out_type: std_msgs/Float64MultiArray + + # What to publish? Can save some bandwidth as most robots only require positions or velocities + publish_joint_positions: true + publish_joint_velocities: false + publish_joint_accelerations: false + + ## Plugins for smoothing outgoing commands + smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" + + # If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service, + # which other nodes can use as a source for information about the planning environment. + # NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node), + # then is_primary_planning_scene_monitor needs to be set to false. + is_primary_planning_scene_monitor: false + + ## MoveIt properties + move_group_name: jaco_arm # Often 'manipulator' or 'arm' + planning_frame: j2n6s200_link_base # The MoveIt planning frame. Often 'base_link' or 'world' + + ## Other frames + ee_frame_name: forkTip # The name of the end effector link, used to return the EE pose + robot_link_command_frame: j2n6s200_link_base # commands must be given in the frame of a robot link. Usually either the base or end effector + + ## Stopping behaviour + incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command + # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. + # Important because ROS may drop some messages and we need the robot to halt reliably. + num_outgoing_halt_msgs_to_publish: 0 + + ## Configure handling of singularities and joint limits + lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity) + hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this + joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. + leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/ros-planning/moveit2/pull/620) + + ## Topic names + cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands + joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands + joint_topic: /joint_states + status_topic: ~/status # Publish status to this topic + command_out_topic: /jaco_arm_servo_controller/commands # Publish outgoing commands here + + ## Collision checking for the entire robot body + check_collisions: true # Check collisions? + collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. + self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m] + scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m] diff --git a/ada_moveit/config/moveit_controllers.yaml b/ada_moveit/config/moveit_controllers.yaml index 2ae0cf6..a1f990d 100644 --- a/ada_moveit/config/moveit_controllers.yaml +++ b/ada_moveit/config/moveit_controllers.yaml @@ -1,9 +1,10 @@ # MoveIt uses this configuration for controller management -moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager +moveit_controller_manager: moveit_ros_control_interface/Ros2ControlManager moveit_simple_controller_manager: controller_names: + - jaco_arm_servo_controller - jaco_arm_controller - hand_controller @@ -18,6 +19,17 @@ moveit_simple_controller_manager: - j2n6s200_joint_4 - j2n6s200_joint_5 - j2n6s200_joint_6 + jaco_arm_servo_controller: + type: "" + action_ns: commands + default: false + joints: + - j2n6s200_joint_1 + - j2n6s200_joint_2 + - j2n6s200_joint_3 + - j2n6s200_joint_4 + - j2n6s200_joint_5 + - j2n6s200_joint_6 hand_controller: type: FollowJointTrajectory action_ns: follow_joint_trajectory diff --git a/ada_moveit/config/real_controllers.yaml b/ada_moveit/config/real_controllers.yaml index bd1ac50..d6d38b6 100644 --- a/ada_moveit/config/real_controllers.yaml +++ b/ada_moveit/config/real_controllers.yaml @@ -7,6 +7,10 @@ controller_manager: type: force_gate_controller/ForceGateController + jaco_arm_servo_controller: + type: force_gate_controller/ForceGateVelocityController + + hand_controller: type: joint_trajectory_controller/JointTrajectoryController @@ -60,6 +64,19 @@ jaco_arm_controller: goal: 0.02 trajectory: 0.05 +jaco_arm_servo_controller: + ros__parameters: + joints: + - j2n6s200_joint_1 + - j2n6s200_joint_2 + - j2n6s200_joint_3 + - j2n6s200_joint_4 + - j2n6s200_joint_5 + - j2n6s200_joint_6 + wrench_threshold: + topic: /wireless_ft/ftSensor1 + fMag: 1.0 + hand_controller: ros__parameters: joints: diff --git a/ada_moveit/config/real_servo.yaml b/ada_moveit/config/real_servo.yaml new file mode 100644 index 0000000..87183ab --- /dev/null +++ b/ada_moveit/config/real_servo.yaml @@ -0,0 +1,75 @@ +# Adapted from https://github.com/ros-planning/moveit2/blob/humble/moveit_ros/moveit_servo/config/panda_simulated_config.yaml +############################################### +# Modify all parameters related to servoing here +############################################### +servo_node: + ros__parameters: + moveit_servo: + use_gazebo: false # Whether the robot is started in a Gazebo simulation environment + + ## 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: + # Scale parameters are only used if command_in_type=="unitless" + linear: 0.1 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. + rotational: 0.3 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands. + # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic. + joint: 0.5 + + # Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling) + # override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0] + + ## Properties of outgoing commands + publish_period: 0.034 # 1/Nominal publish rate [seconds] + low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) + + # What type of topic does your robot driver expect? + # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory + command_out_type: std_msgs/Float64MultiArray + + # What to publish? Can save some bandwidth as most robots only require positions or velocities + publish_joint_positions: false + publish_joint_velocities: true + publish_joint_accelerations: false + + ## Plugins for smoothing outgoing commands + smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" + + # If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service, + # which other nodes can use as a source for information about the planning environment. + # NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node), + # then is_primary_planning_scene_monitor needs to be set to false. + is_primary_planning_scene_monitor: false + + ## MoveIt properties + move_group_name: jaco_arm # Often 'manipulator' or 'arm' + planning_frame: j2n6s200_link_base # The MoveIt planning frame. Often 'base_link' or 'world' + + ## Other frames + ee_frame_name: forkTip # The name of the end effector link, used to return the EE pose + robot_link_command_frame: j2n6s200_link_base # commands must be given in the frame of a robot link. Usually either the base or end effector + + ## Stopping behaviour + incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command + # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. + # Important because ROS may drop some messages and we need the robot to halt reliably. + num_outgoing_halt_msgs_to_publish: 0 + + ## Configure handling of singularities and joint limits + lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity) + hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this + joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. + leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/ros-planning/moveit2/pull/620) + + ## Topic names + cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands + joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands + joint_topic: /joint_states + status_topic: ~/status # Publish status to this topic + command_out_topic: /jaco_arm_servo_controller/commands # Publish outgoing commands here + + ## Collision checking for the entire robot body + check_collisions: true # Check collisions? + collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. + self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m] + scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m] diff --git a/ada_moveit/launch/demo.launch.py b/ada_moveit/launch/demo.launch.py index 5762f9f..88a9a87 100644 --- a/ada_moveit/launch/demo.launch.py +++ b/ada_moveit/launch/demo.launch.py @@ -1,4 +1,5 @@ import os +import yaml from ament_index_python.packages import get_package_share_directory from moveit_configs_utils import MoveItConfigsBuilder from moveit_configs_utils.launches import generate_demo_launch @@ -54,11 +55,19 @@ def generate_launch_description(): ) controllers_file = LaunchConfiguration("controllers_file") + servo_da = DeclareLaunchArgument( + "servo_file", + default_value=[sim, "_servo.yaml"], + description="MoveIt Servo YAML configuration in config folder", + ) + servo_file = LaunchConfiguration("servo_file") + # Copy from generate_demo_launch ld = LaunchDescription() + ld.add_action(calib_da) ld.add_action(sim_da) ld.add_action(ctrl_da) - ld.add_action(calib_da) + ld.add_action(servo_da) # Camera Calibration File ld.add_action( @@ -127,6 +136,7 @@ def generate_launch_description(): ) ) + # Launch the Move Group ld.add_action( IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -172,6 +182,23 @@ def generate_launch_description(): "robot_description": ParameterValue(robot_description_content, value_type=str) } + # Launch MoveIt Servo + servo_config = PathJoinSubstitution( + [str(moveit_config.package_path), "config", servo_file] + ) + ld.add_action(Node( + package="moveit_servo", + executable="servo_node_main", + name="servo_node", + parameters=[ + servo_config, + robot_description, + moveit_config.robot_description_semantic, + # moveit_config.robot_description_kinematics, # If set, use IK instead of the inverse jacobian + ], + output="screen", + )) + robot_controllers = PathJoinSubstitution( [str(moveit_config.package_path), "config", controllers_file] ) diff --git a/ada_moveit/package.xml b/ada_moveit/package.xml index 523e03f..e07939d 100644 --- a/ada_moveit/package.xml +++ b/ada_moveit/package.xml @@ -17,11 +17,13 @@ Ethan K. Gordon ament_cmake + ament_cmake_python moveit_ros_move_group moveit_kinematics moveit_planners moveit_simple_controller_manager + moveit_ros_controller_interface joint_state_publisher joint_state_publisher_gui tf2_ros diff --git a/ada_moveit/scripts/ada_keyboard_teleop.py b/ada_moveit/scripts/ada_keyboard_teleop.py new file mode 100755 index 0000000..92b8ac6 --- /dev/null +++ b/ada_moveit/scripts/ada_keyboard_teleop.py @@ -0,0 +1,239 @@ +#!/usr/bin/env python3 +# Adapted from: +# https://github.com/turtlebot/turtlebot/blob/melodic/turtlebot_teleop/scripts/turtlebot_teleop_key +""" +This module contains a ROS2 node to allow the user to teleoperate the ADA arm +using the keyboard. Specifically, this node allows users to send linear cartesian +velocities in the base frame, angular cartesian velocities in the end-effector +frame, or joint velocities to the robot via MoveIt Servo. +""" + +# Standard imports +import termios +import tty +import select +import sys + +# Third-party imports +from control_msgs.msg import JointJog +from geometry_msgs.msg import TwistStamped +import rclpy +from rclpy.time import Time +from tf2_geometry_msgs import Vector3Stamped # pylint: disable=unused-import +import tf2_py as tf2 +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener + +INSTRUCTION_MSG = """ +Control the ADA arm! +--------------------------- +Cartesian control (linear): + w/s: forward/backwards + a/d: left/right + q/e: up/down + f: toggle between base frame (default) and end effector frame + +Cartesian control (angular): + i/k: +pitch/-pitch + j/l: +yaw/-yaw + u/o: +roll/-roll + +Joint control: + 1-6: joint 1-6 + r: reverse the direction of joint movement + +CTRL-C to quit +""" +BASE_FRAME = "j2n6s200_link_base" +EE_FRAME = "forkTip" +LINEAR_VEL_CMD = 0.1 # m/s +ANGULAR_VEL_CMD = 0.3 # rad/s +JOINT_VEL_CMD = 0.5 # rad/s + + +def get_key(settings): + """ + Read a key from stdin without writing it to terminal. + """ + tty.setraw(sys.stdin.fileno()) + rlist, _, _ = select.select([sys.stdin], [], [], 0.1) + if rlist: + key = sys.stdin.read(1) + else: + key = "" + + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) + return key + + +cartesian_control_linear_bindings = { + "w": (0.0, -1.0, 0.0), # forward + "s": (0.0, 1.0, 0.0), # backwards + "a": (1.0, 0.0, 0.0), # left + "d": (-1.0, 0.0, 0.0), # right + "q": (0.0, 0.0, 1.0), # up + "e": (0.0, 0.0, -1.0), # down +} +toggle_linear_frame_key = "f" # pylint: disable=invalid-name +cartesian_control_angular_bindings = { + "i": (1.0, 0.0, 0.0), # +pitch + "k": (-1.0, 0.0, 0.0), # -pitch + "j": (0.0, 1.0, 0.0), # +yaw + "l": (0.0, -1.0, 0.0), # -yaw + "u": (0.0, 0.0, 1.0), # +roll + "o": (0.0, 0.0, -1.0), # -roll +} +joint_control_bindings = { + "1": "j2n6s200_joint_1", + "2": "j2n6s200_joint_2", + "3": "j2n6s200_joint_3", + "4": "j2n6s200_joint_4", + "5": "j2n6s200_joint_5", + "6": "j2n6s200_joint_6", +} +reverse_joint_direction_key = "r" # pylint: disable=invalid-name + + +def main(args=None): + """ + Launch the ROS node and spin. + """ + # pylint: disable=too-many-locals, too-many-statements, too-many-branches + # pylint: disable=too-many-nested-blocks + # This function does the entire work of teleoperation, so it is expected + # to be somewhat complex. + + settings = termios.tcgetattr(sys.stdin) + + # Initialize the ROS context + rclpy.init(args=args) + node = rclpy.create_node("ada_keyboard_teleop") + twist_pub = node.create_publisher(TwistStamped, "/servo_node/delta_twist_cmds", 1) + joint_pub = node.create_publisher(JointJog, "/servo_node/delta_joint_cmds", 1) + + # Initialize the tf2 buffer and listener + tf_buffer = Buffer() + tf_listener = TransformListener(tf_buffer, node) # pylint: disable=unused-variable + + # Create the cartesian control messages + # The linear velocity is always in the base frame + linear_msg = Vector3Stamped() + linear_msg.header.stamp = Time().to_msg() # use latest time + linear_msg.header.frame_id = BASE_FRAME + # The angular velocity is always in the end effector frame + angular_msg = Vector3Stamped() + angular_msg.header.stamp = Time().to_msg() # use latest time + angular_msg.header.frame_id = EE_FRAME + # The final message should be either in the base or end effector frame. + # It should match the `robot_link_command_frame`` servo param. + twist_msg = TwistStamped() + twist_msg.header.frame_id = BASE_FRAME + + # Create the joint control message + joint_msg = JointJog() + joint_msg.header.frame_id = BASE_FRAME + + prev_key = "" + joint_direction = 1.0 + + try: + node.get_logger().info(INSTRUCTION_MSG) + while 1: + rclpy.spin_once(node, timeout_sec=0) + + publish_joint_msg = False + + key = get_key(settings) + if key in cartesian_control_linear_bindings: + # Due to keyboard delay before repeat, when the user holds down a + # key we will read it as the key, followed by some number of empty + # readings, followed by the key consecutively. To account for this, + # we require two consecutive readings of the same key before + # publishing the velcoity commands. + if prev_key == key: + x, y, z = cartesian_control_linear_bindings[key] + linear_msg.vector.x = x * LINEAR_VEL_CMD + linear_msg.vector.y = y * LINEAR_VEL_CMD + linear_msg.vector.z = z * LINEAR_VEL_CMD + + # Transform the linear message to the overall twist message frame + twist_msg.twist.linear = linear_msg.vector + if linear_msg.header.frame_id != twist_msg.header.frame_id: + try: + linear_transformed = tf_buffer.transform( + linear_msg, twist_msg.header.frame_id + ) + twist_msg.twist.linear = linear_transformed.vector + except tf2.ExtrapolationException as exc: + node.get_logger().warning( + f"Transform from {linear_msg.header.frame_id} to " + f"{twist_msg.header.frame_id} failed: {type(exc)}: {exc}\n" + f"Interpreting the linear velocity in {twist_msg.header.frame_id} " + "without transforming." + ) + elif key in cartesian_control_angular_bindings: + if prev_key == key: + x, y, z = cartesian_control_angular_bindings[key] + angular_msg.vector.x = x * ANGULAR_VEL_CMD + angular_msg.vector.y = y * ANGULAR_VEL_CMD + angular_msg.vector.z = z * ANGULAR_VEL_CMD + + # Transform the angular message to the overall twist message frame + twist_msg.twist.angular = angular_msg.vector + if angular_msg.header.frame_id != twist_msg.header.frame_id: + try: + angular_transformed = tf_buffer.transform( + angular_msg, twist_msg.header.frame_id + ) + twist_msg.twist.angular = angular_transformed.vector + except tf2.ExtrapolationException as exc: + node.get_logger().warning( + f"Transform from {angular_msg.header.frame_id} to " + f"{twist_msg.header.frame_id} failed: {type(exc)}: {exc}\n" + f"Interpreting the angular velocity in {twist_msg.header.frame_id}" + " without transforming." + ) + elif key in joint_control_bindings: + if prev_key == key: + joint_msg.joint_names = [joint_control_bindings[key]] + joint_msg.velocities = [JOINT_VEL_CMD * joint_direction] + publish_joint_msg = True + elif key == reverse_joint_direction_key: + joint_direction *= -1.0 + elif key == toggle_linear_frame_key: + if linear_msg.header.frame_id == BASE_FRAME: + linear_msg.header.frame_id = EE_FRAME + else: + linear_msg.header.frame_id = BASE_FRAME + else: + twist_msg.twist.linear.x = 0.0 + twist_msg.twist.linear.y = 0.0 + twist_msg.twist.linear.z = 0.0 + twist_msg.twist.angular.x = 0.0 + twist_msg.twist.angular.y = 0.0 + twist_msg.twist.angular.z = 0.0 + + # Ctrl+C Interrupt + if key == "\x03": + break + + # Publish the message + if publish_joint_msg: + joint_msg.header.stamp = node.get_clock().now().to_msg() + joint_pub.publish(joint_msg) + else: + twist_msg.header.stamp = node.get_clock().now().to_msg() + twist_pub.publish(twist_msg) + + prev_key = key + except Exception as exc: # pylint: disable=broad-except + print(repr(exc)) + + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) + # Terminate this node + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main()