From e474f37f5d9da757dbc21e88003febeec7f9d339 Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 29 Dec 2023 17:21:51 +0000 Subject: [PATCH 01/76] removed launch redundancy --- lbr_bringup/launch/bringup.launch.py | 5 +- .../lbr_description/launch_mixin.py | 53 +++++++++---------- lbr_fri_ros2/lbr_fri_ros2/launch_mixin.py | 7 +-- .../lbr_ros2_control/launch_mixin.py | 31 ++++++----- 4 files changed, 46 insertions(+), 50 deletions(-) diff --git a/lbr_bringup/launch/bringup.launch.py b/lbr_bringup/launch/bringup.launch.py index c7b7ebdf..03fc2423 100644 --- a/lbr_bringup/launch/bringup.launch.py +++ b/lbr_bringup/launch/bringup.launch.py @@ -1,12 +1,11 @@ from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription, RegisterEventHandler +from launch.actions import IncludeLaunchDescription from launch.conditions import IfCondition, UnlessCondition -from launch.event_handlers import OnIncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.substitutions import FindPackageShare -from lbr_description import LBRDescriptionMixin, RVizMixin +from lbr_description import LBRDescriptionMixin def generate_launch_description() -> LaunchDescription: diff --git a/lbr_description/lbr_description/launch_mixin.py b/lbr_description/lbr_description/launch_mixin.py index 33cd4e3d..73aef543 100644 --- a/lbr_description/lbr_description/launch_mixin.py +++ b/lbr_description/lbr_description/launch_mixin.py @@ -30,10 +30,11 @@ def include_gazebo(**kwargs) -> IncludeLaunchDescription: @staticmethod def node_spawn_entity( - robot_name: Optional[Union[LaunchConfiguration, str]] = None, **kwargs + robot_name: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( + "robot_name", default="lbr" + ), + **kwargs, ) -> Node: - if robot_name is None: - robot_name = LaunchConfiguration("robot_name") return Node( package="gazebo_ros", executable="spawn_entity.py", @@ -41,12 +42,12 @@ def node_spawn_entity( "-topic", "robot_description", "-entity", - LaunchConfiguration("robot_name"), + robot_name, "-robot_namespace", - LaunchConfiguration("robot_name"), + robot_name, ], output="screen", - namespace=LaunchConfiguration("robot_name"), + namespace=robot_name, **kwargs, ) @@ -54,19 +55,19 @@ def node_spawn_entity( class LBRDescriptionMixin: @staticmethod def param_robot_description( - model: Optional[Union[LaunchConfiguration, str]] = None, - robot_name: Optional[Union[LaunchConfiguration, str]] = None, - port_id: Optional[Union[LaunchConfiguration, str]] = None, - sim: Optional[Union[LaunchConfiguration, bool]] = None, + model: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( + "model", default="iiwa7" + ), + robot_name: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( + "robot_name", default="lbr" + ), + port_id: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( + "port_id", default="30200" + ), + sim: Optional[Union[LaunchConfiguration, bool]] = LaunchConfiguration( + "sim", default="true" + ), ) -> Dict[str, str]: - if model is None: - model = LaunchConfiguration("model", default="iiwa7") - if robot_name is None: - robot_name = LaunchConfiguration("robot_name", default="lbr") - if port_id is None: - port_id = LaunchConfiguration("port_id", default="30200") - if sim is None: - sim = LaunchConfiguration("sim", default="true") if type(sim) is bool: sim = "true" if sim else "false" robot_description = { @@ -187,18 +188,14 @@ def arg_rviz_config( @staticmethod def node_rviz( - rviz_config_pkg: Optional[Union[LaunchConfiguration, str]] = None, - rviz_config: Optional[Union[LaunchConfiguration, str]] = None, + rviz_config_pkg: Optional[ + Union[LaunchConfiguration, str] + ] = LaunchConfiguration("rviz_config_pkg", default="lbr_description"), + rviz_config: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( + "rviz_config", default="config/config.rviz" + ), **kwargs, ) -> Node: - if rviz_config_pkg is None: - rviz_config_pkg = LaunchConfiguration( - "rviz_config_pkg", default="lbr_description" - ) - if rviz_config is None: - rviz_config = LaunchConfiguration( - "rviz_config", default="config/config.rviz" - ) return Node( package="rviz2", executable="rviz2", diff --git a/lbr_fri_ros2/lbr_fri_ros2/launch_mixin.py b/lbr_fri_ros2/lbr_fri_ros2/launch_mixin.py index 1f873a69..1c74aa13 100644 --- a/lbr_fri_ros2/lbr_fri_ros2/launch_mixin.py +++ b/lbr_fri_ros2/lbr_fri_ros2/launch_mixin.py @@ -136,10 +136,11 @@ def param_port_id() -> Dict[str, LaunchConfiguration]: @staticmethod def node_app( - robot_name: Optional[Union[LaunchConfiguration, str]] = None, **kwargs + robot_name: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( + "robot_name", default="lbr" + ), + **kwargs ) -> DeclareLaunchArgument: - if robot_name is None: - robot_name = LaunchConfiguration("robot_name", default="lbr") return Node( package="lbr_fri_ros2", executable="app", diff --git a/lbr_ros2_control/lbr_ros2_control/launch_mixin.py b/lbr_ros2_control/lbr_ros2_control/launch_mixin.py index c7fce4a3..65901017 100644 --- a/lbr_ros2_control/lbr_ros2_control/launch_mixin.py +++ b/lbr_ros2_control/lbr_ros2_control/launch_mixin.py @@ -48,11 +48,11 @@ def arg_use_sim_time() -> DeclareLaunchArgument: @staticmethod def node_ros2_control( robot_description: Dict[str, str], - robot_name: Optional[Union[LaunchConfiguration, str]] = None, + robot_name: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( + "robot_name", default="lbr" + ), **kwargs, ) -> Node: - if robot_name is None: - robot_name = LaunchConfiguration("robot_name", default="lbr") return Node( package="controller_manager", executable="ros2_control_node", @@ -78,14 +78,14 @@ def node_ros2_control( @staticmethod def node_controller_spawner( - robot_name: Optional[Union[LaunchConfiguration, str]] = None, - controller: Optional[Union[LaunchConfiguration, str]] = None, + robot_name: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( + "robot_name", default="lbr" + ), + controller: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( + "ctrl" + ), **kwargs, ) -> Node: - if robot_name is None: - robot_name = LaunchConfiguration("robot_name", default="lbr") - if controller is None: - controller = LaunchConfiguration("ctrl") return Node( package="controller_manager", executable="spawner", @@ -102,15 +102,14 @@ def node_controller_spawner( @staticmethod def node_robot_state_publisher( robot_description: Dict[str, str], - robot_name: Optional[LaunchConfiguration] = None, - use_sim_time: Optional[Union[LaunchConfiguration, bool]] = None, + robot_name: Optional[LaunchConfiguration] = LaunchConfiguration( + "robot_name", default="lbr" + ), + use_sim_time: Optional[Union[LaunchConfiguration, bool]] = LaunchConfiguration( + "use_sim_time", default="false" + ), **kwargs, ) -> Node: - if robot_name is None: - robot_name = LaunchConfiguration("robot_name", default="lbr") - if use_sim_time is None: - use_sim_time = LaunchConfiguration("use_sim_time", default="false") - return Node( package="robot_state_publisher", executable="robot_state_publisher", From 29dfa20ad554477cec1b04edf4159c65213c9a62 Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 29 Dec 2023 17:26:33 +0000 Subject: [PATCH 02/76] RViz -> RViZ --- lbr_bringup/launch/move_group.launch.py | 4 ++-- lbr_bringup/launch/real.launch.py | 6 +++--- lbr_bringup/launch/sim.launch.py | 6 +++--- lbr_description/launch/view_robot.launch.py | 4 ++-- lbr_description/lbr_description/__init__.py | 2 +- lbr_description/lbr_description/launch_mixin.py | 2 +- 6 files changed, 12 insertions(+), 12 deletions(-) diff --git a/lbr_bringup/launch/move_group.launch.py b/lbr_bringup/launch/move_group.launch.py index fab6d4e3..c024b173 100644 --- a/lbr_bringup/launch/move_group.launch.py +++ b/lbr_bringup/launch/move_group.launch.py @@ -5,7 +5,7 @@ from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from lbr_bringup import LBRMoveGroupMixin -from lbr_description import LBRDescriptionMixin, RVizMixin +from lbr_description import LBRDescriptionMixin, RViZMixin def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: @@ -54,7 +54,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: ) # RViz - rviz = RVizMixin.node_rviz( + rviz = RViZMixin.node_rviz( rviz_config_pkg=f"{model}_moveit_config", rviz_config="config/moveit.rviz", parameters=LBRMoveGroupMixin.params_rviz( diff --git a/lbr_bringup/launch/real.launch.py b/lbr_bringup/launch/real.launch.py index 5bb5a279..cb9a5bf5 100644 --- a/lbr_bringup/launch/real.launch.py +++ b/lbr_bringup/launch/real.launch.py @@ -12,7 +12,7 @@ ) from lbr_bringup import LBRMoveGroupMixin -from lbr_description import LBRDescriptionMixin, RVizMixin +from lbr_description import LBRDescriptionMixin, RViZMixin from lbr_ros2_control import LBRROS2ControlMixin @@ -108,7 +108,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: ) # RViz and MoveIt - rviz_moveit = RVizMixin.node_rviz( + rviz_moveit = RViZMixin.node_rviz( rviz_config_pkg=f"{model}_moveit_config", rviz_config="config/moveit.rviz", parameters=LBRMoveGroupMixin.params_rviz( @@ -128,7 +128,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: ) # RViz no MoveIt - rviz = RVizMixin.node_rviz( + rviz = RViZMixin.node_rviz( rviz_config_pkg="lbr_bringup", rviz_config="config/config.rviz", condition=IfCondition( diff --git a/lbr_bringup/launch/sim.launch.py b/lbr_bringup/launch/sim.launch.py index 9fd377f9..b8d91a12 100644 --- a/lbr_bringup/launch/sim.launch.py +++ b/lbr_bringup/launch/sim.launch.py @@ -12,7 +12,7 @@ ) from lbr_bringup import LBRMoveGroupMixin -from lbr_description import GazeboMixin, LBRDescriptionMixin, RVizMixin +from lbr_description import GazeboMixin, LBRDescriptionMixin, RViZMixin from lbr_ros2_control import LBRROS2ControlMixin @@ -85,7 +85,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: ) # RViz and MoveIt - rviz_moveit = RVizMixin.node_rviz( + rviz_moveit = RViZMixin.node_rviz( rviz_config_pkg=f"{model}_moveit_config", rviz_config="config/moveit.rviz", parameters=LBRMoveGroupMixin.params_rviz( @@ -105,7 +105,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: ) # RViz no MoveIt - rviz = RVizMixin.node_rviz( + rviz = RViZMixin.node_rviz( rviz_config_pkg="lbr_bringup", rviz_config="config/config.rviz", condition=IfCondition( diff --git a/lbr_description/launch/view_robot.launch.py b/lbr_description/launch/view_robot.launch.py index b3d91782..9654f6f0 100644 --- a/lbr_description/launch/view_robot.launch.py +++ b/lbr_description/launch/view_robot.launch.py @@ -1,7 +1,7 @@ from launch import LaunchDescription from launch_ros.actions import Node -from lbr_description import LBRDescriptionMixin, RVizMixin +from lbr_description import LBRDescriptionMixin, RViZMixin def generate_launch_description() -> LaunchDescription: @@ -24,7 +24,7 @@ def generate_launch_description() -> LaunchDescription: ) ) ld.add_action( - RVizMixin.node_rviz( + RViZMixin.node_rviz( rviz_config_pkg="lbr_description", rviz_config="config/config.rviz", ) diff --git a/lbr_description/lbr_description/__init__.py b/lbr_description/lbr_description/__init__.py index e811a21e..b9310219 100644 --- a/lbr_description/lbr_description/__init__.py +++ b/lbr_description/lbr_description/__init__.py @@ -1 +1 @@ -from .launch_mixin import GazeboMixin, LBRDescriptionMixin, RVizMixin +from .launch_mixin import GazeboMixin, LBRDescriptionMixin, RViZMixin diff --git a/lbr_description/lbr_description/launch_mixin.py b/lbr_description/lbr_description/launch_mixin.py index 73aef543..39f8a646 100644 --- a/lbr_description/lbr_description/launch_mixin.py +++ b/lbr_description/lbr_description/launch_mixin.py @@ -165,7 +165,7 @@ def node_static_tf( ) -class RVizMixin: +class RViZMixin: @staticmethod def arg_rviz_config_pkg( default_value: str = "lbr_description", From 3864a9d6f0671562c44f4f3ca14279714070c8c5 Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 29 Dec 2023 17:29:02 +0000 Subject: [PATCH 03/76] RViZ -> RViz --- lbr_bringup/launch/move_group.launch.py | 4 ++-- lbr_bringup/launch/real.launch.py | 6 +++--- lbr_bringup/launch/sim.launch.py | 6 +++--- lbr_description/launch/view_robot.launch.py | 4 ++-- lbr_description/lbr_description/__init__.py | 2 +- lbr_description/lbr_description/launch_mixin.py | 2 +- 6 files changed, 12 insertions(+), 12 deletions(-) diff --git a/lbr_bringup/launch/move_group.launch.py b/lbr_bringup/launch/move_group.launch.py index c024b173..fab6d4e3 100644 --- a/lbr_bringup/launch/move_group.launch.py +++ b/lbr_bringup/launch/move_group.launch.py @@ -5,7 +5,7 @@ from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from lbr_bringup import LBRMoveGroupMixin -from lbr_description import LBRDescriptionMixin, RViZMixin +from lbr_description import LBRDescriptionMixin, RVizMixin def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: @@ -54,7 +54,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: ) # RViz - rviz = RViZMixin.node_rviz( + rviz = RVizMixin.node_rviz( rviz_config_pkg=f"{model}_moveit_config", rviz_config="config/moveit.rviz", parameters=LBRMoveGroupMixin.params_rviz( diff --git a/lbr_bringup/launch/real.launch.py b/lbr_bringup/launch/real.launch.py index cb9a5bf5..5bb5a279 100644 --- a/lbr_bringup/launch/real.launch.py +++ b/lbr_bringup/launch/real.launch.py @@ -12,7 +12,7 @@ ) from lbr_bringup import LBRMoveGroupMixin -from lbr_description import LBRDescriptionMixin, RViZMixin +from lbr_description import LBRDescriptionMixin, RVizMixin from lbr_ros2_control import LBRROS2ControlMixin @@ -108,7 +108,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: ) # RViz and MoveIt - rviz_moveit = RViZMixin.node_rviz( + rviz_moveit = RVizMixin.node_rviz( rviz_config_pkg=f"{model}_moveit_config", rviz_config="config/moveit.rviz", parameters=LBRMoveGroupMixin.params_rviz( @@ -128,7 +128,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: ) # RViz no MoveIt - rviz = RViZMixin.node_rviz( + rviz = RVizMixin.node_rviz( rviz_config_pkg="lbr_bringup", rviz_config="config/config.rviz", condition=IfCondition( diff --git a/lbr_bringup/launch/sim.launch.py b/lbr_bringup/launch/sim.launch.py index b8d91a12..9fd377f9 100644 --- a/lbr_bringup/launch/sim.launch.py +++ b/lbr_bringup/launch/sim.launch.py @@ -12,7 +12,7 @@ ) from lbr_bringup import LBRMoveGroupMixin -from lbr_description import GazeboMixin, LBRDescriptionMixin, RViZMixin +from lbr_description import GazeboMixin, LBRDescriptionMixin, RVizMixin from lbr_ros2_control import LBRROS2ControlMixin @@ -85,7 +85,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: ) # RViz and MoveIt - rviz_moveit = RViZMixin.node_rviz( + rviz_moveit = RVizMixin.node_rviz( rviz_config_pkg=f"{model}_moveit_config", rviz_config="config/moveit.rviz", parameters=LBRMoveGroupMixin.params_rviz( @@ -105,7 +105,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: ) # RViz no MoveIt - rviz = RViZMixin.node_rviz( + rviz = RVizMixin.node_rviz( rviz_config_pkg="lbr_bringup", rviz_config="config/config.rviz", condition=IfCondition( diff --git a/lbr_description/launch/view_robot.launch.py b/lbr_description/launch/view_robot.launch.py index 9654f6f0..b3d91782 100644 --- a/lbr_description/launch/view_robot.launch.py +++ b/lbr_description/launch/view_robot.launch.py @@ -1,7 +1,7 @@ from launch import LaunchDescription from launch_ros.actions import Node -from lbr_description import LBRDescriptionMixin, RViZMixin +from lbr_description import LBRDescriptionMixin, RVizMixin def generate_launch_description() -> LaunchDescription: @@ -24,7 +24,7 @@ def generate_launch_description() -> LaunchDescription: ) ) ld.add_action( - RViZMixin.node_rviz( + RVizMixin.node_rviz( rviz_config_pkg="lbr_description", rviz_config="config/config.rviz", ) diff --git a/lbr_description/lbr_description/__init__.py b/lbr_description/lbr_description/__init__.py index b9310219..e811a21e 100644 --- a/lbr_description/lbr_description/__init__.py +++ b/lbr_description/lbr_description/__init__.py @@ -1 +1 @@ -from .launch_mixin import GazeboMixin, LBRDescriptionMixin, RViZMixin +from .launch_mixin import GazeboMixin, LBRDescriptionMixin, RVizMixin diff --git a/lbr_description/lbr_description/launch_mixin.py b/lbr_description/lbr_description/launch_mixin.py index 39f8a646..73aef543 100644 --- a/lbr_description/lbr_description/launch_mixin.py +++ b/lbr_description/lbr_description/launch_mixin.py @@ -165,7 +165,7 @@ def node_static_tf( ) -class RViZMixin: +class RVizMixin: @staticmethod def arg_rviz_config_pkg( default_value: str = "lbr_description", From 7ff671eda2b5cf4caeb06bf1985936db9a17a3ea Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 3 Jan 2024 16:03:01 +0000 Subject: [PATCH 04/76] read robot description from robot state publisher --- lbr_bringup/launch/real.launch.py | 25 ++++++++----------- .../launch/system_interface.launch.py | 12 ++++----- .../lbr_ros2_control/launch_mixin.py | 5 ++-- 3 files changed, 18 insertions(+), 24 deletions(-) diff --git a/lbr_bringup/launch/real.launch.py b/lbr_bringup/launch/real.launch.py index 5bb5a279..53c84222 100644 --- a/lbr_bringup/launch/real.launch.py +++ b/lbr_bringup/launch/real.launch.py @@ -3,7 +3,7 @@ from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity from launch.actions import DeclareLaunchArgument, OpaqueFunction, RegisterEventHandler from launch.conditions import IfCondition -from launch.event_handlers import OnProcessExit, OnProcessStart +from launch.event_handlers import OnProcessStart from launch.substitutions import ( AndSubstitution, LaunchConfiguration, @@ -20,9 +20,15 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: ld = LaunchDescription() robot_description = LBRDescriptionMixin.param_robot_description(sim=False) - ros2_control_node = LBRROS2ControlMixin.node_ros2_control( - robot_description=robot_description + + # robot state publisher + robot_state_publisher = LBRROS2ControlMixin.node_robot_state_publisher( + robot_description=robot_description, use_sim_time=False ) + ld.add_action(robot_state_publisher) + + # ros2 control node + ros2_control_node = LBRROS2ControlMixin.node_ros2_control() ld.add_action(ros2_control_node) # joint state broad caster and controller on ros2 control node start @@ -52,17 +58,6 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: ) ld.add_action(controller_event_handler) - # robot state publisher on joint state broadcaster spawn exit - robot_state_publisher = LBRROS2ControlMixin.node_robot_state_publisher( - robot_description=robot_description, use_sim_time=False - ) - robot_state_publisher_event_handler = RegisterEventHandler( - OnProcessExit( - target_action=joint_state_broadcaster, on_exit=[robot_state_publisher] - ) - ) - ld.add_action(robot_state_publisher_event_handler) - # MoveIt 2 ld.add_action(LBRMoveGroupMixin.arg_allow_trajectory_execution()) ld.add_action(LBRMoveGroupMixin.arg_capabilities()) @@ -142,7 +137,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: # RViz event handler rviz_event_handler = RegisterEventHandler( OnProcessStart( - target_action=robot_state_publisher, on_start=[rviz_moveit, rviz] + target_action=joint_state_broadcaster, on_start=[rviz_moveit, rviz] ) ) ld.add_action(rviz_event_handler) diff --git a/lbr_ros2_control/launch/system_interface.launch.py b/lbr_ros2_control/launch/system_interface.launch.py index a3ba70e3..3043ef73 100644 --- a/lbr_ros2_control/launch/system_interface.launch.py +++ b/lbr_ros2_control/launch/system_interface.launch.py @@ -20,9 +20,12 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(LBRSystemInterface.arg_ctrl_cfg_pkg()) ld.add_action(LBRSystemInterface.arg_ctrl_cfg()) ld.add_action(LBRSystemInterface.arg_ctrl()) - ros2_control_node = LBRSystemInterface.node_ros2_control( - robot_description=robot_description + robot_state_publisher = LBRSystemInterface.node_robot_state_publisher( + robot_description=robot_description, + use_sim_time=False, ) + ld.add_action(robot_state_publisher) + ros2_control_node = LBRSystemInterface.node_ros2_control() ld.add_action(ros2_control_node) joint_state_broadcaster = LBRSystemInterface.node_controller_spawner( controller="joint_state_broadcaster" @@ -48,9 +51,4 @@ def generate_launch_description() -> LaunchDescription: ) ) ld.add_action(controller_event_handler) - robot_state_publisher = LBRSystemInterface.node_robot_state_publisher( - robot_description=robot_description, - use_sim_time=False, - ) - ld.add_action(robot_state_publisher) return ld diff --git a/lbr_ros2_control/lbr_ros2_control/launch_mixin.py b/lbr_ros2_control/lbr_ros2_control/launch_mixin.py index 65901017..2a8560bb 100644 --- a/lbr_ros2_control/lbr_ros2_control/launch_mixin.py +++ b/lbr_ros2_control/lbr_ros2_control/launch_mixin.py @@ -47,7 +47,6 @@ def arg_use_sim_time() -> DeclareLaunchArgument: @staticmethod def node_ros2_control( - robot_description: Dict[str, str], robot_name: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( "robot_name", default="lbr" ), @@ -70,9 +69,11 @@ def node_ros2_control( ), ] ), - robot_description, ], namespace=robot_name, + remappings=[ + ("~/robot_description", "robot_description"), + ], **kwargs, ) From a4a331ee8b351915699c0fd373d43f5e891c9a2a Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 3 Jan 2024 17:02:34 +0000 Subject: [PATCH 05/76] added color scheme --- lbr_fri_ros2/include/lbr_fri_ros2/app.hpp | 1 + .../include/lbr_fri_ros2/async_client.hpp | 2 +- .../include/lbr_fri_ros2/command_guard.hpp | 1 + .../{enum_maps.hpp => formatting.hpp} | 17 ++- lbr_fri_ros2/src/app.cpp | 60 +++++++---- lbr_fri_ros2/src/app_component.hpp | 2 +- lbr_fri_ros2/src/async_client.cpp | 18 ++-- lbr_fri_ros2/src/command_guard.cpp | 19 ++-- lbr_fri_ros2/src/command_interface.cpp | 2 +- lbr_fri_ros2/src/filters.cpp | 2 +- .../lbr_ros2_control/system_interface.hpp | 2 +- lbr_ros2_control/src/system_interface.cpp | 101 ++++++++++-------- 12 files changed, 137 insertions(+), 90 deletions(-) rename lbr_fri_ros2/include/lbr_fri_ros2/{enum_maps.hpp => formatting.hpp} (74%) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp index 708fd43e..819668f3 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp @@ -13,6 +13,7 @@ #include "friUdpConnection.h" #include "lbr_fri_ros2/async_client.hpp" +#include "lbr_fri_ros2/formatting.hpp" namespace lbr_fri_ros2 { class App { diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp index 8c3f2bfb..e1741535 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp @@ -11,8 +11,8 @@ #include "friLBRClient.h" #include "lbr_fri_ros2/command_interface.hpp" -#include "lbr_fri_ros2/enum_maps.hpp" #include "lbr_fri_ros2/filters.hpp" +#include "lbr_fri_ros2/formatting.hpp" #include "lbr_fri_ros2/state_interface.hpp" namespace lbr_fri_ros2 { diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp index 7ebc0f2e..e2260371 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp @@ -13,6 +13,7 @@ #include "friLBRState.h" #include "lbr_fri_msgs/msg/lbr_command.hpp" +#include "lbr_fri_ros2/formatting.hpp" namespace lbr_fri_ros2 { struct CommandGuardParameters { diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/enum_maps.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp similarity index 74% rename from lbr_fri_ros2/include/lbr_fri_ros2/enum_maps.hpp rename to lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp index f94393e5..00d3e317 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/enum_maps.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp @@ -1,11 +1,24 @@ -#ifndef LBR_FRI_ROS2__ENUM_MAPS_HPP_ -#define LBR_FRI_ROS2__ENUM_MAPS_HPP_ +#ifndef LBR_FRI_ROS2__FORMATTING_HPP_ +#define LBR_FRI_ROS2__FORMATTING_HPP_ #include #include "friLBRClient.h" namespace lbr_fri_ros2 { +struct ColorScheme { + // refer https://stackoverflow.com/a/287944 + static constexpr char HEADER[] = "\033[95m"; + static constexpr char OKBLUE[] = "\033[94m"; + static constexpr char OKCYAN[] = "\033[96m"; + static constexpr char OKGREEN[] = "\033[92m"; + static constexpr char WARNING[] = "\033[93m"; + static constexpr char FAIL[] = "\033[91m"; + static constexpr char ENDC[] = "\033[0m"; + static constexpr char BOLD[] = "\033[1m"; + static constexpr char UNDERLINE[] = "\033[4m"; +}; + struct EnumMaps { static std::string session_state_map(const int &session_state) { switch (session_state) { diff --git a/lbr_fri_ros2/src/app.cpp b/lbr_fri_ros2/src/app.cpp index e77f39ab..aab47622 100644 --- a/lbr_fri_ros2/src/app.cpp +++ b/lbr_fri_ros2/src/app.cpp @@ -16,98 +16,114 @@ App::~App() { bool App::open_udp_socket(const int &port_id, const char *const remote_host) { if (!connection_ptr_) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Connection not configured."); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "%sConnection not configured%s", + ColorScheme::FAIL, ColorScheme::ENDC); return false; } - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Opening UDP socket with port_id %d.", port_id); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "%sOpening UDP socket with port_id %s%d%s", + ColorScheme::OKBLUE, ColorScheme::BOLD, port_id, ColorScheme::ENDC); if (!valid_port_(port_id)) { return false; } if (connection_ptr_->isOpen()) { - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Socket already open."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Socket already open"); return true; } if (!connection_ptr_->open(port_id, remote_host)) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Failed to open socket."); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "%sFailed to open socket%s", ColorScheme::FAIL, + ColorScheme::ENDC); return false; } - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Socket opened successfully."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "%sSocket opened successfully%s", + ColorScheme::OKGREEN, ColorScheme::ENDC); return true; } bool App::close_udp_socket() { if (!connection_ptr_) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Connection not configured."); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "%sConnection not configured%s", + ColorScheme::FAIL, ColorScheme::ENDC); return false; } - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Closing UDP socket."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "%sClosing UDP socket%s", ColorScheme::OKBLUE, + ColorScheme::ENDC); if (!connection_ptr_->isOpen()) { - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Socket already closed."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Socket already closed"); return true; } connection_ptr_->close(); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Socket closed successfully."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "%sSocket closed successfully%s", + ColorScheme::OKGREEN, ColorScheme::ENDC); return true; } void App::run(int rt_prio) { if (!async_client_ptr_) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "AsyncClient not configured."); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "%sAsyncClient not configured%s", + ColorScheme::FAIL, ColorScheme::ENDC); return; } if (!connection_ptr_) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Connection not configured."); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "%sConnection not configured%s", + ColorScheme::FAIL, ColorScheme::ENDC); return; } if (!connection_ptr_->isOpen()) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Connection not open."); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "%sConnection not open%s", ColorScheme::FAIL, + ColorScheme::ENDC); return; } if (!app_ptr_) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "App not configured."); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "%sApp not configured%s", ColorScheme::FAIL, + ColorScheme::ENDC); return; } if (running_) { - RCLCPP_WARN(rclcpp::get_logger(LOGGER_NAME), "App already running."); + RCLCPP_WARN(rclcpp::get_logger(LOGGER_NAME), "%sApp already running%s", ColorScheme::WARNING, + ColorScheme::ENDC); return; } run_thread_ = std::thread([&]() { if (realtime_tools::has_realtime_kernel()) { if (!realtime_tools::configure_sched_fifo(rt_prio)) { RCLCPP_WARN(rclcpp::get_logger(LOGGER_NAME), - "Failed to set FIFO realtime scheduling policy."); + "%sFailed to set FIFO realtime scheduling policy%s", ColorScheme::WARNING, + ColorScheme::ENDC); } } else { - RCLCPP_WARN(rclcpp::get_logger(LOGGER_NAME), "Realtime kernel recommended but not required."); + RCLCPP_WARN(rclcpp::get_logger(LOGGER_NAME), + "%sRealtime kernel recommended but not required%s", ColorScheme::WARNING, + ColorScheme::ENDC); } - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Starting run thread."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Starting run thread"); should_stop_ = false; running_ = true; bool success = true; while (rclcpp::ok() && success && !should_stop_) { success = app_ptr_->step(); // TODO: blocks until robot heartbeat, stuck if port id mismatches if (async_client_ptr_->robotState().getSessionState() == KUKA::FRI::ESessionState::IDLE) { - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "LBR in session state idle, exiting."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "LBR in session state idle, exiting"); break; } } async_client_ptr_->get_state_interface().uninitialize(); running_ = false; - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Exiting run thread."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Exiting run thread"); }); run_thread_.detach(); } void App::stop_run() { - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Requesting run thread stop."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Requesting run thread stop"); should_stop_ = true; } bool App::valid_port_(const int &port_id) { if (port_id < 30200 || port_id > 30209) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Expected port_id in [30200, 30209], got %d.", - port_id); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), + "%sExpected port_id in [30200, 30209], got %s%d%s", ColorScheme::FAIL, + ColorScheme::BOLD, port_id, ColorScheme::ENDC); return false; } return true; diff --git a/lbr_fri_ros2/src/app_component.hpp b/lbr_fri_ros2/src/app_component.hpp index e818ecae..06c89315 100644 --- a/lbr_fri_ros2/src/app_component.hpp +++ b/lbr_fri_ros2/src/app_component.hpp @@ -17,7 +17,7 @@ #include "lbr_fri_ros2/app.hpp" #include "lbr_fri_ros2/async_client.hpp" #include "lbr_fri_ros2/command_guard.hpp" -#include "lbr_fri_ros2/enum_maps.hpp" +#include "lbr_fri_ros2/formatting.hpp" #include "lbr_fri_ros2/filters.hpp" #include "lbr_fri_ros2/state_interface.hpp" diff --git a/lbr_fri_ros2/src/async_client.cpp b/lbr_fri_ros2/src/async_client.cpp index 923bf6db..2eda3961 100644 --- a/lbr_fri_ros2/src/async_client.cpp +++ b/lbr_fri_ros2/src/async_client.cpp @@ -8,20 +8,24 @@ AsyncClient::AsyncClient(const PIDParameters &pid_parameters, const bool &open_loop) : command_interface_(pid_parameters, command_guard_parameters, command_guard_variant), state_interface_(state_interface_parameters), open_loop_(open_loop) { - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Configuring client."); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Command guard variant: '%s'.", + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "%sConfiguring client%s", ColorScheme::OKBLUE, + ColorScheme::ENDC); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Command guard variant: '%s'", command_guard_variant.c_str()); command_interface_.log_info(); state_interface_.log_info(); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Open loop: '%s'.", open_loop_ ? "true" : "false"); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Client configured."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Open loop: '%s'", open_loop_ ? "true" : "false"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "%sClient configured%s", ColorScheme::OKGREEN, + ColorScheme::ENDC); } void AsyncClient::onStateChange(KUKA::FRI::ESessionState old_state, KUKA::FRI::ESessionState new_state) { - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "LBR switched from '%s' to '%s'.", - EnumMaps::session_state_map(old_state).c_str(), - EnumMaps::session_state_map(new_state).c_str()); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "LBR switched from '%s%s%s%s' to '%s%s%s%s'.", + ColorScheme::OKBLUE, ColorScheme::BOLD, + EnumMaps::session_state_map(old_state).c_str(), ColorScheme::ENDC, + ColorScheme::OKGREEN, ColorScheme::BOLD, + EnumMaps::session_state_map(new_state).c_str(), ColorScheme::ENDC); command_interface_.init_command(robotState()); } diff --git a/lbr_fri_ros2/src/command_guard.cpp b/lbr_fri_ros2/src/command_guard.cpp index f663781d..7911ff86 100644 --- a/lbr_fri_ros2/src/command_guard.cpp +++ b/lbr_fri_ros2/src/command_guard.cpp @@ -28,7 +28,7 @@ bool CommandGuard::is_valid_command(const_idl_command_t_ref lbr_command, } return true; default: - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Invalid EClientCommandMode provided."); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Invalid EClientCommandMode provided"); return false; } return false; @@ -52,8 +52,8 @@ bool CommandGuard::command_in_position_limits_(const_idl_command_t_ref lbr_comma if (lbr_command.joint_position[i] < parameters_.min_position[i] || lbr_command.joint_position[i] > parameters_.max_position[i]) { RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), - "Position command not in limits for joint '%s'.", - parameters_.joint_names[i].c_str()); + "%sPosition command not in limits for joint '%s'%s", ColorScheme::FAIL, + parameters_.joint_names[i].c_str(), ColorScheme::ENDC); return false; } } @@ -66,8 +66,8 @@ bool CommandGuard::command_in_velocity_limits_(const_idl_command_t_ref lbr_comma for (std::size_t i = 0; i < lbr_command.joint_position[i]; ++i) { if (std::abs(lbr_command.joint_position[i] - lbr_state.getMeasuredJointPosition()[i]) / dt > parameters_.max_velocity[i]) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Velocity not in limits for joint '%s'.", - parameters_.joint_names[i].c_str()); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "%sVelocity not in limits for joint '%s'%s", + ColorScheme::FAIL, parameters_.joint_names[i].c_str(), ColorScheme::ENDC); return false; } } @@ -79,8 +79,9 @@ bool CommandGuard::command_in_torque_limits_(const_idl_command_t_ref lbr_command for (std::size_t i = 0; i < lbr_command.torque.size(); ++i) { if (std::abs(lbr_command.torque[i] + lbr_state.getExternalTorque()[i]) > parameters_.max_torque[i]) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Torque command not in limits for joint '%s'.", - parameters_.joint_names[i].c_str()); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), + "%sTorque command not in limits for joint '%s'%s", ColorScheme::FAIL, + parameters_.joint_names[i].c_str(), ColorScheme::ENDC); return false; } } @@ -95,8 +96,8 @@ bool SafeStopCommandGuard::command_in_position_limits_(const_idl_command_t_ref l lbr_command.joint_position[i] > parameters_.max_position[i] - parameters_.max_velocity[i] * lbr_state.getSampleTime()) { RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), - "Position command not in limits for joint '%s'.", - parameters_.joint_names[i].c_str()); + "%sPosition command not in limits for joint '%s'%s", ColorScheme::FAIL, + parameters_.joint_names[i].c_str(), ColorScheme::ENDC); return false; } } diff --git a/lbr_fri_ros2/src/command_interface.cpp b/lbr_fri_ros2/src/command_interface.cpp index 9ff16367..9d759a44 100644 --- a/lbr_fri_ros2/src/command_interface.cpp +++ b/lbr_fri_ros2/src/command_interface.cpp @@ -66,7 +66,7 @@ void CommandInterface::get_torque_command(fri_command_t_ref command, const_fri_s // validate if (!command_guard_->is_valid_command(command_, state)) { - throw std::runtime_error("Invalid command."); + throw std::runtime_error("Invalid command"); } // write joint position and torque to output diff --git a/lbr_fri_ros2/src/filters.cpp b/lbr_fri_ros2/src/filters.cpp index 27d23034..d900876b 100644 --- a/lbr_fri_ros2/src/filters.cpp +++ b/lbr_fri_ros2/src/filters.cpp @@ -16,7 +16,7 @@ void ExponentialFilter::set_cutoff_frequency(const double &cutoff_frequency, sample_time_ = sample_time; alpha_ = compute_alpha_(cutoff_frequency, sample_time); if (!validate_alpha_(alpha_)) { - throw std::runtime_error("Alpha is not within [0, 1]."); + throw std::runtime_error("Alpha is not within [0, 1]"); } } diff --git a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp index e6295b7f..7316ca9e 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp @@ -22,8 +22,8 @@ #include "lbr_fri_ros2/app.hpp" #include "lbr_fri_ros2/async_client.hpp" #include "lbr_fri_ros2/command_guard.hpp" -#include "lbr_fri_ros2/enum_maps.hpp" #include "lbr_fri_ros2/filters.hpp" +#include "lbr_fri_ros2/formatting.hpp" #include "lbr_fri_ros2/ft_estimator.hpp" #include "lbr_fri_ros2/state_interface.hpp" #include "lbr_ros2_control/system_interface_type_values.hpp" diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 4722e417..5bff79c6 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -5,15 +5,18 @@ controller_interface::CallbackReturn SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { auto ret = hardware_interface::SystemInterface::on_init(system_info); if (ret != controller_interface::CallbackReturn::SUCCESS) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Failed to initialize SystemInterface."); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "%sFailed to initialize SystemInterface%s", + lbr_fri_ros2::ColorScheme::FAIL, lbr_fri_ros2::ColorScheme::ENDC); return ret; } // parameters_ from config/lbr_system_interface.xacro parameters_.port_id = std::stoul(info_.hardware_parameters["port_id"]); if (parameters_.port_id < 30200 || parameters_.port_id > 30209) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Expected port_id in [30200, 30209]. Found %d.", - parameters_.port_id); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), + "%sExpected port_id in [30200, 30209]. Found %s%d%s", + lbr_fri_ros2::ColorScheme::FAIL, lbr_fri_ros2::ColorScheme::BOLD, + parameters_.port_id, lbr_fri_ros2::ColorScheme::ENDC); return controller_interface::CallbackReturn::ERROR; } info_.hardware_parameters["remote_host"] == "INADDR_ANY" @@ -195,7 +198,8 @@ SystemInterface::prepare_command_mode_switch(const std::vector & /* controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_lifecycle::State &) { if (!async_client_ptr_) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "AsyncClient not configured."); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "%sAsyncClient not configured%s", + lbr_fri_ros2::ColorScheme::FAIL, lbr_fri_ros2::ColorScheme::ENDC); return controller_interface::CallbackReturn::ERROR; } if (!app_ptr_->open_udp_socket(parameters_.port_id, parameters_.remote_host)) { @@ -203,22 +207,18 @@ controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_l } app_ptr_->run(parameters_.rt_prio); uint8_t attempt = 0; - uint8_t max_attempts = 10; while (!async_client_ptr_->get_state_interface().is_initialized() && rclcpp::ok()) { RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), - "Waiting for robot heartbeat [%d/%d]. Port ID: %d.", attempt + 1, max_attempts, - parameters_.port_id); + "Waiting for robot heartbeat. Attempt: [%d], port id: %s%s%d%s", ++attempt, + lbr_fri_ros2::ColorScheme::OKBLUE, lbr_fri_ros2::ColorScheme::BOLD, + parameters_.port_id, lbr_fri_ros2::ColorScheme::ENDC); std::this_thread::sleep_for(std::chrono::seconds(1)); - if (++attempt >= max_attempts) { - app_ptr_->close_udp_socket(); // hard close as run gets stuck - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Failed to connect to robot on max attempts."); - return controller_interface::CallbackReturn::ERROR; - } } - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Robot connected."); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Control mode: '%s'.", + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "%sRobot connected%s", + lbr_fri_ros2::ColorScheme::OKGREEN, lbr_fri_ros2::ColorScheme::ENDC); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Control mode: '%s'", lbr_fri_ros2::EnumMaps::control_mode_map(hw_lbr_state_.control_mode).c_str()); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Sample time: %.3f s / %.1f Hz.", + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Sample time: %.3f s / %.1f Hz", async_client_ptr_->get_state_interface().get_state().sample_time, 1. / async_client_ptr_->get_state_interface().get_state().sample_time); return controller_interface::CallbackReturn::SUCCESS; @@ -239,7 +239,8 @@ hardware_interface::return_type SystemInterface::read(const rclcpp::Time & /*tim if (exit_commanding_active_(static_cast(hw_session_state_), static_cast(hw_lbr_state_.session_state))) { RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), - "LBR left COMMANDING_ACTIVE. Please re-run lbr_bringup."); + "%sLBR left COMMANDING_ACTIVE. Please re-run lbr_bringup%s", + lbr_fri_ros2::ColorScheme::FAIL, lbr_fri_ros2::ColorScheme::ENDC); app_ptr_->stop_run(); app_ptr_->close_udp_socket(); return hardware_interface::return_type::ERROR; @@ -291,7 +292,7 @@ hardware_interface::return_type SystemInterface::write(const rclcpp::Time & /*ti return hardware_interface::return_type::OK; } if (hw_client_command_mode_ == KUKA::FRI::EClientCommandMode::WRENCH) { - throw std::runtime_error("Wrench command mode currently not implemented."); + throw std::runtime_error("Wrench command mode currently not implemented"); } return hardware_interface::return_type::ERROR; } @@ -334,8 +335,9 @@ void SystemInterface::nan_state_interfaces_() { bool SystemInterface::verify_number_of_joints_() { if (info_.joints.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Expected %d joints in URDF. Found %ld.", - KUKA::FRI::LBRState::NUMBER_OF_JOINTS, info_.joints.size()); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "%sExpected %d joints in URDF. Found %ld%s", + lbr_fri_ros2::ColorScheme::FAIL, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, + info_.joints.size(), lbr_fri_ros2::ColorScheme::ENDC); return false; } return true; @@ -347,17 +349,19 @@ bool SystemInterface::verify_joint_command_interfaces_() { if (joint.command_interfaces.size() != LBR_FRI_COMMAND_INTERFACE_SIZE) { RCLCPP_ERROR( rclcpp::get_logger(LOGGER_NAME), - "Joint %s received invalid number of command interfaces. Received %ld, expected %d.", - joint.name.c_str(), joint.command_interfaces.size(), LBR_FRI_COMMAND_INTERFACE_SIZE); + "%sJoint %s received invalid number of command interfaces. Received %ld, expected %d%s", + lbr_fri_ros2::ColorScheme::FAIL, joint.name.c_str(), joint.command_interfaces.size(), + LBR_FRI_COMMAND_INTERFACE_SIZE, lbr_fri_ros2::ColorScheme::ENDC); return false; } for (auto &ci : joint.command_interfaces) { if (ci.name != hardware_interface::HW_IF_POSITION && ci.name != hardware_interface::HW_IF_EFFORT) { RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), - "Joint %s received invalid command interface: %s. Expected %s or %s.", - joint.name.c_str(), ci.name.c_str(), hardware_interface::HW_IF_POSITION, - hardware_interface::HW_IF_EFFORT); + "%sJoint %s received invalid command interface: %s. Expected %s or %s%s", + lbr_fri_ros2::ColorScheme::FAIL, joint.name.c_str(), ci.name.c_str(), + hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_EFFORT, + lbr_fri_ros2::ColorScheme::ENDC); return false; } } @@ -371,8 +375,9 @@ bool SystemInterface::verify_joint_state_interfaces_() { if (joint.state_interfaces.size() != LBR_FRI_STATE_INTERFACE_SIZE) { RCLCPP_ERROR( rclcpp::get_logger(LOGGER_NAME), - "Joint %s received invalid number of state interfaces. Received %ld, expected %d.", - joint.name.c_str(), joint.state_interfaces.size(), LBR_FRI_STATE_INTERFACE_SIZE); + "%sJoint %s received invalid number of state interfaces. Received %ld, expected %d%s", + lbr_fri_ros2::ColorScheme::FAIL, joint.name.c_str(), joint.state_interfaces.size(), + LBR_FRI_STATE_INTERFACE_SIZE, lbr_fri_ros2::ColorScheme::ENDC); return false; } for (auto &si : joint.state_interfaces) { @@ -381,13 +386,14 @@ bool SystemInterface::verify_joint_state_interfaces_() { si.name != hardware_interface::HW_IF_EFFORT && si.name != HW_IF_COMMANDED_TORQUE && si.name != HW_IF_EXTERNAL_TORQUE && si.name != HW_IF_IPO_JOINT_POSITION && si.name != hardware_interface::HW_IF_VELOCITY) { - RCLCPP_ERROR( - rclcpp::get_logger(LOGGER_NAME), - "Joint %s received invalid state interface: %s. Expected %s, %s, %s, %s, %s, %s or %s.", - joint.name.c_str(), si.name.c_str(), hardware_interface::HW_IF_POSITION, - HW_IF_COMMANDED_JOINT_POSITION, hardware_interface::HW_IF_EFFORT, - HW_IF_COMMANDED_TORQUE, HW_IF_EXTERNAL_TORQUE, HW_IF_IPO_JOINT_POSITION, - hardware_interface::HW_IF_VELOCITY); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), + "%sJoint %s received invalid state interface: %s. Expected %s, %s, %s, %s, " + "%s, %s or %s%s", + lbr_fri_ros2::ColorScheme::FAIL, joint.name.c_str(), si.name.c_str(), + hardware_interface::HW_IF_POSITION, HW_IF_COMMANDED_JOINT_POSITION, + hardware_interface::HW_IF_EFFORT, HW_IF_COMMANDED_TORQUE, + HW_IF_EXTERNAL_TORQUE, HW_IF_IPO_JOINT_POSITION, + hardware_interface::HW_IF_VELOCITY, lbr_fri_ros2::ColorScheme::ENDC); return false; } } @@ -398,8 +404,9 @@ bool SystemInterface::verify_joint_state_interfaces_() { bool SystemInterface::verify_sensors_() { // check lbr specific state interfaces if (info_.sensors.size() != LBR_FRI_SENSORS) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Expected %d sensor, got %ld", LBR_FRI_SENSORS, - info_.sensors.size()); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "%sExpected %d sensor, got %ld%s", + lbr_fri_ros2::ColorScheme::FAIL, LBR_FRI_SENSORS, info_.sensors.size(), + lbr_fri_ros2::ColorScheme::ENDC); return false; } if (!verify_auxiliary_sensor_()) { @@ -416,9 +423,10 @@ bool SystemInterface::verify_auxiliary_sensor_() { const auto &auxiliary_sensor = info_.sensors[0]; if (auxiliary_sensor.state_interfaces.size() != AUXILIARY_SENSOR_SIZE) { RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), - "Sensor %s received invalid state interface. Received %ld, expected %d. ", - auxiliary_sensor.name.c_str(), auxiliary_sensor.state_interfaces.size(), - AUXILIARY_SENSOR_SIZE); + "%sSensor %s received invalid state interface. Received %ld, expected %d%s", + lbr_fri_ros2::ColorScheme::FAIL, auxiliary_sensor.name.c_str(), + auxiliary_sensor.state_interfaces.size(), AUXILIARY_SENSOR_SIZE, + lbr_fri_ros2::ColorScheme::ENDC); return false; } // check only valid interfaces are defined @@ -432,8 +440,9 @@ bool SystemInterface::verify_auxiliary_sensor_() { si.name != HW_IF_COMMANDED_TORQUE && si.name != HW_IF_EXTERNAL_TORQUE && si.name != HW_IF_IPO_JOINT_POSITION && si.name != HW_IF_TRACKING_PERFORMANCE) { RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), - "Sensor %s received invalid state interface %s.", auxiliary_sensor.name.c_str(), - si.name.c_str()); + "%sSensor %s received invalid state interface %s%s", + lbr_fri_ros2::ColorScheme::FAIL, auxiliary_sensor.name.c_str(), si.name.c_str(), + lbr_fri_ros2::ColorScheme::ENDC); return false; } } @@ -444,9 +453,10 @@ bool SystemInterface::verify_estimated_ft_sensor_() { const auto &estimated_ft_sensor = info_.sensors[1]; if (estimated_ft_sensor.state_interfaces.size() != ESTIMATED_FT_SENSOR_SIZE) { RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), - "Sensor %s received invalid state interface. Received %ld, expected %d. ", - estimated_ft_sensor.name.c_str(), estimated_ft_sensor.state_interfaces.size(), - ESTIMATED_FT_SENSOR_SIZE); + "%sSensor %s received invalid state interface. Received %ld, expected %d%s", + lbr_fri_ros2::ColorScheme::FAIL, estimated_ft_sensor.name.c_str(), + estimated_ft_sensor.state_interfaces.size(), ESTIMATED_FT_SENSOR_SIZE, + lbr_fri_ros2::ColorScheme::ENDC); return false; } // check only valid interfaces are defined @@ -454,8 +464,9 @@ bool SystemInterface::verify_estimated_ft_sensor_() { if (si.name != HW_IF_FORCE_X && si.name != HW_IF_FORCE_Y && si.name != HW_IF_FORCE_Z && si.name != HW_IF_TORQUE_X && si.name != HW_IF_TORQUE_Y && si.name != HW_IF_TORQUE_Z) { RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), - "Sensor %s received invalid state interface %s.", - estimated_ft_sensor.name.c_str(), si.name.c_str()); + "%sSensor %s received invalid state interface %s%s", + lbr_fri_ros2::ColorScheme::FAIL, estimated_ft_sensor.name.c_str(), + si.name.c_str(), lbr_fri_ros2::ColorScheme::ENDC); return false; } } From a11c5cecc6ef60963323b010418ab1c871fd283d Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 3 Jan 2024 17:04:52 +0000 Subject: [PATCH 06/76] improved log --- lbr_ros2_control/src/system_interface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 5bff79c6..dd12d206 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -209,7 +209,7 @@ controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_l uint8_t attempt = 0; while (!async_client_ptr_->get_state_interface().is_initialized() && rclcpp::ok()) { RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), - "Waiting for robot heartbeat. Attempt: [%d], port id: %s%s%d%s", ++attempt, + "Waiting for robot heartbeat. Attempt: %d, port id: %s%s%d%s", ++attempt, lbr_fri_ros2::ColorScheme::OKBLUE, lbr_fri_ros2::ColorScheme::BOLD, parameters_.port_id, lbr_fri_ros2::ColorScheme::ENDC); std::this_thread::sleep_for(std::chrono::seconds(1)); From ef2d53c54d03847c1bcd1544c608d9f82af219a6 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 3 Jan 2024 18:48:34 +0000 Subject: [PATCH 07/76] updated namespace --- lbr_description/gazebo/lbr.gazebo.xacro | 5 ++++- lbr_description/lbr_description/launch_mixin.py | 2 -- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/lbr_description/gazebo/lbr.gazebo.xacro b/lbr_description/gazebo/lbr.gazebo.xacro index 11f66a0a..1aaaa19a 100644 --- a/lbr_description/gazebo/lbr.gazebo.xacro +++ b/lbr_description/gazebo/lbr.gazebo.xacro @@ -6,7 +6,10 @@ $(find lbr_ros2_control)/config/lbr_controllers.yaml - /${robot_name} + + /${robot_name} + ~/robot_description:=robot_description + diff --git a/lbr_description/lbr_description/launch_mixin.py b/lbr_description/lbr_description/launch_mixin.py index 73aef543..6c79423e 100644 --- a/lbr_description/lbr_description/launch_mixin.py +++ b/lbr_description/lbr_description/launch_mixin.py @@ -43,8 +43,6 @@ def node_spawn_entity( "robot_description", "-entity", robot_name, - "-robot_namespace", - robot_name, ], output="screen", namespace=robot_name, From d25bf2fddb2a08b25e876e232482958011e0f9ee Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 3 Jan 2024 18:51:20 +0000 Subject: [PATCH 08/76] added comment --- lbr_description/gazebo/lbr.gazebo.xacro | 1 + 1 file changed, 1 insertion(+) diff --git a/lbr_description/gazebo/lbr.gazebo.xacro b/lbr_description/gazebo/lbr.gazebo.xacro index 1aaaa19a..49495d90 100644 --- a/lbr_description/gazebo/lbr.gazebo.xacro +++ b/lbr_description/gazebo/lbr.gazebo.xacro @@ -8,6 +8,7 @@ $(find lbr_ros2_control)/config/lbr_controllers.yaml /${robot_name} + ~/robot_description:=robot_description From 1c9dabbc882c6d6e90d0850fb6d283728d9a7d29 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 3 Jan 2024 19:04:07 +0000 Subject: [PATCH 09/76] replaced foxglove with rviz --- README.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 1ffa6d30..cdae3959 100644 --- a/README.md +++ b/README.md @@ -14,10 +14,10 @@ ROS 2 packages for the KUKA LBR, including communication to the real robot via t LBR Med14 R820 - LBR IIWA7 R800 - LBR IIWA14 R820 - LBR Med7 R800 - LBR Med14 R820 + LBR IIWA7 R800 + LBR IIWA14 R820 + LBR Med7 R800 + LBR Med14 R820 From 1e200b5b6d675d1d023be6a1114aa6c27e17a2f9 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 3 Jan 2024 19:19:54 +0000 Subject: [PATCH 10/76] documentation update --- lbr_bringup/doc/lbr_bringup.rst | 65 +++++++++------------------------ 1 file changed, 18 insertions(+), 47 deletions(-) diff --git a/lbr_bringup/doc/lbr_bringup.rst b/lbr_bringup/doc/lbr_bringup.rst index ff1b15f5..cf70a42d 100644 --- a/lbr_bringup/doc/lbr_bringup.rst +++ b/lbr_bringup/doc/lbr_bringup.rst @@ -1,34 +1,5 @@ LBR Bringup =========== -The ``lbr_fri_ros2_stack`` is designed for research **and** deployment. It runs standalone, with ``ros2_control``, and thus also with ``MoveIt 2``. Details are described in below sections - -- :ref:`ROS 2 Control and MoveIt 2 Launch` -- :ref:`Standalone Launch` - -Users may also refer to :ref:`Software Architecture` for a better understanding of the underlying ``lbr_fri_ros2`` package. - -.. note:: - For the real robot, make sure you have followed :ref:`Robot Setup` first. - -.. warning:: - On the real robot, do always execute in ``T1`` mode first. - -General Information on the FRI ------------------------------- -The ``FRI`` lets the user select a ``FRI control mode`` and a ``FRI client command mode``. When running the ``LBRServer``: - -- .. dropdown:: Select ``FRI control mode`` - - .. thumbnail:: ../../lbr_fri_ros2_stack/doc/img/controller/raw/lbr_server_control_mode.png - -- .. dropdown:: Select ``FRI client command mode`` - - .. thumbnail:: ../../lbr_fri_ros2_stack/doc/img/controller/raw/lbr_server_client_command_mode.png - -The ``FRI control mode`` specifies the mode in which the robot is controlled, and the ``FRI client command mode`` specifies the commands that the user sends. - -ROS 2 Control and MoveIt 2 Launch ---------------------------------- The ``lbr_bringup`` works for the simulation and the real robot. Run: .. code:: bash @@ -42,44 +13,44 @@ The ``lbr_bringup`` works for the simulation and the real robot. Run: .. note:: For a list of available parameters, call ``ros2 launch lbr_bringup bringup.launch.py -s``. -When using the real robot +When using the real robot, select ``sim:=false`` and .. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD`` .. thumbnail:: ../../lbr_demos/doc/img/applications_lbr_server.png -and select: +Select - ``FRI send period``: ``10 ms`` - ``IP address``: ``your configuration`` - ``FRI control mode``: ``POSITION_CONTROL`` or ``JOINT_IMPEDANCE_CONTROL`` (will put the robot into a compliant mode) - ``FRI client command mode``: ``POSITION`` -Make sure that the ``update_rate`` in `lbr_controllers.yaml `_ is greater or equal ``100`` (``FRI send period``). +Users may also refer to :ref:`Software Architecture` for a better understanding of the underlying ``lbr_fri_ros2`` package. -For using other ``FRI send period``, also change the ``sample_time`` in the `lbr_system_interface.xacro `_ (automated in the future). +.. note:: + For the real robot, make sure you have followed :ref:`Robot Setup` first. -Standalone Launch ------------------ -Standalone launch is great for research. Only the the real robot is supported. It can be launched through: +.. warning:: + On the real robot, do always execute in ``T1`` mode first. -.. code:: bash +General Information on the FRI +------------------------------ +The ``FRI`` lets the user select a ``FRI control mode`` and a ``FRI client command mode``. When running the ``LBRServer``: - ros2 launch lbr_fri_ros2 app.launch.py \ - model:=iiwa7 # [iiwa7, iiwa14, med7, med14] \ - robot_name:=lbr # any robot name +- .. dropdown:: Select ``FRI control mode`` -This runs the :lbr_fri_ros2:`AppComponent `, which creates 2 topics, ``/robot_name/command`` for commands and ``/robot_name/state``. See :ref:`LBR Demos FRI ROS 2` for more examples and :ref:`LBR FRI ROS 2` for more documentation. + .. thumbnail:: ../../lbr_fri_ros2_stack/doc/img/controller/raw/lbr_server_control_mode.png -.. note:: - For a list of available parameters, call ``ros2 launch lbr_fri_ros2 app.launch.py -s``. +- .. dropdown:: Select ``FRI client command mode`` + + .. thumbnail:: ../../lbr_fri_ros2_stack/doc/img/controller/raw/lbr_server_client_command_mode.png + +The ``FRI control mode`` specifies the mode in which the robot is controlled, and the ``FRI client command mode`` specifies the commands that the user sends. Troubleshooting --------------- Noisy Execution ~~~~~~~~~~~~~~~ -Three main causes: - -- Frequency: Make sure the ``ros2_control_node`` runs at the same or a higher rate of the ``FRI send period``, change ``update_rate`` in `lbr_controllers.yaml `_. -- Standalone noise: Smoothing might be required, see :ref:`LBR Demos FRI ROS 2`. +- Frequency: Make sure the ``ros2_control_node`` frequency and the ``FRI send period`` are compatible, consider changing ``update_rate`` in `lbr_controllers.yaml `_. - Realtime priority: Set real time priority in ``code /etc/security/limits.conf``, add the line: ``user - rtprio 99``, where user is your username. From e3ed295c1053e5954cbaec1832bdef8a7fa7ea29 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 4 Jan 2024 10:31:38 +0000 Subject: [PATCH 11/76] added re-write hints --- lbr_bringup/doc/lbr_bringup.rst | 2 ++ lbr_demos/doc/lbr_demos.rst | 3 +++ lbr_fri_ros2/doc/lbr_fri_ros2.rst | 2 ++ 3 files changed, 7 insertions(+) diff --git a/lbr_bringup/doc/lbr_bringup.rst b/lbr_bringup/doc/lbr_bringup.rst index cf70a42d..36d0df49 100644 --- a/lbr_bringup/doc/lbr_bringup.rst +++ b/lbr_bringup/doc/lbr_bringup.rst @@ -1,5 +1,7 @@ LBR Bringup =========== +TODO: Mention different controllers here already + The ``lbr_bringup`` works for the simulation and the real robot. Run: .. code:: bash diff --git a/lbr_demos/doc/lbr_demos.rst b/lbr_demos/doc/lbr_demos.rst index bc4ec82f..33f4ec43 100644 --- a/lbr_demos/doc/lbr_demos.rst +++ b/lbr_demos/doc/lbr_demos.rst @@ -5,6 +5,9 @@ Demos for controlling the LBR through the Fast Robot Interface (FRI) from ROS 2. .. warning:: On the real robot, do always execute in ``T1`` mode first. +TODO: Differentiate demos through controllers +TODO: indicate which demos run in simulation and which on the real robot + LBR Demos FRI ROS 2 ------------------- .. note:: diff --git a/lbr_fri_ros2/doc/lbr_fri_ros2.rst b/lbr_fri_ros2/doc/lbr_fri_ros2.rst index c420f8a8..6c6dc8ee 100644 --- a/lbr_fri_ros2/doc/lbr_fri_ros2.rst +++ b/lbr_fri_ros2/doc/lbr_fri_ros2.rst @@ -1,5 +1,7 @@ LBR FRI ROS 2 ============= +TODO: massive re-write of this section, since architecture has changed. + The ``lbr_fri_ros2`` package provides a ROS 2 interface for the KUKA LBRs. It is designed to run stand-alone **and** within ``ros2_control``. Quick Start From f41901b6fe703d52d9f31b268ec08a3082870ce6 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 4 Jan 2024 12:47:38 +0000 Subject: [PATCH 12/76] added world robot transform --- lbr_bringup/launch/real.launch.py | 3 ++- lbr_bringup/launch/sim.launch.py | 5 +++-- lbr_description/lbr_description/launch_mixin.py | 6 +++++- 3 files changed, 10 insertions(+), 4 deletions(-) diff --git a/lbr_bringup/launch/real.launch.py b/lbr_bringup/launch/real.launch.py index 53c84222..42dbf1b5 100644 --- a/lbr_bringup/launch/real.launch.py +++ b/lbr_bringup/launch/real.launch.py @@ -20,6 +20,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: ld = LaunchDescription() robot_description = LBRDescriptionMixin.param_robot_description(sim=False) + world_robot_tf = [0, 0, 0, 0, 0, 0] # keep zero # robot state publisher robot_state_publisher = LBRROS2ControlMixin.node_robot_state_publisher( @@ -72,7 +73,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: robot_name = LaunchConfiguration("robot_name").perform(context) ld.add_action( LBRDescriptionMixin.node_static_tf( - tf=[0, 0, 0, 0, 0, 0], # keep zero + tf=world_robot_tf, parent="world", child=PathJoinSubstitution( [ diff --git a/lbr_bringup/launch/sim.launch.py b/lbr_bringup/launch/sim.launch.py index 9fd377f9..548a35c0 100644 --- a/lbr_bringup/launch/sim.launch.py +++ b/lbr_bringup/launch/sim.launch.py @@ -20,8 +20,9 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: ld = LaunchDescription() robot_description = LBRDescriptionMixin.param_robot_description(sim=True) + world_robot_tf = [0, 0, 0, 0, 0, 0] # keep zero ld.add_action(GazeboMixin.include_gazebo()) # Gazebo has its own controller manager - spawn_entity = GazeboMixin.node_spawn_entity() + spawn_entity = GazeboMixin.node_spawn_entity(tf=world_robot_tf) ld.add_action(spawn_entity) joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner( controller="joint_state_broadcaster" @@ -53,7 +54,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: robot_name = LaunchConfiguration("robot_name").perform(context) ld.add_action( LBRDescriptionMixin.node_static_tf( - tf=[0, 0, 0, 0, 0, 0], # keep zero + tf=world_robot_tf, parent="world", child=PathJoinSubstitution( [ diff --git a/lbr_description/lbr_description/launch_mixin.py b/lbr_description/lbr_description/launch_mixin.py index 6c79423e..6cafd13f 100644 --- a/lbr_description/lbr_description/launch_mixin.py +++ b/lbr_description/lbr_description/launch_mixin.py @@ -33,8 +33,11 @@ def node_spawn_entity( robot_name: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( "robot_name", default="lbr" ), + tf: List[float] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], **kwargs, ) -> Node: + label = ["-x", "-y", "-z", "-R", "-P", "-Y"] + tf = [str(x) for x in tf] return Node( package="gazebo_ros", executable="spawn_entity.py", @@ -43,7 +46,8 @@ def node_spawn_entity( "robot_description", "-entity", robot_name, - ], + ] + + [item for pair in zip(label, tf) for item in pair], output="screen", namespace=robot_name, **kwargs, From 79ad6b3bf8cfb7eb07106e539a2881d1b59807da Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 5 Jan 2024 11:04:29 +0000 Subject: [PATCH 13/76] added connection check on rclcpp::ok exit --- lbr_fri_ros2/src/app.cpp | 4 +--- lbr_ros2_control/src/system_interface.cpp | 15 ++++++++++++--- 2 files changed, 13 insertions(+), 6 deletions(-) diff --git a/lbr_fri_ros2/src/app.cpp b/lbr_fri_ros2/src/app.cpp index aab47622..dab67224 100644 --- a/lbr_fri_ros2/src/app.cpp +++ b/lbr_fri_ros2/src/app.cpp @@ -91,9 +91,7 @@ void App::run(int rt_prio) { ColorScheme::ENDC); } } else { - RCLCPP_WARN(rclcpp::get_logger(LOGGER_NAME), - "%sRealtime kernel recommended but not required%s", ColorScheme::WARNING, - ColorScheme::ENDC); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Realtime kernel recommended but not required"); } RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Starting run thread"); diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index dd12d206..300e49c5 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -209,11 +209,20 @@ controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_l uint8_t attempt = 0; while (!async_client_ptr_->get_state_interface().is_initialized() && rclcpp::ok()) { RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), - "Waiting for robot heartbeat. Attempt: %d, port id: %s%s%d%s", ++attempt, - lbr_fri_ros2::ColorScheme::OKBLUE, lbr_fri_ros2::ColorScheme::BOLD, - parameters_.port_id, lbr_fri_ros2::ColorScheme::ENDC); + "Awaiting robot heartbeat. Attempt: %d, remote host: %s%s%s%s, port id: %s%s%d%s", + ++attempt, lbr_fri_ros2::ColorScheme::OKBLUE, lbr_fri_ros2::ColorScheme::BOLD, + parameters_.remote_host == NULL ? "INADDR_ANY" : parameters_.remote_host, + lbr_fri_ros2::ColorScheme::ENDC, lbr_fri_ros2::ColorScheme::OKBLUE, + lbr_fri_ros2::ColorScheme::BOLD, parameters_.port_id, + lbr_fri_ros2::ColorScheme::ENDC); std::this_thread::sleep_for(std::chrono::seconds(1)); } + if (!async_client_ptr_->get_state_interface() + .is_initialized()) { // check connection should rclcpp::ok() fail + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "%sFailed to connect%s", + lbr_fri_ros2::ColorScheme::FAIL, lbr_fri_ros2::ColorScheme::ENDC); + return controller_interface::CallbackReturn::ERROR; + } RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "%sRobot connected%s", lbr_fri_ros2::ColorScheme::OKGREEN, lbr_fri_ros2::ColorScheme::ENDC); RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Control mode: '%s'", From 753e0815696b66bafd5256e3b9c4b8e8bcf12bf3 Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 5 Jan 2024 11:05:36 +0000 Subject: [PATCH 14/76] changed to info --- lbr_ros2_control/src/system_interface.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 300e49c5..0946b360 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -219,8 +219,7 @@ controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_l } if (!async_client_ptr_->get_state_interface() .is_initialized()) { // check connection should rclcpp::ok() fail - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "%sFailed to connect%s", - lbr_fri_ros2::ColorScheme::FAIL, lbr_fri_ros2::ColorScheme::ENDC); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "%sFailed to connect%s"); return controller_interface::CallbackReturn::ERROR; } RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "%sRobot connected%s", From 68ce7285ba663261e7e977f93189b6cf09b4c87e Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 5 Jan 2024 11:06:09 +0000 Subject: [PATCH 15/76] removed typo --- lbr_ros2_control/src/system_interface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 0946b360..18f945cc 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -219,7 +219,7 @@ controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_l } if (!async_client_ptr_->get_state_interface() .is_initialized()) { // check connection should rclcpp::ok() fail - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "%sFailed to connect%s"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Failed to connect"); return controller_interface::CallbackReturn::ERROR; } RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "%sRobot connected%s", From 96c1994b9a5c8ea6bae542c6d72588c1c92c8cb8 Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 5 Jan 2024 11:25:07 +0000 Subject: [PATCH 16/76] adding ros2 control node event handler https://github.com/ros-controls/ros2_control/issues/1262 --- lbr_bringup/launch/real.launch.py | 8 +++++++- lbr_ros2_control/launch/system_interface.launch.py | 8 +++++++- 2 files changed, 14 insertions(+), 2 deletions(-) diff --git a/lbr_bringup/launch/real.launch.py b/lbr_bringup/launch/real.launch.py index 42dbf1b5..fbf050aa 100644 --- a/lbr_bringup/launch/real.launch.py +++ b/lbr_bringup/launch/real.launch.py @@ -30,7 +30,13 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: # ros2 control node ros2_control_node = LBRROS2ControlMixin.node_ros2_control() - ld.add_action(ros2_control_node) + ros2_control_node_event_handler = RegisterEventHandler( + OnProcessStart( + target_action=robot_state_publisher, + on_start=[ros2_control_node], + ) + ) + ld.add_action(ros2_control_node_event_handler) # joint state broad caster and controller on ros2 control node start joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner( diff --git a/lbr_ros2_control/launch/system_interface.launch.py b/lbr_ros2_control/launch/system_interface.launch.py index 3043ef73..847480dd 100644 --- a/lbr_ros2_control/launch/system_interface.launch.py +++ b/lbr_ros2_control/launch/system_interface.launch.py @@ -26,7 +26,13 @@ def generate_launch_description() -> LaunchDescription: ) ld.add_action(robot_state_publisher) ros2_control_node = LBRSystemInterface.node_ros2_control() - ld.add_action(ros2_control_node) + ros2_control_node_event_handler = RegisterEventHandler( + OnProcessStart( + target_action=robot_state_publisher, + on_start=[ros2_control_node], + ) + ) + ld.add_action(ros2_control_node_event_handler) joint_state_broadcaster = LBRSystemInterface.node_controller_spawner( controller="joint_state_broadcaster" ) From 5d460b0441d1143ca32cf92ae8ce0372c7cf46ec Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 5 Jan 2024 13:35:14 +0000 Subject: [PATCH 17/76] added stream for simpler logs --- lbr_fri_ros2/src/app.cpp | 62 ++++---- lbr_fri_ros2/src/async_client.cpp | 32 +++-- lbr_fri_ros2/src/command_guard.cpp | 27 ++-- lbr_ros2_control/src/system_interface.cpp | 168 +++++++++++++--------- 4 files changed, 163 insertions(+), 126 deletions(-) diff --git a/lbr_fri_ros2/src/app.cpp b/lbr_fri_ros2/src/app.cpp index dab67224..c265af1f 100644 --- a/lbr_fri_ros2/src/app.cpp +++ b/lbr_fri_ros2/src/app.cpp @@ -16,12 +16,13 @@ App::~App() { bool App::open_udp_socket(const int &port_id, const char *const remote_host) { if (!connection_ptr_) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "%sConnection not configured%s", - ColorScheme::FAIL, ColorScheme::ENDC); + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::FAIL << "Connection not configured" << ColorScheme::ENDC); return false; } - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "%sOpening UDP socket with port_id %s%d%s", - ColorScheme::OKBLUE, ColorScheme::BOLD, port_id, ColorScheme::ENDC); + RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::OKBLUE << "Opening UDP socket with port_id '" << ColorScheme::BOLD + << port_id << "'" << ColorScheme::ENDC); if (!valid_port_(port_id)) { return false; } @@ -30,65 +31,65 @@ bool App::open_udp_socket(const int &port_id, const char *const remote_host) { return true; } if (!connection_ptr_->open(port_id, remote_host)) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "%sFailed to open socket%s", ColorScheme::FAIL, - ColorScheme::ENDC); + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::FAIL << "Failed to open socket" << ColorScheme::ENDC); return false; } - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "%sSocket opened successfully%s", - ColorScheme::OKGREEN, ColorScheme::ENDC); + RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::OKGREEN << "Socket opened successfully" << ColorScheme::ENDC); return true; } bool App::close_udp_socket() { if (!connection_ptr_) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "%sConnection not configured%s", - ColorScheme::FAIL, ColorScheme::ENDC); + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::FAIL << "Connection not configured" << ColorScheme::ENDC); return false; } - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "%sClosing UDP socket%s", ColorScheme::OKBLUE, - ColorScheme::ENDC); + RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::OKBLUE << "Closing UDP socket" << ColorScheme::ENDC); if (!connection_ptr_->isOpen()) { RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Socket already closed"); return true; } connection_ptr_->close(); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "%sSocket closed successfully%s", - ColorScheme::OKGREEN, ColorScheme::ENDC); + RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::OKGREEN << "Socket closed successfully" << ColorScheme::ENDC); return true; } void App::run(int rt_prio) { if (!async_client_ptr_) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "%sAsyncClient not configured%s", - ColorScheme::FAIL, ColorScheme::ENDC); + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::FAIL << "AsyncClient not configured" << ColorScheme::ENDC); return; } if (!connection_ptr_) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "%sConnection not configured%s", - ColorScheme::FAIL, ColorScheme::ENDC); + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::FAIL << "Connection not configured" << ColorScheme::ENDC); return; } if (!connection_ptr_->isOpen()) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "%sConnection not open%s", ColorScheme::FAIL, - ColorScheme::ENDC); + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::FAIL << "Connection not open" << ColorScheme::ENDC); return; } if (!app_ptr_) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "%sApp not configured%s", ColorScheme::FAIL, - ColorScheme::ENDC); + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::FAIL << "App not configured" << ColorScheme::ENDC); return; } if (running_) { - RCLCPP_WARN(rclcpp::get_logger(LOGGER_NAME), "%sApp already running%s", ColorScheme::WARNING, - ColorScheme::ENDC); + RCLCPP_WARN_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::WARNING << "App already running" << ColorScheme::ENDC); return; } run_thread_ = std::thread([&]() { if (realtime_tools::has_realtime_kernel()) { if (!realtime_tools::configure_sched_fifo(rt_prio)) { - RCLCPP_WARN(rclcpp::get_logger(LOGGER_NAME), - "%sFailed to set FIFO realtime scheduling policy%s", ColorScheme::WARNING, - ColorScheme::ENDC); + RCLCPP_WARN_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::WARNING << "Failed to set FIFO realtime scheduling policy" + << ColorScheme::ENDC); } } else { RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Realtime kernel recommended but not required"); @@ -119,9 +120,10 @@ void App::stop_run() { bool App::valid_port_(const int &port_id) { if (port_id < 30200 || port_id > 30209) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), - "%sExpected port_id in [30200, 30209], got %s%d%s", ColorScheme::FAIL, - ColorScheme::BOLD, port_id, ColorScheme::ENDC); + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::FAIL << "Expected port_id in [30200, 30209], got '" + << ColorScheme::BOLD << port_id << "'" + << ColorScheme::ENDC); return false; } return true; diff --git a/lbr_fri_ros2/src/async_client.cpp b/lbr_fri_ros2/src/async_client.cpp index 2eda3961..96ccb9e3 100644 --- a/lbr_fri_ros2/src/async_client.cpp +++ b/lbr_fri_ros2/src/async_client.cpp @@ -8,24 +8,27 @@ AsyncClient::AsyncClient(const PIDParameters &pid_parameters, const bool &open_loop) : command_interface_(pid_parameters, command_guard_parameters, command_guard_variant), state_interface_(state_interface_parameters), open_loop_(open_loop) { - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "%sConfiguring client%s", ColorScheme::OKBLUE, - ColorScheme::ENDC); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Command guard variant: '%s'", - command_guard_variant.c_str()); + RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::OKBLUE << "Configuring client" << ColorScheme::ENDC); + RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), + "Command guard variant '" << command_guard_variant.c_str() << "'"); command_interface_.log_info(); state_interface_.log_info(); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Open loop: '%s'", open_loop_ ? "true" : "false"); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "%sClient configured%s", ColorScheme::OKGREEN, - ColorScheme::ENDC); + RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), + "Open loop '" << (open_loop_ ? "true" : "false") << "'"); + RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::OKGREEN << "Client configured" << ColorScheme::ENDC); } void AsyncClient::onStateChange(KUKA::FRI::ESessionState old_state, KUKA::FRI::ESessionState new_state) { - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "LBR switched from '%s%s%s%s' to '%s%s%s%s'.", - ColorScheme::OKBLUE, ColorScheme::BOLD, - EnumMaps::session_state_map(old_state).c_str(), ColorScheme::ENDC, - ColorScheme::OKGREEN, ColorScheme::BOLD, - EnumMaps::session_state_map(new_state).c_str(), ColorScheme::ENDC); + RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), + "LBR switched from '" + << ColorScheme::OKBLUE << ColorScheme::BOLD + << EnumMaps::session_state_map(old_state).c_str() << ColorScheme::ENDC + << "' to '" << ColorScheme::OKGREEN << ColorScheme::BOLD + << EnumMaps::session_state_map(new_state).c_str() << ColorScheme::ENDC + << "'"); command_interface_.init_command(robotState()); } @@ -64,8 +67,9 @@ void AsyncClient::command() { return; default: std::string err = - "Unsupported command mode: " + std::to_string(robotState().getClientCommandMode()) + "."; - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); + "Unsupported command mode '" + std::to_string(robotState().getClientCommandMode()) + "'"; + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::FAIL << err << ColorScheme::ENDC); throw std::runtime_error(err); } } diff --git a/lbr_fri_ros2/src/command_guard.cpp b/lbr_fri_ros2/src/command_guard.cpp index 7911ff86..a8ad519d 100644 --- a/lbr_fri_ros2/src/command_guard.cpp +++ b/lbr_fri_ros2/src/command_guard.cpp @@ -51,9 +51,10 @@ bool CommandGuard::command_in_position_limits_(const_idl_command_t_ref lbr_comma for (std::size_t i = 0; i < lbr_command.joint_position.size(); ++i) { if (lbr_command.joint_position[i] < parameters_.min_position[i] || lbr_command.joint_position[i] > parameters_.max_position[i]) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), - "%sPosition command not in limits for joint '%s'%s", ColorScheme::FAIL, - parameters_.joint_names[i].c_str(), ColorScheme::ENDC); + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::FAIL << "Position not in limits for joint '" + << parameters_.joint_names[i].c_str() << "'" + << ColorScheme::ENDC); return false; } } @@ -66,8 +67,10 @@ bool CommandGuard::command_in_velocity_limits_(const_idl_command_t_ref lbr_comma for (std::size_t i = 0; i < lbr_command.joint_position[i]; ++i) { if (std::abs(lbr_command.joint_position[i] - lbr_state.getMeasuredJointPosition()[i]) / dt > parameters_.max_velocity[i]) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "%sVelocity not in limits for joint '%s'%s", - ColorScheme::FAIL, parameters_.joint_names[i].c_str(), ColorScheme::ENDC); + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::FAIL << "Velocity not in limits for joint '" + << parameters_.joint_names[i].c_str() << "'" + << ColorScheme::ENDC); return false; } } @@ -79,9 +82,10 @@ bool CommandGuard::command_in_torque_limits_(const_idl_command_t_ref lbr_command for (std::size_t i = 0; i < lbr_command.torque.size(); ++i) { if (std::abs(lbr_command.torque[i] + lbr_state.getExternalTorque()[i]) > parameters_.max_torque[i]) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), - "%sTorque command not in limits for joint '%s'%s", ColorScheme::FAIL, - parameters_.joint_names[i].c_str(), ColorScheme::ENDC); + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), ColorScheme::FAIL + << "Torque not in limits for joint '" + << parameters_.joint_names[i].c_str() + << "'" << ColorScheme::ENDC); return false; } } @@ -95,9 +99,10 @@ bool SafeStopCommandGuard::command_in_position_limits_(const_idl_command_t_ref l parameters_.min_position[i] + parameters_.max_velocity[i] * lbr_state.getSampleTime() || lbr_command.joint_position[i] > parameters_.max_position[i] - parameters_.max_velocity[i] * lbr_state.getSampleTime()) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), - "%sPosition command not in limits for joint '%s'%s", ColorScheme::FAIL, - parameters_.joint_names[i].c_str(), ColorScheme::ENDC); + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::FAIL << "Position not in limits for joint '" + << parameters_.joint_names[i].c_str() << "'" + << ColorScheme::ENDC); return false; } } diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 18f945cc..48bf4ef7 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -5,18 +5,20 @@ controller_interface::CallbackReturn SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { auto ret = hardware_interface::SystemInterface::on_init(system_info); if (ret != controller_interface::CallbackReturn::SUCCESS) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "%sFailed to initialize SystemInterface%s", - lbr_fri_ros2::ColorScheme::FAIL, lbr_fri_ros2::ColorScheme::ENDC); + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + lbr_fri_ros2::ColorScheme::FAIL << "Failed to initialize SystemInterface" + << lbr_fri_ros2::ColorScheme::ENDC); return ret; } // parameters_ from config/lbr_system_interface.xacro parameters_.port_id = std::stoul(info_.hardware_parameters["port_id"]); if (parameters_.port_id < 30200 || parameters_.port_id > 30209) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), - "%sExpected port_id in [30200, 30209]. Found %s%d%s", - lbr_fri_ros2::ColorScheme::FAIL, lbr_fri_ros2::ColorScheme::BOLD, - parameters_.port_id, lbr_fri_ros2::ColorScheme::ENDC); + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + lbr_fri_ros2::ColorScheme::FAIL + << "Expected port_id in [30200, 30209], got '" + << lbr_fri_ros2::ColorScheme::BOLD << parameters_.port_id << "'" + << lbr_fri_ros2::ColorScheme::ENDC); return controller_interface::CallbackReturn::ERROR; } info_.hardware_parameters["remote_host"] == "INADDR_ANY" @@ -198,23 +200,26 @@ SystemInterface::prepare_command_mode_switch(const std::vector & /* controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_lifecycle::State &) { if (!async_client_ptr_) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "%sAsyncClient not configured%s", - lbr_fri_ros2::ColorScheme::FAIL, lbr_fri_ros2::ColorScheme::ENDC); + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), lbr_fri_ros2::ColorScheme::FAIL + << "AsyncClient not configured" + << lbr_fri_ros2::ColorScheme::ENDC); return controller_interface::CallbackReturn::ERROR; } if (!app_ptr_->open_udp_socket(parameters_.port_id, parameters_.remote_host)) { return controller_interface::CallbackReturn::ERROR; } app_ptr_->run(parameters_.rt_prio); - uint8_t attempt = 0; + int attempt = 0; while (!async_client_ptr_->get_state_interface().is_initialized() && rclcpp::ok()) { - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), - "Awaiting robot heartbeat. Attempt: %d, remote host: %s%s%s%s, port id: %s%s%d%s", - ++attempt, lbr_fri_ros2::ColorScheme::OKBLUE, lbr_fri_ros2::ColorScheme::BOLD, - parameters_.remote_host == NULL ? "INADDR_ANY" : parameters_.remote_host, - lbr_fri_ros2::ColorScheme::ENDC, lbr_fri_ros2::ColorScheme::OKBLUE, - lbr_fri_ros2::ColorScheme::BOLD, parameters_.port_id, - lbr_fri_ros2::ColorScheme::ENDC); + RCLCPP_INFO_STREAM( + rclcpp::get_logger(LOGGER_NAME), + "Awaiting robot heartbeat. Attempt " + << ++attempt << ", remote_host '" << lbr_fri_ros2::ColorScheme::OKBLUE + << lbr_fri_ros2::ColorScheme::BOLD + << (parameters_.remote_host == NULL ? "INADDR_ANY" : parameters_.remote_host) + << lbr_fri_ros2::ColorScheme::ENDC << "', port_id '" + << lbr_fri_ros2::ColorScheme::OKBLUE << lbr_fri_ros2::ColorScheme::BOLD + << parameters_.port_id << "'" << lbr_fri_ros2::ColorScheme::ENDC); std::this_thread::sleep_for(std::chrono::seconds(1)); } if (!async_client_ptr_->get_state_interface() @@ -222,11 +227,14 @@ controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_l RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Failed to connect"); return controller_interface::CallbackReturn::ERROR; } - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "%sRobot connected%s", - lbr_fri_ros2::ColorScheme::OKGREEN, lbr_fri_ros2::ColorScheme::ENDC); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Control mode: '%s'", - lbr_fri_ros2::EnumMaps::control_mode_map(hw_lbr_state_.control_mode).c_str()); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Sample time: %.3f s / %.1f Hz", + RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), lbr_fri_ros2::ColorScheme::OKGREEN + << "Robot connected" + << lbr_fri_ros2::ColorScheme::ENDC); + RCLCPP_INFO_STREAM( + rclcpp::get_logger(LOGGER_NAME), + "Control mode '" + << lbr_fri_ros2::EnumMaps::control_mode_map(hw_lbr_state_.control_mode).c_str() << "'"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Sample time %.3f s / %.1f Hz", async_client_ptr_->get_state_interface().get_state().sample_time, 1. / async_client_ptr_->get_state_interface().get_state().sample_time); return controller_interface::CallbackReturn::SUCCESS; @@ -246,9 +254,10 @@ hardware_interface::return_type SystemInterface::read(const rclcpp::Time & /*tim // exit once robot exits COMMANDING_ACTIVE (for safety) if (exit_commanding_active_(static_cast(hw_session_state_), static_cast(hw_lbr_state_.session_state))) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), - "%sLBR left COMMANDING_ACTIVE. Please re-run lbr_bringup%s", - lbr_fri_ros2::ColorScheme::FAIL, lbr_fri_ros2::ColorScheme::ENDC); + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + lbr_fri_ros2::ColorScheme::FAIL + << "LBR left COMMANDING_ACTIVE. Please re-run lbr_bringup" + << lbr_fri_ros2::ColorScheme::ENDC); app_ptr_->stop_run(); app_ptr_->close_udp_socket(); return hardware_interface::return_type::ERROR; @@ -343,9 +352,11 @@ void SystemInterface::nan_state_interfaces_() { bool SystemInterface::verify_number_of_joints_() { if (info_.joints.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "%sExpected %d joints in URDF. Found %ld%s", - lbr_fri_ros2::ColorScheme::FAIL, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, - info_.joints.size(), lbr_fri_ros2::ColorScheme::ENDC); + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + lbr_fri_ros2::ColorScheme::FAIL + << "Expected '" << KUKA::FRI::LBRState::NUMBER_OF_JOINTS + << "' joints in URDF, got '" << info_.joints.size() << "'" + << lbr_fri_ros2::ColorScheme::ENDC); return false; } return true; @@ -355,21 +366,25 @@ bool SystemInterface::verify_joint_command_interfaces_() { // check command interfaces for (auto &joint : info_.joints) { if (joint.command_interfaces.size() != LBR_FRI_COMMAND_INTERFACE_SIZE) { - RCLCPP_ERROR( - rclcpp::get_logger(LOGGER_NAME), - "%sJoint %s received invalid number of command interfaces. Received %ld, expected %d%s", - lbr_fri_ros2::ColorScheme::FAIL, joint.name.c_str(), joint.command_interfaces.size(), - LBR_FRI_COMMAND_INTERFACE_SIZE, lbr_fri_ros2::ColorScheme::ENDC); + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + lbr_fri_ros2::ColorScheme::FAIL + << "Joint '" << joint.name.c_str() + << "' received invalid number of command interfaces. Received '" + << joint.command_interfaces.size() << "', expected " + << static_cast(LBR_FRI_COMMAND_INTERFACE_SIZE) + << lbr_fri_ros2::ColorScheme::ENDC); return false; } for (auto &ci : joint.command_interfaces) { if (ci.name != hardware_interface::HW_IF_POSITION && ci.name != hardware_interface::HW_IF_EFFORT) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), - "%sJoint %s received invalid command interface: %s. Expected %s or %s%s", - lbr_fri_ros2::ColorScheme::FAIL, joint.name.c_str(), ci.name.c_str(), - hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_EFFORT, - lbr_fri_ros2::ColorScheme::ENDC); + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + lbr_fri_ros2::ColorScheme::FAIL + << "Joint '" << joint.name.c_str() + << "' received invalid command interface '" << ci.name.c_str() + << "'. Expected '" << hardware_interface::HW_IF_POSITION << "' or '" + << hardware_interface::HW_IF_EFFORT << "'" + << lbr_fri_ros2::ColorScheme::ENDC); return false; } } @@ -381,11 +396,13 @@ bool SystemInterface::verify_joint_state_interfaces_() { // check state interfaces for (auto &joint : info_.joints) { if (joint.state_interfaces.size() != LBR_FRI_STATE_INTERFACE_SIZE) { - RCLCPP_ERROR( - rclcpp::get_logger(LOGGER_NAME), - "%sJoint %s received invalid number of state interfaces. Received %ld, expected %d%s", - lbr_fri_ros2::ColorScheme::FAIL, joint.name.c_str(), joint.state_interfaces.size(), - LBR_FRI_STATE_INTERFACE_SIZE, lbr_fri_ros2::ColorScheme::ENDC); + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + lbr_fri_ros2::ColorScheme::FAIL + << "Joint '" << joint.name.c_str() + << "' received invalid number of state interfaces. Received '" + << joint.state_interfaces.size() << "', expected '" + << static_cast(LBR_FRI_STATE_INTERFACE_SIZE) << "'" + << lbr_fri_ros2::ColorScheme::ENDC); return false; } for (auto &si : joint.state_interfaces) { @@ -394,14 +411,15 @@ bool SystemInterface::verify_joint_state_interfaces_() { si.name != hardware_interface::HW_IF_EFFORT && si.name != HW_IF_COMMANDED_TORQUE && si.name != HW_IF_EXTERNAL_TORQUE && si.name != HW_IF_IPO_JOINT_POSITION && si.name != hardware_interface::HW_IF_VELOCITY) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), - "%sJoint %s received invalid state interface: %s. Expected %s, %s, %s, %s, " - "%s, %s or %s%s", - lbr_fri_ros2::ColorScheme::FAIL, joint.name.c_str(), si.name.c_str(), - hardware_interface::HW_IF_POSITION, HW_IF_COMMANDED_JOINT_POSITION, - hardware_interface::HW_IF_EFFORT, HW_IF_COMMANDED_TORQUE, - HW_IF_EXTERNAL_TORQUE, HW_IF_IPO_JOINT_POSITION, - hardware_interface::HW_IF_VELOCITY, lbr_fri_ros2::ColorScheme::ENDC); + RCLCPP_ERROR_STREAM( + rclcpp::get_logger(LOGGER_NAME), + lbr_fri_ros2::ColorScheme::FAIL + << "Joint '" << joint.name.c_str() << "' received invalid state interface '" + << si.name.c_str() << "'. Expected one of '" << hardware_interface::HW_IF_POSITION + << "', '" << HW_IF_COMMANDED_JOINT_POSITION << "', '" + << hardware_interface::HW_IF_EFFORT << "', '" << HW_IF_COMMANDED_TORQUE << "', '" + << HW_IF_EXTERNAL_TORQUE << "', '" << HW_IF_IPO_JOINT_POSITION << "' or '" + << hardware_interface::HW_IF_VELOCITY << "'" << lbr_fri_ros2::ColorScheme::ENDC); return false; } } @@ -412,9 +430,11 @@ bool SystemInterface::verify_joint_state_interfaces_() { bool SystemInterface::verify_sensors_() { // check lbr specific state interfaces if (info_.sensors.size() != LBR_FRI_SENSORS) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "%sExpected %d sensor, got %ld%s", - lbr_fri_ros2::ColorScheme::FAIL, LBR_FRI_SENSORS, info_.sensors.size(), - lbr_fri_ros2::ColorScheme::ENDC); + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + lbr_fri_ros2::ColorScheme::FAIL + << "Expected '" << static_cast(LBR_FRI_SENSORS) + << "' sensors, got '" << info_.sensors.size() << "'" + << lbr_fri_ros2::ColorScheme::ENDC); return false; } if (!verify_auxiliary_sensor_()) { @@ -430,11 +450,13 @@ bool SystemInterface::verify_auxiliary_sensor_() { // check all interfaces are defined in config/lbr_system_interface.xacro const auto &auxiliary_sensor = info_.sensors[0]; if (auxiliary_sensor.state_interfaces.size() != AUXILIARY_SENSOR_SIZE) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), - "%sSensor %s received invalid state interface. Received %ld, expected %d%s", - lbr_fri_ros2::ColorScheme::FAIL, auxiliary_sensor.name.c_str(), - auxiliary_sensor.state_interfaces.size(), AUXILIARY_SENSOR_SIZE, - lbr_fri_ros2::ColorScheme::ENDC); + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + lbr_fri_ros2::ColorScheme::FAIL + << "Sensor '" << auxiliary_sensor.name.c_str() + << "' received invalid number of state interfaces." + << " Received '" << auxiliary_sensor.state_interfaces.size() + << "', expected '" << static_cast(AUXILIARY_SENSOR_SIZE) << "'" + << lbr_fri_ros2::ColorScheme::ENDC); return false; } // check only valid interfaces are defined @@ -447,10 +469,11 @@ bool SystemInterface::verify_auxiliary_sensor_() { si.name != HW_IF_TIME_STAMP_NANO_SEC && si.name != HW_IF_COMMANDED_JOINT_POSITION && si.name != HW_IF_COMMANDED_TORQUE && si.name != HW_IF_EXTERNAL_TORQUE && si.name != HW_IF_IPO_JOINT_POSITION && si.name != HW_IF_TRACKING_PERFORMANCE) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), - "%sSensor %s received invalid state interface %s%s", - lbr_fri_ros2::ColorScheme::FAIL, auxiliary_sensor.name.c_str(), si.name.c_str(), - lbr_fri_ros2::ColorScheme::ENDC); + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + lbr_fri_ros2::ColorScheme::FAIL + << "Sensor '" << auxiliary_sensor.name.c_str() + << "' received invalid state interface '" << si.name.c_str() << "'" + << lbr_fri_ros2::ColorScheme::ENDC); return false; } } @@ -460,21 +483,24 @@ bool SystemInterface::verify_auxiliary_sensor_() { bool SystemInterface::verify_estimated_ft_sensor_() { const auto &estimated_ft_sensor = info_.sensors[1]; if (estimated_ft_sensor.state_interfaces.size() != ESTIMATED_FT_SENSOR_SIZE) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), - "%sSensor %s received invalid state interface. Received %ld, expected %d%s", - lbr_fri_ros2::ColorScheme::FAIL, estimated_ft_sensor.name.c_str(), - estimated_ft_sensor.state_interfaces.size(), ESTIMATED_FT_SENSOR_SIZE, - lbr_fri_ros2::ColorScheme::ENDC); + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + lbr_fri_ros2::ColorScheme::FAIL + << "Sensor '" << estimated_ft_sensor.name.c_str() + << "' received invalid number of state interfaces. Received '" + << estimated_ft_sensor.state_interfaces.size() << "', expected '" + << static_cast(ESTIMATED_FT_SENSOR_SIZE) << "'" + << lbr_fri_ros2::ColorScheme::ENDC); return false; } // check only valid interfaces are defined for (const auto &si : estimated_ft_sensor.state_interfaces) { if (si.name != HW_IF_FORCE_X && si.name != HW_IF_FORCE_Y && si.name != HW_IF_FORCE_Z && si.name != HW_IF_TORQUE_X && si.name != HW_IF_TORQUE_Y && si.name != HW_IF_TORQUE_Z) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), - "%sSensor %s received invalid state interface %s%s", - lbr_fri_ros2::ColorScheme::FAIL, estimated_ft_sensor.name.c_str(), - si.name.c_str(), lbr_fri_ros2::ColorScheme::ENDC); + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + lbr_fri_ros2::ColorScheme::FAIL + << "Sensor '" << estimated_ft_sensor.name.c_str() + << "' received invalid state interface '" << si.name.c_str() << "'" + << lbr_fri_ros2::ColorScheme::ENDC); return false; } } From 6333ac978fce644d4afc37e7339f298fa5061fe2 Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 5 Jan 2024 13:36:38 +0000 Subject: [PATCH 18/76] FAIL -> ERROR --- .../include/lbr_fri_ros2/formatting.hpp | 2 +- lbr_fri_ros2/src/app.cpp | 20 ++++++------- lbr_fri_ros2/src/async_client.cpp | 2 +- lbr_fri_ros2/src/command_guard.cpp | 20 ++++++------- lbr_ros2_control/src/system_interface.cpp | 30 +++++++++---------- 5 files changed, 37 insertions(+), 37 deletions(-) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp index 00d3e317..464e4d4a 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp @@ -13,7 +13,7 @@ struct ColorScheme { static constexpr char OKCYAN[] = "\033[96m"; static constexpr char OKGREEN[] = "\033[92m"; static constexpr char WARNING[] = "\033[93m"; - static constexpr char FAIL[] = "\033[91m"; + static constexpr char ERROR[] = "\033[91m"; static constexpr char ENDC[] = "\033[0m"; static constexpr char BOLD[] = "\033[1m"; static constexpr char UNDERLINE[] = "\033[4m"; diff --git a/lbr_fri_ros2/src/app.cpp b/lbr_fri_ros2/src/app.cpp index c265af1f..e0a0877f 100644 --- a/lbr_fri_ros2/src/app.cpp +++ b/lbr_fri_ros2/src/app.cpp @@ -17,7 +17,7 @@ App::~App() { bool App::open_udp_socket(const int &port_id, const char *const remote_host) { if (!connection_ptr_) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), - ColorScheme::FAIL << "Connection not configured" << ColorScheme::ENDC); + ColorScheme::ERROR << "Connection not configured" << ColorScheme::ENDC); return false; } RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), @@ -32,7 +32,7 @@ bool App::open_udp_socket(const int &port_id, const char *const remote_host) { } if (!connection_ptr_->open(port_id, remote_host)) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), - ColorScheme::FAIL << "Failed to open socket" << ColorScheme::ENDC); + ColorScheme::ERROR << "Failed to open socket" << ColorScheme::ENDC); return false; } RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), @@ -43,7 +43,7 @@ bool App::open_udp_socket(const int &port_id, const char *const remote_host) { bool App::close_udp_socket() { if (!connection_ptr_) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), - ColorScheme::FAIL << "Connection not configured" << ColorScheme::ENDC); + ColorScheme::ERROR << "Connection not configured" << ColorScheme::ENDC); return false; } RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), @@ -61,22 +61,22 @@ bool App::close_udp_socket() { void App::run(int rt_prio) { if (!async_client_ptr_) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), - ColorScheme::FAIL << "AsyncClient not configured" << ColorScheme::ENDC); + ColorScheme::ERROR << "AsyncClient not configured" << ColorScheme::ENDC); return; } if (!connection_ptr_) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), - ColorScheme::FAIL << "Connection not configured" << ColorScheme::ENDC); + ColorScheme::ERROR << "Connection not configured" << ColorScheme::ENDC); return; } if (!connection_ptr_->isOpen()) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), - ColorScheme::FAIL << "Connection not open" << ColorScheme::ENDC); + ColorScheme::ERROR << "Connection not open" << ColorScheme::ENDC); return; } if (!app_ptr_) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), - ColorScheme::FAIL << "App not configured" << ColorScheme::ENDC); + ColorScheme::ERROR << "App not configured" << ColorScheme::ENDC); return; } if (running_) { @@ -121,9 +121,9 @@ void App::stop_run() { bool App::valid_port_(const int &port_id) { if (port_id < 30200 || port_id > 30209) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), - ColorScheme::FAIL << "Expected port_id in [30200, 30209], got '" - << ColorScheme::BOLD << port_id << "'" - << ColorScheme::ENDC); + ColorScheme::ERROR << "Expected port_id in [30200, 30209], got '" + << ColorScheme::BOLD << port_id << "'" + << ColorScheme::ENDC); return false; } return true; diff --git a/lbr_fri_ros2/src/async_client.cpp b/lbr_fri_ros2/src/async_client.cpp index 96ccb9e3..a08cb58c 100644 --- a/lbr_fri_ros2/src/async_client.cpp +++ b/lbr_fri_ros2/src/async_client.cpp @@ -69,7 +69,7 @@ void AsyncClient::command() { std::string err = "Unsupported command mode '" + std::to_string(robotState().getClientCommandMode()) + "'"; RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), - ColorScheme::FAIL << err << ColorScheme::ENDC); + ColorScheme::ERROR << err << ColorScheme::ENDC); throw std::runtime_error(err); } } diff --git a/lbr_fri_ros2/src/command_guard.cpp b/lbr_fri_ros2/src/command_guard.cpp index a8ad519d..3cc77737 100644 --- a/lbr_fri_ros2/src/command_guard.cpp +++ b/lbr_fri_ros2/src/command_guard.cpp @@ -52,9 +52,9 @@ bool CommandGuard::command_in_position_limits_(const_idl_command_t_ref lbr_comma if (lbr_command.joint_position[i] < parameters_.min_position[i] || lbr_command.joint_position[i] > parameters_.max_position[i]) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), - ColorScheme::FAIL << "Position not in limits for joint '" - << parameters_.joint_names[i].c_str() << "'" - << ColorScheme::ENDC); + ColorScheme::ERROR << "Position not in limits for joint '" + << parameters_.joint_names[i].c_str() << "'" + << ColorScheme::ENDC); return false; } } @@ -68,9 +68,9 @@ bool CommandGuard::command_in_velocity_limits_(const_idl_command_t_ref lbr_comma if (std::abs(lbr_command.joint_position[i] - lbr_state.getMeasuredJointPosition()[i]) / dt > parameters_.max_velocity[i]) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), - ColorScheme::FAIL << "Velocity not in limits for joint '" - << parameters_.joint_names[i].c_str() << "'" - << ColorScheme::ENDC); + ColorScheme::ERROR << "Velocity not in limits for joint '" + << parameters_.joint_names[i].c_str() << "'" + << ColorScheme::ENDC); return false; } } @@ -82,7 +82,7 @@ bool CommandGuard::command_in_torque_limits_(const_idl_command_t_ref lbr_command for (std::size_t i = 0; i < lbr_command.torque.size(); ++i) { if (std::abs(lbr_command.torque[i] + lbr_state.getExternalTorque()[i]) > parameters_.max_torque[i]) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), ColorScheme::FAIL + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), ColorScheme::ERROR << "Torque not in limits for joint '" << parameters_.joint_names[i].c_str() << "'" << ColorScheme::ENDC); @@ -100,9 +100,9 @@ bool SafeStopCommandGuard::command_in_position_limits_(const_idl_command_t_ref l lbr_command.joint_position[i] > parameters_.max_position[i] - parameters_.max_velocity[i] * lbr_state.getSampleTime()) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), - ColorScheme::FAIL << "Position not in limits for joint '" - << parameters_.joint_names[i].c_str() << "'" - << ColorScheme::ENDC); + ColorScheme::ERROR << "Position not in limits for joint '" + << parameters_.joint_names[i].c_str() << "'" + << ColorScheme::ENDC); return false; } } diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 48bf4ef7..732d7520 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -6,8 +6,8 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { auto ret = hardware_interface::SystemInterface::on_init(system_info); if (ret != controller_interface::CallbackReturn::SUCCESS) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), - lbr_fri_ros2::ColorScheme::FAIL << "Failed to initialize SystemInterface" - << lbr_fri_ros2::ColorScheme::ENDC); + lbr_fri_ros2::ColorScheme::ERROR << "Failed to initialize SystemInterface" + << lbr_fri_ros2::ColorScheme::ENDC); return ret; } @@ -15,7 +15,7 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { parameters_.port_id = std::stoul(info_.hardware_parameters["port_id"]); if (parameters_.port_id < 30200 || parameters_.port_id > 30209) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), - lbr_fri_ros2::ColorScheme::FAIL + lbr_fri_ros2::ColorScheme::ERROR << "Expected port_id in [30200, 30209], got '" << lbr_fri_ros2::ColorScheme::BOLD << parameters_.port_id << "'" << lbr_fri_ros2::ColorScheme::ENDC); @@ -200,7 +200,7 @@ SystemInterface::prepare_command_mode_switch(const std::vector & /* controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_lifecycle::State &) { if (!async_client_ptr_) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), lbr_fri_ros2::ColorScheme::FAIL + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), lbr_fri_ros2::ColorScheme::ERROR << "AsyncClient not configured" << lbr_fri_ros2::ColorScheme::ENDC); return controller_interface::CallbackReturn::ERROR; @@ -255,7 +255,7 @@ hardware_interface::return_type SystemInterface::read(const rclcpp::Time & /*tim if (exit_commanding_active_(static_cast(hw_session_state_), static_cast(hw_lbr_state_.session_state))) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), - lbr_fri_ros2::ColorScheme::FAIL + lbr_fri_ros2::ColorScheme::ERROR << "LBR left COMMANDING_ACTIVE. Please re-run lbr_bringup" << lbr_fri_ros2::ColorScheme::ENDC); app_ptr_->stop_run(); @@ -353,7 +353,7 @@ void SystemInterface::nan_state_interfaces_() { bool SystemInterface::verify_number_of_joints_() { if (info_.joints.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), - lbr_fri_ros2::ColorScheme::FAIL + lbr_fri_ros2::ColorScheme::ERROR << "Expected '" << KUKA::FRI::LBRState::NUMBER_OF_JOINTS << "' joints in URDF, got '" << info_.joints.size() << "'" << lbr_fri_ros2::ColorScheme::ENDC); @@ -367,7 +367,7 @@ bool SystemInterface::verify_joint_command_interfaces_() { for (auto &joint : info_.joints) { if (joint.command_interfaces.size() != LBR_FRI_COMMAND_INTERFACE_SIZE) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), - lbr_fri_ros2::ColorScheme::FAIL + lbr_fri_ros2::ColorScheme::ERROR << "Joint '" << joint.name.c_str() << "' received invalid number of command interfaces. Received '" << joint.command_interfaces.size() << "', expected " @@ -379,7 +379,7 @@ bool SystemInterface::verify_joint_command_interfaces_() { if (ci.name != hardware_interface::HW_IF_POSITION && ci.name != hardware_interface::HW_IF_EFFORT) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), - lbr_fri_ros2::ColorScheme::FAIL + lbr_fri_ros2::ColorScheme::ERROR << "Joint '" << joint.name.c_str() << "' received invalid command interface '" << ci.name.c_str() << "'. Expected '" << hardware_interface::HW_IF_POSITION << "' or '" @@ -397,7 +397,7 @@ bool SystemInterface::verify_joint_state_interfaces_() { for (auto &joint : info_.joints) { if (joint.state_interfaces.size() != LBR_FRI_STATE_INTERFACE_SIZE) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), - lbr_fri_ros2::ColorScheme::FAIL + lbr_fri_ros2::ColorScheme::ERROR << "Joint '" << joint.name.c_str() << "' received invalid number of state interfaces. Received '" << joint.state_interfaces.size() << "', expected '" @@ -413,7 +413,7 @@ bool SystemInterface::verify_joint_state_interfaces_() { si.name != hardware_interface::HW_IF_VELOCITY) { RCLCPP_ERROR_STREAM( rclcpp::get_logger(LOGGER_NAME), - lbr_fri_ros2::ColorScheme::FAIL + lbr_fri_ros2::ColorScheme::ERROR << "Joint '" << joint.name.c_str() << "' received invalid state interface '" << si.name.c_str() << "'. Expected one of '" << hardware_interface::HW_IF_POSITION << "', '" << HW_IF_COMMANDED_JOINT_POSITION << "', '" @@ -431,7 +431,7 @@ bool SystemInterface::verify_sensors_() { // check lbr specific state interfaces if (info_.sensors.size() != LBR_FRI_SENSORS) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), - lbr_fri_ros2::ColorScheme::FAIL + lbr_fri_ros2::ColorScheme::ERROR << "Expected '" << static_cast(LBR_FRI_SENSORS) << "' sensors, got '" << info_.sensors.size() << "'" << lbr_fri_ros2::ColorScheme::ENDC); @@ -451,7 +451,7 @@ bool SystemInterface::verify_auxiliary_sensor_() { const auto &auxiliary_sensor = info_.sensors[0]; if (auxiliary_sensor.state_interfaces.size() != AUXILIARY_SENSOR_SIZE) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), - lbr_fri_ros2::ColorScheme::FAIL + lbr_fri_ros2::ColorScheme::ERROR << "Sensor '" << auxiliary_sensor.name.c_str() << "' received invalid number of state interfaces." << " Received '" << auxiliary_sensor.state_interfaces.size() @@ -470,7 +470,7 @@ bool SystemInterface::verify_auxiliary_sensor_() { si.name != HW_IF_COMMANDED_TORQUE && si.name != HW_IF_EXTERNAL_TORQUE && si.name != HW_IF_IPO_JOINT_POSITION && si.name != HW_IF_TRACKING_PERFORMANCE) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), - lbr_fri_ros2::ColorScheme::FAIL + lbr_fri_ros2::ColorScheme::ERROR << "Sensor '" << auxiliary_sensor.name.c_str() << "' received invalid state interface '" << si.name.c_str() << "'" << lbr_fri_ros2::ColorScheme::ENDC); @@ -484,7 +484,7 @@ bool SystemInterface::verify_estimated_ft_sensor_() { const auto &estimated_ft_sensor = info_.sensors[1]; if (estimated_ft_sensor.state_interfaces.size() != ESTIMATED_FT_SENSOR_SIZE) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), - lbr_fri_ros2::ColorScheme::FAIL + lbr_fri_ros2::ColorScheme::ERROR << "Sensor '" << estimated_ft_sensor.name.c_str() << "' received invalid number of state interfaces. Received '" << estimated_ft_sensor.state_interfaces.size() << "', expected '" @@ -497,7 +497,7 @@ bool SystemInterface::verify_estimated_ft_sensor_() { if (si.name != HW_IF_FORCE_X && si.name != HW_IF_FORCE_Y && si.name != HW_IF_FORCE_Z && si.name != HW_IF_TORQUE_X && si.name != HW_IF_TORQUE_Y && si.name != HW_IF_TORQUE_Z) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), - lbr_fri_ros2::ColorScheme::FAIL + lbr_fri_ros2::ColorScheme::ERROR << "Sensor '" << estimated_ft_sensor.name.c_str() << "' received invalid state interface '" << si.name.c_str() << "'" << lbr_fri_ros2::ColorScheme::ENDC); From e02444b5db080b32a5d653c5f864005d78480748 Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 5 Jan 2024 13:49:50 +0000 Subject: [PATCH 19/76] added demos --- lbr_fri_ros2_stack/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/lbr_fri_ros2_stack/package.xml b/lbr_fri_ros2_stack/package.xml index 971eec27..f724909c 100644 --- a/lbr_fri_ros2_stack/package.xml +++ b/lbr_fri_ros2_stack/package.xml @@ -10,6 +10,7 @@ ament_cmake lbr_bringup + lbr_demos lbr_description lbr_fri_msgs lbr_fri_ros2 From acb015723684ff04d929aab67ee76bf768ca0de9 Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 5 Jan 2024 13:50:44 +0000 Subject: [PATCH 20/76] updated mail --- lbr_bringup/package.xml | 2 +- lbr_demos/lbr_demos_fri_ros2_advanced_cpp/package.xml | 2 +- lbr_demos/lbr_demos_fri_ros2_advanced_python/package.xml | 2 +- lbr_demos/lbr_demos_fri_ros2_advanced_python/setup.py | 2 +- lbr_demos/lbr_demos_fri_ros2_cpp/package.xml | 2 +- lbr_demos/lbr_demos_fri_ros2_python/package.xml | 2 +- lbr_demos/lbr_demos_fri_ros2_python/setup.py | 2 +- lbr_demos/lbr_demos_ros2_control_cpp/package.xml | 2 +- lbr_demos/lbr_demos_ros2_control_python/package.xml | 2 +- lbr_demos/lbr_demos_ros2_control_python/setup.py | 2 +- lbr_description/package.xml | 2 +- lbr_fri_msgs/package.xml | 2 +- lbr_fri_ros2/package.xml | 2 +- lbr_moveit_config/iiwa14_moveit_config/.setup_assistant | 2 +- lbr_moveit_config/iiwa14_moveit_config/package.xml | 4 ++-- lbr_moveit_config/iiwa7_moveit_config/.setup_assistant | 2 +- lbr_moveit_config/iiwa7_moveit_config/package.xml | 4 ++-- lbr_moveit_config/med14_moveit_config/.setup_assistant | 2 +- lbr_moveit_config/med14_moveit_config/package.xml | 4 ++-- lbr_moveit_config/med7_moveit_config/.setup_assistant | 2 +- lbr_moveit_config/med7_moveit_config/package.xml | 4 ++-- lbr_ros2_control/package.xml | 2 +- 22 files changed, 26 insertions(+), 26 deletions(-) diff --git a/lbr_bringup/package.xml b/lbr_bringup/package.xml index 49749b49..88b4b897 100644 --- a/lbr_bringup/package.xml +++ b/lbr_bringup/package.xml @@ -4,7 +4,7 @@ lbr_bringup 1.4.2 LBR launch files. - mhubii + mhubii MIT ament_cmake diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/package.xml b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/package.xml index 75eba02b..17a39fa8 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/package.xml +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/package.xml @@ -4,7 +4,7 @@ lbr_demos_fri_ros2_advanced_cpp 1.4.2 Advanced C++ demos for the lbr_fri_ros2. - mhubii + mhubii MIT ament_cmake diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/package.xml b/lbr_demos/lbr_demos_fri_ros2_advanced_python/package.xml index 42431403..98f824e5 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_python/package.xml +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_python/package.xml @@ -4,7 +4,7 @@ lbr_demos_fri_ros2_advanced_python 1.4.2 Advanced Python demos for the lbr_fri_ros2. - mhubii + mhubii MIT lbr_description diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/setup.py b/lbr_demos/lbr_demos_fri_ros2_advanced_python/setup.py index 760a1b67..71d7e7bf 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_python/setup.py +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_python/setup.py @@ -16,7 +16,7 @@ install_requires=["setuptools"], zip_safe=True, maintainer="mhubii", - maintainer_email="martin.huber@kcl.ac.uk", + maintainer_email="m.huber_1994@hotmail.de", description="Advanced Python demos for the lbr_fri_ros2.", license="MIT", tests_require=["pytest"], diff --git a/lbr_demos/lbr_demos_fri_ros2_cpp/package.xml b/lbr_demos/lbr_demos_fri_ros2_cpp/package.xml index 2d075c62..066c2ae7 100644 --- a/lbr_demos/lbr_demos_fri_ros2_cpp/package.xml +++ b/lbr_demos/lbr_demos_fri_ros2_cpp/package.xml @@ -4,7 +4,7 @@ lbr_demos_fri_ros2_cpp 1.4.2 C++ demos for the lbr_fri_ros2. - mhubii + mhubii MIT ament_cmake diff --git a/lbr_demos/lbr_demos_fri_ros2_python/package.xml b/lbr_demos/lbr_demos_fri_ros2_python/package.xml index c9c8386f..1c6c94ef 100644 --- a/lbr_demos/lbr_demos_fri_ros2_python/package.xml +++ b/lbr_demos/lbr_demos_fri_ros2_python/package.xml @@ -4,7 +4,7 @@ lbr_demos_fri_ros2_python 1.4.2 Python demos for the lbr_fri_ros2. - mhubii + mhubii MIT lbr_fri_msgs diff --git a/lbr_demos/lbr_demos_fri_ros2_python/setup.py b/lbr_demos/lbr_demos_fri_ros2_python/setup.py index 4b334216..1e9aa377 100644 --- a/lbr_demos/lbr_demos_fri_ros2_python/setup.py +++ b/lbr_demos/lbr_demos_fri_ros2_python/setup.py @@ -16,7 +16,7 @@ install_requires=["setuptools"], zip_safe=True, maintainer="mhubii", - maintainer_email="martin.huber@kcl.ac.uk", + maintainer_email="m.huber_1994@hotmail.de", description="Standalone Python demos for the LBR.", license="MIT", tests_require=["pytest"], diff --git a/lbr_demos/lbr_demos_ros2_control_cpp/package.xml b/lbr_demos/lbr_demos_ros2_control_cpp/package.xml index f1cc7cce..e8427b0c 100644 --- a/lbr_demos/lbr_demos_ros2_control_cpp/package.xml +++ b/lbr_demos/lbr_demos_ros2_control_cpp/package.xml @@ -4,7 +4,7 @@ lbr_demos_ros2_control_cpp 1.4.2 C++ demos for the LBR ros2_control integration. - mhubii + mhubii MIT ament_cmake diff --git a/lbr_demos/lbr_demos_ros2_control_python/package.xml b/lbr_demos/lbr_demos_ros2_control_python/package.xml index a8019b20..c45b17a6 100644 --- a/lbr_demos/lbr_demos_ros2_control_python/package.xml +++ b/lbr_demos/lbr_demos_ros2_control_python/package.xml @@ -4,7 +4,7 @@ lbr_demos_ros2_control_python 1.4.2 Python demos for the LBR ros2_control integration. - mhubii + mhubii MIT control_msgs diff --git a/lbr_demos/lbr_demos_ros2_control_python/setup.py b/lbr_demos/lbr_demos_ros2_control_python/setup.py index 7b5c3f7c..328ef493 100644 --- a/lbr_demos/lbr_demos_ros2_control_python/setup.py +++ b/lbr_demos/lbr_demos_ros2_control_python/setup.py @@ -13,7 +13,7 @@ install_requires=["setuptools"], zip_safe=True, maintainer="mhubii", - maintainer_email="martin.huber@kcl.ac.uk", + maintainer_email="m.huber_1994@hotmail.de", description="Python demos for the LBR ros2_control integration.", license="MIT", tests_require=["pytest"], diff --git a/lbr_description/package.xml b/lbr_description/package.xml index 62fd5434..52ece298 100644 --- a/lbr_description/package.xml +++ b/lbr_description/package.xml @@ -4,7 +4,7 @@ lbr_description 1.4.2 KUKA LBR description files - mhubii + mhubii MIT ament_cmake diff --git a/lbr_fri_msgs/package.xml b/lbr_fri_msgs/package.xml index cb5997e5..0e075820 100644 --- a/lbr_fri_msgs/package.xml +++ b/lbr_fri_msgs/package.xml @@ -4,7 +4,7 @@ lbr_fri_msgs 1.4.2 ROS 2 message for the Fast Robot Interface (FRI) specific states. - mhubii + mhubii MIT ament_cmake diff --git a/lbr_fri_ros2/package.xml b/lbr_fri_ros2/package.xml index 9286012f..44099b38 100644 --- a/lbr_fri_ros2/package.xml +++ b/lbr_fri_ros2/package.xml @@ -5,7 +5,7 @@ 1.4.2 The lbr_fri_ros2 package provides the Fast Robot Interface (FRI) integration into ROS 2. Robot states can be extracted and commanded. - mhubii + mhubii MIT ament_cmake diff --git a/lbr_moveit_config/iiwa14_moveit_config/.setup_assistant b/lbr_moveit_config/iiwa14_moveit_config/.setup_assistant index 885b072a..4323224d 100644 --- a/lbr_moveit_config/iiwa14_moveit_config/.setup_assistant +++ b/lbr_moveit_config/iiwa14_moveit_config/.setup_assistant @@ -6,7 +6,7 @@ moveit_setup_assistant_config: relative_path: config/iiwa14.srdf package_settings: author_name: mhubii - author_email: martin.huber@kcl.ac.uk + author_email: m.huber_1994@hotmail.de generated_timestamp: 1690640386 control_xacro: command: diff --git a/lbr_moveit_config/iiwa14_moveit_config/package.xml b/lbr_moveit_config/iiwa14_moveit_config/package.xml index 6d54af79..5f3d20c0 100644 --- a/lbr_moveit_config/iiwa14_moveit_config/package.xml +++ b/lbr_moveit_config/iiwa14_moveit_config/package.xml @@ -6,7 +6,7 @@ An automatically generated package with all the configuration and launch files for using the iiwa14 with the MoveIt Motion Planning Framework - mhubii + mhubii BSD @@ -14,7 +14,7 @@ https://github.com/ros-planning/moveit2/issues https://github.com/ros-planning/moveit2 - mhubii + mhubii ament_cmake diff --git a/lbr_moveit_config/iiwa7_moveit_config/.setup_assistant b/lbr_moveit_config/iiwa7_moveit_config/.setup_assistant index 171740f4..36c6f93a 100644 --- a/lbr_moveit_config/iiwa7_moveit_config/.setup_assistant +++ b/lbr_moveit_config/iiwa7_moveit_config/.setup_assistant @@ -6,7 +6,7 @@ moveit_setup_assistant_config: relative_path: config/iiwa7.srdf package_settings: author_name: mhubii - author_email: martin.huber@kcl.ac.uk + author_email: m.huber_1994@hotmail.de generated_timestamp: 1690638243 control_xacro: command: diff --git a/lbr_moveit_config/iiwa7_moveit_config/package.xml b/lbr_moveit_config/iiwa7_moveit_config/package.xml index bec4ddad..e42c1af2 100644 --- a/lbr_moveit_config/iiwa7_moveit_config/package.xml +++ b/lbr_moveit_config/iiwa7_moveit_config/package.xml @@ -6,7 +6,7 @@ An automatically generated package with all the configuration and launch files for using the iiwa7 with the MoveIt Motion Planning Framework - mhubii + mhubii BSD @@ -14,7 +14,7 @@ https://github.com/ros-planning/moveit2/issues https://github.com/ros-planning/moveit2 - mhubii + mhubii ament_cmake diff --git a/lbr_moveit_config/med14_moveit_config/.setup_assistant b/lbr_moveit_config/med14_moveit_config/.setup_assistant index 63f8dcc9..480cfd97 100644 --- a/lbr_moveit_config/med14_moveit_config/.setup_assistant +++ b/lbr_moveit_config/med14_moveit_config/.setup_assistant @@ -6,7 +6,7 @@ moveit_setup_assistant_config: relative_path: config/med14.srdf package_settings: author_name: mhubii - author_email: martin.huber@kcl.ac.uk + author_email: m.huber_1994@hotmail.de generated_timestamp: 1690640936 control_xacro: command: diff --git a/lbr_moveit_config/med14_moveit_config/package.xml b/lbr_moveit_config/med14_moveit_config/package.xml index 79714686..a6a6a8ce 100644 --- a/lbr_moveit_config/med14_moveit_config/package.xml +++ b/lbr_moveit_config/med14_moveit_config/package.xml @@ -6,7 +6,7 @@ An automatically generated package with all the configuration and launch files for using the med14 with the MoveIt Motion Planning Framework - mhubii + mhubii BSD @@ -14,7 +14,7 @@ https://github.com/ros-planning/moveit2/issues https://github.com/ros-planning/moveit2 - mhubii + mhubii ament_cmake diff --git a/lbr_moveit_config/med7_moveit_config/.setup_assistant b/lbr_moveit_config/med7_moveit_config/.setup_assistant index d5c9c814..afa4965b 100644 --- a/lbr_moveit_config/med7_moveit_config/.setup_assistant +++ b/lbr_moveit_config/med7_moveit_config/.setup_assistant @@ -6,7 +6,7 @@ moveit_setup_assistant_config: relative_path: config/med7.srdf package_settings: author_name: mhubii - author_email: martin.huber@kcl.ac.uk + author_email: m.huber_1994@hotmail.de generated_timestamp: 1690640668 control_xacro: command: diff --git a/lbr_moveit_config/med7_moveit_config/package.xml b/lbr_moveit_config/med7_moveit_config/package.xml index badb10bb..dd007600 100644 --- a/lbr_moveit_config/med7_moveit_config/package.xml +++ b/lbr_moveit_config/med7_moveit_config/package.xml @@ -6,7 +6,7 @@ An automatically generated package with all the configuration and launch files for using the med7 with the MoveIt Motion Planning Framework - mhubii + mhubii BSD @@ -14,7 +14,7 @@ https://github.com/ros-planning/moveit2/issues https://github.com/ros-planning/moveit2 - mhubii + mhubii ament_cmake diff --git a/lbr_ros2_control/package.xml b/lbr_ros2_control/package.xml index 26daa7c0..5f9c1ffa 100644 --- a/lbr_ros2_control/package.xml +++ b/lbr_ros2_control/package.xml @@ -4,7 +4,7 @@ lbr_ros2_control 1.4.2 ROS 2 hardware hardware_interface for KUKA LBR through Fast Robot Interface (FRI). - mhubii + mhubii MIT ament_cmake From a050f68ecb37aad12b87ecb5ffb6e4f3d1951968 Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 5 Jan 2024 15:09:24 +0000 Subject: [PATCH 21/76] added improved error handling --- lbr_fri_ros2/src/command_guard.cpp | 3 ++- lbr_fri_ros2/src/command_interface.cpp | 18 ++++++++++++------ lbr_ros2_control/src/system_interface.cpp | 15 +++++++++++---- 3 files changed, 25 insertions(+), 11 deletions(-) diff --git a/lbr_fri_ros2/src/command_guard.cpp b/lbr_fri_ros2/src/command_guard.cpp index 3cc77737..9ba83795 100644 --- a/lbr_fri_ros2/src/command_guard.cpp +++ b/lbr_fri_ros2/src/command_guard.cpp @@ -120,7 +120,8 @@ command_guard_factory(const CommandGuardParameters &command_guard_parameters, return std::make_unique(command_guard_parameters); } std::string err = "Invalid CommandGuard variant provided."; - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); throw std::runtime_error(err); } } // end of namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/command_interface.cpp b/lbr_fri_ros2/src/command_interface.cpp index 9d759a44..b19f209e 100644 --- a/lbr_fri_ros2/src/command_interface.cpp +++ b/lbr_fri_ros2/src/command_interface.cpp @@ -13,12 +13,14 @@ void CommandInterface::get_joint_position_command(fri_command_t_ref command, const_fri_state_t_ref state) { if (state.getClientCommandMode() != KUKA::FRI::EClientCommandMode::POSITION) { std::string err = "Set joint position only allowed in position command mode."; - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); throw std::runtime_error(err); } if (!command_guard_) { std::string err = "Uninitialized command guard."; - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); throw std::runtime_error(err); } @@ -34,7 +36,8 @@ void CommandInterface::get_joint_position_command(fri_command_t_ref command, // validate if (!command_guard_->is_valid_command(command_, state)) { std::string err = "Invalid command."; - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); throw std::runtime_error(err); } @@ -45,12 +48,14 @@ void CommandInterface::get_joint_position_command(fri_command_t_ref command, void CommandInterface::get_torque_command(fri_command_t_ref command, const_fri_state_t_ref state) { if (state.getClientCommandMode() != KUKA::FRI::EClientCommandMode::TORQUE) { std::string err = "Set torque only allowed in torque command mode."; - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); throw std::runtime_error(err); } if (!command_guard_) { std::string err = "Uninitialized command guard."; - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); throw std::runtime_error(err); } @@ -99,7 +104,8 @@ void CommandInterface::get_wrench_command(fri_command_t_ref command, const_fri_s // validate if (!command_guard_->is_valid_command(command_, state)) { std::string err = "Invalid command."; - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); throw std::runtime_error(err); } diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 732d7520..85fbc2e9 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -70,10 +70,17 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { state_interface_parameters.measured_torque_cutoff_frequency = parameters_.measured_torque_cutoff_frequency; - async_client_ptr_ = std::make_shared( - pid_parameters, command_guard_parameters, parameters_.command_guard_variant, - state_interface_parameters, parameters_.open_loop); - app_ptr_ = std::make_unique(async_client_ptr_); + try { + async_client_ptr_ = std::make_shared( + pid_parameters, command_guard_parameters, parameters_.command_guard_variant, + state_interface_parameters, parameters_.open_loop); + app_ptr_ = std::make_unique(async_client_ptr_); + } catch (const std::exception &e) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), lbr_fri_ros2::ColorScheme::ERROR + << e.what() + << lbr_fri_ros2::ColorScheme::ENDC); + return controller_interface::CallbackReturn::ERROR; + } nan_command_interfaces_(); nan_state_interfaces_(); From 72d12906f07fb8f12ee495a6f28b875c206b959e Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 5 Jan 2024 15:11:07 +0000 Subject: [PATCH 22/76] improved error output --- lbr_ros2_control/src/system_interface.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 85fbc2e9..7e038672 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -76,9 +76,10 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { state_interface_parameters, parameters_.open_loop); app_ptr_ = std::make_unique(async_client_ptr_); } catch (const std::exception &e) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), lbr_fri_ros2::ColorScheme::ERROR - << e.what() - << lbr_fri_ros2::ColorScheme::ENDC); + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + lbr_fri_ros2::ColorScheme::ERROR + << "Failed to instantiate AsyncClient or App with: " << e.what() + << lbr_fri_ros2::ColorScheme::ENDC); return controller_interface::CallbackReturn::ERROR; } From 492a9fd1a9dccad7e78dba2b153438c669c4038f Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 5 Jan 2024 15:27:00 +0000 Subject: [PATCH 23/76] added control mode to on state change --- lbr_fri_ros2/src/async_client.cpp | 4 +++- lbr_ros2_control/src/system_interface.cpp | 4 ---- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/lbr_fri_ros2/src/async_client.cpp b/lbr_fri_ros2/src/async_client.cpp index a08cb58c..eb6e03ae 100644 --- a/lbr_fri_ros2/src/async_client.cpp +++ b/lbr_fri_ros2/src/async_client.cpp @@ -28,7 +28,9 @@ void AsyncClient::onStateChange(KUKA::FRI::ESessionState old_state, << EnumMaps::session_state_map(old_state).c_str() << ColorScheme::ENDC << "' to '" << ColorScheme::OKGREEN << ColorScheme::BOLD << EnumMaps::session_state_map(new_state).c_str() << ColorScheme::ENDC - << "'"); + << "'. Control mode '" << ColorScheme::OKBLUE << ColorScheme::BOLD + << EnumMaps::control_mode_map(robotState().getControlMode()) + << ColorScheme::ENDC << "'"); command_interface_.init_command(robotState()); } diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 7e038672..760fbaf3 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -238,10 +238,6 @@ controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_l RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), lbr_fri_ros2::ColorScheme::OKGREEN << "Robot connected" << lbr_fri_ros2::ColorScheme::ENDC); - RCLCPP_INFO_STREAM( - rclcpp::get_logger(LOGGER_NAME), - "Control mode '" - << lbr_fri_ros2::EnumMaps::control_mode_map(hw_lbr_state_.control_mode).c_str() << "'"); RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Sample time %.3f s / %.1f Hz", async_client_ptr_->get_state_interface().get_state().sample_time, 1. / async_client_ptr_->get_state_interface().get_state().sample_time); From ff451a7ef073c7e691e473845aa7757fb1eef781 Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 5 Jan 2024 15:28:28 +0000 Subject: [PATCH 24/76] removed control mode output from state change --- lbr_fri_ros2/src/async_client.cpp | 4 +--- lbr_ros2_control/src/system_interface.cpp | 6 ++++++ 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/lbr_fri_ros2/src/async_client.cpp b/lbr_fri_ros2/src/async_client.cpp index eb6e03ae..a08cb58c 100644 --- a/lbr_fri_ros2/src/async_client.cpp +++ b/lbr_fri_ros2/src/async_client.cpp @@ -28,9 +28,7 @@ void AsyncClient::onStateChange(KUKA::FRI::ESessionState old_state, << EnumMaps::session_state_map(old_state).c_str() << ColorScheme::ENDC << "' to '" << ColorScheme::OKGREEN << ColorScheme::BOLD << EnumMaps::session_state_map(new_state).c_str() << ColorScheme::ENDC - << "'. Control mode '" << ColorScheme::OKBLUE << ColorScheme::BOLD - << EnumMaps::control_mode_map(robotState().getControlMode()) - << ColorScheme::ENDC << "'"); + << "'"); command_interface_.init_command(robotState()); } diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 760fbaf3..7507857e 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -238,6 +238,12 @@ controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_l RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), lbr_fri_ros2::ColorScheme::OKGREEN << "Robot connected" << lbr_fri_ros2::ColorScheme::ENDC); + RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), + "Control mode '" + << lbr_fri_ros2::EnumMaps::control_mode_map( + async_client_ptr_->get_state_interface().get_state().control_mode) + .c_str() + << "'"); RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Sample time %.3f s / %.1f Hz", async_client_ptr_->get_state_interface().get_state().sample_time, 1. / async_client_ptr_->get_state_interface().get_state().sample_time); From 81132227d1a88c331cdb251e23b5a3f2d39a314b Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 10 Jan 2024 17:13:06 +0000 Subject: [PATCH 25/76] removed event handler ros2_control_node --- lbr_bringup/launch/real.launch.py | 8 +------- lbr_ros2_control/launch/system_interface.launch.py | 8 +------- 2 files changed, 2 insertions(+), 14 deletions(-) diff --git a/lbr_bringup/launch/real.launch.py b/lbr_bringup/launch/real.launch.py index fbf050aa..42dbf1b5 100644 --- a/lbr_bringup/launch/real.launch.py +++ b/lbr_bringup/launch/real.launch.py @@ -30,13 +30,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: # ros2 control node ros2_control_node = LBRROS2ControlMixin.node_ros2_control() - ros2_control_node_event_handler = RegisterEventHandler( - OnProcessStart( - target_action=robot_state_publisher, - on_start=[ros2_control_node], - ) - ) - ld.add_action(ros2_control_node_event_handler) + ld.add_action(ros2_control_node) # joint state broad caster and controller on ros2 control node start joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner( diff --git a/lbr_ros2_control/launch/system_interface.launch.py b/lbr_ros2_control/launch/system_interface.launch.py index 847480dd..3043ef73 100644 --- a/lbr_ros2_control/launch/system_interface.launch.py +++ b/lbr_ros2_control/launch/system_interface.launch.py @@ -26,13 +26,7 @@ def generate_launch_description() -> LaunchDescription: ) ld.add_action(robot_state_publisher) ros2_control_node = LBRSystemInterface.node_ros2_control() - ros2_control_node_event_handler = RegisterEventHandler( - OnProcessStart( - target_action=robot_state_publisher, - on_start=[ros2_control_node], - ) - ) - ld.add_action(ros2_control_node_event_handler) + ld.add_action(ros2_control_node) joint_state_broadcaster = LBRSystemInterface.node_controller_spawner( controller="joint_state_broadcaster" ) From 460543a63a335994976b9a8fb8194e9ad9eb8268 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 27 Feb 2024 09:54:12 +0000 Subject: [PATCH 26/76] updated header --- lbr_fri_ros2/include/lbr_fri_ros2/command_interface.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/command_interface.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/command_interface.hpp index 7a02da58..883fd100 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/command_interface.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/command_interface.hpp @@ -14,8 +14,8 @@ #include "lbr_fri_msgs/msg/lbr_command.hpp" #include "lbr_fri_ros2/command_guard.hpp" -#include "lbr_fri_ros2/enum_maps.hpp" #include "lbr_fri_ros2/filters.hpp" +#include "lbr_fri_ros2/formatting.hpp" namespace lbr_fri_ros2 { class CommandInterface { From c276753ea623cbbbba6c2ebdf9f92911319948a0 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 27 Feb 2024 18:02:57 +0000 Subject: [PATCH 27/76] added draft --- README.md | 11 ++++++----- lbr_fri_ros2_stack/fri-1.14.repos | 13 +++++++++++++ lbr_fri_ros2_stack/fri-1.15.repos | 13 +++++++++++++ lbr_fri_ros2_stack/fri-1.16.repos | 13 +++++++++++++ lbr_fri_ros2_stack/fri-2.5.repos | 13 +++++++++++++ lbr_fri_ros2_stack/repos.yaml | 9 --------- 6 files changed, 58 insertions(+), 14 deletions(-) create mode 100644 lbr_fri_ros2_stack/fri-1.14.repos create mode 100644 lbr_fri_ros2_stack/fri-1.15.repos create mode 100644 lbr_fri_ros2_stack/fri-1.16.repos create mode 100644 lbr_fri_ros2_stack/fri-2.5.repos delete mode 100644 lbr_fri_ros2_stack/repos.yaml diff --git a/README.md b/README.md index 0bedda3b..757046ec 100644 --- a/README.md +++ b/README.md @@ -33,19 +33,20 @@ Full documentation available [here](https://lbr-fri-ros2-stack-doc.readthedocs.i 2. Create a workspace, clone, and install dependencies ```shell + export FRI_CLIENT_VERSION=1.15 mkdir -p lbr-stack/src && cd lbr-stack - vcs import src --input https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/repos.yaml + vcs import src --input https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/fri-${FRI_CLIENT_VERSION}-repos.yaml rosdep install --from-paths src --ignore-src -r -y ``` +> [!NOTE] +> FRI client is cloned from [fri](https://github.com/lbr-stack/fri) and must be available as branch, refer [README](https://github.com/lbr-stack/fri?tab=readme-ov-file#contributing). + 3. Build ```shell - colcon build --symlink-install --cmake-args -DFRI_CLIENT_VERSION=1.15 --no-warn-unused-cli # replace by your FRI client version + colcon build --symlink-install ``` -> [!NOTE] -> FRI client is added as external CMake project via [fri_vendor](https://github.com/lbr-stack/fri_vendor) and must be available as branch, refer [README](https://github.com/lbr-stack/fri?tab=readme-ov-file#contributing). - 4. Launch the simulation via ```shell source install/setup.bash diff --git a/lbr_fri_ros2_stack/fri-1.14.repos b/lbr_fri_ros2_stack/fri-1.14.repos new file mode 100644 index 00000000..8159c087 --- /dev/null +++ b/lbr_fri_ros2_stack/fri-1.14.repos @@ -0,0 +1,13 @@ +repositories: + fri: + type: git + url: https://github.com/lbr-stack/fri + version: fri-1.14 + lbr_fri_msgs: + type: git + url: https://github.com/lbr-stack/lbr_fri_msgs + version: fri-1 + lbr_fri_ros2_stack: + type: git + url: https://github.com/lbr-stack/lbr_fri_ros2_stack + version: humble diff --git a/lbr_fri_ros2_stack/fri-1.15.repos b/lbr_fri_ros2_stack/fri-1.15.repos new file mode 100644 index 00000000..b869a547 --- /dev/null +++ b/lbr_fri_ros2_stack/fri-1.15.repos @@ -0,0 +1,13 @@ +repositories: + fri: + type: git + url: https://github.com/lbr-stack/fri + version: fri-1.15 + lbr_fri_msgs: + type: git + url: https://github.com/lbr-stack/lbr_fri_msgs + version: fri-1 + lbr_fri_ros2_stack: + type: git + url: https://github.com/lbr-stack/lbr_fri_ros2_stack + version: humble diff --git a/lbr_fri_ros2_stack/fri-1.16.repos b/lbr_fri_ros2_stack/fri-1.16.repos new file mode 100644 index 00000000..55de8589 --- /dev/null +++ b/lbr_fri_ros2_stack/fri-1.16.repos @@ -0,0 +1,13 @@ +repositories: + fri: + type: git + url: https://github.com/lbr-stack/fri + version: fri-1.16 + lbr_fri_msgs: + type: git + url: https://github.com/lbr-stack/lbr_fri_msgs + version: fri-1 + lbr_fri_ros2_stack: + type: git + url: https://github.com/lbr-stack/lbr_fri_ros2_stack + version: humble diff --git a/lbr_fri_ros2_stack/fri-2.5.repos b/lbr_fri_ros2_stack/fri-2.5.repos new file mode 100644 index 00000000..0ab38f23 --- /dev/null +++ b/lbr_fri_ros2_stack/fri-2.5.repos @@ -0,0 +1,13 @@ +repositories: + fri: + type: git + url: https://github.com/lbr-stack/fri + version: fri-2.5 + lbr_fri_msgs: + type: git + url: https://github.com/lbr-stack/lbr_fri_msgs + version: fri-2 + lbr_fri_ros2_stack: + type: git + url: https://github.com/lbr-stack/lbr_fri_ros2_stack + version: humble diff --git a/lbr_fri_ros2_stack/repos.yaml b/lbr_fri_ros2_stack/repos.yaml deleted file mode 100644 index 5be7a84a..00000000 --- a/lbr_fri_ros2_stack/repos.yaml +++ /dev/null @@ -1,9 +0,0 @@ -repositories: - fri_vendor: - type: git - url: https://github.com/lbr-stack/fri_vendor - version: ros2 - lbr_fri_ros2_stack: - type: git - url: https://github.com/lbr-stack/lbr_fri_ros2_stack - version: humble From 84bda8f9ec15e8f2b8b99255f34198bf947962ac Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 27 Feb 2024 18:03:42 +0000 Subject: [PATCH 28/76] fixed readme --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 757046ec..22049eed 100644 --- a/README.md +++ b/README.md @@ -35,7 +35,7 @@ Full documentation available [here](https://lbr-fri-ros2-stack-doc.readthedocs.i ```shell export FRI_CLIENT_VERSION=1.15 mkdir -p lbr-stack/src && cd lbr-stack - vcs import src --input https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/fri-${FRI_CLIENT_VERSION}-repos.yaml + vcs import src --input https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/fri-${FRI_CLIENT_VERSION}.repos rosdep install --from-paths src --ignore-src -r -y ``` From a26d31421e169db8e6d76d99716ceb219fe72996 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 13 Mar 2024 11:41:39 +0000 Subject: [PATCH 29/76] added linting --- .github/workflows/black.yml | 10 ++++++++++ README.md | 8 +++++--- 2 files changed, 15 insertions(+), 3 deletions(-) create mode 100644 .github/workflows/black.yml diff --git a/.github/workflows/black.yml b/.github/workflows/black.yml new file mode 100644 index 00000000..9065b5e0 --- /dev/null +++ b/.github/workflows/black.yml @@ -0,0 +1,10 @@ +name: Lint + +on: [push, pull_request] + +jobs: + lint: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - uses: psf/black@stable diff --git a/README.md b/README.md index 180aa605..0b7fc742 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,9 @@ # lbr_fri_ros2_stack -![Build status](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build.yml/badge.svg?branch=humble) ![GitHub](https://img.shields.io/github/license/lbr-stack/lbr_fri_ros2_stack) -[![Documentation Status](https://readthedocs.org/projects/lbr-fri-ros2-stack-doc/badge/?version=humble)](https://lbr-fri-ros2-stack-doc.readthedocs.io/en/humble/?badge=humble) -[![status](https://joss.theoj.org/papers/c43c82bed833c02503dd47f2637192ef/status.svg)](https://joss.theoj.org/papers/c43c82bed833c02503dd47f2637192ef) +[![Build status](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build.yml/badge.svg?branch=humble)](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions) +[![License](https://img.shields.io/github/license/lbr-stack/lbr_fri_ros2_stack)](https://github.com/lbr-stack/lbr_fri_ros2_stack/tree/humble?tab=Apache-2.0-1-ov-file#readme) +[![Documentation Status](https://readthedocs.org/projects/lbr-fri-ros2-stack-doc/badge/?version=humble)](https://lbr-fri-ros2-stack-doc.readthedocs.io/en/humble/?badge=humble) +[![JOSS](https://joss.theoj.org/papers/c43c82bed833c02503dd47f2637192ef/status.svg)](https://joss.theoj.org/papers/c43c82bed833c02503dd47f2637192ef) +[![Code Style: Black](https://img.shields.io/badge/code%20style-black-000000.svg)](https://github.com/psf/black) ROS 2 packages for the KUKA LBR, including communication to the real robot via the Fast Robot Interface ([FRI](https://github.com/lbr-stack/fri)) and [Gazebo](http://gazebosim.org/) simulation support. Included are the `iiwa7`, `iiwa14`, `med7`, and `med14`. From 1dd97f728af53e0bedd9a4ca4d4fc443751f23d5 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 13 Mar 2024 11:48:58 +0000 Subject: [PATCH 30/76] formatted https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/147 --- .../test/test_copyright.py | 8 +++++--- .../test/test_flake8.py | 6 +++--- .../test/test_pep257.py | 4 ++-- .../lbr_demos_fri_ros2_python/test/test_copyright.py | 8 +++++--- lbr_demos/lbr_demos_fri_ros2_python/test/test_flake8.py | 6 +++--- lbr_demos/lbr_demos_fri_ros2_python/test/test_pep257.py | 4 ++-- .../lbr_demos_ros2_control_python/test/test_copyright.py | 8 +++++--- .../lbr_demos_ros2_control_python/test/test_flake8.py | 6 +++--- .../lbr_demos_ros2_control_python/test/test_pep257.py | 4 ++-- .../iiwa14_moveit_config/launch/moveit_rviz.launch.py | 4 +++- .../iiwa14_moveit_config/launch/setup_assistant.launch.py | 4 +++- .../iiwa7_moveit_config/launch/moveit_rviz.launch.py | 4 +++- .../iiwa7_moveit_config/launch/setup_assistant.launch.py | 4 +++- .../med14_moveit_config/launch/moveit_rviz.launch.py | 4 +++- .../med14_moveit_config/launch/setup_assistant.launch.py | 4 +++- .../med7_moveit_config/launch/moveit_rviz.launch.py | 4 +++- .../med7_moveit_config/launch/setup_assistant.launch.py | 4 +++- 17 files changed, 54 insertions(+), 32 deletions(-) diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/test/test_copyright.py b/lbr_demos/lbr_demos_fri_ros2_advanced_python/test/test_copyright.py index 97a39196..ceffe896 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_python/test/test_copyright.py +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_python/test/test_copyright.py @@ -17,9 +17,11 @@ # 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.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' + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/test/test_flake8.py b/lbr_demos/lbr_demos_fri_ros2_advanced_python/test/test_flake8.py index 27ee1078..ee79f31a 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_python/test/test_flake8.py +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_python/test/test_flake8.py @@ -20,6 +20,6 @@ @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) + assert rc == 0, "Found %d code style errors / warnings:\n" % len( + errors + ) + "\n".join(errors) diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/test/test_pep257.py b/lbr_demos/lbr_demos_fri_ros2_advanced_python/test/test_pep257.py index b234a384..a2c3deb8 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_python/test/test_pep257.py +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_python/test/test_pep257.py @@ -19,5 +19,5 @@ @pytest.mark.linter @pytest.mark.pep257 def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings" diff --git a/lbr_demos/lbr_demos_fri_ros2_python/test/test_copyright.py b/lbr_demos/lbr_demos_fri_ros2_python/test/test_copyright.py index 97a39196..ceffe896 100644 --- a/lbr_demos/lbr_demos_fri_ros2_python/test/test_copyright.py +++ b/lbr_demos/lbr_demos_fri_ros2_python/test/test_copyright.py @@ -17,9 +17,11 @@ # 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.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' + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/lbr_demos/lbr_demos_fri_ros2_python/test/test_flake8.py b/lbr_demos/lbr_demos_fri_ros2_python/test/test_flake8.py index 27ee1078..ee79f31a 100644 --- a/lbr_demos/lbr_demos_fri_ros2_python/test/test_flake8.py +++ b/lbr_demos/lbr_demos_fri_ros2_python/test/test_flake8.py @@ -20,6 +20,6 @@ @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) + assert rc == 0, "Found %d code style errors / warnings:\n" % len( + errors + ) + "\n".join(errors) diff --git a/lbr_demos/lbr_demos_fri_ros2_python/test/test_pep257.py b/lbr_demos/lbr_demos_fri_ros2_python/test/test_pep257.py index b234a384..a2c3deb8 100644 --- a/lbr_demos/lbr_demos_fri_ros2_python/test/test_pep257.py +++ b/lbr_demos/lbr_demos_fri_ros2_python/test/test_pep257.py @@ -19,5 +19,5 @@ @pytest.mark.linter @pytest.mark.pep257 def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings" diff --git a/lbr_demos/lbr_demos_ros2_control_python/test/test_copyright.py b/lbr_demos/lbr_demos_ros2_control_python/test/test_copyright.py index 97a39196..ceffe896 100644 --- a/lbr_demos/lbr_demos_ros2_control_python/test/test_copyright.py +++ b/lbr_demos/lbr_demos_ros2_control_python/test/test_copyright.py @@ -17,9 +17,11 @@ # 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.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' + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/lbr_demos/lbr_demos_ros2_control_python/test/test_flake8.py b/lbr_demos/lbr_demos_ros2_control_python/test/test_flake8.py index 27ee1078..ee79f31a 100644 --- a/lbr_demos/lbr_demos_ros2_control_python/test/test_flake8.py +++ b/lbr_demos/lbr_demos_ros2_control_python/test/test_flake8.py @@ -20,6 +20,6 @@ @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) + assert rc == 0, "Found %d code style errors / warnings:\n" % len( + errors + ) + "\n".join(errors) diff --git a/lbr_demos/lbr_demos_ros2_control_python/test/test_pep257.py b/lbr_demos/lbr_demos_ros2_control_python/test/test_pep257.py index b234a384..a2c3deb8 100644 --- a/lbr_demos/lbr_demos_ros2_control_python/test/test_pep257.py +++ b/lbr_demos/lbr_demos_ros2_control_python/test/test_pep257.py @@ -19,5 +19,5 @@ @pytest.mark.linter @pytest.mark.pep257 def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings" diff --git a/lbr_moveit_config/iiwa14_moveit_config/launch/moveit_rviz.launch.py b/lbr_moveit_config/iiwa14_moveit_config/launch/moveit_rviz.launch.py index 6ebe82cc..86ccf11f 100644 --- a/lbr_moveit_config/iiwa14_moveit_config/launch/moveit_rviz.launch.py +++ b/lbr_moveit_config/iiwa14_moveit_config/launch/moveit_rviz.launch.py @@ -3,5 +3,7 @@ def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("iiwa14", package_name="iiwa14_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder( + "iiwa14", package_name="iiwa14_moveit_config" + ).to_moveit_configs() return generate_moveit_rviz_launch(moveit_config) diff --git a/lbr_moveit_config/iiwa14_moveit_config/launch/setup_assistant.launch.py b/lbr_moveit_config/iiwa14_moveit_config/launch/setup_assistant.launch.py index 90018c97..109ab2be 100644 --- a/lbr_moveit_config/iiwa14_moveit_config/launch/setup_assistant.launch.py +++ b/lbr_moveit_config/iiwa14_moveit_config/launch/setup_assistant.launch.py @@ -3,5 +3,7 @@ def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("iiwa14", package_name="iiwa14_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder( + "iiwa14", package_name="iiwa14_moveit_config" + ).to_moveit_configs() return generate_setup_assistant_launch(moveit_config) diff --git a/lbr_moveit_config/iiwa7_moveit_config/launch/moveit_rviz.launch.py b/lbr_moveit_config/iiwa7_moveit_config/launch/moveit_rviz.launch.py index c9b23478..9c5737b4 100644 --- a/lbr_moveit_config/iiwa7_moveit_config/launch/moveit_rviz.launch.py +++ b/lbr_moveit_config/iiwa7_moveit_config/launch/moveit_rviz.launch.py @@ -3,5 +3,7 @@ def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("iiwa7", package_name="iiwa7_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder( + "iiwa7", package_name="iiwa7_moveit_config" + ).to_moveit_configs() return generate_moveit_rviz_launch(moveit_config) diff --git a/lbr_moveit_config/iiwa7_moveit_config/launch/setup_assistant.launch.py b/lbr_moveit_config/iiwa7_moveit_config/launch/setup_assistant.launch.py index d5063faf..103d2515 100644 --- a/lbr_moveit_config/iiwa7_moveit_config/launch/setup_assistant.launch.py +++ b/lbr_moveit_config/iiwa7_moveit_config/launch/setup_assistant.launch.py @@ -3,5 +3,7 @@ def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("iiwa7", package_name="iiwa7_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder( + "iiwa7", package_name="iiwa7_moveit_config" + ).to_moveit_configs() return generate_setup_assistant_launch(moveit_config) diff --git a/lbr_moveit_config/med14_moveit_config/launch/moveit_rviz.launch.py b/lbr_moveit_config/med14_moveit_config/launch/moveit_rviz.launch.py index 84f78554..899a4551 100644 --- a/lbr_moveit_config/med14_moveit_config/launch/moveit_rviz.launch.py +++ b/lbr_moveit_config/med14_moveit_config/launch/moveit_rviz.launch.py @@ -3,5 +3,7 @@ def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("med14", package_name="med14_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder( + "med14", package_name="med14_moveit_config" + ).to_moveit_configs() return generate_moveit_rviz_launch(moveit_config) diff --git a/lbr_moveit_config/med14_moveit_config/launch/setup_assistant.launch.py b/lbr_moveit_config/med14_moveit_config/launch/setup_assistant.launch.py index 2513e1a1..a96b537a 100644 --- a/lbr_moveit_config/med14_moveit_config/launch/setup_assistant.launch.py +++ b/lbr_moveit_config/med14_moveit_config/launch/setup_assistant.launch.py @@ -3,5 +3,7 @@ def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("med14", package_name="med14_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder( + "med14", package_name="med14_moveit_config" + ).to_moveit_configs() return generate_setup_assistant_launch(moveit_config) diff --git a/lbr_moveit_config/med7_moveit_config/launch/moveit_rviz.launch.py b/lbr_moveit_config/med7_moveit_config/launch/moveit_rviz.launch.py index 6f9d2a1b..c432cad9 100644 --- a/lbr_moveit_config/med7_moveit_config/launch/moveit_rviz.launch.py +++ b/lbr_moveit_config/med7_moveit_config/launch/moveit_rviz.launch.py @@ -3,5 +3,7 @@ def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("med7", package_name="med7_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder( + "med7", package_name="med7_moveit_config" + ).to_moveit_configs() return generate_moveit_rviz_launch(moveit_config) diff --git a/lbr_moveit_config/med7_moveit_config/launch/setup_assistant.launch.py b/lbr_moveit_config/med7_moveit_config/launch/setup_assistant.launch.py index 3aecb3f4..68744b08 100644 --- a/lbr_moveit_config/med7_moveit_config/launch/setup_assistant.launch.py +++ b/lbr_moveit_config/med7_moveit_config/launch/setup_assistant.launch.py @@ -3,5 +3,7 @@ def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("med7", package_name="med7_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder( + "med7", package_name="med7_moveit_config" + ).to_moveit_configs() return generate_setup_assistant_launch(moveit_config) From f26b51802bbfb1407012e1f5b3784eede1dcfead Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 15 Mar 2024 17:09:33 +0000 Subject: [PATCH 31/76] udpated doc --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index d45a9834..1d3891da 100644 --- a/README.md +++ b/README.md @@ -25,7 +25,7 @@ ROS 2 packages for the KUKA LBR, including communication to the real robot via t ## Documentation -Full documentation available [here](https://lbr-fri-ros2-stack-doc.readthedocs.io/en/humble/index.html). +Full documentation available [here](https://lbr-stack.readthedocs.io/en/latest/?badge=latest). ## Quick Start 1. Install ROS 2 development tools @@ -61,7 +61,7 @@ Full documentation available [here](https://lbr-fri-ros2-stack-doc.readthedocs.i > [!TIP] > List all arguments for the launch file via `ros2 launch lbr_bringup bringup.launch.py -s` -Now, run the [demos](https://lbr-fri-ros2-stack-doc.readthedocs.io/en/humble/lbr_fri_ros2_stack/lbr_demos/doc/lbr_demos.html). To get started with the real robot, checkout the [Documentation](https://lbr-fri-ros2-stack-doc.readthedocs.io/en/humble/index.html) above. +Now, run the [demos](https://lbr-fri-ros2-stack-doc.readthedocs.io/en/humble/lbr_fri_ros2_stack/lbr_demos/doc/lbr_demos.html). To get started with the real robot, checkout the [Documentation](https://lbr-stack.readthedocs.io/en/latest/?badge=latest) above. ## Citation If you enjoyed using this repository for your work, we would really appreciate ❤️ if you could cite it, as it helps us to continue offering support. From 1ec871ee53b030f495a547af5bdb37e9381d5bb5 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 4 Apr 2024 11:47:45 +0100 Subject: [PATCH 32/76] added a source --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index efe1d01b..ac120373 100644 --- a/README.md +++ b/README.md @@ -37,6 +37,7 @@ Full documentation available [here](https://lbr-stack.readthedocs.io/en/latest/? 2. Create a workspace, clone, and install dependencies ```shell + source /opt/ros/humble/setup.bash mkdir -p lbr-stack/src && cd lbr-stack vcs import src --input https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/repos.yaml rosdep install --from-paths src -i -r -y From e3fd16f023f6d66336a0f7c55efbb110baf54bf0 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 4 Apr 2024 12:01:36 +0100 Subject: [PATCH 33/76] re-named demos in accordance to ros 2 control --- .../CMakeLists.txt | 0 .../config/admittance_control_node.yaml | 0 .../doc/lbr_demos_fri_ros2_advanced_cpp.rst | 0 .../launch/admittance_control_node.launch.py | 0 .../package.xml | 0 .../src/admittance_control_node.cpp | 0 .../src/admittance_controller.hpp | 0 .../doc/lbr_demos_fri_ros2_advanced_python.rst | 0 .../launch/admittance_control_node.launch.py | 0 .../lbr_demos_admittance_control_python}/__init__.py | 0 .../admittance_control_node.py | 0 .../lbr_demos_admittance_control_python}/admittance_controller.py | 0 .../package.xml | 0 .../resource/lbr_demos_fri_ros2_advanced_python | 0 .../setup.cfg | 0 .../setup.py | 0 .../test/test_copyright.py | 0 .../test/test_flake8.py | 0 .../test/test_pep257.py | 0 .../CMakeLists.txt | 0 .../doc/lbr_demos_fri_ros2_cpp.rst | 0 .../launch/joint_sine_overlay.launch.py | 0 .../launch/torque_sine_overlay.launch.py | 0 .../launch/wrench_sine_overlay.launch.py | 0 .../package.xml | 0 .../src/joint_sine_overlay_node.cpp | 0 .../src/torque_sine_overlay_node.cpp | 0 .../src/wrench_sine_overlay_node.cpp | 0 .../doc/lbr_demos_fri_ros2_python.rst | 0 .../launch/joint_sine_overlay.launch.py | 0 .../launch/torque_sine_overlay.launch.py | 0 .../launch/wrench_sine_overlay.launch.py | 0 .../lbr_demos_forward_command_controllers_python}/__init__.py | 0 .../joint_sine_overlay_node.py | 0 .../torque_sine_overlay_node.py | 0 .../wrench_sine_overlay_node.py | 0 .../package.xml | 0 .../resource/lbr_demos_fri_ros2_python | 0 .../setup.cfg | 0 .../setup.py | 0 .../test/test_copyright.py | 0 .../test/test_flake8.py | 0 .../test/test_pep257.py | 0 .../CMakeLists.txt | 0 .../doc/lbr_demos_ros2_control_cpp.rst | 0 .../package.xml | 0 .../src/joint_trajectory_executioner_node.cpp | 0 .../doc/lbr_demos_ros2_control_python.rst | 0 .../lbr_demos_joint_trajectory_controller_python}/__init__.py | 0 .../joint_trajectory_executioner_node.py | 0 .../package.xml | 0 .../resource/lbr_demos_ros2_control_python | 0 .../setup.cfg | 0 .../setup.py | 0 .../test/test_copyright.py | 0 .../test/test_flake8.py | 0 .../test/test_pep257.py | 0 57 files changed, 0 insertions(+), 0 deletions(-) rename lbr_demos/{lbr_demos_fri_ros2_advanced_cpp => lbr_demos_admittance_control_cpp}/CMakeLists.txt (100%) rename lbr_demos/{lbr_demos_fri_ros2_advanced_cpp => lbr_demos_admittance_control_cpp}/config/admittance_control_node.yaml (100%) rename lbr_demos/{lbr_demos_fri_ros2_advanced_cpp => lbr_demos_admittance_control_cpp}/doc/lbr_demos_fri_ros2_advanced_cpp.rst (100%) rename lbr_demos/{lbr_demos_fri_ros2_advanced_cpp => lbr_demos_admittance_control_cpp}/launch/admittance_control_node.launch.py (100%) rename lbr_demos/{lbr_demos_fri_ros2_advanced_cpp => lbr_demos_admittance_control_cpp}/package.xml (100%) rename lbr_demos/{lbr_demos_fri_ros2_advanced_cpp => lbr_demos_admittance_control_cpp}/src/admittance_control_node.cpp (100%) rename lbr_demos/{lbr_demos_fri_ros2_advanced_cpp => lbr_demos_admittance_control_cpp}/src/admittance_controller.hpp (100%) rename lbr_demos/{lbr_demos_fri_ros2_advanced_python => lbr_demos_admittance_control_python}/doc/lbr_demos_fri_ros2_advanced_python.rst (100%) rename lbr_demos/{lbr_demos_fri_ros2_advanced_python => lbr_demos_admittance_control_python}/launch/admittance_control_node.launch.py (100%) rename lbr_demos/{lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python => lbr_demos_admittance_control_python/lbr_demos_admittance_control_python}/__init__.py (100%) rename lbr_demos/{lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python => lbr_demos_admittance_control_python/lbr_demos_admittance_control_python}/admittance_control_node.py (100%) rename lbr_demos/{lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python => lbr_demos_admittance_control_python/lbr_demos_admittance_control_python}/admittance_controller.py (100%) rename lbr_demos/{lbr_demos_fri_ros2_advanced_python => lbr_demos_admittance_control_python}/package.xml (100%) rename lbr_demos/{lbr_demos_fri_ros2_advanced_python => lbr_demos_admittance_control_python}/resource/lbr_demos_fri_ros2_advanced_python (100%) rename lbr_demos/{lbr_demos_fri_ros2_advanced_python => lbr_demos_admittance_control_python}/setup.cfg (100%) rename lbr_demos/{lbr_demos_fri_ros2_advanced_python => lbr_demos_admittance_control_python}/setup.py (100%) rename lbr_demos/{lbr_demos_fri_ros2_advanced_python => lbr_demos_admittance_control_python}/test/test_copyright.py (100%) rename lbr_demos/{lbr_demos_fri_ros2_advanced_python => lbr_demos_admittance_control_python}/test/test_flake8.py (100%) rename lbr_demos/{lbr_demos_fri_ros2_advanced_python => lbr_demos_admittance_control_python}/test/test_pep257.py (100%) rename lbr_demos/{lbr_demos_fri_ros2_cpp => lbr_demos_forward_command_controllers_cpp}/CMakeLists.txt (100%) rename lbr_demos/{lbr_demos_fri_ros2_cpp => lbr_demos_forward_command_controllers_cpp}/doc/lbr_demos_fri_ros2_cpp.rst (100%) rename lbr_demos/{lbr_demos_fri_ros2_cpp => lbr_demos_forward_command_controllers_cpp}/launch/joint_sine_overlay.launch.py (100%) rename lbr_demos/{lbr_demos_fri_ros2_cpp => lbr_demos_forward_command_controllers_cpp}/launch/torque_sine_overlay.launch.py (100%) rename lbr_demos/{lbr_demos_fri_ros2_cpp => lbr_demos_forward_command_controllers_cpp}/launch/wrench_sine_overlay.launch.py (100%) rename lbr_demos/{lbr_demos_fri_ros2_cpp => lbr_demos_forward_command_controllers_cpp}/package.xml (100%) rename lbr_demos/{lbr_demos_fri_ros2_cpp => lbr_demos_forward_command_controllers_cpp}/src/joint_sine_overlay_node.cpp (100%) rename lbr_demos/{lbr_demos_fri_ros2_cpp => lbr_demos_forward_command_controllers_cpp}/src/torque_sine_overlay_node.cpp (100%) rename lbr_demos/{lbr_demos_fri_ros2_cpp => lbr_demos_forward_command_controllers_cpp}/src/wrench_sine_overlay_node.cpp (100%) rename lbr_demos/{lbr_demos_fri_ros2_python => lbr_demos_forward_command_controllers_python}/doc/lbr_demos_fri_ros2_python.rst (100%) rename lbr_demos/{lbr_demos_fri_ros2_python => lbr_demos_forward_command_controllers_python}/launch/joint_sine_overlay.launch.py (100%) rename lbr_demos/{lbr_demos_fri_ros2_python => lbr_demos_forward_command_controllers_python}/launch/torque_sine_overlay.launch.py (100%) rename lbr_demos/{lbr_demos_fri_ros2_python => lbr_demos_forward_command_controllers_python}/launch/wrench_sine_overlay.launch.py (100%) rename lbr_demos/{lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python => lbr_demos_forward_command_controllers_python/lbr_demos_forward_command_controllers_python}/__init__.py (100%) rename lbr_demos/{lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python => lbr_demos_forward_command_controllers_python/lbr_demos_forward_command_controllers_python}/joint_sine_overlay_node.py (100%) rename lbr_demos/{lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python => lbr_demos_forward_command_controllers_python/lbr_demos_forward_command_controllers_python}/torque_sine_overlay_node.py (100%) rename lbr_demos/{lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python => lbr_demos_forward_command_controllers_python/lbr_demos_forward_command_controllers_python}/wrench_sine_overlay_node.py (100%) rename lbr_demos/{lbr_demos_fri_ros2_python => lbr_demos_forward_command_controllers_python}/package.xml (100%) rename lbr_demos/{lbr_demos_fri_ros2_python => lbr_demos_forward_command_controllers_python}/resource/lbr_demos_fri_ros2_python (100%) rename lbr_demos/{lbr_demos_fri_ros2_python => lbr_demos_forward_command_controllers_python}/setup.cfg (100%) rename lbr_demos/{lbr_demos_fri_ros2_python => lbr_demos_forward_command_controllers_python}/setup.py (100%) rename lbr_demos/{lbr_demos_fri_ros2_python => lbr_demos_forward_command_controllers_python}/test/test_copyright.py (100%) rename lbr_demos/{lbr_demos_fri_ros2_python => lbr_demos_forward_command_controllers_python}/test/test_flake8.py (100%) rename lbr_demos/{lbr_demos_fri_ros2_python => lbr_demos_forward_command_controllers_python}/test/test_pep257.py (100%) rename lbr_demos/{lbr_demos_ros2_control_cpp => lbr_demos_joint_trajectory_controller_cpp}/CMakeLists.txt (100%) rename lbr_demos/{lbr_demos_ros2_control_cpp => lbr_demos_joint_trajectory_controller_cpp}/doc/lbr_demos_ros2_control_cpp.rst (100%) rename lbr_demos/{lbr_demos_ros2_control_cpp => lbr_demos_joint_trajectory_controller_cpp}/package.xml (100%) rename lbr_demos/{lbr_demos_ros2_control_cpp => lbr_demos_joint_trajectory_controller_cpp}/src/joint_trajectory_executioner_node.cpp (100%) rename lbr_demos/{lbr_demos_ros2_control_python => lbr_demos_joint_trajectory_controller_python}/doc/lbr_demos_ros2_control_python.rst (100%) rename lbr_demos/{lbr_demos_ros2_control_python/lbr_demos_ros2_control_python => lbr_demos_joint_trajectory_controller_python/lbr_demos_joint_trajectory_controller_python}/__init__.py (100%) rename lbr_demos/{lbr_demos_ros2_control_python/lbr_demos_ros2_control_python => lbr_demos_joint_trajectory_controller_python/lbr_demos_joint_trajectory_controller_python}/joint_trajectory_executioner_node.py (100%) rename lbr_demos/{lbr_demos_ros2_control_python => lbr_demos_joint_trajectory_controller_python}/package.xml (100%) rename lbr_demos/{lbr_demos_ros2_control_python => lbr_demos_joint_trajectory_controller_python}/resource/lbr_demos_ros2_control_python (100%) rename lbr_demos/{lbr_demos_ros2_control_python => lbr_demos_joint_trajectory_controller_python}/setup.cfg (100%) rename lbr_demos/{lbr_demos_ros2_control_python => lbr_demos_joint_trajectory_controller_python}/setup.py (100%) rename lbr_demos/{lbr_demos_ros2_control_python => lbr_demos_joint_trajectory_controller_python}/test/test_copyright.py (100%) rename lbr_demos/{lbr_demos_ros2_control_python => lbr_demos_joint_trajectory_controller_python}/test/test_flake8.py (100%) rename lbr_demos/{lbr_demos_ros2_control_python => lbr_demos_joint_trajectory_controller_python}/test/test_pep257.py (100%) diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/CMakeLists.txt b/lbr_demos/lbr_demos_admittance_control_cpp/CMakeLists.txt similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_advanced_cpp/CMakeLists.txt rename to lbr_demos/lbr_demos_admittance_control_cpp/CMakeLists.txt diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/config/admittance_control_node.yaml b/lbr_demos/lbr_demos_admittance_control_cpp/config/admittance_control_node.yaml similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_advanced_cpp/config/admittance_control_node.yaml rename to lbr_demos/lbr_demos_admittance_control_cpp/config/admittance_control_node.yaml diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/doc/lbr_demos_fri_ros2_advanced_cpp.rst b/lbr_demos/lbr_demos_admittance_control_cpp/doc/lbr_demos_fri_ros2_advanced_cpp.rst similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_advanced_cpp/doc/lbr_demos_fri_ros2_advanced_cpp.rst rename to lbr_demos/lbr_demos_admittance_control_cpp/doc/lbr_demos_fri_ros2_advanced_cpp.rst diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/launch/admittance_control_node.launch.py b/lbr_demos/lbr_demos_admittance_control_cpp/launch/admittance_control_node.launch.py similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_advanced_cpp/launch/admittance_control_node.launch.py rename to lbr_demos/lbr_demos_admittance_control_cpp/launch/admittance_control_node.launch.py diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/package.xml b/lbr_demos/lbr_demos_admittance_control_cpp/package.xml similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_advanced_cpp/package.xml rename to lbr_demos/lbr_demos_admittance_control_cpp/package.xml diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_control_node.cpp b/lbr_demos/lbr_demos_admittance_control_cpp/src/admittance_control_node.cpp similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_control_node.cpp rename to lbr_demos/lbr_demos_admittance_control_cpp/src/admittance_control_node.cpp diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_controller.hpp b/lbr_demos/lbr_demos_admittance_control_cpp/src/admittance_controller.hpp similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_controller.hpp rename to lbr_demos/lbr_demos_admittance_control_cpp/src/admittance_controller.hpp diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/doc/lbr_demos_fri_ros2_advanced_python.rst b/lbr_demos/lbr_demos_admittance_control_python/doc/lbr_demos_fri_ros2_advanced_python.rst similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_advanced_python/doc/lbr_demos_fri_ros2_advanced_python.rst rename to lbr_demos/lbr_demos_admittance_control_python/doc/lbr_demos_fri_ros2_advanced_python.rst diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/launch/admittance_control_node.launch.py b/lbr_demos/lbr_demos_admittance_control_python/launch/admittance_control_node.launch.py similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_advanced_python/launch/admittance_control_node.launch.py rename to lbr_demos/lbr_demos_admittance_control_python/launch/admittance_control_node.launch.py diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/__init__.py b/lbr_demos/lbr_demos_admittance_control_python/lbr_demos_admittance_control_python/__init__.py similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/__init__.py rename to lbr_demos/lbr_demos_admittance_control_python/lbr_demos_admittance_control_python/__init__.py diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/admittance_control_node.py b/lbr_demos/lbr_demos_admittance_control_python/lbr_demos_admittance_control_python/admittance_control_node.py similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/admittance_control_node.py rename to lbr_demos/lbr_demos_admittance_control_python/lbr_demos_admittance_control_python/admittance_control_node.py diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/admittance_controller.py b/lbr_demos/lbr_demos_admittance_control_python/lbr_demos_admittance_control_python/admittance_controller.py similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/admittance_controller.py rename to lbr_demos/lbr_demos_admittance_control_python/lbr_demos_admittance_control_python/admittance_controller.py diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/package.xml b/lbr_demos/lbr_demos_admittance_control_python/package.xml similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_advanced_python/package.xml rename to lbr_demos/lbr_demos_admittance_control_python/package.xml diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/resource/lbr_demos_fri_ros2_advanced_python b/lbr_demos/lbr_demos_admittance_control_python/resource/lbr_demos_fri_ros2_advanced_python similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_advanced_python/resource/lbr_demos_fri_ros2_advanced_python rename to lbr_demos/lbr_demos_admittance_control_python/resource/lbr_demos_fri_ros2_advanced_python diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/setup.cfg b/lbr_demos/lbr_demos_admittance_control_python/setup.cfg similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_advanced_python/setup.cfg rename to lbr_demos/lbr_demos_admittance_control_python/setup.cfg diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/setup.py b/lbr_demos/lbr_demos_admittance_control_python/setup.py similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_advanced_python/setup.py rename to lbr_demos/lbr_demos_admittance_control_python/setup.py diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/test/test_copyright.py b/lbr_demos/lbr_demos_admittance_control_python/test/test_copyright.py similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_advanced_python/test/test_copyright.py rename to lbr_demos/lbr_demos_admittance_control_python/test/test_copyright.py diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/test/test_flake8.py b/lbr_demos/lbr_demos_admittance_control_python/test/test_flake8.py similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_advanced_python/test/test_flake8.py rename to lbr_demos/lbr_demos_admittance_control_python/test/test_flake8.py diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/test/test_pep257.py b/lbr_demos/lbr_demos_admittance_control_python/test/test_pep257.py similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_advanced_python/test/test_pep257.py rename to lbr_demos/lbr_demos_admittance_control_python/test/test_pep257.py diff --git a/lbr_demos/lbr_demos_fri_ros2_cpp/CMakeLists.txt b/lbr_demos/lbr_demos_forward_command_controllers_cpp/CMakeLists.txt similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_cpp/CMakeLists.txt rename to lbr_demos/lbr_demos_forward_command_controllers_cpp/CMakeLists.txt diff --git a/lbr_demos/lbr_demos_fri_ros2_cpp/doc/lbr_demos_fri_ros2_cpp.rst b/lbr_demos/lbr_demos_forward_command_controllers_cpp/doc/lbr_demos_fri_ros2_cpp.rst similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_cpp/doc/lbr_demos_fri_ros2_cpp.rst rename to lbr_demos/lbr_demos_forward_command_controllers_cpp/doc/lbr_demos_fri_ros2_cpp.rst diff --git a/lbr_demos/lbr_demos_fri_ros2_cpp/launch/joint_sine_overlay.launch.py b/lbr_demos/lbr_demos_forward_command_controllers_cpp/launch/joint_sine_overlay.launch.py similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_cpp/launch/joint_sine_overlay.launch.py rename to lbr_demos/lbr_demos_forward_command_controllers_cpp/launch/joint_sine_overlay.launch.py diff --git a/lbr_demos/lbr_demos_fri_ros2_cpp/launch/torque_sine_overlay.launch.py b/lbr_demos/lbr_demos_forward_command_controllers_cpp/launch/torque_sine_overlay.launch.py similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_cpp/launch/torque_sine_overlay.launch.py rename to lbr_demos/lbr_demos_forward_command_controllers_cpp/launch/torque_sine_overlay.launch.py diff --git a/lbr_demos/lbr_demos_fri_ros2_cpp/launch/wrench_sine_overlay.launch.py b/lbr_demos/lbr_demos_forward_command_controllers_cpp/launch/wrench_sine_overlay.launch.py similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_cpp/launch/wrench_sine_overlay.launch.py rename to lbr_demos/lbr_demos_forward_command_controllers_cpp/launch/wrench_sine_overlay.launch.py diff --git a/lbr_demos/lbr_demos_fri_ros2_cpp/package.xml b/lbr_demos/lbr_demos_forward_command_controllers_cpp/package.xml similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_cpp/package.xml rename to lbr_demos/lbr_demos_forward_command_controllers_cpp/package.xml diff --git a/lbr_demos/lbr_demos_fri_ros2_cpp/src/joint_sine_overlay_node.cpp b/lbr_demos/lbr_demos_forward_command_controllers_cpp/src/joint_sine_overlay_node.cpp similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_cpp/src/joint_sine_overlay_node.cpp rename to lbr_demos/lbr_demos_forward_command_controllers_cpp/src/joint_sine_overlay_node.cpp diff --git a/lbr_demos/lbr_demos_fri_ros2_cpp/src/torque_sine_overlay_node.cpp b/lbr_demos/lbr_demos_forward_command_controllers_cpp/src/torque_sine_overlay_node.cpp similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_cpp/src/torque_sine_overlay_node.cpp rename to lbr_demos/lbr_demos_forward_command_controllers_cpp/src/torque_sine_overlay_node.cpp diff --git a/lbr_demos/lbr_demos_fri_ros2_cpp/src/wrench_sine_overlay_node.cpp b/lbr_demos/lbr_demos_forward_command_controllers_cpp/src/wrench_sine_overlay_node.cpp similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_cpp/src/wrench_sine_overlay_node.cpp rename to lbr_demos/lbr_demos_forward_command_controllers_cpp/src/wrench_sine_overlay_node.cpp diff --git a/lbr_demos/lbr_demos_fri_ros2_python/doc/lbr_demos_fri_ros2_python.rst b/lbr_demos/lbr_demos_forward_command_controllers_python/doc/lbr_demos_fri_ros2_python.rst similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_python/doc/lbr_demos_fri_ros2_python.rst rename to lbr_demos/lbr_demos_forward_command_controllers_python/doc/lbr_demos_fri_ros2_python.rst diff --git a/lbr_demos/lbr_demos_fri_ros2_python/launch/joint_sine_overlay.launch.py b/lbr_demos/lbr_demos_forward_command_controllers_python/launch/joint_sine_overlay.launch.py similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_python/launch/joint_sine_overlay.launch.py rename to lbr_demos/lbr_demos_forward_command_controllers_python/launch/joint_sine_overlay.launch.py diff --git a/lbr_demos/lbr_demos_fri_ros2_python/launch/torque_sine_overlay.launch.py b/lbr_demos/lbr_demos_forward_command_controllers_python/launch/torque_sine_overlay.launch.py similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_python/launch/torque_sine_overlay.launch.py rename to lbr_demos/lbr_demos_forward_command_controllers_python/launch/torque_sine_overlay.launch.py diff --git a/lbr_demos/lbr_demos_fri_ros2_python/launch/wrench_sine_overlay.launch.py b/lbr_demos/lbr_demos_forward_command_controllers_python/launch/wrench_sine_overlay.launch.py similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_python/launch/wrench_sine_overlay.launch.py rename to lbr_demos/lbr_demos_forward_command_controllers_python/launch/wrench_sine_overlay.launch.py diff --git a/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/__init__.py b/lbr_demos/lbr_demos_forward_command_controllers_python/lbr_demos_forward_command_controllers_python/__init__.py similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/__init__.py rename to lbr_demos/lbr_demos_forward_command_controllers_python/lbr_demos_forward_command_controllers_python/__init__.py diff --git a/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/joint_sine_overlay_node.py b/lbr_demos/lbr_demos_forward_command_controllers_python/lbr_demos_forward_command_controllers_python/joint_sine_overlay_node.py similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/joint_sine_overlay_node.py rename to lbr_demos/lbr_demos_forward_command_controllers_python/lbr_demos_forward_command_controllers_python/joint_sine_overlay_node.py diff --git a/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/torque_sine_overlay_node.py b/lbr_demos/lbr_demos_forward_command_controllers_python/lbr_demos_forward_command_controllers_python/torque_sine_overlay_node.py similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/torque_sine_overlay_node.py rename to lbr_demos/lbr_demos_forward_command_controllers_python/lbr_demos_forward_command_controllers_python/torque_sine_overlay_node.py diff --git a/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/wrench_sine_overlay_node.py b/lbr_demos/lbr_demos_forward_command_controllers_python/lbr_demos_forward_command_controllers_python/wrench_sine_overlay_node.py similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/wrench_sine_overlay_node.py rename to lbr_demos/lbr_demos_forward_command_controllers_python/lbr_demos_forward_command_controllers_python/wrench_sine_overlay_node.py diff --git a/lbr_demos/lbr_demos_fri_ros2_python/package.xml b/lbr_demos/lbr_demos_forward_command_controllers_python/package.xml similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_python/package.xml rename to lbr_demos/lbr_demos_forward_command_controllers_python/package.xml diff --git a/lbr_demos/lbr_demos_fri_ros2_python/resource/lbr_demos_fri_ros2_python b/lbr_demos/lbr_demos_forward_command_controllers_python/resource/lbr_demos_fri_ros2_python similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_python/resource/lbr_demos_fri_ros2_python rename to lbr_demos/lbr_demos_forward_command_controllers_python/resource/lbr_demos_fri_ros2_python diff --git a/lbr_demos/lbr_demos_fri_ros2_python/setup.cfg b/lbr_demos/lbr_demos_forward_command_controllers_python/setup.cfg similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_python/setup.cfg rename to lbr_demos/lbr_demos_forward_command_controllers_python/setup.cfg diff --git a/lbr_demos/lbr_demos_fri_ros2_python/setup.py b/lbr_demos/lbr_demos_forward_command_controllers_python/setup.py similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_python/setup.py rename to lbr_demos/lbr_demos_forward_command_controllers_python/setup.py diff --git a/lbr_demos/lbr_demos_fri_ros2_python/test/test_copyright.py b/lbr_demos/lbr_demos_forward_command_controllers_python/test/test_copyright.py similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_python/test/test_copyright.py rename to lbr_demos/lbr_demos_forward_command_controllers_python/test/test_copyright.py diff --git a/lbr_demos/lbr_demos_fri_ros2_python/test/test_flake8.py b/lbr_demos/lbr_demos_forward_command_controllers_python/test/test_flake8.py similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_python/test/test_flake8.py rename to lbr_demos/lbr_demos_forward_command_controllers_python/test/test_flake8.py diff --git a/lbr_demos/lbr_demos_fri_ros2_python/test/test_pep257.py b/lbr_demos/lbr_demos_forward_command_controllers_python/test/test_pep257.py similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_python/test/test_pep257.py rename to lbr_demos/lbr_demos_forward_command_controllers_python/test/test_pep257.py diff --git a/lbr_demos/lbr_demos_ros2_control_cpp/CMakeLists.txt b/lbr_demos/lbr_demos_joint_trajectory_controller_cpp/CMakeLists.txt similarity index 100% rename from lbr_demos/lbr_demos_ros2_control_cpp/CMakeLists.txt rename to lbr_demos/lbr_demos_joint_trajectory_controller_cpp/CMakeLists.txt diff --git a/lbr_demos/lbr_demos_ros2_control_cpp/doc/lbr_demos_ros2_control_cpp.rst b/lbr_demos/lbr_demos_joint_trajectory_controller_cpp/doc/lbr_demos_ros2_control_cpp.rst similarity index 100% rename from lbr_demos/lbr_demos_ros2_control_cpp/doc/lbr_demos_ros2_control_cpp.rst rename to lbr_demos/lbr_demos_joint_trajectory_controller_cpp/doc/lbr_demos_ros2_control_cpp.rst diff --git a/lbr_demos/lbr_demos_ros2_control_cpp/package.xml b/lbr_demos/lbr_demos_joint_trajectory_controller_cpp/package.xml similarity index 100% rename from lbr_demos/lbr_demos_ros2_control_cpp/package.xml rename to lbr_demos/lbr_demos_joint_trajectory_controller_cpp/package.xml diff --git a/lbr_demos/lbr_demos_ros2_control_cpp/src/joint_trajectory_executioner_node.cpp b/lbr_demos/lbr_demos_joint_trajectory_controller_cpp/src/joint_trajectory_executioner_node.cpp similarity index 100% rename from lbr_demos/lbr_demos_ros2_control_cpp/src/joint_trajectory_executioner_node.cpp rename to lbr_demos/lbr_demos_joint_trajectory_controller_cpp/src/joint_trajectory_executioner_node.cpp diff --git a/lbr_demos/lbr_demos_ros2_control_python/doc/lbr_demos_ros2_control_python.rst b/lbr_demos/lbr_demos_joint_trajectory_controller_python/doc/lbr_demos_ros2_control_python.rst similarity index 100% rename from lbr_demos/lbr_demos_ros2_control_python/doc/lbr_demos_ros2_control_python.rst rename to lbr_demos/lbr_demos_joint_trajectory_controller_python/doc/lbr_demos_ros2_control_python.rst diff --git a/lbr_demos/lbr_demos_ros2_control_python/lbr_demos_ros2_control_python/__init__.py b/lbr_demos/lbr_demos_joint_trajectory_controller_python/lbr_demos_joint_trajectory_controller_python/__init__.py similarity index 100% rename from lbr_demos/lbr_demos_ros2_control_python/lbr_demos_ros2_control_python/__init__.py rename to lbr_demos/lbr_demos_joint_trajectory_controller_python/lbr_demos_joint_trajectory_controller_python/__init__.py diff --git a/lbr_demos/lbr_demos_ros2_control_python/lbr_demos_ros2_control_python/joint_trajectory_executioner_node.py b/lbr_demos/lbr_demos_joint_trajectory_controller_python/lbr_demos_joint_trajectory_controller_python/joint_trajectory_executioner_node.py similarity index 100% rename from lbr_demos/lbr_demos_ros2_control_python/lbr_demos_ros2_control_python/joint_trajectory_executioner_node.py rename to lbr_demos/lbr_demos_joint_trajectory_controller_python/lbr_demos_joint_trajectory_controller_python/joint_trajectory_executioner_node.py diff --git a/lbr_demos/lbr_demos_ros2_control_python/package.xml b/lbr_demos/lbr_demos_joint_trajectory_controller_python/package.xml similarity index 100% rename from lbr_demos/lbr_demos_ros2_control_python/package.xml rename to lbr_demos/lbr_demos_joint_trajectory_controller_python/package.xml diff --git a/lbr_demos/lbr_demos_ros2_control_python/resource/lbr_demos_ros2_control_python b/lbr_demos/lbr_demos_joint_trajectory_controller_python/resource/lbr_demos_ros2_control_python similarity index 100% rename from lbr_demos/lbr_demos_ros2_control_python/resource/lbr_demos_ros2_control_python rename to lbr_demos/lbr_demos_joint_trajectory_controller_python/resource/lbr_demos_ros2_control_python diff --git a/lbr_demos/lbr_demos_ros2_control_python/setup.cfg b/lbr_demos/lbr_demos_joint_trajectory_controller_python/setup.cfg similarity index 100% rename from lbr_demos/lbr_demos_ros2_control_python/setup.cfg rename to lbr_demos/lbr_demos_joint_trajectory_controller_python/setup.cfg diff --git a/lbr_demos/lbr_demos_ros2_control_python/setup.py b/lbr_demos/lbr_demos_joint_trajectory_controller_python/setup.py similarity index 100% rename from lbr_demos/lbr_demos_ros2_control_python/setup.py rename to lbr_demos/lbr_demos_joint_trajectory_controller_python/setup.py diff --git a/lbr_demos/lbr_demos_ros2_control_python/test/test_copyright.py b/lbr_demos/lbr_demos_joint_trajectory_controller_python/test/test_copyright.py similarity index 100% rename from lbr_demos/lbr_demos_ros2_control_python/test/test_copyright.py rename to lbr_demos/lbr_demos_joint_trajectory_controller_python/test/test_copyright.py diff --git a/lbr_demos/lbr_demos_ros2_control_python/test/test_flake8.py b/lbr_demos/lbr_demos_joint_trajectory_controller_python/test/test_flake8.py similarity index 100% rename from lbr_demos/lbr_demos_ros2_control_python/test/test_flake8.py rename to lbr_demos/lbr_demos_joint_trajectory_controller_python/test/test_flake8.py diff --git a/lbr_demos/lbr_demos_ros2_control_python/test/test_pep257.py b/lbr_demos/lbr_demos_joint_trajectory_controller_python/test/test_pep257.py similarity index 100% rename from lbr_demos/lbr_demos_ros2_control_python/test/test_pep257.py rename to lbr_demos/lbr_demos_joint_trajectory_controller_python/test/test_pep257.py From 1eef8bd8e19fdfad67cd689f686b7c9cfff84772 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 9 Apr 2024 11:34:03 +0100 Subject: [PATCH 34/76] added a note --- lbr_demos/doc/lbr_demos.rst | 2 ++ 1 file changed, 2 insertions(+) diff --git a/lbr_demos/doc/lbr_demos.rst b/lbr_demos/doc/lbr_demos.rst index 33f4ec43..9992de56 100644 --- a/lbr_demos/doc/lbr_demos.rst +++ b/lbr_demos/doc/lbr_demos.rst @@ -7,6 +7,8 @@ Demos for controlling the LBR through the Fast Robot Interface (FRI) from ROS 2. TODO: Differentiate demos through controllers TODO: indicate which demos run in simulation and which on the real robot +TODO: Introduce controller concept + LBR Demos FRI ROS 2 ------------------- From 45354ec28f3ed9e77a4ab1d03ab8de5dbc7177e8 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 8 May 2024 11:12:13 +0100 Subject: [PATCH 35/76] demo re-structure --- lbr_demos/doc/lbr_demos.rst | 8 ++-- .../setup.cfg | 4 -- .../CMakeLists.txt | 2 +- .../config/admittance_control.yaml | 0 .../doc/lbr_demos_advanced_cpp.rst} | 10 ++--- .../package.xml | 2 +- .../src/admittance_control_node.cpp | 0 .../src/admittance_controller.hpp | 6 +-- .../src/lbr_base_position_command_node.hpp | 6 +-- .../src/pose_control_node.cpp | 0 .../src/pose_planning_node.cpp | 0 .../config/admittance_control.yaml | 0 .../config/admittance_rcm_control.yaml | 0 .../doc/lbr_demos_advanced_py.rst} | 12 +++--- .../lbr_demos_advanced_py}/__init__.py | 0 .../admittance_control_node.py | 0 .../admittance_controller.py | 0 .../admittance_rcm_control_node.py | 0 .../admittance_rcm_controller.py | 0 .../lbr_base_position_command_node.py | 0 .../package.xml | 2 +- .../resource/lbr_demos_advanced_py} | 0 lbr_demos/lbr_demos_advanced_py/setup.cfg | 4 ++ .../setup.py | 6 +-- .../test/test_copyright.py | 0 .../test/test_flake8.py | 0 .../test/test_pep257.py | 0 .../CMakeLists.txt | 25 ++++++++++- .../doc/lbr_demos_cpp.rst} | 18 ++++---- .../doc/lbr_demos_ros2_control_cpp.rst | 0 .../launch/joint_sine_overlay.launch.py | 2 +- .../launch/torque_sine_overlay.launch.py | 2 +- .../launch/wrench_sine_overlay.launch.py | 2 +- .../package.xml | 6 +-- .../src/joint_sine_overlay_node.cpp | 0 .../src/joint_trajectory_executioner_node.cpp | 0 .../src/torque_sine_overlay_node.cpp | 0 .../src/wrench_sine_overlay_node.cpp | 0 .../package.xml | 24 ----------- .../package.xml | 22 ---------- .../setup.cfg | 4 -- .../CMakeLists.txt | 42 ------------------- .../__init__.py | 0 .../resource/lbr_demos_ros2_control_python | 0 .../setup.cfg | 4 -- .../setup.py | 25 ----------- .../test/test_copyright.py | 27 ------------ .../test/test_flake8.py | 25 ----------- .../test/test_pep257.py | 23 ---------- .../doc/lbr_demos_py.rst} | 18 ++++---- .../doc/lbr_demos_ros2_control_python.rst | 0 .../launch/joint_sine_overlay.launch.py | 2 +- .../launch/torque_sine_overlay.launch.py | 2 +- .../launch/wrench_sine_overlay.launch.py | 2 +- .../lbr_demos_py}/__init__.py | 0 .../lbr_demos_py}/joint_sine_overlay_node.py | 0 .../joint_trajectory_executioner_node.py | 0 .../lbr_demos_py}/torque_sine_overlay_node.py | 0 .../lbr_demos_py}/wrench_sine_overlay_node.py | 0 .../package.xml | 6 +-- .../resource/lbr_demos_py} | 0 lbr_demos/lbr_demos_py/setup.cfg | 4 ++ .../setup.py | 9 ++-- .../test/test_copyright.py | 0 .../test/test_flake8.py | 0 .../test/test_pep257.py | 0 66 files changed, 93 insertions(+), 263 deletions(-) delete mode 100644 lbr_demos/lbr_demos_admittance_control_python/setup.cfg rename lbr_demos/{lbr_demos_admittance_control_cpp => lbr_demos_advanced_cpp}/CMakeLists.txt (98%) rename lbr_demos/{lbr_demos_admittance_control_cpp => lbr_demos_advanced_cpp}/config/admittance_control.yaml (100%) rename lbr_demos/{lbr_demos_admittance_control_cpp/doc/lbr_demos_fri_ros2_advanced_cpp.rst => lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst} (78%) rename lbr_demos/{lbr_demos_admittance_control_cpp => lbr_demos_advanced_cpp}/package.xml (94%) rename lbr_demos/{lbr_demos_admittance_control_cpp => lbr_demos_advanced_cpp}/src/admittance_control_node.cpp (100%) rename lbr_demos/{lbr_demos_admittance_control_cpp => lbr_demos_advanced_cpp}/src/admittance_controller.hpp (93%) rename lbr_demos/{lbr_demos_admittance_control_cpp => lbr_demos_advanced_cpp}/src/lbr_base_position_command_node.hpp (91%) rename lbr_demos/{lbr_demos_admittance_control_cpp => lbr_demos_advanced_cpp}/src/pose_control_node.cpp (100%) rename lbr_demos/{lbr_demos_admittance_control_cpp => lbr_demos_advanced_cpp}/src/pose_planning_node.cpp (100%) rename lbr_demos/{lbr_demos_fri_ros2_advanced_python => lbr_demos_advanced_py}/config/admittance_control.yaml (100%) rename lbr_demos/{lbr_demos_fri_ros2_advanced_python => lbr_demos_advanced_py}/config/admittance_rcm_control.yaml (100%) rename lbr_demos/{lbr_demos_admittance_control_python/doc/lbr_demos_fri_ros2_advanced_python.rst => lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst} (69%) rename lbr_demos/{lbr_demos_admittance_control_python/lbr_demos_admittance_control_python => lbr_demos_advanced_py/lbr_demos_advanced_py}/__init__.py (100%) rename lbr_demos/{lbr_demos_admittance_control_python/lbr_demos_admittance_control_python => lbr_demos_advanced_py/lbr_demos_advanced_py}/admittance_control_node.py (100%) rename lbr_demos/{lbr_demos_admittance_control_python/lbr_demos_admittance_control_python => lbr_demos_advanced_py/lbr_demos_advanced_py}/admittance_controller.py (100%) rename lbr_demos/{lbr_demos_admittance_control_python/lbr_demos_admittance_control_python => lbr_demos_advanced_py/lbr_demos_advanced_py}/admittance_rcm_control_node.py (100%) rename lbr_demos/{lbr_demos_admittance_control_python/lbr_demos_admittance_control_python => lbr_demos_advanced_py/lbr_demos_advanced_py}/admittance_rcm_controller.py (100%) rename lbr_demos/{lbr_demos_admittance_control_python/lbr_demos_admittance_control_python => lbr_demos_advanced_py/lbr_demos_advanced_py}/lbr_base_position_command_node.py (100%) rename lbr_demos/{lbr_demos_admittance_control_python => lbr_demos_advanced_py}/package.xml (94%) rename lbr_demos/{lbr_demos_admittance_control_python/resource/lbr_demos_fri_ros2_advanced_python => lbr_demos_advanced_py/resource/lbr_demos_advanced_py} (100%) create mode 100644 lbr_demos/lbr_demos_advanced_py/setup.cfg rename lbr_demos/{lbr_demos_admittance_control_python => lbr_demos_advanced_py}/setup.py (74%) rename lbr_demos/{lbr_demos_admittance_control_python => lbr_demos_advanced_py}/test/test_copyright.py (100%) rename lbr_demos/{lbr_demos_admittance_control_python => lbr_demos_advanced_py}/test/test_flake8.py (100%) rename lbr_demos/{lbr_demos_admittance_control_python => lbr_demos_advanced_py}/test/test_pep257.py (100%) rename lbr_demos/{lbr_demos_forward_command_controllers_cpp => lbr_demos_cpp}/CMakeLists.txt (73%) rename lbr_demos/{lbr_demos_forward_command_controllers_cpp/doc/lbr_demos_fri_ros2_cpp.rst => lbr_demos_cpp/doc/lbr_demos_cpp.rst} (64%) rename lbr_demos/{lbr_demos_joint_trajectory_controller_cpp => lbr_demos_cpp}/doc/lbr_demos_ros2_control_cpp.rst (100%) rename lbr_demos/{lbr_demos_forward_command_controllers_cpp => lbr_demos_cpp}/launch/joint_sine_overlay.launch.py (94%) rename lbr_demos/{lbr_demos_forward_command_controllers_cpp => lbr_demos_cpp}/launch/torque_sine_overlay.launch.py (94%) rename lbr_demos/{lbr_demos_forward_command_controllers_cpp => lbr_demos_cpp}/launch/wrench_sine_overlay.launch.py (94%) rename lbr_demos/{lbr_demos_joint_trajectory_controller_cpp => lbr_demos_cpp}/package.xml (83%) rename lbr_demos/{lbr_demos_forward_command_controllers_cpp => lbr_demos_cpp}/src/joint_sine_overlay_node.cpp (100%) rename lbr_demos/{lbr_demos_joint_trajectory_controller_cpp => lbr_demos_cpp}/src/joint_trajectory_executioner_node.cpp (100%) rename lbr_demos/{lbr_demos_forward_command_controllers_cpp => lbr_demos_cpp}/src/torque_sine_overlay_node.cpp (100%) rename lbr_demos/{lbr_demos_forward_command_controllers_cpp => lbr_demos_cpp}/src/wrench_sine_overlay_node.cpp (100%) delete mode 100644 lbr_demos/lbr_demos_forward_command_controllers_cpp/package.xml delete mode 100644 lbr_demos/lbr_demos_forward_command_controllers_python/package.xml delete mode 100644 lbr_demos/lbr_demos_forward_command_controllers_python/setup.cfg delete mode 100644 lbr_demos/lbr_demos_joint_trajectory_controller_cpp/CMakeLists.txt delete mode 100644 lbr_demos/lbr_demos_joint_trajectory_controller_python/lbr_demos_joint_trajectory_controller_python/__init__.py delete mode 100644 lbr_demos/lbr_demos_joint_trajectory_controller_python/resource/lbr_demos_ros2_control_python delete mode 100644 lbr_demos/lbr_demos_joint_trajectory_controller_python/setup.cfg delete mode 100644 lbr_demos/lbr_demos_joint_trajectory_controller_python/setup.py delete mode 100644 lbr_demos/lbr_demos_joint_trajectory_controller_python/test/test_copyright.py delete mode 100644 lbr_demos/lbr_demos_joint_trajectory_controller_python/test/test_flake8.py delete mode 100644 lbr_demos/lbr_demos_joint_trajectory_controller_python/test/test_pep257.py rename lbr_demos/{lbr_demos_forward_command_controllers_python/doc/lbr_demos_fri_ros2_python.rst => lbr_demos_py/doc/lbr_demos_py.rst} (63%) rename lbr_demos/{lbr_demos_joint_trajectory_controller_python => lbr_demos_py}/doc/lbr_demos_ros2_control_python.rst (100%) rename lbr_demos/{lbr_demos_forward_command_controllers_python => lbr_demos_py}/launch/joint_sine_overlay.launch.py (93%) rename lbr_demos/{lbr_demos_forward_command_controllers_python => lbr_demos_py}/launch/torque_sine_overlay.launch.py (93%) rename lbr_demos/{lbr_demos_forward_command_controllers_python => lbr_demos_py}/launch/wrench_sine_overlay.launch.py (93%) rename lbr_demos/{lbr_demos_forward_command_controllers_python/lbr_demos_forward_command_controllers_python => lbr_demos_py/lbr_demos_py}/__init__.py (100%) rename lbr_demos/{lbr_demos_forward_command_controllers_python/lbr_demos_forward_command_controllers_python => lbr_demos_py/lbr_demos_py}/joint_sine_overlay_node.py (100%) rename lbr_demos/{lbr_demos_joint_trajectory_controller_python/lbr_demos_joint_trajectory_controller_python => lbr_demos_py/lbr_demos_py}/joint_trajectory_executioner_node.py (100%) rename lbr_demos/{lbr_demos_forward_command_controllers_python/lbr_demos_forward_command_controllers_python => lbr_demos_py/lbr_demos_py}/torque_sine_overlay_node.py (100%) rename lbr_demos/{lbr_demos_forward_command_controllers_python/lbr_demos_forward_command_controllers_python => lbr_demos_py/lbr_demos_py}/wrench_sine_overlay_node.py (100%) rename lbr_demos/{lbr_demos_joint_trajectory_controller_python => lbr_demos_py}/package.xml (82%) rename lbr_demos/{lbr_demos_forward_command_controllers_python/resource/lbr_demos_fri_ros2_python => lbr_demos_py/resource/lbr_demos_py} (100%) create mode 100644 lbr_demos/lbr_demos_py/setup.cfg rename lbr_demos/{lbr_demos_forward_command_controllers_python => lbr_demos_py}/setup.py (62%) rename lbr_demos/{lbr_demos_forward_command_controllers_python => lbr_demos_py}/test/test_copyright.py (100%) rename lbr_demos/{lbr_demos_forward_command_controllers_python => lbr_demos_py}/test/test_flake8.py (100%) rename lbr_demos/{lbr_demos_forward_command_controllers_python => lbr_demos_py}/test/test_pep257.py (100%) diff --git a/lbr_demos/doc/lbr_demos.rst b/lbr_demos/doc/lbr_demos.rst index 9992de56..dff60db6 100644 --- a/lbr_demos/doc/lbr_demos.rst +++ b/lbr_demos/doc/lbr_demos.rst @@ -21,10 +21,10 @@ LBR Demos FRI ROS 2 .. toctree:: :titlesonly: - LBR Demos FRI ROS 2 C++ <../lbr_demos_fri_ros2_cpp/doc/lbr_demos_fri_ros2_cpp.rst> - LBR Demos FRI ROS 2 Python <../lbr_demos_fri_ros2_python/doc/lbr_demos_fri_ros2_python.rst> - LBR Demos FRI ROS 2 Advanced C++ <../lbr_demos_fri_ros2_advanced_cpp/doc/lbr_demos_fri_ros2_advanced_cpp.rst> - LBR Demos FRI ROS 2 Advanced Python <../lbr_demos_fri_ros2_advanced_python/doc/lbr_demos_fri_ros2_advanced_python.rst> + LBR Demos FRI ROS 2 C++ <../lbr_demos_cpp/doc/lbr_demos_cpp.rst> + LBR Demos FRI ROS 2 Python <../lbr_demos_py/doc/lbr_demos_py.rst> + LBR Demos FRI ROS 2 Advanced C++ <../lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst> + LBR Demos FRI ROS 2 Advanced Python <../lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst> LBR ROS 2 Control Demos ----------------------- diff --git a/lbr_demos/lbr_demos_admittance_control_python/setup.cfg b/lbr_demos/lbr_demos_admittance_control_python/setup.cfg deleted file mode 100644 index 75aa9c55..00000000 --- a/lbr_demos/lbr_demos_admittance_control_python/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/lbr_demos_fri_ros2_advanced_python -[install] -install_scripts=$base/lib/lbr_demos_fri_ros2_advanced_python diff --git a/lbr_demos/lbr_demos_admittance_control_cpp/CMakeLists.txt b/lbr_demos/lbr_demos_advanced_cpp/CMakeLists.txt similarity index 98% rename from lbr_demos/lbr_demos_admittance_control_cpp/CMakeLists.txt rename to lbr_demos/lbr_demos_advanced_cpp/CMakeLists.txt index 37bda587..85f3490b 100644 --- a/lbr_demos/lbr_demos_admittance_control_cpp/CMakeLists.txt +++ b/lbr_demos/lbr_demos_advanced_cpp/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.22) -project(lbr_demos_fri_ros2_advanced_cpp) +project(lbr_demos_advanced_cpp) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) diff --git a/lbr_demos/lbr_demos_admittance_control_cpp/config/admittance_control.yaml b/lbr_demos/lbr_demos_advanced_cpp/config/admittance_control.yaml similarity index 100% rename from lbr_demos/lbr_demos_admittance_control_cpp/config/admittance_control.yaml rename to lbr_demos/lbr_demos_advanced_cpp/config/admittance_control.yaml diff --git a/lbr_demos/lbr_demos_admittance_control_cpp/doc/lbr_demos_fri_ros2_advanced_cpp.rst b/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst similarity index 78% rename from lbr_demos/lbr_demos_admittance_control_cpp/doc/lbr_demos_fri_ros2_advanced_cpp.rst rename to lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst index 51886e5f..2b15e3a1 100644 --- a/lbr_demos/lbr_demos_admittance_control_cpp/doc/lbr_demos_fri_ros2_advanced_cpp.rst +++ b/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst @@ -23,13 +23,13 @@ Admittance Controller ctrl:=forward_lbr_position_command_controller \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] -#. Launch the `admittance_control `_: +#. Launch the `admittance_control `_: .. code-block:: bash - ros2 run lbr_demos_fri_ros2_advanced_cpp admittance_control --ros-args \ + ros2 run lbr_demos_advanced_cpp admittance_control --ros-args \ -r __ns:=/lbr \ - --params-file `ros2 pkg prefix lbr_demos_fri_ros2_advanced_cpp`/share/lbr_demos_fri_ros2_advanced_cpp/config/admittance_control.yaml + --params-file `ros2 pkg prefix lbr_demos_advanced_cpp`/share/lbr_demos_advanced_cpp/config/admittance_control.yaml #. Now gently move the robot at the end-effector. @@ -55,12 +55,12 @@ kinematics to move the robot's end-effector along the z-axis in Cartesian space. .. code-block:: bash - ros2 run lbr_demos_fri_ros2_advanced_cpp pose_control --ros-args \ + ros2 run lbr_demos_advanced_cpp pose_control --ros-args \ -r __ns:=/lbr #. Launch the path planning .. code-block:: bash - ros2 run lbr_demos_fri_ros2_advanced_cpp pose_planning --ros-args \ + ros2 run lbr_demos_advanced_cpp pose_planning --ros-args \ -r __ns:=/lbr diff --git a/lbr_demos/lbr_demos_admittance_control_cpp/package.xml b/lbr_demos/lbr_demos_advanced_cpp/package.xml similarity index 94% rename from lbr_demos/lbr_demos_admittance_control_cpp/package.xml rename to lbr_demos/lbr_demos_advanced_cpp/package.xml index fb66de7b..6dad3a2e 100644 --- a/lbr_demos/lbr_demos_admittance_control_cpp/package.xml +++ b/lbr_demos/lbr_demos_advanced_cpp/package.xml @@ -1,7 +1,7 @@ - lbr_demos_fri_ros2_advanced_cpp + lbr_demos_advanced_cpp 1.4.3 Advanced C++ demos for the lbr_fri_ros2. mhubii diff --git a/lbr_demos/lbr_demos_admittance_control_cpp/src/admittance_control_node.cpp b/lbr_demos/lbr_demos_advanced_cpp/src/admittance_control_node.cpp similarity index 100% rename from lbr_demos/lbr_demos_admittance_control_cpp/src/admittance_control_node.cpp rename to lbr_demos/lbr_demos_advanced_cpp/src/admittance_control_node.cpp diff --git a/lbr_demos/lbr_demos_admittance_control_cpp/src/admittance_controller.hpp b/lbr_demos/lbr_demos_advanced_cpp/src/admittance_controller.hpp similarity index 93% rename from lbr_demos/lbr_demos_admittance_control_cpp/src/admittance_controller.hpp rename to lbr_demos/lbr_demos_advanced_cpp/src/admittance_controller.hpp index cbe056dc..fbdc6dfb 100644 --- a/lbr_demos/lbr_demos_admittance_control_cpp/src/admittance_controller.hpp +++ b/lbr_demos/lbr_demos_advanced_cpp/src/admittance_controller.hpp @@ -1,5 +1,5 @@ -#ifndef LBR_DEMOS_FRI_ROS2_ADVANCED_CPP__ADMITTANCE_CONTROLLER_HPP_ -#define LBR_DEMOS_FRI_ROS2_ADVANCED_CPP__ADMITTANCE_CONTROLLER_HPP_ +#ifndef LBR_DEMOS_ADVANCED_CPP__ADMITTANCE_CONTROLLER_HPP_ +#define LBR_DEMOS_ADVANCED_CPP__ADMITTANCE_CONTROLLER_HPP_ #include #include @@ -86,4 +86,4 @@ class AdmittanceController { cartesian_vector_t f_ext_th_; }; } // end of namespace lbr_demos -#endif // LBR_DEMOS_FRI_ROS2_ADVANCED_CPP__ADMITTANCE_CONTROLLER_HPP_ +#endif // LBR_DEMOS_ADVANCED_CPP__ADMITTANCE_CONTROLLER_HPP_ diff --git a/lbr_demos/lbr_demos_admittance_control_cpp/src/lbr_base_position_command_node.hpp b/lbr_demos/lbr_demos_advanced_cpp/src/lbr_base_position_command_node.hpp similarity index 91% rename from lbr_demos/lbr_demos_admittance_control_cpp/src/lbr_base_position_command_node.hpp rename to lbr_demos/lbr_demos_advanced_cpp/src/lbr_base_position_command_node.hpp index 82dd552f..49787090 100644 --- a/lbr_demos/lbr_demos_admittance_control_cpp/src/lbr_base_position_command_node.hpp +++ b/lbr_demos/lbr_demos_advanced_cpp/src/lbr_base_position_command_node.hpp @@ -1,5 +1,5 @@ -#ifndef LBR_DEMOS_FRI_ROS2_ADVANCED_CPP__LBR_BASE_POSITION_COMMAND_NODE_HPP_ -#define LBR_DEMOS_FRI_ROS2_ADVANCED_CPP__LBR_BASE_POSITION_COMMAND_NODE_HPP_ +#ifndef LBR_DEMOS_ADVANCED_CPP__LBR_BASE_POSITION_COMMAND_NODE_HPP_ +#define LBR_DEMOS_ADVANCED_CPP__LBR_BASE_POSITION_COMMAND_NODE_HPP_ #include #include @@ -64,4 +64,4 @@ class LBRBasePositionCommandNode : public rclcpp::Node { } }; } // end of namespace lbr_demos -#endif // LBR_DEMOS_FRI_ROS2_ADVANCED_CPP__LBR_BASE_POSITION_COMMAND_NODE_HPP_ +#endif // LBR_DEMOS_ADVANCED_CPP__LBR_BASE_POSITION_COMMAND_NODE_HPP_ diff --git a/lbr_demos/lbr_demos_admittance_control_cpp/src/pose_control_node.cpp b/lbr_demos/lbr_demos_advanced_cpp/src/pose_control_node.cpp similarity index 100% rename from lbr_demos/lbr_demos_admittance_control_cpp/src/pose_control_node.cpp rename to lbr_demos/lbr_demos_advanced_cpp/src/pose_control_node.cpp diff --git a/lbr_demos/lbr_demos_admittance_control_cpp/src/pose_planning_node.cpp b/lbr_demos/lbr_demos_advanced_cpp/src/pose_planning_node.cpp similarity index 100% rename from lbr_demos/lbr_demos_admittance_control_cpp/src/pose_planning_node.cpp rename to lbr_demos/lbr_demos_advanced_cpp/src/pose_planning_node.cpp diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/config/admittance_control.yaml b/lbr_demos/lbr_demos_advanced_py/config/admittance_control.yaml similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_advanced_python/config/admittance_control.yaml rename to lbr_demos/lbr_demos_advanced_py/config/admittance_control.yaml diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/config/admittance_rcm_control.yaml b/lbr_demos/lbr_demos_advanced_py/config/admittance_rcm_control.yaml similarity index 100% rename from lbr_demos/lbr_demos_fri_ros2_advanced_python/config/admittance_rcm_control.yaml rename to lbr_demos/lbr_demos_advanced_py/config/admittance_rcm_control.yaml diff --git a/lbr_demos/lbr_demos_admittance_control_python/doc/lbr_demos_fri_ros2_advanced_python.rst b/lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst similarity index 69% rename from lbr_demos/lbr_demos_admittance_control_python/doc/lbr_demos_fri_ros2_advanced_python.rst rename to lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst index 9c8777f6..337fb120 100644 --- a/lbr_demos/lbr_demos_admittance_control_python/doc/lbr_demos_fri_ros2_advanced_python.rst +++ b/lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst @@ -25,13 +25,13 @@ This demo implements a simple admittance controller. ctrl:=forward_lbr_position_command_controller \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] -#. Run the `admittance_control `_ with remapping and parameter file: +#. Run the `admittance_control `_ with remapping and parameter file: .. code-block:: bash - ros2 run lbr_demos_fri_ros2_advanced_python admittance_control --ros-args \ + ros2 run lbr_demos_advanced_py admittance_control --ros-args \ -r __ns:=/lbr \ - --params-file `ros2 pkg prefix lbr_demos_fri_ros2_advanced_python`/share/lbr_demos_fri_ros2_advanced_python/config/admittance_control.yaml + --params-file `ros2 pkg prefix lbr_demos_advanced_py`/share/lbr_demos_advanced_py/config/admittance_control.yaml #. Now gently move the robot at the end-effector. @@ -52,12 +52,12 @@ This demo implements an admittance controller with a remote center of motion (RC ctrl:=forward_lbr_position_command_controller \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] -#. Run the `admittance_rcm_control `_ with remapping and parameter file: +#. Run the `admittance_rcm_control `_ with remapping and parameter file: .. code-block:: bash - ros2 run lbr_demos_fri_ros2_advanced_python admittance_rcm_control --ros-args \ + ros2 run lbr_demos_advanced_py admittance_rcm_control --ros-args \ -r __ns:=/lbr \ - --params-file `ros2 pkg prefix lbr_demos_fri_ros2_advanced_python`/share/lbr_demos_fri_ros2_advanced_python/config/admittance_rcm_control.yaml + --params-file `ros2 pkg prefix lbr_demos_advanced_py`/share/lbr_demos_advanced_py/config/admittance_rcm_control.yaml #. Now gently move the robot at the end-effector. diff --git a/lbr_demos/lbr_demos_admittance_control_python/lbr_demos_admittance_control_python/__init__.py b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/__init__.py similarity index 100% rename from lbr_demos/lbr_demos_admittance_control_python/lbr_demos_admittance_control_python/__init__.py rename to lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/__init__.py diff --git a/lbr_demos/lbr_demos_admittance_control_python/lbr_demos_admittance_control_python/admittance_control_node.py b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_control_node.py similarity index 100% rename from lbr_demos/lbr_demos_admittance_control_python/lbr_demos_admittance_control_python/admittance_control_node.py rename to lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_control_node.py diff --git a/lbr_demos/lbr_demos_admittance_control_python/lbr_demos_admittance_control_python/admittance_controller.py b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_controller.py similarity index 100% rename from lbr_demos/lbr_demos_admittance_control_python/lbr_demos_admittance_control_python/admittance_controller.py rename to lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_controller.py diff --git a/lbr_demos/lbr_demos_admittance_control_python/lbr_demos_admittance_control_python/admittance_rcm_control_node.py b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_control_node.py similarity index 100% rename from lbr_demos/lbr_demos_admittance_control_python/lbr_demos_admittance_control_python/admittance_rcm_control_node.py rename to lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_control_node.py diff --git a/lbr_demos/lbr_demos_admittance_control_python/lbr_demos_admittance_control_python/admittance_rcm_controller.py b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_controller.py similarity index 100% rename from lbr_demos/lbr_demos_admittance_control_python/lbr_demos_admittance_control_python/admittance_rcm_controller.py rename to lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_controller.py diff --git a/lbr_demos/lbr_demos_admittance_control_python/lbr_demos_admittance_control_python/lbr_base_position_command_node.py b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/lbr_base_position_command_node.py similarity index 100% rename from lbr_demos/lbr_demos_admittance_control_python/lbr_demos_admittance_control_python/lbr_base_position_command_node.py rename to lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/lbr_base_position_command_node.py diff --git a/lbr_demos/lbr_demos_admittance_control_python/package.xml b/lbr_demos/lbr_demos_advanced_py/package.xml similarity index 94% rename from lbr_demos/lbr_demos_admittance_control_python/package.xml rename to lbr_demos/lbr_demos_advanced_py/package.xml index 58d8d1ec..b6001330 100644 --- a/lbr_demos/lbr_demos_admittance_control_python/package.xml +++ b/lbr_demos/lbr_demos_advanced_py/package.xml @@ -1,7 +1,7 @@ - lbr_demos_fri_ros2_advanced_python + lbr_demos_advanced_py 1.4.3 Advanced Python demos for the lbr_fri_ros2. mhubii diff --git a/lbr_demos/lbr_demos_admittance_control_python/resource/lbr_demos_fri_ros2_advanced_python b/lbr_demos/lbr_demos_advanced_py/resource/lbr_demos_advanced_py similarity index 100% rename from lbr_demos/lbr_demos_admittance_control_python/resource/lbr_demos_fri_ros2_advanced_python rename to lbr_demos/lbr_demos_advanced_py/resource/lbr_demos_advanced_py diff --git a/lbr_demos/lbr_demos_advanced_py/setup.cfg b/lbr_demos/lbr_demos_advanced_py/setup.cfg new file mode 100644 index 00000000..6dc56b66 --- /dev/null +++ b/lbr_demos/lbr_demos_advanced_py/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/lbr_demos_advanced_py +[install] +install_scripts=$base/lib/lbr_demos_advanced_py diff --git a/lbr_demos/lbr_demos_admittance_control_python/setup.py b/lbr_demos/lbr_demos_advanced_py/setup.py similarity index 74% rename from lbr_demos/lbr_demos_admittance_control_python/setup.py rename to lbr_demos/lbr_demos_advanced_py/setup.py index 23f340ff..73b1bbab 100644 --- a/lbr_demos/lbr_demos_admittance_control_python/setup.py +++ b/lbr_demos/lbr_demos_advanced_py/setup.py @@ -2,7 +2,7 @@ from setuptools import setup -package_name = "lbr_demos_fri_ros2_advanced_python" +package_name = "lbr_demos_advanced_py" setup( name=package_name, @@ -23,8 +23,8 @@ tests_require=["pytest"], entry_points={ "console_scripts": [ - "admittance_control = lbr_demos_fri_ros2_advanced_python.admittance_control_node:main", - "admittance_rcm_control = lbr_demos_fri_ros2_advanced_python.admittance_rcm_control_node:main", + "admittance_control = lbr_demos_advanced_py.admittance_control_node:main", + "admittance_rcm_control = lbr_demos_advanced_py.admittance_rcm_control_node:main", ], }, ) diff --git a/lbr_demos/lbr_demos_admittance_control_python/test/test_copyright.py b/lbr_demos/lbr_demos_advanced_py/test/test_copyright.py similarity index 100% rename from lbr_demos/lbr_demos_admittance_control_python/test/test_copyright.py rename to lbr_demos/lbr_demos_advanced_py/test/test_copyright.py diff --git a/lbr_demos/lbr_demos_admittance_control_python/test/test_flake8.py b/lbr_demos/lbr_demos_advanced_py/test/test_flake8.py similarity index 100% rename from lbr_demos/lbr_demos_admittance_control_python/test/test_flake8.py rename to lbr_demos/lbr_demos_advanced_py/test/test_flake8.py diff --git a/lbr_demos/lbr_demos_admittance_control_python/test/test_pep257.py b/lbr_demos/lbr_demos_advanced_py/test/test_pep257.py similarity index 100% rename from lbr_demos/lbr_demos_admittance_control_python/test/test_pep257.py rename to lbr_demos/lbr_demos_advanced_py/test/test_pep257.py diff --git a/lbr_demos/lbr_demos_forward_command_controllers_cpp/CMakeLists.txt b/lbr_demos/lbr_demos_cpp/CMakeLists.txt similarity index 73% rename from lbr_demos/lbr_demos_forward_command_controllers_cpp/CMakeLists.txt rename to lbr_demos/lbr_demos_cpp/CMakeLists.txt index 1a774411..c498d292 100644 --- a/lbr_demos/lbr_demos_forward_command_controllers_cpp/CMakeLists.txt +++ b/lbr_demos/lbr_demos_cpp/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.22) -project(lbr_demos_fri_ros2_cpp) +project(lbr_demos_cpp) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -11,10 +11,13 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) +find_package(control_msgs REQUIRED) find_package(fri_vendor REQUIRED) find_package(FRIClient REQUIRED) find_package(lbr_fri_msgs REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(trajectory_msgs REQUIRED) # joint sine overlay add_executable(joint_sine_overlay_node @@ -30,7 +33,24 @@ ament_target_dependencies(joint_sine_overlay_node target_link_libraries(joint_sine_overlay_node FRIClient::FRIClient ) - + +# joint trajectory executioner +add_executable(joint_trajectory_executioner_node + src/joint_trajectory_executioner_node.cpp +) + +ament_target_dependencies(joint_trajectory_executioner_node + control_msgs + fri_vendor + rclcpp + rclcpp_action + trajectory_msgs +) + +target_link_libraries(joint_trajectory_executioner_node + FRIClient::FRIClient +) + # torque sine overlay add_executable(torque_sine_overlay_node src/torque_sine_overlay_node.cpp @@ -63,6 +83,7 @@ target_link_libraries(wrench_sine_overlay_node install(TARGETS joint_sine_overlay_node + joint_trajectory_executioner_node torque_sine_overlay_node wrench_sine_overlay_node DESTINATION lib/${PROJECT_NAME} diff --git a/lbr_demos/lbr_demos_forward_command_controllers_cpp/doc/lbr_demos_fri_ros2_cpp.rst b/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst similarity index 64% rename from lbr_demos/lbr_demos_forward_command_controllers_cpp/doc/lbr_demos_fri_ros2_cpp.rst rename to lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst index 195fee73..85d24b4e 100644 --- a/lbr_demos/lbr_demos_forward_command_controllers_cpp/doc/lbr_demos_fri_ros2_cpp.rst +++ b/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst @@ -14,13 +14,13 @@ Joint Sine Overlay .. thumbnail:: ../../doc/img/applications_joint_sine_overlay.png -#. Launch the `joint_sine_overlay.launch.py `_ launch file: +#. Launch the `joint_sine_overlay.launch.py `_ launch file: .. code-block:: bash - ros2 launch lbr_demos_fri_ros2_cpp joint_sine_overlay.launch.py model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + ros2 launch lbr_demos_cpp joint_sine_overlay.launch.py model:=iiwa7 # [iiwa7, iiwa14, med7, med14] -The robot will move to the initial position via position control, then execute a rotation on joint ``A1``. A sinusoidal motion is overlayed on joint ``A4`` via `joint_sine_overlay_node `_. +The robot will move to the initial position via position control, then execute a rotation on joint ``A1``. A sinusoidal motion is overlayed on joint ``A4`` via `joint_sine_overlay_node `_. Torque Sine Overlay ------------------- @@ -28,13 +28,13 @@ Torque Sine Overlay .. thumbnail:: ../../doc/img/applications_torque_sine_overlay.png -#. Launch the `torque_sine_overlay.launch.py `_ launch file: +#. Launch the `torque_sine_overlay.launch.py `_ launch file: .. code-block:: bash - ros2 launch lbr_demos_fri_ros2_cpp torque_sine_overlay.launch.py model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + ros2 launch lbr_demos_cpp torque_sine_overlay.launch.py model:=iiwa7 # [iiwa7, iiwa14, med7, med14] -The robot will move to the initial position via joint impedance control. A sinusoidal torque is overlayed on joint ``A4`` via `torque_sine_overlay_node `_. +The robot will move to the initial position via joint impedance control. A sinusoidal torque is overlayed on joint ``A4`` via `torque_sine_overlay_node `_. Wrench Sine Overlay ------------------- @@ -42,10 +42,10 @@ Wrench Sine Overlay .. thumbnail:: ../../doc/img/applications_wrench_sine_overlay.png -#. Launch the `wrench_sine_overlay.launch.py `_ launch file: +#. Launch the `wrench_sine_overlay.launch.py `_ launch file: .. code-block:: bash - ros2 launch lbr_demos_fri_ros2_cpp wrench_sine_overlay.launch.py model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + ros2 launch lbr_demos_cpp wrench_sine_overlay.launch.py model:=iiwa7 # [iiwa7, iiwa14, med7, med14] -The robot will move to the initial position via cartesian impedance control. A sinusoidal force is overlayed on the x- and y-axis via `wrench_sine_overlay_node `_. +The robot will move to the initial position via cartesian impedance control. A sinusoidal force is overlayed on the x- and y-axis via `wrench_sine_overlay_node `_. diff --git a/lbr_demos/lbr_demos_joint_trajectory_controller_cpp/doc/lbr_demos_ros2_control_cpp.rst b/lbr_demos/lbr_demos_cpp/doc/lbr_demos_ros2_control_cpp.rst similarity index 100% rename from lbr_demos/lbr_demos_joint_trajectory_controller_cpp/doc/lbr_demos_ros2_control_cpp.rst rename to lbr_demos/lbr_demos_cpp/doc/lbr_demos_ros2_control_cpp.rst diff --git a/lbr_demos/lbr_demos_forward_command_controllers_cpp/launch/joint_sine_overlay.launch.py b/lbr_demos/lbr_demos_cpp/launch/joint_sine_overlay.launch.py similarity index 94% rename from lbr_demos/lbr_demos_forward_command_controllers_cpp/launch/joint_sine_overlay.launch.py rename to lbr_demos/lbr_demos_cpp/launch/joint_sine_overlay.launch.py index 321d3e3f..d46fa2fb 100644 --- a/lbr_demos/lbr_demos_forward_command_controllers_cpp/launch/joint_sine_overlay.launch.py +++ b/lbr_demos/lbr_demos_cpp/launch/joint_sine_overlay.launch.py @@ -19,7 +19,7 @@ def generate_launch_description() -> LaunchDescription: ) ld.add_action( Node( - package="lbr_demos_fri_ros2_cpp", + package="lbr_demos_cpp", executable="joint_sine_overlay_node", output="screen", ) diff --git a/lbr_demos/lbr_demos_forward_command_controllers_cpp/launch/torque_sine_overlay.launch.py b/lbr_demos/lbr_demos_cpp/launch/torque_sine_overlay.launch.py similarity index 94% rename from lbr_demos/lbr_demos_forward_command_controllers_cpp/launch/torque_sine_overlay.launch.py rename to lbr_demos/lbr_demos_cpp/launch/torque_sine_overlay.launch.py index ace9a08d..7c936c90 100644 --- a/lbr_demos/lbr_demos_forward_command_controllers_cpp/launch/torque_sine_overlay.launch.py +++ b/lbr_demos/lbr_demos_cpp/launch/torque_sine_overlay.launch.py @@ -19,7 +19,7 @@ def generate_launch_description() -> LaunchDescription: ) ld.add_action( Node( - package="lbr_demos_fri_ros2_cpp", + package="lbr_demos_cpp", executable="torque_sine_overlay_node", output="screen", ) diff --git a/lbr_demos/lbr_demos_forward_command_controllers_cpp/launch/wrench_sine_overlay.launch.py b/lbr_demos/lbr_demos_cpp/launch/wrench_sine_overlay.launch.py similarity index 94% rename from lbr_demos/lbr_demos_forward_command_controllers_cpp/launch/wrench_sine_overlay.launch.py rename to lbr_demos/lbr_demos_cpp/launch/wrench_sine_overlay.launch.py index 1b6e210a..f784a21b 100644 --- a/lbr_demos/lbr_demos_forward_command_controllers_cpp/launch/wrench_sine_overlay.launch.py +++ b/lbr_demos/lbr_demos_cpp/launch/wrench_sine_overlay.launch.py @@ -19,7 +19,7 @@ def generate_launch_description() -> LaunchDescription: ) ld.add_action( Node( - package="lbr_demos_fri_ros2_cpp", + package="lbr_demos_cpp", executable="wrench_sine_overlay_node", output="screen", ) diff --git a/lbr_demos/lbr_demos_joint_trajectory_controller_cpp/package.xml b/lbr_demos/lbr_demos_cpp/package.xml similarity index 83% rename from lbr_demos/lbr_demos_joint_trajectory_controller_cpp/package.xml rename to lbr_demos/lbr_demos_cpp/package.xml index 79045ef4..eb81534e 100644 --- a/lbr_demos/lbr_demos_joint_trajectory_controller_cpp/package.xml +++ b/lbr_demos/lbr_demos_cpp/package.xml @@ -1,9 +1,9 @@ - lbr_demos_ros2_control_cpp + lbr_demos_cpp 1.4.3 - C++ demos for the LBR ros2_control integration. + C++ demos for the lbr_fri_ros2. mhubii Apache License 2.0 @@ -11,13 +11,13 @@ control_msgs fri_vendor + lbr_fri_msgs rclcpp rclcpp_action trajectory_msgs lbr_bringup lbr_description - lbr_fri_ros2 lbr_ros2_control ament_lint_auto diff --git a/lbr_demos/lbr_demos_forward_command_controllers_cpp/src/joint_sine_overlay_node.cpp b/lbr_demos/lbr_demos_cpp/src/joint_sine_overlay_node.cpp similarity index 100% rename from lbr_demos/lbr_demos_forward_command_controllers_cpp/src/joint_sine_overlay_node.cpp rename to lbr_demos/lbr_demos_cpp/src/joint_sine_overlay_node.cpp diff --git a/lbr_demos/lbr_demos_joint_trajectory_controller_cpp/src/joint_trajectory_executioner_node.cpp b/lbr_demos/lbr_demos_cpp/src/joint_trajectory_executioner_node.cpp similarity index 100% rename from lbr_demos/lbr_demos_joint_trajectory_controller_cpp/src/joint_trajectory_executioner_node.cpp rename to lbr_demos/lbr_demos_cpp/src/joint_trajectory_executioner_node.cpp diff --git a/lbr_demos/lbr_demos_forward_command_controllers_cpp/src/torque_sine_overlay_node.cpp b/lbr_demos/lbr_demos_cpp/src/torque_sine_overlay_node.cpp similarity index 100% rename from lbr_demos/lbr_demos_forward_command_controllers_cpp/src/torque_sine_overlay_node.cpp rename to lbr_demos/lbr_demos_cpp/src/torque_sine_overlay_node.cpp diff --git a/lbr_demos/lbr_demos_forward_command_controllers_cpp/src/wrench_sine_overlay_node.cpp b/lbr_demos/lbr_demos_cpp/src/wrench_sine_overlay_node.cpp similarity index 100% rename from lbr_demos/lbr_demos_forward_command_controllers_cpp/src/wrench_sine_overlay_node.cpp rename to lbr_demos/lbr_demos_cpp/src/wrench_sine_overlay_node.cpp diff --git a/lbr_demos/lbr_demos_forward_command_controllers_cpp/package.xml b/lbr_demos/lbr_demos_forward_command_controllers_cpp/package.xml deleted file mode 100644 index 3954d49f..00000000 --- a/lbr_demos/lbr_demos_forward_command_controllers_cpp/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - lbr_demos_fri_ros2_cpp - 1.4.3 - C++ demos for the lbr_fri_ros2. - mhubii - Apache License 2.0 - - ament_cmake - - fri_vendor - lbr_fri_msgs - rclcpp - - lbr_fri_ros2 - - ament_lint_auto - ament_lint_common - - - ament_cmake - - \ No newline at end of file diff --git a/lbr_demos/lbr_demos_forward_command_controllers_python/package.xml b/lbr_demos/lbr_demos_forward_command_controllers_python/package.xml deleted file mode 100644 index 93823ee2..00000000 --- a/lbr_demos/lbr_demos_forward_command_controllers_python/package.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - - lbr_demos_fri_ros2_python - 1.4.3 - Python demos for the lbr_fri_ros2. - mhubii - Apache License 2.0 - - lbr_fri_msgs - lbr_fri_ros2 - rclpy - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - \ No newline at end of file diff --git a/lbr_demos/lbr_demos_forward_command_controllers_python/setup.cfg b/lbr_demos/lbr_demos_forward_command_controllers_python/setup.cfg deleted file mode 100644 index 54a91b6a..00000000 --- a/lbr_demos/lbr_demos_forward_command_controllers_python/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/lbr_demos_fri_ros2_python -[install] -install_scripts=$base/lib/lbr_demos_fri_ros2_python diff --git a/lbr_demos/lbr_demos_joint_trajectory_controller_cpp/CMakeLists.txt b/lbr_demos/lbr_demos_joint_trajectory_controller_cpp/CMakeLists.txt deleted file mode 100644 index aea6ab31..00000000 --- a/lbr_demos/lbr_demos_joint_trajectory_controller_cpp/CMakeLists.txt +++ /dev/null @@ -1,42 +0,0 @@ -cmake_minimum_required(VERSION 3.22) -project(lbr_demos_ros2_control_cpp) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -if(NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release CACHE STRING "Build type" FORCE) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(control_msgs REQUIRED) -find_package(fri_vendor REQUIRED) -find_package(FRIClient REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(trajectory_msgs REQUIRED) - -add_executable(joint_trajectory_executioner_node - src/joint_trajectory_executioner_node.cpp -) - -ament_target_dependencies(joint_trajectory_executioner_node - control_msgs - fri_vendor - rclcpp - rclcpp_action - trajectory_msgs -) - -target_link_libraries(joint_trajectory_executioner_node - FRIClient::FRIClient -) - -install(TARGETS - joint_trajectory_executioner_node - DESTINATION lib/${PROJECT_NAME} -) - -ament_package() diff --git a/lbr_demos/lbr_demos_joint_trajectory_controller_python/lbr_demos_joint_trajectory_controller_python/__init__.py b/lbr_demos/lbr_demos_joint_trajectory_controller_python/lbr_demos_joint_trajectory_controller_python/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/lbr_demos/lbr_demos_joint_trajectory_controller_python/resource/lbr_demos_ros2_control_python b/lbr_demos/lbr_demos_joint_trajectory_controller_python/resource/lbr_demos_ros2_control_python deleted file mode 100644 index e69de29b..00000000 diff --git a/lbr_demos/lbr_demos_joint_trajectory_controller_python/setup.cfg b/lbr_demos/lbr_demos_joint_trajectory_controller_python/setup.cfg deleted file mode 100644 index 4777a801..00000000 --- a/lbr_demos/lbr_demos_joint_trajectory_controller_python/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/lbr_demos_ros2_control_python -[install] -install_scripts=$base/lib/lbr_demos_ros2_control_python diff --git a/lbr_demos/lbr_demos_joint_trajectory_controller_python/setup.py b/lbr_demos/lbr_demos_joint_trajectory_controller_python/setup.py deleted file mode 100644 index 1ffb6af7..00000000 --- a/lbr_demos/lbr_demos_joint_trajectory_controller_python/setup.py +++ /dev/null @@ -1,25 +0,0 @@ -from setuptools import setup - -package_name = "lbr_demos_ros2_control_python" - -setup( - name=package_name, - version="1.4.3", - packages=[package_name], - data_files=[ - ("share/ament_index/resource_index/packages", ["resource/" + package_name]), - ("share/" + package_name, ["package.xml"]), - ], - install_requires=["setuptools"], - zip_safe=True, - maintainer="mhubii", - maintainer_email="m.huber_1994@hotmail.de", - description="Python demos for the LBR ros2_control integration.", - license="MIT", - tests_require=["pytest"], - entry_points={ - "console_scripts": [ - "joint_trajectory_executioner_node = lbr_demos_ros2_control_python.joint_trajectory_executioner_node:main", - ], - }, -) diff --git a/lbr_demos/lbr_demos_joint_trajectory_controller_python/test/test_copyright.py b/lbr_demos/lbr_demos_joint_trajectory_controller_python/test/test_copyright.py deleted file mode 100644 index ceffe896..00000000 --- a/lbr_demos/lbr_demos_joint_trajectory_controller_python/test/test_copyright.py +++ /dev/null @@ -1,27 +0,0 @@ -# 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_demos_joint_trajectory_controller_python/test/test_flake8.py b/lbr_demos/lbr_demos_joint_trajectory_controller_python/test/test_flake8.py deleted file mode 100644 index ee79f31a..00000000 --- a/lbr_demos/lbr_demos_joint_trajectory_controller_python/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# 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_demos_joint_trajectory_controller_python/test/test_pep257.py b/lbr_demos/lbr_demos_joint_trajectory_controller_python/test/test_pep257.py deleted file mode 100644 index a2c3deb8..00000000 --- a/lbr_demos/lbr_demos_joint_trajectory_controller_python/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# 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_demos_forward_command_controllers_python/doc/lbr_demos_fri_ros2_python.rst b/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst similarity index 63% rename from lbr_demos/lbr_demos_forward_command_controllers_python/doc/lbr_demos_fri_ros2_python.rst rename to lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst index 690a9fbb..bf476820 100644 --- a/lbr_demos/lbr_demos_forward_command_controllers_python/doc/lbr_demos_fri_ros2_python.rst +++ b/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst @@ -14,13 +14,13 @@ Joint Sine Overlay .. thumbnail:: ../../doc/img/applications_joint_sine_overlay.png -#. Launch the `joint_sine_overlay.launch.py `_ launch file: +#. Launch the `joint_sine_overlay.launch.py `_ launch file: .. code-block:: bash - ros2 launch lbr_demos_fri_ros2_python joint_sine_overlay.launch.py model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + ros2 launch lbr_demos_py joint_sine_overlay.launch.py model:=iiwa7 # [iiwa7, iiwa14, med7, med14] -The robot will move to the initial position via position control, then execute a rotation on joint ``A1``. A sinusoidal motion is overlayed on joint ``A4`` via `joint_sine_overlay_node `_. +The robot will move to the initial position via position control, then execute a rotation on joint ``A1``. A sinusoidal motion is overlayed on joint ``A4`` via `joint_sine_overlay_node `_. Torque Sine Overlay ------------------- @@ -28,13 +28,13 @@ Torque Sine Overlay .. thumbnail:: ../../doc/img/applications_torque_sine_overlay.png -#. Launch the `torque_sine_overlay.launch.py `_ launch file: +#. Launch the `torque_sine_overlay.launch.py `_ launch file: .. code-block:: bash - ros2 launch lbr_demos_fri_ros2_python torque_sine_overlay.launch.py model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + ros2 launch lbr_demos_py torque_sine_overlay.launch.py model:=iiwa7 # [iiwa7, iiwa14, med7, med14] -The robot will move to the initial position via joint impedance control. A sinusoidal torque is overlayed on joint ``A4`` via `torque_sine_overlay_node `_. +The robot will move to the initial position via joint impedance control. A sinusoidal torque is overlayed on joint ``A4`` via `torque_sine_overlay_node `_. Wrench Sine Overlay ------------------- @@ -42,10 +42,10 @@ Wrench Sine Overlay .. thumbnail:: ../../doc/img/applications_wrench_sine_overlay.png -#. Launch the `wrench_sine_overlay.launch.py `_ launch file: +#. Launch the `wrench_sine_overlay.launch.py `_ launch file: .. code-block:: bash - ros2 launch lbr_demos_fri_ros2_python wrench_sine_overlay.launch.py model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + ros2 launch lbr_demos_py wrench_sine_overlay.launch.py model:=iiwa7 # [iiwa7, iiwa14, med7, med14] -The robot will move to the initial position via cartesian impedance control. A sinusoidal force is overlayed on the x- and y-axis via `wrench_sine_overlay_node `_. +The robot will move to the initial position via cartesian impedance control. A sinusoidal force is overlayed on the x- and y-axis via `wrench_sine_overlay_node `_. diff --git a/lbr_demos/lbr_demos_joint_trajectory_controller_python/doc/lbr_demos_ros2_control_python.rst b/lbr_demos/lbr_demos_py/doc/lbr_demos_ros2_control_python.rst similarity index 100% rename from lbr_demos/lbr_demos_joint_trajectory_controller_python/doc/lbr_demos_ros2_control_python.rst rename to lbr_demos/lbr_demos_py/doc/lbr_demos_ros2_control_python.rst diff --git a/lbr_demos/lbr_demos_forward_command_controllers_python/launch/joint_sine_overlay.launch.py b/lbr_demos/lbr_demos_py/launch/joint_sine_overlay.launch.py similarity index 93% rename from lbr_demos/lbr_demos_forward_command_controllers_python/launch/joint_sine_overlay.launch.py rename to lbr_demos/lbr_demos_py/launch/joint_sine_overlay.launch.py index 0eee126d..3ef3fad6 100644 --- a/lbr_demos/lbr_demos_forward_command_controllers_python/launch/joint_sine_overlay.launch.py +++ b/lbr_demos/lbr_demos_py/launch/joint_sine_overlay.launch.py @@ -19,7 +19,7 @@ def generate_launch_description() -> LaunchDescription: ) ld.add_action( Node( - package="lbr_demos_fri_ros2_python", + package="lbr_demos_py", executable="joint_sine_overlay_node", output="screen", ) diff --git a/lbr_demos/lbr_demos_forward_command_controllers_python/launch/torque_sine_overlay.launch.py b/lbr_demos/lbr_demos_py/launch/torque_sine_overlay.launch.py similarity index 93% rename from lbr_demos/lbr_demos_forward_command_controllers_python/launch/torque_sine_overlay.launch.py rename to lbr_demos/lbr_demos_py/launch/torque_sine_overlay.launch.py index c4769a14..f2912370 100644 --- a/lbr_demos/lbr_demos_forward_command_controllers_python/launch/torque_sine_overlay.launch.py +++ b/lbr_demos/lbr_demos_py/launch/torque_sine_overlay.launch.py @@ -19,7 +19,7 @@ def generate_launch_description() -> LaunchDescription: ) ld.add_action( Node( - package="lbr_demos_fri_ros2_python", + package="lbr_demos_py", executable="torque_sine_overlay_node", output="screen", ) diff --git a/lbr_demos/lbr_demos_forward_command_controllers_python/launch/wrench_sine_overlay.launch.py b/lbr_demos/lbr_demos_py/launch/wrench_sine_overlay.launch.py similarity index 93% rename from lbr_demos/lbr_demos_forward_command_controllers_python/launch/wrench_sine_overlay.launch.py rename to lbr_demos/lbr_demos_py/launch/wrench_sine_overlay.launch.py index c6a3e565..f74efbe4 100644 --- a/lbr_demos/lbr_demos_forward_command_controllers_python/launch/wrench_sine_overlay.launch.py +++ b/lbr_demos/lbr_demos_py/launch/wrench_sine_overlay.launch.py @@ -19,7 +19,7 @@ def generate_launch_description() -> LaunchDescription: ) ld.add_action( Node( - package="lbr_demos_fri_ros2_python", + package="lbr_demos_py", executable="wrench_sine_overlay_node", output="screen", ) diff --git a/lbr_demos/lbr_demos_forward_command_controllers_python/lbr_demos_forward_command_controllers_python/__init__.py b/lbr_demos/lbr_demos_py/lbr_demos_py/__init__.py similarity index 100% rename from lbr_demos/lbr_demos_forward_command_controllers_python/lbr_demos_forward_command_controllers_python/__init__.py rename to lbr_demos/lbr_demos_py/lbr_demos_py/__init__.py diff --git a/lbr_demos/lbr_demos_forward_command_controllers_python/lbr_demos_forward_command_controllers_python/joint_sine_overlay_node.py b/lbr_demos/lbr_demos_py/lbr_demos_py/joint_sine_overlay_node.py similarity index 100% rename from lbr_demos/lbr_demos_forward_command_controllers_python/lbr_demos_forward_command_controllers_python/joint_sine_overlay_node.py rename to lbr_demos/lbr_demos_py/lbr_demos_py/joint_sine_overlay_node.py diff --git a/lbr_demos/lbr_demos_joint_trajectory_controller_python/lbr_demos_joint_trajectory_controller_python/joint_trajectory_executioner_node.py b/lbr_demos/lbr_demos_py/lbr_demos_py/joint_trajectory_executioner_node.py similarity index 100% rename from lbr_demos/lbr_demos_joint_trajectory_controller_python/lbr_demos_joint_trajectory_controller_python/joint_trajectory_executioner_node.py rename to lbr_demos/lbr_demos_py/lbr_demos_py/joint_trajectory_executioner_node.py diff --git a/lbr_demos/lbr_demos_forward_command_controllers_python/lbr_demos_forward_command_controllers_python/torque_sine_overlay_node.py b/lbr_demos/lbr_demos_py/lbr_demos_py/torque_sine_overlay_node.py similarity index 100% rename from lbr_demos/lbr_demos_forward_command_controllers_python/lbr_demos_forward_command_controllers_python/torque_sine_overlay_node.py rename to lbr_demos/lbr_demos_py/lbr_demos_py/torque_sine_overlay_node.py diff --git a/lbr_demos/lbr_demos_forward_command_controllers_python/lbr_demos_forward_command_controllers_python/wrench_sine_overlay_node.py b/lbr_demos/lbr_demos_py/lbr_demos_py/wrench_sine_overlay_node.py similarity index 100% rename from lbr_demos/lbr_demos_forward_command_controllers_python/lbr_demos_forward_command_controllers_python/wrench_sine_overlay_node.py rename to lbr_demos/lbr_demos_py/lbr_demos_py/wrench_sine_overlay_node.py diff --git a/lbr_demos/lbr_demos_joint_trajectory_controller_python/package.xml b/lbr_demos/lbr_demos_py/package.xml similarity index 82% rename from lbr_demos/lbr_demos_joint_trajectory_controller_python/package.xml rename to lbr_demos/lbr_demos_py/package.xml index 9d4d6725..34f54070 100644 --- a/lbr_demos/lbr_demos_joint_trajectory_controller_python/package.xml +++ b/lbr_demos/lbr_demos_py/package.xml @@ -1,16 +1,16 @@ - lbr_demos_ros2_control_python + lbr_demos_py 1.4.3 - Python demos for the LBR ros2_control integration. + Python demos for the lbr_fri_ros2. mhubii Apache License 2.0 control_msgs lbr_bringup lbr_description - lbr_fri_ros2 + lbr_fri_msgs lbr_ros2_control rclpy trajectory_msgs diff --git a/lbr_demos/lbr_demos_forward_command_controllers_python/resource/lbr_demos_fri_ros2_python b/lbr_demos/lbr_demos_py/resource/lbr_demos_py similarity index 100% rename from lbr_demos/lbr_demos_forward_command_controllers_python/resource/lbr_demos_fri_ros2_python rename to lbr_demos/lbr_demos_py/resource/lbr_demos_py diff --git a/lbr_demos/lbr_demos_py/setup.cfg b/lbr_demos/lbr_demos_py/setup.cfg new file mode 100644 index 00000000..2056f2c7 --- /dev/null +++ b/lbr_demos/lbr_demos_py/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/lbr_demos_py +[install] +install_scripts=$base/lib/lbr_demos_py diff --git a/lbr_demos/lbr_demos_forward_command_controllers_python/setup.py b/lbr_demos/lbr_demos_py/setup.py similarity index 62% rename from lbr_demos/lbr_demos_forward_command_controllers_python/setup.py rename to lbr_demos/lbr_demos_py/setup.py index ce3dedf8..5521ab8d 100644 --- a/lbr_demos/lbr_demos_forward_command_controllers_python/setup.py +++ b/lbr_demos/lbr_demos_py/setup.py @@ -2,7 +2,7 @@ from setuptools import setup -package_name = "lbr_demos_fri_ros2_python" +package_name = "lbr_demos_py" setup( name=package_name, @@ -22,9 +22,10 @@ tests_require=["pytest"], entry_points={ "console_scripts": [ - "joint_sine_overlay_node = lbr_demos_fri_ros2_python.joint_sine_overlay_node:main", - "torque_sine_overlay_node = lbr_demos_fri_ros2_python.torque_sine_overlay_node:main", - "wrench_sine_overlay_node = lbr_demos_fri_ros2_python.wrench_sine_overlay_node:main", + "joint_sine_overlay_node = lbr_demos_py.joint_sine_overlay_node:main", + "joint_trajectory_executioner_node = lbr_demos_ros2_control_python.joint_trajectory_executioner_node:main", + "torque_sine_overlay_node = lbr_demos_py.torque_sine_overlay_node:main", + "wrench_sine_overlay_node = lbr_demos_py.wrench_sine_overlay_node:main", ], }, ) diff --git a/lbr_demos/lbr_demos_forward_command_controllers_python/test/test_copyright.py b/lbr_demos/lbr_demos_py/test/test_copyright.py similarity index 100% rename from lbr_demos/lbr_demos_forward_command_controllers_python/test/test_copyright.py rename to lbr_demos/lbr_demos_py/test/test_copyright.py diff --git a/lbr_demos/lbr_demos_forward_command_controllers_python/test/test_flake8.py b/lbr_demos/lbr_demos_py/test/test_flake8.py similarity index 100% rename from lbr_demos/lbr_demos_forward_command_controllers_python/test/test_flake8.py rename to lbr_demos/lbr_demos_py/test/test_flake8.py diff --git a/lbr_demos/lbr_demos_forward_command_controllers_python/test/test_pep257.py b/lbr_demos/lbr_demos_py/test/test_pep257.py similarity index 100% rename from lbr_demos/lbr_demos_forward_command_controllers_python/test/test_pep257.py rename to lbr_demos/lbr_demos_py/test/test_pep257.py From 1a4cb51a4b0712dcace14e4315d650b771a717d0 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 8 May 2024 18:43:35 +0100 Subject: [PATCH 36/76] cartesian impedance control draft --- lbr_demos/doc/lbr_demos.rst | 39 ++---- .../doc/lbr_demos_advanced_cpp.rst | 8 +- lbr_demos/lbr_demos_advanced_cpp/package.xml | 2 +- .../src/lbr_base_position_command_node.hpp | 6 +- .../doc/lbr_demos_advanced_py.rst | 8 +- lbr_demos/lbr_demos_advanced_py/package.xml | 2 +- lbr_demos/lbr_demos_advanced_py/setup.py | 3 +- lbr_demos/lbr_demos_cpp/CMakeLists.txt | 49 ++++---- lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst | 92 +++++++++++--- .../doc/lbr_demos_ros2_control_cpp.rst | 41 ------- .../launch/joint_sine_overlay.launch.py | 27 ---- .../launch/torque_sine_overlay.launch.py | 27 ---- .../launch/wrench_sine_overlay.launch.py | 27 ---- lbr_demos/lbr_demos_cpp/package.xml | 3 +- ...verlay_node.cpp => joint_sine_overlay.cpp} | 40 +++--- ...r_node.cpp => joint_trajectory_client.cpp} | 17 ++- ...erlay_node.cpp => torque_sine_overlay.cpp} | 36 +++--- ...erlay_node.cpp => wrench_sine_overlay.cpp} | 39 +++--- lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst | 46 ++++++- .../doc/lbr_demos_ros2_control_python.rst | 41 ------- .../launch/joint_sine_overlay.launch.py | 27 ---- .../launch/torque_sine_overlay.launch.py | 27 ---- .../launch/wrench_sine_overlay.launch.py | 27 ---- ..._overlay_node.py => joint_sine_overlay.py} | 10 +- ...ner_node.py => joint_trajectory_client.py} | 0 ...overlay_node.py => torque_sine_overlay.py} | 10 +- ...overlay_node.py => wrench_sine_overlay.py} | 10 +- lbr_demos/lbr_demos_py/package.xml | 2 +- lbr_demos/lbr_demos_py/setup.py | 13 +- lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp | 32 +++++ lbr_ros2_control/CMakeLists.txt | 5 +- lbr_ros2_control/config/lbr_controllers.yaml | 11 +- .../config/lbr_system_interface.xacro | 14 ++- ...pp => lbr_position_command_controller.hpp} | 10 +- ....hpp => lbr_torque_command_controller.hpp} | 10 +- .../lbr_wrench_command_controller.hpp | 63 ++++++++++ .../lbr_ros2_control/system_interface.hpp | 2 + .../lbr_ros2_control/launch_mixin.py | 5 +- .../forward_command_controllers.xml | 16 ++- ...pp => lbr_position_command_controller.cpp} | 31 +++-- ....cpp => lbr_torque_command_controller.cpp} | 40 +++--- .../src/lbr_wrench_command_controller.cpp | 116 ++++++++++++++++++ lbr_ros2_control/src/system_interface.cpp | 50 +++++++- 43 files changed, 611 insertions(+), 473 deletions(-) delete mode 100644 lbr_demos/lbr_demos_cpp/doc/lbr_demos_ros2_control_cpp.rst delete mode 100644 lbr_demos/lbr_demos_cpp/launch/joint_sine_overlay.launch.py delete mode 100644 lbr_demos/lbr_demos_cpp/launch/torque_sine_overlay.launch.py delete mode 100644 lbr_demos/lbr_demos_cpp/launch/wrench_sine_overlay.launch.py rename lbr_demos/lbr_demos_cpp/src/{joint_sine_overlay_node.cpp => joint_sine_overlay.cpp} (58%) rename lbr_demos/lbr_demos_cpp/src/{joint_trajectory_executioner_node.cpp => joint_trajectory_client.cpp} (82%) rename lbr_demos/lbr_demos_cpp/src/{torque_sine_overlay_node.cpp => torque_sine_overlay.cpp} (57%) rename lbr_demos/lbr_demos_cpp/src/{wrench_sine_overlay_node.cpp => wrench_sine_overlay.cpp} (59%) delete mode 100644 lbr_demos/lbr_demos_py/doc/lbr_demos_ros2_control_python.rst delete mode 100644 lbr_demos/lbr_demos_py/launch/joint_sine_overlay.launch.py delete mode 100644 lbr_demos/lbr_demos_py/launch/torque_sine_overlay.launch.py delete mode 100644 lbr_demos/lbr_demos_py/launch/wrench_sine_overlay.launch.py rename lbr_demos/lbr_demos_py/lbr_demos_py/{joint_sine_overlay_node.py => joint_sine_overlay.py} (82%) rename lbr_demos/lbr_demos_py/lbr_demos_py/{joint_trajectory_executioner_node.py => joint_trajectory_client.py} (100%) rename lbr_demos/lbr_demos_py/lbr_demos_py/{torque_sine_overlay_node.py => torque_sine_overlay.py} (80%) rename lbr_demos/lbr_demos_py/lbr_demos_py/{wrench_sine_overlay_node.py => wrench_sine_overlay.py} (84%) create mode 100644 lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp rename lbr_ros2_control/include/lbr_ros2_control/{forward_lbr_position_command_controller.hpp => lbr_position_command_controller.hpp} (82%) rename lbr_ros2_control/include/lbr_ros2_control/{forward_lbr_torque_command_controller.hpp => lbr_torque_command_controller.hpp} (85%) create mode 100644 lbr_ros2_control/include/lbr_ros2_control/lbr_wrench_command_controller.hpp rename lbr_ros2_control/src/{forward_lbr_position_command_controller.cpp => lbr_position_command_controller.cpp} (64%) rename lbr_ros2_control/src/{forward_lbr_torque_command_controller.cpp => lbr_torque_command_controller.cpp} (70%) create mode 100644 lbr_ros2_control/src/lbr_wrench_command_controller.cpp diff --git a/lbr_demos/doc/lbr_demos.rst b/lbr_demos/doc/lbr_demos.rst index dff60db6..042b4ac2 100644 --- a/lbr_demos/doc/lbr_demos.rst +++ b/lbr_demos/doc/lbr_demos.rst @@ -1,40 +1,25 @@ LBR Demos ========= -Demos for controlling the LBR through the Fast Robot Interface (FRI) from ROS 2. +Contolling the LBR through ROS 2 control. -.. warning:: - On the real robot, do always execute in ``T1`` mode first. - -TODO: Differentiate demos through controllers -TODO: indicate which demos run in simulation and which on the real robot -TODO: Introduce controller concept - - -LBR Demos FRI ROS 2 -------------------- .. note:: - These demos are compatible with and closely follow KUKA's FRI example applications. They send commands to ``/lbr/command`` and read states from ``/lbr/state``. + Some demos require a real robot. Make sure you followed :ref:`Robot Setup` first. -.. note:: - A real robot is required to run these demos. Make sure you followed :ref:`Robot Setup` first. +.. warning:: + On the real robot, do always execute in ``T1`` mode first. +Basic Demos +----------- .. toctree:: :titlesonly: - LBR Demos FRI ROS 2 C++ <../lbr_demos_cpp/doc/lbr_demos_cpp.rst> - LBR Demos FRI ROS 2 Python <../lbr_demos_py/doc/lbr_demos_py.rst> - LBR Demos FRI ROS 2 Advanced C++ <../lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst> - LBR Demos FRI ROS 2 Advanced Python <../lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst> - -LBR ROS 2 Control Demos ------------------------ -These demos demonstrate the LBR integration into the ROS 2 ecosystem through ``ros2_control``. - -.. note:: - These demos run in simulation **and** on the real robot. For the real robot, make sure you followed :ref:`Robot Setup` first. + LBR Demos C++ <../lbr_demos_cpp/doc/lbr_demos_cpp.rst> + LBR Demos Python <../lbr_demos_py/doc/lbr_demos_py.rst> +Advanced Demos +-------------- .. toctree:: :titlesonly: - LBR Demos ROS 2 Control C++ <../lbr_demos_ros2_control_cpp/doc/lbr_demos_ros2_control_cpp.rst> - LBR Demos ROS 2 Control Python <../lbr_demos_ros2_control_python/doc/lbr_demos_ros2_control_python.rst> + LBR Demos Advanced C++ <../lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst> + LBR Demos Advanced Python <../lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst> diff --git a/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst b/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst index 2b15e3a1..4f366a38 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst +++ b/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst @@ -1,5 +1,5 @@ -LBR Demos FRI ROS 2 Advanced C++ -================================ +LBR Demos Advanced C++ +====================== Collection of advanced usage examples for the ``lbr_fri_ros2`` package through C++. .. warning:: @@ -20,7 +20,7 @@ Admittance Controller ros2 launch lbr_bringup bringup.launch.py \ sim:=false \ - ctrl:=forward_lbr_position_command_controller \ + ctrl:=lbr_position_command_controller \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] #. Launch the `admittance_control `_: @@ -48,7 +48,7 @@ kinematics to move the robot's end-effector along the z-axis in Cartesian space. ros2 launch lbr_bringup bringup.launch.py \ sim:=false \ - ctrl:=forward_lbr_position_command_controller \ + ctrl:=lbr_position_command_controller \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] #. Launch the pose control diff --git a/lbr_demos/lbr_demos_advanced_cpp/package.xml b/lbr_demos/lbr_demos_advanced_cpp/package.xml index 6dad3a2e..ab16a313 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/package.xml +++ b/lbr_demos/lbr_demos_advanced_cpp/package.xml @@ -3,7 +3,7 @@ lbr_demos_advanced_cpp 1.4.3 - Advanced C++ demos for the lbr_fri_ros2. + Advanced C++ demos for the lbr_ros2_control. mhubii Apache License 2.0 diff --git a/lbr_demos/lbr_demos_advanced_cpp/src/lbr_base_position_command_node.hpp b/lbr_demos/lbr_demos_advanced_cpp/src/lbr_base_position_command_node.hpp index 49787090..28c80266 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/src/lbr_base_position_command_node.hpp +++ b/lbr_demos/lbr_demos_advanced_cpp/src/lbr_base_position_command_node.hpp @@ -42,8 +42,8 @@ class LBRBasePositionCommandNode : public rclcpp::Node { rclcpp::Parameter retrieve_parameter_(const std::string &remote_node_name, const std::string ¶meter_name) { - rclcpp::AsyncParametersClient robot_description_client(this, remote_node_name); - while (!robot_description_client.wait_for_service(std::chrono::seconds(1))) { + rclcpp::AsyncParametersClient parameter_client(this, remote_node_name); + while (!parameter_client.wait_for_service(std::chrono::seconds(1))) { if (!rclcpp::ok()) { std::string err = "Interrupted while waiting for the service. Exiting."; RCLCPP_ERROR(this->get_logger(), err.c_str()); @@ -51,7 +51,7 @@ class LBRBasePositionCommandNode : public rclcpp::Node { } RCLCPP_INFO(this->get_logger(), "Wating for '%s' service...", remote_node_name.c_str()); } - auto future = robot_description_client.get_parameters({parameter_name}); + auto future = parameter_client.get_parameters({parameter_name}); if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) != rclcpp::FutureReturnCode::SUCCESS) { std::string err = "Failed to retrieve '" + parameter_name + "'."; diff --git a/lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst b/lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst index 337fb120..5dfd7c09 100644 --- a/lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst +++ b/lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst @@ -1,5 +1,5 @@ -LBR Demos FRI ROS 2 Advanced Python -=================================== +LBR Demos Advanced Python +========================= Collection of advanced usage examples for the ``lbr_ros2_control`` package through Python. .. warning:: @@ -22,7 +22,7 @@ This demo implements a simple admittance controller. ros2 launch lbr_bringup bringup.launch.py \ sim:=false \ - ctrl:=forward_lbr_position_command_controller \ + ctrl:=lbr_position_command_controller \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] #. Run the `admittance_control `_ with remapping and parameter file: @@ -49,7 +49,7 @@ This demo implements an admittance controller with a remote center of motion (RC ros2 launch lbr_bringup bringup.launch.py \ sim:=false \ - ctrl:=forward_lbr_position_command_controller \ + ctrl:=lbr_position_command_controller \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] #. Run the `admittance_rcm_control `_ with remapping and parameter file: diff --git a/lbr_demos/lbr_demos_advanced_py/package.xml b/lbr_demos/lbr_demos_advanced_py/package.xml index b6001330..48644cb5 100644 --- a/lbr_demos/lbr_demos_advanced_py/package.xml +++ b/lbr_demos/lbr_demos_advanced_py/package.xml @@ -3,7 +3,7 @@ lbr_demos_advanced_py 1.4.3 - Advanced Python demos for the lbr_fri_ros2. + Advanced Python demos for the lbr_ros2_control. mhubii Apache License 2.0 diff --git a/lbr_demos/lbr_demos_advanced_py/setup.py b/lbr_demos/lbr_demos_advanced_py/setup.py index 73b1bbab..03ca687a 100644 --- a/lbr_demos/lbr_demos_advanced_py/setup.py +++ b/lbr_demos/lbr_demos_advanced_py/setup.py @@ -12,13 +12,12 @@ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), ("share/" + package_name, ["package.xml"]), ("share/" + package_name + "/config", glob.glob("config/*.yaml")), - ("share/" + package_name + "/launch", glob.glob("launch/*.py")), ], install_requires=["setuptools"], zip_safe=True, maintainer="mhubii", maintainer_email="m.huber_1994@hotmail.de", - description="Advanced Python demos for the lbr_fri_ros2.", + description="Advanced Python demos for the lbr_ros2_control.", license="MIT", tests_require=["pytest"], entry_points={ diff --git a/lbr_demos/lbr_demos_cpp/CMakeLists.txt b/lbr_demos/lbr_demos_cpp/CMakeLists.txt index c498d292..433c0675 100644 --- a/lbr_demos/lbr_demos_cpp/CMakeLists.txt +++ b/lbr_demos/lbr_demos_cpp/CMakeLists.txt @@ -15,31 +15,33 @@ find_package(control_msgs REQUIRED) find_package(fri_vendor REQUIRED) find_package(FRIClient REQUIRED) find_package(lbr_fri_msgs REQUIRED) +find_package(lbr_fri_ros2 REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(trajectory_msgs REQUIRED) # joint sine overlay -add_executable(joint_sine_overlay_node - src/joint_sine_overlay_node.cpp +add_executable(joint_sine_overlay + src/joint_sine_overlay.cpp ) -ament_target_dependencies(joint_sine_overlay_node +ament_target_dependencies(joint_sine_overlay fri_vendor lbr_fri_msgs + lbr_fri_ros2 rclcpp ) -target_link_libraries(joint_sine_overlay_node +target_link_libraries(joint_sine_overlay FRIClient::FRIClient ) # joint trajectory executioner -add_executable(joint_trajectory_executioner_node - src/joint_trajectory_executioner_node.cpp +add_executable(joint_trajectory_client + src/joint_trajectory_client.cpp ) -ament_target_dependencies(joint_trajectory_executioner_node +ament_target_dependencies(joint_trajectory_client control_msgs fri_vendor rclcpp @@ -47,51 +49,48 @@ ament_target_dependencies(joint_trajectory_executioner_node trajectory_msgs ) -target_link_libraries(joint_trajectory_executioner_node +target_link_libraries(joint_trajectory_client FRIClient::FRIClient ) # torque sine overlay -add_executable(torque_sine_overlay_node - src/torque_sine_overlay_node.cpp +add_executable(torque_sine_overlay + src/torque_sine_overlay.cpp ) -ament_target_dependencies(torque_sine_overlay_node +ament_target_dependencies(torque_sine_overlay fri_vendor lbr_fri_msgs + lbr_fri_ros2 rclcpp ) -target_link_libraries(torque_sine_overlay_node +target_link_libraries(torque_sine_overlay FRIClient::FRIClient ) # wrench sine overlay -add_executable(wrench_sine_overlay_node - src/wrench_sine_overlay_node.cpp +add_executable(wrench_sine_overlay + src/wrench_sine_overlay.cpp ) -ament_target_dependencies(wrench_sine_overlay_node +ament_target_dependencies(wrench_sine_overlay fri_vendor lbr_fri_msgs + lbr_fri_ros2 rclcpp ) -target_link_libraries(wrench_sine_overlay_node +target_link_libraries(wrench_sine_overlay FRIClient::FRIClient ) install(TARGETS - joint_sine_overlay_node - joint_trajectory_executioner_node - torque_sine_overlay_node - wrench_sine_overlay_node + joint_sine_overlay + joint_trajectory_client + torque_sine_overlay + wrench_sine_overlay DESTINATION lib/${PROJECT_NAME} ) -install(DIRECTORY - launch - DESTINATION share/${PROJECT_NAME} -) - ament_package() diff --git a/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst b/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst index 85d24b4e..2751aac5 100644 --- a/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst +++ b/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst @@ -1,9 +1,6 @@ -LBR Demos FRI ROS 2 C++ -======================= -Collection of basic usage examples for the ``lbr_fri_ros2`` package through C++. - -.. note:: - These demos are compatible with and closely follow KUKA's FRI example applications. +LBR Demos C++ +============= + add table of contents TODO with sim / real matrix .. warning:: Do always execute in ``T1`` mode first. @@ -14,13 +11,56 @@ Joint Sine Overlay .. thumbnail:: ../../doc/img/applications_joint_sine_overlay.png -#. Launch the `joint_sine_overlay.launch.py `_ launch file: +#. Run the robot driver: + +.. code-block:: bash + + ros2 launch lbr_bringup bringup.launch.py \ + ctrl:=lbr_position_command_controller \ + sim:=false \ + model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + +The robot will move to the initial position through the Java application. + +#. Run the `joint_sine_overlay `_ node: + +.. code-block:: bash + + ros2 run lbr_demos_cpp joint_sine_overlay --ros-args -r __ns:=/lbr + +This node overlays a sinusoidal motion on joint ``A4``. + +Joint Trajectory Controller +--------------------------- +Simulation +~~~~~~~~~~ +#. Launch the simulated robot: + +.. code-block:: bash + + ros2 launch lbr_bringup bringup.launch.py sim:=true model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + +#. Run the `joint_trajectory_client `_: .. code-block:: bash - ros2 launch lbr_demos_cpp joint_sine_overlay.launch.py model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + ros2 run lbr_demos_cpp joint_trajectory_client --ros-args -r __ns:=/lbr -The robot will move to the initial position via position control, then execute a rotation on joint ``A1``. A sinusoidal motion is overlayed on joint ``A4`` via `joint_sine_overlay_node `_. +The robot will twist, then move to the zero configuration. + +Real Robot +~~~~~~~~~~ +#. .. 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 ``sim:=false``. Torque Sine Overlay ------------------- @@ -28,13 +68,24 @@ Torque Sine Overlay .. thumbnail:: ../../doc/img/applications_torque_sine_overlay.png -#. Launch the `torque_sine_overlay.launch.py `_ launch file: +#. Run the robot driver: + +.. code-block:: bash + + ros2 launch lbr_bringup bringup.launch.py \ + ctrl:=lbr_torque_command_controller \ + sim:=false \ + model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + +The robot will move to the initial position through the Java application. + +#. Run the `torque_sine_overlay `_ node: .. code-block:: bash - ros2 launch lbr_demos_cpp torque_sine_overlay.launch.py model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + ros2 run lbr_demos_cpp torque_sine_overlay --ros-args -r __ns:=/lbr -The robot will move to the initial position via joint impedance control. A sinusoidal torque is overlayed on joint ``A4`` via `torque_sine_overlay_node `_. +This node overlays a sinusoidal torque on joint ``A4``. Wrench Sine Overlay ------------------- @@ -42,10 +93,21 @@ Wrench Sine Overlay .. thumbnail:: ../../doc/img/applications_wrench_sine_overlay.png -#. Launch the `wrench_sine_overlay.launch.py `_ launch file: +#. Run the robot driver: + +.. code-block:: bash + + ros2 launch lbr_bringup bringup.launch.py \ + ctrl:=lbr_wrench_command_controller \ + sim:=false \ + model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + +The robot will move to the initial position through the Java application. + +#. Run the `wrench_sine_overlay `_ node: .. code-block:: bash - ros2 launch lbr_demos_cpp wrench_sine_overlay.launch.py model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + ros2 run lbr_demos_cpp wrench_sine_overlay --ros-args -r __ns:=/lbr -The robot will move to the initial position via cartesian impedance control. A sinusoidal force is overlayed on the x- and y-axis via `wrench_sine_overlay_node `_. +This node overlays a sinusoidal force on the x- and y-axis. diff --git a/lbr_demos/lbr_demos_cpp/doc/lbr_demos_ros2_control_cpp.rst b/lbr_demos/lbr_demos_cpp/doc/lbr_demos_ros2_control_cpp.rst deleted file mode 100644 index 6973209f..00000000 --- a/lbr_demos/lbr_demos_cpp/doc/lbr_demos_ros2_control_cpp.rst +++ /dev/null @@ -1,41 +0,0 @@ -LBR Demos ROS 2 Control C++ -=========================== -Collection of basic usage examples for ``lbr_ros2_control`` through C++. - -.. note:: - These examples can be run in simulation **and** on the real robot. - -.. warning:: - Do always execute in simulation first, then in ``T1`` mode on the real robot. - -Joint Trajectory Controller ---------------------------- -Simulation -~~~~~~~~~~ -#. Launch the ``LBRBringup``: - -.. code-block:: bash - - ros2 launch lbr_bringup bringup.launch.py sim:=true model:=iiwa7 # [iiwa7, iiwa14, med7, med14] - -#. Run the `joint_trajectory_executioner_node `_: - -.. code-block:: bash - - ros2 run lbr_demos_ros2_control_cpp joint_trajectory_executioner_node - -The robot will twist, then move to the zero configuration. - -Real Robot -~~~~~~~~~~ -#. .. 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 ``sim:=false``. diff --git a/lbr_demos/lbr_demos_cpp/launch/joint_sine_overlay.launch.py b/lbr_demos/lbr_demos_cpp/launch/joint_sine_overlay.launch.py deleted file mode 100644 index d46fa2fb..00000000 --- a/lbr_demos/lbr_demos_cpp/launch/joint_sine_overlay.launch.py +++ /dev/null @@ -1,27 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node - -from lbr_description import LBRDescriptionMixin -from lbr_fri_ros2 import LBRFRIROS2Mixin - - -def generate_launch_description() -> LaunchDescription: - ld = LaunchDescription() - ld.add_action(LBRDescriptionMixin.arg_model()) - ld.add_action(LBRDescriptionMixin.arg_robot_name()) - robot_description = LBRDescriptionMixin.param_robot_description(sim=False) - ld.add_action( - LBRFRIROS2Mixin.node_app( - parameters=[ - robot_description, - ] - ) - ) - ld.add_action( - Node( - package="lbr_demos_cpp", - executable="joint_sine_overlay_node", - output="screen", - ) - ) - return ld diff --git a/lbr_demos/lbr_demos_cpp/launch/torque_sine_overlay.launch.py b/lbr_demos/lbr_demos_cpp/launch/torque_sine_overlay.launch.py deleted file mode 100644 index 7c936c90..00000000 --- a/lbr_demos/lbr_demos_cpp/launch/torque_sine_overlay.launch.py +++ /dev/null @@ -1,27 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node - -from lbr_description import LBRDescriptionMixin -from lbr_fri_ros2 import LBRFRIROS2Mixin - - -def generate_launch_description() -> LaunchDescription: - ld = LaunchDescription() - ld.add_action(LBRDescriptionMixin.arg_model()) - ld.add_action(LBRDescriptionMixin.arg_robot_name()) - robot_description = LBRDescriptionMixin.param_robot_description(sim=False) - ld.add_action( - LBRFRIROS2Mixin.node_app( - parameters=[ - robot_description, - ] - ) - ) - ld.add_action( - Node( - package="lbr_demos_cpp", - executable="torque_sine_overlay_node", - output="screen", - ) - ) - return ld diff --git a/lbr_demos/lbr_demos_cpp/launch/wrench_sine_overlay.launch.py b/lbr_demos/lbr_demos_cpp/launch/wrench_sine_overlay.launch.py deleted file mode 100644 index f784a21b..00000000 --- a/lbr_demos/lbr_demos_cpp/launch/wrench_sine_overlay.launch.py +++ /dev/null @@ -1,27 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node - -from lbr_description import LBRDescriptionMixin -from lbr_fri_ros2 import LBRFRIROS2Mixin - - -def generate_launch_description() -> LaunchDescription: - ld = LaunchDescription() - ld.add_action(LBRDescriptionMixin.arg_model()) - ld.add_action(LBRDescriptionMixin.arg_robot_name()) - robot_description = LBRDescriptionMixin.param_robot_description(sim=False) - ld.add_action( - LBRFRIROS2Mixin.node_app( - parameters=[ - robot_description, - ] - ) - ) - ld.add_action( - Node( - package="lbr_demos_cpp", - executable="wrench_sine_overlay_node", - output="screen", - ) - ) - return ld diff --git a/lbr_demos/lbr_demos_cpp/package.xml b/lbr_demos/lbr_demos_cpp/package.xml index eb81534e..8d122381 100644 --- a/lbr_demos/lbr_demos_cpp/package.xml +++ b/lbr_demos/lbr_demos_cpp/package.xml @@ -3,7 +3,7 @@ lbr_demos_cpp 1.4.3 - C++ demos for the lbr_fri_ros2. + C++ demos for lbr_ros2_control. mhubii Apache License 2.0 @@ -12,6 +12,7 @@ control_msgs fri_vendor lbr_fri_msgs + lbr_fri_ros2 rclcpp rclcpp_action trajectory_msgs diff --git a/lbr_demos/lbr_demos_cpp/src/joint_sine_overlay_node.cpp b/lbr_demos/lbr_demos_cpp/src/joint_sine_overlay.cpp similarity index 58% rename from lbr_demos/lbr_demos_cpp/src/joint_sine_overlay_node.cpp rename to lbr_demos/lbr_demos_cpp/src/joint_sine_overlay.cpp index dd2234e3..80f0c375 100644 --- a/lbr_demos/lbr_demos_cpp/src/joint_sine_overlay_node.cpp +++ b/lbr_demos/lbr_demos_cpp/src/joint_sine_overlay.cpp @@ -3,29 +3,34 @@ #include #include +#include "rclcpp/rclcpp.hpp" + // include fri for session state #include "friClientIf.h" -#include "rclcpp/rclcpp.hpp" - // include lbr_fri_msgs #include "lbr_fri_msgs/msg/lbr_position_command.hpp" #include "lbr_fri_msgs/msg/lbr_state.hpp" +#include "lbr_fri_ros2/utils.hpp" -class JointSineOverlayNode : public rclcpp::Node { +class JointSineOverlay { static constexpr double amplitude_ = 0.04; // rad static constexpr double frequency_ = 0.25; // Hz public: - JointSineOverlayNode(const std::string &node_name) : Node(node_name), phase_(0.) { - // create publisher to /lbr/command/joint_position - lbr_position_command_pub_ = this->create_publisher( - "/lbr/command/joint_position", 1); - - // create subscription to /lbr/state - lbr_state_sub_ = this->create_subscription( - "/lbr/state", 1, - std::bind(&JointSineOverlayNode::on_lbr_state_, this, std::placeholders::_1)); + JointSineOverlay(const rclcpp::Node::SharedPtr node) : node_(node), phase_(0.) { + // create publisher to command/joint_position + lbr_position_command_pub_ = + node_->create_publisher("command/joint_position", 1); + + // create subscription to state + lbr_state_sub_ = node_->create_subscription( + "state", 1, std::bind(&JointSineOverlay::on_lbr_state_, this, std::placeholders::_1)); + + // get control rate from controller_manager + auto update_rate = + lbr_fri_ros2::retrieve_paramter(node_, "controller_manager", "update_rate").as_int(); + dt_ = 1.0 / static_cast(update_rate); }; protected: @@ -35,7 +40,7 @@ class JointSineOverlayNode : public rclcpp::Node { if (lbr_state->session_state == KUKA::FRI::COMMANDING_ACTIVE) { // overlay sine wave on 4th joint lbr_position_command_.joint_position[3] += amplitude_ * sin(phase_); - phase_ += 2 * M_PI * frequency_ * lbr_state->sample_time; + phase_ += 2 * M_PI * frequency_ * dt_; lbr_position_command_pub_->publish(lbr_position_command_); } else { @@ -44,17 +49,20 @@ class JointSineOverlayNode : public rclcpp::Node { } }; +protected: + rclcpp::Node::SharedPtr node_; + double dt_; double phase_; - rclcpp::Publisher::SharedPtr lbr_position_command_pub_; rclcpp::Subscription::SharedPtr lbr_state_sub_; - lbr_fri_msgs::msg::LBRPositionCommand lbr_position_command_; }; int main(int argc, char **argv) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared("joint_sine_overlay_node")); + auto node = std::make_shared("joint_sine_overlay"); + JointSineOverlay joint_sine_overlay(node); + rclcpp::spin(node); rclcpp::shutdown(); return 0; }; diff --git a/lbr_demos/lbr_demos_cpp/src/joint_trajectory_executioner_node.cpp b/lbr_demos/lbr_demos_cpp/src/joint_trajectory_client.cpp similarity index 82% rename from lbr_demos/lbr_demos_cpp/src/joint_trajectory_executioner_node.cpp rename to lbr_demos/lbr_demos_cpp/src/joint_trajectory_client.cpp index fee75cd6..bfdd0dae 100644 --- a/lbr_demos/lbr_demos_cpp/src/joint_trajectory_executioner_node.cpp +++ b/lbr_demos/lbr_demos_cpp/src/joint_trajectory_client.cpp @@ -10,13 +10,13 @@ #include "rclcpp_action/rclcpp_action.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" -class LBRJointTrajectoryExecutionerNode : public rclcpp::Node { +class JointTrajectoryClient : public rclcpp::Node { public: - LBRJointTrajectoryExecutionerNode(const std::string &node_name) : Node(node_name) { + JointTrajectoryClient(const std::string &node_name) : Node(node_name) { joint_trajectory_action_client_ = rclcpp_action::create_client( - this, "/lbr/joint_trajectory_controller/follow_joint_trajectory"); + this, "joint_trajectory_controller/follow_joint_trajectory"); while (!joint_trajectory_action_client_->wait_for_action_server(std::chrono::seconds(1))) { RCLCPP_INFO(this->get_logger(), "Waiting for action server to become available..."); @@ -73,12 +73,11 @@ class LBRJointTrajectoryExecutionerNode : public rclcpp::Node { int main(int argc, char **argv) { rclcpp::init(argc, argv); - auto joint_trajectory_executioner_node = - std::make_shared("joint_trajectory_executioner_node"); + auto joint_trajectory_client = std::make_shared("joint_trajectory_client"); // rotate odd joints - RCLCPP_INFO(joint_trajectory_executioner_node->get_logger(), "Rotating odd joints."); - joint_trajectory_executioner_node->execute({ + RCLCPP_INFO(joint_trajectory_client->get_logger(), "Rotating odd joints."); + joint_trajectory_client->execute({ 1.0, 0.0, 1.0, @@ -89,8 +88,8 @@ int main(int argc, char **argv) { }); // move to zero position - RCLCPP_INFO(joint_trajectory_executioner_node->get_logger(), "Moving to zero position."); - joint_trajectory_executioner_node->execute({ + RCLCPP_INFO(joint_trajectory_client->get_logger(), "Moving to zero position."); + joint_trajectory_client->execute({ 0.0, 0.0, 0.0, diff --git a/lbr_demos/lbr_demos_cpp/src/torque_sine_overlay_node.cpp b/lbr_demos/lbr_demos_cpp/src/torque_sine_overlay.cpp similarity index 57% rename from lbr_demos/lbr_demos_cpp/src/torque_sine_overlay_node.cpp rename to lbr_demos/lbr_demos_cpp/src/torque_sine_overlay.cpp index cc7b6b04..15d08439 100644 --- a/lbr_demos/lbr_demos_cpp/src/torque_sine_overlay_node.cpp +++ b/lbr_demos/lbr_demos_cpp/src/torque_sine_overlay.cpp @@ -3,29 +3,34 @@ #include #include +#include "rclcpp/rclcpp.hpp" + // include fri for session state #include "friClientIf.h" -#include "rclcpp/rclcpp.hpp" - // include lbr_fri_msgs #include "lbr_fri_msgs/msg/lbr_state.hpp" #include "lbr_fri_msgs/msg/lbr_torque_command.hpp" +#include "lbr_fri_ros2/utils.hpp" -class TorqueSineOverlayNode : public rclcpp::Node { +class TorqueSineOverlay { static constexpr double amplitude_ = 15.; // Nm static constexpr double frequency_ = 0.25; // Hz public: - TorqueSineOverlayNode(const std::string &node_name) : Node(node_name), phase_(0.) { - // create publisher to /lbr/command/torque + TorqueSineOverlay(const rclcpp::Node::SharedPtr node) : node_(node), phase_(0.) { + // create publisher to command/torque lbr_torque_command_pub_ = - this->create_publisher("/lbr/command/torque", 1); + node_->create_publisher("command/torque", 1); - // create subscription to /lbr/state - lbr_state_sub_ = this->create_subscription( - "/lbr/state", 1, - std::bind(&TorqueSineOverlayNode::on_lbr_state_, this, std::placeholders::_1)); + // create subscription to state + lbr_state_sub_ = node_->create_subscription( + "state", 1, std::bind(&TorqueSineOverlay::on_lbr_state_, this, std::placeholders::_1)); + + // get control rate from controller_manager + auto update_rate = + lbr_fri_ros2::retrieve_paramter(node_, "controller_manager", "update_rate").as_int(); + dt_ = 1.0 / static_cast(update_rate); }; protected: @@ -35,7 +40,7 @@ class TorqueSineOverlayNode : public rclcpp::Node { if (lbr_state->session_state == KUKA::FRI::COMMANDING_ACTIVE) { // overlay torque sine wave on 4th joint lbr_torque_command_.torque[3] = amplitude_ * sin(phase_); - phase_ += 2 * M_PI * frequency_ * lbr_state->sample_time; + phase_ += 2 * M_PI * frequency_ * dt_; lbr_torque_command_pub_->publish(lbr_torque_command_); } else { @@ -44,17 +49,20 @@ class TorqueSineOverlayNode : public rclcpp::Node { } }; +protected: + rclcpp::Node::SharedPtr node_; + double dt_; double phase_; - rclcpp::Publisher::SharedPtr lbr_torque_command_pub_; rclcpp::Subscription::SharedPtr lbr_state_sub_; - lbr_fri_msgs::msg::LBRTorqueCommand lbr_torque_command_; }; int main(int argc, char **argv) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared("torque_sine_overlay_node")); + auto node = std::make_shared("torque_sine_overlay"); + TorqueSineOverlay torque_sine_overlay(node); + rclcpp::spin(node); rclcpp::shutdown(); return 0; }; diff --git a/lbr_demos/lbr_demos_cpp/src/wrench_sine_overlay_node.cpp b/lbr_demos/lbr_demos_cpp/src/wrench_sine_overlay.cpp similarity index 59% rename from lbr_demos/lbr_demos_cpp/src/wrench_sine_overlay_node.cpp rename to lbr_demos/lbr_demos_cpp/src/wrench_sine_overlay.cpp index 4f211dd4..4fa4ce63 100644 --- a/lbr_demos/lbr_demos_cpp/src/wrench_sine_overlay_node.cpp +++ b/lbr_demos/lbr_demos_cpp/src/wrench_sine_overlay.cpp @@ -3,32 +3,36 @@ #include #include +#include "rclcpp/rclcpp.hpp" + // include fri for session state #include "friClientIf.h" -#include "rclcpp/rclcpp.hpp" - // include lbr_fri_msgs #include "lbr_fri_msgs/msg/lbr_state.hpp" #include "lbr_fri_msgs/msg/lbr_wrench_command.hpp" +#include "lbr_fri_ros2/utils.hpp" -class WrenchSineOverlayNode : public rclcpp::Node { +class WrenchSineOverlay { static constexpr double amplitude_x_ = 5.; // N static constexpr double amplitude_y_ = 5.; // N static constexpr double frequency_x_ = 0.25; // Hz static constexpr double frequency_y_ = 0.25; // Hz public: - WrenchSineOverlayNode(const std::string &node_name) - : Node(node_name), phase_x_(0.), phase_y_(0.) { - // create publisher to /lbr/command/wrench + WrenchSineOverlay(const rclcpp::Node::SharedPtr node) : node_(node), phase_x_(0.), phase_y_(0.) { + // create publisher to command/wrench lbr_wrench_command_pub_ = - this->create_publisher("/lbr/command/wrench", 1); + node_->create_publisher("command/wrench", 1); - // create subscription to /lbr/state - lbr_state_sub_ = this->create_subscription( - "/lbr/state", 1, - std::bind(&WrenchSineOverlayNode::on_lbr_state_, this, std::placeholders::_1)); + // create subscription to state + lbr_state_sub_ = node_->create_subscription( + "state", 1, std::bind(&WrenchSineOverlay::on_lbr_state_, this, std::placeholders::_1)); + + // get control rate from controller_manager + auto update_rate = + lbr_fri_ros2::retrieve_paramter(node_, "controller_manager", "update_rate").as_int(); + dt_ = 1.0 / static_cast(update_rate); }; protected: @@ -39,8 +43,8 @@ class WrenchSineOverlayNode : public rclcpp::Node { // overlay wrench sine wave on x / y direction lbr_wrench_command_.wrench[0] = amplitude_x_ * sin(phase_x_); lbr_wrench_command_.wrench[1] = amplitude_y_ * sin(phase_y_); - phase_x_ += 2 * M_PI * frequency_x_ * lbr_state->sample_time; - phase_y_ += 2 * M_PI * frequency_y_ * lbr_state->sample_time; + phase_x_ += 2 * M_PI * frequency_x_ * dt_; + phase_y_ += 2 * M_PI * frequency_y_ * dt_; lbr_wrench_command_pub_->publish(lbr_wrench_command_); } else { @@ -50,17 +54,20 @@ class WrenchSineOverlayNode : public rclcpp::Node { } }; +protected: + rclcpp::Node::SharedPtr node_; + double dt_; double phase_x_, phase_y_; - rclcpp::Publisher::SharedPtr lbr_wrench_command_pub_; rclcpp::Subscription::SharedPtr lbr_state_sub_; - lbr_fri_msgs::msg::LBRWrenchCommand lbr_wrench_command_; }; int main(int argc, char **argv) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared("wrench_sine_overlay_node")); + auto node = std::make_shared("wrench_sine_overlay"); + WrenchSineOverlay wrench_sine_overlay(node); + rclcpp::spin(node); rclcpp::shutdown(); return 0; }; diff --git a/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst b/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst index bf476820..f5a91c71 100644 --- a/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst +++ b/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst @@ -1,5 +1,5 @@ -LBR Demos FRI ROS 2 Python -========================== +LBR Demos Python +================ Collection of basic usage examples for the ``lbr_fri_ros2`` package through Python. .. note:: @@ -22,6 +22,48 @@ Joint Sine Overlay The robot will move to the initial position via position control, then execute a rotation on joint ``A1``. A sinusoidal motion is overlayed on joint ``A4`` via `joint_sine_overlay_node `_. +LBR Demos ROS 2 Control Python +============================== +Collection of basic usage examples for ``lbr_ros2_control`` through Python. + +.. note:: + These examples can be run in simulation **and** on the real robot. + +.. warning:: + Do always execute in simulation first, then in ``T1`` mode on the real robot. + +Joint Trajectory Controller +--------------------------- +Simulation +~~~~~~~~~~ +#. Launch the ``LBRBringup``: + +.. code-block:: bash + + ros2 launch lbr_bringup bringup.launch.py sim:=true model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + +#. Run the `joint_trajectory_executioner_node `_: + +.. code-block:: bash + + ros2 run lbr_demos_ros2_control_python joint_trajectory_executioner_node + +The robot will twist, then move to the zero configuration. + +Real Robot +~~~~~~~~~~ +#. .. 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 ``sim:=false``. + Torque Sine Overlay ------------------- #. .. dropdown:: Launch the ``LBRTorqueSineOverlay`` application on the ``KUKA smartPAD`` diff --git a/lbr_demos/lbr_demos_py/doc/lbr_demos_ros2_control_python.rst b/lbr_demos/lbr_demos_py/doc/lbr_demos_ros2_control_python.rst deleted file mode 100644 index 2a85c0a7..00000000 --- a/lbr_demos/lbr_demos_py/doc/lbr_demos_ros2_control_python.rst +++ /dev/null @@ -1,41 +0,0 @@ -LBR Demos ROS 2 Control Python -============================== -Collection of basic usage examples for ``lbr_ros2_control`` through Python. - -.. note:: - These examples can be run in simulation **and** on the real robot. - -.. warning:: - Do always execute in simulation first, then in ``T1`` mode on the real robot. - -Joint Trajectory Controller ---------------------------- -Simulation -~~~~~~~~~~ -#. Launch the ``LBRBringup``: - -.. code-block:: bash - - ros2 launch lbr_bringup bringup.launch.py sim:=true model:=iiwa7 # [iiwa7, iiwa14, med7, med14] - -#. Run the `joint_trajectory_executioner_node `_: - -.. code-block:: bash - - ros2 run lbr_demos_ros2_control_python joint_trajectory_executioner_node - -The robot will twist, then move to the zero configuration. - -Real Robot -~~~~~~~~~~ -#. .. 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 ``sim:=false``. diff --git a/lbr_demos/lbr_demos_py/launch/joint_sine_overlay.launch.py b/lbr_demos/lbr_demos_py/launch/joint_sine_overlay.launch.py deleted file mode 100644 index 3ef3fad6..00000000 --- a/lbr_demos/lbr_demos_py/launch/joint_sine_overlay.launch.py +++ /dev/null @@ -1,27 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node - -from lbr_description import LBRDescriptionMixin -from lbr_fri_ros2 import LBRFRIROS2Mixin - - -def generate_launch_description() -> LaunchDescription: - ld = LaunchDescription() - ld.add_action(LBRDescriptionMixin.arg_model()) - ld.add_action(LBRDescriptionMixin.arg_robot_name()) - robot_description = LBRDescriptionMixin.param_robot_description(sim=False) - ld.add_action( - LBRFRIROS2Mixin.node_app( - parameters=[ - robot_description, - ] - ) - ) - ld.add_action( - Node( - package="lbr_demos_py", - executable="joint_sine_overlay_node", - output="screen", - ) - ) - return ld diff --git a/lbr_demos/lbr_demos_py/launch/torque_sine_overlay.launch.py b/lbr_demos/lbr_demos_py/launch/torque_sine_overlay.launch.py deleted file mode 100644 index f2912370..00000000 --- a/lbr_demos/lbr_demos_py/launch/torque_sine_overlay.launch.py +++ /dev/null @@ -1,27 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node - -from lbr_description import LBRDescriptionMixin -from lbr_fri_ros2 import LBRFRIROS2Mixin - - -def generate_launch_description() -> LaunchDescription: - ld = LaunchDescription() - ld.add_action(LBRDescriptionMixin.arg_model()) - ld.add_action(LBRDescriptionMixin.arg_robot_name()) - robot_description = LBRDescriptionMixin.param_robot_description(sim=False) - ld.add_action( - LBRFRIROS2Mixin.node_app( - parameters=[ - robot_description, - ] - ) - ) - ld.add_action( - Node( - package="lbr_demos_py", - executable="torque_sine_overlay_node", - output="screen", - ) - ) - return ld diff --git a/lbr_demos/lbr_demos_py/launch/wrench_sine_overlay.launch.py b/lbr_demos/lbr_demos_py/launch/wrench_sine_overlay.launch.py deleted file mode 100644 index f74efbe4..00000000 --- a/lbr_demos/lbr_demos_py/launch/wrench_sine_overlay.launch.py +++ /dev/null @@ -1,27 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node - -from lbr_description import LBRDescriptionMixin -from lbr_fri_ros2 import LBRFRIROS2Mixin - - -def generate_launch_description() -> LaunchDescription: - ld = LaunchDescription() - ld.add_action(LBRDescriptionMixin.arg_model()) - ld.add_action(LBRDescriptionMixin.arg_robot_name()) - robot_description = LBRDescriptionMixin.param_robot_description(sim=False) - ld.add_action( - LBRFRIROS2Mixin.node_app( - parameters=[ - robot_description, - ] - ) - ) - ld.add_action( - Node( - package="lbr_demos_py", - executable="wrench_sine_overlay_node", - output="screen", - ) - ) - return ld diff --git a/lbr_demos/lbr_demos_py/lbr_demos_py/joint_sine_overlay_node.py b/lbr_demos/lbr_demos_py/lbr_demos_py/joint_sine_overlay.py similarity index 82% rename from lbr_demos/lbr_demos_py/lbr_demos_py/joint_sine_overlay_node.py rename to lbr_demos/lbr_demos_py/lbr_demos_py/joint_sine_overlay.py index f6c80141..8a07b0b0 100644 --- a/lbr_demos/lbr_demos_py/lbr_demos_py/joint_sine_overlay_node.py +++ b/lbr_demos/lbr_demos_py/lbr_demos_py/joint_sine_overlay.py @@ -16,20 +16,20 @@ def __init__(self, node_name: str) -> None: self._phase = 0.0 self._lbr_position_command = LBRPositionCommand() - # create publisher to /lbr/command/joint_position + # create publisher to command/joint_position self._lbr_position_command_pub = self.create_publisher( LBRPositionCommand, - "/lbr/command/joint_position", + "command/joint_position", 1, ) - # create subscription to /lbr_state + # create subscription to state self._lbr_state_sub_ = self.create_subscription( - LBRState, "/lbr/state", self._on_lbr_state, 1 + LBRState, "state", self._on_lbr_state, 1 ) def _on_lbr_state(self, lbr_state: LBRState) -> None: - self._lbr_position_command.joint_position = lbr_state.ipo_joint_position + self._lbr_position_command.joint_position = lbr_state.measured_joint_position if lbr_state.session_state == 4: # KUKA::FRI::COMMANDING_ACTIVE == 4 # overlay sine wave on 4th joint diff --git a/lbr_demos/lbr_demos_py/lbr_demos_py/joint_trajectory_executioner_node.py b/lbr_demos/lbr_demos_py/lbr_demos_py/joint_trajectory_client.py similarity index 100% rename from lbr_demos/lbr_demos_py/lbr_demos_py/joint_trajectory_executioner_node.py rename to lbr_demos/lbr_demos_py/lbr_demos_py/joint_trajectory_client.py diff --git a/lbr_demos/lbr_demos_py/lbr_demos_py/torque_sine_overlay_node.py b/lbr_demos/lbr_demos_py/lbr_demos_py/torque_sine_overlay.py similarity index 80% rename from lbr_demos/lbr_demos_py/lbr_demos_py/torque_sine_overlay_node.py rename to lbr_demos/lbr_demos_py/lbr_demos_py/torque_sine_overlay.py index 6c71f75c..7abbe9b8 100644 --- a/lbr_demos/lbr_demos_py/lbr_demos_py/torque_sine_overlay_node.py +++ b/lbr_demos/lbr_demos_py/lbr_demos_py/torque_sine_overlay.py @@ -16,18 +16,18 @@ def __init__(self, node_name: str) -> None: self._phase = 0.0 self._lbr_torque_command = LBRTorqueCommand() - # create publisher to /lbr/command/torque + # create publisher to command/torque self._lbr_torque_command_pub = self.create_publisher( - LBRTorqueCommand, "/lbr/command/torque", 1 + LBRTorqueCommand, "command/torque", 1 ) - # create subscription to /lbr_state + # create subscription to state self._lbr_state_sub = self.create_subscription( - LBRState, "/lbr/state", self._on_lbr_state, 1 + LBRState, "state", self._on_lbr_state, 1 ) def _on_lbr_state(self, lbr_state: LBRState) -> None: - self._lbr_torque_command.joint_position = lbr_state.ipo_joint_position + self._lbr_torque_command.joint_position = lbr_state.measured_joint_position if lbr_state.session_state == 4: # KUKA::FRI::COMMANDING_ACTIVE == 4 # overlay torque sine wave on 4th joint diff --git a/lbr_demos/lbr_demos_py/lbr_demos_py/wrench_sine_overlay_node.py b/lbr_demos/lbr_demos_py/lbr_demos_py/wrench_sine_overlay.py similarity index 84% rename from lbr_demos/lbr_demos_py/lbr_demos_py/wrench_sine_overlay_node.py rename to lbr_demos/lbr_demos_py/lbr_demos_py/wrench_sine_overlay.py index a636f34c..1d8a194f 100644 --- a/lbr_demos/lbr_demos_py/lbr_demos_py/wrench_sine_overlay_node.py +++ b/lbr_demos/lbr_demos_py/lbr_demos_py/wrench_sine_overlay.py @@ -16,18 +16,18 @@ def __init__(self, node_name: str) -> None: self._phase_x, self._phase_y = 0.0, 0.0 self._lbr_wrench_command = LBRWrenchCommand() - # create publisher to /lbr/command/wrench + # create publisher to command/wrench self._lbr_wrench_command_pub = self.create_publisher( - LBRWrenchCommand, "/lbr/command/wrench", 1 + LBRWrenchCommand, "command/wrench", 1 ) - # create subscription to /lbr_state + # create subscription to state self._lbr_state_sub = self.create_subscription( - LBRState, "/lbr/state", self._on_lbr_state, 1 + LBRState, "state", self._on_lbr_state, 1 ) def _on_lbr_state(self, lbr_state: LBRState) -> None: - self._lbr_wrench_command.joint_position = lbr_state.ipo_joint_position + self._lbr_wrench_command.joint_position = lbr_state.measured_joint_position if lbr_state.session_state == 4: # KUKA::FRI::COMMANDING_ACTIVE == 4 # overlay wrench sine wave on x / y direction diff --git a/lbr_demos/lbr_demos_py/package.xml b/lbr_demos/lbr_demos_py/package.xml index 34f54070..765205e3 100644 --- a/lbr_demos/lbr_demos_py/package.xml +++ b/lbr_demos/lbr_demos_py/package.xml @@ -3,7 +3,7 @@ lbr_demos_py 1.4.3 - Python demos for the lbr_fri_ros2. + Python demos for lbr_ros2_control. mhubii Apache License 2.0 diff --git a/lbr_demos/lbr_demos_py/setup.py b/lbr_demos/lbr_demos_py/setup.py index 5521ab8d..04582df7 100644 --- a/lbr_demos/lbr_demos_py/setup.py +++ b/lbr_demos/lbr_demos_py/setup.py @@ -1,5 +1,3 @@ -from glob import glob - from setuptools import setup package_name = "lbr_demos_py" @@ -11,21 +9,20 @@ data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), ("share/" + package_name, ["package.xml"]), - ("share/" + package_name + "/launch", glob("launch/*.py")), ], install_requires=["setuptools"], zip_safe=True, maintainer="mhubii", maintainer_email="m.huber_1994@hotmail.de", - description="Standalone Python demos for the LBR.", + description="Python demos for lbr_ros2_control.", license="MIT", tests_require=["pytest"], entry_points={ "console_scripts": [ - "joint_sine_overlay_node = lbr_demos_py.joint_sine_overlay_node:main", - "joint_trajectory_executioner_node = lbr_demos_ros2_control_python.joint_trajectory_executioner_node:main", - "torque_sine_overlay_node = lbr_demos_py.torque_sine_overlay_node:main", - "wrench_sine_overlay_node = lbr_demos_py.wrench_sine_overlay_node:main", + "joint_sine_overlay = lbr_demos_py.joint_sine_overlay:main", + "joint_trajectory_client = lbr_demos_ros2_control_python.joint_trajectory_client:main", + "torque_sine_overlay = lbr_demos_py.torque_sine_overlay:main", + "wrench_sine_overlay = lbr_demos_py.wrench_sine_overlay:main", ], }, ) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp new file mode 100644 index 00000000..7f677ee6 --- /dev/null +++ b/lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp @@ -0,0 +1,32 @@ +#ifndef LBR_FRI_ROS2__UTILS_HPP_ +#define LBR_FRI_ROS2__UTILS_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" + +namespace lbr_fri_ros2 { +rclcpp::Parameter retrieve_paramter(const rclcpp::Node::SharedPtr node, + const std::string &remote_node_name, + const std::string ¶meter_name) { + rclcpp::AsyncParametersClient parameter_client(node, remote_node_name); + while (!parameter_client.wait_for_service(std::chrono::seconds(1))) { + if (!rclcpp::ok()) { + std::string err = "Interrupted while waiting for the service. Exiting."; + RCLCPP_ERROR(node->get_logger(), err.c_str()); + throw std::runtime_error(err); + } + RCLCPP_INFO(node->get_logger(), "Wating for '%s' service...", remote_node_name.c_str()); + } + auto future = parameter_client.get_parameters({parameter_name}); + if (rclcpp::spin_until_future_complete(node->get_node_base_interface(), future) != + rclcpp::FutureReturnCode::SUCCESS) { + std::string err = "Failed to retrieve '%s'."; + RCLCPP_ERROR(node->get_logger(), err.c_str()); + throw std::runtime_error(err); + } + return future.get()[0]; +}; +} // namespace lbr_fri_ros2 +#endif // LBR_FRI_ROS2__UTILS_HPP_ diff --git a/lbr_ros2_control/CMakeLists.txt b/lbr_ros2_control/CMakeLists.txt index 7f202da5..293660a2 100644 --- a/lbr_ros2_control/CMakeLists.txt +++ b/lbr_ros2_control/CMakeLists.txt @@ -31,8 +31,9 @@ find_package(realtime_tools REQUIRED) add_library( ${PROJECT_NAME} SHARED - src/forward_lbr_position_command_controller.cpp - src/forward_lbr_torque_command_controller.cpp + src/lbr_position_command_controller.cpp + src/lbr_torque_command_controller.cpp + src/lbr_wrench_command_controller.cpp src/lbr_state_broadcaster.cpp src/system_interface.cpp ) diff --git a/lbr_ros2_control/config/lbr_controllers.yaml b/lbr_ros2_control/config/lbr_controllers.yaml index e3e52d30..045de27e 100644 --- a/lbr_ros2_control/config/lbr_controllers.yaml +++ b/lbr_ros2_control/config/lbr_controllers.yaml @@ -23,11 +23,14 @@ type: position_controllers/JointGroupPositionController # LBR ROS 2 control controllers - forward_lbr_position_command_controller: - type: lbr_ros2_control/ForwardLBRPositionCommandController + lbr_position_command_controller: + type: lbr_ros2_control/LBRPositionCommandController - forward_lbr_torque_command_controller: - type: lbr_ros2_control/ForwardLBRTorqueCommandController + lbr_torque_command_controller: + type: lbr_ros2_control/LBRTorqueCommandController + + lbr_wrench_command_controller: + type: lbr_ros2_control/LBRWrenchCommandController /**/force_torque_broadcaster: ros__parameters: diff --git a/lbr_ros2_control/config/lbr_system_interface.xacro b/lbr_ros2_control/config/lbr_system_interface.xacro index 3f389121..cd7fe628 100644 --- a/lbr_ros2_control/config/lbr_system_interface.xacro +++ b/lbr_ros2_control/config/lbr_system_interface.xacro @@ -87,7 +87,8 @@ - + ${chain_root} ${chain_tip} ${damping} @@ -104,6 +105,17 @@ + + + + + + + + + + diff --git a/lbr_ros2_control/include/lbr_ros2_control/forward_lbr_position_command_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/lbr_position_command_controller.hpp similarity index 82% rename from lbr_ros2_control/include/lbr_ros2_control/forward_lbr_position_command_controller.hpp rename to lbr_ros2_control/include/lbr_ros2_control/lbr_position_command_controller.hpp index e769b459..b29c6948 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/forward_lbr_position_command_controller.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/lbr_position_command_controller.hpp @@ -1,5 +1,5 @@ -#ifndef LBR_ROS2_CONTROL__FORWARD_POSITION_COMMAND_CONTROLLER_HPP_ -#define LBR_ROS2_CONTROL__FORWARD_POSITION_COMMAND_CONTROLLER_HPP_ +#ifndef LBR_ROS2_CONTROL__LBR_POSITION_COMMAND_CONTROLLER_HPP_ +#define LBR_ROS2_CONTROL__LBR_POSITION_COMMAND_CONTROLLER_HPP_ #include #include @@ -17,9 +17,9 @@ #include "lbr_fri_msgs/msg/lbr_position_command.hpp" namespace lbr_ros2_control { -class ForwardLBRPositionCommandController : public controller_interface::ControllerInterface { +class LBRPositionCommandController : public controller_interface::ControllerInterface { public: - ForwardLBRPositionCommandController(); + LBRPositionCommandController(); controller_interface::InterfaceConfiguration command_interface_configuration() const override; @@ -49,4 +49,4 @@ class ForwardLBRPositionCommandController : public controller_interface::Control lbr_position_command_subscription_ptr_; }; } // end of namespace lbr_ros2_control -#endif // LBR_ROS2_CONTROL__FORWARD_POSITION_COMMAND_CONTROLLER_HPP_ +#endif // LBR_ROS2_CONTROL__LBR_POSITION_COMMAND_CONTROLLER_HPP_ diff --git a/lbr_ros2_control/include/lbr_ros2_control/forward_lbr_torque_command_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/lbr_torque_command_controller.hpp similarity index 85% rename from lbr_ros2_control/include/lbr_ros2_control/forward_lbr_torque_command_controller.hpp rename to lbr_ros2_control/include/lbr_ros2_control/lbr_torque_command_controller.hpp index adad4304..0be853b6 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/forward_lbr_torque_command_controller.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/lbr_torque_command_controller.hpp @@ -1,5 +1,5 @@ -#ifndef LBR_ROS2_CONTROL__FORWARD_TORQUE_COMMAND_CONTROLLER_HPP_ -#define LBR_ROS2_CONTROL__FORWARD_TORQUE_COMMAND_CONTROLLER_HPP_ +#ifndef LBR_ROS2_CONTROL__LBR_TORQUE_COMMAND_CONTROLLER_HPP_ +#define LBR_ROS2_CONTROL__LBR_TORQUE_COMMAND_CONTROLLER_HPP_ #include #include @@ -19,9 +19,9 @@ #include "lbr_fri_msgs/msg/lbr_torque_command.hpp" namespace lbr_ros2_control { -class ForwardLBRTorqueCommandController : public controller_interface::ControllerInterface { +class LBRTorqueCommandController : public controller_interface::ControllerInterface { public: - ForwardLBRTorqueCommandController(); + LBRTorqueCommandController(); controller_interface::InterfaceConfiguration command_interface_configuration() const override; @@ -57,4 +57,4 @@ class ForwardLBRTorqueCommandController : public controller_interface::Controlle lbr_torque_command_subscription_ptr_; }; } // end of namespace lbr_ros2_control -#endif // LBR_ROS2_CONTROL__FORWARD_TORQUE_COMMAND_CONTROLLER_HPP_ +#endif // LBR_ROS2_CONTROL__LBR_TORQUE_COMMAND_CONTROLLER_HPP_ diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_wrench_command_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/lbr_wrench_command_controller.hpp new file mode 100644 index 00000000..20dceb12 --- /dev/null +++ b/lbr_ros2_control/include/lbr_ros2_control/lbr_wrench_command_controller.hpp @@ -0,0 +1,63 @@ +#ifndef LBR_ROS2_CONTROL__LBR_WRENCH_COMMAND_CONTROLLER_HPP_ +#define LBR_ROS2_CONTROL__LBR_WRENCH_COMMAND_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" +#include "realtime_tools/realtime_buffer.h" + +#include "friLBRState.h" + +#include "lbr_fri_msgs/msg/lbr_wrench_command.hpp" +#include "lbr_ros2_control/system_interface_type_values.hpp" + +namespace lbr_ros2_control { +class LBRWrenchCommandController : public controller_interface::ControllerInterface { + static constexpr uint8_t CARTESIAN_DOF = 6; + +public: + LBRWrenchCommandController(); + + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + controller_interface::CallbackReturn on_init() override; + + controller_interface::return_type update(const rclcpp::Time &time, + const rclcpp::Duration &period) override; + + controller_interface::CallbackReturn + on_configure(const rclcpp_lifecycle::State &previous_state) override; + + controller_interface::CallbackReturn + on_activate(const rclcpp_lifecycle::State &previous_state) override; + + controller_interface::CallbackReturn + on_deactivate(const rclcpp_lifecycle::State &previous_state) override; + +protected: + bool reference_command_interfaces_(); + void clear_command_interfaces_(); + + std::array joint_names_ = { + "A1", "A2", "A3", "A4", "A5", "A6", "A7"}; + + std::vector> + joint_position_command_interfaces_, wrench_command_interfaces_; + + realtime_tools::RealtimeBuffer + rt_lbr_wrench_command_ptr_; + rclcpp::Subscription::SharedPtr + lbr_wrench_command_subscription_ptr_; +}; +} // end of namespace lbr_ros2_control +#endif // LBR_ROS2_CONTROL__LBR_WRENCH_COMMAND_CONTROLLER_HPP_ diff --git a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp index 3453c888..1dcfa84a 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp @@ -67,6 +67,7 @@ class SystemInterface : public hardware_interface::SystemInterface { static constexpr uint8_t LBR_FRI_SENSORS = 2; static constexpr uint8_t AUXILIARY_SENSOR_SIZE = 12; static constexpr uint8_t ESTIMATED_FT_SENSOR_SIZE = 6; + static constexpr uint8_t GPIO_SIZE = 1; public: SystemInterface() = default; @@ -101,6 +102,7 @@ class SystemInterface : public hardware_interface::SystemInterface { bool verify_sensors_(); bool verify_auxiliary_sensor_(); bool verify_estimated_ft_sensor_(); + bool verify_gpios_(); // monitor end of commanding active bool exit_commanding_active_(const KUKA::FRI::ESessionState &previous_session_state, diff --git a/lbr_ros2_control/lbr_ros2_control/launch_mixin.py b/lbr_ros2_control/lbr_ros2_control/launch_mixin.py index 2a8560bb..51906b46 100644 --- a/lbr_ros2_control/lbr_ros2_control/launch_mixin.py +++ b/lbr_ros2_control/lbr_ros2_control/launch_mixin.py @@ -32,8 +32,9 @@ def arg_ctrl() -> DeclareLaunchArgument: choices=[ "joint_trajectory_controller", "forward_position_controller", - "forward_lbr_position_command_controller", - "forward_lbr_torque_command_controller", + "lbr_position_command_controller", + "lbr_torque_command_controller", + "lbr_wrench_command_controller", ], ) diff --git a/lbr_ros2_control/plugin_description_files/forward_command_controllers.xml b/lbr_ros2_control/plugin_description_files/forward_command_controllers.xml index dd26c929..4b0f625c 100644 --- a/lbr_ros2_control/plugin_description_files/forward_command_controllers.xml +++ b/lbr_ros2_control/plugin_description_files/forward_command_controllers.xml @@ -1,18 +1,26 @@ - Forward command controller for LBRPositionCommand message, see lbr_fri_msgs/msg/LBRPositionCommand.msg. - Forward command controller for LBRTorqueCommand message, see lbr_fri_msgs/msg/LBRTorqueCommand.msg. + + + + Forward command controller for LBRWrenchCommand message, see + lbr_fri_msgs/msg/LBRWrenchCommand.msg. + \ No newline at end of file diff --git a/lbr_ros2_control/src/forward_lbr_position_command_controller.cpp b/lbr_ros2_control/src/lbr_position_command_controller.cpp similarity index 64% rename from lbr_ros2_control/src/forward_lbr_position_command_controller.cpp rename to lbr_ros2_control/src/lbr_position_command_controller.cpp index bfe960fe..e2caf30b 100644 --- a/lbr_ros2_control/src/forward_lbr_position_command_controller.cpp +++ b/lbr_ros2_control/src/lbr_position_command_controller.cpp @@ -1,11 +1,11 @@ -#include "lbr_ros2_control/forward_lbr_position_command_controller.hpp" +#include "lbr_ros2_control/lbr_position_command_controller.hpp" namespace lbr_ros2_control { -ForwardLBRPositionCommandController::ForwardLBRPositionCommandController() +LBRPositionCommandController::LBRPositionCommandController() : rt_lbr_position_command_ptr_(nullptr), lbr_position_command_subscription_ptr_(nullptr) {} controller_interface::InterfaceConfiguration -ForwardLBRPositionCommandController::command_interface_configuration() const { +LBRPositionCommandController::command_interface_configuration() const { controller_interface::InterfaceConfiguration interface_configuration; interface_configuration.type = controller_interface::interface_configuration_type::INDIVIDUAL; for (const auto &joint_name : joint_names_) { @@ -15,12 +15,12 @@ ForwardLBRPositionCommandController::command_interface_configuration() const { } controller_interface::InterfaceConfiguration -ForwardLBRPositionCommandController::state_interface_configuration() const { +LBRPositionCommandController::state_interface_configuration() const { return controller_interface::InterfaceConfiguration{ controller_interface::interface_configuration_type::NONE}; } -controller_interface::CallbackReturn ForwardLBRPositionCommandController::on_init() { +controller_interface::CallbackReturn LBRPositionCommandController::on_init() { try { lbr_position_command_subscription_ptr_ = this->get_node()->create_subscription( @@ -30,8 +30,7 @@ controller_interface::CallbackReturn ForwardLBRPositionCommandController::on_ini }); } catch (const std::exception &e) { RCLCPP_ERROR(this->get_node()->get_logger(), - "Failed to initialize forward LBR position command controller with: %s.", - e.what()); + "Failed to initialize LBR position command controller with: %s.", e.what()); return controller_interface::CallbackReturn::ERROR; } @@ -39,8 +38,8 @@ controller_interface::CallbackReturn ForwardLBRPositionCommandController::on_ini } controller_interface::return_type -ForwardLBRPositionCommandController::update(const rclcpp::Time & /*time*/, - const rclcpp::Duration & /*period*/) { +LBRPositionCommandController::update(const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) { auto lbr_position_command = rt_lbr_position_command_ptr_.readFromRT(); if (!lbr_position_command || !(*lbr_position_command)) { return controller_interface::return_type::OK; @@ -52,23 +51,23 @@ ForwardLBRPositionCommandController::update(const rclcpp::Time & /*time*/, return controller_interface::return_type::OK; } -controller_interface::CallbackReturn ForwardLBRPositionCommandController::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) { +controller_interface::CallbackReturn +LBRPositionCommandController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn ForwardLBRPositionCommandController::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) { +controller_interface::CallbackReturn +LBRPositionCommandController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn ForwardLBRPositionCommandController::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) { +controller_interface::CallbackReturn +LBRPositionCommandController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { return controller_interface::CallbackReturn::SUCCESS; } } // end of namespace lbr_ros2_control #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(lbr_ros2_control::ForwardLBRPositionCommandController, +PLUGINLIB_EXPORT_CLASS(lbr_ros2_control::LBRPositionCommandController, controller_interface::ControllerInterface) diff --git a/lbr_ros2_control/src/forward_lbr_torque_command_controller.cpp b/lbr_ros2_control/src/lbr_torque_command_controller.cpp similarity index 70% rename from lbr_ros2_control/src/forward_lbr_torque_command_controller.cpp rename to lbr_ros2_control/src/lbr_torque_command_controller.cpp index 58becd53..26b961c6 100644 --- a/lbr_ros2_control/src/forward_lbr_torque_command_controller.cpp +++ b/lbr_ros2_control/src/lbr_torque_command_controller.cpp @@ -1,11 +1,11 @@ -#include "lbr_ros2_control/forward_lbr_torque_command_controller.hpp" +#include "lbr_ros2_control/lbr_torque_command_controller.hpp" namespace lbr_ros2_control { -ForwardLBRTorqueCommandController::ForwardLBRTorqueCommandController() +LBRTorqueCommandController::LBRTorqueCommandController() : rt_lbr_torque_command_ptr_(nullptr), lbr_torque_command_subscription_ptr_(nullptr) {} controller_interface::InterfaceConfiguration -ForwardLBRTorqueCommandController::command_interface_configuration() const { +LBRTorqueCommandController::command_interface_configuration() const { controller_interface::InterfaceConfiguration interface_configuration; interface_configuration.type = controller_interface::interface_configuration_type::INDIVIDUAL; for (const auto &joint_name : joint_names_) { @@ -16,12 +16,12 @@ ForwardLBRTorqueCommandController::command_interface_configuration() const { } controller_interface::InterfaceConfiguration -ForwardLBRTorqueCommandController::state_interface_configuration() const { +LBRTorqueCommandController::state_interface_configuration() const { return controller_interface::InterfaceConfiguration{ controller_interface::interface_configuration_type::NONE}; } -controller_interface::CallbackReturn ForwardLBRTorqueCommandController::on_init() { +controller_interface::CallbackReturn LBRTorqueCommandController::on_init() { try { lbr_torque_command_subscription_ptr_ = this->get_node()->create_subscription( @@ -30,7 +30,7 @@ controller_interface::CallbackReturn ForwardLBRTorqueCommandController::on_init( }); } catch (const std::exception &e) { RCLCPP_ERROR(this->get_node()->get_logger(), - "Failed to initialize forward LBR torque command controller with: %s.", e.what()); + "Failed to initialize LBR torque command controller with: %s.", e.what()); return controller_interface::CallbackReturn::ERROR; } @@ -38,8 +38,8 @@ controller_interface::CallbackReturn ForwardLBRTorqueCommandController::on_init( } controller_interface::return_type -ForwardLBRTorqueCommandController::update(const rclcpp::Time & /*time*/, - const rclcpp::Duration & /*period*/) { +LBRTorqueCommandController::update(const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) { auto lbr_torque_command = rt_lbr_torque_command_ptr_.readFromRT(); if (!lbr_torque_command || !(*lbr_torque_command)) { return controller_interface::return_type::OK; @@ -52,26 +52,26 @@ ForwardLBRTorqueCommandController::update(const rclcpp::Time & /*time*/, return controller_interface::return_type::OK; } -controller_interface::CallbackReturn ForwardLBRTorqueCommandController::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) { +controller_interface::CallbackReturn +LBRTorqueCommandController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { return controller_interface::CallbackReturn::SUCCESS; } controller_interface::CallbackReturn -ForwardLBRTorqueCommandController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { +LBRTorqueCommandController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { if (!reference_command_interfaces_()) { return controller_interface::CallbackReturn::ERROR; } return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn ForwardLBRTorqueCommandController::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) { +controller_interface::CallbackReturn +LBRTorqueCommandController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { clear_command_interfaces_(); return controller_interface::CallbackReturn::SUCCESS; } -bool ForwardLBRTorqueCommandController::reference_command_interfaces_() { +bool LBRTorqueCommandController::reference_command_interfaces_() { for (auto &command_interface : command_interfaces_) { if (command_interface.get_interface_name() == hardware_interface::HW_IF_POSITION) { joint_position_command_interfaces_.emplace_back(std::ref(command_interface)); @@ -83,22 +83,22 @@ bool ForwardLBRTorqueCommandController::reference_command_interfaces_() { if (joint_position_command_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { RCLCPP_ERROR( this->get_node()->get_logger(), - "Number of joint position command interfaces (%ld) does not match the number of joints " - "in the robot (%d).", + "Number of joint position command interfaces '%ld' does not match the number of joints " + "in the robot '%d'.", joint_position_command_interfaces_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); return false; } if (torque_command_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { RCLCPP_ERROR(this->get_node()->get_logger(), - "Number of torque command interfaces (%ld) does not match the number of joints " - "in the robot (%d).", + "Number of torque command interfaces '%ld' does not match the number of joints " + "in the robot '%d'.", torque_command_interfaces_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); return false; } return true; } -void ForwardLBRTorqueCommandController::clear_command_interfaces_() { +void LBRTorqueCommandController::clear_command_interfaces_() { joint_position_command_interfaces_.clear(); torque_command_interfaces_.clear(); } @@ -106,5 +106,5 @@ void ForwardLBRTorqueCommandController::clear_command_interfaces_() { #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(lbr_ros2_control::ForwardLBRTorqueCommandController, +PLUGINLIB_EXPORT_CLASS(lbr_ros2_control::LBRTorqueCommandController, controller_interface::ControllerInterface) diff --git a/lbr_ros2_control/src/lbr_wrench_command_controller.cpp b/lbr_ros2_control/src/lbr_wrench_command_controller.cpp new file mode 100644 index 00000000..3eb4f258 --- /dev/null +++ b/lbr_ros2_control/src/lbr_wrench_command_controller.cpp @@ -0,0 +1,116 @@ +#include "lbr_ros2_control/lbr_wrench_command_controller.hpp" + +namespace lbr_ros2_control { +LBRWrenchCommandController::LBRWrenchCommandController() + : rt_lbr_wrench_command_ptr_(nullptr), lbr_wrench_command_subscription_ptr_(nullptr) {} + +controller_interface::InterfaceConfiguration +LBRWrenchCommandController::command_interface_configuration() const { + controller_interface::InterfaceConfiguration interface_configuration; + interface_configuration.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto &joint_name : joint_names_) { + interface_configuration.names.push_back(joint_name + "/" + hardware_interface::HW_IF_POSITION); + } + interface_configuration.names.push_back(std::string(HW_IF_WRENCH) + "/" + HW_IF_FORCE_X); + interface_configuration.names.push_back(std::string(HW_IF_WRENCH) + "/" + HW_IF_FORCE_Y); + interface_configuration.names.push_back(std::string(HW_IF_WRENCH) + "/" + HW_IF_FORCE_Z); + interface_configuration.names.push_back(std::string(HW_IF_WRENCH) + "/" + HW_IF_TORQUE_X); + interface_configuration.names.push_back(std::string(HW_IF_WRENCH) + "/" + HW_IF_TORQUE_Y); + interface_configuration.names.push_back(std::string(HW_IF_WRENCH) + "/" + HW_IF_TORQUE_Z); + return interface_configuration; +} + +controller_interface::InterfaceConfiguration +LBRWrenchCommandController::state_interface_configuration() const { + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::NONE}; +} + +controller_interface::CallbackReturn LBRWrenchCommandController::on_init() { + try { + lbr_wrench_command_subscription_ptr_ = + this->get_node()->create_subscription( + "command/wrench", 1, [this](const lbr_fri_msgs::msg::LBRWrenchCommand::SharedPtr msg) { + rt_lbr_wrench_command_ptr_.writeFromNonRT(msg); + }); + } catch (const std::exception &e) { + RCLCPP_ERROR(this->get_node()->get_logger(), + "Failed to initialize LBR wrench command controller with: %s.", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type +LBRWrenchCommandController::update(const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) { + auto lbr_wrench_command = rt_lbr_wrench_command_ptr_.readFromRT(); + if (!lbr_wrench_command || !(*lbr_wrench_command)) { + return controller_interface::return_type::OK; + } + for (std::size_t idx = 0; idx < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; ++idx) { + joint_position_command_interfaces_[idx].get().set_value( + (*lbr_wrench_command)->joint_position[idx]); + } + for (std::size_t idx = 0; idx < CARTESIAN_DOF; ++idx) { + wrench_command_interfaces_[idx].get().set_value((*lbr_wrench_command)->wrench[idx]); + } + return controller_interface::return_type::OK; +} + +controller_interface::CallbackReturn +LBRWrenchCommandController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +LBRWrenchCommandController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { + if (!reference_command_interfaces_()) { + return controller_interface::CallbackReturn::ERROR; + } + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +LBRWrenchCommandController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { + clear_command_interfaces_(); + return controller_interface::CallbackReturn::SUCCESS; +} + +bool LBRWrenchCommandController::reference_command_interfaces_() { + for (auto &command_interface : command_interfaces_) { + if (command_interface.get_interface_name() == hardware_interface::HW_IF_POSITION) { + joint_position_command_interfaces_.emplace_back(std::ref(command_interface)); + } + if (command_interface.get_prefix_name() == HW_IF_WRENCH) { + wrench_command_interfaces_.emplace_back(std::ref(command_interface)); + } + } + if (joint_position_command_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + RCLCPP_ERROR( + this->get_node()->get_logger(), + "Number of joint position command interfaces '%ld' does not match the number of joints " + "in the robot '%d'.", + joint_position_command_interfaces_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + return false; + } + if (wrench_command_interfaces_.size() != CARTESIAN_DOF) { + RCLCPP_ERROR(this->get_node()->get_logger(), + "Number of wrench command interfaces '%ld' does not equal %d.", + wrench_command_interfaces_.size(), CARTESIAN_DOF); + return false; + } + return true; +} + +void LBRWrenchCommandController::clear_command_interfaces_() { + joint_position_command_interfaces_.clear(); + wrench_command_interfaces_.clear(); +} +} // end of namespace lbr_ros2_control + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(lbr_ros2_control::LBRWrenchCommandController, + controller_interface::ControllerInterface) diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 0813083e..55905e62 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -124,6 +124,10 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { return controller_interface::CallbackReturn::ERROR; } + if (!verify_gpios_()) { + return controller_interface::CallbackReturn::ERROR; + } + return controller_interface::CallbackReturn::SUCCESS; } @@ -197,6 +201,14 @@ std::vector SystemInterface::export_comman command_interfaces.emplace_back(info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_lbr_command_.torque[i]); } + + // Cartesian impedance control command interfaces + command_interfaces.emplace_back(HW_IF_WRENCH, HW_IF_FORCE_X, &hw_lbr_command_.wrench[0]); + command_interfaces.emplace_back(HW_IF_WRENCH, HW_IF_FORCE_Y, &hw_lbr_command_.wrench[1]); + command_interfaces.emplace_back(HW_IF_WRENCH, HW_IF_FORCE_Z, &hw_lbr_command_.wrench[2]); + command_interfaces.emplace_back(HW_IF_WRENCH, HW_IF_TORQUE_X, &hw_lbr_command_.wrench[3]); + command_interfaces.emplace_back(HW_IF_WRENCH, HW_IF_TORQUE_Y, &hw_lbr_command_.wrench[4]); + command_interfaces.emplace_back(HW_IF_WRENCH, HW_IF_TORQUE_Z, &hw_lbr_command_.wrench[5]); return command_interfaces; } @@ -326,9 +338,14 @@ hardware_interface::return_type SystemInterface::write(const rclcpp::Time & /*ti return hardware_interface::return_type::OK; } if (hw_client_command_mode_ == KUKA::FRI::EClientCommandMode::WRENCH) { - throw std::runtime_error( - lbr_fri_ros2::EnumMaps::client_command_mode_map(hw_client_command_mode_) + - " command mode currently not implemented."); + if (std::any_of(hw_lbr_command_.wrench.cbegin(), hw_lbr_command_.wrench.cend(), + [](const double &v) { return std::isnan(v); }) || + std::any_of(hw_lbr_command_.joint_position.cbegin(), hw_lbr_command_.joint_position.cend(), + [](const double &v) { return std::isnan(v); })) { + return hardware_interface::return_type::OK; + } + async_client_ptr_->get_command_interface().set_command_target(hw_lbr_command_); + return hardware_interface::return_type::OK; } #if FRICLIENT_VERSION_MAJOR == 2 if (hw_client_command_mode_ == KUKA::FRI::EClientCommandMode::CARTESIAN_POSE) { @@ -479,9 +496,9 @@ bool SystemInterface::verify_auxiliary_sensor_() { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), lbr_fri_ros2::ColorScheme::ERROR << "Sensor '" << auxiliary_sensor.name.c_str() - << "' received invalid number of state interfaces." - << " Received '" << auxiliary_sensor.state_interfaces.size() - << "', expected '" << static_cast(AUXILIARY_SENSOR_SIZE) << "'" + << "' received invalid number of state interfaces." << " Received '" + << auxiliary_sensor.state_interfaces.size() << "', expected '" + << static_cast(AUXILIARY_SENSOR_SIZE) << "'" << lbr_fri_ros2::ColorScheme::ENDC); return false; } @@ -533,6 +550,27 @@ bool SystemInterface::verify_estimated_ft_sensor_() { return true; } +bool SystemInterface::verify_gpios_() { + if (info_.gpios.size() != GPIO_SIZE) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + lbr_fri_ros2::ColorScheme::ERROR + << "Expected '" << static_cast(GPIO_SIZE) << "' GPIOs, got '" + << info_.gpios.size() << "'" << lbr_fri_ros2::ColorScheme::ENDC); + return false; + } + if (info_.gpios[0].command_interfaces.size() != hw_lbr_command_.wrench.size()) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + lbr_fri_ros2::ColorScheme::ERROR + << "GPIO '" << info_.gpios[0].name.c_str() + << "' received invalid number of command interfaces. Received '" + << info_.gpios[0].command_interfaces.size() << "', expected '" + << hw_lbr_command_.wrench.size() << "'" + << lbr_fri_ros2::ColorScheme::ENDC); + return false; + } + return true; +} + bool SystemInterface::exit_commanding_active_( const KUKA::FRI::ESessionState &previous_session_state, const KUKA::FRI::ESessionState &session_state) { From 92b1c9918834b1b1e204ba934ec2a904c2751fbd Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 8 May 2024 19:04:00 +0100 Subject: [PATCH 37/76] updated robot description source --- .../src/lbr_base_position_command_node.hpp | 2 +- .../lbr_demos_advanced_py/lbr_base_position_command_node.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/lbr_demos/lbr_demos_advanced_cpp/src/lbr_base_position_command_node.hpp b/lbr_demos/lbr_demos_advanced_cpp/src/lbr_base_position_command_node.hpp index 28c80266..c455f7f4 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/src/lbr_base_position_command_node.hpp +++ b/lbr_demos/lbr_demos_advanced_cpp/src/lbr_base_position_command_node.hpp @@ -16,7 +16,7 @@ class LBRBasePositionCommandNode : public rclcpp::Node { : Node(node_name, options) { // retrieve parameters robot_description_ = - this->retrieve_parameter_("controller_manager", "robot_description").as_string(); + this->retrieve_parameter_("robot_state_publisher", "robot_description").as_string(); update_rate_ = this->retrieve_parameter_("controller_manager", "update_rate").as_int(); dt_ = 1.0 / static_cast(update_rate_); diff --git a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/lbr_base_position_command_node.py b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/lbr_base_position_command_node.py index e27020b2..a4d4c65a 100644 --- a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/lbr_base_position_command_node.py +++ b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/lbr_base_position_command_node.py @@ -21,7 +21,7 @@ def __init__(self, node_name: str) -> None: # retrieve parameters self._robot_description = self._retrieve_parameter( - "controller_manager/get_parameters", "robot_description" + "robot_state_publisher/get_parameters", "robot_description" ).string_value self._update_rate = self._retrieve_parameter( "controller_manager/get_parameters", "update_rate" From ac7b6de766c83421ffe8545499fac361b9ccb4b5 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 9 May 2024 14:53:19 +0100 Subject: [PATCH 38/76] updated demos --- .../config/admittance_control.yaml | 2 +- .../src/admittance_control_node.cpp | 23 +++++++++++- .../src/admittance_controller.hpp | 2 +- .../config/admittance_control.yaml | 2 +- .../config/admittance_rcm_control.yaml | 4 +- .../admittance_control_node.py | 22 ++++++++++- .../admittance_controller.py | 12 +++--- .../admittance_rcm_control_node.py | 36 ++++++++++++++---- lbr_demos/lbr_demos_cpp/CMakeLists.txt | 4 +- lbr_demos/lbr_demos_cpp/package.xml | 1 + .../lbr_demos_cpp/src/joint_sine_overlay.cpp | 8 +++- .../src/joint_trajectory_client.cpp | 5 +++ .../lbr_demos_cpp/src/torque_sine_overlay.cpp | 8 +++- .../lbr_demos_cpp/src/wrench_sine_overlay.cpp | 8 +++- .../lbr_demos_py/joint_sine_overlay.py | 33 ++++++++++++++++- .../lbr_demos_py/torque_sine_overlay.py | 33 ++++++++++++++++- .../lbr_demos_py/wrench_sine_overlay.py | 37 +++++++++++++++++-- .../config/lbr_system_interface.xacro | 2 +- 18 files changed, 207 insertions(+), 35 deletions(-) diff --git a/lbr_demos/lbr_demos_advanced_cpp/config/admittance_control.yaml b/lbr_demos/lbr_demos_advanced_cpp/config/admittance_control.yaml index 4291d202..2cf31107 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/config/admittance_control.yaml +++ b/lbr_demos/lbr_demos_advanced_cpp/config/admittance_control.yaml @@ -3,6 +3,6 @@ admittance_control: base_link: "link_0" end_effector_link: "link_ee" f_ext_th: [2., 2., 2., 0.5, 0.5, 0.5] - dq_gains: [20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0] + dq_gains: [2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0] dx_gains: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] exp_smooth: 0.95 diff --git a/lbr_demos/lbr_demos_advanced_cpp/src/admittance_control_node.cpp b/lbr_demos/lbr_demos_advanced_cpp/src/admittance_control_node.cpp index 7463adfb..feb5d788 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/src/admittance_control_node.cpp +++ b/lbr_demos/lbr_demos_advanced_cpp/src/admittance_control_node.cpp @@ -15,7 +15,7 @@ class AdmittanceControlNode : public LBRBasePositionCommandNode { this->declare_parameter("base_link", "link_0"); this->declare_parameter("end_effector_link", "link_ee"); this->declare_parameter>("f_ext_th", {2., 2., 2., 0.5, 0.5, 0.5}); - this->declare_parameter>("dq_gains", {20., 20., 20., 20., 20., 20., 20.}); + this->declare_parameter>("dq_gains", {2., 2., 2., 2., 2., 2., 2.}); this->declare_parameter>("dx_gains", {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}); this->declare_parameter("exp_smooth", 0.95); @@ -30,9 +30,30 @@ class AdmittanceControlNode : public LBRBasePositionCommandNode { this->get_parameter("f_ext_th").as_double_array(), this->get_parameter("dq_gains").as_double_array(), this->get_parameter("dx_gains").as_double_array()); + + // log parameters to terminal + this->log_paramters_(); } protected: + void log_paramters_() { + RCLCPP_INFO(this->get_logger(), "*** Parameters:"); + RCLCPP_INFO(this->get_logger(), "* base_link: %s", + this->get_parameter("base_link").as_string().c_str()); + RCLCPP_INFO(this->get_logger(), "* end_effector_link: %s", + this->get_parameter("end_effector_link").as_string().c_str()); + auto f_ext_th = this->get_parameter("f_ext_th").as_double_array(); + RCLCPP_INFO(this->get_logger(), "* f_ext_th: %.1f, %.1f, %.1f, %.1f, %.1f, %.1f", f_ext_th[0], + f_ext_th[1], f_ext_th[2], f_ext_th[3], f_ext_th[4], f_ext_th[5]); + auto dq_gains = this->get_parameter("dq_gains").as_double_array(); + RCLCPP_INFO(this->get_logger(), "* dq_gains: %.1f, %.1f, %.1f, %.1f, %.1f, %.1f, %.1f", + dq_gains[0], dq_gains[1], dq_gains[2], dq_gains[3], dq_gains[4], dq_gains[5], + dq_gains[6]); + auto dx_gains = this->get_parameter("dx_gains").as_double_array(); + RCLCPP_INFO(this->get_logger(), "* dx_gains: %.1f, %.1f, %.1f, %.1f, %.1f, %.1f", dx_gains[0], + dx_gains[1], dx_gains[2], dx_gains[3], dx_gains[4], dx_gains[5]); + } + void on_lbr_state_(const lbr_fri_msgs::msg::LBRState::SharedPtr lbr_state) override { if (!lbr_state) { return; diff --git a/lbr_demos/lbr_demos_advanced_cpp/src/admittance_controller.hpp b/lbr_demos/lbr_demos_advanced_cpp/src/admittance_controller.hpp index fbdc6dfb..07c11956 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/src/admittance_controller.hpp +++ b/lbr_demos/lbr_demos_advanced_cpp/src/admittance_controller.hpp @@ -26,7 +26,7 @@ class AdmittanceController { const std::string &base_link = "link_0", const std::string &end_effector_link = "link_ee", const std::vector &f_ext_th = {2., 2., 2., 0.5, 0.5, 0.5}, - const std::vector &dq_gains = {20., 20., 20., 20., 20., 20., 20.}, + const std::vector &dq_gains = {2., 2., 2., 2., 2., 2., 2.}, const std::vector &dx_gains = {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}) : dq_gains_(dq_gains.data()), dx_gains_(dx_gains.data()), f_ext_th_(f_ext_th.data()) { if (!kdl_parser::treeFromString(robot_description, tree_)) { diff --git a/lbr_demos/lbr_demos_advanced_py/config/admittance_control.yaml b/lbr_demos/lbr_demos_advanced_py/config/admittance_control.yaml index 31e57382..4146d02f 100644 --- a/lbr_demos/lbr_demos_advanced_py/config/admittance_control.yaml +++ b/lbr_demos/lbr_demos_advanced_py/config/admittance_control.yaml @@ -3,6 +3,6 @@ base_link: "link_0" end_effector_link: "link_ee" f_ext_th: [2., 2., 2., 0.5, 0.5, 0.5] - dq_gains: [10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0] + dq_gains: [1., 1., 1., 1., 1., 1., 1.] dx_gains: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] exp_smooth: 0.95 diff --git a/lbr_demos/lbr_demos_advanced_py/config/admittance_rcm_control.yaml b/lbr_demos/lbr_demos_advanced_py/config/admittance_rcm_control.yaml index 84a25e4e..901d1d87 100644 --- a/lbr_demos/lbr_demos_advanced_py/config/admittance_rcm_control.yaml +++ b/lbr_demos/lbr_demos_advanced_py/config/admittance_rcm_control.yaml @@ -2,7 +2,7 @@ ros__parameters: base_link: "link_0" end_effector_link: "link_ee" - f_ext_th: [2., 2., 2., 0.5, 0.5, 0.5] - dq_gains: [10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0] + f_ext_th: [4.0, 4.0, 4.0, 1.0, 1.0, 1.0] + dq_gains: [1., 1., 1., 1., 1., 1., 1.] dx_gains: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] exp_smooth: 0.95 diff --git a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_control_node.py b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_control_node.py index 68d09652..51300203 100755 --- a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_control_node.py +++ b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_control_node.py @@ -15,8 +15,8 @@ def __init__(self, node_name: str = "admittance_control") -> None: self.declare_parameter("base_link", "link_0") self.declare_parameter("end_effector_link", "link_ee") self.declare_parameter("f_ext_th", [2.0, 2.0, 2.0, 0.5, 0.5, 0.5]) - self.declare_parameter("dq_gain", [10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0]) - self.declare_parameter("dx_gain", [0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) + self.declare_parameter("dq_gains", [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]) + self.declare_parameter("dx_gains", [0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) self.declare_parameter("exp_smooth", 0.95) self._init = False @@ -37,6 +37,24 @@ def __init__(self, node_name: str = "admittance_control") -> None: .string_value, ) + # log parameters to terminal + self._log_parameters() + + def _log_parameters(self) -> None: + self.get_logger().info("*** Paramters:") + self.get_logger().info( + f"* base_link: {self.get_parameter('base_link').value}" + ) + self.get_logger().info( + f"* end_effector_link: {self.get_parameter('end_effector_link').value}" + ) + self.get_logger().info(f"* f_ext_th: {self.get_parameter('f_ext_th').value}") + self.get_logger().info(f"* dq_gains: {self.get_parameter('dq_gains').value}") + self.get_logger().info(f"* dx_gains: {self.get_parameter('dx_gains').value}") + self.get_logger().info( + f"* exp_smooth: {self.get_parameter('exp_smooth').value}" + ) + def _on_lbr_state(self, lbr_state: LBRState) -> None: self._smooth_lbr_state(lbr_state) diff --git a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_controller.py b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_controller.py index c95738e3..697b5e33 100644 --- a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_controller.py +++ b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_controller.py @@ -11,8 +11,8 @@ def __init__( base_link: str = "link_0", end_effector_link: str = "link_ee", f_ext_th: np.ndarray = np.array([2.0, 2.0, 2.0, 0.5, 0.5, 0.5]), - dq_gain: np.ndarray = np.array([10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0]), - dx_gain: np.ndarray = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]), + dq_gains: np.ndarray = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]), + dx_gains: np.ndarray = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]), ) -> None: self._lbr_position_command = LBRPositionCommand() @@ -28,8 +28,8 @@ def __init__( self._q = np.zeros(self._dof) self._dq = np.zeros(self._dof) self._tau_ext = np.zeros(6) - self._dq_gain = np.diag(dq_gain) - self._dx_gain = np.diag(dx_gain) + self._dq_gains = np.diag(dq_gains) + self._dx_gains = np.diag(dx_gains) self._f_ext = np.zeros(6) self._f_ext_th = f_ext_th self._alpha = 0.95 @@ -44,14 +44,14 @@ def __call__(self, lbr_state: LBRState, dt: float) -> LBRPositionCommand: dx = np.where( abs(self._f_ext) > self._f_ext_th, - self._dx_gain @ np.sign(self._f_ext) * (abs(self._f_ext) - self._f_ext_th), + self._dx_gains @ np.sign(self._f_ext) * (abs(self._f_ext) - self._f_ext_th), 0.0, ) # additional smoothing required in python self._dq = ( self._alpha * self._dq - + (1 - self._alpha) * self._dq_gain @ self._jacobian_inv @ dx + + (1 - self._alpha) * self._dq_gains @ self._jacobian_inv @ dx ) self._lbr_position_command.joint_position = ( diff --git a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_control_node.py b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_control_node.py index ac29bc3b..17d85db4 100644 --- a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_control_node.py +++ b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_control_node.py @@ -15,18 +15,18 @@ def __init__(self, node_name: str = "admittance_rcm_control") -> None: self.declare_parameter("base_link", "link_0") self.declare_parameter("end_effector_link", "link_ee") self.declare_parameter("f_ext_th", [4.0, 4.0, 4.0, 1.0, 1.0, 1.0]) - self.declare_parameter("dq_gain", [10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0]) - self.declare_parameter("dx_gain", [0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) + self.declare_parameter("dq_gains", [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]) + self.declare_parameter("dx_gains", [0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) self.declare_parameter("exp_smooth", 0.95) self._f_ext_th = np.array( self.get_parameter("f_ext_th").get_parameter_value().double_array_value ) - self._dq_gain = np.diag( - self.get_parameter("dq_gain").get_parameter_value().double_array_value + self._dq_gains = np.diag( + self.get_parameter("dq_gains").get_parameter_value().double_array_value ) - self._dx_gain = np.diag( - self.get_parameter("dx_gain").get_parameter_value().double_array_value + self._dx_gains = np.diag( + self.get_parameter("dx_gains").get_parameter_value().double_array_value ) self._exp_smooth = ( @@ -46,6 +46,24 @@ def __init__(self, node_name: str = "admittance_rcm_control") -> None: .string_value, ) + # log parameters to terminal + self._log_parameters() + + def _log_parameters(self) -> None: + self.get_logger().info("*** Paramters:") + self.get_logger().info( + f"* base_link: {self.get_parameter('base_link').value}" + ) + self.get_logger().info( + f"* end_effector_link: {self.get_parameter('end_effector_link').value}" + ) + self.get_logger().info(f"* f_ext_th: {self.get_parameter('f_ext_th').value}") + self.get_logger().info(f"* dq_gains: {self.get_parameter('dq_gains').value}") + self.get_logger().info(f"* dx_gains: {self.get_parameter('dx_gains').value}") + self.get_logger().info( + f"* exp_smooth: {self.get_parameter('exp_smooth').value}" + ) + def _command(self, q) -> None: lbr_command = LBRPositionCommand() lbr_command.joint_position = q @@ -57,7 +75,7 @@ def _admittance(self, tau_ext, qc) -> None: f_ext = Jinv.T @ tau_ext dx = np.where( abs(f_ext) > self._f_ext_th, - self._dx_gain @ np.sign(f_ext) * (abs(f_ext) - self._f_ext_th), + self._dx_gains @ np.sign(f_ext) * (abs(f_ext) - self._f_ext_th), 0.0, ).flatten() dx = np.clip(dx, -1.0, 1.0) @@ -76,10 +94,12 @@ def _on_lbr_state(self, msg: LBRState) -> None: if self._controller.solve(): qc = np.array(msg.measured_joint_position) dq = self._controller.get_qd_target() - qn = qc + self._dt * self._dq_gain @ dq + qn = qc + self._dt * self._dq_gains @ dq self._command(qn.tolist()) else: self.get_logger().error("Solver failed!") + qn = np.array(msg.measured_joint_position) + self._command(qn.tolist()) def main(args=None) -> None: diff --git a/lbr_demos/lbr_demos_cpp/CMakeLists.txt b/lbr_demos/lbr_demos_cpp/CMakeLists.txt index 433c0675..2fc4d100 100644 --- a/lbr_demos/lbr_demos_cpp/CMakeLists.txt +++ b/lbr_demos/lbr_demos_cpp/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(lbr_fri_msgs REQUIRED) find_package(lbr_fri_ros2 REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) +find_package(sensor_msgs REQUIRED) find_package(trajectory_msgs REQUIRED) # joint sine overlay @@ -36,7 +37,7 @@ target_link_libraries(joint_sine_overlay FRIClient::FRIClient ) -# joint trajectory executioner +# joint trajectory client add_executable(joint_trajectory_client src/joint_trajectory_client.cpp ) @@ -46,6 +47,7 @@ ament_target_dependencies(joint_trajectory_client fri_vendor rclcpp rclcpp_action + sensor_msgs trajectory_msgs ) diff --git a/lbr_demos/lbr_demos_cpp/package.xml b/lbr_demos/lbr_demos_cpp/package.xml index 8d122381..389f86ed 100644 --- a/lbr_demos/lbr_demos_cpp/package.xml +++ b/lbr_demos/lbr_demos_cpp/package.xml @@ -15,6 +15,7 @@ lbr_fri_ros2 rclcpp rclcpp_action + sensor_msgs trajectory_msgs lbr_bringup diff --git a/lbr_demos/lbr_demos_cpp/src/joint_sine_overlay.cpp b/lbr_demos/lbr_demos_cpp/src/joint_sine_overlay.cpp index 80f0c375..57918728 100644 --- a/lbr_demos/lbr_demos_cpp/src/joint_sine_overlay.cpp +++ b/lbr_demos/lbr_demos_cpp/src/joint_sine_overlay.cpp @@ -35,7 +35,11 @@ class JointSineOverlay { protected: void on_lbr_state_(const lbr_fri_msgs::msg::LBRState::SharedPtr lbr_state) { - lbr_position_command_.joint_position = lbr_state->ipo_joint_position; + if (!lbr_state_init_) { + lbr_state_ = *lbr_state; + lbr_state_init_ = true; + } + lbr_position_command_.joint_position = lbr_state_.measured_joint_position; if (lbr_state->session_state == KUKA::FRI::COMMANDING_ACTIVE) { // overlay sine wave on 4th joint @@ -55,6 +59,8 @@ class JointSineOverlay { double phase_; rclcpp::Publisher::SharedPtr lbr_position_command_pub_; rclcpp::Subscription::SharedPtr lbr_state_sub_; + bool lbr_state_init_ = false; + lbr_fri_msgs::msg::LBRState lbr_state_; lbr_fri_msgs::msg::LBRPositionCommand lbr_position_command_; }; diff --git a/lbr_demos/lbr_demos_cpp/src/joint_trajectory_client.cpp b/lbr_demos/lbr_demos_cpp/src/joint_trajectory_client.cpp index bfdd0dae..0ec69677 100644 --- a/lbr_demos/lbr_demos_cpp/src/joint_trajectory_client.cpp +++ b/lbr_demos/lbr_demos_cpp/src/joint_trajectory_client.cpp @@ -19,6 +19,11 @@ class JointTrajectoryClient : public rclcpp::Node { this, "joint_trajectory_controller/follow_joint_trajectory"); while (!joint_trajectory_action_client_->wait_for_action_server(std::chrono::seconds(1))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(this->get_logger(), + "Interrupted while waiting for the action server. Exiting."); + return; + } RCLCPP_INFO(this->get_logger(), "Waiting for action server to become available..."); } RCLCPP_INFO(this->get_logger(), "Action server available."); diff --git a/lbr_demos/lbr_demos_cpp/src/torque_sine_overlay.cpp b/lbr_demos/lbr_demos_cpp/src/torque_sine_overlay.cpp index 15d08439..4d33b3b3 100644 --- a/lbr_demos/lbr_demos_cpp/src/torque_sine_overlay.cpp +++ b/lbr_demos/lbr_demos_cpp/src/torque_sine_overlay.cpp @@ -35,7 +35,11 @@ class TorqueSineOverlay { protected: void on_lbr_state_(const lbr_fri_msgs::msg::LBRState::SharedPtr lbr_state) { - lbr_torque_command_.joint_position = lbr_state->ipo_joint_position; + if (!lbr_state_init_) { + lbr_state_ = *lbr_state; + lbr_state_init_ = true; + } + lbr_torque_command_.joint_position = lbr_state_.measured_joint_position; if (lbr_state->session_state == KUKA::FRI::COMMANDING_ACTIVE) { // overlay torque sine wave on 4th joint @@ -55,6 +59,8 @@ class TorqueSineOverlay { double phase_; rclcpp::Publisher::SharedPtr lbr_torque_command_pub_; rclcpp::Subscription::SharedPtr lbr_state_sub_; + bool lbr_state_init_ = false; + lbr_fri_msgs::msg::LBRState lbr_state_; lbr_fri_msgs::msg::LBRTorqueCommand lbr_torque_command_; }; diff --git a/lbr_demos/lbr_demos_cpp/src/wrench_sine_overlay.cpp b/lbr_demos/lbr_demos_cpp/src/wrench_sine_overlay.cpp index 4fa4ce63..68e2e9ff 100644 --- a/lbr_demos/lbr_demos_cpp/src/wrench_sine_overlay.cpp +++ b/lbr_demos/lbr_demos_cpp/src/wrench_sine_overlay.cpp @@ -37,7 +37,11 @@ class WrenchSineOverlay { protected: void on_lbr_state_(const lbr_fri_msgs::msg::LBRState::SharedPtr lbr_state) { - lbr_wrench_command_.joint_position = lbr_state->ipo_joint_position; + if (!lbr_state_init_) { + lbr_state_ = *lbr_state; + lbr_state_init_ = true; + } + lbr_wrench_command_.joint_position = lbr_state_.measured_joint_position; if (lbr_state->session_state == KUKA::FRI::COMMANDING_ACTIVE) { // overlay wrench sine wave on x / y direction @@ -60,6 +64,8 @@ class WrenchSineOverlay { double phase_x_, phase_y_; rclcpp::Publisher::SharedPtr lbr_wrench_command_pub_; rclcpp::Subscription::SharedPtr lbr_state_sub_; + bool lbr_state_init_ = false; + lbr_fri_msgs::msg::LBRState lbr_state_; lbr_fri_msgs::msg::LBRWrenchCommand lbr_wrench_command_; }; diff --git a/lbr_demos/lbr_demos_py/lbr_demos_py/joint_sine_overlay.py b/lbr_demos/lbr_demos_py/lbr_demos_py/joint_sine_overlay.py index 8a07b0b0..91b42c46 100644 --- a/lbr_demos/lbr_demos_py/lbr_demos_py/joint_sine_overlay.py +++ b/lbr_demos/lbr_demos_py/lbr_demos_py/joint_sine_overlay.py @@ -1,6 +1,8 @@ import math +from copy import deepcopy import rclpy +from rcl_interfaces.srv import GetParameters from rclpy.node import Node # import lbr_fri_msgs @@ -24,25 +26,52 @@ def __init__(self, node_name: str) -> None: ) # create subscription to state + self._lbr_state = None self._lbr_state_sub_ = self.create_subscription( LBRState, "state", self._on_lbr_state, 1 ) + # get control rate from controller_manager + self._dt = self._retrieve_update_rate() + def _on_lbr_state(self, lbr_state: LBRState) -> None: - self._lbr_position_command.joint_position = lbr_state.measured_joint_position + if self._lbr_state is None: + self._lbr_state = lbr_state + self._lbr_position_command.joint_position = ( + deepcopy(self._lbr_state.measured_joint_position) + ) if lbr_state.session_state == 4: # KUKA::FRI::COMMANDING_ACTIVE == 4 # overlay sine wave on 4th joint self._lbr_position_command.joint_position[3] += self._amplitude * math.sin( self._phase ) - self._phase += 2 * math.pi * self._frequency * lbr_state.sample_time + self._phase += 2 * math.pi * self._frequency * self._dt self._lbr_position_command_pub.publish(self._lbr_position_command) else: # reset phase self._phase = 0.0 + def _retrieve_update_rate(self) -> float: + paramter_client = self.create_client( + GetParameters, "controller_manager/get_parameters" + ) + paramter_name = "update_rate" + while not paramter_client.wait_for_service(timeout_sec=1.0): + if not rclpy.ok(): + raise RuntimeError("Interrupted while waiting for service.") + self.get_logger().info(f"Waiting for {paramter_client.srv_name}...") + future = paramter_client.call_async( + GetParameters.Request(names=[paramter_name]) + ) + rclpy.spin_until_future_complete(self, future) + if future.result() is None: + raise RuntimeError(f"Failed to get parameter '{paramter_name}'.") + update_rate = future.result().values[0].integer_value + self.get_logger().info(f"{paramter_name}: {update_rate} Hz") + return 1.0 / float(update_rate) + def main(args: list = None) -> None: rclpy.init(args=args) diff --git a/lbr_demos/lbr_demos_py/lbr_demos_py/torque_sine_overlay.py b/lbr_demos/lbr_demos_py/lbr_demos_py/torque_sine_overlay.py index 7abbe9b8..904ee29e 100644 --- a/lbr_demos/lbr_demos_py/lbr_demos_py/torque_sine_overlay.py +++ b/lbr_demos/lbr_demos_py/lbr_demos_py/torque_sine_overlay.py @@ -1,6 +1,8 @@ import math +from copy import deepcopy import rclpy +from rcl_interfaces.srv import GetParameters from rclpy.node import Node # import lbr_fri_msgs @@ -22,23 +24,50 @@ def __init__(self, node_name: str) -> None: ) # create subscription to state + self._lbr_state = None self._lbr_state_sub = self.create_subscription( LBRState, "state", self._on_lbr_state, 1 ) + # get control rate from controller_manager + self._dt = self._retrieve_update_rate() + def _on_lbr_state(self, lbr_state: LBRState) -> None: - self._lbr_torque_command.joint_position = lbr_state.measured_joint_position + if self._lbr_state is None: + self._lbr_state = lbr_state + self._lbr_torque_command.joint_position = deepcopy( + self._lbr_state.measured_joint_position + ) if lbr_state.session_state == 4: # KUKA::FRI::COMMANDING_ACTIVE == 4 # overlay torque sine wave on 4th joint self._lbr_torque_command.torque[3] = self._amplitude * math.sin(self._phase) - self._phase += 2 * math.pi * self._frequency * lbr_state.sample_time + self._phase += 2 * math.pi * self._frequency * self._dt self._lbr_torque_command_pub.publish(self._lbr_torque_command) else: # reset phase self._phase = 0.0 + def _retrieve_update_rate(self) -> float: + paramter_client = self.create_client( + GetParameters, "controller_manager/get_parameters" + ) + paramter_name = "update_rate" + while not paramter_client.wait_for_service(timeout_sec=1.0): + if not rclpy.ok(): + raise RuntimeError("Interrupted while waiting for service.") + self.get_logger().info(f"Waiting for {paramter_client.srv_name}...") + future = paramter_client.call_async( + GetParameters.Request(names=[paramter_name]) + ) + rclpy.spin_until_future_complete(self, future) + if future.result() is None: + raise RuntimeError(f"Failed to get parameter '{paramter_name}'.") + update_rate = future.result().values[0].integer_value + self.get_logger().info(f"{paramter_name}: {update_rate} Hz") + return 1.0 / float(update_rate) + def main(args: list = None) -> None: rclpy.init(args=args) diff --git a/lbr_demos/lbr_demos_py/lbr_demos_py/wrench_sine_overlay.py b/lbr_demos/lbr_demos_py/lbr_demos_py/wrench_sine_overlay.py index 1d8a194f..af1d951d 100644 --- a/lbr_demos/lbr_demos_py/lbr_demos_py/wrench_sine_overlay.py +++ b/lbr_demos/lbr_demos_py/lbr_demos_py/wrench_sine_overlay.py @@ -1,10 +1,12 @@ import math +from copy import deepcopy import rclpy +from rcl_interfaces.srv import GetParameters from rclpy.node import Node # import lbr_fri_msgs -from lbr_fri_msgs.msg import LBRWrenchCommand, LBRState +from lbr_fri_msgs.msg import LBRState, LBRWrenchCommand class WrenchSineOverlayNode(Node): @@ -22,12 +24,20 @@ def __init__(self, node_name: str) -> None: ) # create subscription to state + self._lbr_state = None self._lbr_state_sub = self.create_subscription( LBRState, "state", self._on_lbr_state, 1 ) + # get control rate from controller_manager + self._dt = self._retrieve_update_rate() + def _on_lbr_state(self, lbr_state: LBRState) -> None: - self._lbr_wrench_command.joint_position = lbr_state.measured_joint_position + if self._lbr_state is None: + self._lbr_state = lbr_state + self._lbr_wrench_command.joint_position = deepcopy( + self._lbr_state.measured_joint_position + ) if lbr_state.session_state == 4: # KUKA::FRI::COMMANDING_ACTIVE == 4 # overlay wrench sine wave on x / y direction @@ -37,14 +47,33 @@ def _on_lbr_state(self, lbr_state: LBRState) -> None: self._lbr_wrench_command.wrench[1] = self._amplitude_y * math.sin( self._phase_y ) - self._phase_x += 2 * math.pi * self._frequency_x * lbr_state.sample_time - self._phase_y += 2 * math.pi * self._frequency_y * lbr_state.sample_time + self._phase_x += 2 * math.pi * self._frequency_x * self._dt + self._phase_y += 2 * math.pi * self._frequency_y * self._dt self._lbr_wrench_command_pub.publish(self._lbr_wrench_command) else: # reset phase self._phase_x, self._phase_y = 0.0, 0.0 + def _retrieve_update_rate(self) -> float: + paramter_client = self.create_client( + GetParameters, "controller_manager/get_parameters" + ) + paramter_name = "update_rate" + while not paramter_client.wait_for_service(timeout_sec=1.0): + if not rclpy.ok(): + raise RuntimeError("Interrupted while waiting for service.") + self.get_logger().info(f"Waiting for {paramter_client.srv_name}...") + future = paramter_client.call_async( + GetParameters.Request(names=[paramter_name]) + ) + rclpy.spin_until_future_complete(self, future) + if future.result() is None: + raise RuntimeError(f"Failed to get parameter '{paramter_name}'.") + update_rate = future.result().values[0].integer_value + self.get_logger().info(f"{paramter_name}: {update_rate} Hz") + return 1.0 / float(update_rate) + def main(args: list = None) -> None: rclpy.init(args=args) diff --git a/lbr_ros2_control/config/lbr_system_interface.xacro b/lbr_ros2_control/config/lbr_system_interface.xacro index cd7fe628..63a1c7aa 100644 --- a/lbr_ros2_control/config/lbr_system_interface.xacro +++ b/lbr_ros2_control/config/lbr_system_interface.xacro @@ -21,7 +21,7 @@ port_id:=^|30200 remote_host:=^|INADDR_ANY rt_prio:=^|80 - pid_p:=^|1.0 + pid_p:=^|10.0 pid_i:=^|0.0 pid_d:=^|0.0 pid_i_max:=^|0.0 From 0b8294338c799c37c888e9c104831b220a4b6ccd Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 9 May 2024 14:54:50 +0100 Subject: [PATCH 39/76] fixed formatting --- lbr_demos/lbr_demos_py/lbr_demos_py/joint_sine_overlay.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/lbr_demos/lbr_demos_py/lbr_demos_py/joint_sine_overlay.py b/lbr_demos/lbr_demos_py/lbr_demos_py/joint_sine_overlay.py index 91b42c46..70dc7ee3 100644 --- a/lbr_demos/lbr_demos_py/lbr_demos_py/joint_sine_overlay.py +++ b/lbr_demos/lbr_demos_py/lbr_demos_py/joint_sine_overlay.py @@ -37,8 +37,8 @@ def __init__(self, node_name: str) -> None: def _on_lbr_state(self, lbr_state: LBRState) -> None: if self._lbr_state is None: self._lbr_state = lbr_state - self._lbr_position_command.joint_position = ( - deepcopy(self._lbr_state.measured_joint_position) + self._lbr_position_command.joint_position = deepcopy( + self._lbr_state.measured_joint_position ) if lbr_state.session_state == 4: # KUKA::FRI::COMMANDING_ACTIVE == 4 From 14e8a7cd316d569463dd3ccdbfb5f8a5095380d4 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 9 May 2024 15:30:21 +0100 Subject: [PATCH 40/76] added new prefixes --- .../system_interface_type_values.hpp | 6 ++- .../src/lbr_wrench_command_controller.cpp | 14 +++---- lbr_ros2_control/src/system_interface.cpp | 37 ++++++++++++++++--- 3 files changed, 42 insertions(+), 15 deletions(-) diff --git a/lbr_ros2_control/include/lbr_ros2_control/system_interface_type_values.hpp b/lbr_ros2_control/include/lbr_ros2_control/system_interface_type_values.hpp index 8e898f6c..819e196f 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface_type_values.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface_type_values.hpp @@ -26,7 +26,7 @@ constexpr char HW_IF_EXTERNAL_TORQUE[] = "external_torque"; constexpr char HW_IF_IPO_JOINT_POSITION[] = "ipo_joint_position"; constexpr char HW_IF_TRACKING_PERFORMANCE[] = "tracking_performance"; -// additional estimated force-torque state interface +// additional force-torque command and state interfaces constexpr char HW_IF_FORCE_X[] = "force.x"; constexpr char HW_IF_FORCE_Y[] = "force.y"; constexpr char HW_IF_FORCE_Z[] = "force.z"; @@ -35,6 +35,8 @@ constexpr char HW_IF_TORQUE_Y[] = "torque.y"; constexpr char HW_IF_TORQUE_Z[] = "torque.z"; // additional LBR command interfaces, reference KUKA::FRI::LBRCommand -constexpr char HW_IF_WRENCH[] = "wrench"; +constexpr char HW_IF_WRENCH_PREFIX[] = "wrench"; +constexpr char HW_IF_AUXILIARY_PREFIX[] = "auxiliary_sensor"; +constexpr char HW_IF_ESTIMATED_FT_PREFIX[] = "estimated_ft_sensor"; } // end of namespace lbr_ros2_control #endif // LBR_ROS2_CONTROL__SYSTEM_INTERFACE_TYPE_VALUES_HPP_ diff --git a/lbr_ros2_control/src/lbr_wrench_command_controller.cpp b/lbr_ros2_control/src/lbr_wrench_command_controller.cpp index 3eb4f258..12461925 100644 --- a/lbr_ros2_control/src/lbr_wrench_command_controller.cpp +++ b/lbr_ros2_control/src/lbr_wrench_command_controller.cpp @@ -11,12 +11,12 @@ LBRWrenchCommandController::command_interface_configuration() const { for (const auto &joint_name : joint_names_) { interface_configuration.names.push_back(joint_name + "/" + hardware_interface::HW_IF_POSITION); } - interface_configuration.names.push_back(std::string(HW_IF_WRENCH) + "/" + HW_IF_FORCE_X); - interface_configuration.names.push_back(std::string(HW_IF_WRENCH) + "/" + HW_IF_FORCE_Y); - interface_configuration.names.push_back(std::string(HW_IF_WRENCH) + "/" + HW_IF_FORCE_Z); - interface_configuration.names.push_back(std::string(HW_IF_WRENCH) + "/" + HW_IF_TORQUE_X); - interface_configuration.names.push_back(std::string(HW_IF_WRENCH) + "/" + HW_IF_TORQUE_Y); - interface_configuration.names.push_back(std::string(HW_IF_WRENCH) + "/" + HW_IF_TORQUE_Z); + interface_configuration.names.push_back(std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_FORCE_X); + interface_configuration.names.push_back(std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_FORCE_Y); + interface_configuration.names.push_back(std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_FORCE_Z); + interface_configuration.names.push_back(std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_TORQUE_X); + interface_configuration.names.push_back(std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_TORQUE_Y); + interface_configuration.names.push_back(std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_TORQUE_Z); return interface_configuration; } @@ -83,7 +83,7 @@ bool LBRWrenchCommandController::reference_command_interfaces_() { if (command_interface.get_interface_name() == hardware_interface::HW_IF_POSITION) { joint_position_command_interfaces_.emplace_back(std::ref(command_interface)); } - if (command_interface.get_prefix_name() == HW_IF_WRENCH) { + if (command_interface.get_prefix_name() == HW_IF_WRENCH_PREFIX) { wrench_command_interfaces_.emplace_back(std::ref(command_interface)); } } diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 55905e62..2a1de447 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -203,12 +203,13 @@ std::vector SystemInterface::export_comman } // Cartesian impedance control command interfaces - command_interfaces.emplace_back(HW_IF_WRENCH, HW_IF_FORCE_X, &hw_lbr_command_.wrench[0]); - command_interfaces.emplace_back(HW_IF_WRENCH, HW_IF_FORCE_Y, &hw_lbr_command_.wrench[1]); - command_interfaces.emplace_back(HW_IF_WRENCH, HW_IF_FORCE_Z, &hw_lbr_command_.wrench[2]); - command_interfaces.emplace_back(HW_IF_WRENCH, HW_IF_TORQUE_X, &hw_lbr_command_.wrench[3]); - command_interfaces.emplace_back(HW_IF_WRENCH, HW_IF_TORQUE_Y, &hw_lbr_command_.wrench[4]); - command_interfaces.emplace_back(HW_IF_WRENCH, HW_IF_TORQUE_Z, &hw_lbr_command_.wrench[5]); + const auto &wrench = info_.gpios[0]; + command_interfaces.emplace_back(wrench.name, HW_IF_FORCE_X, &hw_lbr_command_.wrench[0]); + command_interfaces.emplace_back(wrench.name, HW_IF_FORCE_Y, &hw_lbr_command_.wrench[1]); + command_interfaces.emplace_back(wrench.name, HW_IF_FORCE_Z, &hw_lbr_command_.wrench[2]); + command_interfaces.emplace_back(wrench.name, HW_IF_TORQUE_X, &hw_lbr_command_.wrench[3]); + command_interfaces.emplace_back(wrench.name, HW_IF_TORQUE_Y, &hw_lbr_command_.wrench[4]); + command_interfaces.emplace_back(wrench.name, HW_IF_TORQUE_Z, &hw_lbr_command_.wrench[5]); return command_interfaces; } @@ -492,6 +493,14 @@ bool SystemInterface::verify_sensors_() { bool SystemInterface::verify_auxiliary_sensor_() { // check all interfaces are defined in config/lbr_system_interface.xacro const auto &auxiliary_sensor = info_.sensors[0]; + if (auxiliary_sensor.name != HW_IF_AUXILIARY_PREFIX) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + lbr_fri_ros2::ColorScheme::ERROR + << "Sensor '" << auxiliary_sensor.name.c_str() + << "' received invalid name. Expected '" << HW_IF_AUXILIARY_PREFIX + << "'" << lbr_fri_ros2::ColorScheme::ENDC); + return false; + } if (auxiliary_sensor.state_interfaces.size() != AUXILIARY_SENSOR_SIZE) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), lbr_fri_ros2::ColorScheme::ERROR @@ -525,6 +534,14 @@ bool SystemInterface::verify_auxiliary_sensor_() { bool SystemInterface::verify_estimated_ft_sensor_() { const auto &estimated_ft_sensor = info_.sensors[1]; + if (estimated_ft_sensor.name != HW_IF_ESTIMATED_FT_PREFIX) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + lbr_fri_ros2::ColorScheme::ERROR + << "Sensor '" << estimated_ft_sensor.name.c_str() + << "' received invalid name. Expected '" << HW_IF_ESTIMATED_FT_PREFIX + << "'" << lbr_fri_ros2::ColorScheme::ENDC); + return false; + } if (estimated_ft_sensor.state_interfaces.size() != ESTIMATED_FT_SENSOR_SIZE) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), lbr_fri_ros2::ColorScheme::ERROR @@ -558,6 +575,14 @@ bool SystemInterface::verify_gpios_() { << info_.gpios.size() << "'" << lbr_fri_ros2::ColorScheme::ENDC); return false; } + if (info_.gpios[0].name != HW_IF_WRENCH_PREFIX) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + lbr_fri_ros2::ColorScheme::ERROR << "GPIO '" << info_.gpios[0].name.c_str() + << "' received invalid name. Expected '" + << HW_IF_WRENCH_PREFIX << "'" + << lbr_fri_ros2::ColorScheme::ENDC); + return false; + } if (info_.gpios[0].command_interfaces.size() != hw_lbr_command_.wrench.size()) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), lbr_fri_ros2::ColorScheme::ERROR From 5c20aa82b2a1059fc2bad47993fc6c3107a62bb8 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 9 May 2024 16:26:09 +0100 Subject: [PATCH 41/76] velocity limit check via previous position --- .../include/lbr_fri_ros2/command_guard.hpp | 7 +++-- lbr_fri_ros2/src/command_guard.cpp | 26 +++++++++---------- 2 files changed, 18 insertions(+), 15 deletions(-) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp index 19f3e968..b176c349 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include "rclcpp/logger.hpp" @@ -47,7 +48,7 @@ class CommandGuard { CommandGuard() = default; CommandGuard(const CommandGuardParameters &command_guard_parameters); virtual bool is_valid_command(const_idl_command_t_ref lbr_command, - const_fri_state_t_ref lbr_state) const; + const_fri_state_t_ref lbr_state); void log_info() const; @@ -55,11 +56,13 @@ class CommandGuard { virtual bool command_in_position_limits_(const_idl_command_t_ref lbr_command, const_fri_state_t_ref /*lbr_state*/) const; virtual bool command_in_velocity_limits_(const_idl_command_t_ref lbr_command, - const_fri_state_t_ref lbr_state) const; + const_fri_state_t_ref lbr_state); virtual bool command_in_torque_limits_(const_idl_command_t_ref lbr_command, const_fri_state_t_ref lbr_state) const; CommandGuardParameters parameters_; + bool prev_measured_joint_position_init_; + jnt_array_t prev_measured_joint_position_; }; class SafeStopCommandGuard : public CommandGuard { diff --git a/lbr_fri_ros2/src/command_guard.cpp b/lbr_fri_ros2/src/command_guard.cpp index 30b7db1d..425129f9 100644 --- a/lbr_fri_ros2/src/command_guard.cpp +++ b/lbr_fri_ros2/src/command_guard.cpp @@ -5,7 +5,7 @@ CommandGuard::CommandGuard(const CommandGuardParameters &command_guard_parameter : parameters_(command_guard_parameters){}; bool CommandGuard::is_valid_command(const_idl_command_t_ref lbr_command, - const_fri_state_t_ref lbr_state) const { + const_fri_state_t_ref lbr_state) { switch (lbr_state.getClientCommandMode()) { case KUKA::FRI::EClientCommandMode::NO_COMMAND_MODE: return false; @@ -15,6 +15,8 @@ bool CommandGuard::is_valid_command(const_idl_command_t_ref lbr_command, #if FRICLIENT_VERSION_MAJOR == 2 case KUKA::FRI::EClientCommandMode::JOINT_POSITION: #endif + case KUKA::FRI::EClientCommandMode::TORQUE: + case KUKA::FRI::EClientCommandMode::WRENCH: if (!command_in_position_limits_(lbr_command, lbr_state)) { return false; } @@ -26,16 +28,6 @@ bool CommandGuard::is_valid_command(const_idl_command_t_ref lbr_command, case KUKA::FRI::EClientCommandMode::CARTESIAN_POSE: return false; #endif - case KUKA::FRI::EClientCommandMode::WRENCH: - if (!command_in_position_limits_(lbr_command, lbr_state)) { - return false; - } - return true; - case KUKA::FRI::EClientCommandMode::TORQUE: - if (!command_in_position_limits_(lbr_command, lbr_state)) { - return false; - } - return true; default: RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Invalid EClientCommandMode provided"); return false; @@ -71,10 +63,16 @@ bool CommandGuard::command_in_position_limits_(const_idl_command_t_ref lbr_comma } bool CommandGuard::command_in_velocity_limits_(const_idl_command_t_ref lbr_command, - const_fri_state_t_ref lbr_state) const { + const_fri_state_t_ref lbr_state) { const double &dt = lbr_state.getSampleTime(); + if (!prev_measured_joint_position_init_) { + prev_measured_joint_position_init_ = true; + std::memcpy(prev_measured_joint_position_.data(), lbr_state.getMeasuredJointPosition(), + sizeof(double) * KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + return true; + } for (std::size_t i = 0; i < lbr_command.joint_position[i]; ++i) { - if (std::abs(lbr_command.joint_position[i] - lbr_state.getMeasuredJointPosition()[i]) / dt > + if (std::abs(prev_measured_joint_position_[i] - lbr_state.getMeasuredJointPosition()[i]) / dt > parameters_.max_velocity[i]) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), ColorScheme::ERROR << "Velocity not in limits for joint '" @@ -83,6 +81,8 @@ bool CommandGuard::command_in_velocity_limits_(const_idl_command_t_ref lbr_comma return false; } } + std::memcpy(prev_measured_joint_position_.data(), lbr_state.getMeasuredJointPosition(), + sizeof(double) * KUKA::FRI::LBRState::NUMBER_OF_JOINTS); return true; } From 2f74b03a5cdb77b3a9c314ed1bb9818a04684b32 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 9 May 2024 17:11:51 +0100 Subject: [PATCH 42/76] delet app, msgs, fri_vendor --- .../lbr_demos_advanced_cpp/CMakeLists.txt | 2 - lbr_demos/lbr_demos_advanced_cpp/package.xml | 2 +- lbr_demos/lbr_demos_cpp/CMakeLists.txt | 5 - lbr_demos/lbr_demos_cpp/package.xml | 2 +- lbr_fri_msgs/CMakeLists.txt | 37 --- lbr_fri_msgs/msg/LBRCommand.msg | 11 - lbr_fri_msgs/msg/LBRPositionCommand.msg | 6 - lbr_fri_msgs/msg/LBRState.msg | 45 --- lbr_fri_msgs/msg/LBRTorqueCommand.msg | 9 - lbr_fri_msgs/msg/LBRWrenchCommand.msg | 9 - lbr_fri_msgs/package.xml | 25 -- lbr_fri_msgs/srv/AppConnect.srv | 16 - lbr_fri_msgs/srv/AppDisconnect.srv | 10 - lbr_fri_ros2/CMakeLists.txt | 48 --- lbr_fri_ros2/config/app.yaml | 15 - lbr_fri_ros2/launch/app.launch.py | 27 -- lbr_fri_ros2/lbr_fri_ros2/__init__.py | 1 - lbr_fri_ros2/lbr_fri_ros2/launch_mixin.py | 151 --------- lbr_fri_ros2/package.xml | 4 +- lbr_fri_ros2/src/app_component.cpp | 290 ------------------ lbr_fri_ros2/src/app_component.hpp | 94 ------ lbr_ros2_control/CMakeLists.txt | 3 - lbr_ros2_control/package.xml | 2 +- 23 files changed, 4 insertions(+), 810 deletions(-) delete mode 100644 lbr_fri_msgs/CMakeLists.txt delete mode 100644 lbr_fri_msgs/msg/LBRCommand.msg delete mode 100644 lbr_fri_msgs/msg/LBRPositionCommand.msg delete mode 100644 lbr_fri_msgs/msg/LBRState.msg delete mode 100644 lbr_fri_msgs/msg/LBRTorqueCommand.msg delete mode 100644 lbr_fri_msgs/msg/LBRWrenchCommand.msg delete mode 100644 lbr_fri_msgs/package.xml delete mode 100644 lbr_fri_msgs/srv/AppConnect.srv delete mode 100644 lbr_fri_msgs/srv/AppDisconnect.srv delete mode 100644 lbr_fri_ros2/config/app.yaml delete mode 100644 lbr_fri_ros2/launch/app.launch.py delete mode 100644 lbr_fri_ros2/lbr_fri_ros2/__init__.py delete mode 100644 lbr_fri_ros2/lbr_fri_ros2/launch_mixin.py delete mode 100644 lbr_fri_ros2/src/app_component.cpp delete mode 100644 lbr_fri_ros2/src/app_component.hpp diff --git a/lbr_demos/lbr_demos_advanced_cpp/CMakeLists.txt b/lbr_demos/lbr_demos_advanced_cpp/CMakeLists.txt index 85f3490b..60151efb 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/CMakeLists.txt +++ b/lbr_demos/lbr_demos_advanced_cpp/CMakeLists.txt @@ -11,7 +11,6 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) -find_package(fri_vendor REQUIRED) find_package(FRIClient REQUIRED) find_package(geometry_msgs REQUIRED) find_package(kdl_parser REQUIRED) @@ -81,7 +80,6 @@ target_include_directories(pose_control_component ) ament_target_dependencies(pose_control_component - fri_vendor geometry_msgs kdl_parser lbr_fri_msgs diff --git a/lbr_demos/lbr_demos_advanced_cpp/package.xml b/lbr_demos/lbr_demos_advanced_cpp/package.xml index ab16a313..d950d9f6 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/package.xml +++ b/lbr_demos/lbr_demos_advanced_cpp/package.xml @@ -11,7 +11,7 @@ lbr_description - fri_vendor + FRIClient geometry_msgs kdl_parser lbr_fri_ros2 diff --git a/lbr_demos/lbr_demos_cpp/CMakeLists.txt b/lbr_demos/lbr_demos_cpp/CMakeLists.txt index 2fc4d100..6a5afc3f 100644 --- a/lbr_demos/lbr_demos_cpp/CMakeLists.txt +++ b/lbr_demos/lbr_demos_cpp/CMakeLists.txt @@ -12,7 +12,6 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(control_msgs REQUIRED) -find_package(fri_vendor REQUIRED) find_package(FRIClient REQUIRED) find_package(lbr_fri_msgs REQUIRED) find_package(lbr_fri_ros2 REQUIRED) @@ -27,7 +26,6 @@ add_executable(joint_sine_overlay ) ament_target_dependencies(joint_sine_overlay - fri_vendor lbr_fri_msgs lbr_fri_ros2 rclcpp @@ -44,7 +42,6 @@ add_executable(joint_trajectory_client ament_target_dependencies(joint_trajectory_client control_msgs - fri_vendor rclcpp rclcpp_action sensor_msgs @@ -61,7 +58,6 @@ add_executable(torque_sine_overlay ) ament_target_dependencies(torque_sine_overlay - fri_vendor lbr_fri_msgs lbr_fri_ros2 rclcpp @@ -77,7 +73,6 @@ add_executable(wrench_sine_overlay ) ament_target_dependencies(wrench_sine_overlay - fri_vendor lbr_fri_msgs lbr_fri_ros2 rclcpp diff --git a/lbr_demos/lbr_demos_cpp/package.xml b/lbr_demos/lbr_demos_cpp/package.xml index 389f86ed..36d9ca50 100644 --- a/lbr_demos/lbr_demos_cpp/package.xml +++ b/lbr_demos/lbr_demos_cpp/package.xml @@ -10,7 +10,7 @@ ament_cmake control_msgs - fri_vendor + FRIClient lbr_fri_msgs lbr_fri_ros2 rclcpp diff --git a/lbr_fri_msgs/CMakeLists.txt b/lbr_fri_msgs/CMakeLists.txt deleted file mode 100644 index 5a2f44ce..00000000 --- a/lbr_fri_msgs/CMakeLists.txt +++ /dev/null @@ -1,37 +0,0 @@ -cmake_minimum_required(VERSION 3.22) -project(lbr_fri_msgs) - -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -if(NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release CACHE STRING "Build type" FORCE) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(builtin_interfaces REQUIRED) -find_package(rosidl_default_generators REQUIRED) -find_package(std_msgs REQUIRED) - -rosidl_generate_interfaces(lbr_fri_msgs - msg/LBRCommand.msg - msg/LBRPositionCommand.msg - msg/LBRState.msg - msg/LBRTorqueCommand.msg - msg/LBRWrenchCommand.msg - srv/AppConnect.srv - srv/AppDisconnect.srv - DEPENDENCIES builtin_interfaces std_msgs - ADD_LINTER_TESTS -) - -ament_export_dependencies(rosidl_default_runtime) - -ament_package() diff --git a/lbr_fri_msgs/msg/LBRCommand.msg b/lbr_fri_msgs/msg/LBRCommand.msg deleted file mode 100644 index 1bff9ecc..00000000 --- a/lbr_fri_msgs/msg/LBRCommand.msg +++ /dev/null @@ -1,11 +0,0 @@ -# Message that holds commands that are exposed through the -# Fast Robot Interface (FRI). -# Following commands are exposed (refer to KUKA::FRI::LBRCommand in friLBRCommand.h for documentation): -# -# * joint_position [rad] -# * torque [Nm] -# * wrench [N/Nm] - -float64[7] joint_position -float64[7] torque -float64[6] wrench diff --git a/lbr_fri_msgs/msg/LBRPositionCommand.msg b/lbr_fri_msgs/msg/LBRPositionCommand.msg deleted file mode 100644 index d6658de2..00000000 --- a/lbr_fri_msgs/msg/LBRPositionCommand.msg +++ /dev/null @@ -1,6 +0,0 @@ -# Position command for the Fast Robot Interface (FRI). -# Refer to KUKA::FRI::LBRCommand in friLBRCommand.h for documentation. -# -# * joint_position [rad] - -float64[7] joint_position diff --git a/lbr_fri_msgs/msg/LBRState.msg b/lbr_fri_msgs/msg/LBRState.msg deleted file mode 100644 index 43d7de90..00000000 --- a/lbr_fri_msgs/msg/LBRState.msg +++ /dev/null @@ -1,45 +0,0 @@ -# Message that holds states that are exposed through the -# Fast Robot Interface (FRI). -# Following states are exposed (refer to KUKA::FRI::LBRState in friLBRState.h for documentation): -# -# * sample_time [s] -# * session_state [#] -# * connection_quality [#] -# * safety_state [#] -# * operation_mode [#] -# * drive_state [#]: -# * client_command_mode [#] -# * overlay_type [#] -# * control_mode [#] -# -# * time_stamp_sec [s] -# * time_stamp_nano_sec [ns] -# -# * measured_joint_position [rad] -# * commanded_joint_position [rad] -# * measured_torque [Nm] -# * commanded_torque [Nm] -# * external_torque [Nm] -# * ipo_joint_position [rad] -# * tracking_performance [a.u.] - -float64 sample_time -int8 session_state -int8 connection_quality -int8 safety_state -int8 operation_mode -int8 drive_state -int8 client_command_mode -int8 overlay_type -int8 control_mode - -uint32 time_stamp_sec -uint32 time_stamp_nano_sec - -float64[7] measured_joint_position -float64[7] commanded_joint_position -float64[7] measured_torque -float64[7] commanded_torque -float64[7] external_torque -float64[7] ipo_joint_position -float64 tracking_performance diff --git a/lbr_fri_msgs/msg/LBRTorqueCommand.msg b/lbr_fri_msgs/msg/LBRTorqueCommand.msg deleted file mode 100644 index 16fe41cf..00000000 --- a/lbr_fri_msgs/msg/LBRTorqueCommand.msg +++ /dev/null @@ -1,9 +0,0 @@ -# Torque command for the Fast Robot Interface (FRI). -# Requires joint position and torque overlay. -# Refer to KUKA::FRI::LBRCommand in friLBRCommand.h for documentation. -# -# * joint_position [rad] -# * torque [Nm] - -float64[7] joint_position -float64[7] torque diff --git a/lbr_fri_msgs/msg/LBRWrenchCommand.msg b/lbr_fri_msgs/msg/LBRWrenchCommand.msg deleted file mode 100644 index 9a634317..00000000 --- a/lbr_fri_msgs/msg/LBRWrenchCommand.msg +++ /dev/null @@ -1,9 +0,0 @@ -# Wrench command for the Fast Robot Interface (FRI). -# Requires joint position and wrench overlay. -# Refer to KUKA::FRI::LBRCommand in friLBRCommand.h for documentation. -# -# * joint_position [rad] -# * wrench [N/Nm] - -float64[7] joint_position -float64[6] wrench diff --git a/lbr_fri_msgs/package.xml b/lbr_fri_msgs/package.xml deleted file mode 100644 index 036f31ff..00000000 --- a/lbr_fri_msgs/package.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - - lbr_fri_msgs - 1.4.3 - ROS 2 message for the Fast Robot Interface (FRI) specific states. - mhubii - Apache License 2.0 - - ament_cmake - rosidl_default_generators - - builtin_interfaces - std_msgs - - rosidl_default_runtime - - ament_lint_common - - rosidl_interface_packages - - - ament_cmake - - \ No newline at end of file diff --git a/lbr_fri_msgs/srv/AppConnect.srv b/lbr_fri_msgs/srv/AppConnect.srv deleted file mode 100644 index c4d851a3..00000000 --- a/lbr_fri_msgs/srv/AppConnect.srv +++ /dev/null @@ -1,16 +0,0 @@ -# This service message is used to call the Fast Robot Interface's (FRI) -# KUKA::FRI::ClientApplication::connect method via a service call. -# -# * port [int32]: The port ID. -# * remote_host [string]: The address of the remote host. -# --- -# * connected [bool]: True when connection established successfully. -# * message [string]: Informational message, containing e.g. errors. - -int32 port_id -string remote_host -uint8 rt_prio -uint8 max_attempts ---- -bool connected -string message diff --git a/lbr_fri_msgs/srv/AppDisconnect.srv b/lbr_fri_msgs/srv/AppDisconnect.srv deleted file mode 100644 index cab70b1a..00000000 --- a/lbr_fri_msgs/srv/AppDisconnect.srv +++ /dev/null @@ -1,10 +0,0 @@ -# This service message is used to call the Fast Robot Interface's (FRI) -# KUKA::FRI::ClientApplication::disconnect method via a service call. -# -# --- -# * disconnected [bool]: True when connection closed successfully. -# * message [string]: Informational message, containing e.g. errors. - ---- -bool disconnected -string message diff --git a/lbr_fri_ros2/CMakeLists.txt b/lbr_fri_ros2/CMakeLists.txt index 189794d6..b6c9c3bc 100644 --- a/lbr_fri_ros2/CMakeLists.txt +++ b/lbr_fri_ros2/CMakeLists.txt @@ -11,17 +11,14 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) -find_package(ament_cmake_python REQUIRED) find_package(control_toolbox REQUIRED) find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3) -find_package(fri_vendor REQUIRED) find_package(FRIClient REQUIRED) find_package(kdl_parser REQUIRED) find_package(lbr_fri_msgs REQUIRED) find_package(orocos_kdl_vendor REQUIRED) find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) find_package(realtime_tools REQUIRED) find_package(urdf REQUIRED) @@ -62,7 +59,6 @@ ament_export_dependencies( control_toolbox eigen3_cmake_module Eigen3 - fri_vendor FRIClient kdl_parser lbr_fri_msgs @@ -82,48 +78,4 @@ install( LIBRARY DESTINATION lib ) -install( - DIRECTORY launch - DESTINATION share/lbr_fri_ros2 -) - -# app_component -add_library(app_component - SHARED - src/app_component.cpp -) - -target_include_directories(app_component - PRIVATE src -) - -ament_target_dependencies(app_component - rclcpp - urdf - rclcpp_components -) - -target_link_libraries(app_component - lbr_fri_ros2 -) - -rclcpp_components_register_node(app_component - PLUGIN lbr_fri_ros2::AppComponent - EXECUTABLE app -) - -install( - TARGETS app_component - LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/lbr_fri_ros2 -) - -install( - DIRECTORY config launch - DESTINATION share/lbr_fri_ros2 -) - -# Launch mixins -ament_python_install_package(${PROJECT_NAME}) - ament_package() diff --git a/lbr_fri_ros2/config/app.yaml b/lbr_fri_ros2/config/app.yaml deleted file mode 100644 index 7b34cbc6..00000000 --- a/lbr_fri_ros2/config/app.yaml +++ /dev/null @@ -1,15 +0,0 @@ -app: - ros__parameters: - port_id: 30200 # valid in range [30200, 30209], usefull for multi-robot setup - remote_host: null # if null, any IP is accepted - rt_prio: 80 # real-time priority of the FRI thread - pid.p: 1.0 # proportional gain on joint position, which will be scaled by sample time - pid.i: 0.0 # integral gain on joint position, which will be scaled by sample time - pid.d: 0.0 # derivative gain on joint position, which will be scaled by sample time - pid.i_max: 0.0 # maximum integral term - pid.i_min: 0.0 # minimum integral term - pid.antiwindup: false # if true, anti-windup is enabled - command_guard_variant: "safe_stop" # ["default", "safe_stop"] - default uses exact position limits, safe_stop uses safe zone - external_torque_cutoff_frequency: 10.0 # cutoff frequency of the low-pass filter for the external torque in Hz - measured_torque_cutoff_frequency: 10.0 # cutoff frequency of the low-pass filter for the measured torque in Hz - open_loop: true # if true, the robot is controlled in open loop mode diff --git a/lbr_fri_ros2/launch/app.launch.py b/lbr_fri_ros2/launch/app.launch.py deleted file mode 100644 index c9b0e211..00000000 --- a/lbr_fri_ros2/launch/app.launch.py +++ /dev/null @@ -1,27 +0,0 @@ -from launch import LaunchDescription - -from lbr_description import LBRDescriptionMixin -from lbr_fri_ros2 import LBRFRIROS2Mixin - - -def generate_launch_description() -> LaunchDescription: - ld = LaunchDescription() - ld.add_action(LBRDescriptionMixin.arg_model()) - ld.add_action(LBRDescriptionMixin.arg_robot_name()) - robot_description = LBRDescriptionMixin.param_robot_description(sim=False) - ld.add_action(LBRFRIROS2Mixin.arg_open_loop()) - ld.add_action(LBRFRIROS2Mixin.arg_rt_prio()) - ld.add_action(LBRFRIROS2Mixin.arg_pid_p()) - ld.add_action(LBRFRIROS2Mixin.arg_port_id()) - ld.add_action( - LBRFRIROS2Mixin.node_app( - parameters=[ - robot_description, - LBRFRIROS2Mixin.param_open_loop(), - LBRFRIROS2Mixin.param_rt_prio(), - LBRFRIROS2Mixin.param_pid_p(), - LBRFRIROS2Mixin.param_port_id(), - ] - ) - ) - return ld diff --git a/lbr_fri_ros2/lbr_fri_ros2/__init__.py b/lbr_fri_ros2/lbr_fri_ros2/__init__.py deleted file mode 100644 index 4561623e..00000000 --- a/lbr_fri_ros2/lbr_fri_ros2/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from .launch_mixin import LBRFRIROS2Mixin diff --git a/lbr_fri_ros2/lbr_fri_ros2/launch_mixin.py b/lbr_fri_ros2/lbr_fri_ros2/launch_mixin.py deleted file mode 100644 index 1c74aa13..00000000 --- a/lbr_fri_ros2/lbr_fri_ros2/launch_mixin.py +++ /dev/null @@ -1,151 +0,0 @@ -from typing import Dict, Optional, Union - -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - - -class LBRFRIROS2Mixin: - @staticmethod - def arg_command_guard_variant() -> DeclareLaunchArgument: - return DeclareLaunchArgument( - name="command_guard_variant", - default_value="safe_stop", - description="Command guard variant.", - choices=["default", "safe_stop"], - ) - - @staticmethod - def arg_open_loop() -> DeclareLaunchArgument: - return DeclareLaunchArgument( - name="open_loop", - default_value="true", - description="Open loop control. Works best for LBRs. Should only be set to false by experienced users.", - ) - - @staticmethod - def arg_rt_prio() -> DeclareLaunchArgument: - return DeclareLaunchArgument( - name="rt_prio", - default_value="80", - description="Realtime priority of the FRI thread. Realtime kernel required.\n" - "\tRequires configuration in /etc/security/limits.conf. Add the line:\n" - "\t'user - rtprio 99', where user is your username.", - ) - - @staticmethod - def arg_pid_p() -> DeclareLaunchArgument: - return DeclareLaunchArgument( - name="pid.p", - default_value="1.0", - description="Joint position PID controller proportional gain.", - ) - - @staticmethod - def arg_pid_i() -> DeclareLaunchArgument: - return DeclareLaunchArgument( - name="pid.i", - default_value="0.0", - description="Joint position PID controller integral gain.", - ) - - @staticmethod - def arg_pid_d() -> DeclareLaunchArgument: - return DeclareLaunchArgument( - name="pid.d", - default_value="1.0", - description="Joint position PID controller derivative gain.", - ) - - @staticmethod - def arg_pid_i_max() -> DeclareLaunchArgument: - return DeclareLaunchArgument( - name="pid.i_max", - default_value="0.0", - description="Joint position PID controller maximum integral value.", - ) - - @staticmethod - def arg_pid_i_min() -> DeclareLaunchArgument: - return DeclareLaunchArgument( - name="pid.i_min", - default_value="0.0", - description="Joint position PID controller minimum integral value.", - ) - - @staticmethod - def arg_pid_antiwindup() -> DeclareLaunchArgument: - return DeclareLaunchArgument( - name="pid.antiwindup", - default_value="0.0", - description="Joint position PID controller antiwindup.", - ) - - @staticmethod - def arg_port_id() -> DeclareLaunchArgument: - return DeclareLaunchArgument( - name="port_id", - default_value="30200", - description="Port ID of the FRI communication. Valid in range [30200, 30209].\n" - "\tUsefull in multi-robot setups.", - ) - - @staticmethod - def param_command_guard_variant() -> Dict[str, LaunchConfiguration]: - return { - "command_guard_variant": LaunchConfiguration( - "command_guard_variant", default="safe_stop" - ) - } - - @staticmethod - def param_open_loop() -> Dict[str, LaunchConfiguration]: - return {"open_loop": LaunchConfiguration("open_loop", default="true")} - - @staticmethod - def param_rt_prio() -> Dict[str, LaunchConfiguration]: - return {"rt_prio": LaunchConfiguration("rt_prio", default="80")} - - @staticmethod - def param_pid_p() -> Dict[str, LaunchConfiguration]: - return {"pid.p": LaunchConfiguration("pid.p", default="1.0")} - - @staticmethod - def param_pid_i() -> Dict[str, LaunchConfiguration]: - return {"pid.i": LaunchConfiguration("pid.i", default="0.0")} - - @staticmethod - def param_pid_d() -> Dict[str, LaunchConfiguration]: - return {"pid.d": LaunchConfiguration("pid.d", default="0.0")} - - @staticmethod - def param_pid_i_max() -> Dict[str, LaunchConfiguration]: - return {"pid.i_max": LaunchConfiguration("pid.i_max", default="0.0")} - - @staticmethod - def param_pid_i_min() -> Dict[str, LaunchConfiguration]: - return {"pid.i_min": LaunchConfiguration("pid.i_min", default="0.0")} - - @staticmethod - def param_pid_antiwindup() -> Dict[str, LaunchConfiguration]: - return {"pid.antiwindup": LaunchConfiguration("pid.antiwindup", default="0.0")} - - @staticmethod - def param_port_id() -> Dict[str, LaunchConfiguration]: - return {"port_id": LaunchConfiguration("port_id", default="30200")} - - @staticmethod - def node_app( - robot_name: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( - "robot_name", default="lbr" - ), - **kwargs - ) -> DeclareLaunchArgument: - return Node( - package="lbr_fri_ros2", - executable="app", - namespace=robot_name, - emulate_tty=True, - output="screen", - **kwargs, - ) diff --git a/lbr_fri_ros2/package.xml b/lbr_fri_ros2/package.xml index 350bfb97..3cd4bccb 100644 --- a/lbr_fri_ros2/package.xml +++ b/lbr_fri_ros2/package.xml @@ -9,18 +9,16 @@ Apache License 2.0 ament_cmake - ament_cmake_python eigen3_cmake_module eigen control_toolbox - fri_vendor + FRIClient kdl_parser lbr_fri_msgs orocos_kdl_vendor rclcpp - rclcpp_components realtime_tools urdf diff --git a/lbr_fri_ros2/src/app_component.cpp b/lbr_fri_ros2/src/app_component.cpp deleted file mode 100644 index 6ab7a4e2..00000000 --- a/lbr_fri_ros2/src/app_component.cpp +++ /dev/null @@ -1,290 +0,0 @@ -#include "app_component.hpp" - -namespace lbr_fri_ros2 { -AppComponent::AppComponent(const rclcpp::NodeOptions &options) { - app_node_ptr_ = std::make_shared("app", options); - - app_node_ptr_->declare_parameter("port_id", 30200); - app_node_ptr_->declare_parameter("remote_host", std::string("")); - app_node_ptr_->declare_parameter("rt_prio", 80); - app_node_ptr_->declare_parameter("robot_description", std::string("")); - app_node_ptr_->declare_parameter("pid.p", 1.0); - app_node_ptr_->declare_parameter("pid.i", 0.0); - app_node_ptr_->declare_parameter("pid.d", 0.0); - app_node_ptr_->declare_parameter("pid.i_max", 0.0); - app_node_ptr_->declare_parameter("pid.i_min", 0.0); - app_node_ptr_->declare_parameter("pid.antiwindup", false); - app_node_ptr_->declare_parameter("command_guard_variant", std::string("safe_stop")); - app_node_ptr_->declare_parameter("external_torque_cutoff_frequency", 10.); - app_node_ptr_->declare_parameter("measured_torque_cutoff_frequency", 10.); - app_node_ptr_->declare_parameter("open_loop", true); - - // prepare parameters - PIDParameters pid_parameters; - pid_parameters.p = app_node_ptr_->get_parameter("pid.p").as_double(); - pid_parameters.i = app_node_ptr_->get_parameter("pid.i").as_double(); - pid_parameters.d = app_node_ptr_->get_parameter("pid.d").as_double(); - pid_parameters.i_max = app_node_ptr_->get_parameter("pid.i_max").as_double(); - pid_parameters.i_min = app_node_ptr_->get_parameter("pid.i_min").as_double(); - pid_parameters.antiwindup = app_node_ptr_->get_parameter("pid.antiwindup").as_bool(); - CommandGuardParameters command_guard_parameters; - std::string command_guard_variant = - app_node_ptr_->get_parameter("command_guard_variant").as_string(); - StateInterfaceParameters state_interface_parameters; - state_interface_parameters.external_torque_cutoff_frequency = - app_node_ptr_->get_parameter("external_torque_cutoff_frequency").as_double(); - state_interface_parameters.measured_torque_cutoff_frequency = - app_node_ptr_->get_parameter("measured_torque_cutoff_frequency").as_double(); - bool open_loop = app_node_ptr_->get_parameter("open_loop").as_bool(); - - // load robot description and parse limits - auto robot_description_param = app_node_ptr_->get_parameter("robot_description"); - urdf::Model model; - if (!model.initString(robot_description_param.as_string())) { - std::string err = "Failed to intialize urdf model from '" + robot_description_param.get_name() + - "' parameter."; - RCLCPP_ERROR(app_node_ptr_->get_logger(), err.c_str()); - throw std::runtime_error(err); - } - - std::size_t jnt_cnt = 0; - for (const auto &name_joint_pair : model.joints_) { - const auto joint = name_joint_pair.second; - if (joint->type == urdf::Joint::REVOLUTE) { - if (jnt_cnt >= command_guard_parameters.joint_names.size()) { - std::string errs = - "Found too many joints in '" + robot_description_param.get_name() + "' parameter."; - RCLCPP_ERROR(app_node_ptr_->get_logger(), errs.c_str()); - throw std::runtime_error(errs); - } - command_guard_parameters.joint_names[jnt_cnt] = name_joint_pair.first; - command_guard_parameters.min_position[jnt_cnt] = joint->limits->lower; - command_guard_parameters.max_position[jnt_cnt] = joint->limits->upper; - command_guard_parameters.max_velocity[jnt_cnt] = joint->limits->velocity; - command_guard_parameters.max_torque[jnt_cnt] = joint->limits->effort; - ++jnt_cnt; - } - } - - // configure client - async_client_ptr_ = - std::make_shared(pid_parameters, command_guard_parameters, command_guard_variant, - state_interface_parameters, open_loop); - app_ptr_ = std::make_unique(async_client_ptr_); - - // default connect - connect_(app_node_ptr_->get_parameter("port_id").as_int(), - app_node_ptr_->get_parameter("remote_host").as_string().empty() - ? NULL - : app_node_ptr_->get_parameter("remote_host").as_string().c_str(), - app_node_ptr_->get_parameter("rt_prio").as_int()); - - // services - app_connect_srv_ = app_node_ptr_->create_service( - "app/connect", std::bind(&AppComponent::on_app_connect_, this, std::placeholders::_1, - std::placeholders::_2)); - app_disconnect_srv_ = app_node_ptr_->create_service( - "app/disconnect", std::bind(&AppComponent::on_app_disconnect_, this, std::placeholders::_1, - std::placeholders::_2)); -} - -rclcpp::node_interfaces::NodeBaseInterface::SharedPtr -AppComponent::get_node_base_interface() const { - return app_node_ptr_->get_node_base_interface(); -} - -void AppComponent::connect_(const int &port_id, const char *const remote_host, - const uint8_t &rt_prio, const uint8_t &max_attempts) { - if (!app_ptr_->open_udp_socket(port_id, remote_host)) { - return; - }; - app_ptr_->run(rt_prio); - uint8_t attempt = 0; - while (!async_client_ptr_->get_state_interface().is_initialized() && rclcpp::ok()) { - RCLCPP_INFO(app_node_ptr_->get_logger(), "Waiting for robot heartbeat [%d/%d]. Port ID: %d.", - attempt + 1, max_attempts, port_id); - std::this_thread::sleep_for(std::chrono::seconds(1)); - if (++attempt >= max_attempts) { - app_ptr_->close_udp_socket(); - RCLCPP_ERROR(app_node_ptr_->get_logger(), "Failed to connect to robot on max attempts."); - return; - } - } - RCLCPP_INFO(app_node_ptr_->get_logger(), "Robot connected."); - - RCLCPP_INFO( - app_node_ptr_->get_logger(), "Control mode: '%s'.", - EnumMaps::control_mode_map(async_client_ptr_->get_state_interface().get_state().control_mode) - .c_str()); - RCLCPP_INFO(app_node_ptr_->get_logger(), "Sample time: %.3f s / %.1f Hz.", - async_client_ptr_->get_state_interface().get_state().sample_time, - 1. / async_client_ptr_->get_state_interface().get_state().sample_time); - - // publisher - state_pub_ = app_node_ptr_->create_publisher("state", 1); - state_pub_timer_ = app_node_ptr_->create_wall_timer( - std::chrono::milliseconds(static_cast( - async_client_ptr_->get_state_interface().get_state().sample_time * 1.e3)), - std::bind(&AppComponent::on_state_pub_timer_, this)); - - // await commanding active thread - std::thread await_commanding_active_thread([this]() { - while (async_client_ptr_->get_state_interface().get_state().session_state != - KUKA::FRI::ESessionState::COMMANDING_ACTIVE && - rclcpp::ok()) { - RCLCPP_INFO(app_node_ptr_->get_logger(), "Waiting for robot to enter '%s' state.", - EnumMaps::session_state_map(KUKA::FRI::ESessionState::COMMANDING_ACTIVE).c_str()); - std::this_thread::sleep_for(std::chrono::seconds(2)); - } - - RCLCPP_INFO(app_node_ptr_->get_logger(), "AsyncClient command mode: '%s'.", - EnumMaps::client_command_mode_map( - async_client_ptr_->get_state_interface().get_state().client_command_mode) - .c_str()); - - // subscriptions - switch (async_client_ptr_->get_state_interface().get_state().client_command_mode) { -#if FRICLIENT_VERSION_MAJOR == 1 - case KUKA::FRI::EClientCommandMode::POSITION: -#endif -#if FRICLIENT_VERSION_MAJOR == 2 - case KUKA::FRI::EClientCommandMode::JOINT_POSITION: -#endif - position_command_sub_ = - app_node_ptr_->create_subscription( - "command/joint_position", 1, - std::bind(&AppComponent::on_position_command_, this, std::placeholders::_1)); - break; - case KUKA::FRI::EClientCommandMode::TORQUE: - torque_command_sub_ = app_node_ptr_->create_subscription( - "command/torque", 1, - std::bind(&AppComponent::on_torque_command_, this, std::placeholders::_1)); - break; - case KUKA::FRI::EClientCommandMode::WRENCH: - wrench_command_sub_ = app_node_ptr_->create_subscription( - "command/wrench", 1, - std::bind(&AppComponent::on_wrench_command_, this, std::placeholders::_1)); - break; - default: - break; - } - }); - await_commanding_active_thread.detach(); -} - -void AppComponent::on_position_command_( - const lbr_fri_msgs::msg::LBRPositionCommand::SharedPtr lbr_position_command) { -#if FRICLIENT_VERSION_MAJOR == 1 - if (!on_command_checks_(KUKA::FRI::EClientCommandMode::POSITION)) -#endif -#if FRICLIENT_VERSION_MAJOR == 2 - if (!on_command_checks_(KUKA::FRI::EClientCommandMode::JOINT_POSITION)) -#endif - { - return; - } - - if (async_client_ptr_->get_state_interface().get_state().session_state == - KUKA::FRI::ESessionState::COMMANDING_ACTIVE) { - lbr_command_.joint_position = lbr_position_command->joint_position; - async_client_ptr_->get_command_interface().set_command_target(lbr_command_); - return; - } - - // if not commanding active, reset - lbr_command_.joint_position = - async_client_ptr_->get_state_interface().get_state().measured_joint_position; -} - -void AppComponent::on_torque_command_( - const lbr_fri_msgs::msg::LBRTorqueCommand::SharedPtr lbr_torque_command) { - if (!on_command_checks_(KUKA::FRI::EClientCommandMode::TORQUE)) { - return; - } - - if (async_client_ptr_->get_state_interface().get_state().session_state == - KUKA::FRI::ESessionState::COMMANDING_ACTIVE) { - lbr_command_.joint_position = lbr_torque_command->joint_position; - lbr_command_.torque = lbr_torque_command->torque; - async_client_ptr_->get_command_interface().set_command_target(lbr_command_); - return; - } - - // if not active, reset - lbr_command_.joint_position = - async_client_ptr_->get_state_interface().get_state().measured_joint_position; - std::fill(lbr_command_.torque.begin(), lbr_command_.torque.end(), 0.0); -} - -void AppComponent::on_wrench_command_( - const lbr_fri_msgs::msg::LBRWrenchCommand::SharedPtr lbr_wrench_command) { - if (!on_command_checks_(KUKA::FRI::EClientCommandMode::WRENCH)) { - return; - } - - if (async_client_ptr_->get_state_interface().get_state().session_state == - KUKA::FRI::ESessionState::COMMANDING_ACTIVE) { - lbr_command_.joint_position = lbr_wrench_command->joint_position; - lbr_command_.wrench = lbr_wrench_command->wrench; - async_client_ptr_->get_command_interface().set_command_target(lbr_command_); - return; - } - - // if not active, reset - lbr_command_.joint_position = - async_client_ptr_->get_state_interface().get_state().measured_joint_position; - std::fill(lbr_command_.wrench.begin(), lbr_command_.wrench.end(), 0.0); -} - -bool AppComponent::on_command_checks_(const int &expected_command_mode) { - if (!async_client_ptr_) { - RCLCPP_ERROR(app_node_ptr_->get_logger(), "AsyncClient not configured."); - return false; - } - if (async_client_ptr_->get_state_interface().get_state().client_command_mode == - KUKA::FRI::EClientCommandMode::NO_COMMAND_MODE) { - return false; - } - if (async_client_ptr_->get_state_interface().get_state().client_command_mode != - expected_command_mode) { - RCLCPP_ERROR(app_node_ptr_->get_logger(), - "Wrench command only allowed in wrench command mode."); - return false; - } - return true; -} - -void AppComponent::on_state_pub_timer_() { - state_pub_->publish(async_client_ptr_->get_state_interface().get_state()); -} - -void AppComponent::on_app_connect_(const lbr_fri_msgs::srv::AppConnect::Request::SharedPtr request, - lbr_fri_msgs::srv::AppConnect::Response::SharedPtr response) { - RCLCPP_INFO(app_node_ptr_->get_logger(), - "Connecting to robot via service. Port ID: %d, remote host: '%s'.", request->port_id, - request->remote_host.c_str()); - connect_(request->port_id, request->remote_host.empty() ? NULL : request->remote_host.c_str(), - request->rt_prio, request->max_attempts); - response->connected = async_client_ptr_->get_state_interface().is_initialized(); - response->message = response->connected ? "Robot connected." : "Failed."; -} - -void AppComponent::on_app_disconnect_( - const lbr_fri_msgs::srv::AppDisconnect::Request::SharedPtr /*request*/, - lbr_fri_msgs::srv::AppDisconnect::Response::SharedPtr response) { - RCLCPP_INFO(app_node_ptr_->get_logger(), "Disconnecting from robot via service."); - app_ptr_->stop_run(); - response->disconnected = app_ptr_->close_udp_socket(); - state_pub_timer_.reset(); - state_pub_.reset(); - position_command_sub_.reset(); - torque_command_sub_.reset(); - wrench_command_sub_.reset(); - response->message = response->disconnected ? "Robot disconnected." : "Failed."; - RCLCPP_INFO(app_node_ptr_->get_logger(), response->message.c_str()); -} -} // end of namespace lbr_fri_ros2 - -#include "rclcpp_components/register_node_macro.hpp" - -RCLCPP_COMPONENTS_REGISTER_NODE(lbr_fri_ros2::AppComponent) diff --git a/lbr_fri_ros2/src/app_component.hpp b/lbr_fri_ros2/src/app_component.hpp deleted file mode 100644 index db5dba7b..00000000 --- a/lbr_fri_ros2/src/app_component.hpp +++ /dev/null @@ -1,94 +0,0 @@ -#ifndef LBR_FRI_ROS2__APP_COMPONENT_HPP_ -#define LBR_FRI_ROS2__APP_COMPONENT_HPP_ - -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "urdf/model.h" - -#include "friVersion.h" - -#include "lbr_fri_msgs/msg/lbr_position_command.hpp" -#include "lbr_fri_msgs/msg/lbr_state.hpp" -#include "lbr_fri_msgs/msg/lbr_torque_command.hpp" -#include "lbr_fri_msgs/msg/lbr_wrench_command.hpp" -#include "lbr_fri_msgs/srv/app_connect.hpp" -#include "lbr_fri_msgs/srv/app_disconnect.hpp" -#include "lbr_fri_ros2/app.hpp" -#include "lbr_fri_ros2/async_client.hpp" -#include "lbr_fri_ros2/command_guard.hpp" -#include "lbr_fri_ros2/formatting.hpp" -#include "lbr_fri_ros2/filters.hpp" -#include "lbr_fri_ros2/state_interface.hpp" - -namespace lbr_fri_ros2 { -/** - * @brief Component for instantiating #lbr_fri_ros2::App. - * - */ -class AppComponent { -public: - /** - * @brief Construct a new AppComponent object. - * - * @param options Node options - */ - AppComponent(const rclcpp::NodeOptions &options); - - /** - * @brief Get the node base interface object. Implementing this is necessary for components via - * composition. - * - * @return rclcpp::node_interfaces::NodeBaseInterface::SharedPtr - */ - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() const; - -protected: - void connect_(const int &port_id = 30200, const char *const remote_host = NULL, - const uint8_t &rt_prio = 80, const uint8_t &max_attempts = 10); - - // command buffer - lbr_fri_msgs::msg::LBRCommand lbr_command_; /**< The command to be sent to the robot.*/ - - // command subscriptions - rclcpp::Subscription::SharedPtr - position_command_sub_; /**< Subscribes to position commands.*/ - rclcpp::Subscription::SharedPtr - torque_command_sub_; /**< Subscribes to torque commands.*/ - rclcpp::Subscription::SharedPtr - wrench_command_sub_; /**< Subscribes to wrench commands.*/ - - void - on_position_command_(const lbr_fri_msgs::msg::LBRPositionCommand::SharedPtr lbr_position_command); - void on_torque_command_(const lbr_fri_msgs::msg::LBRTorqueCommand::SharedPtr lbr_torque_command); - void on_wrench_command_(const lbr_fri_msgs::msg::LBRWrenchCommand::SharedPtr lbr_wrench_command); - bool on_command_checks_(const int &expected_command_mode); - - // state publisher - rclcpp::Publisher::SharedPtr - state_pub_; /**< Publishes the robot state.*/ - rclcpp::TimerBase::SharedPtr state_pub_timer_; /**< Timer for publishing the robot state.*/ - void on_state_pub_timer_(); /**< Callback for publishing the robot state.*/ - - // services - rclcpp::Service::SharedPtr - app_connect_srv_; /**< Service to connect to robot via #on_app_connect_ callback.*/ - rclcpp::Service::SharedPtr - app_disconnect_srv_; /**< Service to disconnect from robot via #on_app_disconnect_ - callback.*/ - - void on_app_connect_(const lbr_fri_msgs::srv::AppConnect::Request::SharedPtr request, - lbr_fri_msgs::srv::AppConnect::Response::SharedPtr response); - - void on_app_disconnect_(const lbr_fri_msgs::srv::AppDisconnect::Request::SharedPtr request, - lbr_fri_msgs::srv::AppDisconnect::Response::SharedPtr response); - - // app - rclcpp::Node::SharedPtr app_node_ptr_; /** Node for communicating with ROS.<*/ - std::shared_ptr async_client_ptr_; - std::unique_ptr app_ptr_; /** #lbr_fri_ros2::App for communicating with the hardware.<*/ -}; -} // end of namespace lbr_fri_ros2 -#endif // LBR_FRI_ROS2__LBR_COMPONENT_HPP_ diff --git a/lbr_ros2_control/CMakeLists.txt b/lbr_ros2_control/CMakeLists.txt index 293660a2..946a581b 100644 --- a/lbr_ros2_control/CMakeLists.txt +++ b/lbr_ros2_control/CMakeLists.txt @@ -17,7 +17,6 @@ endif() find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) find_package(controller_interface REQUIRED) -find_package(fri_vendor REQUIRED) find_package(FRIClient REQUIRED) find_package(hardware_interface REQUIRED) find_package(lbr_fri_msgs REQUIRED) @@ -52,7 +51,6 @@ target_include_directories( ament_target_dependencies( ${PROJECT_NAME} controller_interface - fri_vendor hardware_interface lbr_fri_msgs lbr_fri_ros2 @@ -76,7 +74,6 @@ ament_export_targets( ament_export_dependencies( controller_interface - fri_vendor FRIClient hardware_interface lbr_fri_msgs diff --git a/lbr_ros2_control/package.xml b/lbr_ros2_control/package.xml index 35510f15..264ebd02 100644 --- a/lbr_ros2_control/package.xml +++ b/lbr_ros2_control/package.xml @@ -10,7 +10,7 @@ ament_cmake ament_cmake_python - fri_vendor + FRIClient lbr_fri_msgs lbr_fri_ros2 pluginlib From ee70180f4afc6bf3ec1758f1a2a007a27e4ce103 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 9 May 2024 17:30:25 +0100 Subject: [PATCH 43/76] updated repos --- .github/workflows/build.yml | 3 ++- README.md | 2 +- lbr_fri_ros2_stack/{fri-1.14.repos => repos-fri-1.14.yaml} | 0 lbr_fri_ros2_stack/{fri-1.15.repos => repos-fri-1.15.yaml} | 0 lbr_fri_ros2_stack/{fri-1.16.repos => repos-fri-1.16.yaml} | 0 lbr_fri_ros2_stack/{fri-2.5.repos => repos-fri-2.5.yaml} | 0 6 files changed, 3 insertions(+), 2 deletions(-) rename lbr_fri_ros2_stack/{fri-1.14.repos => repos-fri-1.14.yaml} (100%) rename lbr_fri_ros2_stack/{fri-1.15.repos => repos-fri-1.15.yaml} (100%) rename lbr_fri_ros2_stack/{fri-1.16.repos => repos-fri-1.16.yaml} (100%) rename lbr_fri_ros2_stack/{fri-2.5.repos => repos-fri-2.5.yaml} (100%) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 1aa60da2..e1f52824 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -15,10 +15,11 @@ jobs: strategy: matrix: os: [ubuntu-latest] + fri_version: [1.11, 1.14, 1.15, 1.16, 2.5] steps: - uses: ros-tooling/setup-ros@v0.7 - uses: ros-tooling/action-ros-ci@v0.2 with: package-name: lbr_fri_ros2_stack target-ros2-distro: humble - vcs-repo-file-url: https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/repos.yaml + vcs-repo-file-url: https://github.com/lbr-stack/lbr_fri_ros2_stack/tree/dev-humble-launch/repos-fri-${{ matrix.fri_version }}.yaml diff --git a/README.md b/README.md index edde1bea..026b8c54 100644 --- a/README.md +++ b/README.md @@ -40,7 +40,7 @@ Full documentation available [here](https://lbr-stack.readthedocs.io/en/latest). source /opt/ros/humble/setup.bash export FRI_CLIENT_VERSION=1.15 mkdir -p lbr-stack/src && cd lbr-stack - vcs import src --input https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/fri-${FRI_CLIENT_VERSION}.repos + vcs import src --input https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/repos-fri-${FRI_CLIENT_VERSION}.yaml rosdep install --from-paths src -i -r -y ``` diff --git a/lbr_fri_ros2_stack/fri-1.14.repos b/lbr_fri_ros2_stack/repos-fri-1.14.yaml similarity index 100% rename from lbr_fri_ros2_stack/fri-1.14.repos rename to lbr_fri_ros2_stack/repos-fri-1.14.yaml diff --git a/lbr_fri_ros2_stack/fri-1.15.repos b/lbr_fri_ros2_stack/repos-fri-1.15.yaml similarity index 100% rename from lbr_fri_ros2_stack/fri-1.15.repos rename to lbr_fri_ros2_stack/repos-fri-1.15.yaml diff --git a/lbr_fri_ros2_stack/fri-1.16.repos b/lbr_fri_ros2_stack/repos-fri-1.16.yaml similarity index 100% rename from lbr_fri_ros2_stack/fri-1.16.repos rename to lbr_fri_ros2_stack/repos-fri-1.16.yaml diff --git a/lbr_fri_ros2_stack/fri-2.5.repos b/lbr_fri_ros2_stack/repos-fri-2.5.yaml similarity index 100% rename from lbr_fri_ros2_stack/fri-2.5.repos rename to lbr_fri_ros2_stack/repos-fri-2.5.yaml From 62f76ae5b1125cf9b3e1bcad24e08e43a8b050a4 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 9 May 2024 17:35:09 +0100 Subject: [PATCH 44/76] fixed link --- .github/workflows/build.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index e1f52824..01695888 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -5,7 +5,7 @@ on: branches: - humble schedule: - - cron: '0 0 * * 0' + - cron: "0 0 * * 0" # ros 2 ci: https://github.com/marketplace/actions/ros-2-ci-action # doc: https://ubuntu.com/blog/ros-2-ci-with-github-actions @@ -22,4 +22,4 @@ jobs: with: package-name: lbr_fri_ros2_stack target-ros2-distro: humble - vcs-repo-file-url: https://github.com/lbr-stack/lbr_fri_ros2_stack/tree/dev-humble-launch/repos-fri-${{ matrix.fri_version }}.yaml + vcs-repo-file-url: https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/dev-humble-launch/lbr_fri_ros2_stack/repos-fri-${{ matrix.fri_version }}.yaml From 3b3cee03d1c3c33ce3d3c1317f58ae60e583050d Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 9 May 2024 19:33:29 +0100 Subject: [PATCH 45/76] added fri-1.11 --- lbr_fri_ros2_stack/repos-fri-1.11.yaml | 13 +++++++++++++ 1 file changed, 13 insertions(+) create mode 100644 lbr_fri_ros2_stack/repos-fri-1.11.yaml diff --git a/lbr_fri_ros2_stack/repos-fri-1.11.yaml b/lbr_fri_ros2_stack/repos-fri-1.11.yaml new file mode 100644 index 00000000..e05ac2d8 --- /dev/null +++ b/lbr_fri_ros2_stack/repos-fri-1.11.yaml @@ -0,0 +1,13 @@ +repositories: + fri: + type: git + url: https://github.com/lbr-stack/fri + version: fri-1.11 + lbr_fri_msgs: + type: git + url: https://github.com/lbr-stack/lbr_fri_msgs + version: fri-1 + lbr_fri_ros2_stack: + type: git + url: https://github.com/lbr-stack/lbr_fri_ros2_stack + version: humble From 2a6f58e666d038909aa1c8cd33b75e468c20876f Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 9 May 2024 19:50:04 +0100 Subject: [PATCH 46/76] centralized launch mixins --- lbr_bringup/CMakeLists.txt | 3 +- lbr_bringup/launch/bringup.launch.py | 3 +- lbr_bringup/launch/move_group.launch.py | 5 ++- lbr_bringup/launch/real.launch.py | 7 ++-- lbr_bringup/launch/sim.launch.py | 7 ++-- .../launch/system_interface.launch.py | 5 ++- lbr_bringup/launch_mixins/__init__.py | 0 .../lbr_bringup.py} | 0 .../launch_mixins/lbr_description.py | 0 .../launch_mixins/lbr_ros2_control.py | 0 lbr_bringup/lbr_bringup/__init__.py | 1 - lbr_description/CMakeLists.txt | 6 +--- lbr_description/launch/view_robot.launch.py | 32 ------------------- lbr_description/lbr_description/__init__.py | 1 - lbr_description/package.xml | 1 - lbr_ros2_control/CMakeLists.txt | 6 +--- lbr_ros2_control/lbr_ros2_control/__init__.py | 1 - lbr_ros2_control/package.xml | 1 - 18 files changed, 15 insertions(+), 64 deletions(-) rename {lbr_ros2_control => lbr_bringup}/launch/system_interface.launch.py (93%) create mode 100644 lbr_bringup/launch_mixins/__init__.py rename lbr_bringup/{lbr_bringup/launch_mixin.py => launch_mixins/lbr_bringup.py} (100%) rename lbr_description/lbr_description/launch_mixin.py => lbr_bringup/launch_mixins/lbr_description.py (100%) rename lbr_ros2_control/lbr_ros2_control/launch_mixin.py => lbr_bringup/launch_mixins/lbr_ros2_control.py (100%) delete mode 100644 lbr_bringup/lbr_bringup/__init__.py delete mode 100644 lbr_description/launch/view_robot.launch.py delete mode 100644 lbr_description/lbr_description/__init__.py delete mode 100644 lbr_ros2_control/lbr_ros2_control/__init__.py diff --git a/lbr_bringup/CMakeLists.txt b/lbr_bringup/CMakeLists.txt index 59822af9..cd7e1288 100644 --- a/lbr_bringup/CMakeLists.txt +++ b/lbr_bringup/CMakeLists.txt @@ -7,6 +7,7 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) install( DIRECTORY config launch @@ -14,6 +15,6 @@ install( ) # Launch mixins -ament_python_install_package(${PROJECT_NAME}) +ament_python_install_package(launch_mixins) ament_package() diff --git a/lbr_bringup/launch/bringup.launch.py b/lbr_bringup/launch/bringup.launch.py index 03fc2423..b4fb2420 100644 --- a/lbr_bringup/launch/bringup.launch.py +++ b/lbr_bringup/launch/bringup.launch.py @@ -3,10 +3,9 @@ from launch.conditions import IfCondition, UnlessCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_mixins.lbr_description import LBRDescriptionMixin from launch_ros.substitutions import FindPackageShare -from lbr_description import LBRDescriptionMixin - def generate_launch_description() -> LaunchDescription: ld = LaunchDescription() diff --git a/lbr_bringup/launch/move_group.launch.py b/lbr_bringup/launch/move_group.launch.py index 4b506e8a..b39ad7c3 100644 --- a/lbr_bringup/launch/move_group.launch.py +++ b/lbr_bringup/launch/move_group.launch.py @@ -3,9 +3,8 @@ from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity from launch.actions import OpaqueFunction from launch.substitutions import LaunchConfiguration, PathJoinSubstitution - -from lbr_bringup import LBRMoveGroupMixin -from lbr_description import LBRDescriptionMixin, RVizMixin +from launch_mixins.lbr_bringup import LBRMoveGroupMixin +from launch_mixins.lbr_description import LBRDescriptionMixin, RVizMixin def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: diff --git a/lbr_bringup/launch/real.launch.py b/lbr_bringup/launch/real.launch.py index 82dc3a63..6e7ac7f1 100644 --- a/lbr_bringup/launch/real.launch.py +++ b/lbr_bringup/launch/real.launch.py @@ -10,10 +10,9 @@ NotSubstitution, PathJoinSubstitution, ) - -from lbr_bringup import LBRMoveGroupMixin -from lbr_description import LBRDescriptionMixin, RVizMixin -from lbr_ros2_control import LBRROS2ControlMixin +from launch_mixins.lbr_bringup import LBRMoveGroupMixin +from launch_mixins.lbr_description import LBRDescriptionMixin, RVizMixin +from launch_mixins.lbr_ros2_control import LBRROS2ControlMixin def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: diff --git a/lbr_bringup/launch/sim.launch.py b/lbr_bringup/launch/sim.launch.py index 7f30e98f..9342396a 100644 --- a/lbr_bringup/launch/sim.launch.py +++ b/lbr_bringup/launch/sim.launch.py @@ -10,10 +10,9 @@ NotSubstitution, PathJoinSubstitution, ) - -from lbr_bringup import LBRMoveGroupMixin -from lbr_description import GazeboMixin, LBRDescriptionMixin, RVizMixin -from lbr_ros2_control import LBRROS2ControlMixin +from launch_mixins.lbr_bringup import LBRMoveGroupMixin +from launch_mixins.lbr_description import GazeboMixin, LBRDescriptionMixin, RVizMixin +from launch_mixins.lbr_ros2_control import LBRROS2ControlMixin def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: diff --git a/lbr_ros2_control/launch/system_interface.launch.py b/lbr_bringup/launch/system_interface.launch.py similarity index 93% rename from lbr_ros2_control/launch/system_interface.launch.py rename to lbr_bringup/launch/system_interface.launch.py index 3043ef73..efc53910 100644 --- a/lbr_ros2_control/launch/system_interface.launch.py +++ b/lbr_bringup/launch/system_interface.launch.py @@ -2,9 +2,8 @@ from launch.actions import RegisterEventHandler from launch.event_handlers import OnProcessStart from launch.substitutions import LaunchConfiguration - -from lbr_description import LBRDescriptionMixin -from lbr_ros2_control import LBRROS2ControlMixin +from launch_mixins.lbr_description import LBRDescriptionMixin +from launch_mixins.lbr_ros2_control import LBRROS2ControlMixin class LBRSystemInterface(LBRDescriptionMixin, LBRROS2ControlMixin): diff --git a/lbr_bringup/launch_mixins/__init__.py b/lbr_bringup/launch_mixins/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/lbr_bringup/lbr_bringup/launch_mixin.py b/lbr_bringup/launch_mixins/lbr_bringup.py similarity index 100% rename from lbr_bringup/lbr_bringup/launch_mixin.py rename to lbr_bringup/launch_mixins/lbr_bringup.py diff --git a/lbr_description/lbr_description/launch_mixin.py b/lbr_bringup/launch_mixins/lbr_description.py similarity index 100% rename from lbr_description/lbr_description/launch_mixin.py rename to lbr_bringup/launch_mixins/lbr_description.py diff --git a/lbr_ros2_control/lbr_ros2_control/launch_mixin.py b/lbr_bringup/launch_mixins/lbr_ros2_control.py similarity index 100% rename from lbr_ros2_control/lbr_ros2_control/launch_mixin.py rename to lbr_bringup/launch_mixins/lbr_ros2_control.py diff --git a/lbr_bringup/lbr_bringup/__init__.py b/lbr_bringup/lbr_bringup/__init__.py deleted file mode 100644 index c123a786..00000000 --- a/lbr_bringup/lbr_bringup/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from .launch_mixin import LBRMoveGroupMixin diff --git a/lbr_description/CMakeLists.txt b/lbr_description/CMakeLists.txt index 664aab8e..b2f9ac44 100644 --- a/lbr_description/CMakeLists.txt +++ b/lbr_description/CMakeLists.txt @@ -8,19 +8,15 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(ament_cmake_pytest REQUIRED) -find_package(ament_cmake_python REQUIRED) # install install( - DIRECTORY config gazebo launch meshes urdf + DIRECTORY config gazebo meshes urdf DESTINATION share/${PROJECT_NAME} ) ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/${PROJECT_NAME}.dsv") -# Launch mixins -ament_python_install_package(${PROJECT_NAME}) - if(BUILD_TESTING) ament_add_pytest_test(test_urdf ${CMAKE_CURRENT_SOURCE_DIR}/test/test_urdf.py) endif() diff --git a/lbr_description/launch/view_robot.launch.py b/lbr_description/launch/view_robot.launch.py deleted file mode 100644 index b3d91782..00000000 --- a/lbr_description/launch/view_robot.launch.py +++ /dev/null @@ -1,32 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node - -from lbr_description import LBRDescriptionMixin, RVizMixin - - -def generate_launch_description() -> LaunchDescription: - ld = LaunchDescription() - ld.add_action(LBRDescriptionMixin.arg_model()) - ld.add_action(LBRDescriptionMixin.arg_robot_name()) - robot_description = LBRDescriptionMixin.param_robot_description(sim=True) - ld.add_action( - Node( - package="joint_state_publisher_gui", - executable="joint_state_publisher_gui", - ) - ) - ld.add_action( - Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) - ) - ld.add_action( - RVizMixin.node_rviz( - rviz_config_pkg="lbr_description", - rviz_config="config/config.rviz", - ) - ) - return ld diff --git a/lbr_description/lbr_description/__init__.py b/lbr_description/lbr_description/__init__.py deleted file mode 100644 index e811a21e..00000000 --- a/lbr_description/lbr_description/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from .launch_mixin import GazeboMixin, LBRDescriptionMixin, RVizMixin diff --git a/lbr_description/package.xml b/lbr_description/package.xml index f0bf6468..ea766e15 100644 --- a/lbr_description/package.xml +++ b/lbr_description/package.xml @@ -9,7 +9,6 @@ ament_cmake ament_cmake_pytest - ament_cmake_python gazebo_ros2_control joint_state_publisher_gui diff --git a/lbr_ros2_control/CMakeLists.txt b/lbr_ros2_control/CMakeLists.txt index 946a581b..108ecb9d 100644 --- a/lbr_ros2_control/CMakeLists.txt +++ b/lbr_ros2_control/CMakeLists.txt @@ -15,7 +15,6 @@ if(NOT CMAKE_BUILD_TYPE) endif() find_package(ament_cmake REQUIRED) -find_package(ament_cmake_python REQUIRED) find_package(controller_interface REQUIRED) find_package(FRIClient REQUIRED) find_package(hardware_interface REQUIRED) @@ -97,11 +96,8 @@ install( ) install( - DIRECTORY config launch + DIRECTORY config DESTINATION share/${PROJECT_NAME} ) -# Launch mixins -ament_python_install_package(${PROJECT_NAME}) - ament_package() diff --git a/lbr_ros2_control/lbr_ros2_control/__init__.py b/lbr_ros2_control/lbr_ros2_control/__init__.py deleted file mode 100644 index 9ca9a8fe..00000000 --- a/lbr_ros2_control/lbr_ros2_control/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from .launch_mixin import LBRROS2ControlMixin diff --git a/lbr_ros2_control/package.xml b/lbr_ros2_control/package.xml index 264ebd02..9844fd9c 100644 --- a/lbr_ros2_control/package.xml +++ b/lbr_ros2_control/package.xml @@ -8,7 +8,6 @@ Apache License 2.0 ament_cmake - ament_cmake_python FRIClient lbr_fri_msgs From 053a69c0ceff022b9f0081003768270775fbfaee Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 9 May 2024 19:55:11 +0100 Subject: [PATCH 47/76] support fri 2.x --- .../include/lbr_ros2_control/lbr_state_broadcaster.hpp | 1 + lbr_ros2_control/src/lbr_state_broadcaster.cpp | 4 ++++ 2 files changed, 5 insertions(+) diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_state_broadcaster.hpp b/lbr_ros2_control/include/lbr_ros2_control/lbr_state_broadcaster.hpp index 14ef402c..07d53e97 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/lbr_state_broadcaster.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/lbr_state_broadcaster.hpp @@ -16,6 +16,7 @@ #include "friClientIf.h" #include "friLBRState.h" +#include "friVersion.h" #include "lbr_fri_msgs/msg/lbr_state.hpp" #include "lbr_ros2_control/system_interface_type_values.hpp" diff --git a/lbr_ros2_control/src/lbr_state_broadcaster.cpp b/lbr_ros2_control/src/lbr_state_broadcaster.cpp index f6c988b3..f731e439 100644 --- a/lbr_ros2_control/src/lbr_state_broadcaster.cpp +++ b/lbr_ros2_control/src/lbr_state_broadcaster.cpp @@ -77,8 +77,10 @@ controller_interface::return_type LBRStateBroadcaster::update(const rclcpp::Time // joint related states std::for_each(joint_names_.begin(), joint_names_.end(), [&, idx = 0](const std::string &joint_name) mutable { +#if FRICLIENT_VERSION_MAJOR == 1 rt_state_publisher_ptr_->msg_.commanded_joint_position[idx] = state_interface_map_[joint_name][HW_IF_COMMANDED_JOINT_POSITION]; +#endif rt_state_publisher_ptr_->msg_.commanded_torque[idx] = state_interface_map_[joint_name][HW_IF_COMMANDED_TORQUE]; rt_state_publisher_ptr_->msg_.external_torque[idx] = @@ -131,8 +133,10 @@ void LBRStateBroadcaster::init_state_interface_map_() { void LBRStateBroadcaster::init_state_msg_() { rt_state_publisher_ptr_->msg_.client_command_mode = std::numeric_limits::quiet_NaN(); +#if FRICLIENT_VERSION_MAJOR == 1 rt_state_publisher_ptr_->msg_.commanded_joint_position.fill( std::numeric_limits::quiet_NaN()); +#endif rt_state_publisher_ptr_->msg_.commanded_torque.fill(std::numeric_limits::quiet_NaN()); rt_state_publisher_ptr_->msg_.connection_quality = std::numeric_limits::quiet_NaN(); rt_state_publisher_ptr_->msg_.control_mode = std::numeric_limits::quiet_NaN(); From 2911a866804b6575a340fecc0bd0cc022ffa2a67 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 9 May 2024 20:15:01 +0100 Subject: [PATCH 48/76] user warning --- lbr_ros2_control/src/system_interface.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 2a1de447..a0bd0e90 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -271,9 +271,17 @@ SystemInterface::on_deactivate(const rclcpp_lifecycle::State &) { } hardware_interface::return_type SystemInterface::read(const rclcpp::Time & /*time*/, - const rclcpp::Duration & /*period*/) { + const rclcpp::Duration &period) { hw_lbr_state_ = async_client_ptr_->get_state_interface().get_state(); + if (period.seconds() - hw_lbr_state_.sample_time * 0.1 > hw_lbr_state_.sample_time) { + RCLCPP_WARN_STREAM(rclcpp::get_logger(LOGGER_NAME), + lbr_fri_ros2::ColorScheme::WARNING + << "Increase update_rate parameter for controller_manager to " + << std::to_string(static_cast(1. / hw_lbr_state_.sample_time)) + << " Hz or more" << lbr_fri_ros2::ColorScheme::ENDC); + } + // exit once robot exits COMMANDING_ACTIVE (for safety) if (exit_commanding_active_(static_cast(hw_session_state_), static_cast(hw_lbr_state_.session_state))) { From 4f99e92f956231f66cdba84036cd7d5127233d5b Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 9 May 2024 20:54:12 +0100 Subject: [PATCH 49/76] added commanding active condition --- lbr_ros2_control/src/system_interface.cpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index a0bd0e90..9cfd9613 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -260,6 +260,20 @@ controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_l RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Sample time %.3f s / %.1f Hz", async_client_ptr_->get_state_interface().get_state().sample_time, 1. / async_client_ptr_->get_state_interface().get_state().sample_time); + while (!(async_client_ptr_->get_state_interface().get_state().session_state >= + KUKA::FRI::ESessionState::COMMANDING_WAIT)) { + RCLCPP_INFO_STREAM( + rclcpp::get_logger(LOGGER_NAME), + "Awaiting '" << lbr_fri_ros2::ColorScheme::BOLD << lbr_fri_ros2::ColorScheme::OKBLUE + << lbr_fri_ros2::EnumMaps::session_state_map( + KUKA::FRI::ESessionState::COMMANDING_WAIT) + << lbr_fri_ros2::ColorScheme::ENDC << "' state. Current state '" + << lbr_fri_ros2::ColorScheme::BOLD << lbr_fri_ros2::ColorScheme::OKBLUE + << lbr_fri_ros2::EnumMaps::session_state_map( + async_client_ptr_->get_state_interface().get_state().session_state) + << lbr_fri_ros2::ColorScheme::ENDC << "'."); + std::this_thread::sleep_for(std::chrono::seconds(1)); + } return controller_interface::CallbackReturn::SUCCESS; } @@ -274,7 +288,7 @@ hardware_interface::return_type SystemInterface::read(const rclcpp::Time & /*tim const rclcpp::Duration &period) { hw_lbr_state_ = async_client_ptr_->get_state_interface().get_state(); - if (period.seconds() - hw_lbr_state_.sample_time * 0.1 > hw_lbr_state_.sample_time) { + if (period.seconds() - hw_lbr_state_.sample_time * 0.2 > hw_lbr_state_.sample_time) { RCLCPP_WARN_STREAM(rclcpp::get_logger(LOGGER_NAME), lbr_fri_ros2::ColorScheme::WARNING << "Increase update_rate parameter for controller_manager to " From 754161c92fd3543ba18939b69cb26af068eedd36 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 9 May 2024 20:55:35 +0100 Subject: [PATCH 50/76] catch rclcpp::ok --- lbr_ros2_control/src/system_interface.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 9cfd9613..90b44d8e 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -261,7 +261,8 @@ controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_l async_client_ptr_->get_state_interface().get_state().sample_time, 1. / async_client_ptr_->get_state_interface().get_state().sample_time); while (!(async_client_ptr_->get_state_interface().get_state().session_state >= - KUKA::FRI::ESessionState::COMMANDING_WAIT)) { + KUKA::FRI::ESessionState::COMMANDING_WAIT) && + rclcpp::ok()) { RCLCPP_INFO_STREAM( rclcpp::get_logger(LOGGER_NAME), "Awaiting '" << lbr_fri_ros2::ColorScheme::BOLD << lbr_fri_ros2::ColorScheme::OKBLUE @@ -274,6 +275,9 @@ controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_l << lbr_fri_ros2::ColorScheme::ENDC << "'."); std::this_thread::sleep_for(std::chrono::seconds(1)); } + if (!rclcpp::ok()) { + return controller_interface::CallbackReturn::ERROR; + } return controller_interface::CallbackReturn::SUCCESS; } From ff8582bf14edfbbecf421babd39fb35cb3ab95de Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 9 May 2024 21:00:12 +0100 Subject: [PATCH 51/76] fri 2.x support --- lbr_ros2_control/src/system_interface.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 90b44d8e..2ce132ac 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -138,8 +138,10 @@ std::vector SystemInterface::export_state_in state_interfaces.emplace_back(info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_lbr_state_.measured_joint_position[i]); +#if FRICLIENT_VERSION_MAJOR == 1 state_interfaces.emplace_back(info_.joints[i].name, HW_IF_COMMANDED_JOINT_POSITION, &hw_lbr_state_.commanded_joint_position[i]); +#endif state_interfaces.emplace_back(info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_lbr_state_.measured_torque[i]); @@ -393,7 +395,9 @@ void SystemInterface::nan_command_interfaces_() { void SystemInterface::nan_state_interfaces_() { // state interfaces of type double hw_lbr_state_.measured_joint_position.fill(std::numeric_limits::quiet_NaN()); +#if FRICLIENT_VERSION_MAJOR == 1 hw_lbr_state_.commanded_joint_position.fill(std::numeric_limits::quiet_NaN()); +#endif hw_lbr_state_.measured_torque.fill(std::numeric_limits::quiet_NaN()); hw_lbr_state_.commanded_torque.fill(std::numeric_limits::quiet_NaN()); hw_lbr_state_.external_torque.fill(std::numeric_limits::quiet_NaN()); From dd17a0ef8b3ac20983c8ac543a975eaf255eddad Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 9 May 2024 21:07:25 +0100 Subject: [PATCH 52/76] init catch --- lbr_ros2_control/src/system_interface.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 2ce132ac..92ca85df 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -292,6 +292,10 @@ SystemInterface::on_deactivate(const rclcpp_lifecycle::State &) { hardware_interface::return_type SystemInterface::read(const rclcpp::Time & /*time*/, const rclcpp::Duration &period) { + if (!async_client_ptr_->get_state_interface().is_initialized()) { + return hardware_interface::return_type::OK; + } + hw_lbr_state_ = async_client_ptr_->get_state_interface().get_state(); if (period.seconds() - hw_lbr_state_.sample_time * 0.2 > hw_lbr_state_.sample_time) { From 749edf2e221bc8ea0ca03bee39e25e38b06ddab6 Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 10 May 2024 17:37:10 +0100 Subject: [PATCH 53/76] added initial client command mode de-couple --- lbr_fri_ros2/CMakeLists.txt | 14 +- lbr_fri_ros2/include/lbr_fri_ros2/app.hpp | 4 +- .../include/lbr_fri_ros2/async_client.hpp | 20 ++- .../base_command.hpp} | 26 ++- .../interfaces/position_command.hpp | 20 +++ .../state.hpp} | 6 +- .../interfaces/torque_command.hpp | 20 +++ .../interfaces/wrench_command.hpp | 20 +++ lbr_fri_ros2/src/app.cpp | 14 +- lbr_fri_ros2/src/async_client.cpp | 98 +++++------ lbr_fri_ros2/src/command_guard.cpp | 28 +--- lbr_fri_ros2/src/command_interface.cpp | 150 ----------------- lbr_fri_ros2/src/interfaces/base_command.cpp | 31 ++++ .../src/interfaces/position_command.cpp | 58 +++++++ .../state.cpp} | 2 +- .../src/interfaces/torque_command.cpp | 45 +++++ .../src/interfaces/wrench_command.cpp | 46 ++++++ lbr_fri_ros2/test/test_command_interfaces.cpp | 155 ++++++++++++++++++ 18 files changed, 493 insertions(+), 264 deletions(-) rename lbr_fri_ros2/include/lbr_fri_ros2/{command_interface.hpp => interfaces/base_command.hpp} (58%) create mode 100644 lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp rename lbr_fri_ros2/include/lbr_fri_ros2/{state_interface.hpp => interfaces/state.hpp} (92%) create mode 100644 lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp create mode 100644 lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp delete mode 100644 lbr_fri_ros2/src/command_interface.cpp create mode 100644 lbr_fri_ros2/src/interfaces/base_command.cpp create mode 100644 lbr_fri_ros2/src/interfaces/position_command.cpp rename lbr_fri_ros2/src/{state_interface.cpp => interfaces/state.cpp} (99%) create mode 100644 lbr_fri_ros2/src/interfaces/torque_command.cpp create mode 100644 lbr_fri_ros2/src/interfaces/wrench_command.cpp create mode 100644 lbr_fri_ros2/test/test_command_interfaces.cpp diff --git a/lbr_fri_ros2/CMakeLists.txt b/lbr_fri_ros2/CMakeLists.txt index b6c9c3bc..1f4572bf 100644 --- a/lbr_fri_ros2/CMakeLists.txt +++ b/lbr_fri_ros2/CMakeLists.txt @@ -25,13 +25,16 @@ find_package(urdf REQUIRED) # lbr_fri_ros2 add_library(lbr_fri_ros2 SHARED + src/interfaces/base_command.cpp + src/interfaces/position_command.cpp + src/interfaces/state.cpp + src/interfaces/torque_command.cpp + src/interfaces/wrench_command.cpp src/app.cpp src/async_client.cpp src/command_guard.cpp - src/command_interface.cpp src/filters.cpp src/ft_estimator.cpp - src/state_interface.cpp ) target_include_directories(lbr_fri_ros2 @@ -78,4 +81,11 @@ install( LIBRARY DESTINATION lib ) +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(test_command_interfaces test/test_command_interfaces.cpp) + target_link_libraries(test_command_interfaces lbr_fri_ros2) +endif() + ament_package() diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp index 819668f3..3aea94f2 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp @@ -26,8 +26,8 @@ class App { bool open_udp_socket(const int &port_id = 30200, const char *const remote_host = NULL); bool close_udp_socket(); - void run(int rt_prio = 80); - void stop_run(); + void run_async(int rt_prio = 80); + void request_stop(); protected: bool valid_port_(const int &port_id); diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp index 318f944a..2696ca44 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp @@ -11,10 +11,13 @@ #include "friLBRClient.h" #include "friVersion.h" -#include "lbr_fri_ros2/command_interface.hpp" #include "lbr_fri_ros2/filters.hpp" #include "lbr_fri_ros2/formatting.hpp" -#include "lbr_fri_ros2/state_interface.hpp" +#include "lbr_fri_ros2/interfaces/base_command.hpp" +#include "lbr_fri_ros2/interfaces/position_command.hpp" +#include "lbr_fri_ros2/interfaces/state.hpp" +#include "lbr_fri_ros2/interfaces/torque_command.hpp" +#include "lbr_fri_ros2/interfaces/wrench_command.hpp" namespace lbr_fri_ros2 { class AsyncClient : public KUKA::FRI::LBRClient { @@ -23,14 +26,17 @@ class AsyncClient : public KUKA::FRI::LBRClient { public: AsyncClient() = delete; - AsyncClient(const PIDParameters &pid_parameters, + AsyncClient(const KUKA::FRI::EClientCommandMode &client_command_mode, + const PIDParameters &pid_parameters, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant, const StateInterfaceParameters &state_interface_parameters = {10.0, 10.0}, const bool &open_loop = true); - inline CommandInterface &get_command_interface() { return command_interface_; } - inline StateInterface &get_state_interface() { return state_interface_; } + inline std::shared_ptr get_command_interface() { + return command_interface_ptr_; + } + inline std::shared_ptr get_state_interface() { return state_interface_ptr_; } void onStateChange(KUKA::FRI::ESessionState old_state, KUKA::FRI::ESessionState new_state) override; @@ -39,8 +45,8 @@ class AsyncClient : public KUKA::FRI::LBRClient { void command() override; protected: - CommandInterface command_interface_; - StateInterface state_interface_; + std::shared_ptr command_interface_ptr_; + std::shared_ptr state_interface_ptr_; bool open_loop_; }; diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/command_interface.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp similarity index 58% rename from lbr_fri_ros2/include/lbr_fri_ros2/command_interface.hpp rename to lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp index 883fd100..708916a0 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/command_interface.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp @@ -1,5 +1,5 @@ -#ifndef LBR_FRI_ROS2__COMMAND_INTERFACE_HPP_ -#define LBR_FRI_ROS2__COMMAND_INTERFACE_HPP_ +#ifndef LBR_FRI_ROS2__INTERACES__COMMAND_HPP_ +#define LBR_FRI_ROS2__INTERACES__COMMAND_HPP_ #include #include @@ -18,9 +18,9 @@ #include "lbr_fri_ros2/formatting.hpp" namespace lbr_fri_ros2 { -class CommandInterface { +class BaseCommandInterface { protected: - static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::CommandInterface"; + virtual std::string LOGGER_NAME() const = 0; // ROS IDL types using idl_command_t = lbr_fri_msgs::msg::LBRCommand; @@ -33,17 +33,15 @@ class CommandInterface { using const_fri_state_t_ref = const fri_state_t &; public: - CommandInterface() = delete; - CommandInterface(const PIDParameters &pid_parameters, - const CommandGuardParameters &command_guard_parameters, - const std::string &command_guard_variant = "default"); - - void get_joint_position_command(fri_command_t_ref command, const_fri_state_t_ref state); - void get_torque_command(fri_command_t_ref command, const_fri_state_t_ref state); - void get_wrench_command(fri_command_t_ref command, const_fri_state_t_ref state); + BaseCommandInterface() = delete; + BaseCommandInterface(const PIDParameters &pid_parameters, + const CommandGuardParameters &command_guard_parameters, + const std::string &command_guard_variant = "default"); + virtual void buffered_command_to_fri(fri_command_t_ref command, const_fri_state_t_ref state) = 0; + inline void buffer_command_target(const_idl_command_t_ref command) { command_target_ = command; } void init_command(const_fri_state_t_ref state); - inline void set_command_target(const_idl_command_t_ref command) { command_target_ = command; } + inline const_idl_command_t_ref get_command() const { return command_; } inline const_idl_command_t_ref get_command_target() const { return command_target_; } @@ -56,4 +54,4 @@ class CommandInterface { idl_command_t command_, command_target_; }; } // namespace lbr_fri_ros2 -#endif // LBR_FRI_ROS2__COMMAND_INTERFACE_HPP_ +#endif // LBR_FRI_ROS2__INTERACES__COMMAND_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp new file mode 100644 index 00000000..8330581f --- /dev/null +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp @@ -0,0 +1,20 @@ +#ifndef LBR_FRI_ROS2__INTERFACES__POSITION_COMMAND_HPP_ +#define LBR_FRI_ROS2__INTERFACES__POSITION_COMMAND_HPP_ + +#include "lbr_fri_ros2/interfaces/base_command.hpp" + +namespace lbr_fri_ros2 { +class PositionCommandInterface : public BaseCommandInterface { +protected: + std::string LOGGER_NAME() const override { return "lbr_fri_ros2::PositionCommandInterface"; } + +public: + PositionCommandInterface() = delete; + PositionCommandInterface(const PIDParameters &pid_parameters, + const CommandGuardParameters &command_guard_parameters, + const std::string &command_guard_variant = "default"); + + void buffered_command_to_fri(fri_command_t_ref command, const_fri_state_t_ref state) override; +}; +} // end of namespace lbr_fri_ros2 +#endif // LBR_FRI_ROS2__INTERFACES__POSITION_COMMAND_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/state_interface.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp similarity index 92% rename from lbr_fri_ros2/include/lbr_fri_ros2/state_interface.hpp rename to lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp index 179d410d..656d7714 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/state_interface.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp @@ -1,5 +1,5 @@ -#ifndef LBR_FRI_ROS2__STATE_INTERFACE_HPP_ -#define LBR_FRI_ROS2__STATE_INTERFACE_HPP_ +#ifndef LBR_FRI_ROS2__INTERFACES__STATE_HPP_ +#define LBR_FRI_ROS2__INTERFACES__STATE_HPP_ #include #include @@ -56,4 +56,4 @@ class StateInterface { JointExponentialFilterArray external_torque_filter_, measured_torque_filter_; }; } // end of namespace lbr_fri_ros2 -#endif // LBR_FRI_ROS2__STATE_INTERFACE_HPP_ +#endif // LBR_FRI_ROS2__INTERFACES__STATE_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp new file mode 100644 index 00000000..c8d08610 --- /dev/null +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp @@ -0,0 +1,20 @@ +#ifndef LBR_FRI_ROS2__INTERFACES__TORQUE_COMMAND_HPP_ +#define LBR_FRI_ROS2__INTERFACES__TORQUE_COMMAND_HPP_ + +#include "lbr_fri_ros2/interfaces/base_command.hpp" + +namespace lbr_fri_ros2 { +class TorqueCommandInterface : public BaseCommandInterface { +protected: + std::string LOGGER_NAME() const override { return "lbr_fri_ros2::TorqueCommandInterface"; } + +public: + TorqueCommandInterface() = delete; + TorqueCommandInterface(const PIDParameters &pid_parameters, + const CommandGuardParameters &command_guard_parameters, + const std::string &command_guard_variant = "default"); + + void buffered_command_to_fri(fri_command_t_ref command, const_fri_state_t_ref state) override; +}; +} // end of namespace lbr_fri_ros2 +#endif // LBR_FRI_ROS2__INTERFACES__TORQUE_COMMAND_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp new file mode 100644 index 00000000..d67aaaed --- /dev/null +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp @@ -0,0 +1,20 @@ +#ifndef LBR_FRI_ROS2__INTERFACES__WRENCH_COMMAND_HPP_ +#define LBR_FRI_ROS2__INTERFACES__WRENCH_COMMAND_HPP_ + +#include "lbr_fri_ros2/interfaces/base_command.hpp" + +namespace lbr_fri_ros2 { +class WrenchCommandInterface : public BaseCommandInterface { +protected: + std::string LOGGER_NAME() const override { return "lbr_fri_ros2::WrenchCommandInterface"; } + +public: + WrenchCommandInterface() = delete; + WrenchCommandInterface(const PIDParameters &pid_parameters, + const CommandGuardParameters &command_guard_parameters, + const std::string &command_guard_variant = "default"); + + void buffered_command_to_fri(fri_command_t_ref command, const_fri_state_t_ref state) override; +}; +} // end of namespace lbr_fri_ros2 +#endif // LBR_FRI_ROS2__INTERFACES__WRENCH_COMMAND_HPP_ diff --git a/lbr_fri_ros2/src/app.cpp b/lbr_fri_ros2/src/app.cpp index e0a0877f..da9c291a 100644 --- a/lbr_fri_ros2/src/app.cpp +++ b/lbr_fri_ros2/src/app.cpp @@ -10,7 +10,7 @@ App::App(const std::shared_ptr async_client_ptr) } App::~App() { - stop_run(); + request_stop(); close_udp_socket(); } @@ -46,6 +46,10 @@ bool App::close_udp_socket() { ColorScheme::ERROR << "Connection not configured" << ColorScheme::ENDC); return false; } + while (running_) { + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Waiting for run thread termination"); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), ColorScheme::OKBLUE << "Closing UDP socket" << ColorScheme::ENDC); if (!connection_ptr_->isOpen()) { @@ -58,7 +62,7 @@ bool App::close_udp_socket() { return true; } -void App::run(int rt_prio) { +void App::run_async(int rt_prio) { if (!async_client_ptr_) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), ColorScheme::ERROR << "AsyncClient not configured" << ColorScheme::ENDC); @@ -100,20 +104,20 @@ void App::run(int rt_prio) { running_ = true; bool success = true; while (rclcpp::ok() && success && !should_stop_) { - success = app_ptr_->step(); // TODO: blocks until robot heartbeat, stuck if port id mismatches + success = app_ptr_->step(); if (async_client_ptr_->robotState().getSessionState() == KUKA::FRI::ESessionState::IDLE) { RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "LBR in session state idle, exiting"); break; } } - async_client_ptr_->get_state_interface().uninitialize(); + async_client_ptr_->get_state_interface()->uninitialize(); running_ = false; RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Exiting run thread"); }); run_thread_.detach(); } -void App::stop_run() { +void App::request_stop() { RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Requesting run thread stop"); should_stop_ = true; } diff --git a/lbr_fri_ros2/src/async_client.cpp b/lbr_fri_ros2/src/async_client.cpp index 4b326cfa..3f2cf68d 100644 --- a/lbr_fri_ros2/src/async_client.cpp +++ b/lbr_fri_ros2/src/async_client.cpp @@ -1,19 +1,46 @@ #include "lbr_fri_ros2/async_client.hpp" namespace lbr_fri_ros2 { -AsyncClient::AsyncClient(const PIDParameters &pid_parameters, +AsyncClient::AsyncClient(const KUKA::FRI::EClientCommandMode &client_command_mode, + const PIDParameters &pid_parameters, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant, const StateInterfaceParameters &state_interface_parameters, const bool &open_loop) - : command_interface_(pid_parameters, command_guard_parameters, command_guard_variant), - state_interface_(state_interface_parameters), open_loop_(open_loop) { + : open_loop_(open_loop) { RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), ColorScheme::OKBLUE << "Configuring client" << ColorScheme::ENDC); + + // create command interface + RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), + "Client command mode: '" + << EnumMaps::client_command_mode_map(client_command_mode).c_str() << "'"); RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), "Command guard variant '" << command_guard_variant.c_str() << "'"); - command_interface_.log_info(); - state_interface_.log_info(); + switch (client_command_mode) { + case KUKA::FRI::EClientCommandMode::POSITION: + command_interface_ptr_ = std::make_shared( + pid_parameters, command_guard_parameters, command_guard_variant); + break; + case KUKA::FRI::EClientCommandMode::TORQUE: + command_interface_ptr_ = std::make_shared( + pid_parameters, command_guard_parameters, command_guard_variant); + break; + case KUKA::FRI::EClientCommandMode::WRENCH: + command_interface_ptr_ = std::make_shared( + pid_parameters, command_guard_parameters, command_guard_variant); + break; + default: + std::string err = "Unsupported client command mode."; + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); + throw std::runtime_error(err); + } + command_interface_ptr_->log_info(); + + // create state interface + state_interface_ptr_ = std::make_shared(state_interface_parameters); + state_interface_ptr_->log_info(); RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), "Open loop '" << (open_loop_ ? "true" : "false") << "'"); RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), @@ -29,67 +56,26 @@ void AsyncClient::onStateChange(KUKA::FRI::ESessionState old_state, << "' to '" << ColorScheme::OKGREEN << ColorScheme::BOLD << EnumMaps::session_state_map(new_state).c_str() << ColorScheme::ENDC << "'"); - command_interface_.init_command(robotState()); + + // initialize command + command_interface_ptr_->init_command(robotState()); } -void AsyncClient::monitor() { state_interface_.set_state(robotState()); }; +void AsyncClient::monitor() { state_interface_ptr_->set_state(robotState()); }; void AsyncClient::waitForCommand() { KUKA::FRI::LBRClient::waitForCommand(); - state_interface_.set_state(robotState()); - - if (robotState().getClientCommandMode() == KUKA::FRI::EClientCommandMode::TORQUE) { - command_interface_.get_torque_command(robotCommand(), robotState()); - } - - if (robotState().getClientCommandMode() == KUKA::FRI::EClientCommandMode::WRENCH) { - command_interface_.get_wrench_command(robotCommand(), robotState()); - } + state_interface_ptr_->set_state(robotState()); + command_interface_ptr_->buffered_command_to_fri(robotCommand(), robotState()); } void AsyncClient::command() { if (open_loop_) { - state_interface_.set_state_open_loop(robotState(), - command_interface_.get_command().joint_position); + state_interface_ptr_->set_state_open_loop(robotState(), + command_interface_ptr_->get_command().joint_position); } else { - state_interface_.set_state(robotState()); - } - - switch (robotState().getClientCommandMode()) { -#if FRICLIENT_VERSION_MAJOR == 1 - case KUKA::FRI::EClientCommandMode::POSITION: -#endif -#if FRICLIENT_VERSION_MAJOR == 2 - case KUKA::FRI::EClientCommandMode::JOINT_POSITION: -#endif - { - command_interface_.get_joint_position_command(robotCommand(), robotState()); - return; - } -#if FRICLIENT_VERSION_MAJOR == 2 - case KUKA::FRI::EClientCommandMode::CARTESIAN_POSE: { - std::string err = - EnumMaps::client_command_mode_map(KUKA::FRI::EClientCommandMode::CARTESIAN_POSE) + - " command mode not supported yet."; - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); - throw std::runtime_error(err); - } -#endif - case KUKA::FRI::EClientCommandMode::TORQUE: { - command_interface_.get_torque_command(robotCommand(), robotState()); - return; - } - case KUKA::FRI::EClientCommandMode::WRENCH: { - command_interface_.get_wrench_command(robotCommand(), robotState()); - return; - } - default: { - std::string err = - "Unsupported command mode '" + std::to_string(robotState().getClientCommandMode()) + "'"; - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), - ColorScheme::ERROR << err << ColorScheme::ENDC); - throw std::runtime_error(err); - } + state_interface_ptr_->set_state(robotState()); } + command_interface_ptr_->buffered_command_to_fri(robotCommand(), robotState()); } } // end of namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/command_guard.cpp b/lbr_fri_ros2/src/command_guard.cpp index 425129f9..7429356c 100644 --- a/lbr_fri_ros2/src/command_guard.cpp +++ b/lbr_fri_ros2/src/command_guard.cpp @@ -6,33 +6,13 @@ CommandGuard::CommandGuard(const CommandGuardParameters &command_guard_parameter bool CommandGuard::is_valid_command(const_idl_command_t_ref lbr_command, const_fri_state_t_ref lbr_state) { - switch (lbr_state.getClientCommandMode()) { - case KUKA::FRI::EClientCommandMode::NO_COMMAND_MODE: + if (!command_in_position_limits_(lbr_command, lbr_state)) { return false; -#if FRICLIENT_VERSION_MAJOR == 1 - case KUKA::FRI::EClientCommandMode::POSITION: -#endif -#if FRICLIENT_VERSION_MAJOR == 2 - case KUKA::FRI::EClientCommandMode::JOINT_POSITION: -#endif - case KUKA::FRI::EClientCommandMode::TORQUE: - case KUKA::FRI::EClientCommandMode::WRENCH: - if (!command_in_position_limits_(lbr_command, lbr_state)) { - return false; - } - if (!command_in_velocity_limits_(lbr_command, lbr_state)) { - return false; - } - return true; -#if FRICLIENT_VERSION_MAJOR == 2 - case KUKA::FRI::EClientCommandMode::CARTESIAN_POSE: - return false; -#endif - default: - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Invalid EClientCommandMode provided"); + } + if (!command_in_velocity_limits_(lbr_command, lbr_state)) { return false; } - return false; + return true; } void CommandGuard::log_info() const { diff --git a/lbr_fri_ros2/src/command_interface.cpp b/lbr_fri_ros2/src/command_interface.cpp deleted file mode 100644 index a31039cc..00000000 --- a/lbr_fri_ros2/src/command_interface.cpp +++ /dev/null @@ -1,150 +0,0 @@ -#include "lbr_fri_ros2/command_interface.hpp" - -namespace lbr_fri_ros2 { - -CommandInterface::CommandInterface(const PIDParameters &pid_parameters, - const CommandGuardParameters &command_guard_parameters, - const std::string &command_guard_variant) - : pid_parameters_(pid_parameters) { - command_guard_ = command_guard_factory(command_guard_parameters, command_guard_variant); -}; - -void CommandInterface::get_joint_position_command(fri_command_t_ref command, - const_fri_state_t_ref state) { -#if FRICLIENT_VERSION_MAJOR == 1 - if (state.getClientCommandMode() != KUKA::FRI::EClientCommandMode::POSITION) { - std::string err = "Set joint position only allowed in " + - EnumMaps::client_command_mode_map(KUKA::FRI::EClientCommandMode::POSITION) + - " command mode."; - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), - ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); - throw std::runtime_error(err); - } -#endif -#if FRICLIENT_VERSION_MAJOR == 2 - if (state.getClientCommandMode() != KUKA::FRI::EClientCommandMode::JOINT_POSITION) { - std::string err = - "Set joint position only allowed in " + - EnumMaps::client_command_mode_map(KUKA::FRI::EClientCommandMode::JOINT_POSITION) + - " command mode."; - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); - throw std::runtime_error(err); - } -#endif - if (!command_guard_) { - std::string err = "Uninitialized command guard."; - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), - ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); - throw std::runtime_error(err); - } - - // PID - if (!joint_position_pid_.is_initialized()) { - joint_position_pid_.initialize(pid_parameters_, state.getSampleTime()); - } - joint_position_pid_.compute( - command_target_.joint_position, state.getMeasuredJointPosition(), - std::chrono::nanoseconds(static_cast(state.getSampleTime() * 1.e9)), - command_.joint_position); - - // validate - if (!command_guard_->is_valid_command(command_, state)) { - std::string err = "Invalid command."; - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), - ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); - throw std::runtime_error(err); - } - - // write joint position to output - command.setJointPosition(command_.joint_position.data()); -} - -void CommandInterface::get_torque_command(fri_command_t_ref command, const_fri_state_t_ref state) { - if (state.getClientCommandMode() != KUKA::FRI::EClientCommandMode::TORQUE) { - std::string err = "Set torque only allowed in torque command mode."; - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), - ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); - throw std::runtime_error(err); - } - if (!command_guard_) { - std::string err = "Uninitialized command guard."; - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), - ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); - throw std::runtime_error(err); - } - - // PID - if (!joint_position_pid_.is_initialized()) { - joint_position_pid_.initialize(pid_parameters_, state.getSampleTime()); - } - joint_position_pid_.compute( - command_target_.joint_position, state.getMeasuredJointPosition(), - std::chrono::nanoseconds(static_cast(state.getSampleTime() * 1.e9)), - command_.joint_position); - command_.torque = command_target_.torque; - - // validate - if (!command_guard_->is_valid_command(command_, state)) { - throw std::runtime_error("Invalid command"); - } - - // write joint position and torque to output - command.setJointPosition(command_.joint_position.data()); - command.setTorque(command_.torque.data()); -} - -void CommandInterface::get_wrench_command(fri_command_t_ref command, const_fri_state_t_ref state) { - if (state.getClientCommandMode() != KUKA::FRI::EClientCommandMode::WRENCH) { - std::string err = "Set wrench only allowed in wrench command mode."; - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); - throw std::runtime_error(err); - } - if (!command_guard_) { - std::string err = "Uninitialized command guard."; - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); - throw std::runtime_error(err); - } - - // PID - if (!joint_position_pid_.is_initialized()) { - joint_position_pid_.initialize(pid_parameters_, state.getSampleTime()); - } - joint_position_pid_.compute( - command_target_.joint_position, state.getMeasuredJointPosition(), - std::chrono::nanoseconds(static_cast(state.getSampleTime() * 1.e9)), - command_.joint_position); - command_.wrench = command_target_.wrench; - - // validate - if (!command_guard_->is_valid_command(command_, state)) { - std::string err = "Invalid command."; - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), - ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); - throw std::runtime_error(err); - } - - // write joint position and wrench to output - command.setJointPosition(command_.joint_position.data()); - command.setWrench(command_.wrench.data()); -} - -void CommandInterface::init_command(const_fri_state_t_ref state) { - std::memcpy(command_target_.joint_position.data(), state.getMeasuredJointPosition(), - sizeof(double) * KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - command_target_.torque.fill(0.); - command_target_.wrench.fill(0.); - command_ = command_target_; -} - -void CommandInterface::log_info() const { - command_guard_->log_info(); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:"); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* pid.p: %.1f", pid_parameters_.p); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* pid.i: %.1f", pid_parameters_.i); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* pid.d: %.1f", pid_parameters_.d); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* pid.i_max: %.1f", pid_parameters_.i_max); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* pid.i_min: %.1f", pid_parameters_.i_min); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* pid.antiwindup: %s", - pid_parameters_.antiwindup ? "true" : "false"); -} -} // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/interfaces/base_command.cpp b/lbr_fri_ros2/src/interfaces/base_command.cpp new file mode 100644 index 00000000..9eb02f03 --- /dev/null +++ b/lbr_fri_ros2/src/interfaces/base_command.cpp @@ -0,0 +1,31 @@ +#include "lbr_fri_ros2/interfaces/base_command.hpp" + +namespace lbr_fri_ros2 { + +BaseCommandInterface::BaseCommandInterface(const PIDParameters &pid_parameters, + const CommandGuardParameters &command_guard_parameters, + const std::string &command_guard_variant) + : pid_parameters_(pid_parameters) { + command_guard_ = command_guard_factory(command_guard_parameters, command_guard_variant); +}; + +void BaseCommandInterface::init_command(const_fri_state_t_ref state) { + std::memcpy(command_target_.joint_position.data(), state.getMeasuredJointPosition(), + sizeof(double) * KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + command_target_.torque.fill(0.); + command_target_.wrench.fill(0.); + command_ = command_target_; +} + +void BaseCommandInterface::log_info() const { + command_guard_->log_info(); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "*** Parameters:"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "* pid.p: %.1f", pid_parameters_.p); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "* pid.i: %.1f", pid_parameters_.i); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "* pid.d: %.1f", pid_parameters_.d); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "* pid.i_max: %.1f", pid_parameters_.i_max); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "* pid.i_min: %.1f", pid_parameters_.i_min); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "* pid.antiwindup: %s", + pid_parameters_.antiwindup ? "true" : "false"); +} +} // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/interfaces/position_command.cpp b/lbr_fri_ros2/src/interfaces/position_command.cpp new file mode 100644 index 00000000..610f44c3 --- /dev/null +++ b/lbr_fri_ros2/src/interfaces/position_command.cpp @@ -0,0 +1,58 @@ +#include "lbr_fri_ros2/interfaces/position_command.hpp" + +namespace lbr_fri_ros2 { +PositionCommandInterface::PositionCommandInterface( + const PIDParameters &pid_parameters, const CommandGuardParameters &command_guard_parameters, + const std::string &command_guard_variant) + : BaseCommandInterface(pid_parameters, command_guard_parameters, command_guard_variant) {} + +void PositionCommandInterface::buffered_command_to_fri(fri_command_t_ref command, + const_fri_state_t_ref state) { +#if FRICLIENT_VERSION_MAJOR == 1 + if (state.getClientCommandMode() != KUKA::FRI::EClientCommandMode::POSITION) { + std::string err = "Expected robot in " + + EnumMaps::client_command_mode_map(KUKA::FRI::EClientCommandMode::POSITION) + + " command mode."; + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), + ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); + throw std::runtime_error(err); + } +#endif +#if FRICLIENT_VERSION_MAJOR == 2 + if (state.getClientCommandMode() != KUKA::FRI::EClientCommandMode::JOINT_POSITION) { + std::string err = + "Expected robot in " + + EnumMaps::client_command_mode_map(KUKA::FRI::EClientCommandMode::JOINT_POSITION) + + " command mode."; + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME()), err.c_str()); + throw std::runtime_error(err); + } +#endif + if (!command_guard_) { + std::string err = "Uninitialized command guard."; + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), + ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); + throw std::runtime_error(err); + } + + // PID + if (!joint_position_pid_.is_initialized()) { + joint_position_pid_.initialize(pid_parameters_, state.getSampleTime()); + } + joint_position_pid_.compute( + command_target_.joint_position, state.getMeasuredJointPosition(), + std::chrono::nanoseconds(static_cast(state.getSampleTime() * 1.e9)), + command_.joint_position); + + // validate + if (!command_guard_->is_valid_command(command_, state)) { + std::string err = "Invalid command."; + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), + ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); + throw std::runtime_error(err); + } + + // write joint position to output + command.setJointPosition(command_.joint_position.data()); +} +} // end of namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/state_interface.cpp b/lbr_fri_ros2/src/interfaces/state.cpp similarity index 99% rename from lbr_fri_ros2/src/state_interface.cpp rename to lbr_fri_ros2/src/interfaces/state.cpp index 3a914df7..31137b88 100644 --- a/lbr_fri_ros2/src/state_interface.cpp +++ b/lbr_fri_ros2/src/interfaces/state.cpp @@ -1,4 +1,4 @@ -#include "lbr_fri_ros2/state_interface.hpp" +#include "lbr_fri_ros2/interfaces/state.hpp" namespace lbr_fri_ros2 { StateInterface::StateInterface(const StateInterfaceParameters &state_interface_parameters) diff --git a/lbr_fri_ros2/src/interfaces/torque_command.cpp b/lbr_fri_ros2/src/interfaces/torque_command.cpp new file mode 100644 index 00000000..96644360 --- /dev/null +++ b/lbr_fri_ros2/src/interfaces/torque_command.cpp @@ -0,0 +1,45 @@ +#include "lbr_fri_ros2/interfaces/torque_command.hpp" + +namespace lbr_fri_ros2 { +TorqueCommandInterface::TorqueCommandInterface( + const PIDParameters &pid_parameters, const CommandGuardParameters &command_guard_parameters, + const std::string &command_guard_variant) + : BaseCommandInterface(pid_parameters, command_guard_parameters, command_guard_variant) {} + +void TorqueCommandInterface::buffered_command_to_fri(fri_command_t_ref command, + const_fri_state_t_ref state) { + if (state.getClientCommandMode() != KUKA::FRI::EClientCommandMode::TORQUE) { + std::string err = "Expected robot in " + + EnumMaps::client_command_mode_map(KUKA::FRI::EClientCommandMode::TORQUE) + + " command mode."; + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), + ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); + throw std::runtime_error(err); + } + if (!command_guard_) { + std::string err = "Uninitialized command guard."; + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), + ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); + throw std::runtime_error(err); + } + + // PID + if (!joint_position_pid_.is_initialized()) { + joint_position_pid_.initialize(pid_parameters_, state.getSampleTime()); + } + joint_position_pid_.compute( + command_target_.joint_position, state.getMeasuredJointPosition(), + std::chrono::nanoseconds(static_cast(state.getSampleTime() * 1.e9)), + command_.joint_position); + command_.torque = command_target_.torque; + + // validate + if (!command_guard_->is_valid_command(command_, state)) { + throw std::runtime_error("Invalid command"); + } + + // write joint position and torque to output + command.setJointPosition(command_.joint_position.data()); + command.setTorque(command_.torque.data()); +} +} // end of namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/interfaces/wrench_command.cpp b/lbr_fri_ros2/src/interfaces/wrench_command.cpp new file mode 100644 index 00000000..c0d6fc9a --- /dev/null +++ b/lbr_fri_ros2/src/interfaces/wrench_command.cpp @@ -0,0 +1,46 @@ +#include "lbr_fri_ros2/interfaces/wrench_command.hpp" + +namespace lbr_fri_ros2 { +WrenchCommandInterface::WrenchCommandInterface( + const PIDParameters &pid_parameters, const CommandGuardParameters &command_guard_parameters, + const std::string &command_guard_variant) + : BaseCommandInterface(pid_parameters, command_guard_parameters, command_guard_variant) {} + +void WrenchCommandInterface::buffered_command_to_fri(fri_command_t_ref command, + const_fri_state_t_ref state) { + if (state.getClientCommandMode() != KUKA::FRI::EClientCommandMode::WRENCH) { + std::string err = "Expected robot in " + + EnumMaps::client_command_mode_map(KUKA::FRI::EClientCommandMode::WRENCH) + + " command mode."; + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME()), err.c_str()); + throw std::runtime_error(err); + } + if (!command_guard_) { + std::string err = "Uninitialized command guard."; + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME()), err.c_str()); + throw std::runtime_error(err); + } + + // PID + if (!joint_position_pid_.is_initialized()) { + joint_position_pid_.initialize(pid_parameters_, state.getSampleTime()); + } + joint_position_pid_.compute( + command_target_.joint_position, state.getMeasuredJointPosition(), + std::chrono::nanoseconds(static_cast(state.getSampleTime() * 1.e9)), + command_.joint_position); + command_.wrench = command_target_.wrench; + + // validate + if (!command_guard_->is_valid_command(command_, state)) { + std::string err = "Invalid command."; + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), + ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); + throw std::runtime_error(err); + } + + // write joint position and wrench to output + command.setJointPosition(command_.joint_position.data()); + command.setWrench(command_.wrench.data()); +} +} // end of namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/test/test_command_interfaces.cpp b/lbr_fri_ros2/test/test_command_interfaces.cpp new file mode 100644 index 00000000..bc65fed4 --- /dev/null +++ b/lbr_fri_ros2/test/test_command_interfaces.cpp @@ -0,0 +1,155 @@ +#include +#include +#include + +#include "friClientApplication.h" +#include "friClientIf.h" +#include "friLBRClient.h" +#include "friUdpConnection.h" +#include "friVersion.h" + +#include "lbr_fri_msgs/msg/lbr_command.hpp" +#include "lbr_fri_ros2/interfaces/base_command.hpp" +#include "lbr_fri_ros2/interfaces/position_command.hpp" +#include "lbr_fri_ros2/interfaces/torque_command.hpp" +#include "lbr_fri_ros2/interfaces/wrench_command.hpp" + +class TestCommandInterfaces : public ::testing::Test { +public: + TestCommandInterfaces() : random_engine_(std::random_device{}()) { + pid_params_ = lbr_fri_ros2::PIDParameters(); + cmd_guard_params_ = lbr_fri_ros2::CommandGuardParameters(); + + // pseudo application + udp_connection_ = std::make_unique(); + lbr_client_ = std::make_shared(); + pseudo_application_ = + std::make_unique(*udp_connection_, *lbr_client_); + } + + void set_up(const KUKA::FRI::EClientCommandMode &client_command_mode) { + switch (client_command_mode) { +#if FRICLIENT_VERSION_MAJOR == 1 + case KUKA::FRI::EClientCommandMode::POSITION: +#endif +#if FRICLIENT_VERSION_MAJOR == 2 + case KUKA::FRI::EClientCommandMode::JOINT_POSITION: +#endif + { + command_interface_ = + std::make_shared(pid_params_, cmd_guard_params_); + break; + } + case KUKA::FRI::EClientCommandMode::TORQUE: + command_interface_ = + std::make_shared(pid_params_, cmd_guard_params_); + break; + case KUKA::FRI::EClientCommandMode::WRENCH: + command_interface_ = + std::make_shared(pid_params_, cmd_guard_params_); + break; + default: + throw std::runtime_error("Unsupported client command mode."); + } + } + + lbr_fri_msgs::msg::LBRCommand random_idl_command() { + lbr_fri_msgs::msg::LBRCommand idl_command; + for (auto &joint_position : idl_command.joint_position) { + joint_position = uniform_real_dist_(random_engine_); + } + for (auto &torque : idl_command.torque) { + torque = uniform_real_dist_(random_engine_); + } + for (auto &wrench : idl_command.wrench) { + wrench = uniform_real_dist_(random_engine_); + } + return idl_command; + } + +protected: + void test_simple() { + // test read only + auto idl_command = command_interface_->get_command(); + auto idl_command_target = command_interface_->get_command_target(); + + // modify and expect unchanged + idl_command.joint_position[0] += 1.0; + idl_command_target.joint_position[0] += 1.0; + + EXPECT_FALSE(idl_command.joint_position[0] == + command_interface_->get_command().joint_position[0]); + EXPECT_FALSE(idl_command_target.joint_position[0] == + command_interface_->get_command_target().joint_position[0]); + + // assume user sets random command target + idl_command_target = random_idl_command(); + command_interface_->buffer_command_target(idl_command_target); + + // initialize commands to state + command_interface_->init_command(lbr_client_->robotState()); + + // expect command target is state now (zero initialized here) + for (std::size_t i = 0; i < idl_command_target.joint_position.size(); ++i) { + // expect joint position is initialized with current robot state + EXPECT_DOUBLE_EQ(command_interface_->get_command().joint_position[i], + lbr_client_->robotState().getMeasuredJointPosition()[i]); + EXPECT_DOUBLE_EQ(command_interface_->get_command_target().joint_position[i], + lbr_client_->robotState().getMeasuredJointPosition()[i]); + + // expect torques are zero + EXPECT_DOUBLE_EQ(command_interface_->get_command().torque[i], 0.0); + EXPECT_DOUBLE_EQ(command_interface_->get_command_target().torque[i], 0.0); + } + for (std::size_t i = 0; i < idl_command_target.wrench.size(); ++i) { + // expect wrenches are zero + EXPECT_DOUBLE_EQ(command_interface_->get_command().wrench[i], 0.0); + EXPECT_DOUBLE_EQ(command_interface_->get_command_target().wrench[i], 0.0); + } + + // buffer a random command and expect invalid client command mode + bool invalid_mode_triggered = false; + try { + idl_command_target = random_idl_command(); + command_interface_->buffer_command_target(idl_command_target); + command_interface_->buffered_command_to_fri(lbr_client_->robotCommand(), + lbr_client_->robotState()); + } catch (const std::exception &) { + invalid_mode_triggered = true; + } + EXPECT_TRUE(invalid_mode_triggered); + } + + std::default_random_engine random_engine_; + std::uniform_real_distribution uniform_real_dist_{-1.0, 1.0}; + + lbr_fri_ros2::PIDParameters pid_params_; + lbr_fri_ros2::CommandGuardParameters cmd_guard_params_; + + std::shared_ptr command_interface_; + + // pseudo application + std::shared_ptr lbr_client_; + std::unique_ptr udp_connection_; + std::unique_ptr pseudo_application_; +}; + +TEST_F(TestCommandInterfaces, TestPositionCommandInterface) { + set_up(KUKA::FRI::EClientCommandMode::POSITION); + test_simple(); +} + +TEST_F(TestCommandInterfaces, TestTorqueCommandInterface) { + set_up(KUKA::FRI::EClientCommandMode::TORQUE); + test_simple(); +} + +TEST_F(TestCommandInterfaces, TestWrenchCommandInterface) { + set_up(KUKA::FRI::EClientCommandMode::WRENCH); + test_simple(); +} + +int main(int argc, char **argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From d57a5a59fc8d6ecd7c4e679f7b979fbd8a3950d4 Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 10 May 2024 17:51:32 +0100 Subject: [PATCH 54/76] added some more temporary examples --- lbr_fri_ros2/CMakeLists.txt | 10 ++ lbr_fri_ros2/src/app.cpp | 2 +- .../src/interfaces/position_command.cpp | 5 +- .../src/interfaces/torque_command.cpp | 5 +- .../src/interfaces/wrench_command.cpp | 5 +- lbr_fri_ros2/test/test_position_command.cpp | 99 +++++++++++++++++++ lbr_fri_ros2/test/test_torque_command.cpp | 82 +++++++++++++++ lbr_fri_ros2/test/test_wrench_command.cpp | 82 +++++++++++++++ 8 files changed, 283 insertions(+), 7 deletions(-) create mode 100644 lbr_fri_ros2/test/test_position_command.cpp create mode 100644 lbr_fri_ros2/test/test_torque_command.cpp create mode 100644 lbr_fri_ros2/test/test_wrench_command.cpp diff --git a/lbr_fri_ros2/CMakeLists.txt b/lbr_fri_ros2/CMakeLists.txt index 1f4572bf..d7496dc4 100644 --- a/lbr_fri_ros2/CMakeLists.txt +++ b/lbr_fri_ros2/CMakeLists.txt @@ -86,6 +86,16 @@ if(BUILD_TESTING) ament_add_gtest(test_command_interfaces test/test_command_interfaces.cpp) target_link_libraries(test_command_interfaces lbr_fri_ros2) + + # some examples of how to use the interfaces + add_executable(test_position_command test/test_position_command.cpp) + target_link_libraries(test_position_command lbr_fri_ros2) + + add_executable(test_torque_command test/test_torque_command.cpp) + target_link_libraries(test_torque_command lbr_fri_ros2) + + add_executable(test_wrench_command test/test_wrench_command.cpp) + target_link_libraries(test_wrench_command lbr_fri_ros2) endif() ament_package() diff --git a/lbr_fri_ros2/src/app.cpp b/lbr_fri_ros2/src/app.cpp index da9c291a..70d7553a 100644 --- a/lbr_fri_ros2/src/app.cpp +++ b/lbr_fri_ros2/src/app.cpp @@ -104,7 +104,7 @@ void App::run_async(int rt_prio) { running_ = true; bool success = true; while (rclcpp::ok() && success && !should_stop_) { - success = app_ptr_->step(); + success = app_ptr_->step(); // stuck if never connected if (async_client_ptr_->robotState().getSessionState() == KUKA::FRI::ESessionState::IDLE) { RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "LBR in session state idle, exiting"); break; diff --git a/lbr_fri_ros2/src/interfaces/position_command.cpp b/lbr_fri_ros2/src/interfaces/position_command.cpp index 610f44c3..2174c7bd 100644 --- a/lbr_fri_ros2/src/interfaces/position_command.cpp +++ b/lbr_fri_ros2/src/interfaces/position_command.cpp @@ -10,9 +10,10 @@ void PositionCommandInterface::buffered_command_to_fri(fri_command_t_ref command const_fri_state_t_ref state) { #if FRICLIENT_VERSION_MAJOR == 1 if (state.getClientCommandMode() != KUKA::FRI::EClientCommandMode::POSITION) { - std::string err = "Expected robot in " + + std::string err = "Expected robot in '" + EnumMaps::client_command_mode_map(KUKA::FRI::EClientCommandMode::POSITION) + - " command mode."; + "' command mode got '" + + EnumMaps::client_command_mode_map(state.getClientCommandMode()) + "'"; RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); throw std::runtime_error(err); diff --git a/lbr_fri_ros2/src/interfaces/torque_command.cpp b/lbr_fri_ros2/src/interfaces/torque_command.cpp index 96644360..3020a44d 100644 --- a/lbr_fri_ros2/src/interfaces/torque_command.cpp +++ b/lbr_fri_ros2/src/interfaces/torque_command.cpp @@ -9,9 +9,10 @@ TorqueCommandInterface::TorqueCommandInterface( void TorqueCommandInterface::buffered_command_to_fri(fri_command_t_ref command, const_fri_state_t_ref state) { if (state.getClientCommandMode() != KUKA::FRI::EClientCommandMode::TORQUE) { - std::string err = "Expected robot in " + + std::string err = "Expected robot in '" + EnumMaps::client_command_mode_map(KUKA::FRI::EClientCommandMode::TORQUE) + - " command mode."; + "' command mode got '" + + EnumMaps::client_command_mode_map(state.getClientCommandMode()) + "'"; RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); throw std::runtime_error(err); diff --git a/lbr_fri_ros2/src/interfaces/wrench_command.cpp b/lbr_fri_ros2/src/interfaces/wrench_command.cpp index c0d6fc9a..1944eafd 100644 --- a/lbr_fri_ros2/src/interfaces/wrench_command.cpp +++ b/lbr_fri_ros2/src/interfaces/wrench_command.cpp @@ -9,9 +9,10 @@ WrenchCommandInterface::WrenchCommandInterface( void WrenchCommandInterface::buffered_command_to_fri(fri_command_t_ref command, const_fri_state_t_ref state) { if (state.getClientCommandMode() != KUKA::FRI::EClientCommandMode::WRENCH) { - std::string err = "Expected robot in " + + std::string err = "Expected robot in '" + EnumMaps::client_command_mode_map(KUKA::FRI::EClientCommandMode::WRENCH) + - " command mode."; + "' command mode got '" + + EnumMaps::client_command_mode_map(state.getClientCommandMode()) + "'"; RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME()), err.c_str()); throw std::runtime_error(err); } diff --git a/lbr_fri_ros2/test/test_position_command.cpp b/lbr_fri_ros2/test/test_position_command.cpp new file mode 100644 index 00000000..d3d10830 --- /dev/null +++ b/lbr_fri_ros2/test/test_position_command.cpp @@ -0,0 +1,99 @@ +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "friClientIf.h" + +#include "lbr_fri_ros2/app.hpp" +#include "lbr_fri_ros2/async_client.hpp" +#include "lbr_fri_ros2/command_guard.hpp" +#include "lbr_fri_ros2/filters.hpp" + +int main() { + rclcpp::init(0, nullptr); + + lbr_fri_ros2::PIDParameters pid_params; + lbr_fri_ros2::CommandGuardParameters cmd_guard_params; + lbr_fri_ros2::StateInterfaceParameters state_interface_params; + + // #include "hardware_interface/hardware_info.hpp" + // hardware_interface::HardwareInfo hardware_info; + + // 1. read this info!!!! from robot description + + pid_params.p = 10.0; + + cmd_guard_params.joint_names = {"A1", "A2", "A3", "A4", "A5", "A6", "A7"}; + cmd_guard_params.max_position = {170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., + 120. * M_PI / 180., 170. * M_PI / 180., 120. * M_PI / 180., + 175. * M_PI / 180.}; + cmd_guard_params.min_position = {-170. * M_PI / 180., -120. * M_PI / 180., -170. * M_PI / 180., + -120. * M_PI / 180., -170. * M_PI / 180., -120. * M_PI / 180., + -175. * M_PI / 180.}; + cmd_guard_params.max_velocity = {98. * M_PI / 180., 98. * M_PI / 180., 100. * M_PI / 180., + 130. * M_PI / 180., 140. * M_PI / 180., 180. * M_PI / 180., + 180. * M_PI / 180.}; + cmd_guard_params.max_torque = { + 200., 200., 200., 200., 200., 200., 200., + }; + + auto client = std::make_shared(KUKA::FRI::EClientCommandMode::POSITION, + pid_params, cmd_guard_params, "default", + state_interface_params, true); + lbr_fri_ros2::App app(client); + + app.open_udp_socket(); + app.run_async(); + + // 2. build some sort of dummy udp connection to emulate the robot + + auto node = std::make_shared("test_async_client"); + + while (!client->get_state_interface()->is_initialized()) { + if (!rclcpp::ok()) { + app.request_stop(); + app.close_udp_socket(); + return 0; + } + RCLCPP_INFO(node->get_logger(), "Waiting for state interface initialization"); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + lbr_fri_msgs::msg::LBRCommand command; + while (rclcpp::ok()) { + // read state + auto state = client->get_state_interface()->get_state(); + RCLCPP_INFO(node->get_logger(), "Measured joint position: %f %f %f %f %f %f %f", + state.measured_joint_position[0], state.measured_joint_position[1], + state.measured_joint_position[2], state.measured_joint_position[3], + state.measured_joint_position[4], state.measured_joint_position[5], + state.measured_joint_position[6]); + + // set command + command.joint_position = state.measured_joint_position; + command.joint_position[6] += 0.001; + client->get_command_interface()->buffer_command_target(command); + + // 3. test the interfaced for safe interaction + + auto command_target = client->get_command_interface()->get_command_target(); + command_target.joint_position[6] += 0.001; // must not change internal value! + command_target = client->get_command_interface()->get_command_target(); + + RCLCPP_INFO(node->get_logger(), "Command joint position: %f %f %f %f %f %f %f", + command_target.joint_position[0], command_target.joint_position[1], + command_target.joint_position[2], command_target.joint_position[3], + command_target.joint_position[4], command_target.joint_position[5], + command_target.joint_position[6]); + + // spin + rclcpp::spin_some(node); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + app.request_stop(); + app.close_udp_socket(); + + return 0; +} diff --git a/lbr_fri_ros2/test/test_torque_command.cpp b/lbr_fri_ros2/test/test_torque_command.cpp new file mode 100644 index 00000000..164c16da --- /dev/null +++ b/lbr_fri_ros2/test/test_torque_command.cpp @@ -0,0 +1,82 @@ +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "friClientIf.h" + +#include "lbr_fri_ros2/app.hpp" +#include "lbr_fri_ros2/async_client.hpp" +#include "lbr_fri_ros2/command_guard.hpp" +#include "lbr_fri_ros2/filters.hpp" + +int main() { + rclcpp::init(0, nullptr); + + lbr_fri_ros2::PIDParameters pid_params; + lbr_fri_ros2::CommandGuardParameters cmd_guard_params; + lbr_fri_ros2::StateInterfaceParameters state_interface_params; + + // #include "hardware_interface/hardware_info.hpp" + // hardware_interface::HardwareInfo hardware_info; + + // 1. read this info!!!! from robot description + + pid_params.p = 10.0; + + cmd_guard_params.joint_names = {"A1", "A2", "A3", "A4", "A5", "A6", "A7"}; + cmd_guard_params.max_position = {170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., + 120. * M_PI / 180., 170. * M_PI / 180., 120. * M_PI / 180., + 175. * M_PI / 180.}; + cmd_guard_params.min_position = {-170. * M_PI / 180., -120. * M_PI / 180., -170. * M_PI / 180., + -120. * M_PI / 180., -170. * M_PI / 180., -120. * M_PI / 180., + -175. * M_PI / 180.}; + cmd_guard_params.max_velocity = {98. * M_PI / 180., 98. * M_PI / 180., 100. * M_PI / 180., + 130. * M_PI / 180., 140. * M_PI / 180., 180. * M_PI / 180., + 180. * M_PI / 180.}; + cmd_guard_params.max_torque = { + 200., 200., 200., 200., 200., 200., 200., + }; + + auto client = std::make_shared(KUKA::FRI::EClientCommandMode::TORQUE, + pid_params, cmd_guard_params, "default", + state_interface_params, true); + lbr_fri_ros2::App app(client); + + app.open_udp_socket(); + app.run_async(); + + // 2. build some sort of dummy udp connection to emulate the robot + + auto node = std::make_shared("test_async_client"); + + while (!client->get_state_interface()->is_initialized()) { + if (!rclcpp::ok()) { + app.request_stop(); + app.close_udp_socket(); + return 0; + } + RCLCPP_INFO(node->get_logger(), "Waiting for state interface initialization"); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + lbr_fri_msgs::msg::LBRCommand command; + while (rclcpp::ok()) { + // read state + auto state = client->get_state_interface()->get_state(); + RCLCPP_INFO(node->get_logger(), "Measured joint position: %f %f %f %f %f %f %f", + state.measured_joint_position[0], state.measured_joint_position[1], + state.measured_joint_position[2], state.measured_joint_position[3], + state.measured_joint_position[4], state.measured_joint_position[5], + state.measured_joint_position[6]); + + // spin + rclcpp::spin_some(node); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + app.request_stop(); + app.close_udp_socket(); + + return 0; +} diff --git a/lbr_fri_ros2/test/test_wrench_command.cpp b/lbr_fri_ros2/test/test_wrench_command.cpp new file mode 100644 index 00000000..81f79743 --- /dev/null +++ b/lbr_fri_ros2/test/test_wrench_command.cpp @@ -0,0 +1,82 @@ +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "friClientIf.h" + +#include "lbr_fri_ros2/app.hpp" +#include "lbr_fri_ros2/async_client.hpp" +#include "lbr_fri_ros2/command_guard.hpp" +#include "lbr_fri_ros2/filters.hpp" + +int main() { + rclcpp::init(0, nullptr); + + lbr_fri_ros2::PIDParameters pid_params; + lbr_fri_ros2::CommandGuardParameters cmd_guard_params; + lbr_fri_ros2::StateInterfaceParameters state_interface_params; + + // #include "hardware_interface/hardware_info.hpp" + // hardware_interface::HardwareInfo hardware_info; + + // 1. read this info!!!! from robot description + + pid_params.p = 10.0; + + cmd_guard_params.joint_names = {"A1", "A2", "A3", "A4", "A5", "A6", "A7"}; + cmd_guard_params.max_position = {170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., + 120. * M_PI / 180., 170. * M_PI / 180., 120. * M_PI / 180., + 175. * M_PI / 180.}; + cmd_guard_params.min_position = {-170. * M_PI / 180., -120. * M_PI / 180., -170. * M_PI / 180., + -120. * M_PI / 180., -170. * M_PI / 180., -120. * M_PI / 180., + -175. * M_PI / 180.}; + cmd_guard_params.max_velocity = {98. * M_PI / 180., 98. * M_PI / 180., 100. * M_PI / 180., + 130. * M_PI / 180., 140. * M_PI / 180., 180. * M_PI / 180., + 180. * M_PI / 180.}; + cmd_guard_params.max_torque = { + 200., 200., 200., 200., 200., 200., 200., + }; + + auto client = std::make_shared(KUKA::FRI::EClientCommandMode::WRENCH, + pid_params, cmd_guard_params, "default", + state_interface_params, true); + lbr_fri_ros2::App app(client); + + app.open_udp_socket(); + app.run_async(); + + // 2. build some sort of dummy udp connection to emulate the robot + + auto node = std::make_shared("test_async_client"); + + while (!client->get_state_interface()->is_initialized()) { + if (!rclcpp::ok()) { + app.request_stop(); + app.close_udp_socket(); + return 0; + } + RCLCPP_INFO(node->get_logger(), "Waiting for state interface initialization"); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + lbr_fri_msgs::msg::LBRCommand command; + while (rclcpp::ok()) { + // read state + auto state = client->get_state_interface()->get_state(); + RCLCPP_INFO(node->get_logger(), "Measured joint position: %f %f %f %f %f %f %f", + state.measured_joint_position[0], state.measured_joint_position[1], + state.measured_joint_position[2], state.measured_joint_position[3], + state.measured_joint_position[4], state.measured_joint_position[5], + state.measured_joint_position[6]); + + // spin + rclcpp::spin_some(node); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + app.request_stop(); + app.close_udp_socket(); + + return 0; +} From 7e9ee5598cf46e3f6931a49418fdae745906c432 Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 10 May 2024 17:53:23 +0100 Subject: [PATCH 55/76] fri 2.x support --- lbr_fri_ros2/src/async_client.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/lbr_fri_ros2/src/async_client.cpp b/lbr_fri_ros2/src/async_client.cpp index 3f2cf68d..d44d0830 100644 --- a/lbr_fri_ros2/src/async_client.cpp +++ b/lbr_fri_ros2/src/async_client.cpp @@ -18,10 +18,17 @@ AsyncClient::AsyncClient(const KUKA::FRI::EClientCommandMode &client_command_mod RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), "Command guard variant '" << command_guard_variant.c_str() << "'"); switch (client_command_mode) { +#if FRICLIENT_VERSION_MAJOR == 1 case KUKA::FRI::EClientCommandMode::POSITION: +#endif +#if FRICLIENT_VERSION_MAJOR == 2 + case KUKA::FRI::EClientCommandMode::JOINT_POSITION: +#endif + { command_interface_ptr_ = std::make_shared( pid_parameters, command_guard_parameters, command_guard_variant); break; + } case KUKA::FRI::EClientCommandMode::TORQUE: command_interface_ptr_ = std::make_shared( pid_parameters, command_guard_parameters, command_guard_variant); From ae1616ebb42fddce156fbcc21f3a8fe6654c89ce Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 10 May 2024 20:55:18 +0100 Subject: [PATCH 56/76] updated system interface for new clients --- .../interfaces/position_command.hpp | 2 + .../interfaces/torque_command.hpp | 2 + .../interfaces/wrench_command.hpp | 2 + .../src/interfaces/position_command.cpp | 4 + .../src/interfaces/torque_command.cpp | 6 ++ .../src/interfaces/wrench_command.cpp | 6 ++ .../config/lbr_system_interface.xacro | 2 + .../lbr_ros2_control/system_interface.hpp | 3 +- lbr_ros2_control/src/system_interface.cpp | 94 ++++++++----------- 9 files changed, 63 insertions(+), 58 deletions(-) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp index 8330581f..b707dd51 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp @@ -1,6 +1,8 @@ #ifndef LBR_FRI_ROS2__INTERFACES__POSITION_COMMAND_HPP_ #define LBR_FRI_ROS2__INTERFACES__POSITION_COMMAND_HPP_ +#include + #include "lbr_fri_ros2/interfaces/base_command.hpp" namespace lbr_fri_ros2 { diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp index c8d08610..f68e6b9f 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp @@ -1,6 +1,8 @@ #ifndef LBR_FRI_ROS2__INTERFACES__TORQUE_COMMAND_HPP_ #define LBR_FRI_ROS2__INTERFACES__TORQUE_COMMAND_HPP_ +#include + #include "lbr_fri_ros2/interfaces/base_command.hpp" namespace lbr_fri_ros2 { diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp index d67aaaed..d42ad4e0 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp @@ -1,6 +1,8 @@ #ifndef LBR_FRI_ROS2__INTERFACES__WRENCH_COMMAND_HPP_ #define LBR_FRI_ROS2__INTERFACES__WRENCH_COMMAND_HPP_ +#include + #include "lbr_fri_ros2/interfaces/base_command.hpp" namespace lbr_fri_ros2 { diff --git a/lbr_fri_ros2/src/interfaces/position_command.cpp b/lbr_fri_ros2/src/interfaces/position_command.cpp index 2174c7bd..8f504a3e 100644 --- a/lbr_fri_ros2/src/interfaces/position_command.cpp +++ b/lbr_fri_ros2/src/interfaces/position_command.cpp @@ -29,6 +29,10 @@ void PositionCommandInterface::buffered_command_to_fri(fri_command_t_ref command throw std::runtime_error(err); } #endif + if (std::any_of(command_target_.joint_position.cbegin(), command_target_.joint_position.cend(), + [](const double &v) { return std::isnan(v); })) { + this->init_command(); + } if (!command_guard_) { std::string err = "Uninitialized command guard."; RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), diff --git a/lbr_fri_ros2/src/interfaces/torque_command.cpp b/lbr_fri_ros2/src/interfaces/torque_command.cpp index 3020a44d..cc9dff21 100644 --- a/lbr_fri_ros2/src/interfaces/torque_command.cpp +++ b/lbr_fri_ros2/src/interfaces/torque_command.cpp @@ -17,6 +17,12 @@ void TorqueCommandInterface::buffered_command_to_fri(fri_command_t_ref command, ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); throw std::runtime_error(err); } + if (std::any_of(command_target_.joint_position.cbegin(), command_target_.joint_position.cend(), + [](const double &v) { return std::isnan(v); }) || + std::any_of(command_target_.torque.cbegin(), command_target_.torque.cend(), + [](const double &v) { return std::isnan(v); })) { + this->init_command(); + } if (!command_guard_) { std::string err = "Uninitialized command guard."; RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), diff --git a/lbr_fri_ros2/src/interfaces/wrench_command.cpp b/lbr_fri_ros2/src/interfaces/wrench_command.cpp index 1944eafd..baf922bc 100644 --- a/lbr_fri_ros2/src/interfaces/wrench_command.cpp +++ b/lbr_fri_ros2/src/interfaces/wrench_command.cpp @@ -16,6 +16,12 @@ void WrenchCommandInterface::buffered_command_to_fri(fri_command_t_ref command, RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME()), err.c_str()); throw std::runtime_error(err); } + if (std::any_of(command_target_.joint_position.cbegin(), command_target_.joint_position.cend(), + [](const double &v) { return std::isnan(v); }) || + std::any_of(command_target_.wrench.cbegin(), command_target_.wrench.cend(), + [](const double &v) { return std::isnan(v); })) { + this->init_command(); + } if (!command_guard_) { std::string err = "Uninitialized command guard."; RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME()), err.c_str()); diff --git a/lbr_ros2_control/config/lbr_system_interface.xacro b/lbr_ros2_control/config/lbr_system_interface.xacro index 63a1c7aa..57aea356 100644 --- a/lbr_ros2_control/config/lbr_system_interface.xacro +++ b/lbr_ros2_control/config/lbr_system_interface.xacro @@ -17,6 +17,7 @@ A6_velocity_limit A7_velocity_limit max_torque + client_command_mode:=^|position sim:=^|true port_id:=^|30200 remote_host:=^|INADDR_ANY @@ -50,6 +51,7 @@ lbr_ros2_control::SystemInterface + ${client_command_mode} ${port_id} ${remote_host} ${rt_prio} diff --git a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp index 1dcfa84a..7b4c1811 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp @@ -26,11 +26,12 @@ #include "lbr_fri_ros2/filters.hpp" #include "lbr_fri_ros2/formatting.hpp" #include "lbr_fri_ros2/ft_estimator.hpp" -#include "lbr_fri_ros2/state_interface.hpp" +#include "lbr_fri_ros2/interfaces/state.hpp" #include "lbr_ros2_control/system_interface_type_values.hpp" namespace lbr_ros2_control { struct SystemInterfaceParameters { + KUKA::FRI::EClientCommandMode client_command_mode{KUKA::FRI::EClientCommandMode::POSITION}; int32_t port_id{30200}; const char *remote_host{nullptr}; int32_t rt_prio{80}; diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 92ca85df..591920ea 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -12,6 +12,27 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { } // parameters_ from config/lbr_system_interface.xacro + std::string client_command_mode = system_info.hardware_parameters.at("client_command_mode"); + if (client_command_mode == "position") { +#if FRICLIENT_VERSION_MAJOR == 1 + parameters_.client_command_mode = KUKA::FRI::EClientCommandMode::POSITION; +#endif +#if FRICLIENT_VERSION_MAJOR == 2 + parameters_.client_command_mode = KUKA::FRI::EClientCommandMode::JOINT_POSITION; +#endif + } else if (client_command_mode == "torque") { + parameters_.client_command_mode = KUKA::FRI::EClientCommandMode::TORQUE; + } else if (client_command_mode == "wrench") { + parameters_.client_command_mode = KUKA::FRI::EClientCommandMode::WRENCH; + } else { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger(LOGGER_NAME), + lbr_fri_ros2::ColorScheme::ERROR + << "Expected client_command_mode 'position', 'torque' or 'wrench', got '" + << lbr_fri_ros2::ColorScheme::BOLD << parameters_.client_command_mode << "'" + << lbr_fri_ros2::ColorScheme::ENDC); + return controller_interface::CallbackReturn::ERROR; + } parameters_.port_id = std::stoul(info_.hardware_parameters["port_id"]); if (parameters_.port_id < 30200 || parameters_.port_id > 30209) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), @@ -72,8 +93,8 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { try { async_client_ptr_ = std::make_shared( - pid_parameters, command_guard_parameters, parameters_.command_guard_variant, - state_interface_parameters, parameters_.open_loop); + parameters_.client_command_mode, pid_parameters, command_guard_parameters, + parameters_.command_guard_variant, state_interface_parameters, parameters_.open_loop); app_ptr_ = std::make_unique(async_client_ptr_); } catch (const std::exception &e) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), @@ -231,9 +252,9 @@ controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_l if (!app_ptr_->open_udp_socket(parameters_.port_id, parameters_.remote_host)) { return controller_interface::CallbackReturn::ERROR; } - app_ptr_->run(parameters_.rt_prio); + app_ptr_->run_async(parameters_.rt_prio); int attempt = 0; - while (!async_client_ptr_->get_state_interface().is_initialized() && rclcpp::ok()) { + while (!async_client_ptr_->get_state_interface()->is_initialized() && rclcpp::ok()) { RCLCPP_INFO_STREAM( rclcpp::get_logger(LOGGER_NAME), "Awaiting robot heartbeat. Attempt " @@ -246,7 +267,7 @@ controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_l std::this_thread::sleep_for(std::chrono::seconds(1)); } if (!async_client_ptr_->get_state_interface() - .is_initialized()) { // check connection should rclcpp::ok() fail + ->is_initialized()) { // check connection should rclcpp::ok() fail RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Failed to connect"); return controller_interface::CallbackReturn::ERROR; } @@ -256,13 +277,13 @@ controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_l RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), "Control mode '" << lbr_fri_ros2::EnumMaps::control_mode_map( - async_client_ptr_->get_state_interface().get_state().control_mode) + async_client_ptr_->get_state_interface()->get_state().control_mode) .c_str() << "'"); RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Sample time %.3f s / %.1f Hz", - async_client_ptr_->get_state_interface().get_state().sample_time, - 1. / async_client_ptr_->get_state_interface().get_state().sample_time); - while (!(async_client_ptr_->get_state_interface().get_state().session_state >= + async_client_ptr_->get_state_interface()->get_state().sample_time, + 1. / async_client_ptr_->get_state_interface()->get_state().sample_time); + while (!(async_client_ptr_->get_state_interface()->get_state().session_state >= KUKA::FRI::ESessionState::COMMANDING_WAIT) && rclcpp::ok()) { RCLCPP_INFO_STREAM( @@ -273,7 +294,7 @@ controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_l << lbr_fri_ros2::ColorScheme::ENDC << "' state. Current state '" << lbr_fri_ros2::ColorScheme::BOLD << lbr_fri_ros2::ColorScheme::OKBLUE << lbr_fri_ros2::EnumMaps::session_state_map( - async_client_ptr_->get_state_interface().get_state().session_state) + async_client_ptr_->get_state_interface()->get_state().session_state) << lbr_fri_ros2::ColorScheme::ENDC << "'."); std::this_thread::sleep_for(std::chrono::seconds(1)); } @@ -285,18 +306,18 @@ controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_l controller_interface::CallbackReturn SystemInterface::on_deactivate(const rclcpp_lifecycle::State &) { - app_ptr_->stop_run(); + app_ptr_->request_stop(); app_ptr_->close_udp_socket(); return controller_interface::CallbackReturn::SUCCESS; } hardware_interface::return_type SystemInterface::read(const rclcpp::Time & /*time*/, const rclcpp::Duration &period) { - if (!async_client_ptr_->get_state_interface().is_initialized()) { + if (!async_client_ptr_->get_state_interface()->is_initialized()) { return hardware_interface::return_type::OK; } - hw_lbr_state_ = async_client_ptr_->get_state_interface().get_state(); + hw_lbr_state_ = async_client_ptr_->get_state_interface()->get_state(); if (period.seconds() - hw_lbr_state_.sample_time * 0.2 > hw_lbr_state_.sample_time) { RCLCPP_WARN_STREAM(rclcpp::get_logger(LOGGER_NAME), @@ -313,7 +334,7 @@ hardware_interface::return_type SystemInterface::read(const rclcpp::Time & /*tim lbr_fri_ros2::ColorScheme::ERROR << "LBR left COMMANDING_ACTIVE. Please re-run lbr_bringup" << lbr_fri_ros2::ColorScheme::ENDC); - app_ptr_->stop_run(); + app_ptr_->request_stop(); app_ptr_->close_udp_socket(); return hardware_interface::return_type::ERROR; } @@ -345,49 +366,8 @@ hardware_interface::return_type SystemInterface::write(const rclcpp::Time & /*ti if (hw_session_state_ != KUKA::FRI::COMMANDING_ACTIVE) { return hardware_interface::return_type::OK; } -#if FRICLIENT_VERSION_MAJOR == 1 - if (hw_client_command_mode_ == KUKA::FRI::EClientCommandMode::POSITION) -#endif -#if FRICLIENT_VERSION_MAJOR == 2 - if (hw_client_command_mode_ == KUKA::FRI::EClientCommandMode::JOINT_POSITION) -#endif - { - if (std::any_of(hw_lbr_command_.joint_position.cbegin(), - hw_lbr_command_.joint_position.cend(), - [](const double &v) { return std::isnan(v); })) { - return hardware_interface::return_type::OK; - } - async_client_ptr_->get_command_interface().set_command_target(hw_lbr_command_); - return hardware_interface::return_type::OK; - } - if (hw_client_command_mode_ == KUKA::FRI::EClientCommandMode::TORQUE) { - if (std::any_of(hw_lbr_command_.joint_position.cbegin(), hw_lbr_command_.joint_position.cend(), - [](const double &v) { return std::isnan(v); }) || - std::any_of(hw_lbr_command_.torque.cbegin(), hw_lbr_command_.torque.cend(), - [](const double &v) { return std::isnan(v); })) { - return hardware_interface::return_type::OK; - } - async_client_ptr_->get_command_interface().set_command_target(hw_lbr_command_); - return hardware_interface::return_type::OK; - } - if (hw_client_command_mode_ == KUKA::FRI::EClientCommandMode::WRENCH) { - if (std::any_of(hw_lbr_command_.wrench.cbegin(), hw_lbr_command_.wrench.cend(), - [](const double &v) { return std::isnan(v); }) || - std::any_of(hw_lbr_command_.joint_position.cbegin(), hw_lbr_command_.joint_position.cend(), - [](const double &v) { return std::isnan(v); })) { - return hardware_interface::return_type::OK; - } - async_client_ptr_->get_command_interface().set_command_target(hw_lbr_command_); - return hardware_interface::return_type::OK; - } -#if FRICLIENT_VERSION_MAJOR == 2 - if (hw_client_command_mode_ == KUKA::FRI::EClientCommandMode::CARTESIAN_POSE) { - throw std::runtime_error( - lbr_fri_ros2::EnumMaps::client_command_mode_map(hw_client_command_mode_) + - " command mode currently not implemented."); - } -#endif - return hardware_interface::return_type::ERROR; + async_client_ptr_->get_command_interface()->buffer_command_target(hw_lbr_command_); + return hardware_interface::return_type::OK; } void SystemInterface::nan_command_interfaces_() { From 971eac11ccc66e1b52636743fd07d7dfbd841a64 Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 10 May 2024 21:07:05 +0100 Subject: [PATCH 57/76] fixed nan init --- lbr_fri_ros2/src/interfaces/position_command.cpp | 2 +- lbr_fri_ros2/src/interfaces/torque_command.cpp | 2 +- lbr_fri_ros2/src/interfaces/wrench_command.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/lbr_fri_ros2/src/interfaces/position_command.cpp b/lbr_fri_ros2/src/interfaces/position_command.cpp index 8f504a3e..96d6e615 100644 --- a/lbr_fri_ros2/src/interfaces/position_command.cpp +++ b/lbr_fri_ros2/src/interfaces/position_command.cpp @@ -31,7 +31,7 @@ void PositionCommandInterface::buffered_command_to_fri(fri_command_t_ref command #endif if (std::any_of(command_target_.joint_position.cbegin(), command_target_.joint_position.cend(), [](const double &v) { return std::isnan(v); })) { - this->init_command(); + this->init_command(state); } if (!command_guard_) { std::string err = "Uninitialized command guard."; diff --git a/lbr_fri_ros2/src/interfaces/torque_command.cpp b/lbr_fri_ros2/src/interfaces/torque_command.cpp index cc9dff21..b4503c79 100644 --- a/lbr_fri_ros2/src/interfaces/torque_command.cpp +++ b/lbr_fri_ros2/src/interfaces/torque_command.cpp @@ -21,7 +21,7 @@ void TorqueCommandInterface::buffered_command_to_fri(fri_command_t_ref command, [](const double &v) { return std::isnan(v); }) || std::any_of(command_target_.torque.cbegin(), command_target_.torque.cend(), [](const double &v) { return std::isnan(v); })) { - this->init_command(); + this->init_command(state); } if (!command_guard_) { std::string err = "Uninitialized command guard."; diff --git a/lbr_fri_ros2/src/interfaces/wrench_command.cpp b/lbr_fri_ros2/src/interfaces/wrench_command.cpp index baf922bc..49fb173f 100644 --- a/lbr_fri_ros2/src/interfaces/wrench_command.cpp +++ b/lbr_fri_ros2/src/interfaces/wrench_command.cpp @@ -20,7 +20,7 @@ void WrenchCommandInterface::buffered_command_to_fri(fri_command_t_ref command, [](const double &v) { return std::isnan(v); }) || std::any_of(command_target_.wrench.cbegin(), command_target_.wrench.cend(), [](const double &v) { return std::isnan(v); })) { - this->init_command(); + this->init_command(state); } if (!command_guard_) { std::string err = "Uninitialized command guard."; From da5ece7a886c792a39875e783faac7bfc436f48b Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 10 May 2024 21:22:38 +0100 Subject: [PATCH 58/76] fri 2.x support --- lbr_fri_ros2/test/test_command_interfaces.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/lbr_fri_ros2/test/test_command_interfaces.cpp b/lbr_fri_ros2/test/test_command_interfaces.cpp index bc65fed4..de5a1939 100644 --- a/lbr_fri_ros2/test/test_command_interfaces.cpp +++ b/lbr_fri_ros2/test/test_command_interfaces.cpp @@ -135,7 +135,12 @@ class TestCommandInterfaces : public ::testing::Test { }; TEST_F(TestCommandInterfaces, TestPositionCommandInterface) { +#if FRICLIENT_VERSION_MAJOR == 1 set_up(KUKA::FRI::EClientCommandMode::POSITION); +#endif +#if FRICLIENT_VERSION_MAJOR == 2 + set_up(KUKA::FRI::EClientCommandMode::JOINT_POSITION); +#endif test_simple(); } From 2c6ac59e0ea39c1e3712781c971e00985a0b3b81 Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 10 May 2024 22:43:39 +0100 Subject: [PATCH 59/76] remove temporary tests from build --- lbr_fri_ros2/CMakeLists.txt | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/lbr_fri_ros2/CMakeLists.txt b/lbr_fri_ros2/CMakeLists.txt index d7496dc4..52856dac 100644 --- a/lbr_fri_ros2/CMakeLists.txt +++ b/lbr_fri_ros2/CMakeLists.txt @@ -87,15 +87,15 @@ if(BUILD_TESTING) ament_add_gtest(test_command_interfaces test/test_command_interfaces.cpp) target_link_libraries(test_command_interfaces lbr_fri_ros2) - # some examples of how to use the interfaces - add_executable(test_position_command test/test_position_command.cpp) - target_link_libraries(test_position_command lbr_fri_ros2) + # # some examples of how to use the interfaces + # add_executable(test_position_command test/test_position_command.cpp) + # target_link_libraries(test_position_command lbr_fri_ros2) - add_executable(test_torque_command test/test_torque_command.cpp) - target_link_libraries(test_torque_command lbr_fri_ros2) + # add_executable(test_torque_command test/test_torque_command.cpp) + # target_link_libraries(test_torque_command lbr_fri_ros2) - add_executable(test_wrench_command test/test_wrench_command.cpp) - target_link_libraries(test_wrench_command lbr_fri_ros2) + # add_executable(test_wrench_command test/test_wrench_command.cpp) + # target_link_libraries(test_wrench_command lbr_fri_ros2) endif() ament_package() From d7b7497be96827786174e5b42d2ff01fdc25e42e Mon Sep 17 00:00:00 2001 From: mhubii Date: Sat, 11 May 2024 11:03:20 +0100 Subject: [PATCH 60/76] fri 2.x support --- .../include/lbr_ros2_control/system_interface.hpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp index 7b4c1811..50006a12 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp @@ -31,7 +31,12 @@ namespace lbr_ros2_control { struct SystemInterfaceParameters { +#if FRICLIENT_VERSION_MAJOR == 1 KUKA::FRI::EClientCommandMode client_command_mode{KUKA::FRI::EClientCommandMode::POSITION}; +#endif +#if FRICLIENT_VERSION_MAJOR == 2 + KUKA::FRI::EClientCommandMode client_command_mode{KUKA::FRI::EClientCommandMode::JOINT_POSITION}; +#endif int32_t port_id{30200}; const char *remote_host{nullptr}; int32_t rt_prio{80}; From 9f95c4156b897977f33aa1c28d5436a3aae8339f Mon Sep 17 00:00:00 2001 From: mhubii Date: Sat, 11 May 2024 11:24:14 +0100 Subject: [PATCH 61/76] msgs -> idl --- .../lbr_demos_advanced_cpp/CMakeLists.txt | 6 +++--- lbr_demos/lbr_demos_advanced_cpp/package.xml | 2 +- .../src/admittance_control_node.cpp | 8 ++++---- .../src/admittance_controller.hpp | 8 ++++---- .../src/lbr_base_position_command_node.hpp | 14 ++++++------- .../src/pose_control_node.cpp | 16 +++++++-------- .../admittance_control_node.py | 2 +- .../admittance_controller.py | 2 +- .../admittance_rcm_control_node.py | 2 +- .../lbr_base_position_command_node.py | 2 +- lbr_demos/lbr_demos_advanced_py/package.xml | 2 +- lbr_demos/lbr_demos_cpp/CMakeLists.txt | 8 ++++---- lbr_demos/lbr_demos_cpp/package.xml | 2 +- .../lbr_demos_cpp/src/joint_sine_overlay.cpp | 20 +++++++++---------- .../lbr_demos_cpp/src/torque_sine_overlay.cpp | 20 +++++++++---------- .../lbr_demos_cpp/src/wrench_sine_overlay.cpp | 20 +++++++++---------- .../lbr_demos_py/joint_sine_overlay.py | 4 ++-- .../lbr_demos_py/torque_sine_overlay.py | 4 ++-- .../lbr_demos_py/wrench_sine_overlay.py | 4 ++-- lbr_demos/lbr_demos_py/package.xml | 2 +- lbr_fri_ros2/CMakeLists.txt | 6 +++--- lbr_fri_ros2/doc/lbr_fri_ros2.rst | 6 +++--- .../include/lbr_fri_ros2/command_guard.hpp | 4 ++-- lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp | 2 +- .../include/lbr_fri_ros2/ft_estimator.hpp | 6 +++--- .../lbr_fri_ros2/interfaces/base_command.hpp | 4 ++-- .../include/lbr_fri_ros2/interfaces/state.hpp | 4 ++-- lbr_fri_ros2/package.xml | 2 +- lbr_fri_ros2/test/test_command_interfaces.cpp | 6 +++--- lbr_fri_ros2/test/test_position_command.cpp | 2 +- lbr_fri_ros2/test/test_torque_command.cpp | 2 +- lbr_fri_ros2/test/test_wrench_command.cpp | 2 +- lbr_fri_ros2_stack/doc/lbr_fri_ros2_stack.rst | 2 +- lbr_fri_ros2_stack/package.xml | 2 +- lbr_fri_ros2_stack/repos-fri-1.11.yaml | 4 ++-- lbr_fri_ros2_stack/repos-fri-1.14.yaml | 4 ++-- lbr_fri_ros2_stack/repos-fri-1.15.yaml | 4 ++-- lbr_fri_ros2_stack/repos-fri-1.16.yaml | 4 ++-- lbr_fri_ros2_stack/repos-fri-2.5.yaml | 4 ++-- lbr_ros2_control/CMakeLists.txt | 6 +++--- .../lbr_position_command_controller.hpp | 6 +++--- .../lbr_state_broadcaster.hpp | 6 +++--- .../lbr_torque_command_controller.hpp | 6 +++--- .../lbr_wrench_command_controller.hpp | 6 +++--- .../lbr_ros2_control/system_interface.hpp | 12 +++++------ lbr_ros2_control/package.xml | 2 +- .../plugin_description_files/broadcasters.xml | 2 +- .../forward_command_controllers.xml | 6 +++--- .../src/lbr_position_command_controller.cpp | 4 ++-- .../src/lbr_state_broadcaster.cpp | 4 ++-- .../src/lbr_torque_command_controller.cpp | 4 ++-- .../src/lbr_wrench_command_controller.cpp | 4 ++-- 52 files changed, 143 insertions(+), 143 deletions(-) diff --git a/lbr_demos/lbr_demos_advanced_cpp/CMakeLists.txt b/lbr_demos/lbr_demos_advanced_cpp/CMakeLists.txt index 60151efb..14040864 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/CMakeLists.txt +++ b/lbr_demos/lbr_demos_advanced_cpp/CMakeLists.txt @@ -14,8 +14,8 @@ find_package(ament_cmake REQUIRED) find_package(FRIClient REQUIRED) find_package(geometry_msgs REQUIRED) find_package(kdl_parser REQUIRED) +find_package(lbr_fri_idl REQUIRED) find_package(lbr_fri_ros2 REQUIRED) -find_package(lbr_fri_msgs REQUIRED) find_package(orocos_kdl_vendor REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) @@ -32,8 +32,8 @@ target_include_directories(admittance_control_component ament_target_dependencies(admittance_control_component kdl_parser + lbr_fri_idl lbr_fri_ros2 - lbr_fri_msgs orocos_kdl_vendor rclcpp rclcpp_components @@ -82,7 +82,7 @@ target_include_directories(pose_control_component ament_target_dependencies(pose_control_component geometry_msgs kdl_parser - lbr_fri_msgs + lbr_fri_idl orocos_kdl_vendor rclcpp rclcpp_components diff --git a/lbr_demos/lbr_demos_advanced_cpp/package.xml b/lbr_demos/lbr_demos_advanced_cpp/package.xml index d950d9f6..eab5792e 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/package.xml +++ b/lbr_demos/lbr_demos_advanced_cpp/package.xml @@ -14,8 +14,8 @@ FRIClient geometry_msgs kdl_parser + lbr_fri_idl lbr_fri_ros2 - lbr_fri_msgs orocos_kdl_vendor rclcpp rclcpp_components diff --git a/lbr_demos/lbr_demos_advanced_cpp/src/admittance_control_node.cpp b/lbr_demos/lbr_demos_advanced_cpp/src/admittance_control_node.cpp index feb5d788..85e71be8 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/src/admittance_control_node.cpp +++ b/lbr_demos/lbr_demos_advanced_cpp/src/admittance_control_node.cpp @@ -2,7 +2,7 @@ #include "rclcpp/rclcpp.hpp" -#include "lbr_fri_msgs/msg/lbr_state.hpp" +#include "lbr_fri_idl/msg/lbr_state.hpp" #include "admittance_controller.hpp" #include "lbr_base_position_command_node.hpp" @@ -54,7 +54,7 @@ class AdmittanceControlNode : public LBRBasePositionCommandNode { dx_gains[1], dx_gains[2], dx_gains[3], dx_gains[4], dx_gains[5]); } - void on_lbr_state_(const lbr_fri_msgs::msg::LBRState::SharedPtr lbr_state) override { + void on_lbr_state_(const lbr_fri_idl::msg::LBRState::SharedPtr lbr_state) override { if (!lbr_state) { return; } @@ -65,7 +65,7 @@ class AdmittanceControlNode : public LBRBasePositionCommandNode { lbr_position_command_pub_->publish(lbr_command); }; - void smooth_lbr_state_(const lbr_fri_msgs::msg::LBRState::SharedPtr lbr_state) { + void smooth_lbr_state_(const lbr_fri_idl::msg::LBRState::SharedPtr lbr_state) { if (!init_) { lbr_state_ = *lbr_state; init_ = true; @@ -83,7 +83,7 @@ class AdmittanceControlNode : public LBRBasePositionCommandNode { double exp_smooth_; bool init_{false}; - lbr_fri_msgs::msg::LBRState lbr_state_; + lbr_fri_idl::msg::LBRState lbr_state_; std::unique_ptr admittance_controller_; }; diff --git a/lbr_demos/lbr_demos_advanced_cpp/src/admittance_controller.hpp b/lbr_demos/lbr_demos_advanced_cpp/src/admittance_controller.hpp index 07c11956..9a8aa0e8 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/src/admittance_controller.hpp +++ b/lbr_demos/lbr_demos_advanced_cpp/src/admittance_controller.hpp @@ -12,8 +12,8 @@ #include "friLBRState.h" -#include "lbr_fri_msgs/msg/lbr_position_command.hpp" -#include "lbr_fri_msgs/msg/lbr_state.hpp" +#include "lbr_fri_idl/msg/lbr_position_command.hpp" +#include "lbr_fri_idl/msg/lbr_state.hpp" #include "lbr_fri_ros2/pinv.hpp" namespace lbr_demos { @@ -41,7 +41,7 @@ class AdmittanceController { q_.resize(chain_.getNrOfJoints()); }; - const lbr_fri_msgs::msg::LBRPositionCommand &update(const lbr_fri_msgs::msg::LBRState &lbr_state, + const lbr_fri_idl::msg::LBRPositionCommand &update(const lbr_fri_idl::msg::LBRState &lbr_state, const double &dt) { std::memcpy(q_.data.data(), lbr_state.measured_joint_position.data(), sizeof(double) * KUKA::FRI::LBRState::NUMBER_OF_JOINTS); @@ -71,7 +71,7 @@ class AdmittanceController { }; protected: - lbr_fri_msgs::msg::LBRPositionCommand lbr_position_command_; + lbr_fri_idl::msg::LBRPositionCommand lbr_position_command_; KDL::Tree tree_; KDL::Chain chain_; std::unique_ptr jacobian_solver_; diff --git a/lbr_demos/lbr_demos_advanced_cpp/src/lbr_base_position_command_node.hpp b/lbr_demos/lbr_demos_advanced_cpp/src/lbr_base_position_command_node.hpp index c455f7f4..26f6249d 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/src/lbr_base_position_command_node.hpp +++ b/lbr_demos/lbr_demos_advanced_cpp/src/lbr_base_position_command_node.hpp @@ -6,8 +6,8 @@ #include "rclcpp/rclcpp.hpp" -#include "lbr_fri_msgs/msg/lbr_position_command.hpp" -#include "lbr_fri_msgs/msg/lbr_state.hpp" +#include "lbr_fri_idl/msg/lbr_position_command.hpp" +#include "lbr_fri_idl/msg/lbr_state.hpp" namespace lbr_demos { class LBRBasePositionCommandNode : public rclcpp::Node { @@ -22,23 +22,23 @@ class LBRBasePositionCommandNode : public rclcpp::Node { // publishers and subscribers lbr_position_command_pub_ = - create_publisher("command/joint_position", 1); + create_publisher("command/joint_position", 1); - lbr_state_sub_ = create_subscription( + lbr_state_sub_ = create_subscription( "state", 1, std::bind(&LBRBasePositionCommandNode::on_lbr_state_, this, std::placeholders::_1)); } protected: - rclcpp::Publisher::SharedPtr lbr_position_command_pub_; - rclcpp::Subscription::SharedPtr lbr_state_sub_; + rclcpp::Publisher::SharedPtr lbr_position_command_pub_; + rclcpp::Subscription::SharedPtr lbr_state_sub_; std::string robot_description_; int64_t update_rate_; double dt_; protected: - virtual void on_lbr_state_(const lbr_fri_msgs::msg::LBRState::SharedPtr lbr_state) = 0; + virtual void on_lbr_state_(const lbr_fri_idl::msg::LBRState::SharedPtr lbr_state) = 0; rclcpp::Parameter retrieve_parameter_(const std::string &remote_node_name, const std::string ¶meter_name) { diff --git a/lbr_demos/lbr_demos_advanced_cpp/src/pose_control_node.cpp b/lbr_demos/lbr_demos_advanced_cpp/src/pose_control_node.cpp index dc8b24c9..09832c3e 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/src/pose_control_node.cpp +++ b/lbr_demos/lbr_demos_advanced_cpp/src/pose_control_node.cpp @@ -8,7 +8,7 @@ #include "friClientIf.h" #include "friLBRState.h" -#include "lbr_fri_msgs/msg/lbr_state.hpp" +#include "lbr_fri_idl/msg/lbr_state.hpp" #include "lbr_base_position_command_node.hpp" @@ -18,8 +18,8 @@ class PoseControlNode : public LBRBasePositionCommandNode { rclcpp::Publisher::SharedPtr pose_pub_; rclcpp::Subscription::SharedPtr pose_sub_; - lbr_fri_msgs::msg::LBRState current_lbr_state_; // robot state, including joint positions - geometry_msgs::msg::Pose current_pose_; // current pose of robot + lbr_fri_idl::msg::LBRState current_lbr_state_; // robot state, including joint positions + geometry_msgs::msg::Pose current_pose_; // current pose of robot KDL::Chain chain_; // robot kinematics chain exetracted from robot URDF file @@ -50,7 +50,7 @@ class PoseControlNode : public LBRBasePositionCommandNode { } protected: - void on_lbr_state_(const lbr_fri_msgs::msg::LBRState::SharedPtr lbr_state) override { + void on_lbr_state_(const lbr_fri_idl::msg::LBRState::SharedPtr lbr_state) override { current_lbr_state_ = *lbr_state; double joint_position[KUKA::FRI::LBRState::NUMBER_OF_JOINTS]; @@ -69,7 +69,7 @@ class PoseControlNode : public LBRBasePositionCommandNode { current_joint_positions(i) = current_lbr_state_.measured_joint_position[i]; } - lbr_fri_msgs::msg::LBRPositionCommand joint_position_command = + lbr_fri_idl::msg::LBRPositionCommand joint_position_command = compute_ik_(msg, current_joint_positions); lbr_position_command_pub_->publish(joint_position_command); } @@ -106,11 +106,11 @@ class PoseControlNode : public LBRBasePositionCommandNode { return pose; } - lbr_fri_msgs::msg::LBRPositionCommand compute_ik_(const geometry_msgs::msg::Pose &desired_pose, - KDL::JntArray ¤t_joint_positions) { + lbr_fri_idl::msg::LBRPositionCommand compute_ik_(const geometry_msgs::msg::Pose &desired_pose, + KDL::JntArray ¤t_joint_positions) { KDL::ChainIkSolverPos_LMA ik_solver(chain_); KDL::JntArray result_joint_positions = KDL::JntArray(chain_.getNrOfJoints()); - lbr_fri_msgs::msg::LBRPositionCommand joint_position_command; + lbr_fri_idl::msg::LBRPositionCommand joint_position_command; // transfer data type 'geometry::msg::Pose' to be 'KDL::Frame' KDL::Vector position(desired_pose.position.x, desired_pose.position.y, desired_pose.position.z); diff --git a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_control_node.py b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_control_node.py index 51300203..ee2e1a41 100755 --- a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_control_node.py +++ b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_control_node.py @@ -1,7 +1,7 @@ import numpy as np import rclpy -from lbr_fri_msgs.msg import LBRState +from lbr_fri_idl.msg import LBRState from .admittance_controller import AdmittanceController from .lbr_base_position_command_node import LBRBasePositionCommandNode diff --git a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_controller.py b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_controller.py index 697b5e33..ecb19cc8 100644 --- a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_controller.py +++ b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_controller.py @@ -1,7 +1,7 @@ import numpy as np import optas -from lbr_fri_msgs.msg import LBRPositionCommand, LBRState +from lbr_fri_idl.msg import LBRPositionCommand, LBRState class AdmittanceController(object): diff --git a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_control_node.py b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_control_node.py index 17d85db4..84194cd2 100644 --- a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_control_node.py +++ b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_control_node.py @@ -1,7 +1,7 @@ import numpy as np import rclpy -from lbr_fri_msgs.msg import LBRPositionCommand, LBRState +from lbr_fri_idl.msg import LBRPositionCommand, LBRState from .admittance_rcm_controller import AdmittanceRCMController from .lbr_base_position_command_node import LBRBasePositionCommandNode diff --git a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/lbr_base_position_command_node.py b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/lbr_base_position_command_node.py index a4d4c65a..6962950c 100644 --- a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/lbr_base_position_command_node.py +++ b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/lbr_base_position_command_node.py @@ -4,7 +4,7 @@ from rcl_interfaces.srv import GetParameters from rclpy.node import Node -from lbr_fri_msgs.msg import LBRPositionCommand, LBRState +from lbr_fri_idl.msg import LBRPositionCommand, LBRState class LBRBasePositionCommandNode(Node): diff --git a/lbr_demos/lbr_demos_advanced_py/package.xml b/lbr_demos/lbr_demos_advanced_py/package.xml index 48644cb5..14eef8dc 100644 --- a/lbr_demos/lbr_demos_advanced_py/package.xml +++ b/lbr_demos/lbr_demos_advanced_py/package.xml @@ -8,7 +8,7 @@ Apache License 2.0 lbr_description - lbr_fri_msgs + lbr_fri_idl lbr_fri_ros2 rclpy rcl_interfaces diff --git a/lbr_demos/lbr_demos_cpp/CMakeLists.txt b/lbr_demos/lbr_demos_cpp/CMakeLists.txt index 6a5afc3f..7c17273d 100644 --- a/lbr_demos/lbr_demos_cpp/CMakeLists.txt +++ b/lbr_demos/lbr_demos_cpp/CMakeLists.txt @@ -13,7 +13,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(control_msgs REQUIRED) find_package(FRIClient REQUIRED) -find_package(lbr_fri_msgs REQUIRED) +find_package(lbr_fri_idl REQUIRED) find_package(lbr_fri_ros2 REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) @@ -26,7 +26,7 @@ add_executable(joint_sine_overlay ) ament_target_dependencies(joint_sine_overlay - lbr_fri_msgs + lbr_fri_idl lbr_fri_ros2 rclcpp ) @@ -58,7 +58,7 @@ add_executable(torque_sine_overlay ) ament_target_dependencies(torque_sine_overlay - lbr_fri_msgs + lbr_fri_idl lbr_fri_ros2 rclcpp ) @@ -73,7 +73,7 @@ add_executable(wrench_sine_overlay ) ament_target_dependencies(wrench_sine_overlay - lbr_fri_msgs + lbr_fri_idl lbr_fri_ros2 rclcpp ) diff --git a/lbr_demos/lbr_demos_cpp/package.xml b/lbr_demos/lbr_demos_cpp/package.xml index 36d9ca50..32ea387e 100644 --- a/lbr_demos/lbr_demos_cpp/package.xml +++ b/lbr_demos/lbr_demos_cpp/package.xml @@ -11,7 +11,7 @@ control_msgs FRIClient - lbr_fri_msgs + lbr_fri_idl lbr_fri_ros2 rclcpp rclcpp_action diff --git a/lbr_demos/lbr_demos_cpp/src/joint_sine_overlay.cpp b/lbr_demos/lbr_demos_cpp/src/joint_sine_overlay.cpp index 57918728..20baa25a 100644 --- a/lbr_demos/lbr_demos_cpp/src/joint_sine_overlay.cpp +++ b/lbr_demos/lbr_demos_cpp/src/joint_sine_overlay.cpp @@ -8,9 +8,9 @@ // include fri for session state #include "friClientIf.h" -// include lbr_fri_msgs -#include "lbr_fri_msgs/msg/lbr_position_command.hpp" -#include "lbr_fri_msgs/msg/lbr_state.hpp" +// include lbr_fri_idl +#include "lbr_fri_idl/msg/lbr_position_command.hpp" +#include "lbr_fri_idl/msg/lbr_state.hpp" #include "lbr_fri_ros2/utils.hpp" class JointSineOverlay { @@ -21,10 +21,10 @@ class JointSineOverlay { JointSineOverlay(const rclcpp::Node::SharedPtr node) : node_(node), phase_(0.) { // create publisher to command/joint_position lbr_position_command_pub_ = - node_->create_publisher("command/joint_position", 1); + node_->create_publisher("command/joint_position", 1); // create subscription to state - lbr_state_sub_ = node_->create_subscription( + lbr_state_sub_ = node_->create_subscription( "state", 1, std::bind(&JointSineOverlay::on_lbr_state_, this, std::placeholders::_1)); // get control rate from controller_manager @@ -34,7 +34,7 @@ class JointSineOverlay { }; protected: - void on_lbr_state_(const lbr_fri_msgs::msg::LBRState::SharedPtr lbr_state) { + void on_lbr_state_(const lbr_fri_idl::msg::LBRState::SharedPtr lbr_state) { if (!lbr_state_init_) { lbr_state_ = *lbr_state; lbr_state_init_ = true; @@ -57,11 +57,11 @@ class JointSineOverlay { rclcpp::Node::SharedPtr node_; double dt_; double phase_; - rclcpp::Publisher::SharedPtr lbr_position_command_pub_; - rclcpp::Subscription::SharedPtr lbr_state_sub_; + rclcpp::Publisher::SharedPtr lbr_position_command_pub_; + rclcpp::Subscription::SharedPtr lbr_state_sub_; bool lbr_state_init_ = false; - lbr_fri_msgs::msg::LBRState lbr_state_; - lbr_fri_msgs::msg::LBRPositionCommand lbr_position_command_; + lbr_fri_idl::msg::LBRState lbr_state_; + lbr_fri_idl::msg::LBRPositionCommand lbr_position_command_; }; int main(int argc, char **argv) { diff --git a/lbr_demos/lbr_demos_cpp/src/torque_sine_overlay.cpp b/lbr_demos/lbr_demos_cpp/src/torque_sine_overlay.cpp index 4d33b3b3..9e700ffe 100644 --- a/lbr_demos/lbr_demos_cpp/src/torque_sine_overlay.cpp +++ b/lbr_demos/lbr_demos_cpp/src/torque_sine_overlay.cpp @@ -8,9 +8,9 @@ // include fri for session state #include "friClientIf.h" -// include lbr_fri_msgs -#include "lbr_fri_msgs/msg/lbr_state.hpp" -#include "lbr_fri_msgs/msg/lbr_torque_command.hpp" +// include lbr_fri_idl +#include "lbr_fri_idl/msg/lbr_state.hpp" +#include "lbr_fri_idl/msg/lbr_torque_command.hpp" #include "lbr_fri_ros2/utils.hpp" class TorqueSineOverlay { @@ -21,10 +21,10 @@ class TorqueSineOverlay { TorqueSineOverlay(const rclcpp::Node::SharedPtr node) : node_(node), phase_(0.) { // create publisher to command/torque lbr_torque_command_pub_ = - node_->create_publisher("command/torque", 1); + node_->create_publisher("command/torque", 1); // create subscription to state - lbr_state_sub_ = node_->create_subscription( + lbr_state_sub_ = node_->create_subscription( "state", 1, std::bind(&TorqueSineOverlay::on_lbr_state_, this, std::placeholders::_1)); // get control rate from controller_manager @@ -34,7 +34,7 @@ class TorqueSineOverlay { }; protected: - void on_lbr_state_(const lbr_fri_msgs::msg::LBRState::SharedPtr lbr_state) { + void on_lbr_state_(const lbr_fri_idl::msg::LBRState::SharedPtr lbr_state) { if (!lbr_state_init_) { lbr_state_ = *lbr_state; lbr_state_init_ = true; @@ -57,11 +57,11 @@ class TorqueSineOverlay { rclcpp::Node::SharedPtr node_; double dt_; double phase_; - rclcpp::Publisher::SharedPtr lbr_torque_command_pub_; - rclcpp::Subscription::SharedPtr lbr_state_sub_; + rclcpp::Publisher::SharedPtr lbr_torque_command_pub_; + rclcpp::Subscription::SharedPtr lbr_state_sub_; bool lbr_state_init_ = false; - lbr_fri_msgs::msg::LBRState lbr_state_; - lbr_fri_msgs::msg::LBRTorqueCommand lbr_torque_command_; + lbr_fri_idl::msg::LBRState lbr_state_; + lbr_fri_idl::msg::LBRTorqueCommand lbr_torque_command_; }; int main(int argc, char **argv) { diff --git a/lbr_demos/lbr_demos_cpp/src/wrench_sine_overlay.cpp b/lbr_demos/lbr_demos_cpp/src/wrench_sine_overlay.cpp index 68e2e9ff..869dbfa2 100644 --- a/lbr_demos/lbr_demos_cpp/src/wrench_sine_overlay.cpp +++ b/lbr_demos/lbr_demos_cpp/src/wrench_sine_overlay.cpp @@ -8,9 +8,9 @@ // include fri for session state #include "friClientIf.h" -// include lbr_fri_msgs -#include "lbr_fri_msgs/msg/lbr_state.hpp" -#include "lbr_fri_msgs/msg/lbr_wrench_command.hpp" +// include lbr_fri_idl +#include "lbr_fri_idl/msg/lbr_state.hpp" +#include "lbr_fri_idl/msg/lbr_wrench_command.hpp" #include "lbr_fri_ros2/utils.hpp" class WrenchSineOverlay { @@ -23,10 +23,10 @@ class WrenchSineOverlay { WrenchSineOverlay(const rclcpp::Node::SharedPtr node) : node_(node), phase_x_(0.), phase_y_(0.) { // create publisher to command/wrench lbr_wrench_command_pub_ = - node_->create_publisher("command/wrench", 1); + node_->create_publisher("command/wrench", 1); // create subscription to state - lbr_state_sub_ = node_->create_subscription( + lbr_state_sub_ = node_->create_subscription( "state", 1, std::bind(&WrenchSineOverlay::on_lbr_state_, this, std::placeholders::_1)); // get control rate from controller_manager @@ -36,7 +36,7 @@ class WrenchSineOverlay { }; protected: - void on_lbr_state_(const lbr_fri_msgs::msg::LBRState::SharedPtr lbr_state) { + void on_lbr_state_(const lbr_fri_idl::msg::LBRState::SharedPtr lbr_state) { if (!lbr_state_init_) { lbr_state_ = *lbr_state; lbr_state_init_ = true; @@ -62,11 +62,11 @@ class WrenchSineOverlay { rclcpp::Node::SharedPtr node_; double dt_; double phase_x_, phase_y_; - rclcpp::Publisher::SharedPtr lbr_wrench_command_pub_; - rclcpp::Subscription::SharedPtr lbr_state_sub_; + rclcpp::Publisher::SharedPtr lbr_wrench_command_pub_; + rclcpp::Subscription::SharedPtr lbr_state_sub_; bool lbr_state_init_ = false; - lbr_fri_msgs::msg::LBRState lbr_state_; - lbr_fri_msgs::msg::LBRWrenchCommand lbr_wrench_command_; + lbr_fri_idl::msg::LBRState lbr_state_; + lbr_fri_idl::msg::LBRWrenchCommand lbr_wrench_command_; }; int main(int argc, char **argv) { diff --git a/lbr_demos/lbr_demos_py/lbr_demos_py/joint_sine_overlay.py b/lbr_demos/lbr_demos_py/lbr_demos_py/joint_sine_overlay.py index 70dc7ee3..34c2e323 100644 --- a/lbr_demos/lbr_demos_py/lbr_demos_py/joint_sine_overlay.py +++ b/lbr_demos/lbr_demos_py/lbr_demos_py/joint_sine_overlay.py @@ -5,8 +5,8 @@ from rcl_interfaces.srv import GetParameters from rclpy.node import Node -# import lbr_fri_msgs -from lbr_fri_msgs.msg import LBRPositionCommand, LBRState +# import lbr_fri_idl +from lbr_fri_idl.msg import LBRPositionCommand, LBRState class JointSineOverlayNode(Node): diff --git a/lbr_demos/lbr_demos_py/lbr_demos_py/torque_sine_overlay.py b/lbr_demos/lbr_demos_py/lbr_demos_py/torque_sine_overlay.py index 904ee29e..db7e6a18 100644 --- a/lbr_demos/lbr_demos_py/lbr_demos_py/torque_sine_overlay.py +++ b/lbr_demos/lbr_demos_py/lbr_demos_py/torque_sine_overlay.py @@ -5,8 +5,8 @@ from rcl_interfaces.srv import GetParameters from rclpy.node import Node -# import lbr_fri_msgs -from lbr_fri_msgs.msg import LBRState, LBRTorqueCommand +# import lbr_fri_idl +from lbr_fri_idl.msg import LBRState, LBRTorqueCommand class TorqueSineOverlayNode(Node): diff --git a/lbr_demos/lbr_demos_py/lbr_demos_py/wrench_sine_overlay.py b/lbr_demos/lbr_demos_py/lbr_demos_py/wrench_sine_overlay.py index af1d951d..d3b44290 100644 --- a/lbr_demos/lbr_demos_py/lbr_demos_py/wrench_sine_overlay.py +++ b/lbr_demos/lbr_demos_py/lbr_demos_py/wrench_sine_overlay.py @@ -5,8 +5,8 @@ from rcl_interfaces.srv import GetParameters from rclpy.node import Node -# import lbr_fri_msgs -from lbr_fri_msgs.msg import LBRState, LBRWrenchCommand +# import lbr_fri_idl +from lbr_fri_idl.msg import LBRState, LBRWrenchCommand class WrenchSineOverlayNode(Node): diff --git a/lbr_demos/lbr_demos_py/package.xml b/lbr_demos/lbr_demos_py/package.xml index 765205e3..e514c03a 100644 --- a/lbr_demos/lbr_demos_py/package.xml +++ b/lbr_demos/lbr_demos_py/package.xml @@ -10,7 +10,7 @@ control_msgs lbr_bringup lbr_description - lbr_fri_msgs + lbr_fri_idl lbr_ros2_control rclpy trajectory_msgs diff --git a/lbr_fri_ros2/CMakeLists.txt b/lbr_fri_ros2/CMakeLists.txt index 52856dac..47a55f94 100644 --- a/lbr_fri_ros2/CMakeLists.txt +++ b/lbr_fri_ros2/CMakeLists.txt @@ -16,7 +16,7 @@ find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3) find_package(FRIClient REQUIRED) find_package(kdl_parser REQUIRED) -find_package(lbr_fri_msgs REQUIRED) +find_package(lbr_fri_idl REQUIRED) find_package(orocos_kdl_vendor REQUIRED) find_package(rclcpp REQUIRED) find_package(realtime_tools REQUIRED) @@ -47,7 +47,7 @@ ament_target_dependencies(lbr_fri_ros2 control_toolbox Eigen3 kdl_parser - lbr_fri_msgs + lbr_fri_idl orocos_kdl_vendor rclcpp realtime_tools @@ -64,7 +64,7 @@ ament_export_dependencies( Eigen3 FRIClient kdl_parser - lbr_fri_msgs + lbr_fri_idl orocos_kdl_vendor rclcpp realtime_tools diff --git a/lbr_fri_ros2/doc/lbr_fri_ros2.rst b/lbr_fri_ros2/doc/lbr_fri_ros2.rst index 6c6dc8ee..c5ab8b90 100644 --- a/lbr_fri_ros2/doc/lbr_fri_ros2.rst +++ b/lbr_fri_ros2/doc/lbr_fri_ros2.rst @@ -52,7 +52,7 @@ An overview of the software architecture is shown :ref:`below implemented through :ref:`FRI` package. -- Bridge ``nanopb`` (used within FRI for message definition) with ROS 2 Interface Definition Language (``IDL``) -> implemented through ``lbr_fri_msgs`` package. +- Bridge ``nanopb`` (used within FRI for message definition) with ROS 2 Interface Definition Language (``IDL``) -> implemented through ``lbr_fri_idl`` package. - Support future versions of the FRI -> implemented through ``vcstool`` and by separating the :ref:`FRI` package. - Run stand-alone **and** within ``ros2_control`` -> implemented through :lbr_fri_ros2:`App `. @@ -76,9 +76,9 @@ The ``lbr_fri_ros2`` package implements an :fri:`Client ` The :lbr_fri_ros2:`Client ` has - A publisher to publish states in :lbr_fri_ros2:`pub_lbr_state_ `. - - A subscription to read commands in :lbr_fri_ros2:`on_lbr_command_ `. + - A subscription to read commands in :lbr_fri_ros2:`on_lbr_command_ `. -Commands in :lbr_fri_ros2:`on_lbr_command_ ` are checked for validity via a :lbr_fri_ros2:`CommandGuard `. +Commands in :lbr_fri_ros2:`on_lbr_command_ ` are checked for validity via a :lbr_fri_ros2:`CommandGuard `. API ~~~ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp index b176c349..c9a9d811 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp @@ -14,7 +14,7 @@ #include "friLBRState.h" #include "friVersion.h" -#include "lbr_fri_msgs/msg/lbr_command.hpp" +#include "lbr_fri_idl/msg/lbr_command.hpp" #include "lbr_fri_ros2/formatting.hpp" namespace lbr_fri_ros2 { @@ -35,7 +35,7 @@ class CommandGuard { static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::CommandGuard"; // ROS IDL types - using idl_command_t = lbr_fri_msgs::msg::LBRCommand; + using idl_command_t = lbr_fri_idl::msg::LBRCommand; using const_idl_command_t_ref = const idl_command_t &; using jnt_array_t = idl_command_t::_joint_position_type; using const_jnt_array_t_ref = const jnt_array_t &; diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp index b9929479..54965125 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp @@ -11,7 +11,7 @@ #include "friLBRClient.h" -#include "lbr_fri_msgs/msg/lbr_state.hpp" +#include "lbr_fri_idl/msg/lbr_state.hpp" namespace lbr_fri_ros2 { class ExponentialFilter { diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp index f71c6eb9..57c81292 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp @@ -17,16 +17,16 @@ #include "friLBRState.h" -#include "lbr_fri_msgs/msg/lbr_state.hpp" +#include "lbr_fri_idl/msg/lbr_state.hpp" #include "lbr_fri_ros2/pinv.hpp" namespace lbr_fri_ros2 { class FTEstimator { protected: static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::FTEstimator"; - using jnt_pos_array_t = lbr_fri_msgs::msg::LBRState::_measured_joint_position_type; + using jnt_pos_array_t = lbr_fri_idl::msg::LBRState::_measured_joint_position_type; using const_jnt_pos_array_t_ref = const jnt_pos_array_t &; - using ext_tau_array_t = lbr_fri_msgs::msg::LBRState::_external_torque_type; + using ext_tau_array_t = lbr_fri_idl::msg::LBRState::_external_torque_type; using const_ext_tau_array_t_ref = const ext_tau_array_t &; public: diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp index 708916a0..a29ccf06 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp @@ -12,7 +12,7 @@ #include "friLBRClient.h" #include "friVersion.h" -#include "lbr_fri_msgs/msg/lbr_command.hpp" +#include "lbr_fri_idl/msg/lbr_command.hpp" #include "lbr_fri_ros2/command_guard.hpp" #include "lbr_fri_ros2/filters.hpp" #include "lbr_fri_ros2/formatting.hpp" @@ -23,7 +23,7 @@ class BaseCommandInterface { virtual std::string LOGGER_NAME() const = 0; // ROS IDL types - using idl_command_t = lbr_fri_msgs::msg::LBRCommand; + using idl_command_t = lbr_fri_idl::msg::LBRCommand; using const_idl_command_t_ref = const idl_command_t &; // FRI types diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp index 656d7714..cdfc97a7 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp @@ -9,7 +9,7 @@ #include "friLBRClient.h" #include "friVersion.h" -#include "lbr_fri_msgs/msg/lbr_state.hpp" +#include "lbr_fri_idl/msg/lbr_state.hpp" #include "lbr_fri_ros2/filters.hpp" namespace lbr_fri_ros2 { @@ -23,7 +23,7 @@ class StateInterface { static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::StateInterface"; // ROS IDL types - using idl_state_t = lbr_fri_msgs::msg::LBRState; + using idl_state_t = lbr_fri_idl::msg::LBRState; using const_idl_state_t_ref = const idl_state_t &; using idl_joint_pos_t = idl_state_t::_measured_joint_position_type; using const_idl_joint_pos_t_ref = const idl_joint_pos_t &; diff --git a/lbr_fri_ros2/package.xml b/lbr_fri_ros2/package.xml index 3cd4bccb..ba6e2cbb 100644 --- a/lbr_fri_ros2/package.xml +++ b/lbr_fri_ros2/package.xml @@ -16,7 +16,7 @@ control_toolbox FRIClient kdl_parser - lbr_fri_msgs + lbr_fri_idl orocos_kdl_vendor rclcpp realtime_tools diff --git a/lbr_fri_ros2/test/test_command_interfaces.cpp b/lbr_fri_ros2/test/test_command_interfaces.cpp index de5a1939..21883b5c 100644 --- a/lbr_fri_ros2/test/test_command_interfaces.cpp +++ b/lbr_fri_ros2/test/test_command_interfaces.cpp @@ -8,7 +8,7 @@ #include "friUdpConnection.h" #include "friVersion.h" -#include "lbr_fri_msgs/msg/lbr_command.hpp" +#include "lbr_fri_idl/msg/lbr_command.hpp" #include "lbr_fri_ros2/interfaces/base_command.hpp" #include "lbr_fri_ros2/interfaces/position_command.hpp" #include "lbr_fri_ros2/interfaces/torque_command.hpp" @@ -53,8 +53,8 @@ class TestCommandInterfaces : public ::testing::Test { } } - lbr_fri_msgs::msg::LBRCommand random_idl_command() { - lbr_fri_msgs::msg::LBRCommand idl_command; + lbr_fri_idl::msg::LBRCommand random_idl_command() { + lbr_fri_idl::msg::LBRCommand idl_command; for (auto &joint_position : idl_command.joint_position) { joint_position = uniform_real_dist_(random_engine_); } diff --git a/lbr_fri_ros2/test/test_position_command.cpp b/lbr_fri_ros2/test/test_position_command.cpp index d3d10830..380cf463 100644 --- a/lbr_fri_ros2/test/test_position_command.cpp +++ b/lbr_fri_ros2/test/test_position_command.cpp @@ -60,7 +60,7 @@ int main() { std::this_thread::sleep_for(std::chrono::milliseconds(100)); } - lbr_fri_msgs::msg::LBRCommand command; + lbr_fri_idl::msg::LBRCommand command; while (rclcpp::ok()) { // read state auto state = client->get_state_interface()->get_state(); diff --git a/lbr_fri_ros2/test/test_torque_command.cpp b/lbr_fri_ros2/test/test_torque_command.cpp index 164c16da..14cc35f5 100644 --- a/lbr_fri_ros2/test/test_torque_command.cpp +++ b/lbr_fri_ros2/test/test_torque_command.cpp @@ -60,7 +60,7 @@ int main() { std::this_thread::sleep_for(std::chrono::milliseconds(100)); } - lbr_fri_msgs::msg::LBRCommand command; + lbr_fri_idl::msg::LBRCommand command; while (rclcpp::ok()) { // read state auto state = client->get_state_interface()->get_state(); diff --git a/lbr_fri_ros2/test/test_wrench_command.cpp b/lbr_fri_ros2/test/test_wrench_command.cpp index 81f79743..44240504 100644 --- a/lbr_fri_ros2/test/test_wrench_command.cpp +++ b/lbr_fri_ros2/test/test_wrench_command.cpp @@ -60,7 +60,7 @@ int main() { std::this_thread::sleep_for(std::chrono::milliseconds(100)); } - lbr_fri_msgs::msg::LBRCommand command; + lbr_fri_idl::msg::LBRCommand command; while (rclcpp::ok()) { // read state auto state = client->get_state_interface()->get_state(); diff --git a/lbr_fri_ros2_stack/doc/lbr_fri_ros2_stack.rst b/lbr_fri_ros2_stack/doc/lbr_fri_ros2_stack.rst index 46cb914a..d9c5b572 100644 --- a/lbr_fri_ros2_stack/doc/lbr_fri_ros2_stack.rst +++ b/lbr_fri_ros2_stack/doc/lbr_fri_ros2_stack.rst @@ -5,7 +5,7 @@ Collection of packages for controlling the KUKA LBR iiwa / med through ROS 2. - ``lbr_bringup``: Launch package. - ``lbr_demos``: Demo applications. - ``lbr_description``: Description files. -- ``lbr_fri_msgs``: ``IDL``-equivalent of KUKA's ``nanopb`` message definitions. +- ``lbr_fri_idl``: ``IDL``-equivalent of KUKA's ``nanopb`` message definitions. - ``lbr_fri_ros2``: Exposes ``fri`` to ROS 2 topics / services. - ``lbr_ros2_control``: ``ros2_control`` integration for the LBR. - ``lbr_moveit_config```: ``MoveIt 2`` configurations for thr LBR. diff --git a/lbr_fri_ros2_stack/package.xml b/lbr_fri_ros2_stack/package.xml index 28291377..14cde7f4 100644 --- a/lbr_fri_ros2_stack/package.xml +++ b/lbr_fri_ros2_stack/package.xml @@ -12,7 +12,7 @@ lbr_bringup lbr_demos lbr_description - lbr_fri_msgs + lbr_fri_idl lbr_fri_ros2 lbr_ros2_control diff --git a/lbr_fri_ros2_stack/repos-fri-1.11.yaml b/lbr_fri_ros2_stack/repos-fri-1.11.yaml index e05ac2d8..a0e658db 100644 --- a/lbr_fri_ros2_stack/repos-fri-1.11.yaml +++ b/lbr_fri_ros2_stack/repos-fri-1.11.yaml @@ -3,9 +3,9 @@ repositories: type: git url: https://github.com/lbr-stack/fri version: fri-1.11 - lbr_fri_msgs: + lbr_fri_idl: type: git - url: https://github.com/lbr-stack/lbr_fri_msgs + url: https://github.com/lbr-stack/lbr_fri_idl version: fri-1 lbr_fri_ros2_stack: type: git diff --git a/lbr_fri_ros2_stack/repos-fri-1.14.yaml b/lbr_fri_ros2_stack/repos-fri-1.14.yaml index 8159c087..3bde0b56 100644 --- a/lbr_fri_ros2_stack/repos-fri-1.14.yaml +++ b/lbr_fri_ros2_stack/repos-fri-1.14.yaml @@ -3,9 +3,9 @@ repositories: type: git url: https://github.com/lbr-stack/fri version: fri-1.14 - lbr_fri_msgs: + lbr_fri_idl: type: git - url: https://github.com/lbr-stack/lbr_fri_msgs + url: https://github.com/lbr-stack/lbr_fri_idl version: fri-1 lbr_fri_ros2_stack: type: git diff --git a/lbr_fri_ros2_stack/repos-fri-1.15.yaml b/lbr_fri_ros2_stack/repos-fri-1.15.yaml index b869a547..5e7f40fe 100644 --- a/lbr_fri_ros2_stack/repos-fri-1.15.yaml +++ b/lbr_fri_ros2_stack/repos-fri-1.15.yaml @@ -3,9 +3,9 @@ repositories: type: git url: https://github.com/lbr-stack/fri version: fri-1.15 - lbr_fri_msgs: + lbr_fri_idl: type: git - url: https://github.com/lbr-stack/lbr_fri_msgs + url: https://github.com/lbr-stack/lbr_fri_idl version: fri-1 lbr_fri_ros2_stack: type: git diff --git a/lbr_fri_ros2_stack/repos-fri-1.16.yaml b/lbr_fri_ros2_stack/repos-fri-1.16.yaml index 55de8589..84df1702 100644 --- a/lbr_fri_ros2_stack/repos-fri-1.16.yaml +++ b/lbr_fri_ros2_stack/repos-fri-1.16.yaml @@ -3,9 +3,9 @@ repositories: type: git url: https://github.com/lbr-stack/fri version: fri-1.16 - lbr_fri_msgs: + lbr_fri_idl: type: git - url: https://github.com/lbr-stack/lbr_fri_msgs + url: https://github.com/lbr-stack/lbr_fri_idl version: fri-1 lbr_fri_ros2_stack: type: git diff --git a/lbr_fri_ros2_stack/repos-fri-2.5.yaml b/lbr_fri_ros2_stack/repos-fri-2.5.yaml index 0ab38f23..637e81a0 100644 --- a/lbr_fri_ros2_stack/repos-fri-2.5.yaml +++ b/lbr_fri_ros2_stack/repos-fri-2.5.yaml @@ -3,9 +3,9 @@ repositories: type: git url: https://github.com/lbr-stack/fri version: fri-2.5 - lbr_fri_msgs: + lbr_fri_idl: type: git - url: https://github.com/lbr-stack/lbr_fri_msgs + url: https://github.com/lbr-stack/lbr_fri_idl version: fri-2 lbr_fri_ros2_stack: type: git diff --git a/lbr_ros2_control/CMakeLists.txt b/lbr_ros2_control/CMakeLists.txt index 108ecb9d..aea042a7 100644 --- a/lbr_ros2_control/CMakeLists.txt +++ b/lbr_ros2_control/CMakeLists.txt @@ -18,7 +18,7 @@ find_package(ament_cmake REQUIRED) find_package(controller_interface REQUIRED) find_package(FRIClient REQUIRED) find_package(hardware_interface REQUIRED) -find_package(lbr_fri_msgs REQUIRED) +find_package(lbr_fri_idl REQUIRED) find_package(lbr_fri_ros2 REQUIRED) find_package(kinematics_interface REQUIRED) find_package(pluginlib REQUIRED) @@ -51,7 +51,7 @@ ament_target_dependencies( ${PROJECT_NAME} controller_interface hardware_interface - lbr_fri_msgs + lbr_fri_idl lbr_fri_ros2 pluginlib rclcpp @@ -75,7 +75,7 @@ ament_export_dependencies( controller_interface FRIClient hardware_interface - lbr_fri_msgs + lbr_fri_idl lbr_fri_ros2 pluginlib rclcpp diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_position_command_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/lbr_position_command_controller.hpp index b29c6948..45840582 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/lbr_position_command_controller.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/lbr_position_command_controller.hpp @@ -14,7 +14,7 @@ #include "friLBRState.h" -#include "lbr_fri_msgs/msg/lbr_position_command.hpp" +#include "lbr_fri_idl/msg/lbr_position_command.hpp" namespace lbr_ros2_control { class LBRPositionCommandController : public controller_interface::ControllerInterface { @@ -43,9 +43,9 @@ class LBRPositionCommandController : public controller_interface::ControllerInte std::array joint_names_ = { "A1", "A2", "A3", "A4", "A5", "A6", "A7"}; - realtime_tools::RealtimeBuffer + realtime_tools::RealtimeBuffer rt_lbr_position_command_ptr_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr lbr_position_command_subscription_ptr_; }; } // end of namespace lbr_ros2_control diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_state_broadcaster.hpp b/lbr_ros2_control/include/lbr_ros2_control/lbr_state_broadcaster.hpp index 07d53e97..8df3df4d 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/lbr_state_broadcaster.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/lbr_state_broadcaster.hpp @@ -18,7 +18,7 @@ #include "friLBRState.h" #include "friVersion.h" -#include "lbr_fri_msgs/msg/lbr_state.hpp" +#include "lbr_fri_idl/msg/lbr_state.hpp" #include "lbr_ros2_control/system_interface_type_values.hpp" namespace lbr_ros2_control { @@ -52,8 +52,8 @@ class LBRStateBroadcaster : public controller_interface::ControllerInterface { "A1", "A2", "A3", "A4", "A5", "A6", "A7"}; std::unordered_map> state_interface_map_; - rclcpp::Publisher::SharedPtr state_publisher_ptr_; - std::shared_ptr> + rclcpp::Publisher::SharedPtr state_publisher_ptr_; + std::shared_ptr> rt_state_publisher_ptr_; }; } // end of namespace lbr_ros2_control diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_torque_command_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/lbr_torque_command_controller.hpp index 0be853b6..f86d1c25 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/lbr_torque_command_controller.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/lbr_torque_command_controller.hpp @@ -16,7 +16,7 @@ #include "friLBRState.h" -#include "lbr_fri_msgs/msg/lbr_torque_command.hpp" +#include "lbr_fri_idl/msg/lbr_torque_command.hpp" namespace lbr_ros2_control { class LBRTorqueCommandController : public controller_interface::ControllerInterface { @@ -51,9 +51,9 @@ class LBRTorqueCommandController : public controller_interface::ControllerInterf std::vector> joint_position_command_interfaces_, torque_command_interfaces_; - realtime_tools::RealtimeBuffer + realtime_tools::RealtimeBuffer rt_lbr_torque_command_ptr_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr lbr_torque_command_subscription_ptr_; }; } // end of namespace lbr_ros2_control diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_wrench_command_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/lbr_wrench_command_controller.hpp index 20dceb12..183e4cf4 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/lbr_wrench_command_controller.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/lbr_wrench_command_controller.hpp @@ -16,7 +16,7 @@ #include "friLBRState.h" -#include "lbr_fri_msgs/msg/lbr_wrench_command.hpp" +#include "lbr_fri_idl/msg/lbr_wrench_command.hpp" #include "lbr_ros2_control/system_interface_type_values.hpp" namespace lbr_ros2_control { @@ -54,9 +54,9 @@ class LBRWrenchCommandController : public controller_interface::ControllerInterf std::vector> joint_position_command_interfaces_, wrench_command_interfaces_; - realtime_tools::RealtimeBuffer + realtime_tools::RealtimeBuffer rt_lbr_wrench_command_ptr_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr lbr_wrench_command_subscription_ptr_; }; } // end of namespace lbr_ros2_control diff --git a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp index 50006a12..37b129a7 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp @@ -18,8 +18,8 @@ #include "friLBRState.h" #include "friVersion.h" -#include "lbr_fri_msgs/msg/lbr_command.hpp" -#include "lbr_fri_msgs/msg/lbr_state.hpp" +#include "lbr_fri_idl/msg/lbr_command.hpp" +#include "lbr_fri_idl/msg/lbr_state.hpp" #include "lbr_fri_ros2/app.hpp" #include "lbr_fri_ros2/async_client.hpp" #include "lbr_fri_ros2/command_guard.hpp" @@ -125,7 +125,7 @@ class SystemInterface : public hardware_interface::SystemInterface { // exposed state interfaces (ideally these are taken from async_client_ptr_ but // ros2_control ReadOnlyHandle does not allow for const pointers, refer // https://github.com/ros-controls/ros2_control/issues/1196) - lbr_fri_msgs::msg::LBRState hw_lbr_state_; + lbr_fri_idl::msg::LBRState hw_lbr_state_; // exposed state interfaces that require casting double hw_session_state_; @@ -140,10 +140,10 @@ class SystemInterface : public hardware_interface::SystemInterface { double hw_time_stamp_nano_sec_; // additional velocity state interface - lbr_fri_msgs::msg::LBRState::_measured_joint_position_type last_hw_measured_joint_position_; + lbr_fri_idl::msg::LBRState::_measured_joint_position_type last_hw_measured_joint_position_; double last_hw_time_stamp_sec_; double last_hw_time_stamp_nano_sec_; - lbr_fri_msgs::msg::LBRState::_measured_joint_position_type hw_velocity_; + lbr_fri_idl::msg::LBRState::_measured_joint_position_type hw_velocity_; // compute velocity for state interface double time_stamps_to_sec_(const double &sec, const double &nano_sec) const; @@ -156,7 +156,7 @@ class SystemInterface : public hardware_interface::SystemInterface { std::unique_ptr ft_estimator_ptr_; // exposed command interfaces - lbr_fri_msgs::msg::LBRCommand hw_lbr_command_; + lbr_fri_idl::msg::LBRCommand hw_lbr_command_; }; } // end of namespace lbr_ros2_control #endif // LBR_ROS2_CONTROL__SYSTEM_INTERFACE_HPP_ diff --git a/lbr_ros2_control/package.xml b/lbr_ros2_control/package.xml index 9844fd9c..bf8680f8 100644 --- a/lbr_ros2_control/package.xml +++ b/lbr_ros2_control/package.xml @@ -10,7 +10,7 @@ ament_cmake FRIClient - lbr_fri_msgs + lbr_fri_idl lbr_fri_ros2 pluginlib rclcpp diff --git a/lbr_ros2_control/plugin_description_files/broadcasters.xml b/lbr_ros2_control/plugin_description_files/broadcasters.xml index 684fdfe3..0c2484f0 100644 --- a/lbr_ros2_control/plugin_description_files/broadcasters.xml +++ b/lbr_ros2_control/plugin_description_files/broadcasters.xml @@ -3,6 +3,6 @@ - Broadcaster for LBRState messages, see lbr_fri_msgs/msg/LBRState.msg. + Broadcaster for LBRState messages, see lbr_fri_idl/msg/LBRState.msg. \ No newline at end of file diff --git a/lbr_ros2_control/plugin_description_files/forward_command_controllers.xml b/lbr_ros2_control/plugin_description_files/forward_command_controllers.xml index 4b0f625c..7ef30896 100644 --- a/lbr_ros2_control/plugin_description_files/forward_command_controllers.xml +++ b/lbr_ros2_control/plugin_description_files/forward_command_controllers.xml @@ -5,7 +5,7 @@ type="lbr_ros2_control::LBRPositionCommandController" base_class_type="controller_interface::ControllerInterface"> Forward command controller for LBRPositionCommand message, see - lbr_fri_msgs/msg/LBRPositionCommand.msg. + lbr_fri_idl/msg/LBRPositionCommand.msg. @@ -13,7 +13,7 @@ type="lbr_ros2_control::LBRTorqueCommandController" base_class_type="controller_interface::ControllerInterface"> Forward command controller for LBRTorqueCommand message, see - lbr_fri_msgs/msg/LBRTorqueCommand.msg. + lbr_fri_idl/msg/LBRTorqueCommand.msg. @@ -21,6 +21,6 @@ type="lbr_ros2_control::LBRWrenchCommandController" base_class_type="controller_interface::ControllerInterface"> Forward command controller for LBRWrenchCommand message, see - lbr_fri_msgs/msg/LBRWrenchCommand.msg. + lbr_fri_idl/msg/LBRWrenchCommand.msg. \ No newline at end of file diff --git a/lbr_ros2_control/src/lbr_position_command_controller.cpp b/lbr_ros2_control/src/lbr_position_command_controller.cpp index e2caf30b..f4c77c4c 100644 --- a/lbr_ros2_control/src/lbr_position_command_controller.cpp +++ b/lbr_ros2_control/src/lbr_position_command_controller.cpp @@ -23,9 +23,9 @@ LBRPositionCommandController::state_interface_configuration() const { controller_interface::CallbackReturn LBRPositionCommandController::on_init() { try { lbr_position_command_subscription_ptr_ = - this->get_node()->create_subscription( + this->get_node()->create_subscription( "command/joint_position", 1, - [this](const lbr_fri_msgs::msg::LBRPositionCommand::SharedPtr msg) { + [this](const lbr_fri_idl::msg::LBRPositionCommand::SharedPtr msg) { rt_lbr_position_command_ptr_.writeFromNonRT(msg); }); } catch (const std::exception &e) { diff --git a/lbr_ros2_control/src/lbr_state_broadcaster.cpp b/lbr_ros2_control/src/lbr_state_broadcaster.cpp index f731e439..240befe1 100644 --- a/lbr_ros2_control/src/lbr_state_broadcaster.cpp +++ b/lbr_ros2_control/src/lbr_state_broadcaster.cpp @@ -16,10 +16,10 @@ LBRStateBroadcaster::state_interface_configuration() const { controller_interface::CallbackReturn LBRStateBroadcaster::on_init() { try { state_publisher_ptr_ = - this->get_node()->create_publisher("state", 1); + this->get_node()->create_publisher("state", 1); rt_state_publisher_ptr_ = - std::make_shared>( + std::make_shared>( state_publisher_ptr_); if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { RCLCPP_ERROR( diff --git a/lbr_ros2_control/src/lbr_torque_command_controller.cpp b/lbr_ros2_control/src/lbr_torque_command_controller.cpp index 26b961c6..2892f8eb 100644 --- a/lbr_ros2_control/src/lbr_torque_command_controller.cpp +++ b/lbr_ros2_control/src/lbr_torque_command_controller.cpp @@ -24,8 +24,8 @@ LBRTorqueCommandController::state_interface_configuration() const { controller_interface::CallbackReturn LBRTorqueCommandController::on_init() { try { lbr_torque_command_subscription_ptr_ = - this->get_node()->create_subscription( - "command/torque", 1, [this](const lbr_fri_msgs::msg::LBRTorqueCommand::SharedPtr msg) { + this->get_node()->create_subscription( + "command/torque", 1, [this](const lbr_fri_idl::msg::LBRTorqueCommand::SharedPtr msg) { rt_lbr_torque_command_ptr_.writeFromNonRT(msg); }); } catch (const std::exception &e) { diff --git a/lbr_ros2_control/src/lbr_wrench_command_controller.cpp b/lbr_ros2_control/src/lbr_wrench_command_controller.cpp index 12461925..4f00a986 100644 --- a/lbr_ros2_control/src/lbr_wrench_command_controller.cpp +++ b/lbr_ros2_control/src/lbr_wrench_command_controller.cpp @@ -29,8 +29,8 @@ LBRWrenchCommandController::state_interface_configuration() const { controller_interface::CallbackReturn LBRWrenchCommandController::on_init() { try { lbr_wrench_command_subscription_ptr_ = - this->get_node()->create_subscription( - "command/wrench", 1, [this](const lbr_fri_msgs::msg::LBRWrenchCommand::SharedPtr msg) { + this->get_node()->create_subscription( + "command/wrench", 1, [this](const lbr_fri_idl::msg::LBRWrenchCommand::SharedPtr msg) { rt_lbr_wrench_command_ptr_.writeFromNonRT(msg); }); } catch (const std::exception &e) { From f7d1c58334746e283451f97f8a1699e983a151a7 Mon Sep 17 00:00:00 2001 From: mhubii Date: Sat, 11 May 2024 11:32:46 +0100 Subject: [PATCH 62/76] update docker for new fri source --- docker/Dockerfile | 5 +---- docker/container_build.sh | 2 +- docker/doc/docker.rst | 3 ++- 3 files changed, 4 insertions(+), 6 deletions(-) diff --git a/docker/Dockerfile b/docker/Dockerfile index 26ad0373..7c660dbe 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -3,9 +3,6 @@ FROM ros:humble-ros-base-jammy # change default shell to bash SHELL ["/bin/bash", "-c"] -# set environment variables -ARG FRI_CLIENT_VERSION=1.15 - # upgrade packages RUN apt-get update RUN apt-get upgrade -y @@ -19,7 +16,7 @@ RUN rosdep install -i --from-paths src --rosdistro ${ROS_DISTRO} -y # "--symlink-install" allows the code in the locally mounted volume ./src to be adjusted without rebuilding RUN source /opt/ros/${ROS_DISTRO}/setup.bash && \ - colcon build --symlink-install --cmake-args -DFRI_CLIENT_VERSION=${FRI_CLIENT_VERSION} --no-warn-unused-cli + colcon build --symlink-install # source the workspace RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> ~/.bashrc diff --git a/docker/container_build.sh b/docker/container_build.sh index edf2ed21..3a0513af 100755 --- a/docker/container_build.sh +++ b/docker/container_build.sh @@ -3,7 +3,7 @@ xhost + docker rm lbr_stack_container -docker build --build-arg FRI_CLIENT_VERSION=$1 -t lbr_stack_container . +docker build -t lbr_stack_container . docker run -it \ --network host \ diff --git a/docker/doc/docker.rst b/docker/doc/docker.rst index 0a518f08..01ba9e61 100644 --- a/docker/doc/docker.rst +++ b/docker/doc/docker.rst @@ -12,8 +12,9 @@ To run the ``lbr_fri_ros2_stack`` in a Docker container, follow the instructions .. code-block:: bash + export FRI_CLIENT_VERSION=1.15 # replace by your FRI client version mkdir -p lbr-stack/src && cd lbr-stack - vcs import src --input https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/repos.yaml + vcs import src --input https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/repos-fri-${FRI_CLIENT_VERSION}.yaml #. Copy the Dockerfile and the container scripts to the ``lbr-stack`` directory. Build and start the container From 54a905aa9c8218f14665f2fdfc5ab9eecb1c0a95 Mon Sep 17 00:00:00 2001 From: mhubii Date: Sat, 11 May 2024 11:52:18 +0100 Subject: [PATCH 63/76] new sensor source --- .../src/lbr_state_broadcaster.cpp | 28 +++++++++---------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/lbr_ros2_control/src/lbr_state_broadcaster.cpp b/lbr_ros2_control/src/lbr_state_broadcaster.cpp index 240befe1..3ea4afaf 100644 --- a/lbr_ros2_control/src/lbr_state_broadcaster.cpp +++ b/lbr_ros2_control/src/lbr_state_broadcaster.cpp @@ -49,30 +49,30 @@ controller_interface::return_type LBRStateBroadcaster::update(const rclcpp::Time } if (rt_state_publisher_ptr_->trylock()) { // FRI related states - rt_state_publisher_ptr_->msg_.client_command_mode = - static_cast(state_interface_map_["auxiliary_sensor"][HW_IF_CLIENT_COMMAND_MODE]); + rt_state_publisher_ptr_->msg_.client_command_mode = static_cast( + state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_CLIENT_COMMAND_MODE]); rt_state_publisher_ptr_->msg_.connection_quality = - static_cast(state_interface_map_["auxiliary_sensor"][HW_IF_CONNECTION_QUALITY]); + static_cast(state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_CONNECTION_QUALITY]); rt_state_publisher_ptr_->msg_.control_mode = - static_cast(state_interface_map_["auxiliary_sensor"][HW_IF_CONTROL_MODE]); + static_cast(state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_CONTROL_MODE]); rt_state_publisher_ptr_->msg_.drive_state = - static_cast(state_interface_map_["auxiliary_sensor"][HW_IF_DRIVE_STATE]); + static_cast(state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_DRIVE_STATE]); rt_state_publisher_ptr_->msg_.operation_mode = - static_cast(state_interface_map_["auxiliary_sensor"][HW_IF_OPERATION_MODE]); + static_cast(state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_OPERATION_MODE]); rt_state_publisher_ptr_->msg_.overlay_type = - static_cast(state_interface_map_["auxiliary_sensor"][HW_IF_OVERLAY_TYPE]); + static_cast(state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_OVERLAY_TYPE]); rt_state_publisher_ptr_->msg_.safety_state = - static_cast(state_interface_map_["auxiliary_sensor"][HW_IF_SAFETY_STATE]); + static_cast(state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_SAFETY_STATE]); rt_state_publisher_ptr_->msg_.sample_time = - state_interface_map_["auxiliary_sensor"][HW_IF_SAMPLE_TIME]; + state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_SAMPLE_TIME]; rt_state_publisher_ptr_->msg_.session_state = - static_cast(state_interface_map_["auxiliary_sensor"][HW_IF_SESSION_STATE]); - rt_state_publisher_ptr_->msg_.time_stamp_nano_sec = - static_cast(state_interface_map_["auxiliary_sensor"][HW_IF_TIME_STAMP_NANO_SEC]); + static_cast(state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_SESSION_STATE]); + rt_state_publisher_ptr_->msg_.time_stamp_nano_sec = static_cast( + state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_TIME_STAMP_NANO_SEC]); rt_state_publisher_ptr_->msg_.time_stamp_sec = - static_cast(state_interface_map_["auxiliary_sensor"][HW_IF_TIME_STAMP_SEC]); + static_cast(state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_TIME_STAMP_SEC]); rt_state_publisher_ptr_->msg_.tracking_performance = - state_interface_map_["auxiliary_sensor"][HW_IF_TRACKING_PERFORMANCE]; + state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_TRACKING_PERFORMANCE]; // joint related states std::for_each(joint_names_.begin(), joint_names_.end(), From 769ae8e762c654b050c365fdc75c50cdd2e10eb7 Mon Sep 17 00:00:00 2001 From: mhubii Date: Sat, 11 May 2024 15:44:28 +0100 Subject: [PATCH 64/76] moved urdf parameters to yaml --- .../{lbr.gazebo.xacro => lbr_gazebo.xacro} | 0 lbr_description/urdf/iiwa14/iiwa14.urdf.xacro | 9 +- .../urdf/iiwa14/iiwa14_description.urdf.xacro | 127 +++++++-------- lbr_description/urdf/iiwa14/joint_limits.yaml | 35 ++++ lbr_description/urdf/iiwa7/iiwa7.urdf.xacro | 9 +- .../urdf/iiwa7/iiwa7_description.urdf.xacro | 127 +++++++-------- lbr_description/urdf/iiwa7/joint_limits.yaml | 35 ++++ lbr_description/urdf/med14/joint_limits.yaml | 35 ++++ lbr_description/urdf/med14/med14.urdf.xacro | 9 +- .../urdf/med14/med14_description.urdf.xacro | 128 +++++++-------- lbr_description/urdf/med7/joint_limits.yaml | 35 ++++ lbr_description/urdf/med7/med7.urdf.xacro | 9 +- .../urdf/med7/med7_description.urdf.xacro | 128 +++++++-------- .../include/lbr_fri_ros2/command_guard.hpp | 10 +- lbr_fri_ros2/src/command_guard.cpp | 20 +-- .../config/lbr_system_interface.xacro | 152 ++++++++---------- .../config/lbr_system_parameters.yaml | 26 +++ .../lbr_ros2_control/system_interface.hpp | 1 + lbr_ros2_control/src/system_interface.cpp | 123 +++++++------- 19 files changed, 588 insertions(+), 430 deletions(-) rename lbr_description/gazebo/{lbr.gazebo.xacro => lbr_gazebo.xacro} (100%) create mode 100644 lbr_description/urdf/iiwa14/joint_limits.yaml create mode 100644 lbr_description/urdf/iiwa7/joint_limits.yaml create mode 100644 lbr_description/urdf/med14/joint_limits.yaml create mode 100644 lbr_description/urdf/med7/joint_limits.yaml create mode 100644 lbr_ros2_control/config/lbr_system_parameters.yaml diff --git a/lbr_description/gazebo/lbr.gazebo.xacro b/lbr_description/gazebo/lbr_gazebo.xacro similarity index 100% rename from lbr_description/gazebo/lbr.gazebo.xacro rename to lbr_description/gazebo/lbr_gazebo.xacro diff --git a/lbr_description/urdf/iiwa14/iiwa14.urdf.xacro b/lbr_description/urdf/iiwa14/iiwa14.urdf.xacro index b13a7433..7b7c7eb2 100644 --- a/lbr_description/urdf/iiwa14/iiwa14.urdf.xacro +++ b/lbr_description/urdf/iiwa14/iiwa14.urdf.xacro @@ -7,8 +7,10 @@ - + @@ -21,5 +23,8 @@ - + \ No newline at end of file diff --git a/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro b/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro index e568050d..3a063b67 100644 --- a/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro +++ b/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro @@ -1,34 +1,34 @@ + + - - - - - - - - - - - - - - - - - - - + + + - - - + + + - - + + + + + + + + + + + @@ -56,9 +56,11 @@ - + @@ -88,9 +90,11 @@ - + @@ -120,9 +124,11 @@ - + @@ -152,9 +158,11 @@ - + @@ -184,9 +192,11 @@ - + @@ -216,9 +226,11 @@ - + @@ -248,9 +260,11 @@ - + @@ -282,26 +296,5 @@ - - - - - \ No newline at end of file diff --git a/lbr_description/urdf/iiwa14/joint_limits.yaml b/lbr_description/urdf/iiwa14/joint_limits.yaml new file mode 100644 index 00000000..c101a209 --- /dev/null +++ b/lbr_description/urdf/iiwa14/joint_limits.yaml @@ -0,0 +1,35 @@ +A1: + lower: -170 + upper: 170 + velocity: 85 + effort: 200 +A2: + lower: -120 + upper: 120 + velocity: 85 + effort: 200 +A3: + lower: -170 + upper: 170 + velocity: 100 + effort: 200 +A4: + lower: -120 + upper: 120 + velocity: 75 + effort: 200 +A5: + lower: -170 + upper: 170 + velocity: 130 + effort: 200 +A6: + lower: -120 + upper: 120 + velocity: 135 + effort: 200 +A7: + lower: -175 + upper: 175 + velocity: 135 + effort: 200 diff --git a/lbr_description/urdf/iiwa7/iiwa7.urdf.xacro b/lbr_description/urdf/iiwa7/iiwa7.urdf.xacro index 393c6f31..84d69b17 100644 --- a/lbr_description/urdf/iiwa7/iiwa7.urdf.xacro +++ b/lbr_description/urdf/iiwa7/iiwa7.urdf.xacro @@ -7,8 +7,10 @@ - + @@ -21,5 +23,8 @@ - + \ No newline at end of file diff --git a/lbr_description/urdf/iiwa7/iiwa7_description.urdf.xacro b/lbr_description/urdf/iiwa7/iiwa7_description.urdf.xacro index 21cdbd49..0768ddd9 100644 --- a/lbr_description/urdf/iiwa7/iiwa7_description.urdf.xacro +++ b/lbr_description/urdf/iiwa7/iiwa7_description.urdf.xacro @@ -1,34 +1,34 @@ + + - - - - - - - - - - - - - - - - - - - + + + - - - + + + - - + + + + + + + + + + + @@ -56,9 +56,11 @@ - + @@ -88,9 +90,11 @@ - + @@ -120,9 +124,11 @@ - + @@ -152,9 +158,11 @@ - + @@ -184,9 +192,11 @@ - + @@ -216,9 +226,11 @@ - + @@ -249,9 +261,11 @@ - + @@ -283,26 +297,5 @@ - - - - - \ No newline at end of file diff --git a/lbr_description/urdf/iiwa7/joint_limits.yaml b/lbr_description/urdf/iiwa7/joint_limits.yaml new file mode 100644 index 00000000..d0d1bcae --- /dev/null +++ b/lbr_description/urdf/iiwa7/joint_limits.yaml @@ -0,0 +1,35 @@ +A1: + lower: -170 + upper: 170 + velocity: 98 + effort: 200 +A2: + lower: -120 + upper: 120 + velocity: 98 + effort: 200 +A3: + lower: -170 + upper: 170 + velocity: 100 + effort: 200 +A4: + lower: -120 + upper: 120 + velocity: 130 + effort: 200 +A5: + lower: -170 + upper: 170 + velocity: 140 + effort: 200 +A6: + lower: -120 + upper: 120 + velocity: 180 + effort: 200 +A7: + lower: -175 + upper: 175 + velocity: 180 + effort: 200 diff --git a/lbr_description/urdf/med14/joint_limits.yaml b/lbr_description/urdf/med14/joint_limits.yaml new file mode 100644 index 00000000..c101a209 --- /dev/null +++ b/lbr_description/urdf/med14/joint_limits.yaml @@ -0,0 +1,35 @@ +A1: + lower: -170 + upper: 170 + velocity: 85 + effort: 200 +A2: + lower: -120 + upper: 120 + velocity: 85 + effort: 200 +A3: + lower: -170 + upper: 170 + velocity: 100 + effort: 200 +A4: + lower: -120 + upper: 120 + velocity: 75 + effort: 200 +A5: + lower: -170 + upper: 170 + velocity: 130 + effort: 200 +A6: + lower: -120 + upper: 120 + velocity: 135 + effort: 200 +A7: + lower: -175 + upper: 175 + velocity: 135 + effort: 200 diff --git a/lbr_description/urdf/med14/med14.urdf.xacro b/lbr_description/urdf/med14/med14.urdf.xacro index f481bd58..ba9e8d94 100644 --- a/lbr_description/urdf/med14/med14.urdf.xacro +++ b/lbr_description/urdf/med14/med14.urdf.xacro @@ -7,8 +7,10 @@ - + @@ -21,5 +23,8 @@ - + \ No newline at end of file diff --git a/lbr_description/urdf/med14/med14_description.urdf.xacro b/lbr_description/urdf/med14/med14_description.urdf.xacro index 717dea39..a6e75003 100644 --- a/lbr_description/urdf/med14/med14_description.urdf.xacro +++ b/lbr_description/urdf/med14/med14_description.urdf.xacro @@ -1,34 +1,35 @@ + + - - - - - - - - - - - - - - - - - - - + + + - - - + + + + + + + + + + + + + + + - - @@ -56,9 +57,11 @@ - + @@ -88,9 +91,11 @@ - + @@ -120,9 +125,11 @@ - + @@ -152,9 +159,11 @@ - + @@ -184,9 +193,11 @@ - + @@ -216,9 +227,11 @@ - + @@ -248,9 +261,11 @@ - + @@ -282,26 +297,5 @@ - - - - - \ No newline at end of file diff --git a/lbr_description/urdf/med7/joint_limits.yaml b/lbr_description/urdf/med7/joint_limits.yaml new file mode 100644 index 00000000..43687ac0 --- /dev/null +++ b/lbr_description/urdf/med7/joint_limits.yaml @@ -0,0 +1,35 @@ +A1: + lower: -170 + upper: 170 + velocity: 98 + effort: 200 +A2: + lower: -120 + upper: 120 + velocity: 98 + effort: 200 +A3: + lower: -170 + upper: 170 + velocity: 100 + effort: 200 +A4: + lower: -120 + upper: 120 + velocity: 130 + effort: 200 +A5: + lower: -170 + upper: 170 + velocity: 140 + effort: 200 +A6: + lower: -120 + upper: 120 + velocity: 180 + effort: 200 +A7: + lower: -175 + upper: 175 + velocity: 180 + effort: 200 diff --git a/lbr_description/urdf/med7/med7.urdf.xacro b/lbr_description/urdf/med7/med7.urdf.xacro index b193c07d..04ae010f 100644 --- a/lbr_description/urdf/med7/med7.urdf.xacro +++ b/lbr_description/urdf/med7/med7.urdf.xacro @@ -7,8 +7,10 @@ - + @@ -21,5 +23,8 @@ - + \ No newline at end of file diff --git a/lbr_description/urdf/med7/med7_description.urdf.xacro b/lbr_description/urdf/med7/med7_description.urdf.xacro index cba06813..5125c387 100644 --- a/lbr_description/urdf/med7/med7_description.urdf.xacro +++ b/lbr_description/urdf/med7/med7_description.urdf.xacro @@ -1,35 +1,36 @@ + + - - - - - - - - - - - - - - - - - - - + + + - - - + + + - - + + + + + + + + + + + + @@ -56,9 +57,11 @@ - + @@ -88,9 +91,11 @@ - + @@ -120,9 +125,11 @@ - + @@ -152,9 +159,11 @@ - + @@ -184,9 +193,11 @@ - + @@ -216,9 +227,11 @@ - + @@ -248,9 +261,11 @@ - + @@ -282,26 +297,5 @@ - - - - - \ No newline at end of file diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp index c9a9d811..16bed9c3 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp @@ -23,11 +23,11 @@ struct CommandGuardParameters { using jnt_array_t = std::array; using jnt_name_array_t = std::array; - jnt_name_array_t joint_names; /**< Joint names.*/ - jnt_array_t min_position{0., 0., 0., 0., 0., 0., 0.}; /**< Minimum joint position [rad].*/ - jnt_array_t max_position{0., 0., 0., 0., 0., 0., 0.}; /**< Maximum joint position [rad].*/ - jnt_array_t max_velocity{0., 0., 0., 0., 0., 0., 0.}; /**< Maximum joint velocity [rad/s].*/ - jnt_array_t max_torque{0., 0., 0., 0., 0., 0., 0.}; /**< Maximum joint torque [Nm].*/ + jnt_name_array_t joint_names; /**< Joint names.*/ + jnt_array_t min_positions{0., 0., 0., 0., 0., 0., 0.}; /**< Minimum joint positions [rad].*/ + jnt_array_t max_positions{0., 0., 0., 0., 0., 0., 0.}; /**< Maximum joint positions [rad].*/ + jnt_array_t max_velocities{0., 0., 0., 0., 0., 0., 0.}; /**< Maximum joint velocities [rad/s].*/ + jnt_array_t max_torques{0., 0., 0., 0., 0., 0., 0.}; /**< Maximum joint torque [Nm].*/ }; class CommandGuard { diff --git a/lbr_fri_ros2/src/command_guard.cpp b/lbr_fri_ros2/src/command_guard.cpp index 7429356c..1d885305 100644 --- a/lbr_fri_ros2/src/command_guard.cpp +++ b/lbr_fri_ros2/src/command_guard.cpp @@ -21,17 +21,17 @@ void CommandGuard::log_info() const { RCLCPP_INFO( rclcpp::get_logger(LOGGER_NAME), "* Joint %s limits: Position: [%.1f, %.1f] deg, velocity: %.1f deg/s, torque: %.1f Nm", - parameters_.joint_names[i].c_str(), parameters_.min_position[i] * (180. / M_PI), - parameters_.max_position[i] * (180. / M_PI), parameters_.max_velocity[i] * (180. / M_PI), - parameters_.max_torque[i]); + parameters_.joint_names[i].c_str(), parameters_.min_positions[i] * (180. / M_PI), + parameters_.max_positions[i] * (180. / M_PI), parameters_.max_velocities[i] * (180. / M_PI), + parameters_.max_torques[i]); } } bool CommandGuard::command_in_position_limits_(const_idl_command_t_ref lbr_command, const_fri_state_t_ref /*lbr_state*/) const { for (std::size_t i = 0; i < lbr_command.joint_position.size(); ++i) { - if (lbr_command.joint_position[i] < parameters_.min_position[i] || - lbr_command.joint_position[i] > parameters_.max_position[i]) { + if (lbr_command.joint_position[i] < parameters_.min_positions[i] || + lbr_command.joint_position[i] > parameters_.max_positions[i]) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), ColorScheme::ERROR << "Position not in limits for joint '" << parameters_.joint_names[i].c_str() << "'" @@ -53,7 +53,7 @@ bool CommandGuard::command_in_velocity_limits_(const_idl_command_t_ref lbr_comma } for (std::size_t i = 0; i < lbr_command.joint_position[i]; ++i) { if (std::abs(prev_measured_joint_position_[i] - lbr_state.getMeasuredJointPosition()[i]) / dt > - parameters_.max_velocity[i]) { + parameters_.max_velocities[i]) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), ColorScheme::ERROR << "Velocity not in limits for joint '" << parameters_.joint_names[i].c_str() << "'" @@ -70,7 +70,7 @@ bool CommandGuard::command_in_torque_limits_(const_idl_command_t_ref lbr_command const_fri_state_t_ref lbr_state) const { for (std::size_t i = 0; i < lbr_command.torque.size(); ++i) { if (std::abs(lbr_command.torque[i] + lbr_state.getExternalTorque()[i]) > - parameters_.max_torque[i]) { + parameters_.max_torques[i]) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), ColorScheme::ERROR << "Torque not in limits for joint '" << parameters_.joint_names[i].c_str() @@ -85,9 +85,11 @@ bool SafeStopCommandGuard::command_in_position_limits_(const_idl_command_t_ref l const_fri_state_t_ref lbr_state) const { for (std::size_t i = 0; i < lbr_command.joint_position.size(); ++i) { if (lbr_command.joint_position[i] < - parameters_.min_position[i] + parameters_.max_velocity[i] * lbr_state.getSampleTime() || + parameters_.min_positions[i] + + parameters_.max_velocities[i] * lbr_state.getSampleTime() || lbr_command.joint_position[i] > - parameters_.max_position[i] - parameters_.max_velocity[i] * lbr_state.getSampleTime()) { + parameters_.max_positions[i] - + parameters_.max_velocities[i] * lbr_state.getSampleTime()) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), ColorScheme::ERROR << "Position not in limits for joint '" << parameters_.joint_names[i].c_str() << "'" diff --git a/lbr_ros2_control/config/lbr_system_interface.xacro b/lbr_ros2_control/config/lbr_system_interface.xacro index 57aea356..c7765a22 100644 --- a/lbr_ros2_control/config/lbr_system_interface.xacro +++ b/lbr_ros2_control/config/lbr_system_interface.xacro @@ -1,46 +1,7 @@ + params="sim joint_limits system_parameters"> @@ -51,22 +12,20 @@ lbr_ros2_control::SystemInterface - ${client_command_mode} - ${port_id} - ${remote_host} - ${rt_prio} - ${pid_p} - ${pid_i} - ${pid_d} - ${pid_i_max} - ${pid_i_min} - ${pid_antiwindup} - ${command_guard_variant} - - ${external_torque_cutoff_frequency} - - ${measured_torque_cutoff_frequency} - ${open_loop} + ${system_parameters['hardware']['client_command_mode']} + ${system_parameters['hardware']['port_id']} + ${system_parameters['hardware']['remote_host']} + ${system_parameters['hardware']['rt_prio']} + ${system_parameters['hardware']['pid_p']} + ${system_parameters['hardware']['pid_i']} + ${system_parameters['hardware']['pid_d']} + ${system_parameters['hardware']['pid_i_max']} + ${system_parameters['hardware']['pid_i_min']} + ${system_parameters['hardware']['pid_antiwindup']} + ${system_parameters['hardware']['command_guard_variant']} + ${system_parameters['hardware']['external_torque_cutoff_frequency']} + ${system_parameters['hardware']['measured_torque_cutoff_frequency']} + ${system_parameters['hardware']['open_loop']} @@ -91,15 +50,15 @@ - ${chain_root} - ${chain_tip} - ${damping} - ${force_x_th} - ${force_y_th} - ${force_z_th} - ${torque_x_th} - ${torque_y_th} - ${torque_z_th} + ${system_parameters['estimated_ft_sensor']['chain_root']} + ${system_parameters['estimated_ft_sensor']['chain_tip']} + ${system_parameters['estimated_ft_sensor']['damping']} + ${system_parameters['estimated_ft_sensor']['force_x_th']} + ${system_parameters['estimated_ft_sensor']['force_y_th']} + ${system_parameters['estimated_ft_sensor']['force_z_th']} + ${system_parameters['estimated_ft_sensor']['torque_x_th']} + ${system_parameters['estimated_ft_sensor']['torque_y_th']} + ${system_parameters['estimated_ft_sensor']['torque_z_th']} @@ -148,27 +107,48 @@ - - - - - - - + + + + + + + \ No newline at end of file diff --git a/lbr_ros2_control/config/lbr_system_parameters.yaml b/lbr_ros2_control/config/lbr_system_parameters.yaml new file mode 100644 index 00000000..3227c43d --- /dev/null +++ b/lbr_ros2_control/config/lbr_system_parameters.yaml @@ -0,0 +1,26 @@ +hardware: + client_command_mode: position + port_id: 30200 + remote_host: INADDR_ANY + rt_prio: 80 + pid_p: 10.0 + pid_i: 0.0 + pid_d: 0.0 + pid_i_max: 0.0 + pid_i_min: 0.0 + pid_antiwindup: false + command_guard_variant: default + external_torque_cutoff_frequency: 10 + measured_torque_cutoff_frequency: 10 + open_loop: true + +estimated_ft_sensor: + chain_root: link_0 + chain_tip: link_ee + damping: 0.2 + force_x_th: 2.0 + force_y_th: 2.0 + force_z_th: 2.0 + torque_x_th: 0.5 + torque_y_th: 0.5 + torque_z_th: 0.5 diff --git a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp index 37b129a7..f8756b19 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp @@ -100,6 +100,7 @@ class SystemInterface : public hardware_interface::SystemInterface { protected: // setup + bool parse_parameters_(const hardware_interface::HardwareInfo &info); void nan_command_interfaces_(); void nan_state_interfaces_(); bool verify_number_of_joints_(); diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 591920ea..ec58a513 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -12,58 +12,9 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { } // parameters_ from config/lbr_system_interface.xacro - std::string client_command_mode = system_info.hardware_parameters.at("client_command_mode"); - if (client_command_mode == "position") { -#if FRICLIENT_VERSION_MAJOR == 1 - parameters_.client_command_mode = KUKA::FRI::EClientCommandMode::POSITION; -#endif -#if FRICLIENT_VERSION_MAJOR == 2 - parameters_.client_command_mode = KUKA::FRI::EClientCommandMode::JOINT_POSITION; -#endif - } else if (client_command_mode == "torque") { - parameters_.client_command_mode = KUKA::FRI::EClientCommandMode::TORQUE; - } else if (client_command_mode == "wrench") { - parameters_.client_command_mode = KUKA::FRI::EClientCommandMode::WRENCH; - } else { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger(LOGGER_NAME), - lbr_fri_ros2::ColorScheme::ERROR - << "Expected client_command_mode 'position', 'torque' or 'wrench', got '" - << lbr_fri_ros2::ColorScheme::BOLD << parameters_.client_command_mode << "'" - << lbr_fri_ros2::ColorScheme::ENDC); + if (!parse_parameters_(system_info)) { return controller_interface::CallbackReturn::ERROR; } - parameters_.port_id = std::stoul(info_.hardware_parameters["port_id"]); - if (parameters_.port_id < 30200 || parameters_.port_id > 30209) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), - lbr_fri_ros2::ColorScheme::ERROR - << "Expected port_id in [30200, 30209], got '" - << lbr_fri_ros2::ColorScheme::BOLD << parameters_.port_id << "'" - << lbr_fri_ros2::ColorScheme::ENDC); - return controller_interface::CallbackReturn::ERROR; - } - info_.hardware_parameters["remote_host"] == "INADDR_ANY" - ? parameters_.remote_host = NULL - : parameters_.remote_host = info_.hardware_parameters["remote_host"].c_str(); - parameters_.rt_prio = std::stoul(info_.hardware_parameters["rt_prio"]); - std::transform(info_.hardware_parameters["open_loop"].begin(), - info_.hardware_parameters["open_loop"].end(), - info_.hardware_parameters["open_loop"].begin(), ::tolower); - parameters_.open_loop = info_.hardware_parameters["open_loop"] == "true"; - std::transform(info_.hardware_parameters["pid_antiwindup"].begin(), - info_.hardware_parameters["pid_antiwindup"].end(), - info_.hardware_parameters["pid_antiwindup"].begin(), ::tolower); - parameters_.pid_p = std::stod(info_.hardware_parameters["pid_p"]); - parameters_.pid_i = std::stod(info_.hardware_parameters["pid_i"]); - parameters_.pid_d = std::stod(info_.hardware_parameters["pid_d"]); - parameters_.pid_i_max = std::stod(info_.hardware_parameters["pid_i_max"]); - parameters_.pid_i_min = std::stod(info_.hardware_parameters["pid_i_min"]); - parameters_.pid_antiwindup = info_.hardware_parameters["pid_antiwindup"] == "true"; - parameters_.command_guard_variant = system_info.hardware_parameters.at("command_guard_variant"); - parameters_.external_torque_cutoff_frequency = - std::stod(info_.hardware_parameters["external_torque_cutoff_frequency"]); - parameters_.measured_torque_cutoff_frequency = - std::stod(info_.hardware_parameters["measured_torque_cutoff_frequency"]); // setup driver lbr_fri_ros2::PIDParameters pid_parameters; @@ -77,13 +28,13 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { pid_parameters.antiwindup = parameters_.pid_antiwindup; for (std::size_t idx = 0; idx < system_info.joints.size(); ++idx) { command_guard_parameters.joint_names[idx] = system_info.joints[idx].name; - command_guard_parameters.max_position[idx] = + command_guard_parameters.max_positions[idx] = std::stod(system_info.joints[idx].parameters.at("max_position")); - command_guard_parameters.min_position[idx] = + command_guard_parameters.min_positions[idx] = std::stod(system_info.joints[idx].parameters.at("min_position")); - command_guard_parameters.max_velocity[idx] = + command_guard_parameters.max_velocities[idx] = std::stod(system_info.joints[idx].parameters.at("max_velocity")); - command_guard_parameters.max_torque[idx] = + command_guard_parameters.max_torques[idx] = std::stod(system_info.joints[idx].parameters.at("max_torque")); } state_interface_parameters.external_torque_cutoff_frequency = @@ -370,6 +321,70 @@ hardware_interface::return_type SystemInterface::write(const rclcpp::Time & /*ti return hardware_interface::return_type::OK; } +bool SystemInterface::parse_parameters_(const hardware_interface::HardwareInfo &system_info) { + try { + std::string client_command_mode = system_info.hardware_parameters.at("client_command_mode"); + if (client_command_mode == "position") { +#if FRICLIENT_VERSION_MAJOR == 1 + parameters_.client_command_mode = KUKA::FRI::EClientCommandMode::POSITION; +#endif +#if FRICLIENT_VERSION_MAJOR == 2 + parameters_.client_command_mode = KUKA::FRI::EClientCommandMode::JOINT_POSITION; +#endif + } else if (client_command_mode == "torque") { + parameters_.client_command_mode = KUKA::FRI::EClientCommandMode::TORQUE; + } else if (client_command_mode == "wrench") { + parameters_.client_command_mode = KUKA::FRI::EClientCommandMode::WRENCH; + } else { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger(LOGGER_NAME), + lbr_fri_ros2::ColorScheme::ERROR + << "Expected client_command_mode 'position', 'torque' or 'wrench', got '" + << lbr_fri_ros2::ColorScheme::BOLD << parameters_.client_command_mode << "'" + << lbr_fri_ros2::ColorScheme::ENDC); + return false; + } + parameters_.port_id = std::stoul(info_.hardware_parameters["port_id"]); + if (parameters_.port_id < 30200 || parameters_.port_id > 30209) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + lbr_fri_ros2::ColorScheme::ERROR + << "Expected port_id in [30200, 30209], got '" + << lbr_fri_ros2::ColorScheme::BOLD << parameters_.port_id << "'" + << lbr_fri_ros2::ColorScheme::ENDC); + return false; + } + info_.hardware_parameters["remote_host"] == "INADDR_ANY" + ? parameters_.remote_host = NULL + : parameters_.remote_host = info_.hardware_parameters["remote_host"].c_str(); + parameters_.rt_prio = std::stoul(info_.hardware_parameters["rt_prio"]); + std::transform(info_.hardware_parameters["open_loop"].begin(), + info_.hardware_parameters["open_loop"].end(), + info_.hardware_parameters["open_loop"].begin(), ::tolower); + parameters_.open_loop = info_.hardware_parameters["open_loop"] == "true"; + std::transform(info_.hardware_parameters["pid_antiwindup"].begin(), + info_.hardware_parameters["pid_antiwindup"].end(), + info_.hardware_parameters["pid_antiwindup"].begin(), ::tolower); + parameters_.pid_p = std::stod(info_.hardware_parameters["pid_p"]); + parameters_.pid_i = std::stod(info_.hardware_parameters["pid_i"]); + parameters_.pid_d = std::stod(info_.hardware_parameters["pid_d"]); + parameters_.pid_i_max = std::stod(info_.hardware_parameters["pid_i_max"]); + parameters_.pid_i_min = std::stod(info_.hardware_parameters["pid_i_min"]); + parameters_.pid_antiwindup = info_.hardware_parameters["pid_antiwindup"] == "true"; + parameters_.command_guard_variant = system_info.hardware_parameters.at("command_guard_variant"); + parameters_.external_torque_cutoff_frequency = + std::stod(info_.hardware_parameters["external_torque_cutoff_frequency"]); + parameters_.measured_torque_cutoff_frequency = + std::stod(info_.hardware_parameters["measured_torque_cutoff_frequency"]); + } catch (const std::out_of_range &e) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + lbr_fri_ros2::ColorScheme::ERROR + << "Failed to parse hardware parameters with: " << e.what() + << lbr_fri_ros2::ColorScheme::ENDC); + return false; + } + return true; +} + void SystemInterface::nan_command_interfaces_() { hw_lbr_command_.joint_position.fill(std::numeric_limits::quiet_NaN()); hw_lbr_command_.torque.fill(std::numeric_limits::quiet_NaN()); From 1a899924f5a7663ac4356f2aa74963887e12f0d8 Mon Sep 17 00:00:00 2001 From: mhubii Date: Sat, 11 May 2024 15:48:56 +0100 Subject: [PATCH 65/76] .urdf.xacro -> .xacro (redundancy) --- lbr_bringup/doc/lbr_bringup.rst | 2 -- lbr_bringup/launch_mixins/lbr_bringup.py | 2 +- lbr_bringup/launch_mixins/lbr_description.py | 2 +- lbr_demos/doc/lbr_demos.rst | 8 +++++-- lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst | 24 ++++++++++++++++++- lbr_description/README.md | 2 +- lbr_description/test/test_urdf.py | 2 +- .../{iiwa14.urdf.xacro => iiwa14.xacro} | 2 +- ...on.urdf.xacro => iiwa14_description.xacro} | 0 .../iiwa7/{iiwa7.urdf.xacro => iiwa7.xacro} | 2 +- ...ion.urdf.xacro => iiwa7_description.xacro} | 0 .../med14/{med14.urdf.xacro => med14.xacro} | 2 +- ...ion.urdf.xacro => med14_description.xacro} | 0 .../urdf/med7/{med7.urdf.xacro => med7.xacro} | 2 +- ...tion.urdf.xacro => med7_description.xacro} | 0 lbr_fri_ros2/test/README.md | 12 ++++++++++ lbr_moveit_config/doc/lbr_moveit_config.rst | 2 +- .../iiwa14_moveit_config/.setup_assistant | 2 +- .../launch/move_group.launch.py | 2 +- .../iiwa7_moveit_config/.setup_assistant | 2 +- .../launch/move_group.launch.py | 2 +- .../med14_moveit_config/.setup_assistant | 2 +- .../launch/move_group.launch.py | 2 +- .../med7_moveit_config/.setup_assistant | 2 +- .../launch/move_group.launch.py | 2 +- 25 files changed, 58 insertions(+), 22 deletions(-) rename lbr_description/urdf/iiwa14/{iiwa14.urdf.xacro => iiwa14.xacro} (96%) rename lbr_description/urdf/iiwa14/{iiwa14_description.urdf.xacro => iiwa14_description.xacro} (100%) rename lbr_description/urdf/iiwa7/{iiwa7.urdf.xacro => iiwa7.xacro} (96%) rename lbr_description/urdf/iiwa7/{iiwa7_description.urdf.xacro => iiwa7_description.xacro} (100%) rename lbr_description/urdf/med14/{med14.urdf.xacro => med14.xacro} (96%) rename lbr_description/urdf/med14/{med14_description.urdf.xacro => med14_description.xacro} (100%) rename lbr_description/urdf/med7/{med7.urdf.xacro => med7.xacro} (96%) rename lbr_description/urdf/med7/{med7_description.urdf.xacro => med7_description.xacro} (100%) create mode 100644 lbr_fri_ros2/test/README.md diff --git a/lbr_bringup/doc/lbr_bringup.rst b/lbr_bringup/doc/lbr_bringup.rst index 36d0df49..cf70a42d 100644 --- a/lbr_bringup/doc/lbr_bringup.rst +++ b/lbr_bringup/doc/lbr_bringup.rst @@ -1,7 +1,5 @@ LBR Bringup =========== -TODO: Mention different controllers here already - The ``lbr_bringup`` works for the simulation and the real robot. Run: .. code:: bash diff --git a/lbr_bringup/launch_mixins/lbr_bringup.py b/lbr_bringup/launch_mixins/lbr_bringup.py index 0ae594db..afe38589 100644 --- a/lbr_bringup/launch_mixins/lbr_bringup.py +++ b/lbr_bringup/launch_mixins/lbr_bringup.py @@ -64,7 +64,7 @@ def moveit_configs_builder( .robot_description( os.path.join( get_package_share_directory("lbr_description"), - f"urdf/{robot_name}/{robot_name}.urdf.xacro", + f"urdf/{robot_name}/{robot_name}.xacro", ), ) .planning_pipelines(default_planning_pipeline="ompl", pipelines=["ompl"]) diff --git a/lbr_bringup/launch_mixins/lbr_description.py b/lbr_bringup/launch_mixins/lbr_description.py index 6cafd13f..c91f2f7e 100644 --- a/lbr_bringup/launch_mixins/lbr_description.py +++ b/lbr_bringup/launch_mixins/lbr_description.py @@ -85,7 +85,7 @@ def param_robot_description( model, ] ), - ".urdf.xacro", + ".xacro", " robot_name:=", robot_name, " port_id:=", diff --git a/lbr_demos/doc/lbr_demos.rst b/lbr_demos/doc/lbr_demos.rst index 042b4ac2..feec59f2 100644 --- a/lbr_demos/doc/lbr_demos.rst +++ b/lbr_demos/doc/lbr_demos.rst @@ -8,8 +8,8 @@ Contolling the LBR through ROS 2 control. .. warning:: On the real robot, do always execute in ``T1`` mode first. -Basic Demos ------------ +Demos +----- .. toctree:: :titlesonly: @@ -23,3 +23,7 @@ Advanced Demos LBR Demos Advanced C++ <../lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst> LBR Demos Advanced Python <../lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst> + +MoveIt 2 Demos +-------------- +TODO diff --git a/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst b/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst index 2751aac5..497f640f 100644 --- a/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst +++ b/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst @@ -1,6 +1,6 @@ LBR Demos C++ ============= - add table of contents TODO with sim / real matrix +add table of contents TODO with sim / real matrix .. warning:: Do always execute in ``T1`` mode first. @@ -11,6 +11,13 @@ Joint Sine Overlay .. thumbnail:: ../../doc/img/applications_joint_sine_overlay.png +#. Select + + - ``FRI send period``: ``10 ms`` + - ``IP address``: ``your configuration`` + - ``FRI control mode``: ``POSITION_CONTROL`` + - ``FRI client command mode``: ``POSITION`` + #. Run the robot driver: .. code-block:: bash @@ -60,6 +67,7 @@ Real Robot - ``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 ``sim:=false``. Torque Sine Overlay @@ -68,6 +76,13 @@ Torque Sine Overlay .. thumbnail:: ../../doc/img/applications_torque_sine_overlay.png +#. Select + + - ``FRI send period``: ``2 ms`` + - ``IP address``: ``your configuration`` + - ``FRI control mode``: ``JOINT_IMPEDANCE_CONTROL`` + - ``FRI client command mode``: ``TORQUE`` + #. Run the robot driver: .. code-block:: bash @@ -93,6 +108,13 @@ Wrench Sine Overlay .. thumbnail:: ../../doc/img/applications_wrench_sine_overlay.png +#. Select + + - ``FRI send period``: ``2 ms`` + - ``IP address``: ``your configuration`` + - ``FRI control mode``: ``CARTESIAN_IMPEDANCE_CONTROL`` + - ``FRI client command mode``: ``WRENCH`` + #. Run the robot driver: .. code-block:: bash diff --git a/lbr_description/README.md b/lbr_description/README.md index 81576e8f..5ffded58 100644 --- a/lbr_description/README.md +++ b/lbr_description/README.md @@ -2,5 +2,5 @@ Launch and example with ```shell -ros2 launch lbr_description view_robot.launch.py description_file:=urdf/med7/med7.urdf.xacro +ros2 launch lbr_description view_robot.launch.py description_file:=urdf/med7/med7.xacro ``` diff --git a/lbr_description/test/test_urdf.py b/lbr_description/test/test_urdf.py index e36372c9..ffe18443 100644 --- a/lbr_description/test/test_urdf.py +++ b/lbr_description/test/test_urdf.py @@ -32,7 +32,7 @@ def setup_xml_and_reference(kuka_id: str) -> Tuple[str, LBRSpecification]: get_package_share_directory("lbr_description"), "urdf", lbr_specification.name, - f"{lbr_specification.name}.urdf.xacro", + f"{lbr_specification.name}.xacro", ) xml = xacro.process(path) diff --git a/lbr_description/urdf/iiwa14/iiwa14.urdf.xacro b/lbr_description/urdf/iiwa14/iiwa14.xacro similarity index 96% rename from lbr_description/urdf/iiwa14/iiwa14.urdf.xacro rename to lbr_description/urdf/iiwa14/iiwa14.xacro index 7b7c7eb2..e936c2a6 100644 --- a/lbr_description/urdf/iiwa14/iiwa14.urdf.xacro +++ b/lbr_description/urdf/iiwa14/iiwa14.xacro @@ -4,7 +4,7 @@ - + diff --git a/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro b/lbr_description/urdf/iiwa14/iiwa14_description.xacro similarity index 100% rename from lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro rename to lbr_description/urdf/iiwa14/iiwa14_description.xacro diff --git a/lbr_description/urdf/iiwa7/iiwa7.urdf.xacro b/lbr_description/urdf/iiwa7/iiwa7.xacro similarity index 96% rename from lbr_description/urdf/iiwa7/iiwa7.urdf.xacro rename to lbr_description/urdf/iiwa7/iiwa7.xacro index 84d69b17..a451cf70 100644 --- a/lbr_description/urdf/iiwa7/iiwa7.urdf.xacro +++ b/lbr_description/urdf/iiwa7/iiwa7.xacro @@ -4,7 +4,7 @@ - + diff --git a/lbr_description/urdf/iiwa7/iiwa7_description.urdf.xacro b/lbr_description/urdf/iiwa7/iiwa7_description.xacro similarity index 100% rename from lbr_description/urdf/iiwa7/iiwa7_description.urdf.xacro rename to lbr_description/urdf/iiwa7/iiwa7_description.xacro diff --git a/lbr_description/urdf/med14/med14.urdf.xacro b/lbr_description/urdf/med14/med14.xacro similarity index 96% rename from lbr_description/urdf/med14/med14.urdf.xacro rename to lbr_description/urdf/med14/med14.xacro index ba9e8d94..c3abc0e9 100644 --- a/lbr_description/urdf/med14/med14.urdf.xacro +++ b/lbr_description/urdf/med14/med14.xacro @@ -4,7 +4,7 @@ - + diff --git a/lbr_description/urdf/med14/med14_description.urdf.xacro b/lbr_description/urdf/med14/med14_description.xacro similarity index 100% rename from lbr_description/urdf/med14/med14_description.urdf.xacro rename to lbr_description/urdf/med14/med14_description.xacro diff --git a/lbr_description/urdf/med7/med7.urdf.xacro b/lbr_description/urdf/med7/med7.xacro similarity index 96% rename from lbr_description/urdf/med7/med7.urdf.xacro rename to lbr_description/urdf/med7/med7.xacro index 04ae010f..0e2fe380 100644 --- a/lbr_description/urdf/med7/med7.urdf.xacro +++ b/lbr_description/urdf/med7/med7.xacro @@ -4,7 +4,7 @@ - + diff --git a/lbr_description/urdf/med7/med7_description.urdf.xacro b/lbr_description/urdf/med7/med7_description.xacro similarity index 100% rename from lbr_description/urdf/med7/med7_description.urdf.xacro rename to lbr_description/urdf/med7/med7_description.xacro diff --git a/lbr_fri_ros2/test/README.md b/lbr_fri_ros2/test/README.md new file mode 100644 index 00000000..2a077acc --- /dev/null +++ b/lbr_fri_ros2/test/README.md @@ -0,0 +1,12 @@ +# To test: + +- procedural: + - monitoring wait + - monitoring ready + - commanding wait + - commanding active + +- invalid client command mode +- invalid control mode +- invalid velocity +- invalid position diff --git a/lbr_moveit_config/doc/lbr_moveit_config.rst b/lbr_moveit_config/doc/lbr_moveit_config.rst index 245aab41..71ba4118 100644 --- a/lbr_moveit_config/doc/lbr_moveit_config.rst +++ b/lbr_moveit_config/doc/lbr_moveit_config.rst @@ -20,7 +20,7 @@ This procedure applies to all LBRs: ``iiwa7``, ``iiwa14``, ``med7``, and ``med14 ros2 launch moveit_setup_assistant setup_assistant.launch.py -#. .. dropdown:: ``Load Files``: E.g. ``lbr_fri_ros2_stack_ws/install/lbr_description/share/lbr_description/urdf/iiwa7/iiwa7.urdf.xacro`` +#. .. dropdown:: ``Load Files``: E.g. ``lbr_fri_ros2_stack_ws/install/lbr_description/share/lbr_description/urdf/iiwa7/iiwa7.xacro`` .. thumbnail:: img/00_start_screen.png diff --git a/lbr_moveit_config/iiwa14_moveit_config/.setup_assistant b/lbr_moveit_config/iiwa14_moveit_config/.setup_assistant index 4323224d..55161cd3 100644 --- a/lbr_moveit_config/iiwa14_moveit_config/.setup_assistant +++ b/lbr_moveit_config/iiwa14_moveit_config/.setup_assistant @@ -1,7 +1,7 @@ moveit_setup_assistant_config: urdf: package: lbr_description - relative_path: urdf/iiwa14/iiwa14.urdf.xacro + relative_path: urdf/iiwa14/iiwa14.xacro srdf: relative_path: config/iiwa14.srdf package_settings: diff --git a/lbr_moveit_config/iiwa14_moveit_config/launch/move_group.launch.py b/lbr_moveit_config/iiwa14_moveit_config/launch/move_group.launch.py index 85eecf46..deb65967 100644 --- a/lbr_moveit_config/iiwa14_moveit_config/launch/move_group.launch.py +++ b/lbr_moveit_config/iiwa14_moveit_config/launch/move_group.launch.py @@ -11,7 +11,7 @@ def generate_launch_description(): .robot_description( os.path.join( get_package_share_directory("lbr_description"), - "urdf/iiwa14/iiwa14.urdf.xacro", + "urdf/iiwa14/iiwa14.xacro", ) ) .to_moveit_configs() diff --git a/lbr_moveit_config/iiwa7_moveit_config/.setup_assistant b/lbr_moveit_config/iiwa7_moveit_config/.setup_assistant index 36c6f93a..1b331588 100644 --- a/lbr_moveit_config/iiwa7_moveit_config/.setup_assistant +++ b/lbr_moveit_config/iiwa7_moveit_config/.setup_assistant @@ -1,7 +1,7 @@ moveit_setup_assistant_config: urdf: package: lbr_description - relative_path: urdf/iiwa7/iiwa7.urdf.xacro + relative_path: urdf/iiwa7/iiwa7.xacro srdf: relative_path: config/iiwa7.srdf package_settings: diff --git a/lbr_moveit_config/iiwa7_moveit_config/launch/move_group.launch.py b/lbr_moveit_config/iiwa7_moveit_config/launch/move_group.launch.py index 73f769e1..ef3c7579 100644 --- a/lbr_moveit_config/iiwa7_moveit_config/launch/move_group.launch.py +++ b/lbr_moveit_config/iiwa7_moveit_config/launch/move_group.launch.py @@ -11,7 +11,7 @@ def generate_launch_description(): .robot_description( os.path.join( get_package_share_directory("lbr_description"), - "urdf/iiwa7/iiwa7.urdf.xacro", + "urdf/iiwa7/iiwa7.xacro", ) ) .to_moveit_configs() diff --git a/lbr_moveit_config/med14_moveit_config/.setup_assistant b/lbr_moveit_config/med14_moveit_config/.setup_assistant index 480cfd97..a9a44cf4 100644 --- a/lbr_moveit_config/med14_moveit_config/.setup_assistant +++ b/lbr_moveit_config/med14_moveit_config/.setup_assistant @@ -1,7 +1,7 @@ moveit_setup_assistant_config: urdf: package: lbr_description - relative_path: urdf/med14/med14.urdf.xacro + relative_path: urdf/med14/med14.xacro srdf: relative_path: config/med14.srdf package_settings: diff --git a/lbr_moveit_config/med14_moveit_config/launch/move_group.launch.py b/lbr_moveit_config/med14_moveit_config/launch/move_group.launch.py index 43368b43..89ccedbb 100644 --- a/lbr_moveit_config/med14_moveit_config/launch/move_group.launch.py +++ b/lbr_moveit_config/med14_moveit_config/launch/move_group.launch.py @@ -11,7 +11,7 @@ def generate_launch_description(): .robot_description( os.path.join( get_package_share_directory("lbr_description"), - "urdf/med14/med14.urdf.xacro", + "urdf/med14/med14.xacro", ) ) .to_moveit_configs() diff --git a/lbr_moveit_config/med7_moveit_config/.setup_assistant b/lbr_moveit_config/med7_moveit_config/.setup_assistant index afa4965b..d788c22c 100644 --- a/lbr_moveit_config/med7_moveit_config/.setup_assistant +++ b/lbr_moveit_config/med7_moveit_config/.setup_assistant @@ -1,7 +1,7 @@ moveit_setup_assistant_config: urdf: package: lbr_description - relative_path: urdf/med7/med7.urdf.xacro + relative_path: urdf/med7/med7.xacro srdf: relative_path: config/med7.srdf package_settings: diff --git a/lbr_moveit_config/med7_moveit_config/launch/move_group.launch.py b/lbr_moveit_config/med7_moveit_config/launch/move_group.launch.py index 1ce1122f..cc87e1fa 100644 --- a/lbr_moveit_config/med7_moveit_config/launch/move_group.launch.py +++ b/lbr_moveit_config/med7_moveit_config/launch/move_group.launch.py @@ -11,7 +11,7 @@ def generate_launch_description(): .robot_description( os.path.join( get_package_share_directory("lbr_description"), - "urdf/med7/med7.urdf.xacro", + "urdf/med7/med7.xacro", ) ) .to_moveit_configs() From 85918082694fcffbb12eefd881718325ac01b595 Mon Sep 17 00:00:00 2001 From: mhubii Date: Sat, 11 May 2024 16:48:26 +0100 Subject: [PATCH 66/76] updated tests --- lbr_fri_ros2/test/test_position_command.cpp | 20 ++++++++++---------- lbr_fri_ros2/test/test_torque_command.cpp | 20 ++++++++++---------- lbr_fri_ros2/test/test_wrench_command.cpp | 20 ++++++++++---------- 3 files changed, 30 insertions(+), 30 deletions(-) diff --git a/lbr_fri_ros2/test/test_position_command.cpp b/lbr_fri_ros2/test/test_position_command.cpp index 380cf463..8c6779af 100644 --- a/lbr_fri_ros2/test/test_position_command.cpp +++ b/lbr_fri_ros2/test/test_position_command.cpp @@ -25,16 +25,16 @@ int main() { pid_params.p = 10.0; cmd_guard_params.joint_names = {"A1", "A2", "A3", "A4", "A5", "A6", "A7"}; - cmd_guard_params.max_position = {170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., - 120. * M_PI / 180., 170. * M_PI / 180., 120. * M_PI / 180., - 175. * M_PI / 180.}; - cmd_guard_params.min_position = {-170. * M_PI / 180., -120. * M_PI / 180., -170. * M_PI / 180., - -120. * M_PI / 180., -170. * M_PI / 180., -120. * M_PI / 180., - -175. * M_PI / 180.}; - cmd_guard_params.max_velocity = {98. * M_PI / 180., 98. * M_PI / 180., 100. * M_PI / 180., - 130. * M_PI / 180., 140. * M_PI / 180., 180. * M_PI / 180., - 180. * M_PI / 180.}; - cmd_guard_params.max_torque = { + cmd_guard_params.max_positions = {170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., + 120. * M_PI / 180., 170. * M_PI / 180., 120. * M_PI / 180., + 175. * M_PI / 180.}; + cmd_guard_params.min_positions = {-170. * M_PI / 180., -120. * M_PI / 180., -170. * M_PI / 180., + -120. * M_PI / 180., -170. * M_PI / 180., -120. * M_PI / 180., + -175. * M_PI / 180.}; + cmd_guard_params.max_velocities = {98. * M_PI / 180., 98. * M_PI / 180., 100. * M_PI / 180., + 130. * M_PI / 180., 140. * M_PI / 180., 180. * M_PI / 180., + 180. * M_PI / 180.}; + cmd_guard_params.max_torques = { 200., 200., 200., 200., 200., 200., 200., }; diff --git a/lbr_fri_ros2/test/test_torque_command.cpp b/lbr_fri_ros2/test/test_torque_command.cpp index 14cc35f5..2af7f93a 100644 --- a/lbr_fri_ros2/test/test_torque_command.cpp +++ b/lbr_fri_ros2/test/test_torque_command.cpp @@ -25,16 +25,16 @@ int main() { pid_params.p = 10.0; cmd_guard_params.joint_names = {"A1", "A2", "A3", "A4", "A5", "A6", "A7"}; - cmd_guard_params.max_position = {170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., - 120. * M_PI / 180., 170. * M_PI / 180., 120. * M_PI / 180., - 175. * M_PI / 180.}; - cmd_guard_params.min_position = {-170. * M_PI / 180., -120. * M_PI / 180., -170. * M_PI / 180., - -120. * M_PI / 180., -170. * M_PI / 180., -120. * M_PI / 180., - -175. * M_PI / 180.}; - cmd_guard_params.max_velocity = {98. * M_PI / 180., 98. * M_PI / 180., 100. * M_PI / 180., - 130. * M_PI / 180., 140. * M_PI / 180., 180. * M_PI / 180., - 180. * M_PI / 180.}; - cmd_guard_params.max_torque = { + cmd_guard_params.max_positions = {170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., + 120. * M_PI / 180., 170. * M_PI / 180., 120. * M_PI / 180., + 175. * M_PI / 180.}; + cmd_guard_params.min_positions = {-170. * M_PI / 180., -120. * M_PI / 180., -170. * M_PI / 180., + -120. * M_PI / 180., -170. * M_PI / 180., -120. * M_PI / 180., + -175. * M_PI / 180.}; + cmd_guard_params.max_velocities = {98. * M_PI / 180., 98. * M_PI / 180., 100. * M_PI / 180., + 130. * M_PI / 180., 140. * M_PI / 180., 180. * M_PI / 180., + 180. * M_PI / 180.}; + cmd_guard_params.max_torques = { 200., 200., 200., 200., 200., 200., 200., }; diff --git a/lbr_fri_ros2/test/test_wrench_command.cpp b/lbr_fri_ros2/test/test_wrench_command.cpp index 44240504..de88e0e6 100644 --- a/lbr_fri_ros2/test/test_wrench_command.cpp +++ b/lbr_fri_ros2/test/test_wrench_command.cpp @@ -25,16 +25,16 @@ int main() { pid_params.p = 10.0; cmd_guard_params.joint_names = {"A1", "A2", "A3", "A4", "A5", "A6", "A7"}; - cmd_guard_params.max_position = {170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., - 120. * M_PI / 180., 170. * M_PI / 180., 120. * M_PI / 180., - 175. * M_PI / 180.}; - cmd_guard_params.min_position = {-170. * M_PI / 180., -120. * M_PI / 180., -170. * M_PI / 180., - -120. * M_PI / 180., -170. * M_PI / 180., -120. * M_PI / 180., - -175. * M_PI / 180.}; - cmd_guard_params.max_velocity = {98. * M_PI / 180., 98. * M_PI / 180., 100. * M_PI / 180., - 130. * M_PI / 180., 140. * M_PI / 180., 180. * M_PI / 180., - 180. * M_PI / 180.}; - cmd_guard_params.max_torque = { + cmd_guard_params.max_positions = {170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., + 120. * M_PI / 180., 170. * M_PI / 180., 120. * M_PI / 180., + 175. * M_PI / 180.}; + cmd_guard_params.min_positions = {-170. * M_PI / 180., -120. * M_PI / 180., -170. * M_PI / 180., + -120. * M_PI / 180., -170. * M_PI / 180., -120. * M_PI / 180., + -175. * M_PI / 180.}; + cmd_guard_params.max_velocities = {98. * M_PI / 180., 98. * M_PI / 180., 100. * M_PI / 180., + 130. * M_PI / 180., 140. * M_PI / 180., 180. * M_PI / 180., + 180. * M_PI / 180.}; + cmd_guard_params.max_torques = { 200., 200., 200., 200., 200., 200., 200., }; From 8f06496945c62ad9a048b58e13e5bfe5b5ad0665 Mon Sep 17 00:00:00 2001 From: mhubii Date: Sat, 11 May 2024 18:15:50 +0100 Subject: [PATCH 67/76] fixed open loop --- .../include/lbr_fri_ros2/command_guard.hpp | 17 ++++++----- .../lbr_fri_ros2/interfaces/base_command.hpp | 8 +++--- .../interfaces/position_command.hpp | 2 +- .../interfaces/torque_command.hpp | 2 +- .../interfaces/wrench_command.hpp | 2 +- lbr_fri_ros2/src/async_client.cpp | 12 ++++++-- lbr_fri_ros2/src/command_guard.cpp | 28 ++++++++----------- lbr_fri_ros2/src/interfaces/base_command.cpp | 5 ++-- .../src/interfaces/position_command.cpp | 14 +++++----- .../src/interfaces/torque_command.cpp | 18 ++++++------ .../src/interfaces/wrench_command.cpp | 12 ++++---- lbr_fri_ros2/test/test_command_interfaces.cpp | 12 ++++++-- 12 files changed, 71 insertions(+), 61 deletions(-) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp index 16bed9c3..7d2edcdf 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp @@ -15,6 +15,7 @@ #include "friVersion.h" #include "lbr_fri_idl/msg/lbr_command.hpp" +#include "lbr_fri_idl/msg/lbr_state.hpp" #include "lbr_fri_ros2/formatting.hpp" namespace lbr_fri_ros2 { @@ -37,28 +38,26 @@ class CommandGuard { // ROS IDL types using idl_command_t = lbr_fri_idl::msg::LBRCommand; using const_idl_command_t_ref = const idl_command_t &; + using idl_state_t = lbr_fri_idl::msg::LBRState; + using const_idl_state_t_ref = const idl_state_t &; using jnt_array_t = idl_command_t::_joint_position_type; using const_jnt_array_t_ref = const jnt_array_t &; - // FRI types - using fri_state_t = KUKA::FRI::LBRState; - using const_fri_state_t_ref = const fri_state_t &; - public: CommandGuard() = default; CommandGuard(const CommandGuardParameters &command_guard_parameters); virtual bool is_valid_command(const_idl_command_t_ref lbr_command, - const_fri_state_t_ref lbr_state); + const_idl_state_t_ref lbr_state); void log_info() const; protected: virtual bool command_in_position_limits_(const_idl_command_t_ref lbr_command, - const_fri_state_t_ref /*lbr_state*/) const; + const_idl_state_t_ref /*lbr_state*/) const; virtual bool command_in_velocity_limits_(const_idl_command_t_ref lbr_command, - const_fri_state_t_ref lbr_state); + const_idl_state_t_ref lbr_state); virtual bool command_in_torque_limits_(const_idl_command_t_ref lbr_command, - const_fri_state_t_ref lbr_state) const; + const_idl_state_t_ref lbr_state) const; CommandGuardParameters parameters_; bool prev_measured_joint_position_init_; @@ -72,7 +71,7 @@ class SafeStopCommandGuard : public CommandGuard { protected: virtual bool command_in_position_limits_(const_idl_command_t_ref lbr_command, - const_fri_state_t_ref lbr_state) const override; + const_idl_state_t_ref lbr_state) const override; }; std::unique_ptr diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp index a29ccf06..d5f61579 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp @@ -25,12 +25,12 @@ class BaseCommandInterface { // ROS IDL types using idl_command_t = lbr_fri_idl::msg::LBRCommand; using const_idl_command_t_ref = const idl_command_t &; + using idl_state_t = lbr_fri_idl::msg::LBRState; + using const_idl_state_t_ref = const idl_state_t &; // FRI types using fri_command_t = KUKA::FRI::LBRCommand; using fri_command_t_ref = fri_command_t &; - using fri_state_t = KUKA::FRI::LBRState; - using const_fri_state_t_ref = const fri_state_t &; public: BaseCommandInterface() = delete; @@ -38,9 +38,9 @@ class BaseCommandInterface { const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant = "default"); - virtual void buffered_command_to_fri(fri_command_t_ref command, const_fri_state_t_ref state) = 0; + virtual void buffered_command_to_fri(fri_command_t_ref command, const_idl_state_t_ref state) = 0; inline void buffer_command_target(const_idl_command_t_ref command) { command_target_ = command; } - void init_command(const_fri_state_t_ref state); + void init_command(const_idl_state_t_ref state); inline const_idl_command_t_ref get_command() const { return command_; } inline const_idl_command_t_ref get_command_target() const { return command_target_; } diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp index b707dd51..d2f345c5 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp @@ -16,7 +16,7 @@ class PositionCommandInterface : public BaseCommandInterface { const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant = "default"); - void buffered_command_to_fri(fri_command_t_ref command, const_fri_state_t_ref state) override; + void buffered_command_to_fri(fri_command_t_ref command, const_idl_state_t_ref state) override; }; } // end of namespace lbr_fri_ros2 #endif // LBR_FRI_ROS2__INTERFACES__POSITION_COMMAND_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp index f68e6b9f..fab800eb 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp @@ -16,7 +16,7 @@ class TorqueCommandInterface : public BaseCommandInterface { const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant = "default"); - void buffered_command_to_fri(fri_command_t_ref command, const_fri_state_t_ref state) override; + void buffered_command_to_fri(fri_command_t_ref command, const_idl_state_t_ref state) override; }; } // end of namespace lbr_fri_ros2 #endif // LBR_FRI_ROS2__INTERFACES__TORQUE_COMMAND_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp index d42ad4e0..69b55b1a 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp @@ -16,7 +16,7 @@ class WrenchCommandInterface : public BaseCommandInterface { const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant = "default"); - void buffered_command_to_fri(fri_command_t_ref command, const_fri_state_t_ref state) override; + void buffered_command_to_fri(fri_command_t_ref command, const_idl_state_t_ref state) override; }; } // end of namespace lbr_fri_ros2 #endif // LBR_FRI_ROS2__INTERFACES__WRENCH_COMMAND_HPP_ diff --git a/lbr_fri_ros2/src/async_client.cpp b/lbr_fri_ros2/src/async_client.cpp index d44d0830..a32e7ae1 100644 --- a/lbr_fri_ros2/src/async_client.cpp +++ b/lbr_fri_ros2/src/async_client.cpp @@ -65,7 +65,8 @@ void AsyncClient::onStateChange(KUKA::FRI::ESessionState old_state, << "'"); // initialize command - command_interface_ptr_->init_command(robotState()); + state_interface_ptr_->set_state(robotState()); + command_interface_ptr_->init_command(state_interface_ptr_->get_state()); } void AsyncClient::monitor() { state_interface_ptr_->set_state(robotState()); }; @@ -73,7 +74,9 @@ void AsyncClient::monitor() { state_interface_ptr_->set_state(robotState()); }; void AsyncClient::waitForCommand() { KUKA::FRI::LBRClient::waitForCommand(); state_interface_ptr_->set_state(robotState()); - command_interface_ptr_->buffered_command_to_fri(robotCommand(), robotState()); + command_interface_ptr_->init_command(state_interface_ptr_->get_state()); + command_interface_ptr_->buffered_command_to_fri(robotCommand(), + state_interface_ptr_->get_state()); } void AsyncClient::command() { @@ -83,6 +86,9 @@ void AsyncClient::command() { } else { state_interface_ptr_->set_state(robotState()); } - command_interface_ptr_->buffered_command_to_fri(robotCommand(), robotState()); + command_interface_ptr_->buffered_command_to_fri( + robotCommand(), + state_interface_ptr_->get_state()); // current state accessed via state interface (allows for + // open loop and is statically sized) } } // end of namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/command_guard.cpp b/lbr_fri_ros2/src/command_guard.cpp index 1d885305..431dce75 100644 --- a/lbr_fri_ros2/src/command_guard.cpp +++ b/lbr_fri_ros2/src/command_guard.cpp @@ -5,7 +5,7 @@ CommandGuard::CommandGuard(const CommandGuardParameters &command_guard_parameter : parameters_(command_guard_parameters){}; bool CommandGuard::is_valid_command(const_idl_command_t_ref lbr_command, - const_fri_state_t_ref lbr_state) { + const_idl_state_t_ref lbr_state) { if (!command_in_position_limits_(lbr_command, lbr_state)) { return false; } @@ -28,7 +28,7 @@ void CommandGuard::log_info() const { } bool CommandGuard::command_in_position_limits_(const_idl_command_t_ref lbr_command, - const_fri_state_t_ref /*lbr_state*/) const { + const_idl_state_t_ref /*lbr_state*/) const { for (std::size_t i = 0; i < lbr_command.joint_position.size(); ++i) { if (lbr_command.joint_position[i] < parameters_.min_positions[i] || lbr_command.joint_position[i] > parameters_.max_positions[i]) { @@ -43,16 +43,15 @@ bool CommandGuard::command_in_position_limits_(const_idl_command_t_ref lbr_comma } bool CommandGuard::command_in_velocity_limits_(const_idl_command_t_ref lbr_command, - const_fri_state_t_ref lbr_state) { - const double &dt = lbr_state.getSampleTime(); + const_idl_state_t_ref lbr_state) { + const double &dt = lbr_state.sample_time; if (!prev_measured_joint_position_init_) { prev_measured_joint_position_init_ = true; - std::memcpy(prev_measured_joint_position_.data(), lbr_state.getMeasuredJointPosition(), - sizeof(double) * KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + prev_measured_joint_position_ = lbr_state.measured_joint_position; return true; } for (std::size_t i = 0; i < lbr_command.joint_position[i]; ++i) { - if (std::abs(prev_measured_joint_position_[i] - lbr_state.getMeasuredJointPosition()[i]) / dt > + if (std::abs(prev_measured_joint_position_[i] - lbr_state.measured_joint_position[i]) / dt > parameters_.max_velocities[i]) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), ColorScheme::ERROR << "Velocity not in limits for joint '" @@ -61,15 +60,14 @@ bool CommandGuard::command_in_velocity_limits_(const_idl_command_t_ref lbr_comma return false; } } - std::memcpy(prev_measured_joint_position_.data(), lbr_state.getMeasuredJointPosition(), - sizeof(double) * KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + prev_measured_joint_position_ = lbr_state.measured_joint_position; return true; } bool CommandGuard::command_in_torque_limits_(const_idl_command_t_ref lbr_command, - const_fri_state_t_ref lbr_state) const { + const_idl_state_t_ref lbr_state) const { for (std::size_t i = 0; i < lbr_command.torque.size(); ++i) { - if (std::abs(lbr_command.torque[i] + lbr_state.getExternalTorque()[i]) > + if (std::abs(lbr_command.torque[i] + lbr_state.external_torque[i]) > parameters_.max_torques[i]) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), ColorScheme::ERROR << "Torque not in limits for joint '" @@ -82,14 +80,12 @@ bool CommandGuard::command_in_torque_limits_(const_idl_command_t_ref lbr_command } bool SafeStopCommandGuard::command_in_position_limits_(const_idl_command_t_ref lbr_command, - const_fri_state_t_ref lbr_state) const { + const_idl_state_t_ref lbr_state) const { for (std::size_t i = 0; i < lbr_command.joint_position.size(); ++i) { if (lbr_command.joint_position[i] < - parameters_.min_positions[i] + - parameters_.max_velocities[i] * lbr_state.getSampleTime() || + parameters_.min_positions[i] + parameters_.max_velocities[i] * lbr_state.sample_time || lbr_command.joint_position[i] > - parameters_.max_positions[i] - - parameters_.max_velocities[i] * lbr_state.getSampleTime()) { + parameters_.max_positions[i] - parameters_.max_velocities[i] * lbr_state.sample_time) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), ColorScheme::ERROR << "Position not in limits for joint '" << parameters_.joint_names[i].c_str() << "'" diff --git a/lbr_fri_ros2/src/interfaces/base_command.cpp b/lbr_fri_ros2/src/interfaces/base_command.cpp index 9eb02f03..c4cc5ea9 100644 --- a/lbr_fri_ros2/src/interfaces/base_command.cpp +++ b/lbr_fri_ros2/src/interfaces/base_command.cpp @@ -9,9 +9,8 @@ BaseCommandInterface::BaseCommandInterface(const PIDParameters &pid_parameters, command_guard_ = command_guard_factory(command_guard_parameters, command_guard_variant); }; -void BaseCommandInterface::init_command(const_fri_state_t_ref state) { - std::memcpy(command_target_.joint_position.data(), state.getMeasuredJointPosition(), - sizeof(double) * KUKA::FRI::LBRState::NUMBER_OF_JOINTS); +void BaseCommandInterface::init_command(const_idl_state_t_ref state) { + command_target_.joint_position = state.measured_joint_position; command_target_.torque.fill(0.); command_target_.wrench.fill(0.); command_ = command_target_; diff --git a/lbr_fri_ros2/src/interfaces/position_command.cpp b/lbr_fri_ros2/src/interfaces/position_command.cpp index 96d6e615..786fd479 100644 --- a/lbr_fri_ros2/src/interfaces/position_command.cpp +++ b/lbr_fri_ros2/src/interfaces/position_command.cpp @@ -7,20 +7,20 @@ PositionCommandInterface::PositionCommandInterface( : BaseCommandInterface(pid_parameters, command_guard_parameters, command_guard_variant) {} void PositionCommandInterface::buffered_command_to_fri(fri_command_t_ref command, - const_fri_state_t_ref state) { + const_idl_state_t_ref state) { #if FRICLIENT_VERSION_MAJOR == 1 - if (state.getClientCommandMode() != KUKA::FRI::EClientCommandMode::POSITION) { + if (state.client_command_mode != KUKA::FRI::EClientCommandMode::POSITION) { std::string err = "Expected robot in '" + EnumMaps::client_command_mode_map(KUKA::FRI::EClientCommandMode::POSITION) + "' command mode got '" + - EnumMaps::client_command_mode_map(state.getClientCommandMode()) + "'"; + EnumMaps::client_command_mode_map(state.client_command_mode) + "'"; RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); throw std::runtime_error(err); } #endif #if FRICLIENT_VERSION_MAJOR == 2 - if (state.getClientCommandMode() != KUKA::FRI::EClientCommandMode::JOINT_POSITION) { + if (state.client_command_mode != KUKA::FRI::EClientCommandMode::JOINT_POSITION) { std::string err = "Expected robot in " + EnumMaps::client_command_mode_map(KUKA::FRI::EClientCommandMode::JOINT_POSITION) + @@ -42,11 +42,11 @@ void PositionCommandInterface::buffered_command_to_fri(fri_command_t_ref command // PID if (!joint_position_pid_.is_initialized()) { - joint_position_pid_.initialize(pid_parameters_, state.getSampleTime()); + joint_position_pid_.initialize(pid_parameters_, state.sample_time); } joint_position_pid_.compute( - command_target_.joint_position, state.getMeasuredJointPosition(), - std::chrono::nanoseconds(static_cast(state.getSampleTime() * 1.e9)), + command_target_.joint_position, state.measured_joint_position, + std::chrono::nanoseconds(static_cast(state.sample_time * 1.e9)), command_.joint_position); // validate diff --git a/lbr_fri_ros2/src/interfaces/torque_command.cpp b/lbr_fri_ros2/src/interfaces/torque_command.cpp index b4503c79..5228a443 100644 --- a/lbr_fri_ros2/src/interfaces/torque_command.cpp +++ b/lbr_fri_ros2/src/interfaces/torque_command.cpp @@ -7,12 +7,12 @@ TorqueCommandInterface::TorqueCommandInterface( : BaseCommandInterface(pid_parameters, command_guard_parameters, command_guard_variant) {} void TorqueCommandInterface::buffered_command_to_fri(fri_command_t_ref command, - const_fri_state_t_ref state) { - if (state.getClientCommandMode() != KUKA::FRI::EClientCommandMode::TORQUE) { + const_idl_state_t_ref state) { + if (state.client_command_mode != KUKA::FRI::EClientCommandMode::TORQUE) { std::string err = "Expected robot in '" + EnumMaps::client_command_mode_map(KUKA::FRI::EClientCommandMode::TORQUE) + "' command mode got '" + - EnumMaps::client_command_mode_map(state.getClientCommandMode()) + "'"; + EnumMaps::client_command_mode_map(state.client_command_mode) + "'"; RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); throw std::runtime_error(err); @@ -32,19 +32,21 @@ void TorqueCommandInterface::buffered_command_to_fri(fri_command_t_ref command, // PID if (!joint_position_pid_.is_initialized()) { - joint_position_pid_.initialize(pid_parameters_, state.getSampleTime()); + joint_position_pid_.initialize(pid_parameters_, state.sample_time); } joint_position_pid_.compute( - command_target_.joint_position, state.getMeasuredJointPosition(), - std::chrono::nanoseconds(static_cast(state.getSampleTime() * 1.e9)), + command_target_.joint_position, state.measured_joint_position, + std::chrono::nanoseconds(static_cast(state.sample_time * 1.e9)), command_.joint_position); command_.torque = command_target_.torque; // validate if (!command_guard_->is_valid_command(command_, state)) { - throw std::runtime_error("Invalid command"); + std::string err = "Invalid command."; + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), + ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); + throw std::runtime_error(err); } - // write joint position and torque to output command.setJointPosition(command_.joint_position.data()); command.setTorque(command_.torque.data()); diff --git a/lbr_fri_ros2/src/interfaces/wrench_command.cpp b/lbr_fri_ros2/src/interfaces/wrench_command.cpp index 49fb173f..ee34c25b 100644 --- a/lbr_fri_ros2/src/interfaces/wrench_command.cpp +++ b/lbr_fri_ros2/src/interfaces/wrench_command.cpp @@ -7,12 +7,12 @@ WrenchCommandInterface::WrenchCommandInterface( : BaseCommandInterface(pid_parameters, command_guard_parameters, command_guard_variant) {} void WrenchCommandInterface::buffered_command_to_fri(fri_command_t_ref command, - const_fri_state_t_ref state) { - if (state.getClientCommandMode() != KUKA::FRI::EClientCommandMode::WRENCH) { + const_idl_state_t_ref state) { + if (state.client_command_mode != KUKA::FRI::EClientCommandMode::WRENCH) { std::string err = "Expected robot in '" + EnumMaps::client_command_mode_map(KUKA::FRI::EClientCommandMode::WRENCH) + "' command mode got '" + - EnumMaps::client_command_mode_map(state.getClientCommandMode()) + "'"; + EnumMaps::client_command_mode_map(state.client_command_mode) + "'"; RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME()), err.c_str()); throw std::runtime_error(err); } @@ -30,11 +30,11 @@ void WrenchCommandInterface::buffered_command_to_fri(fri_command_t_ref command, // PID if (!joint_position_pid_.is_initialized()) { - joint_position_pid_.initialize(pid_parameters_, state.getSampleTime()); + joint_position_pid_.initialize(pid_parameters_, state.sample_time); } joint_position_pid_.compute( - command_target_.joint_position, state.getMeasuredJointPosition(), - std::chrono::nanoseconds(static_cast(state.getSampleTime() * 1.e9)), + command_target_.joint_position, state.measured_joint_position, + std::chrono::nanoseconds(static_cast(state.sample_time * 1.e9)), command_.joint_position); command_.wrench = command_target_.wrench; diff --git a/lbr_fri_ros2/test/test_command_interfaces.cpp b/lbr_fri_ros2/test/test_command_interfaces.cpp index 21883b5c..9f956ca0 100644 --- a/lbr_fri_ros2/test/test_command_interfaces.cpp +++ b/lbr_fri_ros2/test/test_command_interfaces.cpp @@ -11,6 +11,7 @@ #include "lbr_fri_idl/msg/lbr_command.hpp" #include "lbr_fri_ros2/interfaces/base_command.hpp" #include "lbr_fri_ros2/interfaces/position_command.hpp" +#include "lbr_fri_ros2/interfaces/state.hpp" #include "lbr_fri_ros2/interfaces/torque_command.hpp" #include "lbr_fri_ros2/interfaces/wrench_command.hpp" @@ -19,6 +20,9 @@ class TestCommandInterfaces : public ::testing::Test { TestCommandInterfaces() : random_engine_(std::random_device{}()) { pid_params_ = lbr_fri_ros2::PIDParameters(); cmd_guard_params_ = lbr_fri_ros2::CommandGuardParameters(); + state_interface_params_ = lbr_fri_ros2::StateInterfaceParameters(); + + state_interface_ = std::make_shared(state_interface_params_); // pseudo application udp_connection_ = std::make_unique(); @@ -87,7 +91,9 @@ class TestCommandInterfaces : public ::testing::Test { command_interface_->buffer_command_target(idl_command_target); // initialize commands to state - command_interface_->init_command(lbr_client_->robotState()); + state_interface_->set_state(lbr_client_->robotState()); + command_interface_->init_command( + state_interface_->get_state()); // get state from state interface // expect command target is state now (zero initialized here) for (std::size_t i = 0; i < idl_command_target.joint_position.size(); ++i) { @@ -113,7 +119,7 @@ class TestCommandInterfaces : public ::testing::Test { idl_command_target = random_idl_command(); command_interface_->buffer_command_target(idl_command_target); command_interface_->buffered_command_to_fri(lbr_client_->robotCommand(), - lbr_client_->robotState()); + state_interface_->get_state()); } catch (const std::exception &) { invalid_mode_triggered = true; } @@ -125,7 +131,9 @@ class TestCommandInterfaces : public ::testing::Test { lbr_fri_ros2::PIDParameters pid_params_; lbr_fri_ros2::CommandGuardParameters cmd_guard_params_; + lbr_fri_ros2::StateInterfaceParameters state_interface_params_; + std::shared_ptr state_interface_; std::shared_ptr command_interface_; // pseudo application From 47e2e3586c0f4a26e283431bb7a8531f28b2b3dd Mon Sep 17 00:00:00 2001 From: mhubii Date: Sun, 12 May 2024 10:58:53 +0100 Subject: [PATCH 68/76] updated admittance params --- .../lbr_demos_advanced_cpp/config/admittance_control.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/lbr_demos/lbr_demos_advanced_cpp/config/admittance_control.yaml b/lbr_demos/lbr_demos_advanced_cpp/config/admittance_control.yaml index 2cf31107..7df54a6d 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/config/admittance_control.yaml +++ b/lbr_demos/lbr_demos_advanced_cpp/config/admittance_control.yaml @@ -1,8 +1,8 @@ -admittance_control: +/**/admittance_control: ros__parameters: base_link: "link_0" end_effector_link: "link_ee" f_ext_th: [2., 2., 2., 0.5, 0.5, 0.5] - dq_gains: [2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0] + dq_gains: [4.0, 4.0, 4.0, 4.0, 4.0, 4.0, 4.0] dx_gains: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] exp_smooth: 0.95 From 7de7e77305b8b02de11ba1b31cccfd6eb5efb0e4 Mon Sep 17 00:00:00 2001 From: mhubii Date: Sun, 12 May 2024 15:46:35 +0100 Subject: [PATCH 69/76] initial doc re-structure --- docker/doc/docker.rst | 2 +- lbr_bringup/doc/lbr_bringup.rst | 4 +- lbr_demos/doc/lbr_demos.rst | 28 ++++---- .../doc/lbr_demos_advanced_cpp.rst | 46 ++++++------ .../doc/lbr_demos_advanced_py.rst | 40 +++++------ lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst | 71 ++++++++++--------- lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst | 45 +++++------- lbr_fri_ros2/doc/lbr_fri_ros2.rst | 6 +- .../{robot_setup.rst => hardware_setup.rst} | 5 -- lbr_fri_ros2_stack/doc/lbr_fri_ros2_stack.rst | 47 +----------- lbr_moveit_config/doc/lbr_moveit_config.rst | 6 +- lbr_ros2_control/doc/lbr_ros2_control.rst | 2 + 12 files changed, 126 insertions(+), 176 deletions(-) rename lbr_fri_ros2_stack/doc/{robot_setup.rst => hardware_setup.rst} (98%) create mode 100644 lbr_ros2_control/doc/lbr_ros2_control.rst diff --git a/docker/doc/docker.rst b/docker/doc/docker.rst index 01ba9e61..1ef56d04 100644 --- a/docker/doc/docker.rst +++ b/docker/doc/docker.rst @@ -1,4 +1,4 @@ -Docker +docker ====== To run the ``lbr_fri_ros2_stack`` in a Docker container, follow the instructions below. diff --git a/lbr_bringup/doc/lbr_bringup.rst b/lbr_bringup/doc/lbr_bringup.rst index cf70a42d..7cf4edea 100644 --- a/lbr_bringup/doc/lbr_bringup.rst +++ b/lbr_bringup/doc/lbr_bringup.rst @@ -1,4 +1,4 @@ -LBR Bringup +lbr_bringup =========== The ``lbr_bringup`` works for the simulation and the real robot. Run: @@ -29,7 +29,7 @@ Select Users may also refer to :ref:`Software Architecture` for a better understanding of the underlying ``lbr_fri_ros2`` package. .. note:: - For the real robot, make sure you have followed :ref:`Robot Setup` first. + For the real robot, make sure you have followed :doc:`Hardware Setup <../../lbr_fri_ros2_stack/doc/hardware_setup>` first. .. warning:: On the real robot, do always execute in ``T1`` mode first. diff --git a/lbr_demos/doc/lbr_demos.rst b/lbr_demos/doc/lbr_demos.rst index feec59f2..b1845019 100644 --- a/lbr_demos/doc/lbr_demos.rst +++ b/lbr_demos/doc/lbr_demos.rst @@ -1,29 +1,33 @@ -LBR Demos +lbr_demos ========= Contolling the LBR through ROS 2 control. .. note:: - Some demos require a real robot. Make sure you followed :ref:`Robot Setup` first. + Some demos require a real robot. Make sure you followed :doc:`Hardware Setup <../../lbr_fri_ros2_stack/doc/hardware_setup>` first. .. warning:: On the real robot, do always execute in ``T1`` mode first. -Demos ------ +Simple +------ .. toctree:: :titlesonly: - LBR Demos C++ <../lbr_demos_cpp/doc/lbr_demos_cpp.rst> - LBR Demos Python <../lbr_demos_py/doc/lbr_demos_py.rst> + lbr_demos_cpp <../lbr_demos_cpp/doc/lbr_demos_cpp.rst> + lbr_demos_py <../lbr_demos_py/doc/lbr_demos_py.rst> -Advanced Demos --------------- +Advanced +-------- .. toctree:: :titlesonly: - LBR Demos Advanced C++ <../lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst> - LBR Demos Advanced Python <../lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst> + lbr_demos_advanced_cpp <../lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst> + lbr_demos_advanced_py <../lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst> -MoveIt 2 Demos +MoveIt 2 -------------- -TODO +TODO moveit simple action client demos + +Integration +----------- +TODO system integration demos diff --git a/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst b/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst index 4f366a38..e870f26f 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst +++ b/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst @@ -1,4 +1,4 @@ -LBR Demos Advanced C++ +lbr_demos_advanced_cpp ====================== Collection of advanced usage examples for the ``lbr_fri_ros2`` package through C++. @@ -16,20 +16,20 @@ Admittance Controller #. Launch the robot driver: -.. code-block:: bash + .. code-block:: bash - ros2 launch lbr_bringup bringup.launch.py \ - sim:=false \ - ctrl:=lbr_position_command_controller \ - model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + ros2 launch lbr_bringup bringup.launch.py \ + sim:=false \ + ctrl:=lbr_position_command_controller \ + model:=iiwa7 # [iiwa7, iiwa14, med7, med14] -#. Launch the `admittance_control `_: +#. Launch the `admittance_control `_: -.. code-block:: bash - - ros2 run lbr_demos_advanced_cpp admittance_control --ros-args \ - -r __ns:=/lbr \ - --params-file `ros2 pkg prefix lbr_demos_advanced_cpp`/share/lbr_demos_advanced_cpp/config/admittance_control.yaml + .. code-block:: bash + + ros2 run lbr_demos_a dvanced_cpp admittance_control --ros-args \ + -r __ns:=/lbr \ + --params-file `ros2 pkg prefix lbr_demos_advanced_cpp`/share/lbr_demos_advanced_cpp/config/admittance_control.yaml #. Now gently move the robot at the end-effector. @@ -44,23 +44,23 @@ kinematics to move the robot's end-effector along the z-axis in Cartesian space. #. Launch the robot driver: -.. code-block:: bash + .. code-block:: bash - ros2 launch lbr_bringup bringup.launch.py \ - sim:=false \ - ctrl:=lbr_position_command_controller \ - model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + ros2 launch lbr_bringup bringup.launch.py \ + sim:=false \ + ctrl:=lbr_position_command_controller \ + model:=iiwa7 # [iiwa7, iiwa14, med7, med14] #. Launch the pose control -.. code-block:: bash + .. code-block:: bash - ros2 run lbr_demos_advanced_cpp pose_control --ros-args \ - -r __ns:=/lbr + ros2 run lbr_demos_advanced_cpp pose_control --ros-args \ + -r __ns:=/lbr #. Launch the path planning -.. code-block:: bash + .. code-block:: bash - ros2 run lbr_demos_advanced_cpp pose_planning --ros-args \ - -r __ns:=/lbr + ros2 run lbr_demos_advanced_cpp pose_planning --ros-args \ + -r __ns:=/lbr diff --git a/lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst b/lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst index 5dfd7c09..245814e6 100644 --- a/lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst +++ b/lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst @@ -1,5 +1,5 @@ -LBR Demos Advanced Python -========================= +lbr_demos_advanced_py +===================== Collection of advanced usage examples for the ``lbr_ros2_control`` package through Python. .. warning:: @@ -18,20 +18,20 @@ This demo implements a simple admittance controller. #. Launch the robot driver: -.. code-block:: bash + .. code-block:: bash - ros2 launch lbr_bringup bringup.launch.py \ - sim:=false \ - ctrl:=lbr_position_command_controller \ - model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + ros2 launch lbr_bringup bringup.launch.py \ + sim:=false \ + ctrl:=lbr_position_command_controller \ + model:=iiwa7 # [iiwa7, iiwa14, med7, med14] #. Run the `admittance_control `_ with remapping and parameter file: -.. code-block:: bash + .. code-block:: bash - ros2 run lbr_demos_advanced_py admittance_control --ros-args \ - -r __ns:=/lbr \ - --params-file `ros2 pkg prefix lbr_demos_advanced_py`/share/lbr_demos_advanced_py/config/admittance_control.yaml + ros2 run lbr_demos_advanced_py admittance_control --ros-args \ + -r __ns:=/lbr \ + --params-file `ros2 pkg prefix lbr_demos_advanced_py`/share/lbr_demos_advanced_py/config/admittance_control.yaml #. Now gently move the robot at the end-effector. @@ -45,19 +45,19 @@ This demo implements an admittance controller with a remote center of motion (RC #. Launch the robot driver: -.. code-block:: bash + .. code-block:: bash - ros2 launch lbr_bringup bringup.launch.py \ - sim:=false \ - ctrl:=lbr_position_command_controller \ - model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + ros2 launch lbr_bringup bringup.launch.py \ + sim:=false \ + ctrl:=lbr_position_command_controller \ + model:=iiwa7 # [iiwa7, iiwa14, med7, med14] #. Run the `admittance_rcm_control `_ with remapping and parameter file: -.. code-block:: bash + .. code-block:: bash - ros2 run lbr_demos_advanced_py admittance_rcm_control --ros-args \ - -r __ns:=/lbr \ - --params-file `ros2 pkg prefix lbr_demos_advanced_py`/share/lbr_demos_advanced_py/config/admittance_rcm_control.yaml + ros2 run lbr_demos_advanced_py admittance_rcm_control --ros-args \ + -r __ns:=/lbr \ + --params-file `ros2 pkg prefix lbr_demos_advanced_py`/share/lbr_demos_advanced_py/config/admittance_rcm_control.yaml #. Now gently move the robot at the end-effector. diff --git a/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst b/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst index 497f640f..e5aca94a 100644 --- a/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst +++ b/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst @@ -1,4 +1,4 @@ -LBR Demos C++ +lbr_demos_cpp ============= add table of contents TODO with sim / real matrix @@ -20,22 +20,23 @@ Joint Sine Overlay #. Run the robot driver: -.. code-block:: bash + .. code-block:: bash - ros2 launch lbr_bringup bringup.launch.py \ - ctrl:=lbr_position_command_controller \ - sim:=false \ - model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + ros2 launch lbr_bringup bringup.launch.py \ + ctrl:=lbr_position_command_controller \ + sim:=false \ + model:=iiwa7 # [iiwa7, iiwa14, med7, med14] -The robot will move to the initial position through the Java application. + + The robot will move to the initial position through the Java application. #. Run the `joint_sine_overlay `_ node: -.. code-block:: bash + .. code-block:: bash - ros2 run lbr_demos_cpp joint_sine_overlay --ros-args -r __ns:=/lbr + ros2 run lbr_demos_cpp joint_sine_overlay --ros-args -r __ns:=/lbr -This node overlays a sinusoidal motion on joint ``A4``. + This node overlays a sinusoidal motion on joint ``A4``. Joint Trajectory Controller --------------------------- @@ -43,20 +44,20 @@ Simulation ~~~~~~~~~~ #. Launch the simulated robot: -.. code-block:: bash + .. code-block:: bash - ros2 launch lbr_bringup bringup.launch.py sim:=true model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + ros2 launch lbr_bringup bringup.launch.py sim:=true model:=iiwa7 # [iiwa7, iiwa14, med7, med14] #. Run the `joint_trajectory_client `_: -.. code-block:: bash + .. code-block:: bash - ros2 run lbr_demos_cpp joint_trajectory_client --ros-args -r __ns:=/lbr + ros2 run lbr_demos_cpp joint_trajectory_client --ros-args -r __ns:=/lbr -The robot will twist, then move to the zero configuration. + The robot will twist, then move to the zero configuration. -Real Robot -~~~~~~~~~~ +Hardware +~~~~~~~~ #. .. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD`` .. thumbnail:: ../../doc/img/applications_lbr_server.png @@ -85,22 +86,22 @@ Torque Sine Overlay #. Run the robot driver: -.. code-block:: bash + .. code-block:: bash - ros2 launch lbr_bringup bringup.launch.py \ - ctrl:=lbr_torque_command_controller \ - sim:=false \ - model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + ros2 launch lbr_bringup bringup.launch.py \ + ctrl:=lbr_torque_command_controller \ + sim:=false \ + model:=iiwa7 # [iiwa7, iiwa14, med7, med14] -The robot will move to the initial position through the Java application. + The robot will move to the initial position through the Java application. #. Run the `torque_sine_overlay `_ node: -.. code-block:: bash + .. code-block:: bash - ros2 run lbr_demos_cpp torque_sine_overlay --ros-args -r __ns:=/lbr + ros2 run lbr_demos_cpp torque_sine_overlay --ros-args -r __ns:=/lbr -This node overlays a sinusoidal torque on joint ``A4``. + This node overlays a sinusoidal torque on joint ``A4``. Wrench Sine Overlay ------------------- @@ -117,19 +118,19 @@ Wrench Sine Overlay #. Run the robot driver: -.. code-block:: bash + .. code-block:: bash - ros2 launch lbr_bringup bringup.launch.py \ - ctrl:=lbr_wrench_command_controller \ - sim:=false \ - model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + ros2 launch lbr_bringup bringup.launch.py \ + ctrl:=lbr_wrench_command_controller \ + sim:=false \ + model:=iiwa7 # [iiwa7, iiwa14, med7, med14] -The robot will move to the initial position through the Java application. + The robot will move to the initial position through the Java application. #. Run the `wrench_sine_overlay `_ node: -.. code-block:: bash + .. code-block:: bash - ros2 run lbr_demos_cpp wrench_sine_overlay --ros-args -r __ns:=/lbr + ros2 run lbr_demos_cpp wrench_sine_overlay --ros-args -r __ns:=/lbr -This node overlays a sinusoidal force on the x- and y-axis. + This node overlays a sinusoidal force on the x- and y-axis. diff --git a/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst b/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst index f5a91c71..c7315b69 100644 --- a/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst +++ b/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst @@ -1,5 +1,5 @@ -LBR Demos Python -================ +lbr_demos_py +============ Collection of basic usage examples for the ``lbr_fri_ros2`` package through Python. .. note:: @@ -16,21 +16,11 @@ Joint Sine Overlay #. Launch the `joint_sine_overlay.launch.py `_ launch file: -.. code-block:: bash + .. code-block:: bash - ros2 launch lbr_demos_py joint_sine_overlay.launch.py model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + ros2 launch lbr_demos_py joint_sine_overlay.launch.py model:=iiwa7 # [iiwa7, iiwa14, med7, med14] -The robot will move to the initial position via position control, then execute a rotation on joint ``A1``. A sinusoidal motion is overlayed on joint ``A4`` via `joint_sine_overlay_node `_. - -LBR Demos ROS 2 Control Python -============================== -Collection of basic usage examples for ``lbr_ros2_control`` through Python. - -.. note:: - These examples can be run in simulation **and** on the real robot. - -.. warning:: - Do always execute in simulation first, then in ``T1`` mode on the real robot. + The robot will move to the initial position via position control, then execute a rotation on joint ``A1``. A sinusoidal motion is overlayed on joint ``A4`` via `joint_sine_overlay_node `_. Joint Trajectory Controller --------------------------- @@ -38,20 +28,20 @@ Simulation ~~~~~~~~~~ #. Launch the ``LBRBringup``: -.. code-block:: bash + .. code-block:: bash - ros2 launch lbr_bringup bringup.launch.py sim:=true model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + ros2 launch lbr_bringup bringup.launch.py sim:=true model:=iiwa7 # [iiwa7, iiwa14, med7, med14] #. Run the `joint_trajectory_executioner_node `_: -.. code-block:: bash + .. code-block:: bash - ros2 run lbr_demos_ros2_control_python joint_trajectory_executioner_node + ros2 run lbr_demos_ros2_control_python joint_trajectory_executioner_node The robot will twist, then move to the zero configuration. -Real Robot -~~~~~~~~~~ +Hardware +~~~~~~~~ #. .. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD`` .. thumbnail:: ../../doc/img/applications_lbr_server.png @@ -62,6 +52,7 @@ Real Robot - ``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 ``sim:=false``. Torque Sine Overlay @@ -72,11 +63,11 @@ Torque Sine Overlay #. Launch the `torque_sine_overlay.launch.py `_ launch file: -.. code-block:: bash + .. code-block:: bash - ros2 launch lbr_demos_py torque_sine_overlay.launch.py model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + ros2 launch lbr_demos_py torque_sine_overlay.launch.py model:=iiwa7 # [iiwa7, iiwa14, med7, med14] -The robot will move to the initial position via joint impedance control. A sinusoidal torque is overlayed on joint ``A4`` via `torque_sine_overlay_node `_. + The robot will move to the initial position via joint impedance control. A sinusoidal torque is overlayed on joint ``A4`` via `torque_sine_overlay_node `_. Wrench Sine Overlay ------------------- @@ -86,8 +77,8 @@ Wrench Sine Overlay #. Launch the `wrench_sine_overlay.launch.py `_ launch file: -.. code-block:: bash + .. code-block:: bash - ros2 launch lbr_demos_py wrench_sine_overlay.launch.py model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + ros2 launch lbr_demos_py wrench_sine_overlay.launch.py model:=iiwa7 # [iiwa7, iiwa14, med7, med14] -The robot will move to the initial position via cartesian impedance control. A sinusoidal force is overlayed on the x- and y-axis via `wrench_sine_overlay_node `_. + The robot will move to the initial position via cartesian impedance control. A sinusoidal force is overlayed on the x- and y-axis via `wrench_sine_overlay_node `_. diff --git a/lbr_fri_ros2/doc/lbr_fri_ros2.rst b/lbr_fri_ros2/doc/lbr_fri_ros2.rst index c5ab8b90..51b2adbe 100644 --- a/lbr_fri_ros2/doc/lbr_fri_ros2.rst +++ b/lbr_fri_ros2/doc/lbr_fri_ros2.rst @@ -1,5 +1,5 @@ -LBR FRI ROS 2 -============= +lbr_fri_ros2 +============ TODO: massive re-write of this section, since architecture has changed. The ``lbr_fri_ros2`` package provides a ROS 2 interface for the KUKA LBRs. It is designed to run stand-alone **and** within ``ros2_control``. @@ -10,7 +10,7 @@ Quick Start Do always execute in ``T1`` mode first. .. note:: - Make sure you followed the install instructions in :ref:`Robot Setup`. + Make sure you followed the install instructions in :doc:`Hardware Setup <../../lbr_fri_ros2_stack/doc/hardware_setup>`. #. .. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD`` diff --git a/lbr_fri_ros2_stack/doc/robot_setup.rst b/lbr_fri_ros2_stack/doc/hardware_setup.rst similarity index 98% rename from lbr_fri_ros2_stack/doc/robot_setup.rst rename to lbr_fri_ros2_stack/doc/hardware_setup.rst index bd11dfcf..0fdd86ac 100644 --- a/lbr_fri_ros2_stack/doc/robot_setup.rst +++ b/lbr_fri_ros2_stack/doc/hardware_setup.rst @@ -1,8 +1,3 @@ -Robot Setup -=========== -.. note:: - Please also refer to :ref:`Additional Resources`. - Verify Connection to the Robot ------------------------------ #. Turn on the robot (enable green power switch on the robot controller). diff --git a/lbr_fri_ros2_stack/doc/lbr_fri_ros2_stack.rst b/lbr_fri_ros2_stack/doc/lbr_fri_ros2_stack.rst index d9c5b572..87c96dee 100644 --- a/lbr_fri_ros2_stack/doc/lbr_fri_ros2_stack.rst +++ b/lbr_fri_ros2_stack/doc/lbr_fri_ros2_stack.rst @@ -1,45 +1,2 @@ -LBR FRI ROS 2 Stack -=================== -Collection of packages for controlling the KUKA LBR iiwa / med through ROS 2. - -- ``lbr_bringup``: Launch package. -- ``lbr_demos``: Demo applications. -- ``lbr_description``: Description files. -- ``lbr_fri_idl``: ``IDL``-equivalent of KUKA's ``nanopb`` message definitions. -- ``lbr_fri_ros2``: Exposes ``fri`` to ROS 2 topics / services. -- ``lbr_ros2_control``: ``ros2_control`` integration for the LBR. -- ``lbr_moveit_config```: ``MoveIt 2`` configurations for thr LBR. -- ``fri``: Integration of KUKA's Fast Robot Interface (FRI) into ROS 2 ``ament_cmake`` build system. - -Installation ------------- -#. Install `colcon `_, `rosdep `_ and `vcstool `_. -#. Install the ``lbr_fri_ros2_stack``: - -.. code-block:: bash - - mkdir -p lbr-stack/src && cd lbr-stack - wget https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/repos.yaml -P src - vcs import src < src/repos.yaml - rosdep install --from-paths src --ignore-src -r -y - colcon build --symlink-install - -.. note:: - For the real robot, additional steps are required: - - .. toctree:: - - robot_setup - -Usage ------ -#. Launch the simulation / real robot: - -.. code-block:: bash - - source install/setup.bash - ros2 launch lbr_bringup bringup.launch.py model:=med7 sim:=true # model:=[iiwa7/iiwa14/med7/med14] - -For the real robot, set ``sim:=false`` and run the ``LBRServer`` on the ``KUKA smartPAD``, see :ref:`Install Applications to the Robot`. - -#. Checkout bringup and demos, see :ref:`LBR Bringup` and :ref:`LBR Demos`. +.. include:: ../../README.md + :parser: myst_parser.sphinx_ diff --git a/lbr_moveit_config/doc/lbr_moveit_config.rst b/lbr_moveit_config/doc/lbr_moveit_config.rst index 71ba4118..61b5a3f6 100644 --- a/lbr_moveit_config/doc/lbr_moveit_config.rst +++ b/lbr_moveit_config/doc/lbr_moveit_config.rst @@ -1,4 +1,4 @@ -LBR MoveIt Config +lbr_moveit_config ================= Documentation for generating (:ref:`Generate MoveIt Configuration`) and updating (:ref:`Update MoveIt Configuration`) the ``MoveIt 2`` configurations for the LBRs. For full documentation see `MoveIt Documentation `_. @@ -98,8 +98,8 @@ Update MoveIt Configuration #. Run the setup assistant for the existing configuration. -.. code-block:: bash + .. code-block:: bash - ros2 launch iiwa7_moveit_config setup_assistant.launch.py # [iiwa7, iiwa14, med7, med14] + ros2 launch iiwa7_moveit_config setup_assistant.launch.py # [iiwa7, iiwa14, med7, med14] #. Update and save the configurations, similar to :ref:`Generate MoveIt Configuration`. diff --git a/lbr_ros2_control/doc/lbr_ros2_control.rst b/lbr_ros2_control/doc/lbr_ros2_control.rst new file mode 100644 index 00000000..e6b88078 --- /dev/null +++ b/lbr_ros2_control/doc/lbr_ros2_control.rst @@ -0,0 +1,2 @@ +lbr_ros2_control +================ From 2ff07169eb9b07495a8136c1c95be1328a8f537f Mon Sep 17 00:00:00 2001 From: mhubii Date: Sun, 12 May 2024 16:43:50 +0100 Subject: [PATCH 70/76] clean up references --- README.md | 8 ++++---- lbr_fri_ros2/doc/lbr_fri_ros2.rst | 11 +++++++++-- lbr_fri_ros2/test/README.md | 12 ------------ 3 files changed, 13 insertions(+), 18 deletions(-) delete mode 100644 lbr_fri_ros2/test/README.md diff --git a/README.md b/README.md index 026b8c54..b3ed855b 100644 --- a/README.md +++ b/README.md @@ -16,10 +16,10 @@ ROS 2 packages for the KUKA LBR, including communication to the real robot via t LBR Med14 R820 - LBR IIWA7 R800 - LBR IIWA14 R820 - LBR Med7 R800 - LBR Med14 R820 + LBR IIWA 7 R800 + LBR IIWA 14 R820 + LBR Med 7 R800 + LBR Med 14 R820 diff --git a/lbr_fri_ros2/doc/lbr_fri_ros2.rst b/lbr_fri_ros2/doc/lbr_fri_ros2.rst index 51b2adbe..73720fb1 100644 --- a/lbr_fri_ros2/doc/lbr_fri_ros2.rst +++ b/lbr_fri_ros2/doc/lbr_fri_ros2.rst @@ -1,8 +1,15 @@ lbr_fri_ros2 ============ -TODO: massive re-write of this section, since architecture has changed. +.. note:: + + Users will interface the hardware through :ref:`lbr_ros2_control`. Documentation for ``lbr_fri_ros2`` is intended for developers. + +.. toctree:: + :caption: API Reference + + ../../../docs/doxygen/lbr_fri_ros2/html/annotated_classes -The ``lbr_fri_ros2`` package provides a ROS 2 interface for the KUKA LBRs. It is designed to run stand-alone **and** within ``ros2_control``. +The ``lbr_fri_ros2`` package adds :lbr_fri_ros2:`AsyncClient `. Quick Start ----------- diff --git a/lbr_fri_ros2/test/README.md b/lbr_fri_ros2/test/README.md deleted file mode 100644 index 2a077acc..00000000 --- a/lbr_fri_ros2/test/README.md +++ /dev/null @@ -1,12 +0,0 @@ -# To test: - -- procedural: - - monitoring wait - - monitoring ready - - commanding wait - - commanding active - -- invalid client command mode -- invalid control mode -- invalid velocity -- invalid position From 3f1136dbe2494f01a11f5d32d17b28622e23cf51 Mon Sep 17 00:00:00 2001 From: mhubii Date: Sun, 12 May 2024 16:44:42 +0100 Subject: [PATCH 71/76] clean titles --- README.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index b3ed855b..388750f7 100644 --- a/README.md +++ b/README.md @@ -10,10 +10,10 @@ ROS 2 packages for the KUKA LBR, including communication to the real robot via t - - - - + + + + From 115035d91de5a00f3bc53a5a8ea8ca1518f0defb Mon Sep 17 00:00:00 2001 From: mhubii Date: Sun, 12 May 2024 17:03:48 +0100 Subject: [PATCH 72/76] trigger build test commit --- lbr_fri_ros2/doc/lbr_fri_ros2.rst | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/lbr_fri_ros2/doc/lbr_fri_ros2.rst b/lbr_fri_ros2/doc/lbr_fri_ros2.rst index 73720fb1..f18cafa4 100644 --- a/lbr_fri_ros2/doc/lbr_fri_ros2.rst +++ b/lbr_fri_ros2/doc/lbr_fri_ros2.rst @@ -52,9 +52,9 @@ Software Architecture --------------------- An overview of the software architecture is shown :ref:`below `: -.. _target to software architecture figure: -.. thumbnail:: img/lbr_fri_ros2.drawio.svg - :alt: lbr_fri_ros2 +.. .. _target to software architecture figure: +.. .. thumbnail:: img/lbr_fri_ros2.drawio.svg +.. :alt: lbr_fri_ros2 Design Principles ~~~~~~~~~~~~~~~~~ From 87c2f780ac795f598669fc8daebe3da2483454ad Mon Sep 17 00:00:00 2001 From: mhubii Date: Sun, 12 May 2024 17:10:47 +0100 Subject: [PATCH 73/76] webhhok test commit --- lbr_fri_ros2/doc/lbr_fri_ros2.rst | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/lbr_fri_ros2/doc/lbr_fri_ros2.rst b/lbr_fri_ros2/doc/lbr_fri_ros2.rst index f18cafa4..73720fb1 100644 --- a/lbr_fri_ros2/doc/lbr_fri_ros2.rst +++ b/lbr_fri_ros2/doc/lbr_fri_ros2.rst @@ -52,9 +52,9 @@ Software Architecture --------------------- An overview of the software architecture is shown :ref:`below `: -.. .. _target to software architecture figure: -.. .. thumbnail:: img/lbr_fri_ros2.drawio.svg -.. :alt: lbr_fri_ros2 +.. _target to software architecture figure: +.. thumbnail:: img/lbr_fri_ros2.drawio.svg + :alt: lbr_fri_ros2 Design Principles ~~~~~~~~~~~~~~~~~ From 2ba408da1ecc0119d38de1bbdc8c3d05a8a45c55 Mon Sep 17 00:00:00 2001 From: mhubii Date: Sun, 12 May 2024 17:43:19 +0100 Subject: [PATCH 74/76] removed redundant quick start --- lbr_fri_ros2/doc/lbr_fri_ros2.rst | 37 ------------------------------- 1 file changed, 37 deletions(-) diff --git a/lbr_fri_ros2/doc/lbr_fri_ros2.rst b/lbr_fri_ros2/doc/lbr_fri_ros2.rst index 73720fb1..25de79ca 100644 --- a/lbr_fri_ros2/doc/lbr_fri_ros2.rst +++ b/lbr_fri_ros2/doc/lbr_fri_ros2.rst @@ -11,43 +11,6 @@ lbr_fri_ros2 The ``lbr_fri_ros2`` package adds :lbr_fri_ros2:`AsyncClient `. -Quick Start ------------ -.. warning:: - Do always execute in ``T1`` mode first. - -.. note:: - Make sure you followed the install instructions in :doc:`Hardware Setup <../../lbr_fri_ros2_stack/doc/hardware_setup>`. - -#. .. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD`` - - .. thumbnail:: ../../lbr_demos/doc/img/applications_lbr_server.png - -#. Run the :lbr_fri_ros2:`App ` node via `app.launch.py `_: - -.. code-block:: bash - - ros2 launch lbr_fri_ros2 app.launch.py model:=iiwa7 # [iiwa7, iiwa14, med7, med14] - -This launch file does 2 things: - - - Loads the ``robot_description`` (to read joint limits) - - Runs the :lbr_fri_ros2:`AppComponent `, which has an instance of :lbr_fri_ros2:`App ` to - - - Create services to connect to / disconnect from the robot - - Publish robot states to ``/lbr/state`` via :lbr_fri_ros2:`Client ` - - Read robot commands from ``/lbr/command`` via :lbr_fri_ros2:`Client ` - -The topic names change with the robot's name. When running - -.. code-block:: bash - - ros2 launch lbr_fri_ros2 app.launch.py robot_name:=lbr_1 model:=iiwa7 # [iiwa7, iiwa14, med7, med14] - -Commands / states will be published to ``/lbr_1/state`` / ``/lbr_1/command``. - -See :ref:`LBR Demos` for more examples. - Software Architecture --------------------- An overview of the software architecture is shown :ref:`below `: From 8100e250f39ac9c3287f4ed51c34bcf3aa8d9def Mon Sep 17 00:00:00 2001 From: mhubii Date: Mon, 13 May 2024 15:27:02 +0100 Subject: [PATCH 75/76] updated doc --- lbr_fri_ros2/doc/img/lbr_fri_ros2.drawio.svg | 4 -- lbr_fri_ros2/doc/img/lbr_fri_ros2_v1.5.0.svg | 4 ++ lbr_fri_ros2/doc/lbr_fri_ros2.rst | 46 ++++---------------- 3 files changed, 12 insertions(+), 42 deletions(-) delete mode 100644 lbr_fri_ros2/doc/img/lbr_fri_ros2.drawio.svg create mode 100644 lbr_fri_ros2/doc/img/lbr_fri_ros2_v1.5.0.svg diff --git a/lbr_fri_ros2/doc/img/lbr_fri_ros2.drawio.svg b/lbr_fri_ros2/doc/img/lbr_fri_ros2.drawio.svg deleted file mode 100644 index 6c207b67..00000000 --- a/lbr_fri_ros2/doc/img/lbr_fri_ros2.drawio.svg +++ /dev/null @@ -1,4 +0,0 @@ - - - -
lbr_fri_msgs
lbr_fri_msgs
lbr_fri_ros2
lbr_fri_ros2
fri
fri
KUKA::FRI::LBRCommand
KUKA::FRI::LBRCommand
KUKA::FRI::LBRState
KUKA::FRI::LBRState
KUKA::FRI::UdpConnection
KUKA::FRI::UdpConnection
KUKA::FRI::LBRClient
KUKA::FRI::LBRClient
+ monitor(): void
+ waitForCommand(): void
+ command(): void
+ onStateChange(): void
+ monitor(): void...
send encoded
KUKA::FRI::LBRCommand
send encoded...
send encoded
KUKA::FRI::LBRState
send encoded...
KUKA::FRI::ClientApplication
KUKA::FRI::ClientApplication
+ connect(port: int, remoteHost: const char*): bool
+ disconnect(): void
+ step(): bool
+ connect(port: int, remoteHost: const char*):...
parent
parent
child
child
lbr_fri_ros2::CommandGuard
lbr_fri_ros2::CommandGuard
+ CommandGuard(robot_description: const std::string &)
+ is_valid_command(
lbr_command: const lbr_fri_msgs::msg::LBRCommand &,
lbr_state: const KUKA::FRI::LBRState &
): bool {query}
+ CommandGuard(robot_description: const std::string &)...
rclcpp::Node
rclcpp::Node
subscribe lbr_fri_msg::msg::LBRState
to state
subscribe lbr_fri_msg::msg::LBRState...
lbr_fri_ros2::App
lbr_fri_ros2::App
# node_: rclcpp::Node::SharedPtr
# run_thread_: std::unique_ptr<std::thread>
# app_connect_srv_: rclcpp::Service<lbr_fri_msgs::srv::AppConnect>::SharedPtr
# app_disconnect_srv_: rclcpp::Service<lbr_fri_msgs::srv::AppDisconnect>::SharedPtr
# node_: rclcpp::Node::SharedPtr...
# run_(): void
LBR IIWA7 R800LBR IIWA14 R820LBR Med7 R800LBR Med14 R820LBR IIWA 7 R800LBR IIWA 14 R820LBR Med 7 R800LBR Med 14 R820
LBR IIWA 7 R800

# run_(): void
publish lbr_fri_msg::msg::LBRCommand
to command
publish lbr_fri_msg::msg::LBRCommand...
lbr_fri_msgs::srv::AppConnect
lbr_fri_msgs::srv::AppConnect
lbr_fri_msgs::srv::AppDisconnect
lbr_fri_msgs::srv::AppDisconnect
lbr_fri_msgs::msg::LBRCommand
lbr_fri_msgs::msg::LBRCommand
lbr_fri_msgs::msg::LBRState
lbr_fri_msgs::msg::LBRState
Userland
Userland
Robot
Robot
High-level
High-level
KUKA
KUKA
lbr_fri_ros2_stack
lbr_fri_ros2_stack
ROS 2
ROS 2
lbr_fri_ros2::Client
lbr_fri_ros2::Client
# node_: rclcpp::Node::SharedPtr
# lbr_command_: lbr_fri_msgs::msg::LBRCommand
# lbr_state_: lbr_fri_msgs::msg::LBRState
# lbr_command_sub_: rclcpp::Subscription<lbr_fri_msgs::msg::LBRCommand>::SharedPtr
# lbr_state_pub_: rclcpp::Publisher<lbr_fri_msgs::msg::LBRState>::SharedPtr
# lbr_command_guard: std::unique_ptr<lbr_fri_ros2::CommandGuard>
# node_: rclcpp::Node::SharedPtr...
+ monitor(): void
+ waitForCommand(): void
+ command(): void
+ onStateChange(): void
# pub_lbr_state_(): void
# on_lbr_command_(const lbr_fri_msgs::msg::LBRCommand::SharedPtr): void
+ monitor(): void...Text is not SVG - cannot display \ No newline at end of file diff --git a/lbr_fri_ros2/doc/img/lbr_fri_ros2_v1.5.0.svg b/lbr_fri_ros2/doc/img/lbr_fri_ros2_v1.5.0.svg new file mode 100644 index 00000000..0f7b302b --- /dev/null +++ b/lbr_fri_ros2/doc/img/lbr_fri_ros2_v1.5.0.svg @@ -0,0 +1,4 @@ + + + +
lbr_fri_ros2
App
Utility class for FRI connection management and asynchronous spinning.

+ void run_async (int rt_prio=80)
AsyncClient
Shared object between User and App. User sets commands / gets states. App gets commands / sets states.

+ std::shared_ptr<BaseCommandInterface> get_command_interface ()
+ std::shared_ptr<StateInterface> get_state_interface ()
1
BaseCommandInterface
+ void buffer_command_target (const_idl_command_t_ref command)
+ virtual void buffered_command_to_fri (fri_command_t_ref command, const_idl_state_t_ref state) = 0
StateInterface
+ const_idl_state_t_ref get_state () const
+ void set_state (const_fri_state_t_ref state)

PositionCommandInterface
+ void buffered_command_to_fri (fri_command_t_ref command, const_idl_state_t_ref state)
TorqueCommandInterface
+ void buffered_command_to_fri (fri_command_t_ref command, const_idl_state_t_ref state)
WrenchCommandInterface
+ void buffered_command_to_fri (fri_command_t_ref command, const_idl_state_t_ref state)
1
child
parent
child
child
1
User
User code manages an lbr_fri_ros2::App and a shared lbr_fri_ros2::AsyncClient.

User runs the lbr_fri_ros2::App asynchronously and buffers commands to / gets states from the lbr_fri_ros2::AsyncClient via the interfaces.
1
1
 \ No newline at end of file diff --git a/lbr_fri_ros2/doc/lbr_fri_ros2.rst b/lbr_fri_ros2/doc/lbr_fri_ros2.rst index 25de79ca..2dfd4b25 100644 --- a/lbr_fri_ros2/doc/lbr_fri_ros2.rst +++ b/lbr_fri_ros2/doc/lbr_fri_ros2.rst @@ -9,47 +9,17 @@ lbr_fri_ros2 ../../../docs/doxygen/lbr_fri_ros2/html/annotated_classes -The ``lbr_fri_ros2`` package adds :lbr_fri_ros2:`AsyncClient `. - Software Architecture --------------------- -An overview of the software architecture is shown :ref:`below `: - -.. _target to software architecture figure: -.. thumbnail:: img/lbr_fri_ros2.drawio.svg - :alt: lbr_fri_ros2 - -Design Principles -~~~~~~~~~~~~~~~~~ -- Leave KUKA's FRI **untouched** (except for new ``ament_cmake`` build system) -> implemented through :ref:`FRI` package. -- Bridge ``nanopb`` (used within FRI for message definition) with ROS 2 Interface Definition Language (``IDL``) -> implemented through ``lbr_fri_idl`` package. -- Support future versions of the FRI -> implemented through ``vcstool`` and by separating the :ref:`FRI` package. -- Run stand-alone **and** within ``ros2_control`` -> implemented through :lbr_fri_ros2:`App `. - -Implementation Details -~~~~~~~~~~~~~~~~~~~~~~ -The FRI lets users communicate to the robot via a :fri:`ClientApplication `. The :fri:`ClientApplication ` has (see :ref:`above `): - -- :fri:`UdpConnection ` (UDP socket for reading states / sending commands) -- :fri:`Client ` (interface for reading states / sending commands) +The ``lbr_fri_ros2`` package extends the FRI with: -The user calls :fri:`step `, which, depending on the robot's state, callbacks: +#. :lbr_fri_ros2:`AsyncClient ` for asynchronous communication to the hardware +#. :lbr_fri_ros2:`App ` for running the :lbr_fri_ros2:`AsyncClient ` asynchronously -- :fri:`monitor ` -- :fri:`waitForCommand ` -- :fri:`command ` +An overview of the software architecture is shown :ref:`below `: -The user can implement these callbacks to read states / send commands by implementing an :fri:`Client `. - -The ``lbr_fri_ros2`` package implements an :fri:`Client ` in :lbr_fri_ros2:`Client `. - -The :lbr_fri_ros2:`Client ` has - - - A publisher to publish states in :lbr_fri_ros2:`pub_lbr_state_ `. - - A subscription to read commands in :lbr_fri_ros2:`on_lbr_command_ `. - -Commands in :lbr_fri_ros2:`on_lbr_command_ ` are checked for validity via a :lbr_fri_ros2:`CommandGuard `. +.. _software architecture figure: +.. thumbnail:: img/lbr_fri_ros2_v1.5.0.svg + :alt: lbr_fri_ros2 -API -~~~ -For the ``Doxygen`` generated API, checkout `lbr_fri_ros2 <../../../docs/doxygen/lbr_fri_ros2/html/hierarchy.html>`_. +The :ref:`lbr_ros2_control` package can be considered a **User** in the above figure. It builds on top of the ``lbr_fri_ros2`` package to provide a ROS 2 interface to the hardware. From 5a4fd86bb38cc746251178e9a820269a9a69864b Mon Sep 17 00:00:00 2001 From: mhubii Date: Mon, 13 May 2024 17:09:00 +0100 Subject: [PATCH 76/76] updating doc --- README.md | 4 ++-- docker/doc/docker.rst | 2 +- lbr_demos/doc/lbr_demos.rst | 2 +- .../doc/lbr_demos_advanced_cpp.rst | 2 +- lbr_fri_ros2/doc/lbr_fri_ros2.rst | 4 ++-- lbr_ros2_control/doc/lbr_ros2_control.rst | 22 +++++++++++++++++++ 6 files changed, 29 insertions(+), 7 deletions(-) diff --git a/README.md b/README.md index 388750f7..ac34ab13 100644 --- a/README.md +++ b/README.md @@ -25,7 +25,7 @@ ROS 2 packages for the KUKA LBR, including communication to the real robot via t ## Documentation -Full documentation available [here](https://lbr-stack.readthedocs.io/en/latest). +Full documentation available on [Read the Docs](https://lbr-stack.readthedocs.io/en/latest). ## Quick Start 1. Install ROS 2 development tools @@ -67,7 +67,7 @@ Full documentation available [here](https://lbr-stack.readthedocs.io/en/latest). > [!TIP] > List all arguments for the launch file via `ros2 launch lbr_bringup bringup.launch.py -s` -Now, run the [demos](https://lbr-fri-ros2-stack-doc.readthedocs.io/en/humble/lbr_fri_ros2_stack/lbr_demos/doc/lbr_demos.html). To get started with the real robot, checkout the [Documentation](https://lbr-stack.readthedocs.io/en/latest) above. +Now, run the [demos](https://lbr-stack.readthedocs.io/en/latest/lbr_fri_ros2_stack/lbr_demos/doc/lbr_demos.html). To get started with the real robot, checkout the [Hardware Setup](https://lbr-stack.readthedocs.io/en/latest/lbr_fri_ros2_stack/lbr_fri_ros2_stack/doc/hardware_setup.html). ## Citation If you enjoyed using this repository for your work, we would really appreciate ❤️ if you could cite it, as it helps us to continue offering support. diff --git a/docker/doc/docker.rst b/docker/doc/docker.rst index 1ef56d04..813bb07f 100644 --- a/docker/doc/docker.rst +++ b/docker/doc/docker.rst @@ -22,7 +22,7 @@ To run the ``lbr_fri_ros2_stack`` in a Docker container, follow the instructions cp -r src/lbr_fri_ros2_stack/docker/* . chmod +x container_build.sh - sudo ./container_build.sh 1.15 # replace by your FRI client version + sudo ./container_build.sh #. Inside the container, launch e.g. simulation via (might require re-launch after Gazebo launched first time) diff --git a/lbr_demos/doc/lbr_demos.rst b/lbr_demos/doc/lbr_demos.rst index b1845019..2b0eb131 100644 --- a/lbr_demos/doc/lbr_demos.rst +++ b/lbr_demos/doc/lbr_demos.rst @@ -1,6 +1,6 @@ lbr_demos ========= -Contolling the LBR through ROS 2 control. +Contolling the LBR through ``ros2_control``. .. note:: Some demos require a real robot. Make sure you followed :doc:`Hardware Setup <../../lbr_fri_ros2_stack/doc/hardware_setup>` first. diff --git a/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst b/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst index e870f26f..06e86293 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst +++ b/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst @@ -27,7 +27,7 @@ Admittance Controller .. code-block:: bash - ros2 run lbr_demos_a dvanced_cpp admittance_control --ros-args \ + ros2 run lbr_demos_advanced_cpp admittance_control --ros-args \ -r __ns:=/lbr \ --params-file `ros2 pkg prefix lbr_demos_advanced_cpp`/share/lbr_demos_advanced_cpp/config/admittance_control.yaml diff --git a/lbr_fri_ros2/doc/lbr_fri_ros2.rst b/lbr_fri_ros2/doc/lbr_fri_ros2.rst index 2dfd4b25..b151807d 100644 --- a/lbr_fri_ros2/doc/lbr_fri_ros2.rst +++ b/lbr_fri_ros2/doc/lbr_fri_ros2.rst @@ -16,9 +16,9 @@ The ``lbr_fri_ros2`` package extends the FRI with: #. :lbr_fri_ros2:`AsyncClient ` for asynchronous communication to the hardware #. :lbr_fri_ros2:`App ` for running the :lbr_fri_ros2:`AsyncClient ` asynchronously -An overview of the software architecture is shown :ref:`below `: +The software architecture is shown :ref:`below ` (click to expand): -.. _software architecture figure: +.. _lbr_fri_ros2 software architecture figure: .. thumbnail:: img/lbr_fri_ros2_v1.5.0.svg :alt: lbr_fri_ros2 diff --git a/lbr_ros2_control/doc/lbr_ros2_control.rst b/lbr_ros2_control/doc/lbr_ros2_control.rst index e6b88078..170e0ca6 100644 --- a/lbr_ros2_control/doc/lbr_ros2_control.rst +++ b/lbr_ros2_control/doc/lbr_ros2_control.rst @@ -1,2 +1,24 @@ lbr_ros2_control ================ +.. note:: + + - This package builds on top of :ref:`lbr_fri_ros2` + - For demo usage refer to :ref:`lbr_demos` + - Refer to `ros2_control `_ for additional documentation + +Relation to ros2_control +------------------------ +The ``ros2_control`` relation is shown :ref:`below ` (click to expand): + +.. _lbr_ros2_control software architecture figure: +.. thumbnail:: img/lbr_ros2_control_v1.5.0.svg + :alt: lbr_ros2_control + + + + +async etc +controller manager +system interface +controllers +lbr_fri_ros2