diff --git a/doc/examples/realtime_servo/CMakeLists.txt b/doc/examples/realtime_servo/CMakeLists.txt index 0dce73a746..a33cf18998 100644 --- a/doc/examples/realtime_servo/CMakeLists.txt +++ b/doc/examples/realtime_servo/CMakeLists.txt @@ -1,15 +1,10 @@ -add_executable(servo_keyboard_input src/servo_keyboard_input.cpp) -target_include_directories(servo_keyboard_input PUBLIC include) -ament_target_dependencies(servo_keyboard_input ${THIS_PACKAGE_INCLUDE_DEPENDS}) - -add_executable(servo_cpp_interface_demo src/servo_cpp_interface_demo.cpp) -target_include_directories(servo_cpp_interface_demo PUBLIC include) -ament_target_dependencies(servo_cpp_interface_demo ${THIS_PACKAGE_INCLUDE_DEPENDS}) +add_executable(pose_tracking_tutorial src/pose_tracking_tutorial.cpp) +target_include_directories(pose_tracking_tutorial PUBLIC include) +ament_target_dependencies(pose_tracking_tutorial ${THIS_PACKAGE_INCLUDE_DEPENDS}) install( TARGETS - servo_keyboard_input - servo_cpp_interface_demo + pose_tracking_tutorial DESTINATION lib/${PROJECT_NAME} ) diff --git a/doc/examples/realtime_servo/config/demo_rviz_config.rviz b/doc/examples/realtime_servo/config/demo_rviz_config.rviz index e44c6604e8..a50d33565d 100644 --- a/doc/examples/realtime_servo/config/demo_rviz_config.rviz +++ b/doc/examples/realtime_servo/config/demo_rviz_config.rviz @@ -6,8 +6,10 @@ Panels: Expanded: - /Global Options1 - /Status1 + - /TF1/Frames1 + - /MarkerArray1 Splitter Ratio: 0.5 - Tree Height: 628 + Tree Height: 549 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -21,6 +23,11 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" Visualization Manager: Class: "" Displays: @@ -46,12 +53,12 @@ Visualization Manager: Enabled: true Move Group Namespace: "" Name: PlanningScene - Planning Scene Topic: /moveit_servo/publish_planning_scene + Planning Scene Topic: /servo_node/publish_planning_scene Robot Description: robot_description Scene Geometry: Scene Alpha: 0.8999999761581421 Scene Color: 50; 230; 50 - Scene Display Time: 0.20000000298023224 + Scene Display Time: 0.009999999776482582 Show Scene Geometry: true Voxel Coloring: Z-Axis Voxel Rendering: Occupied Voxels @@ -136,7 +143,7 @@ Visualization Manager: panda_leftfinger: Value: false panda_link0: - Value: false + Value: true panda_link1: Value: false panda_link2: @@ -156,7 +163,7 @@ Visualization Manager: panda_rightfinger: Value: false world: - Value: true + Value: false Marker Scale: 1 Name: TF Show Arrows: true @@ -180,6 +187,18 @@ Visualization Manager: {} Update Interval: 0 Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /path_markers + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -195,6 +214,9 @@ Visualization Manager: - Class: rviz_default_plugins/Measure Line color: 128; 128; 0 - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 Topic: Depth: 5 Durability Policy: Volatile @@ -223,39 +245,41 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 2.155569553375244 + Distance: 4.086756229400635 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -0.08608254045248032 - Y: -0.20677587389945984 - Z: 0.3424459993839264 + X: 0 + Y: 0 + Z: 0 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.4603978991508484 + Pitch: 0.785398006439209 Target Frame: Value: Orbit (rviz) - Yaw: 0.8753982782363892 + Yaw: 0.785398006439209 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 857 + Height: 846 Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd000000040000000000000216000002fffc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002ff000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000000000010000010f000002d2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002d2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000416000002ff00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false + Time: + collapsed: false Tool Properties: collapsed: false Views: - collapsed: true - Width: 1586 - X: 1179 - Y: 393 + collapsed: false + Width: 1200 + X: 74 + Y: 60 diff --git a/doc/examples/realtime_servo/config/panda_simulated_config.yaml b/doc/examples/realtime_servo/config/panda_simulated_config.yaml new file mode 100644 index 0000000000..35e8eedd25 --- /dev/null +++ b/doc/examples/realtime_servo/config/panda_simulated_config.yaml @@ -0,0 +1,62 @@ +############################################### +# Modify all parameters related to servoing here +############################################### + +# 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] + +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.4 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. + rotational: 0.8 # 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 + +# What type of topic does your robot driver expect? +# Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory +command_out_type: trajectory_msgs/JointTrajectory + +# What to publish? Can save some bandwidth as most robots only require positions or velocities +publish_joint_positions: true +publish_joint_velocities: true +publish_joint_accelerations: false + +## Plugins for smoothing outgoing commands +use_smoothing: true +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: true + +## MoveIt properties +move_group_name: panda_arm # Often 'manipulator' or 'arm' +planning_frame: panda_link0 # The MoveIt planning frame. Often 'base_link' or 'world' + +## Other frames +ee_frame: panda_hand # The name of the end effector link, used to return the EE pose + +## 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. + +## 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: /panda_arm_controller/joint_trajectory # 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/doc/examples/realtime_servo/launch/pose_tracking_tutorial.launch.py b/doc/examples/realtime_servo/launch/pose_tracking_tutorial.launch.py new file mode 100644 index 0000000000..1a09a29175 --- /dev/null +++ b/doc/examples/realtime_servo/launch/pose_tracking_tutorial.launch.py @@ -0,0 +1,152 @@ +import os +import launch +import launch_ros +from ament_index_python.packages import get_package_share_directory +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_param_builder import ParameterBuilder +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .to_moveit_configs() + ) + + # Launch Servo as a standalone node or as a "node component" for better latency/efficiency + launch_as_standalone_node = LaunchConfiguration( + "launch_as_standalone_node", default="false" + ) + + # Get parameters for the Servo node + servo_params = { + "moveit_servo": ParameterBuilder("moveit2_tutorials") + .yaml("config/panda_simulated_config.yaml") + .to_dict() + } + + # This filter parameter should be >1. Increase it for greater smoothing but slower motion. + low_pass_filter_coeff = {"butterworth_filter_coeff": 1.5} + + # RViz + rviz_config_file = ( + get_package_share_directory("moveit2_tutorials") + + "/config/demo_rviz_config.rviz" + ) + rviz_node = launch_ros.actions.Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + ], + ) + + # ros2_control using FakeSystem as hardware + ros2_controllers_path = os.path.join( + get_package_share_directory("moveit_resources_panda_moveit_config"), + "config", + "ros2_controllers.yaml", + ) + ros2_control_node = launch_ros.actions.Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[moveit_config.robot_description, ros2_controllers_path], + output="screen", + ) + + joint_state_broadcaster_spawner = launch_ros.actions.Node( + package="controller_manager", + executable="spawner", + arguments=[ + "joint_state_broadcaster", + "--controller-manager-timeout", + "300", + "--controller-manager", + "/controller_manager", + ], + ) + + panda_arm_controller_spawner = launch_ros.actions.Node( + package="controller_manager", + executable="spawner", + arguments=["panda_arm_controller", "-c", "/controller_manager"], + ) + + # Launch as much as possible in components + container = launch_ros.actions.ComposableNodeContainer( + name="moveit_servo_demo_container", + namespace="/", + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=[ + # Example of launching Servo as a node component + # Launching as a node component makes ROS 2 intraprocess communication more efficient. + launch_ros.descriptions.ComposableNode( + package="moveit_servo", + plugin="moveit_servo::ServoNode", + name="servo_node", + parameters=[ + servo_params, + low_pass_filter_coeff, + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + ], + condition=UnlessCondition(launch_as_standalone_node), + ), + launch_ros.descriptions.ComposableNode( + package="robot_state_publisher", + plugin="robot_state_publisher::RobotStatePublisher", + name="robot_state_publisher", + parameters=[moveit_config.robot_description], + ), + launch_ros.descriptions.ComposableNode( + package="tf2_ros", + plugin="tf2_ros::StaticTransformBroadcasterNode", + name="static_tf2_broadcaster", + parameters=[{"child_frame_id": "/panda_link0", "frame_id": "/world"}], + ), + ], + output="screen", + ) + # Launch a standalone Servo node. + # As opposed to a node component, this may be necessary (for example) if Servo is running on a different PC + servo_node = launch_ros.actions.Node( + package="moveit_servo", + executable="servo_node", + name="servo_node", + parameters=[ + servo_params, + low_pass_filter_coeff, + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + ], + output="screen", + condition=IfCondition(launch_as_standalone_node), + ) + + demo_node = launch_ros.actions.Node( + package="moveit2_tutorials", + executable="pose_tracking_tutorial", + name="pose_tracking_tutorial", + output="screen", + ) + + return launch.LaunchDescription( + [ + rviz_node, + ros2_control_node, + joint_state_broadcaster_spawner, + panda_arm_controller_spawner, + servo_node, + container, + launch.actions.TimerAction(period=10.0, actions=[demo_node]), + ] + ) diff --git a/doc/examples/realtime_servo/launch/servo_cpp_interface_demo.launch.py b/doc/examples/realtime_servo/launch/servo_cpp_interface_demo.launch.py deleted file mode 100644 index 4e8aefc6e2..0000000000 --- a/doc/examples/realtime_servo/launch/servo_cpp_interface_demo.launch.py +++ /dev/null @@ -1,99 +0,0 @@ -import os -from launch import LaunchDescription -from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory -from launch.actions import ExecuteProcess -from moveit_configs_utils import MoveItConfigsBuilder -from launch_param_builder import ParameterBuilder - - -def generate_launch_description(): - moveit_config = ( - MoveItConfigsBuilder("moveit_resources_panda") - .robot_description(file_path="config/panda.urdf.xacro") - .to_moveit_configs() - ) - # Get parameters for the Servo node - servo_params = { - "moveit_servo": ParameterBuilder("moveit_servo") - .yaml("config/panda_simulated_config.yaml") - .to_dict() - } - - # A node to publish world -> panda_link0 transform - static_tf = Node( - package="tf2_ros", - executable="static_transform_publisher", - name="static_transform_publisher", - output="log", - arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"], - ) - - # The servo cpp interface demo - # Creates the Servo node and publishes commands to it - servo_node = Node( - package="moveit2_tutorials", - executable="servo_cpp_interface_demo", - output="screen", - parameters=[ - servo_params, - moveit_config.robot_description, - moveit_config.robot_description_semantic, - ], - ) - - # Publishes tf's for the robot - robot_state_publisher = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="screen", - parameters=[moveit_config.robot_description], - ) - - # RViz - rviz_config_file = ( - get_package_share_directory("moveit2_tutorials") - + "/config/demo_rviz_config.rviz" - ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - parameters=[ - moveit_config.robot_description, - moveit_config.robot_description_semantic, - ], - ) - - # ros2_control using FakeSystem as hardware - ros2_control_node = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[ - moveit_config.robot_description, - os.path.join( - get_package_share_directory("moveit_resources_panda_moveit_config"), - "config", - "ros2_controllers.yaml", - ), - ], - output="both", - ) - - # Load controllers - load_controllers = [] - for controller in ["panda_arm_controller", "joint_state_broadcaster"]: - load_controllers += [ - ExecuteProcess( - cmd=["ros2 run controller_manager spawner {}".format(controller)], - shell=True, - output="screen", - ) - ] - - return LaunchDescription( - [rviz_node, static_tf, servo_node, ros2_control_node, robot_state_publisher] - + load_controllers - ) diff --git a/doc/examples/realtime_servo/realtime_servo_tutorial.rst b/doc/examples/realtime_servo/realtime_servo_tutorial.rst index 819a0fd0ea..186b7239ee 100644 --- a/doc/examples/realtime_servo/realtime_servo_tutorial.rst +++ b/doc/examples/realtime_servo/realtime_servo_tutorial.rst @@ -1,116 +1,55 @@ -Realtime Arm Servoing -===================== +Realtime Servo +=============== -MoveIt Servo allows you to stream End Effector (EEF) velocity commands to your manipulator and have it execute them concurrently. This enables teleoperation via a wide range of input schemes, or for other autonomous software to control the robot - in visual servoing or closed loop position control for instance. +MoveIt Servo facilitates realtime control of your robot arm. -This tutorial shows how to use MoveIt Servo to send real-time servo commands to a ROS-enabled robot. Some nice features of the servo node are singularity handling and collision checking that prevents the operator from breaking the robot. .. raw:: html
- +
-Getting Started ---------------- -If you haven't already done so, make sure you've completed the steps in :doc:`Getting Started `. - -Launching a Servo Node ----------------------- -MoveIt Servo can be launched as a "node component" or a standalone node. The launch file, moveit_servo/servo_example.launch.py, launches a standalone node by default but also contains a commented component node. Commands are sent via ROS topics. The commands can come from anywhere, such as a joystick, keyboard, or other controller. - -This demo was written for an Xbox 1 controller, but can be easily modified to use any controller compatible with the `Joy package `_ by modifying the `joystick_servo_example.cpp file `_ - -To run the demo, make sure your controller is plugged in and can be detected by :code:`ros2 run joy joy_node`. Usually this happens automatically after plugging the controller in. Then launch with :: - - ros2 launch moveit_servo servo_example.launch.py - -Make a service request to start Servo :: - - ros2 service call /servo_node/start_servo std_srvs/srv/Trigger {} - -You should be able to control the arm with your controller now, with MoveIt Servo automatically avoiding singularities and collisions. - -Without a Controller -^^^^^^^^^^^^^^^^^^^^ - -If you do not have a joystick or game controller, you can still try the demo using your keyboard. With the demo still running, in a new terminal, run :: - - ros2 run moveit2_tutorials servo_keyboard_input - -You will be able to use your keyboard to servo the robot. Send Cartesian commands with arrow keys and the :code:`.` and :code:`;` keys. Change the Cartesian command frame with :code:`W` for world and :code:`E` for End-Effector. Send joint jogging commands with keys 1-7 (use :code:`R` to reverse direction) - -.. raw:: html - - - -Note that the controller overlay here is just for demonstration purposes and is not actually included - -Introspection -------------- - -Here are some tips for inspecting and/or debugging the system. +MoveIt Servo accepts any of the following types of commands: -#. View the :code:`ros2_controllers` that are currently active with :code:`ros2 control list_controllers`. You will see a `JointTrajectoryController `_ that receives the joint position commands from Servo and handles them in the simulated robot driver. The JointTrajectoryController is very flexible; it can handle any combination of position/velocity/(position-and-velocity) input. Servo is also compatible with `JointGroupPosition `_ or `JointGroupVelocity `_-type controllers. + 1. Individual joint velocities. + 2. The desired velocity of end effector. + 3. The desired pose of end effector. -#. :code:`ros2 topic echo /servo_node/status` shows the current state of the Servo node. If :code:`0` is published, all is well. The definition for all enums can be seen :moveit_codedir:`here.` +This enables teleoperation via a wide range of input schemes, or for other autonomous software to control the robot - in visual servoing or closed loop position control for instance. -#. :code:`ros2 node list` shows the following. :code:`ros2 node info` can be used to get more information about any of these nodes. - - - :code:`/joy_node` handles commands from the XBox controller - - - :code:`/moveit_servo_demo_container` holds several ancillary ROS2 "component nodes" that are placed in a container for faster intra-process communication - - - :code:`/servo_node` which does the calculations and collision checking for this demo. :code:`servo_node` may be moved into the demo container in the future - -Using the C++ Interface ------------------------ -Instead of launching Servo as its own component, you can include Servo in your own nodes via the C++ interface. Sending commands to the robot is very similar in both cases, but for the C++ interface a little bit of setup for Servo is necessary. In exchange, you will be able to directly interact with Servo through its C++ API. - -This basic C++ interface demo moves the robot in a predetermined way and can be launched with :: - - ros2 launch moveit2_tutorials servo_cpp_interface_demo.launch.py +Getting Started +--------------- -An Rviz window should appear with a Panda arm and collision object. The arm will joint-jog for a few seconds before switching to a Cartesian movement. As the arm approaches the collision object, it slows and stops. +If you haven't already done so, make sure you've completed the steps in :doc:`Getting Started `. -.. raw:: html - +Design overview +--------------- -Entire Code ------------ -The entire code is available :codedir:`here` +Moveit Servo consists of two main parts: The core implementation ``Servo`` which provides a C++ interface, and the ``ServoNode`` which +wraps the C++ interface and provides a ROS interface.The configuration of Servo is done through ROS parameters specified in :moveit_codedir:`servo_parameters.yaml ` -.. tutorial-formatter:: ./src/servo_cpp_interface_demo.cpp +In addition to the servoing capability, MoveIt Servo has some convenient features such as: + - Checking for singularities + - Checking for collisions + - Motion smoothing + - Joint position and velocity limits enforced -Servo Overview --------------- +Singularity checking and collision checking are safety features that scale down the velocities when approaching singularities or collisions (self collision or collision with other objects). +The collision checking and smoothing are optional features that can be disabled using the ``check_collisions`` parameter and the ``use_smoothing`` parameters respectively. -The following sections give some background information about MoveIt Servo and describe the first steps to set it up on your robot. +The inverse kinematics is handled through either the inverse Jacobain or the robot's IK solver if one was provided. -Servo includes a number of nice features: - 1. Cartesian End-Effector twist commands - 2. Joint commands - 3. Collision checking - 4. Singularity checking - 5. Joint position and velocity limits enforced - 6. Inputs are generic ROS messages Inverse Kinematics in Servo ^^^^^^^^^^^^^^^^^^^^^^^^^^^ Inverse Kinematics may be handled internally by MoveIt Servo via inverse Jacobian calculations. However, you may also use an IK plugin. -To configure an IK plugin for use in Servo, your robot config package must define one in a :code:`kinematics.yaml` file, such as the one -in the `Panda config package `_. -Several IK plugins are available `within MoveIt `_, -as well as `externally `_. +To configure an IK plugin for use in MoveIt Servo, your robot config package must define one in a :code:`kinematics.yaml` file, such as the one +in the :moveit_resources_codedir:`Panda config package `. +Several IK plugins are available :moveit_codedir:`within MoveIt `, as well as `externally `_. :code:`bio_ik/BioIKKinematicsPlugin` is the most common choice. Once your :code:`kinematics.yaml` file has been populated, include it with the ROS parameters passed to the servo node in your launch file: @@ -124,9 +63,10 @@ Once your :code:`kinematics.yaml` file has been populated, include it with the R ) servo_node = Node( package="moveit_servo", - executable="servo_node_main", + executable="servo_node", parameters=[ servo_params, + low_pass_filter_coeff, moveit_config.robot_description, moveit_config.robot_description_semantic, moveit_config.robot_description_kinematics, # here is where kinematics plugin parameters are passed @@ -134,8 +74,8 @@ Once your :code:`kinematics.yaml` file has been populated, include it with the R ) -The above excerpt is taken from `servo_example.launch.py `_ in MoveIt. -In the above example, the :code:`kinematics.yaml` file is taken from the `moveit_resources `_ repository in the workspace, specifically :code:`moveit_resources/panda_moveit_config/config/kinematics.yaml`. +The above excerpt is taken from :moveit_codedir:`servo_example.launch.py ` in MoveIt. +In the above example, the :code:`kinematics.yaml` file is taken from the :moveit_resources_codedir:`moveit_resources ` repository in the workspace, specifically :code:`moveit_resources/panda_moveit_config/config/kinematics.yaml`. The actual ROS parameter names that get passed by loading the yaml file are of the form :code:`robot_description_kinematics..`, e.g. :code:`robot_description_kinematics.panda_arm.kinematics_solver`. Since :code:`moveit_servo` does not allow undeclared parameters found in the :code:`kinematics.yaml` file to be set on the Servo node, custom solver parameters need to be declared from inside your plugin code. @@ -145,37 +85,164 @@ For example, :code:`bio_ik` defines a :code:`getROSParam()` function in `bio_ik/ Setup on a New Robot -------------------- -Preliminaries -^^^^^^^^^^^^^ - The bare minimum requirements for running MoveIt Servo with your robot include: - 1. A valid URDF and SRDF of the robot - 2. A controller that can accept joint positions or velocities from a ROS topic - 3. Joint encoders that provide rapid and accurate joint position feedback + 1. A valid URDF and SRDF of the robot. + 2. A controller that can accept joint positions or velocities. + 3. Joint encoders that provide rapid and accurate joint position feedback. Because the kinematics are handled by the core parts of MoveIt, it is recommended that you have a valid config package for your robot and you can run the demo launch file included with it. -Input Devices -^^^^^^^^^^^^^ -The two primary inputs to MoveIt Servo are Cartesian commands and joint commands. These come into Servo as `TwistStamped `_ and `JointJog `_ messages respectively. The source of the commands can be almost anything including: gamepads, voice commands, a SpaceNav mouse, or PID controllers (e.g. for visual servoing). +Using the C++ API +------------------ +This can be beneficial when there is a performance requirement to avoid the overhead of ROS communication infrastucture, or when the output generated by Servo needs to be fed into some other controller that does not have a ROS interface. + +When using MoveIt Servo with the C++ interface the three input command types are ``JointJogCommand``, ``TwistCommand`` and ``PoseCommand``. +The output from Servo when using the C++ interface is ``KinematicState``, a struct containing joint names, positions, velocities and accelerations. +As given by the definitions in :moveit_codedir:`datatypes ` header file. + +The first step is to create a ``Servo`` instance. + +.. code-block:: c++ + + // Import the Servo headers. + #include + #include + + // The node to be used by Servo. + rclcpp::Node::SharedPtr node = std::make_shared("servo_tutorial"); + + // Get the Servo parameters. + const std::string param_namespace = "moveit_servo"; + const std::shared_ptr servo_param_listener = + std::make_shared(node, param_namespace); + const servo::Params servo_params = servo_param_listener->get_params(); + + // Create the planning scene monitor. + const planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor = + createPlanningSceneMonitor(node, servo_params); + + // Create a Servo instance. + Servo servo = Servo(node, servo_param_listener, planning_scene_monitor); + + +Using the JointJogCommand + +.. code-block:: c++ + + using namespace moveit_servo; + + // Create the command. + JointJogCommand command; + command.joint_names = {"panda_link7"}; + command.velocities = {0.1}; + + // Set JointJogCommand as the input type. + servo.setCommandType(CommandType::JOINT_JOG); + + // Get the joint states required to follow the command. + // This is generally run in a loop. + KinematicState next_joint_state = servo.getNextJointState(command); + +Using the TwistCommand + +.. code-block:: c++ + + using namespace moveit_servo; + + // Create the command. + TwistCommand command{"panda_link0", {0.1, 0.0, 0.0, 0.0, 0.0, 0.0}; + + // Set the command type. + servo.setCommandType(CommandType::TWIST); + + // Get the joint states required to follow the command. + // This is generally run in a loop. + KinematicState next_joint_state = servo.getNextJointState(command); + + +Using the PoseCommand + +.. code-block:: c++ + + using namespace moveit_servo; + + // Create the command. + Eigen::Isometry3d ee_pose = Eigen::Isometry3d::Identity(); // This is a dummy pose. + PoseCommand command{"panda_link0", ee_pose}; + + // Set the command type. + servo.setCommandType(CommandType::POSE); + + // Get the joint states required to follow the command. + // This is generally run in a loop. + KinematicState next_joint_state = servo.getNextJointState(command); + +The ``next_joint_state`` result can then be used for further steps in the control pipeline. + +The status of MoveIt Servo resulting from the last command can be obtained by: + +.. code-block:: c++ + + StatusCode status = servo.getStatus(); + +The user can use status for higher-level decision making. + +See :moveit_codedir:`moveit_servo/demos ` for complete examples of using the C++ interface. +The demos can be launched using the launch files found in :moveit_codedir:`moveit_servo/launch `. + + - ``ros2 launch moveit_servo demo_joint_jog.launch.py`` + - ``ros2 launch moveit_servo demo_twist.launch.py`` + - ``ros2 launch moveit_servo demo_pose.launch.py`` + + +Using the ROS API +----------------- + +To use MoveIt Servo through the ROS interface, it must be launched as a ``Node`` or ``Component`` along with the required parameters as seen :moveit_codedir:`here `. + +When using MoveIt Servo with the ROS interface the commands are ROS messages of the following types published to respective topics specified by the Servo parameters. + + 1. ``control_msgs::msg::JointJog`` on the topic specified by the ``joint_command_in_topic`` parameter. + 2. ``geometry_msgs::msg::TwistStamped`` on the topic specified by the ``cartesian_command_in_topic`` parameter. For now, the twist message must be in the planning frame of the robot. (This will be updated soon.) + 3. ``geometry_msgs::msg::PoseStamped`` on the topic specified by the ``pose_command_in_topic`` parameter. + +Twist and Pose commands require that the ``header.frame_id`` is always specified. +The output from ``ServoNode`` (the ROS interface) can either be ``trajectory_msgs::msg::JointTrajectory`` or ``std_msgs::msg::Float64MultiArray`` +selected using the *command_out_type* parameter, and published on the topic specified by *command_out_topic* parameter. + +The command type can be selected using the ``ServoCommandType`` service, see definition :moveit_msgs_codedir:`ServoCommandType `. + +From cli : ``ros2 service call //switch_command_type moveit_msgs/srv/ServoCommandType "{command_type: 1}"`` + +Programmatically: + +.. code-block:: c++ + + switch_input_client = node->create_client("//switch_command_type"); + auto request = std::make_shared(); + request->command_type = moveit_msgs::srv::ServoCommandType::Request::TWIST; + if (switch_input_client->wait_for_service(std::chrono::seconds(1))) + { + auto result = switch_input_client->async_send_request(request); + if (result.get()->success) + { + RCLCPP_INFO_STREAM(node->get_logger(), "Switched to input type: Twist"); + } + else + { + RCLCPP_WARN_STREAM(node->get_logger(), "Could not switch input to: Twist"); + } + } + +Similarly, servoing can be paused using the pause service ``/pause_servo`` of type ``std_msgs::srv::SetBool``. + +When using the ROS interface, the status of Servo is available on the topic ``//status``, see definition :moveit_msgs_codedir:`ServoStatus `. -Requirements for incoming command messages, regardless of input device are: - 1. **TwistStamped and JointJog:** need a timestamp in the header that is updated when the message is published - 2. **JointJog:** must have valid joint names in the :code:`joint_names` field that correspond with the commands given in the :code:`displacements` or :code:`velocities` fields - 3. **(Optional) TwistStamped:** can provide an arbitrary :code:`frame_id` in the header that the twist will be applied to. If empty, the default from the configs is used +Launch ROS interface demo: ``ros2 launch moveit_servo demo_ros_api.launch.py``. -Servo Configs -^^^^^^^^^^^^^ +Once the demo is running, the robot can be teleoperated through the keyboard. -The `demo config file `_ shows the parameters needed for MoveIt Servo and is well documented. +Launch the keyboard demo: ``ros2 run moveit_servo servo_keyboard_input``. -Start with the parameters from the demo file, but some must be changed for your specific setup: - 1. :code:`robot_link_command_frame`: Update this to be a valid frame in your robot, recommended as the planning frame or EEF frame - 2. :code:`command_in_type`: Set to "unitless" if your input comes from a joystick, "speed_units" if the input will be in meters/second or radians/second - 3. :code:`command_out_topic`: Change this to be the input topic of your controller - 4. :code:`command_out_type`: Change this based on the type of message your controller needs - 5. :code:`publish_joint_positions` and :code:`publish_joint_velocities`: Change these based on what your controller needs. Note if :code:`command_out_type == std_msgs/Float64MultiArray`, only one of these can be True - 6. :code:`joint_topic`: Change this to be the joint_state topic for your arm, usually :code:`/joint_states` - 7. :code:`move_group_name`: Change this to be the name of your move group, as defined in your SRDF - 8. :code:`planning_frame`: This should be the planning frame of your group +An example of using the pose commands in the context of servoing to open a door can be seen in this :codedir:`example `. diff --git a/doc/examples/realtime_servo/src/pose_tracking_tutorial.cpp b/doc/examples/realtime_servo/src/pose_tracking_tutorial.cpp new file mode 100644 index 0000000000..a32bf45cb4 --- /dev/null +++ b/doc/examples/realtime_servo/src/pose_tracking_tutorial.cpp @@ -0,0 +1,285 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Title : pose_tracking_tutorial.cpp + * Project : moveit2_tutorials + * Created : 08/07/2023 + * Author : V Mohammed Ibrahim + * + * Description : Example of using pose tracking via the ROS API in a door opening scenario. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * \brief Handles the simulation of the collision object representing the door. + */ +class Door +{ +public: + Door(const rclcpp::Node::SharedPtr& node) : node_(node) + { + dims_ = Eigen::Vector3d(0.5, 0.02, 0.8); + rotation_radius_ = dims_.x() / 2; + // Hinge is the bottom left corner + hinge_ = Eigen::Vector3d(0.8, 0.0, 0.0); + + center_.x() = hinge_.x() + dims_.x() / 2; + center_.y() = hinge_.y() + dims_.y() / 2 + rotation_radius_; + center_.z() = hinge_.z() + dims_.z() / 2; + + angle_ = (M_PI / 2); + + collision_object_publisher_ = + node_->create_publisher("/planning_scene", rclcpp::SystemDefaultsQoS()); + + createCollisionObject(); + } + + void rotateDoor(double angle) + { + angle_ = angle; + + collision_object_.primitives[0] = door_primitive_; + + center_.x() = hinge_.x() + (rotation_radius_ * cos(angle_)); + center_.y() = hinge_.y() + (rotation_radius_ * sin(angle_)); + geometry_msgs::msg::Pose door_pose; + door_pose.position.x = center_.x(); + door_pose.position.y = center_.y(); + door_pose.position.z = center_.z(); + auto orn = Eigen::Quaterniond(Eigen::AngleAxisd(angle_, Eigen::Vector3d::UnitZ())); + door_pose.orientation.w = orn.w(); + door_pose.orientation.x = orn.x(); + door_pose.orientation.y = orn.y(); + door_pose.orientation.z = orn.z(); + + collision_object_.operation = collision_object_.ADD; + collision_object_.primitive_poses[0] = door_pose; + collision_object_.header.stamp = node_->now(); + + moveit_msgs::msg::PlanningSceneWorld psw; + psw.collision_objects.push_back(collision_object_); + + moveit_msgs::msg::PlanningScene ps; + ps.world = psw; + ps.is_diff = true; + collision_object_publisher_->publish(ps); + } + +private: + void createCollisionObject() + { + collision_object_.id = "door"; + collision_object_.header.frame_id = "panda_link0"; + collision_object_.primitives.resize(1); + collision_object_.primitive_poses.resize(1); + + door_primitive_.type = shape_msgs::msg::SolidPrimitive::BOX; + door_primitive_.dimensions = { dims_[0], dims_[1], dims_[2] }; + } + + rclcpp::Node::SharedPtr node_; + Eigen::Vector3d hinge_, center_, dims_; + double angle_, step_, rotation_radius_; + rclcpp::Publisher::SharedPtr collision_object_publisher_; + moveit_msgs::msg::CollisionObject collision_object_; + shape_msgs::msg::SolidPrimitive door_primitive_; +}; + +/** + * \brief Generates the path to follow when opening the door. + */ +std::vector getPath() +{ + const double start_angle = M_PI / 2 + (M_PI / 8); + const double end_angle = M_PI; + const double step = 0.01745329; + std::vector traj; + + for (double i = start_angle; i < end_angle; i = i + step) + { + double x = 0.8 + (0.5 * cos(i)); + double y = 0.0 + (0.5 * sin(i)); + auto vec = Eigen::Vector3d(x, y, 0.4); + traj.push_back(vec); + } + return traj; +} + +/** + * \brief Creates an Rviz marker message to represent a waypoint in the path. + */ +visualization_msgs::msg::Marker getMarker(int id, const Eigen::Vector3d& position, const std::string& frame) +{ + visualization_msgs::msg::Marker marker; + marker.header.frame_id = frame; + marker.header.stamp = rclcpp::Time(0.0); + marker.id = id; + marker.type = visualization_msgs::msg::Marker::SPHERE; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.position.x = position.x(); + marker.pose.position.y = position.y(); + marker.pose.position.z = position.z(); + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 0.01; + marker.scale.y = 0.01; + marker.scale.z = 0.01; + marker.color.a = 1.0; + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + id++; + return marker; +} + +/** + * \brief Generates a PoseStamped message with the given position and orientation. + */ +geometry_msgs::msg::PoseStamped getPose(const Eigen::Vector3d& position, const Eigen::Quaterniond& rotation) +{ + geometry_msgs::msg::PoseStamped target_pose; + target_pose.header.frame_id = "panda_link0"; + target_pose.pose.orientation.w = rotation.w(); + target_pose.pose.orientation.x = rotation.x(); + target_pose.pose.orientation.y = rotation.y(); + target_pose.pose.orientation.z = rotation.z(); + target_pose.pose.position.x = position.x(); + target_pose.pose.position.y = position.y(); + target_pose.pose.position.z = position.z(); + + return target_pose; +} + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = std::make_shared("servo_tutorial"); + + auto marker_publisher = + node->create_publisher("/path_markers", rclcpp::SystemDefaultsQoS()); + auto pose_publisher = node->create_publisher("/servo_node/pose_target_cmds", + rclcpp::SystemDefaultsQoS()); + + auto switch_input_client = node->create_client("/servo_node/switch_command_type"); + + auto executor = std::make_shared(); + executor->add_node(node); + + // Spin the node. + std::thread executor_thread([&]() { executor->spin(); }); + + visualization_msgs::msg::MarkerArray marray; + std::vector path = getPath(); + + for (size_t i = 0; i < path.size(); i++) + { + marray.markers.push_back(getMarker(i, path[i], "panda_link0")); + } + marker_publisher->publish(marray); + + // Create the door. + Door door(node); + door.rotateDoor(M_PI / 2); + + // Default end-effector pose of the Panda arm. + auto ee_pose = Eigen::Isometry3d::Identity(); + ee_pose.rotate(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX())); + ee_pose.translate(Eigen::Vector3d(0.3, 0.0, 0.4)); + + // Create the ROS message. + auto request = std::make_shared(); + request->command_type = moveit_msgs::srv::ServoCommandType::Request::POSE; + auto response = switch_input_client->async_send_request(request); + if (response.get()->success) + { + RCLCPP_INFO_STREAM(node->get_logger(), "Switched to input type: Pose"); + } + else + { + RCLCPP_WARN_STREAM(node->get_logger(), "Could not switch input to: Pose"); + } + + // Follow the trajectory + + const double publish_period = 0.15; + rclcpp::WallRate rate(1 / publish_period); + + // The path needs to be reversed since the last point in the path is where we want to start. + std::reverse(path.begin(), path.end()); + + for (auto& waypoint : path) + { + auto target_pose = getPose(waypoint, Eigen::Quaterniond(ee_pose.rotation())); + target_pose.header.stamp = node->now(); + pose_publisher->publish(target_pose); + rate.sleep(); + } + + // Simulated door opening + + double door_angle = M_PI / 2; + const double step = 0.01745329; // 1 degree in radian + + // Reverse again to so that we follow that path in reverse order. + std::reverse(path.begin(), path.end()); + for (auto& waypoint : path) + { + ee_pose.rotate(Eigen::AngleAxisd(-step, Eigen::Vector3d::UnitZ())); + auto target_pose = getPose(waypoint, Eigen::Quaterniond(ee_pose.rotation())); + target_pose.header.stamp = node->now(); + pose_publisher->publish(target_pose); + rate.sleep(); + + door.rotateDoor(door_angle); + door_angle += step; + } + + executor->cancel(); + if (executor_thread.joinable()) + { + executor_thread.join(); + } + rclcpp::shutdown(); +} diff --git a/doc/examples/realtime_servo/src/servo_cpp_interface_demo.cpp b/doc/examples/realtime_servo/src/servo_cpp_interface_demo.cpp deleted file mode 100644 index dcb05a2b86..0000000000 --- a/doc/examples/realtime_servo/src/servo_cpp_interface_demo.cpp +++ /dev/null @@ -1,194 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, PickNik Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of PickNik Inc. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Title : servo_cpp_interface_demo.cpp - * Project : moveit2_tutorials - * Created : 07/13/2020 - * Author : Adam Pettinger - */ - -// ROS -#include - -// Auto-generated -#include -#include -#include - -using namespace std::chrono_literals; - -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit2_tutorials.servo_demo_node.cpp"); - -// BEGIN_TUTORIAL - -// Setup -// ^^^^^ -// First we declare pointers to the node and publisher that will publish commands to Servo -rclcpp::Node::SharedPtr node_; -rclcpp::Publisher::SharedPtr joint_cmd_pub_; -rclcpp::Publisher::SharedPtr twist_cmd_pub_; -size_t count_ = 0; - -// BEGIN_SUB_TUTORIAL publishCommands -// Here is the timer callback for publishing commands. The C++ interface sends commands through internal ROS topics, -// just like if Servo was launched using ServoNode. -void publishCommands() -{ - // First we will publish 100 joint jogging commands. The :code:`joint_names` field allows you to specify individual - // joints to move, at the velocity in the corresponding :code:`velocities` field. It is important that the message - // contains a recent timestamp, or Servo will think the command is stale and will not move the robot. - if (count_ < 100) - { - auto msg = std::make_unique(); - msg->header.stamp = node_->now(); - msg->joint_names.push_back("panda_joint1"); - msg->velocities.push_back(0.3); - joint_cmd_pub_->publish(std::move(msg)); - ++count_; - } - - // After a while, we switch to publishing twist commands. The provided frame is the frame in which the twist is - // defined, not the robot frame that will follow the command. Again, we need a recent timestamp in the message - else - { - auto msg = std::make_unique(); - msg->header.stamp = node_->now(); - msg->header.frame_id = "panda_link0"; - msg->twist.linear.x = 0.3; - msg->twist.angular.z = 0.5; - twist_cmd_pub_->publish(std::move(msg)); - } -} -// END_SUB_TUTORIAL - -// Next we will set up the node, planning_scene_monitor, and collision object -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - - // This is false for now until we fix the QoS settings in moveit to enable intra process comms - node_options.use_intra_process_comms(false); - node_ = std::make_shared("servo_demo_node", node_options); - - // Pause for RViz to come up. This is necessary in an integrated demo with a single launch file - rclcpp::sleep_for(std::chrono::seconds(4)); - - // Create the planning_scene_monitor. We need to pass this to Servo's constructor, and we should set it up first - // before initializing any collision objects - auto tf_buffer = std::make_shared(node_->get_clock()); - auto planning_scene_monitor = std::make_shared( - node_, "robot_description", tf_buffer, "planning_scene_monitor"); - - // Here we make sure the planning_scene_monitor is updating in real time from the joint states topic - if (planning_scene_monitor->getPlanningScene()) - { - planning_scene_monitor->startStateMonitor("/joint_states"); - planning_scene_monitor->setPlanningScenePublishingFrequency(25); - planning_scene_monitor->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE, - "/moveit_servo/publish_planning_scene"); - planning_scene_monitor->startSceneMonitor(); - planning_scene_monitor->providePlanningSceneService(); - } - else - { - RCLCPP_ERROR(LOGGER, "Planning scene not configured"); - return EXIT_FAILURE; - } - - // These are the publishers that will send commands to MoveIt Servo. Two command types are supported: JointJog - // messages which will directly jog the robot in the joint space, and TwistStamped messages which will move the - // specified link with the commanded Cartesian velocity. In this demo, we jog the end effector link. - joint_cmd_pub_ = node_->create_publisher("servo_demo_node/delta_joint_cmds", 10); - twist_cmd_pub_ = node_->create_publisher("servo_demo_node/delta_twist_cmds", 10); - - // Next we will create a collision object in the way of the arm. As the arm is servoed towards it, it will slow down - // and stop before colliding - moveit_msgs::msg::CollisionObject collision_object; - collision_object.header.frame_id = "panda_link0"; - collision_object.id = "box"; - - // Make a box and put it in the way - shape_msgs::msg::SolidPrimitive box; - box.type = box.BOX; - box.dimensions = { 0.1, 0.4, 0.1 }; - geometry_msgs::msg::Pose box_pose; - box_pose.position.x = 0.6; - box_pose.position.y = 0.0; - box_pose.position.z = 0.6; - - // Add the box as a collision object - collision_object.primitives.push_back(box); - collision_object.primitive_poses.push_back(box_pose); - collision_object.operation = collision_object.ADD; - - // Create the message to publish the collision object - moveit_msgs::msg::PlanningSceneWorld psw; - psw.collision_objects.push_back(collision_object); - moveit_msgs::msg::PlanningScene ps; - ps.is_diff = true; - ps.world = psw; - - // Publish the collision object to the planning scene - auto scene_pub = node_->create_publisher("planning_scene", 10); - scene_pub->publish(ps); - - // Initializing Servo - // ^^^^^^^^^^^^^^^^^^ - auto servo_param_listener = std::make_unique(node_, "moveit_servo"); - - // Initialize the Servo C++ interface by passing a pointer to the node, the parameters, and the PSM - moveit_servo::Servo servo(node_, planning_scene_monitor, std::move(servo_param_listener)); - - // You can start Servo directly using the C++ interface. If launched using the alternative ServoNode, a ROS - // service is used to start Servo. Before it is started, MoveIt Servo will not accept any commands or move the robot - servo.start(); - - // Sending Commands - // ^^^^^^^^^^^^^^^^ - // For this demo, we will use a simple ROS timer to send joint and twist commands to the robot - rclcpp::TimerBase::SharedPtr timer = node_->create_wall_timer(50ms, publishCommands); - - // CALL_SUB_TUTORIAL publishCommands - - // We use a multithreaded executor here because Servo has concurrent processes for moving the robot and avoiding collisions - auto executor = std::make_unique(); - executor->add_node(node_); - executor->spin(); - - // END_TUTORIAL - - rclcpp::shutdown(); - return 0; -} diff --git a/doc/examples/realtime_servo/src/servo_keyboard_input.cpp b/doc/examples/realtime_servo/src/servo_keyboard_input.cpp deleted file mode 100644 index 259952dd80..0000000000 --- a/doc/examples/realtime_servo/src/servo_keyboard_input.cpp +++ /dev/null @@ -1,313 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2021, PickNik LLC - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of PickNik LLC nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Title : servo_keyboard_input.cpp - * Project : moveit2_tutorials - * Created : 05/31/2021 - * Author : Adam Pettinger - */ - -#include -#include -#include - -#include -#include -#include -#include - -// Define used keys -#define KEYCODE_RIGHT 0x43 -#define KEYCODE_LEFT 0x44 -#define KEYCODE_UP 0x41 -#define KEYCODE_DOWN 0x42 -#define KEYCODE_PERIOD 0x2E -#define KEYCODE_SEMICOLON 0x3B -#define KEYCODE_1 0x31 -#define KEYCODE_2 0x32 -#define KEYCODE_3 0x33 -#define KEYCODE_4 0x34 -#define KEYCODE_5 0x35 -#define KEYCODE_6 0x36 -#define KEYCODE_7 0x37 -#define KEYCODE_Q 0x71 -#define KEYCODE_W 0x77 -#define KEYCODE_E 0x65 -#define KEYCODE_R 0x72 - -// Some constants used in the Servo Teleop demo -const std::string TWIST_TOPIC = "/servo_node/delta_twist_cmds"; -const std::string JOINT_TOPIC = "/servo_node/delta_joint_cmds"; -const size_t ROS_QUEUE_SIZE = 10; -const std::string EEF_FRAME_ID = "panda_hand"; -const std::string BASE_FRAME_ID = "panda_link0"; - -// A class for reading the key inputs from the terminal -class KeyboardReader -{ -public: - KeyboardReader() : kfd(0) - { - // get the console in raw mode - tcgetattr(kfd, &cooked); - struct termios raw; - memcpy(&raw, &cooked, sizeof(struct termios)); - raw.c_lflag &= ~(ICANON | ECHO); - // Setting a new line, then end of file - raw.c_cc[VEOL] = 1; - raw.c_cc[VEOF] = 2; - tcsetattr(kfd, TCSANOW, &raw); - } - void readOne(char* c) - { - int rc = read(kfd, c, 1); - if (rc < 0) - { - throw std::runtime_error("read failed"); - } - } - void shutdown() - { - tcsetattr(kfd, TCSANOW, &cooked); - } - -private: - int kfd; - struct termios cooked; -}; - -// Converts key-presses to Twist or Jog commands for Servo, in lieu of a controller -class KeyboardServo -{ -public: - KeyboardServo(); - int keyLoop(); - -private: - void spin(); - - rclcpp::Node::SharedPtr nh_; - - rclcpp::Publisher::SharedPtr twist_pub_; - rclcpp::Publisher::SharedPtr joint_pub_; - - std::string frame_to_publish_; - double joint_vel_cmd_; -}; - -KeyboardServo::KeyboardServo() : frame_to_publish_(BASE_FRAME_ID), joint_vel_cmd_(1.0) -{ - nh_ = rclcpp::Node::make_shared("servo_keyboard_input"); - - twist_pub_ = nh_->create_publisher(TWIST_TOPIC, ROS_QUEUE_SIZE); - joint_pub_ = nh_->create_publisher(JOINT_TOPIC, ROS_QUEUE_SIZE); -} - -KeyboardReader input; - -void quit(int sig) -{ - (void)sig; - input.shutdown(); - rclcpp::shutdown(); - exit(0); -} - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - KeyboardServo keyboard_servo; - - signal(SIGINT, quit); - - int rc = keyboard_servo.keyLoop(); - input.shutdown(); - rclcpp::shutdown(); - - return rc; -} - -void KeyboardServo::spin() -{ - while (rclcpp::ok()) - { - rclcpp::spin_some(nh_); - } -} - -int KeyboardServo::keyLoop() -{ - char c; - bool publish_twist = false; - bool publish_joint = false; - - std::thread{ std::bind(&KeyboardServo::spin, this) }.detach(); - - puts("Reading from keyboard"); - puts("---------------------------"); - puts("Use arrow keys and the '.' and ';' keys to Cartesian jog"); - puts("Use 'W' to Cartesian jog in the world frame, and 'E' for the End-Effector frame"); - puts("Use 1|2|3|4|5|6|7 keys to joint jog. 'R' to reverse the direction of jogging."); - puts("'Q' to quit."); - - for (;;) - { - // get the next event from the keyboard - try - { - input.readOne(&c); - } - catch (const std::runtime_error&) - { - perror("read():"); - return -1; - } - - RCLCPP_DEBUG(nh_->get_logger(), "value: 0x%02X\n", c); - - // // Create the messages we might publish - auto twist_msg = std::make_unique(); - auto joint_msg = std::make_unique(); - - // Use read key-press - switch (c) - { - case KEYCODE_LEFT: - RCLCPP_DEBUG(nh_->get_logger(), "LEFT"); - twist_msg->twist.linear.y = -1.0; - publish_twist = true; - break; - case KEYCODE_RIGHT: - RCLCPP_DEBUG(nh_->get_logger(), "RIGHT"); - twist_msg->twist.linear.y = 1.0; - publish_twist = true; - break; - case KEYCODE_UP: - RCLCPP_DEBUG(nh_->get_logger(), "UP"); - twist_msg->twist.linear.x = 1.0; - publish_twist = true; - break; - case KEYCODE_DOWN: - RCLCPP_DEBUG(nh_->get_logger(), "DOWN"); - twist_msg->twist.linear.x = -1.0; - publish_twist = true; - break; - case KEYCODE_PERIOD: - RCLCPP_DEBUG(nh_->get_logger(), "PERIOD"); - twist_msg->twist.linear.z = -1.0; - publish_twist = true; - break; - case KEYCODE_SEMICOLON: - RCLCPP_DEBUG(nh_->get_logger(), "SEMICOLON"); - twist_msg->twist.linear.z = 1.0; - publish_twist = true; - break; - case KEYCODE_E: - RCLCPP_DEBUG(nh_->get_logger(), "E"); - frame_to_publish_ = EEF_FRAME_ID; - break; - case KEYCODE_W: - RCLCPP_DEBUG(nh_->get_logger(), "W"); - frame_to_publish_ = BASE_FRAME_ID; - break; - case KEYCODE_1: - RCLCPP_DEBUG(nh_->get_logger(), "1"); - joint_msg->joint_names.push_back("panda_joint1"); - joint_msg->velocities.push_back(joint_vel_cmd_); - publish_joint = true; - break; - case KEYCODE_2: - RCLCPP_DEBUG(nh_->get_logger(), "2"); - joint_msg->joint_names.push_back("panda_joint2"); - joint_msg->velocities.push_back(joint_vel_cmd_); - publish_joint = true; - break; - case KEYCODE_3: - RCLCPP_DEBUG(nh_->get_logger(), "3"); - joint_msg->joint_names.push_back("panda_joint3"); - joint_msg->velocities.push_back(joint_vel_cmd_); - publish_joint = true; - break; - case KEYCODE_4: - RCLCPP_DEBUG(nh_->get_logger(), "4"); - joint_msg->joint_names.push_back("panda_joint4"); - joint_msg->velocities.push_back(joint_vel_cmd_); - publish_joint = true; - break; - case KEYCODE_5: - RCLCPP_DEBUG(nh_->get_logger(), "5"); - joint_msg->joint_names.push_back("panda_joint5"); - joint_msg->velocities.push_back(joint_vel_cmd_); - publish_joint = true; - break; - case KEYCODE_6: - RCLCPP_DEBUG(nh_->get_logger(), "6"); - joint_msg->joint_names.push_back("panda_joint6"); - joint_msg->velocities.push_back(joint_vel_cmd_); - publish_joint = true; - break; - case KEYCODE_7: - RCLCPP_DEBUG(nh_->get_logger(), "7"); - joint_msg->joint_names.push_back("panda_joint7"); - joint_msg->velocities.push_back(joint_vel_cmd_); - publish_joint = true; - break; - case KEYCODE_R: - RCLCPP_DEBUG(nh_->get_logger(), "R"); - joint_vel_cmd_ *= -1; - break; - case KEYCODE_Q: - RCLCPP_DEBUG(nh_->get_logger(), "quit"); - return 0; - } - - // If a key requiring a publish was pressed, publish the message now - if (publish_twist) - { - twist_msg->header.stamp = nh_->now(); - twist_msg->header.frame_id = frame_to_publish_; - twist_pub_->publish(std::move(twist_msg)); - publish_twist = false; - } - else if (publish_joint) - { - joint_msg->header.stamp = nh_->now(); - joint_msg->header.frame_id = BASE_FRAME_ID; - joint_pub_->publish(std::move(joint_msg)); - publish_joint = false; - } - } - - return 0; -}