diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 3c303511..f3cc4e47 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -1,6 +1,13 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package LBR FRI ROS 2 Stack ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Humble v2.1.2 (2024-10-18) +-------------------------- +* Adds MoveIt Servo demo, related to https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/50 and https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/211 + + * ``lbr_bringup``: New launch mixin and launch file for MoveIt Servo + * ``lbr_moveit``: New keyboard driver to interface with MoveIt Servo + Humble v2.1.1 (2024-09-27) -------------------------- * Adds support for the new Gazebo and removes support for Gazebo Classic (End-of-Life January 2025, refer https://community.gazebosim.org/t/gazebo-classic-end-of-life/2563). diff --git a/CITATION.cff b/CITATION.cff index 83e295ed..203549cd 100644 --- a/CITATION.cff +++ b/CITATION.cff @@ -19,6 +19,6 @@ authors: title: "LBR-Stack: ROS 2 and Python Integration of KUKA FRI for Med and IIWA Robots" -version: 2.1.1 +version: 2.1.2 doi: 10.48550/arXiv.2311.12709 -date-released: 2024-09-27 +date-released: 2024-10-18 diff --git a/lbr_bringup/config/moveit_servo.yaml b/lbr_bringup/config/moveit_servo.yaml new file mode 100644 index 00000000..9d292fba --- /dev/null +++ b/lbr_bringup/config/moveit_servo.yaml @@ -0,0 +1,72 @@ +# Please do e.g. refer to +# - https://github.com/moveit/moveit2/blob/humble/moveit_ros/moveit_servo/config/panda_simulated_config.yaml +# - https://github.com/moveit/moveit2/blob/humble/moveit_ros/moveit_servo/config/panda_simulated_config_pose_tracking.yaml +/**/servo_node: + ros__parameters: + moveit_servo: + ## Properties of incoming commands + command_in_type: "unitless" # "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 + + # 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.005 # #1/controller manager update 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: true + + ## MoveIt properties + move_group_name: arm # Often 'manipulator' or 'arm' + planning_frame: link_0 # The MoveIt planning frame. Often 'base_link' or 'world' + + ## Other frames + ee_frame_name: link_ee # The name of the end effector link, used to return the EE pose + robot_link_command_frame: link_0 # 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: 4 + + ## 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: /lbr/joint_states + status_topic: ~/status # Publish status to this topic + command_out_topic: /lbr/forward_position_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/lbr_bringup/launch/move_group.launch.py b/lbr_bringup/launch/move_group.launch.py index 8a966915..7d71292a 100644 --- a/lbr_bringup/launch/move_group.launch.py +++ b/lbr_bringup/launch/move_group.launch.py @@ -5,7 +5,7 @@ from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from lbr_bringup.description import LBRDescriptionMixin -from lbr_bringup.move_group import LBRMoveGroupMixin +from lbr_bringup.moveit import LBRMoveGroupMixin from lbr_bringup.rviz import RVizMixin diff --git a/lbr_bringup/launch/moveit_servo.launch.py b/lbr_bringup/launch/moveit_servo.launch.py index e69de29b..400a7258 100644 --- a/lbr_bringup/launch/moveit_servo.launch.py +++ b/lbr_bringup/launch/moveit_servo.launch.py @@ -0,0 +1,72 @@ +from typing import List + +from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity +from launch.actions import OpaqueFunction, RegisterEventHandler +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessStart +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from lbr_bringup.description import LBRDescriptionMixin +from lbr_bringup.moveit import LBRMoveGroupMixin, LBRMoveItServoMixin + + +def hidden_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: + ld = LaunchDescription() + + moveit_servo_config = PathJoinSubstitution( + [FindPackageShare("lbr_bringup"), "config/moveit_servo.yaml"] + ) + model = LaunchConfiguration("model").perform(context) + moveit_configs = LBRMoveGroupMixin.moveit_configs_builder( + robot_name=model, + package_name=f"{model}_moveit_config", + ).to_moveit_configs() + + mode = LaunchConfiguration("mode").perform(context) + use_sim_time = False + if mode == "gazebo": + use_sim_time = True + + # moveit servo node + servo_node = LBRMoveItServoMixin.node_moveit_servo( + parameters=[ + moveit_configs.robot_description_kinematics, + moveit_configs.robot_description_semantic, + {"use_sim_time": use_sim_time}, + moveit_servo_config, + { + "moveit_servo.use_gazebo": mode + == "gazebo", # we configure this parameter dynamically + }, + ], + ) + ld.add_action(servo_node) + + # call start servo after servo node start + ld.add_action( + RegisterEventHandler( + OnProcessStart( + target_action=servo_node, + on_start=[ + LBRMoveItServoMixin.call_start_servo_service( + condition=IfCondition( + LaunchConfiguration("default_enable_servo") + ) + ) + ], + ), + ) + ) + + return ld.entities + + +def generate_launch_description() -> LaunchDescription: + ld = LaunchDescription() + + ld.add_action(LBRDescriptionMixin.arg_mode()) + ld.add_action(LBRDescriptionMixin.arg_model()) + ld.add_action(LBRMoveItServoMixin.arg_default_enable_servo()) + + ld.add_action(OpaqueFunction(function=hidden_setup)) + return ld diff --git a/lbr_bringup/lbr_bringup/move_group.py b/lbr_bringup/lbr_bringup/moveit.py similarity index 72% rename from lbr_bringup/lbr_bringup/move_group.py rename to lbr_bringup/lbr_bringup/moveit.py index ef0b4682..f2a69fbf 100644 --- a/lbr_bringup/lbr_bringup/move_group.py +++ b/lbr_bringup/lbr_bringup/moveit.py @@ -1,17 +1,17 @@ import os -from typing import Any, Dict, List +from typing import Any, Dict, List, Optional, Union from ament_index_python import get_package_share_directory -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument, ExecuteProcess +from launch.substitutions import ( + FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, +) from launch_ros.actions import Node from launch_ros.parameter_descriptions import ParameterValue from moveit_configs_utils import MoveItConfigs, MoveItConfigsBuilder -# NOTE TO SELF: -# due to individual moveit configs, put mixins into lbr_bringup rather than lbr_moveit_config -# most of the configs are taken from Python package moveit_configs_utils.launches - class LBRMoveGroupMixin: @staticmethod @@ -118,3 +118,46 @@ def node_move_group(**kwargs) -> Node: output="screen", **kwargs, ) + + +class LBRMoveItServoMixin: + @staticmethod + def arg_default_enable_servo() -> DeclareLaunchArgument: + return DeclareLaunchArgument( + name="default_enable_servo", + default_value="true", + description="Whether to enable the servo node by default.", + ) + + @staticmethod + def node_moveit_servo( + robot_name: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( + "robot_name", default="lbr" + ), + **kwargs, + ) -> Node: + return Node( + package="moveit_servo", + executable="servo_node_main", + output="screen", + namespace=robot_name, + **kwargs, + ) + + @staticmethod + def call_start_servo_service( + robot_name: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( + "robot_name", default="lbr" + ), + **kwargs, + ) -> ExecuteProcess: + return ExecuteProcess( + cmd=[ + FindExecutable(name="ros2"), + "service", + "call", + PathJoinSubstitution([robot_name, "servo_node/start_servo"]), + "std_srvs/srv/Trigger", + ], + **kwargs, + ) diff --git a/lbr_bringup/package.xml b/lbr_bringup/package.xml index dba3730d..5b351434 100644 --- a/lbr_bringup/package.xml +++ b/lbr_bringup/package.xml @@ -2,10 +2,10 @@ lbr_bringup - 2.1.1 + 2.1.2 LBR launch files. mhubii - Apache License 2.0 + Apache-2.0 ament_cmake ament_cmake_python @@ -17,6 +17,8 @@ lbr_description lbr_fri_ros2 lbr_ros2_control + moveit_ros_move_group + moveit_servo rclpy robot_state_publisher ros_gz_sim diff --git a/lbr_demos/doc/lbr_demos.rst b/lbr_demos/doc/lbr_demos.rst index 7bcbf9e9..8a8b431f 100644 --- a/lbr_demos/doc/lbr_demos.rst +++ b/lbr_demos/doc/lbr_demos.rst @@ -29,8 +29,8 @@ MoveIt .. toctree:: :titlesonly: - lbr_moveit2_py <../lbr_moveit_py/doc/lbr_moveit_py.rst> - lbr_moveit2_cpp <../lbr_moveit_cpp/doc/lbr_moveit_cpp.rst> + lbr_moveit <../lbr_moveit/doc/lbr_moveit.rst> + lbr_moveit_cpp <../lbr_moveit_cpp/doc/lbr_moveit_cpp.rst> Integration ----------- diff --git a/lbr_demos/lbr_demos_advanced_cpp/package.xml b/lbr_demos/lbr_demos_advanced_cpp/package.xml index 069d424a..a1bcf8b9 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/package.xml +++ b/lbr_demos/lbr_demos_advanced_cpp/package.xml @@ -2,10 +2,10 @@ lbr_demos_advanced_cpp - 2.1.1 + 2.1.2 Advanced C++ demos for the lbr_ros2_control. mhubii - Apache License 2.0 + Apache-2.0 ament_cmake diff --git a/lbr_demos/lbr_demos_advanced_py/package.xml b/lbr_demos/lbr_demos_advanced_py/package.xml index 3a646a59..c485e4ac 100644 --- a/lbr_demos/lbr_demos_advanced_py/package.xml +++ b/lbr_demos/lbr_demos_advanced_py/package.xml @@ -2,11 +2,11 @@ lbr_demos_advanced_py - 2.1.1 + 2.1.2 Advanced Python demos for the lbr_ros2_control. mhubii cmower - Apache License 2.0 + Apache-2.0 lbr_description lbr_fri_idl diff --git a/lbr_demos/lbr_demos_advanced_py/setup.py b/lbr_demos/lbr_demos_advanced_py/setup.py index a3ab1449..c465713a 100644 --- a/lbr_demos/lbr_demos_advanced_py/setup.py +++ b/lbr_demos/lbr_demos_advanced_py/setup.py @@ -6,7 +6,7 @@ setup( name=package_name, - version="2.1.1", + version="2.1.2", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/lbr_demos/lbr_demos_cpp/package.xml b/lbr_demos/lbr_demos_cpp/package.xml index 7172ae22..6ed61d1a 100644 --- a/lbr_demos/lbr_demos_cpp/package.xml +++ b/lbr_demos/lbr_demos_cpp/package.xml @@ -2,10 +2,10 @@ lbr_demos_cpp - 2.1.1 + 2.1.2 C++ demos for lbr_ros2_control. mhubii - Apache License 2.0 + Apache-2.0 ament_cmake diff --git a/lbr_demos/lbr_demos_py/package.xml b/lbr_demos/lbr_demos_py/package.xml index dadc747e..3d0ca085 100644 --- a/lbr_demos/lbr_demos_py/package.xml +++ b/lbr_demos/lbr_demos_py/package.xml @@ -2,10 +2,10 @@ lbr_demos_py - 2.1.1 + 2.1.2 Python demos for lbr_ros2_control. mhubii - Apache License 2.0 + Apache-2.0 control_msgs lbr_bringup diff --git a/lbr_demos/lbr_demos_py/setup.py b/lbr_demos/lbr_demos_py/setup.py index a38dae9e..76198e83 100644 --- a/lbr_demos/lbr_demos_py/setup.py +++ b/lbr_demos/lbr_demos_py/setup.py @@ -4,7 +4,7 @@ setup( name=package_name, - version="2.1.1", + version="2.1.2", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/lbr_demos/lbr_moveit/config/forward_keyboard.yaml b/lbr_demos/lbr_moveit/config/forward_keyboard.yaml new file mode 100644 index 00000000..bd51e1e0 --- /dev/null +++ b/lbr_demos/lbr_moveit/config/forward_keyboard.yaml @@ -0,0 +1,52 @@ +/**/forward_keyboard: + ros__parameters: + velocity_scales: + translation: + x: 1.0 + y: 1.0 + z: 1.0 + rotation: + x: 1.0 + y: 1.0 + z: 1.0 + joints: + - 1.0 + - 1.0 + - 1.0 + - 1.0 + - 1.0 + - 1.0 + - 1.0 + keyboard_layout: + # uses pynput: https://pypi.org/project/pynput/ + translation: + x: + increase: "w" + decrease: "s" + y: + increase: "a" + decrease: "d" + z: + increase: "Key.up" + decrease: "Key.down" + rotation: + x: + increase: "u" + decrease: "j" + y: + increase: "h" + decrease: "k" + z: + increase: "Key.left" + decrease: "Key.right" + joints: + - "1" + - "2" + - "3" + - "4" + - "5" + - "6" + - "7" + escape: "Key.esc" + pause: "p" + reverse_joints: "r" diff --git a/lbr_demos/lbr_moveit_py/doc/img/iiwa7_moveit_rviz.png b/lbr_demos/lbr_moveit/doc/img/iiwa7_moveit_rviz.png similarity index 100% rename from lbr_demos/lbr_moveit_py/doc/img/iiwa7_moveit_rviz.png rename to lbr_demos/lbr_moveit/doc/img/iiwa7_moveit_rviz.png diff --git a/lbr_demos/lbr_moveit/doc/lbr_moveit.rst b/lbr_demos/lbr_moveit/doc/lbr_moveit.rst new file mode 100644 index 00000000..97c2d632 --- /dev/null +++ b/lbr_demos/lbr_moveit/doc/lbr_moveit.rst @@ -0,0 +1,140 @@ +lbr_moveit +========== +.. warning:: + On hardware, do always execute in ``T1`` mode first. + +.. contents:: Table of Contents + :depth: 2 + :local: + :backlinks: none + +MoveIt Servo +------------ + +MoveIt Servo - Simulation +~~~~~~~~~~~~~~~~~~~~~~~~~ +#. Run the mock setup (important to run the ``forward_position_controller`` controller for servoing): + + .. code-block:: bash + + ros2 launch lbr_bringup mock.launch.py \ + ctrl:=forward_position_controller \ + model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + + .. hint:: + + For a physics-based simulation, also try Gazebo (remember to set ``mode:=gazebo`` for the next steps): + + .. code-block:: bash + + ros2 launch lbr_bringup gazebo.launch.py \ + model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + +#. Run MoveIt Servo: + + .. code-block:: bash + + ros2 launch lbr_bringup moveit_servo.launch.py \ + mode:=mock \ + model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + + +#. Optionally run RViZ: + + .. code-block:: bash + + ros2 launch lbr_bringup rviz.launch.py + +#. Publish to ``/lbr/servo_node/delta_joint_cmds`` and ``/lbr/servo_node/delta_twist_cmds``. For this demo, we provide a keyboard driver (keyboard layout is printed to terminal): + + .. code-block:: bash + + ros2 launch lbr_moveit keyboard_driver.launch.py + +You can now experiment with + +- Modifying the MoveIt Servo parameters in `moveit_servo.yaml `_:octicon:`link-external`. E.g. the ``robot_link_command_frame`` to change the commanding frame. +- Connect a joystick or game controller. +- Or changing the veloctiy scales for this keyboard driver in `forward_keyboard.yaml `_:octicon:`link-external`. + +MoveIt Servo - Hardware +~~~~~~~~~~~~~~~~~~~~~~~ +#. Client side configurations: + + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + +#. Remote side configurations: + + #. .. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD`` + + .. thumbnail:: ../../doc/img/applications_lbr_server.png + + #. Select + + - ``FRI send period``: ``10 ms`` + - ``IP address``: ``your configuration`` + - ``FRI control mode``: ``POSITION_CONTROL`` or ``JOINT_IMPEDANCE_CONTROL`` + - ``FRI client command mode``: ``POSITION`` + +#. Proceed with steps 1, 2, 3 and 4 from `MoveIt Servo - Simulation`_ but with ``ros2 launch lbr_bringup hardware.launch.py`` in step 1. + +MoveIt via RViz +--------------- +.. image:: img/iiwa7_moveit_rviz.png + :align: center + :alt: MoveIt via RViz +**IIWA 7 R800 in RViz** + +To run MoveIt via RViz, simply follow: + +MoveIt via RViz - Simulation +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +#. Run the mock setup: + + .. code-block:: bash + + ros2 launch lbr_bringup mock.launch.py \ + model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + + .. hint:: + + For a physics-based simulation, also try Gazebo (remember to set ``mode:=gazebo`` for the next steps): + + .. code-block:: bash + + ros2 launch lbr_bringup gazebo.launch.py \ + model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + +#. Run MoveIt with RViz: + + .. code-block:: bash + + ros2 launch lbr_bringup move_group.launch.py \ + mode:=mock \ + rviz:=true \ + model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + +#. You can now move the robot via MoveIt in RViz! + +MoveIt via RViz - Hardware +~~~~~~~~~~~~~~~~~~~~~~~~~~ +#. Client side configurations: + + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + +#. Remote side configurations: + + #. .. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD`` + + .. thumbnail:: ../../doc/img/applications_lbr_server.png + + #. Select + + - ``FRI send period``: ``10 ms`` + - ``IP address``: ``your configuration`` + - ``FRI control mode``: ``POSITION_CONTROL`` or ``JOINT_IMPEDANCE_CONTROL`` + - ``FRI client command mode``: ``POSITION`` + +#. Proceed with steps 1 and 2 from `MoveIt via RViz - Simulation`_ but with ``ros2 launch lbr_bringup hardware.launch.py`` in step 1. diff --git a/lbr_demos/lbr_moveit/launch/keyboard_driver.launch.py b/lbr_demos/lbr_moveit/launch/keyboard_driver.launch.py new file mode 100644 index 00000000..c2368065 --- /dev/null +++ b/lbr_demos/lbr_moveit/launch/keyboard_driver.launch.py @@ -0,0 +1,47 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from lbr_bringup.description import LBRDescriptionMixin + + +def generate_launch_description() -> LaunchDescription: + ld = LaunchDescription() + + # launch arguments + ld.add_action( + DeclareLaunchArgument( + name="keyboard_config_pkg", + default_value="lbr_moveit", + description="The package containing the keyboard configurations.", + ) + ) + ld.add_action( + DeclareLaunchArgument( + name="keyboard_config", + default_value="config/forward_keyboard.yaml", + description="Location of the keyboard configuration file relative to keyboard_config_pkg.", + ) + ) + ld.add_action(LBRDescriptionMixin.arg_robot_name()) + + # forward keyboard node + keyboard_driver_config = PathJoinSubstitution( + [ + FindPackageShare(LaunchConfiguration("keyboard_config_pkg")), + LaunchConfiguration("keyboard_config"), + ] + ) + ld.add_action( + Node( + package="lbr_moveit", + executable="forward_keyboard", + output="screen", + parameters=[ + keyboard_driver_config, + ], + namespace=LaunchConfiguration("robot_name"), + ) + ) + return ld diff --git a/lbr_demos/lbr_moveit/lbr_moveit/__init__.py b/lbr_demos/lbr_moveit/lbr_moveit/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/lbr_demos/lbr_moveit/lbr_moveit/forward_keyboard_node.py b/lbr_demos/lbr_moveit/lbr_moveit/forward_keyboard_node.py new file mode 100644 index 00000000..3af1d950 --- /dev/null +++ b/lbr_demos/lbr_moveit/lbr_moveit/forward_keyboard_node.py @@ -0,0 +1,384 @@ +from dataclasses import dataclass + +import numpy as np +import rclpy +from control_msgs.msg import JointJog +from geometry_msgs.msg import TwistStamped +from rclpy.node import Node +from rclpy.qos import qos_profile_sensor_data +from sensor_msgs.msg import JointState + + +class ForwardKeyboardNode(Node): + @dataclass + class VeloctiyScales: + @dataclass + class Translation: + x: float = 0.0 + y: float = 0.0 + z: float = 0.0 + + @dataclass + class Rotation: + x: float = 0.0 + y: float = 0.0 + z: float = 0.0 + + joints: list + translation: Translation = Translation() + rotation: Rotation = Rotation() + + @dataclass + class KeyboardLayout: + @dataclass + class Translation: + @dataclass + class X: + increase: str = "w" + decrease: str = "s" + + @dataclass + class Y: + increase: str = "a" + decrease: str = "d" + + @dataclass + class Z: + increase: str = "q" + decrease: str = "e" + + x: X = X() + y: Y = Y() + z: Z = Z() + + @dataclass + class Rotation: + @dataclass + class X: + increase: str = "r" + decrease: str = "f" + + @dataclass + class Y: + increase: str = "t" + decrease: str = "g" + + @dataclass + class Z: + increase: str = "y" + decrease: str = "h" + + x: X = X() + y: Y = Y() + z: Z = Z() + + joints: list + translation: Translation = Translation() + rotation: Rotation = Rotation() + escape: str = "Key.esc" + pause: str = "p" + reverse_joints: str = "r" + + def __init__(self): + super().__init__("forward_keyboard") + self._joint_cmd_pub = self.create_publisher( + JointJog, "servo_node/delta_joint_cmds", qos_profile_sensor_data + ) + self._twist_cmd_pub = self.create_publisher( + TwistStamped, "servo_node/delta_twist_cmds", qos_profile_sensor_data + ) + self._joint_state_sub = self.create_subscription( + JointState, + "joint_states", + self._on_joint_state, + qos_profile_sensor_data, + ) + self._cmd_timer = self.create_timer(0.1, self._on_cmd_timer) + + self._joint_state = None + self._dof = None + self._twist_cmd = TwistStamped() + self._joint_cmd = JointJog() + + while not self._joint_state: + self.get_logger().info( + f"Waiting for joint state on {self._joint_state_sub.topic_name}..." + ) + rclpy.spin_once(self, timeout_sec=1.0) + self._veloctiy_scales = self.VeloctiyScales(joints=[0.0] * self._dof) + self._keyboard_layout = self.KeyboardLayout( + joints=[str(i) for i in range(self._dof)] + ) + self._declare_parameters() + self._get_parameters() + + @property + def joint_state(self) -> JointState: + return self._joint_state + + @property + def dof(self) -> int: + return self._dof + + @property + def twist_cmd(self) -> np.ndarray: + return np.array( + [ + self._twist_cmd.twist.linear.x, + self._twist_cmd.twist.linear.y, + self._twist_cmd.twist.linear.z, + self._twist_cmd.twist.angular.x, + self._twist_cmd.twist.angular.y, + self._twist_cmd.twist.angular.z, + ] + ) + + @property + def keyboard_layout(self) -> KeyboardLayout: + return self._keyboard_layout + + def _declare_parameters(self): + # veloctiy scales + self.declare_parameters( + namespace="", + parameters=[ + ("velocity_scales.joints", [0.1] * self._dof), + ("velocity_scales.translation.x", 0.1), + ("velocity_scales.translation.y", 0.1), + ("velocity_scales.translation.z", 0.1), + ("velocity_scales.rotation.x", 0.1), + ("velocity_scales.rotation.y", 0.1), + ("velocity_scales.rotation.z", 0.1), + ], + ) + + # keyboard layout + self.declare_parameters( + namespace="", + parameters=[ + ("keyboard_layout.joints", [str(i) for i in range(self._dof)]), + ("keyboard_layout.translation.x.increase", "w"), + ("keyboard_layout.translation.x.decrease", "s"), + ("keyboard_layout.translation.y.increase", "a"), + ("keyboard_layout.translation.y.decrease", "d"), + ("keyboard_layout.translation.z.increase", "Key.up"), + ("keyboard_layout.translation.z.decrease", "Key.down"), + ("keyboard_layout.rotation.x.increase", "u"), + ("keyboard_layout.rotation.x.decrease", "j"), + ("keyboard_layout.rotation.y.increase", "h"), + ("keyboard_layout.rotation.y.decrease", "k"), + ("keyboard_layout.rotation.z.increase", "Key.left"), + ("keyboard_layout.rotation.z.decrease", "Key.right"), + ("keyboard_layout.escape", "Key.esc"), + ("keyboard_layout.pause", "p"), + ("keyboard_layout.reverse_joints", "r"), + ], + ) + + def _get_parameters(self): + # veloctiy scales + self._veloctiy_scales.joints = ( + self.get_parameter("velocity_scales.joints") + .get_parameter_value() + .double_array_value + ) + if len(self._veloctiy_scales.joints) != self._dof: + raise ValueError( + f"Number of joint velocity scales ({len(self._veloctiy_scales.joints)}) " + f"does not match the number of joints ({self._dof})." + ) + self._veloctiy_scales.translation.x = ( + self.get_parameter("velocity_scales.translation.x") + .get_parameter_value() + .double_value + ) + self._veloctiy_scales.translation.y = ( + self.get_parameter("velocity_scales.translation.y") + .get_parameter_value() + .double_value + ) + self._veloctiy_scales.translation.z = ( + self.get_parameter("velocity_scales.translation.z") + .get_parameter_value() + .double_value + ) + self._veloctiy_scales.rotation.x = ( + self.get_parameter("velocity_scales.rotation.x") + .get_parameter_value() + .double_value + ) + self._veloctiy_scales.rotation.y = ( + self.get_parameter("velocity_scales.rotation.y") + .get_parameter_value() + .double_value + ) + self._veloctiy_scales.rotation.z = ( + self.get_parameter("velocity_scales.rotation.z") + .get_parameter_value() + .double_value + ) + + # keyboard layout + self._keyboard_layout.joints = ( + self.get_parameter("keyboard_layout.joints") + .get_parameter_value() + .string_array_value + ) + self._keyboard_layout.translation.x.increase = ( + self.get_parameter("keyboard_layout.translation.x.increase") + .get_parameter_value() + .string_value + ) + self._keyboard_layout.translation.x.decrease = ( + self.get_parameter("keyboard_layout.translation.x.decrease") + .get_parameter_value() + .string_value + ) + self._keyboard_layout.translation.y.increase = ( + self.get_parameter("keyboard_layout.translation.y.increase") + .get_parameter_value() + .string_value + ) + self._keyboard_layout.translation.y.decrease = ( + self.get_parameter("keyboard_layout.translation.y.decrease") + .get_parameter_value() + .string_value + ) + self._keyboard_layout.translation.z.increase = ( + self.get_parameter("keyboard_layout.translation.z.increase") + .get_parameter_value() + .string_value + ) + self._keyboard_layout.translation.z.decrease = ( + self.get_parameter("keyboard_layout.translation.z.decrease") + .get_parameter_value() + .string_value + ) + self._keyboard_layout.rotation.x.increase = ( + self.get_parameter("keyboard_layout.rotation.x.increase") + .get_parameter_value() + .string_value + ) + self._keyboard_layout.rotation.x.decrease = ( + self.get_parameter("keyboard_layout.rotation.x.decrease") + .get_parameter_value() + .string_value + ) + self._keyboard_layout.rotation.y.increase = ( + self.get_parameter("keyboard_layout.rotation.y.increase") + .get_parameter_value() + .string_value + ) + self._keyboard_layout.rotation.y.decrease = ( + self.get_parameter("keyboard_layout.rotation.y.decrease") + .get_parameter_value() + .string_value + ) + self._keyboard_layout.rotation.z.increase = ( + self.get_parameter("keyboard_layout.rotation.z.increase") + .get_parameter_value() + .string_value + ) + self._keyboard_layout.rotation.z.decrease = ( + self.get_parameter("keyboard_layout.rotation.z.decrease") + .get_parameter_value() + .string_value + ) + self._keyboard_layout.escape = ( + self.get_parameter("keyboard_layout.escape") + .get_parameter_value() + .string_value + ) + self._keyboard_layout.pause = ( + self.get_parameter("keyboard_layout.pause") + .get_parameter_value() + .string_value + ) + self._keyboard_layout.reverse_joints = ( + self.get_parameter("keyboard_layout.reverse_joints") + .get_parameter_value() + .string_value + ) + + self.get_logger().info("Parameters:") + # veloctiy scales + self.get_logger().info(" Velocity Scales:") + self.get_logger().info(" Translation:") + self.get_logger().info(f" x: {self._veloctiy_scales.translation.x}") + self.get_logger().info(f" y: {self._veloctiy_scales.translation.y}") + self.get_logger().info(f" z: {self._veloctiy_scales.translation.z}") + self.get_logger().info(" Rotation:") + self.get_logger().info(f" x: {self._veloctiy_scales.rotation.x}") + self.get_logger().info(f" y: {self._veloctiy_scales.rotation.y}") + self.get_logger().info(f" z: {self._veloctiy_scales.rotation.z}") + self.get_logger().info(f" Joints: {self._veloctiy_scales.joints.tolist()}") + + # keyboard layout + self.get_logger().info(" Keyboard Layout:") + self.get_logger().info(" Translation:") + self.get_logger().info( + f" x: {self._keyboard_layout.translation.x.increase} / {self._keyboard_layout.translation.x.decrease}" + ) + self.get_logger().info( + f" y: {self._keyboard_layout.translation.y.increase} / {self._keyboard_layout.translation.y.decrease}" + ) + self.get_logger().info( + f" z: {self._keyboard_layout.translation.z.increase} / {self._keyboard_layout.translation.z.decrease}" + ) + self.get_logger().info(" Rotation:") + self.get_logger().info( + f" x: {self._keyboard_layout.rotation.x.increase} / {self._keyboard_layout.rotation.x.decrease}" + ) + self.get_logger().info( + f" y: {self._keyboard_layout.rotation.y.increase} / {self._keyboard_layout.rotation.y.decrease}" + ) + self.get_logger().info( + f" z: {self._keyboard_layout.rotation.z.increase} / {self._keyboard_layout.rotation.z.decrease}" + ) + self.get_logger().info(f" Joints: {self._keyboard_layout.joints}") + self.get_logger().info(f" Escape: {self._keyboard_layout.escape}") + self.get_logger().info(f" Pause: {self._keyboard_layout.pause}") + self.get_logger().info( + f" Reverse Joints: {self._keyboard_layout.reverse_joints}" + ) + + def _on_cmd_timer(self) -> None: + self._twist_cmd.header.stamp = self.get_clock().now().to_msg() + self._twist_cmd_pub.publish(self._twist_cmd) + self._joint_cmd.header.stamp = self.get_clock().now().to_msg() + self._joint_cmd_pub.publish(self._joint_cmd) + + @twist_cmd.setter + def twist_cmd(self, twist: np.ndarray) -> None: + if len(twist) != 6: + raise ValueError("Twist command must be a 6-element array.") + self._twist_cmd.twist.linear.x = twist[0] * self._veloctiy_scales.translation.x + self._twist_cmd.twist.linear.y = twist[1] * self._veloctiy_scales.translation.y + self._twist_cmd.twist.linear.z = twist[2] * self._veloctiy_scales.translation.z + self._twist_cmd.twist.angular.x = twist[3] * self._veloctiy_scales.rotation.x + self._twist_cmd.twist.angular.y = twist[4] * self._veloctiy_scales.rotation.y + self._twist_cmd.twist.angular.z = twist[5] * self._veloctiy_scales.rotation.z + + @property + def joint_veloctiy_cmd(self) -> np.ndarray: + return np.array(self._joint_cmd.velocities) + + @joint_veloctiy_cmd.setter + def joint_veloctiy_cmd(self, velocities: np.ndarray) -> None: + if not self._joint_state: + return + if len(velocities) != self._dof: + raise ValueError(f"Joint command must be a {self._dof}-element array.") + + self._joint_cmd.joint_names = self._joint_state.name + # account for potentially unsorted joint state broadcaster + index_map = np.argsort(self._joint_cmd.joint_names) + self._joint_cmd.velocities = velocities[index_map].tolist() + self._joint_cmd.velocities = [ + v * s + for v, s in zip(self._joint_cmd.velocities, self._veloctiy_scales.joints) + ] + + def _on_joint_state(self, msg: JointState): + self._joint_state = msg + self._dof = len(self._joint_state.name) diff --git a/lbr_demos/lbr_moveit/lbr_moveit/keyboard_listener.py b/lbr_demos/lbr_moveit/lbr_moveit/keyboard_listener.py new file mode 100644 index 00000000..6015f504 --- /dev/null +++ b/lbr_demos/lbr_moveit/lbr_moveit/keyboard_listener.py @@ -0,0 +1,192 @@ +import numpy as np +from pynput.keyboard import Key, Listener + +from .forward_keyboard_node import ForwardKeyboardNode + + +class KeyboardListener: + def __init__(self, command_forward_node: ForwardKeyboardNode): + self._command_forward_node = command_forward_node + self._key_listener = Listener( + on_press=self._on_key_press, on_release=self._on_key_release + ) + self._valid_numbers = None + self._twist_cmd = np.zeros(6) + self._joint_velocity_cmd = None + self._joint_vel_direction = 1.0 + + def __enter__(self): + self._key_listener.start() + + def __exit__(self, exc_type, exc_value, traceback): + self._command_forward_node.get_logger().info("Exiting keyboard listener with.") + self._key_listener.stop() + + def _on_key_release(self, key: Key) -> None: + key_str = str(key).replace("'", "") + # zero out the velocity commands + try: + if ( + key_str + == self._command_forward_node.keyboard_layout.translation.z.increase + ): + self._twist_cmd[2] = 0.0 # translation along z-axis + elif ( + key_str + == self._command_forward_node.keyboard_layout.translation.z.decrease + ): + self._twist_cmd[2] = 0.0 + elif ( + key_str + == self._command_forward_node.keyboard_layout.rotation.z.increase + ): + self._twist_cmd[5] = 0.0 # rotation about z-axis + elif ( + key_str + == self._command_forward_node.keyboard_layout.rotation.z.decrease + ): + self._twist_cmd[5] = 0.0 + elif key_str == self._command_forward_node.keyboard_layout.pause: # pause + self._twist_cmd = np.zeros(6) + self._joint_velocity_cmd = np.zeros(self._command_forward_node.dof) + elif ( + key_str + == self._command_forward_node.keyboard_layout.translation.x.increase + ): + self._twist_cmd[0] = 0.0 # translation along x-axis + elif ( + key_str + == self._command_forward_node.keyboard_layout.translation.x.decrease + ): + self._twist_cmd[0] = 0.0 + elif ( + key_str + == self._command_forward_node.keyboard_layout.translation.y.increase + ): + self._twist_cmd[1] = 0.0 # translation along y-axis + elif ( + key_str + == self._command_forward_node.keyboard_layout.translation.y.decrease + ): + self._twist_cmd[1] = 0.0 + elif ( + key_str + == self._command_forward_node.keyboard_layout.rotation.x.increase + ): + self._twist_cmd[3] = 0.0 # rotation about x-axis + elif ( + key_str + == self._command_forward_node.keyboard_layout.rotation.x.decrease + ): + self._twist_cmd[3] = 0.0 + elif ( + key_str + == self._command_forward_node.keyboard_layout.rotation.y.increase + ): + self._twist_cmd[4] = 0.0 # rotation about y-axis + elif ( + key_str + == self._command_forward_node.keyboard_layout.rotation.y.decrease + ): + self._twist_cmd[4] = 0.0 + elif key_str in self._valid_numbers: + # get index of key_str within valid_numbers + index = self._valid_numbers.index(key_str) + if 0 <= index < self._command_forward_node.dof: + self._joint_velocity_cmd[index] = 0.0 + self._command_forward_node.twist_cmd = self._twist_cmd + self._command_forward_node.joint_veloctiy_cmd = self._joint_velocity_cmd + except AttributeError: + pass + + def _on_key_press(self, key: Key) -> None: + key_str = str(key).replace("'", "") + if key_str == self._command_forward_node.keyboard_layout.escape: + self._command_forward_node.get_logger().info("Exiting keyboard listener.") + self._twist_cmd = np.zeros(6) + self._joint_velocity_cmd = np.zeros(self._command_forward_node.dof) + self._command_forward_node.twist_cmd = self._twist_cmd + self._command_forward_node.joint_veloctiy_cmd = self._joint_velocity_cmd + self._key_listener.stop() + if not self._command_forward_node.dof: + return + if not self._valid_numbers: + self._valid_numbers = "".join( + [str(i) for i in self._command_forward_node.keyboard_layout.joints] + ) + self._joint_velocity_cmd = np.zeros(self._command_forward_node.dof) + try: + if ( + key_str + == self._command_forward_node.keyboard_layout.translation.z.increase + ): + self._twist_cmd[2] = 1.0 # translation along z-axis + elif ( + key_str + == self._command_forward_node.keyboard_layout.translation.z.decrease + ): + self._twist_cmd[2] = -1.0 + elif ( + key_str + == self._command_forward_node.keyboard_layout.rotation.z.increase + ): + self._twist_cmd[5] = 1.0 # rotation about z-axis + elif ( + key_str + == self._command_forward_node.keyboard_layout.rotation.z.decrease + ): + self._twist_cmd[5] = -1.0 + elif key_str == self._command_forward_node.keyboard_layout.pause: # pause + self._twist_cmd = np.zeros(6) + self._joint_velocity_cmd = np.zeros(self._command_forward_node.dof) + elif ( + key_str + == self._command_forward_node.keyboard_layout.translation.x.increase + ): + self._twist_cmd[0] = 1.0 # translation along x-axis + elif ( + key_str + == self._command_forward_node.keyboard_layout.translation.x.decrease + ): + self._twist_cmd[0] = -1.0 + elif ( + key_str + == self._command_forward_node.keyboard_layout.translation.y.increase + ): + self._twist_cmd[1] = 1.0 # translation along y-axis + elif ( + key_str + == self._command_forward_node.keyboard_layout.translation.y.decrease + ): + self._twist_cmd[1] = -1.0 + elif ( + key_str + == self._command_forward_node.keyboard_layout.rotation.x.increase + ): + self._twist_cmd[3] = 1.0 # rotation about x-axis + elif ( + key_str + == self._command_forward_node.keyboard_layout.rotation.x.decrease + ): + self._twist_cmd[3] = -1.0 + elif ( + key_str + == self._command_forward_node.keyboard_layout.rotation.y.increase + ): + self._twist_cmd[4] = 1.0 # rotation about y-axis + elif ( + key_str + == self._command_forward_node.keyboard_layout.rotation.y.decrease + ): + self._twist_cmd[4] = -1.0 + elif key_str == self._command_forward_node.keyboard_layout.reverse_joints: + self._joint_vel_direction *= -1.0 + elif key_str in self._valid_numbers: + # get index of key_str within valid_numbers + index = self._valid_numbers.index(key_str) + if 0 <= index < self._command_forward_node.dof: + self._joint_velocity_cmd[index] = self._joint_vel_direction + self._command_forward_node.twist_cmd = self._twist_cmd + self._command_forward_node.joint_veloctiy_cmd = self._joint_velocity_cmd + except AttributeError: + pass diff --git a/lbr_demos/lbr_moveit/package.xml b/lbr_demos/lbr_moveit/package.xml new file mode 100644 index 00000000..f2116fe8 --- /dev/null +++ b/lbr_demos/lbr_moveit/package.xml @@ -0,0 +1,18 @@ + + + + lbr_moveit + 2.1.2 + MoveIt demos for the LBRs + mhubii + Apache-2.0 + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/lbr_demos/lbr_moveit/resource/lbr_moveit b/lbr_demos/lbr_moveit/resource/lbr_moveit new file mode 100644 index 00000000..e69de29b diff --git a/lbr_demos/lbr_moveit/scripts/__init__.py b/lbr_demos/lbr_moveit/scripts/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/lbr_demos/lbr_moveit/scripts/forward_keyboard.py b/lbr_demos/lbr_moveit/scripts/forward_keyboard.py new file mode 100644 index 00000000..62d6531a --- /dev/null +++ b/lbr_demos/lbr_moveit/scripts/forward_keyboard.py @@ -0,0 +1,13 @@ +import rclpy +from lbr_moveit.forward_keyboard_node import ForwardKeyboardNode +from lbr_moveit.keyboard_listener import KeyboardListener + + +def main(): + rclpy.init() + forward_keyboard_node = ForwardKeyboardNode() + try: + with KeyboardListener(forward_keyboard_node): + rclpy.spin(forward_keyboard_node) + except KeyboardInterrupt: + pass diff --git a/lbr_demos/lbr_moveit/setup.cfg b/lbr_demos/lbr_moveit/setup.cfg new file mode 100644 index 00000000..54a2c888 --- /dev/null +++ b/lbr_demos/lbr_moveit/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/lbr_moveit +[install] +install_scripts=$base/lib/lbr_moveit diff --git a/lbr_demos/lbr_moveit/setup.py b/lbr_demos/lbr_moveit/setup.py new file mode 100644 index 00000000..a4d342ce --- /dev/null +++ b/lbr_demos/lbr_moveit/setup.py @@ -0,0 +1,27 @@ +from setuptools import find_packages, setup + +package_name = "lbr_moveit" + +setup( + name=package_name, + version="2.1.2", + packages=find_packages(exclude=["test"]), + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ("share/" + package_name + "/config", ["config/forward_keyboard.yaml"]), + ("share/" + package_name + "/launch", ["launch/keyboard_driver.launch.py"]), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="mhubii", + maintainer_email="m.huber_1994@hotmail.de", + description="MoveIt demos for the LBRs.", + license="Apache-2.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "forward_keyboard = scripts.forward_keyboard:main", + ], + }, +) diff --git a/lbr_demos/lbr_moveit/test/test_copyright.py b/lbr_demos/lbr_moveit/test/test_copyright.py new file mode 100644 index 00000000..ceffe896 --- /dev/null +++ b/lbr_demos/lbr_moveit/test/test_copyright.py @@ -0,0 +1,27 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip( + reason="No copyright header has been placed in the generated source file." +) +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/lbr_demos/lbr_moveit/test/test_flake8.py b/lbr_demos/lbr_moveit/test/test_flake8.py new file mode 100644 index 00000000..ee79f31a --- /dev/null +++ b/lbr_demos/lbr_moveit/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, "Found %d code style errors / warnings:\n" % len( + errors + ) + "\n".join(errors) diff --git a/lbr_demos/lbr_moveit/test/test_pep257.py b/lbr_demos/lbr_moveit/test/test_pep257.py new file mode 100644 index 00000000..a2c3deb8 --- /dev/null +++ b/lbr_demos/lbr_moveit/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings" diff --git a/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst b/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst index d7b52b15..5d907950 100644 --- a/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst +++ b/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst @@ -76,7 +76,7 @@ The ``MoveGroup`` configurations are parsed conveniently through a mixin: .. code-block:: python - from lbr_bringup.move_group import LBRMoveGroupMixin + from lbr_bringup.moveit import LBRMoveGroupMixin ... diff --git a/lbr_demos/lbr_moveit_cpp/launch/hello_moveit.launch.py b/lbr_demos/lbr_moveit_cpp/launch/hello_moveit.launch.py index fa080dfe..3e1577e7 100644 --- a/lbr_demos/lbr_moveit_cpp/launch/hello_moveit.launch.py +++ b/lbr_demos/lbr_moveit_cpp/launch/hello_moveit.launch.py @@ -5,7 +5,7 @@ from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node from lbr_bringup.description import LBRDescriptionMixin -from lbr_bringup.move_group import LBRMoveGroupMixin +from lbr_bringup.moveit import LBRMoveGroupMixin def hidden_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: diff --git a/lbr_demos/lbr_moveit_cpp/package.xml b/lbr_demos/lbr_moveit_cpp/package.xml index 17bcf85c..85d80d66 100644 --- a/lbr_demos/lbr_moveit_cpp/package.xml +++ b/lbr_demos/lbr_moveit_cpp/package.xml @@ -2,7 +2,7 @@ lbr_moveit_cpp - 2.1.1 + 2.1.2 Demo for using MoveIt C++ API. mhubii Apache-2.0 diff --git a/lbr_demos/lbr_moveit_py/doc/lbr_moveit_py.rst b/lbr_demos/lbr_moveit_py/doc/lbr_moveit_py.rst deleted file mode 100644 index 31654530..00000000 --- a/lbr_demos/lbr_moveit_py/doc/lbr_moveit_py.rst +++ /dev/null @@ -1,55 +0,0 @@ -lbr_demos_moveit_py -=================== -.. warning:: - On hardware, do always execute in ``T1`` mode first. - -MoveIt via RViz ------------------ -.. image:: img/iiwa7_moveit_rviz.png - :align: center - :alt: MoveIt via RViz -**IIWA 7 R800 in RViz** - -To run MoveIt via RViz, simply follow: - -Simulation -~~~~~~~~~~ -#. Run the robot driver: - - .. code-block:: bash - - ros2 launch lbr_bringup mock.launch.py \ - model:=iiwa7 # [iiwa7, iiwa14, med7, med14] - -#. Run MoveIt with RViz: - - .. code-block:: bash - - ros2 launch lbr_bringup move_group.launch.py \ - mode:=mock \ - rviz:=true \ - model:=iiwa7 # [iiwa7, iiwa14, med7, med14] - -#. You can now move the robot via MoveIt in RViz! - -Hardware -~~~~~~~~ -#. Client side configurations: - - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` - -#. Remote side configurations: - - #. .. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD`` - - .. thumbnail:: ../../doc/img/applications_lbr_server.png - - #. Select - - - ``FRI send period``: ``10 ms`` - - ``IP address``: ``your configuration`` - - ``FRI control mode``: ``POSITION_CONTROL`` or ``JOINT_IMPEDANCE_CONTROL`` - - ``FRI client command mode``: ``POSITION`` - -#. Proceed with steps 1 and 2 from `Simulation`_ but with ``ros2 launch lbr_bringup hardware.launch.py``. diff --git a/lbr_description/package.xml b/lbr_description/package.xml index a00fd055..a3b807df 100644 --- a/lbr_description/package.xml +++ b/lbr_description/package.xml @@ -2,10 +2,10 @@ lbr_description - 2.1.1 + 2.1.2 KUKA LBR description files mhubii - Apache License 2.0 + Apache-2.0 ament_cmake ament_cmake_pytest diff --git a/lbr_fri_ros2/package.xml b/lbr_fri_ros2/package.xml index 58e5b7ce..62379bc1 100644 --- a/lbr_fri_ros2/package.xml +++ b/lbr_fri_ros2/package.xml @@ -2,11 +2,11 @@ lbr_fri_ros2 - 2.1.1 + 2.1.2 The lbr_fri_ros2 package provides the Fast Robot Interface (FRI) integration into ROS 2. Robot states can be extracted and commanded. mhubii - Apache License 2.0 + Apache-2.0 ament_cmake eigen3_cmake_module diff --git a/lbr_fri_ros2_stack/package.xml b/lbr_fri_ros2_stack/package.xml index 0f611574..bb361f9b 100644 --- a/lbr_fri_ros2_stack/package.xml +++ b/lbr_fri_ros2_stack/package.xml @@ -2,10 +2,10 @@ lbr_fri_ros2_stack - 2.1.1 + 2.1.2 ROS 2 stack for KUKA LBRs. mhubii - Apache License 2.0 + Apache-2.0 ament_cmake diff --git a/lbr_ros2_control/package.xml b/lbr_ros2_control/package.xml index b1326dc5..f6f5b81a 100644 --- a/lbr_ros2_control/package.xml +++ b/lbr_ros2_control/package.xml @@ -2,10 +2,10 @@ lbr_ros2_control - 2.1.1 + 2.1.2 ROS 2 hardware hardware_interface for KUKA LBR through Fast Robot Interface (FRI). mhubii - Apache License 2.0 + Apache-2.0 ament_cmake