From eb6312c8f3a9f94a3d837cf10f1a567a9871de49 Mon Sep 17 00:00:00 2001 From: mhubii Date: Mon, 7 Aug 2023 23:15:46 +0100 Subject: [PATCH 01/33] removed robot name prefix from configs --- lbr_bringup/config/config.rviz | 18 ++--- .../src/admittance_control_node.cpp | 4 +- .../src/admittance_controller.hpp | 4 +- .../admittance_control_node.py | 4 +- .../admittance_controller.py | 4 +- .../lbr_joint_trajectory_executioner_node.cpp | 8 +- .../lbr_joint_trajectory_executioner_node.py | 6 +- lbr_description/config/config.rviz | 18 ++--- lbr_description/gazebo/iiwa.gazebo.xacro | 16 ++-- lbr_description/gazebo/med.gazebo.xacro | 16 ++-- .../ros2_control/lbr.ros2_control.xacro | 16 ++-- .../urdf/iiwa14/iiwa14_description.urdf.xacro | 70 ++++++++-------- .../urdf/iiwa7/iiwa7_description.urdf.xacro | 70 ++++++++-------- .../urdf/med14/med14_description.urdf.xacro | 70 ++++++++-------- .../urdf/med7/med7_description.urdf.xacro | 70 ++++++++-------- .../config/lbr_controllers.yml | 28 +++---- .../launch/lbr_hardware_interface.launch.py | 5 +- .../lbr_hardware_interface/launch_mixin.py | 36 ++++++++- .../iiwa14_moveit_config/config/iiwa14.srdf | 80 +++++++++---------- .../config/joint_limits.yaml | 14 ++-- .../config/moveit_controllers.yaml | 14 ++-- .../iiwa7_moveit_config/config/iiwa7.srdf | 80 +++++++++---------- .../config/joint_limits.yaml | 14 ++-- .../config/moveit_controllers.yaml | 14 ++-- .../config/joint_limits.yaml | 14 ++-- .../med14_moveit_config/config/med14.srdf | 80 +++++++++---------- .../config/moveit_controllers.yaml | 14 ++-- .../config/joint_limits.yaml | 14 ++-- .../med7_moveit_config/config/med7.srdf | 80 +++++++++---------- .../config/moveit_controllers.yaml | 14 ++-- 30 files changed, 460 insertions(+), 435 deletions(-) diff --git a/lbr_bringup/config/config.rviz b/lbr_bringup/config/config.rviz index 361bd233..fd0449c4 100644 --- a/lbr_bringup/config/config.rviz +++ b/lbr_bringup/config/config.rviz @@ -67,47 +67,47 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - lbr_link_0: + link_0: Alpha: 1 Show Axes: false Show Trail: false Value: true - lbr_link_1: + link_1: Alpha: 1 Show Axes: false Show Trail: false Value: true - lbr_link_2: + link_2: Alpha: 1 Show Axes: false Show Trail: false Value: true - lbr_link_3: + link_3: Alpha: 1 Show Axes: false Show Trail: false Value: true - lbr_link_4: + link_4: Alpha: 1 Show Axes: false Show Trail: false Value: true - lbr_link_5: + link_5: Alpha: 1 Show Axes: false Show Trail: false Value: true - lbr_link_6: + link_6: Alpha: 1 Show Axes: false Show Trail: false Value: true - lbr_link_7: + link_7: Alpha: 1 Show Axes: false Show Trail: false Value: true - lbr_link_ee: + link_ee: Alpha: 1 Show Axes: false Show Trail: false diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_control_node.cpp b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_control_node.cpp index 00f162d3..927cb2ff 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_control_node.cpp +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_control_node.cpp @@ -14,8 +14,8 @@ class AdmittanceControlNode : public rclcpp::Node { AdmittanceControlNode(const std::string &node_name, const rclcpp::NodeOptions &options) : rclcpp::Node(node_name, options) { this->declare_parameter("robot_description"); - this->declare_parameter("base_link", "lbr_link_0"); - this->declare_parameter("end_effector_link", "lbr_link_ee"); + this->declare_parameter("base_link", "link_0"); + this->declare_parameter("end_effector_link", "link_ee"); admittance_controller_ = std::make_unique( this->get_parameter("robot_description").as_string(), diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_controller.hpp b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_controller.hpp index 07348352..4a9b83c3 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_controller.hpp +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_controller.hpp @@ -22,8 +22,8 @@ class AdmittanceController { public: AdmittanceController(const std::string &robot_description, - const std::string &base_link = "lbr_link_0", - const std::string &end_effector_link = "lbr_link_ee", + const std::string &base_link = "link_0", + const std::string &end_effector_link = "link_ee", const CartesianVector &f_ext_th = {2., 2., 2., 0.5, 0.5, 0.5}, const JointVector &dq_gains = {2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0}, const CartesianVector &dx_gains = {1.5, 1.5, 1.5, 20., 40., 60.}) 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_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/admittance_control_node.py index 5930ec7e..fa0b4867 100755 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/admittance_control_node.py +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/admittance_control_node.py @@ -15,8 +15,8 @@ def __init__(self, node_name="admittance_control_node") -> None: # parameters self.declare_parameter("robot_description", "") - self.declare_parameter("base_link", "lbr_link_0") - self.declare_parameter("end_effector_link", "lbr_link_ee") + self.declare_parameter("base_link", "link_0") + self.declare_parameter("end_effector_link", "link_ee") self.init_ = False self.lbr_state_ = LBRState() 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_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/admittance_controller.py index 32ca1c5b..5a3463f4 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/admittance_controller.py +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/admittance_controller.py @@ -8,8 +8,8 @@ class AdmittanceController(object): def __init__( self, robot_description: str, - base_link: str = "lbr_link_0", - end_effector_link: str = "lbr_link_ee", + 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([1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5]), dx_gain: np.ndarray = np.array([1.0, 1.0, 1.0, 20.0, 40.0, 60.0]), diff --git a/lbr_demos/lbr_demos_ros2_control_cpp/src/lbr_joint_trajectory_executioner_node.cpp b/lbr_demos/lbr_demos_ros2_control_cpp/src/lbr_joint_trajectory_executioner_node.cpp index aca50aa5..17648435 100644 --- a/lbr_demos/lbr_demos_ros2_control_cpp/src/lbr_joint_trajectory_executioner_node.cpp +++ b/lbr_demos/lbr_demos_ros2_control_cpp/src/lbr_joint_trajectory_executioner_node.cpp @@ -12,9 +12,7 @@ class LBRJointTrajectoryExecutionerNode : public rclcpp::Node { public: - LBRJointTrajectoryExecutionerNode(const std::string &node_name, - const std::string &robot_name = "lbr") - : Node(node_name), robot_name_(robot_name) { + LBRJointTrajectoryExecutionerNode(const std::string &node_name) : Node(node_name) { joint_trajectory_action_client_ = rclcpp_action::create_client( @@ -43,8 +41,7 @@ class LBRJointTrajectoryExecutionerNode : public rclcpp::Node { point.time_from_start.sec = sec_from_start; for (std::size_t i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; ++i) { - joint_trajectory_goal.trajectory.joint_names.push_back(robot_name_ + "_A" + - std::to_string(i + 1)); + joint_trajectory_goal.trajectory.joint_names.push_back("A" + std::to_string(i + 1)); } joint_trajectory_goal.trajectory.points.push_back(point); @@ -71,7 +68,6 @@ class LBRJointTrajectoryExecutionerNode : public rclcpp::Node { } protected: - std::string robot_name_; rclcpp_action::Client::SharedPtr joint_trajectory_action_client_; }; diff --git a/lbr_demos/lbr_demos_ros2_control_python/lbr_demos_ros2_control_python/lbr_joint_trajectory_executioner_node.py b/lbr_demos/lbr_demos_ros2_control_python/lbr_demos_ros2_control_python/lbr_joint_trajectory_executioner_node.py index d8e0357b..425c0835 100644 --- a/lbr_demos/lbr_demos_ros2_control_python/lbr_demos_ros2_control_python/lbr_joint_trajectory_executioner_node.py +++ b/lbr_demos/lbr_demos_ros2_control_python/lbr_demos_ros2_control_python/lbr_joint_trajectory_executioner_node.py @@ -9,10 +9,8 @@ class LBRJointTrajectoryExecutionerNode(Node): def __init__( self, node_name: str, - robot_name: str = "lbr", ) -> None: super().__init__(node_name=node_name) - self.robot_name_ = robot_name self.joint_trajectory_action_client_ = ActionClient( node=self, action_type=FollowJointTrajectory, @@ -38,9 +36,7 @@ def execute(self, positions: list, sec_from_start: int = 10): point.time_from_start.sec = sec_from_start for i in range(7): - joint_trajectory_goal.trajectory.joint_names.append( - f"{self.robot_name_}_A{i + 1}" - ) + joint_trajectory_goal.trajectory.joint_names.append(f"A{i + 1}") joint_trajectory_goal.trajectory.points.append(point) diff --git a/lbr_description/config/config.rviz b/lbr_description/config/config.rviz index 51be7502..4dadd954 100644 --- a/lbr_description/config/config.rviz +++ b/lbr_description/config/config.rviz @@ -61,47 +61,47 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - lbr_link_0: + link_0: Alpha: 1 Show Axes: false Show Trail: false Value: true - lbr_link_1: + link_1: Alpha: 1 Show Axes: false Show Trail: false Value: true - lbr_link_2: + link_2: Alpha: 1 Show Axes: false Show Trail: false Value: true - lbr_link_3: + link_3: Alpha: 1 Show Axes: false Show Trail: false Value: true - lbr_link_4: + link_4: Alpha: 1 Show Axes: false Show Trail: false Value: true - lbr_link_5: + link_5: Alpha: 1 Show Axes: false Show Trail: false Value: true - lbr_link_6: + link_6: Alpha: 1 Show Axes: false Show Trail: false Value: true - lbr_link_7: + link_7: Alpha: 1 Show Axes: false Show Trail: false Value: true - lbr_link_ee: + link_ee: Alpha: 1 Show Axes: false Show Trail: false diff --git a/lbr_description/gazebo/iiwa.gazebo.xacro b/lbr_description/gazebo/iiwa.gazebo.xacro index 7f9d2c06..d22dc303 100644 --- a/lbr_description/gazebo/iiwa.gazebo.xacro +++ b/lbr_description/gazebo/iiwa.gazebo.xacro @@ -11,56 +11,56 @@ - + 0.2 0.2 Gazebo/Grey - + 0.2 0.2 Gazebo/Orange - + 0.2 0.2 Gazebo/Orange - + 0.2 0.2 Gazebo/Orange - + 0.2 0.2 Gazebo/Orange - + 0.2 0.2 Gazebo/Orange - + 0.2 0.2 Gazebo/Orange - + 0.2 0.2 Gazebo/Grey diff --git a/lbr_description/gazebo/med.gazebo.xacro b/lbr_description/gazebo/med.gazebo.xacro index d8474350..903d9f54 100644 --- a/lbr_description/gazebo/med.gazebo.xacro +++ b/lbr_description/gazebo/med.gazebo.xacro @@ -11,56 +11,56 @@ - + 0.2 0.2 Gazebo/White - + 0.2 0.2 Gazebo/White - + 0.2 0.2 Gazebo/White - + 0.2 0.2 Gazebo/White - + 0.2 0.2 Gazebo/White - + 0.2 0.2 Gazebo/White - + 0.2 0.2 Gazebo/White - + 0.2 0.2 Gazebo/Grey diff --git a/lbr_description/ros2_control/lbr.ros2_control.xacro b/lbr_description/ros2_control/lbr.ros2_control.xacro index acbeec2e..c886646e 100644 --- a/lbr_description/ros2_control/lbr.ros2_control.xacro +++ b/lbr_description/ros2_control/lbr.ros2_control.xacro @@ -33,7 +33,7 @@ - + @@ -53,7 +53,7 @@ - + -${joint_0_position_limit} @@ -73,7 +73,7 @@ - + -${joint_1_position_limit} ${joint_1_position_limit} @@ -92,7 +92,7 @@ - + -${joint_2_position_limit} ${joint_2_position_limit} @@ -111,7 +111,7 @@ - + -${joint_3_position_limit} ${joint_3_position_limit} @@ -130,7 +130,7 @@ - + -${joint_4_position_limit} ${joint_4_position_limit} @@ -149,7 +149,7 @@ - + -${joint_5_position_limit} ${joint_5_position_limit} @@ -168,7 +168,7 @@ - + -${joint_6_position_limit} ${joint_6_position_limit} diff --git a/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro b/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro index d6a2126a..f2276110 100644 --- a/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro +++ b/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro @@ -32,12 +32,12 @@ - + - + - + @@ -60,10 +60,10 @@ - + - - + + - + @@ -94,10 +94,10 @@ - + - - + + - + @@ -128,10 +128,10 @@ - + - - + + - + @@ -162,10 +162,10 @@ - + - - + + - + @@ -196,10 +196,10 @@ - + - - + + - + @@ -230,10 +230,10 @@ - + - - + + - + @@ -264,10 +264,10 @@ - + - - + + - + @@ -298,13 +298,13 @@ - - - + + + - + diff --git a/lbr_description/urdf/iiwa7/iiwa7_description.urdf.xacro b/lbr_description/urdf/iiwa7/iiwa7_description.urdf.xacro index 6b1c839a..ab100233 100644 --- a/lbr_description/urdf/iiwa7/iiwa7_description.urdf.xacro +++ b/lbr_description/urdf/iiwa7/iiwa7_description.urdf.xacro @@ -32,12 +32,12 @@ - + - + - + @@ -60,10 +60,10 @@ - + - - + + - + @@ -94,10 +94,10 @@ - + - - + + - + @@ -128,10 +128,10 @@ - + - - + + - + @@ -162,10 +162,10 @@ - + - - + + - + @@ -196,10 +196,10 @@ - + - - + + - + @@ -230,10 +230,10 @@ - + - - + + - + @@ -265,10 +265,10 @@ - + - - + + - + @@ -299,13 +299,13 @@ - - - + + + - + diff --git a/lbr_description/urdf/med14/med14_description.urdf.xacro b/lbr_description/urdf/med14/med14_description.urdf.xacro index 718099d9..4c3c0b5e 100644 --- a/lbr_description/urdf/med14/med14_description.urdf.xacro +++ b/lbr_description/urdf/med14/med14_description.urdf.xacro @@ -32,12 +32,12 @@ - + - + - + @@ -60,10 +60,10 @@ - + - - + + - + @@ -94,10 +94,10 @@ - + - - + + - + @@ -128,10 +128,10 @@ - + - - + + - + @@ -162,10 +162,10 @@ - + - - + + - + @@ -196,10 +196,10 @@ - + - - + + - + @@ -230,10 +230,10 @@ - + - - + + - + @@ -264,10 +264,10 @@ - + - - + + - + @@ -298,13 +298,13 @@ - - - + + + - + diff --git a/lbr_description/urdf/med7/med7_description.urdf.xacro b/lbr_description/urdf/med7/med7_description.urdf.xacro index 2e6c44f8..487d9690 100644 --- a/lbr_description/urdf/med7/med7_description.urdf.xacro +++ b/lbr_description/urdf/med7/med7_description.urdf.xacro @@ -32,12 +32,12 @@ - + - + - + @@ -60,10 +60,10 @@ - + - - + + - + @@ -94,10 +94,10 @@ - + - - + + - + @@ -128,10 +128,10 @@ - + - - + + - + @@ -162,10 +162,10 @@ - + - - + + - + @@ -196,10 +196,10 @@ - + - - + + - + @@ -230,10 +230,10 @@ - + - - + + - + @@ -264,10 +264,10 @@ - + - - + + - + @@ -298,13 +298,13 @@ - - - + + + - + diff --git a/lbr_hardware_interface/config/lbr_controllers.yml b/lbr_hardware_interface/config/lbr_controllers.yml index 2a9ef275..3b57d72a 100644 --- a/lbr_hardware_interface/config/lbr_controllers.yml +++ b/lbr_hardware_interface/config/lbr_controllers.yml @@ -14,13 +14,13 @@ controller_manager: position_trajectory_controller: ros__parameters: joints: - - lbr_A1 - - lbr_A2 - - lbr_A3 - - lbr_A4 - - lbr_A5 - - lbr_A6 - - lbr_A7 + - A1 + - A2 + - A3 + - A4 + - A5 + - A6 + - A7 command_interfaces: - position state_interfaces: @@ -32,11 +32,11 @@ position_trajectory_controller: forward_position_controller: ros__parameters: joints: - - lbr_A1 - - lbr_A2 - - lbr_A3 - - lbr_A4 - - lbr_A5 - - lbr_A6 - - lbr_A7 + - A1 + - A2 + - A3 + - A4 + - A5 + - A6 + - A7 interface_name: position diff --git a/lbr_hardware_interface/launch/lbr_hardware_interface.launch.py b/lbr_hardware_interface/launch/lbr_hardware_interface.launch.py index e9a8b68e..b78c5b88 100644 --- a/lbr_hardware_interface/launch/lbr_hardware_interface.launch.py +++ b/lbr_hardware_interface/launch/lbr_hardware_interface.launch.py @@ -19,6 +19,7 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(LBRHardwareInterface.arg_ctrl_cfg_pkg()) ld.add_action(LBRHardwareInterface.arg_ctrl_cfg()) ld.add_action(LBRHardwareInterface.arg_ctrl()) + ld.add_action(LBRHardwareInterface.arg_frame_prefix()) ros2_control_node = LBRHardwareInterface.node_ros2_control( robot_description=robot_description ) @@ -33,7 +34,9 @@ def generate_launch_description() -> LaunchDescription: ) ld.add_action(controller_event_handler) robot_state_publisher = LBRHardwareInterface.node_robot_state_publisher( - robot_description=robot_description + robot_description=robot_description, + use_sim_time=False, + frame_prefix=LBRHardwareInterface.param_frame_prefix(), ) ld.add_action(robot_state_publisher) return ld diff --git a/lbr_hardware_interface/lbr_hardware_interface/launch_mixin.py b/lbr_hardware_interface/lbr_hardware_interface/launch_mixin.py index 15c365e7..82431542 100644 --- a/lbr_hardware_interface/lbr_hardware_interface/launch_mixin.py +++ b/lbr_hardware_interface/lbr_hardware_interface/launch_mixin.py @@ -1,4 +1,4 @@ -from typing import Dict +from typing import Dict, Optional, Union from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration, PathJoinSubstitution @@ -32,6 +32,26 @@ def arg_ctrl() -> DeclareLaunchArgument: choices=["position_trajectory_controller", "forward_position_controller"], ) + @staticmethod + def arg_frame_prefix() -> DeclareLaunchArgument: + return DeclareLaunchArgument( + name="frame_prefix", + default_value="", + description="Prefix for the tf frame names. Useful for multi-robot setups. E.g. 'robot1/'", + ) + + @staticmethod + def arg_use_sim_time() -> DeclareLaunchArgument: + return DeclareLaunchArgument( + name="use_sim_time", + default_value="false", + description="Use simulation (Gazebo) clock if true.", + ) + + @staticmethod + def param_frame_prefix() -> Dict[str, LaunchConfiguration]: + return {"frame_prefix": LaunchConfiguration("frame_prefix", default="")} + @staticmethod def node_ros2_control(robot_description: Dict[str, str], **kwargs) -> Node: return Node( @@ -85,14 +105,24 @@ def node_controller(**kwargs) -> Node: ) @staticmethod - def node_robot_state_publisher(robot_description: Dict[str, str], **kwargs) -> Node: + def node_robot_state_publisher( + robot_description: Dict[str, str], + use_sim_time: Optional[Union[LaunchConfiguration, bool]] = None, + frame_prefix: Optional[Union[LaunchConfiguration, str]] = None, + **kwargs + ) -> Node: + if use_sim_time is None: + use_sim_time = LaunchConfiguration("use_sim_time", default="false") + if frame_prefix is None: + frame_prefix = LaunchConfiguration("frame_prefix", default="") return Node( package="robot_state_publisher", executable="robot_state_publisher", output="screen", parameters=[ robot_description, - {"use_sim_time": False}, + {"use_sim_time": use_sim_time}, + {"frame_prefix": frame_prefix}, ], **kwargs ) diff --git a/lbr_moveit_config/iiwa14_moveit_config/config/iiwa14.srdf b/lbr_moveit_config/iiwa14_moveit_config/config/iiwa14.srdf index 934159c3..f1b96afe 100644 --- a/lbr_moveit_config/iiwa14_moveit_config/config/iiwa14.srdf +++ b/lbr_moveit_config/iiwa14_moveit_config/config/iiwa14.srdf @@ -10,51 +10,51 @@ - + - - - - - - - + + + + + + + - - - - - - - + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/lbr_moveit_config/iiwa14_moveit_config/config/joint_limits.yaml b/lbr_moveit_config/iiwa14_moveit_config/config/joint_limits.yaml index 0ab19715..ae4149e9 100644 --- a/lbr_moveit_config/iiwa14_moveit_config/config/joint_limits.yaml +++ b/lbr_moveit_config/iiwa14_moveit_config/config/joint_limits.yaml @@ -8,37 +8,37 @@ default_acceleration_scaling_factor: 0.1 # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - lbr_A1: + A1: has_velocity_limits: true max_velocity: 1.4835298641951802 has_acceleration_limits: true max_acceleration: 10.0 - lbr_A2: + A2: has_velocity_limits: true max_velocity: 1.4835298641951802 has_acceleration_limits: true max_acceleration: 10.0 - lbr_A3: + A3: has_velocity_limits: true max_velocity: 1.7453292519943295 has_acceleration_limits: true max_acceleration: 10.0 - lbr_A4: + A4: has_velocity_limits: true max_velocity: 1.3089969389957472 has_acceleration_limits: true max_acceleration: 10.0 - lbr_A5: + A5: has_velocity_limits: true max_velocity: 2.2689280275926285 has_acceleration_limits: true max_acceleration: 10.0 - lbr_A6: + A6: has_velocity_limits: true max_velocity: 2.3561944901923448 has_acceleration_limits: true max_acceleration: 10.0 - lbr_A7: + A7: has_velocity_limits: true max_velocity: 2.3561944901923448 has_acceleration_limits: true diff --git a/lbr_moveit_config/iiwa14_moveit_config/config/moveit_controllers.yaml b/lbr_moveit_config/iiwa14_moveit_config/config/moveit_controllers.yaml index a63de224..27b3961f 100644 --- a/lbr_moveit_config/iiwa14_moveit_config/config/moveit_controllers.yaml +++ b/lbr_moveit_config/iiwa14_moveit_config/config/moveit_controllers.yaml @@ -11,10 +11,10 @@ moveit_simple_controller_manager: action_ns: follow_joint_trajectory default: true joints: - - lbr_A1 - - lbr_A2 - - lbr_A3 - - lbr_A4 - - lbr_A5 - - lbr_A6 - - lbr_A7 \ No newline at end of file + - A1 + - A2 + - A3 + - A4 + - A5 + - A6 + - A7 \ No newline at end of file diff --git a/lbr_moveit_config/iiwa7_moveit_config/config/iiwa7.srdf b/lbr_moveit_config/iiwa7_moveit_config/config/iiwa7.srdf index 5ef397ba..37a9aa08 100644 --- a/lbr_moveit_config/iiwa7_moveit_config/config/iiwa7.srdf +++ b/lbr_moveit_config/iiwa7_moveit_config/config/iiwa7.srdf @@ -10,51 +10,51 @@ - + - - - - - - - + + + + + + + - - - - - - - + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/lbr_moveit_config/iiwa7_moveit_config/config/joint_limits.yaml b/lbr_moveit_config/iiwa7_moveit_config/config/joint_limits.yaml index 2a2bc84e..e97eee4e 100644 --- a/lbr_moveit_config/iiwa7_moveit_config/config/joint_limits.yaml +++ b/lbr_moveit_config/iiwa7_moveit_config/config/joint_limits.yaml @@ -8,37 +8,37 @@ default_acceleration_scaling_factor: 0.1 # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - lbr_A1: + A1: has_velocity_limits: true max_velocity: 1.7104226669544429 has_acceleration_limits: true max_acceleration: 10.0 - lbr_A2: + A2: has_velocity_limits: true max_velocity: 1.7104226669544429 has_acceleration_limits: true max_acceleration: 10.0 - lbr_A3: + A3: has_velocity_limits: true max_velocity: 1.7453292519943295 has_acceleration_limits: true max_acceleration: 10.0 - lbr_A4: + A4: has_velocity_limits: true max_velocity: 2.2689280275926285 has_acceleration_limits: true max_acceleration: 10.0 - lbr_A5: + A5: has_velocity_limits: true max_velocity: 2.4434609527920612 has_acceleration_limits: true max_acceleration: 10.0 - lbr_A6: + A6: has_velocity_limits: true max_velocity: 3.1415926535897931 has_acceleration_limits: true max_acceleration: 10.0 - lbr_A7: + A7: has_velocity_limits: true max_velocity: 3.1415926535897931 has_acceleration_limits: true diff --git a/lbr_moveit_config/iiwa7_moveit_config/config/moveit_controllers.yaml b/lbr_moveit_config/iiwa7_moveit_config/config/moveit_controllers.yaml index a63de224..27b3961f 100644 --- a/lbr_moveit_config/iiwa7_moveit_config/config/moveit_controllers.yaml +++ b/lbr_moveit_config/iiwa7_moveit_config/config/moveit_controllers.yaml @@ -11,10 +11,10 @@ moveit_simple_controller_manager: action_ns: follow_joint_trajectory default: true joints: - - lbr_A1 - - lbr_A2 - - lbr_A3 - - lbr_A4 - - lbr_A5 - - lbr_A6 - - lbr_A7 \ No newline at end of file + - A1 + - A2 + - A3 + - A4 + - A5 + - A6 + - A7 \ No newline at end of file diff --git a/lbr_moveit_config/med14_moveit_config/config/joint_limits.yaml b/lbr_moveit_config/med14_moveit_config/config/joint_limits.yaml index 0ab19715..ae4149e9 100644 --- a/lbr_moveit_config/med14_moveit_config/config/joint_limits.yaml +++ b/lbr_moveit_config/med14_moveit_config/config/joint_limits.yaml @@ -8,37 +8,37 @@ default_acceleration_scaling_factor: 0.1 # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - lbr_A1: + A1: has_velocity_limits: true max_velocity: 1.4835298641951802 has_acceleration_limits: true max_acceleration: 10.0 - lbr_A2: + A2: has_velocity_limits: true max_velocity: 1.4835298641951802 has_acceleration_limits: true max_acceleration: 10.0 - lbr_A3: + A3: has_velocity_limits: true max_velocity: 1.7453292519943295 has_acceleration_limits: true max_acceleration: 10.0 - lbr_A4: + A4: has_velocity_limits: true max_velocity: 1.3089969389957472 has_acceleration_limits: true max_acceleration: 10.0 - lbr_A5: + A5: has_velocity_limits: true max_velocity: 2.2689280275926285 has_acceleration_limits: true max_acceleration: 10.0 - lbr_A6: + A6: has_velocity_limits: true max_velocity: 2.3561944901923448 has_acceleration_limits: true max_acceleration: 10.0 - lbr_A7: + A7: has_velocity_limits: true max_velocity: 2.3561944901923448 has_acceleration_limits: true diff --git a/lbr_moveit_config/med14_moveit_config/config/med14.srdf b/lbr_moveit_config/med14_moveit_config/config/med14.srdf index 1925c3e5..f5ef369c 100644 --- a/lbr_moveit_config/med14_moveit_config/config/med14.srdf +++ b/lbr_moveit_config/med14_moveit_config/config/med14.srdf @@ -10,51 +10,51 @@ - + - - - - - - - + + + + + + + - - - - - - - + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/lbr_moveit_config/med14_moveit_config/config/moveit_controllers.yaml b/lbr_moveit_config/med14_moveit_config/config/moveit_controllers.yaml index a63de224..27b3961f 100644 --- a/lbr_moveit_config/med14_moveit_config/config/moveit_controllers.yaml +++ b/lbr_moveit_config/med14_moveit_config/config/moveit_controllers.yaml @@ -11,10 +11,10 @@ moveit_simple_controller_manager: action_ns: follow_joint_trajectory default: true joints: - - lbr_A1 - - lbr_A2 - - lbr_A3 - - lbr_A4 - - lbr_A5 - - lbr_A6 - - lbr_A7 \ No newline at end of file + - A1 + - A2 + - A3 + - A4 + - A5 + - A6 + - A7 \ No newline at end of file diff --git a/lbr_moveit_config/med7_moveit_config/config/joint_limits.yaml b/lbr_moveit_config/med7_moveit_config/config/joint_limits.yaml index 2a2bc84e..e97eee4e 100644 --- a/lbr_moveit_config/med7_moveit_config/config/joint_limits.yaml +++ b/lbr_moveit_config/med7_moveit_config/config/joint_limits.yaml @@ -8,37 +8,37 @@ default_acceleration_scaling_factor: 0.1 # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - lbr_A1: + A1: has_velocity_limits: true max_velocity: 1.7104226669544429 has_acceleration_limits: true max_acceleration: 10.0 - lbr_A2: + A2: has_velocity_limits: true max_velocity: 1.7104226669544429 has_acceleration_limits: true max_acceleration: 10.0 - lbr_A3: + A3: has_velocity_limits: true max_velocity: 1.7453292519943295 has_acceleration_limits: true max_acceleration: 10.0 - lbr_A4: + A4: has_velocity_limits: true max_velocity: 2.2689280275926285 has_acceleration_limits: true max_acceleration: 10.0 - lbr_A5: + A5: has_velocity_limits: true max_velocity: 2.4434609527920612 has_acceleration_limits: true max_acceleration: 10.0 - lbr_A6: + A6: has_velocity_limits: true max_velocity: 3.1415926535897931 has_acceleration_limits: true max_acceleration: 10.0 - lbr_A7: + A7: has_velocity_limits: true max_velocity: 3.1415926535897931 has_acceleration_limits: true diff --git a/lbr_moveit_config/med7_moveit_config/config/med7.srdf b/lbr_moveit_config/med7_moveit_config/config/med7.srdf index 63e98c43..1b4932a5 100644 --- a/lbr_moveit_config/med7_moveit_config/config/med7.srdf +++ b/lbr_moveit_config/med7_moveit_config/config/med7.srdf @@ -10,51 +10,51 @@ - + - - - - - - - + + + + + + + - - - - - - - + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/lbr_moveit_config/med7_moveit_config/config/moveit_controllers.yaml b/lbr_moveit_config/med7_moveit_config/config/moveit_controllers.yaml index a63de224..27b3961f 100644 --- a/lbr_moveit_config/med7_moveit_config/config/moveit_controllers.yaml +++ b/lbr_moveit_config/med7_moveit_config/config/moveit_controllers.yaml @@ -11,10 +11,10 @@ moveit_simple_controller_manager: action_ns: follow_joint_trajectory default: true joints: - - lbr_A1 - - lbr_A2 - - lbr_A3 - - lbr_A4 - - lbr_A5 - - lbr_A6 - - lbr_A7 \ No newline at end of file + - A1 + - A2 + - A3 + - A4 + - A5 + - A6 + - A7 \ No newline at end of file From 4b21cc04156f4fa50e84f974a766b4e16b2a1e24 Mon Sep 17 00:00:00 2001 From: mhubii Date: Mon, 7 Aug 2023 23:20:21 +0100 Subject: [PATCH 02/33] added empty frame prefixes for now --- lbr_bringup/launch/real.launch.py | 2 +- lbr_bringup/launch/sim.launch.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/lbr_bringup/launch/real.launch.py b/lbr_bringup/launch/real.launch.py index e43d23f7..f5299c4d 100644 --- a/lbr_bringup/launch/real.launch.py +++ b/lbr_bringup/launch/real.launch.py @@ -34,7 +34,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: # robot state publisher on joint state broadcaster spawn exit robot_state_publisher = LBRHardwareInterfaceMixin.node_robot_state_publisher( - robot_description=robot_description + robot_description=robot_description, use_sim_time=False, frame_prefix="" ) robot_state_publisher_event_handler = RegisterEventHandler( OnProcessExit( diff --git a/lbr_bringup/launch/sim.launch.py b/lbr_bringup/launch/sim.launch.py index d705c2b6..8cfbd94c 100644 --- a/lbr_bringup/launch/sim.launch.py +++ b/lbr_bringup/launch/sim.launch.py @@ -21,7 +21,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: joint_state_broadcaster = LBRHardwareInterfaceMixin.node_joint_state_broadcaster() ld.add_action(joint_state_broadcaster) robot_state_publisher = LBRHardwareInterfaceMixin.node_robot_state_publisher( - robot_description=robot_description + robot_description=robot_description, use_sim_time=True, frame_prefix="" ) ld.add_action( robot_state_publisher From 7173d47fd7685dd18afa40c7ba1e0e88b8a0680a Mon Sep 17 00:00:00 2001 From: mhubii Date: Mon, 7 Aug 2023 23:23:35 +0100 Subject: [PATCH 03/33] 1.2.3 -> 1.2.4 --- 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_fri_ros2_stack/package.xml | 2 +- lbr_hardware_interface/package.xml | 2 +- 15 files changed, 15 insertions(+), 15 deletions(-) diff --git a/lbr_bringup/package.xml b/lbr_bringup/package.xml index 01d7f443..a173b420 100644 --- a/lbr_bringup/package.xml +++ b/lbr_bringup/package.xml @@ -2,7 +2,7 @@ lbr_bringup - 1.2.3 + 1.2.4 LBR launch files. mhubii MIT 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 67c30843..19bb42f7 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/package.xml +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/package.xml @@ -2,7 +2,7 @@ lbr_demos_fri_ros2_advanced_cpp - 1.2.3 + 1.2.4 Advanced C++ demos for the lbr_fri_ros2. mhubii MIT 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 4d1bc911..eeef06d7 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_python/package.xml +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_python/package.xml @@ -2,7 +2,7 @@ lbr_demos_fri_ros2_advanced_python - 1.2.3 + 1.2.4 Advanced Python demos for the lbr_fri_ros2. mhubii MIT 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 6432a079..5b68b0a9 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_python/setup.py +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_python/setup.py @@ -6,7 +6,7 @@ setup( name=package_name, - version="1.2.3", + version="1.2.4", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/lbr_demos/lbr_demos_fri_ros2_cpp/package.xml b/lbr_demos/lbr_demos_fri_ros2_cpp/package.xml index f1cf2619..200b681b 100644 --- a/lbr_demos/lbr_demos_fri_ros2_cpp/package.xml +++ b/lbr_demos/lbr_demos_fri_ros2_cpp/package.xml @@ -2,7 +2,7 @@ lbr_demos_fri_ros2_cpp - 1.2.3 + 1.2.4 C++ demos for the lbr_fri_ros2. mhubii MIT diff --git a/lbr_demos/lbr_demos_fri_ros2_python/package.xml b/lbr_demos/lbr_demos_fri_ros2_python/package.xml index 4f4a8a1f..f445b477 100644 --- a/lbr_demos/lbr_demos_fri_ros2_python/package.xml +++ b/lbr_demos/lbr_demos_fri_ros2_python/package.xml @@ -2,7 +2,7 @@ lbr_demos_fri_ros2_python - 1.2.3 + 1.2.4 Python demos for the lbr_fri_ros2. mhubii MIT diff --git a/lbr_demos/lbr_demos_fri_ros2_python/setup.py b/lbr_demos/lbr_demos_fri_ros2_python/setup.py index 2cbac05f..d2c0f439 100644 --- a/lbr_demos/lbr_demos_fri_ros2_python/setup.py +++ b/lbr_demos/lbr_demos_fri_ros2_python/setup.py @@ -6,7 +6,7 @@ setup( name=package_name, - version="1.2.3", + version="1.2.4", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/lbr_demos/lbr_demos_ros2_control_cpp/package.xml b/lbr_demos/lbr_demos_ros2_control_cpp/package.xml index 551e23c2..a4de7769 100644 --- a/lbr_demos/lbr_demos_ros2_control_cpp/package.xml +++ b/lbr_demos/lbr_demos_ros2_control_cpp/package.xml @@ -2,7 +2,7 @@ lbr_demos_ros2_control_cpp - 1.2.3 + 1.2.4 C++ demos for the LBR ros2_control integration. mhubii MIT diff --git a/lbr_demos/lbr_demos_ros2_control_python/package.xml b/lbr_demos/lbr_demos_ros2_control_python/package.xml index 3f84d888..f4d99db7 100644 --- a/lbr_demos/lbr_demos_ros2_control_python/package.xml +++ b/lbr_demos/lbr_demos_ros2_control_python/package.xml @@ -2,7 +2,7 @@ lbr_demos_ros2_control_python - 1.2.3 + 1.2.4 Python demos for the LBR ros2_control integration. mhubii MIT diff --git a/lbr_demos/lbr_demos_ros2_control_python/setup.py b/lbr_demos/lbr_demos_ros2_control_python/setup.py index 8b03c98b..31abc6c4 100644 --- a/lbr_demos/lbr_demos_ros2_control_python/setup.py +++ b/lbr_demos/lbr_demos_ros2_control_python/setup.py @@ -4,7 +4,7 @@ setup( name=package_name, - version="1.2.3", + version="1.2.4", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/lbr_description/package.xml b/lbr_description/package.xml index e11130a5..2dbe5f84 100644 --- a/lbr_description/package.xml +++ b/lbr_description/package.xml @@ -2,7 +2,7 @@ lbr_description - 1.2.3 + 1.2.4 KUKA LBR description files mhubii MIT diff --git a/lbr_fri_msgs/package.xml b/lbr_fri_msgs/package.xml index e5e33470..2a48a457 100644 --- a/lbr_fri_msgs/package.xml +++ b/lbr_fri_msgs/package.xml @@ -2,7 +2,7 @@ lbr_fri_msgs - 1.2.3 + 1.2.4 ROS 2 message for the Fast Robot Interface (FRI) specific states. mhubii MIT diff --git a/lbr_fri_ros2/package.xml b/lbr_fri_ros2/package.xml index e52eecde..6aeb656d 100644 --- a/lbr_fri_ros2/package.xml +++ b/lbr_fri_ros2/package.xml @@ -2,7 +2,7 @@ lbr_fri_ros2 - 1.2.3 + 1.2.4 The lbr_fri_ros2 package provides the Fast Robot Interface (FRI) integration into ROS 2. Robot states can be extracted and commanded. mhubii diff --git a/lbr_fri_ros2_stack/package.xml b/lbr_fri_ros2_stack/package.xml index 35d78730..a48259a7 100644 --- a/lbr_fri_ros2_stack/package.xml +++ b/lbr_fri_ros2_stack/package.xml @@ -2,7 +2,7 @@ lbr_fri_ros2_stack - 1.2.3 + 1.2.4 ROS 2 stack for KUKA LBRs. mhubii MIT diff --git a/lbr_hardware_interface/package.xml b/lbr_hardware_interface/package.xml index 43da3db7..dd7d7450 100644 --- a/lbr_hardware_interface/package.xml +++ b/lbr_hardware_interface/package.xml @@ -2,7 +2,7 @@ lbr_hardware_interface - 1.2.3 + 1.2.4 ROS 2 hardware hardware_interface for KUKA LBR through Fast Robot Interface (FRI). mhubii MIT From 5217d9c2a650e7489ee27fb8126f0be424e2ef76 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 8 Aug 2023 11:43:35 +0100 Subject: [PATCH 04/33] updated names in hw if --- .../src/lbr_hardware_interface.cpp | 26 ++++++++++--------- 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/lbr_hardware_interface/src/lbr_hardware_interface.cpp b/lbr_hardware_interface/src/lbr_hardware_interface.cpp index 3efceba5..2a616b3a 100644 --- a/lbr_hardware_interface/src/lbr_hardware_interface.cpp +++ b/lbr_hardware_interface/src/lbr_hardware_interface.cpp @@ -25,7 +25,7 @@ LBRHardwareInterface::on_init(const hardware_interface::HardwareInfo &system_inf } // setup node - lbr_node_ = std::make_shared(robot_name_ + "_node", + lbr_node_ = std::make_shared(robot_name_, rclcpp::NodeOptions().use_intra_process_comms(true)); lbr_node_->declare_parameter("robot_name", robot_name_); @@ -385,16 +385,16 @@ bool LBRHardwareInterface::spawn_com_layer_() { rclcpp::strategies::message_pool_memory_strategy::MessagePoolMemoryStrategy< lbr_fri_msgs::msg::LBRState, 1>::make_shared(); lbr_state_sub_ = lbr_node_->create_subscription( - robot_name_ + "/state", + "~/state", rclcpp::QoS(1) .deadline(std::chrono::milliseconds(sample_time_)) .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE), std::bind(&LBRHardwareInterface::on_lbr_state_, this, std::placeholders::_1), rclcpp::SubscriptionOptions(), memory_strategy); lbr_command_pub_ = lbr_node_->create_publisher( - robot_name_ + "/command", rclcpp::QoS(1) - .deadline(std::chrono::milliseconds(sample_time_)) - .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE)); + "~/command", rclcpp::QoS(1) + .deadline(std::chrono::milliseconds(sample_time_)) + .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE)); } catch (const std::exception &e) { RCLCPP_ERROR(lbr_node_->get_logger(), "Failed to spawn real time layer.\n%s.", e.what()); return false; @@ -408,7 +408,7 @@ bool LBRHardwareInterface::spawn_clients_() { return false; } - std::string app_connect_service_name = robot_name_ + "/connect"; + std::string app_connect_service_name = "~/connect"; app_connect_clt_ = lbr_node_->create_client( app_connect_service_name, rmw_qos_profile_services_default); RCLCPP_INFO(lbr_node_->get_logger(), "Waiting for service %s to spawn...", @@ -419,7 +419,7 @@ bool LBRHardwareInterface::spawn_clients_() { } RCLCPP_INFO(lbr_node_->get_logger(), "Done."); - std::string app_disconnect_service_name = robot_name_ + "/disconnect"; + std::string app_disconnect_service_name = "~/disconnect"; app_disconnect_clt_ = lbr_node_->create_client( app_disconnect_service_name, rmw_qos_profile_services_default); RCLCPP_INFO(lbr_node_->get_logger(), "Waiting for service %s to spawn...", @@ -508,11 +508,13 @@ void LBRHardwareInterface::compute_hw_velocity_() { return; } - for (uint8_t i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; ++i) { - hw_velocity_[i] = (hw_position_[i] - last_hw_position_[i]) / - (time_stamps_to_sec_(hw_time_stamp_sec_, hw_time_stamp_nano_sec_) - - time_stamps_to_sec_(last_hw_time_stamp_sec_, last_hw_time_stamp_nano_sec_)); - } + double dt = time_stamps_to_sec_(hw_time_stamp_sec_, hw_time_stamp_nano_sec_) - + time_stamps_to_sec_(last_hw_time_stamp_sec_, last_hw_time_stamp_nano_sec_); + std::size_t i = 0; + std::for_each(hw_velocity_.begin(), hw_velocity_.end(), [&](double &v) { + v = (hw_position_[i] - last_hw_position_[i]) / dt; + ++i; + }); } } // end of namespace lbr_hardware_interface From 541d217931b30668907eccc255b4ac062f42045c Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 8 Aug 2023 15:26:06 +0100 Subject: [PATCH 05/33] added initial pid version --- .../include/lbr_fri_ros2/lbr_client.hpp | 26 ++++++-- lbr_fri_ros2/src/lbr_client.cpp | 62 +++++++++++-------- 2 files changed, 56 insertions(+), 32 deletions(-) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/lbr_client.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/lbr_client.hpp index 2667b36d..5b7a202a 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/lbr_client.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/lbr_client.hpp @@ -1,13 +1,14 @@ #ifndef LBR_FRI_ROS2__LBR_CLIENT_HPP_ #define LBR_FRI_ROS2__LBR_CLIENT_HPP_ +#include #include #include #include #include #include -#include "control_toolbox/filters.hpp" +#include "control_toolbox/pid_ros.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp/strategies/message_pool_memory_strategy.hpp" @@ -114,6 +115,17 @@ class LBRClient : public KUKA::FRI::LBRClient { */ void on_lbr_command_(const lbr_fri_msgs::msg::LBRCommand::SharedPtr lbr_command); + /** + * @brief Calculate the command for the robot using the current state and the desired command. + * + * @param[in] lbr_command_target The desired command + * @param[in] lbr_state The current state + * @param[out] lbr_command The command to be written to the robot + */ + void lbr_command_pid_(const lbr_fri_msgs::msg::LBRCommand &lbr_command_target, + const lbr_fri_msgs::msg::LBRState &lbr_state, + lbr_fri_msgs::msg::LBRCommand &lbr_command); + rclcpp::Node::SharedPtr node_; /**< Shared pointer to node.*/ std::unique_ptr lbr_command_guard_; /**< Validating commands prior to writing them to #robotCommand.*/ @@ -123,12 +135,14 @@ class LBRClient : public KUKA::FRI::LBRClient { uint32_t missed_deadlines_sub_; /**< Counter for commands that were not received within specified deadline.*/ - lbr_fri_msgs::msg::LBRCommand lbr_command_; /**< Command buffer.*/ - lbr_fri_msgs::msg::LBRState lbr_state_; /**< State buffer.*/ + lbr_fri_msgs::msg::LBRCommand lbr_command_target_; /**< Desired command buffer.*/ + lbr_fri_msgs::msg::LBRCommand lbr_command_; /**< Command buffer.*/ + lbr_fri_msgs::msg::LBRState lbr_state_; /**< State buffer.*/ + + std::array + position_pid_controllers_; /**< Array of PidROS controllers for each joint.*/ - std::string robot_name_; /**< Name of the robot.*/ - double smoothing_; /**< Exponential smoothing factor for position commands.*/ - bool open_loop_; /**< Flag for open loop control. Best way to command the LBRs.*/ + bool open_loop_; /**< Open loop control if true. Best way to command the LBRs.*/ rclcpp::Publisher::SharedPtr lbr_state_pub_; /**< Publisher of lbr_fri_msgs::msg::LBRState.*/ diff --git a/lbr_fri_ros2/src/lbr_client.cpp b/lbr_fri_ros2/src/lbr_client.cpp index 2bfc52ae..1f5c84d9 100644 --- a/lbr_fri_ros2/src/lbr_client.cpp +++ b/lbr_fri_ros2/src/lbr_client.cpp @@ -3,7 +3,14 @@ namespace lbr_fri_ros2 { LBRClient::LBRClient(const rclcpp::Node::SharedPtr node, std::unique_ptr lbr_command_guard) - : node_(node), lbr_command_guard_(std::move(lbr_command_guard)) { + : node_(node), lbr_command_guard_(std::move(lbr_command_guard)), + position_pid_controllers_( + {control_toolbox::PidROS{node, "A1"}, control_toolbox::PidROS{node, "A2"}, + control_toolbox::PidROS{node, "A3"}, control_toolbox::PidROS{node, "A4"}, + control_toolbox::PidROS{node, "A5"}, control_toolbox::PidROS{node, "A6"}, + control_toolbox::PidROS{node, "A7"}}) { + std::for_each(position_pid_controllers_.begin(), position_pid_controllers_.end(), + [](auto &pid) { pid.initPid(0.02, 0.0, 0.0, 0.0, 0.0, false); }); missed_deadlines_pub_ = 0; missed_deadlines_sub_ = 0; @@ -47,6 +54,9 @@ void LBRClient::waitForCommand() { void LBRClient::command() { pub_lbr_state_(); + // compute command + lbr_command_pid_(lbr_command_target_, lbr_state_, lbr_command_); + // validate command if (!lbr_command_guard_->is_valid_command(lbr_command_, robotState())) { RCLCPP_ERROR(node_->get_logger(), "Invalid command received. Triggering disconnect."); @@ -72,10 +82,11 @@ void LBRClient::command() { } void LBRClient::init_lbr_command_() { - std::memcpy(lbr_command_.joint_position.data(), robotState().getMeasuredJointPosition(), + std::memcpy(lbr_command_target_.joint_position.data(), robotState().getMeasuredJointPosition(), sizeof(double) * KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - lbr_command_.torque.fill(0.); - lbr_command_.wrench.fill(0.); + lbr_command_target_.torque.fill(0.); + lbr_command_target_.wrench.fill(0.); + lbr_command_ = lbr_command_target_; } void LBRClient::init_topics_() { @@ -86,10 +97,10 @@ void LBRClient::init_topics_() { }; lbr_state_pub_ = node_->create_publisher( - robot_name_ + "/state", rclcpp::QoS(1) - .deadline(std::chrono::milliseconds( - static_cast(robotState().getSampleTime() * 1e3))) - .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE)); + "~/state", rclcpp::QoS(1) + .deadline(std::chrono::milliseconds( + static_cast(robotState().getSampleTime() * 1e3))) + .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE)); } if (!lbr_command_sub_) { @@ -103,7 +114,7 @@ void LBRClient::init_topics_() { }; lbr_command_sub_ = node_->create_subscription( - robot_name_ + "/command", + "~/command", rclcpp::QoS(1) .deadline( std::chrono::milliseconds(static_cast(robotState().getSampleTime() * 1e3))) @@ -114,22 +125,12 @@ void LBRClient::init_topics_() { } void LBRClient::declare_parameters_() { - if (!node_->has_parameter("robot_name")) { - node_->declare_parameter("robot_name", "lbr"); - } - if (!node_->has_parameter("smoothing")) { - node_->declare_parameter("smoothing", 0.99); - } if (!node_->has_parameter("open_loop")) { node_->declare_parameter("open_loop", true); } } -void LBRClient::get_parameters_() { - robot_name_ = node_->get_parameter("robot_name").as_string(); - smoothing_ = node_->get_parameter("smoothing").as_double(); - open_loop_ = node_->get_parameter("open_loop").as_bool(); -} +void LBRClient::get_parameters_() { open_loop_ = node_->get_parameter("open_loop").as_bool(); } void LBRClient::pub_lbr_state_() { lbr_state_.client_command_mode = robotState().getClientCommandMode(); @@ -170,11 +171,20 @@ void LBRClient::pub_lbr_state_() { } void LBRClient::on_lbr_command_(const lbr_fri_msgs::msg::LBRCommand::SharedPtr lbr_command) { - for (int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; ++i) { - lbr_command_.joint_position[i] = filters::exponentialSmoothing( - lbr_command_.joint_position[i], lbr_command->joint_position[i], smoothing_); - } - lbr_command_.torque = lbr_command->torque; - lbr_command_.wrench = lbr_command->wrench; + lbr_command_target_ = *lbr_command; +} + +void LBRClient::lbr_command_pid_(const lbr_fri_msgs::msg::LBRCommand &lbr_command_target, + const lbr_fri_msgs::msg::LBRState &lbr_state, + lbr_fri_msgs::msg::LBRCommand &lbr_command) { + int i = 0; + std::for_each(lbr_command.joint_position.begin(), lbr_command.joint_position.end(), + [&](double &joint_position) { + joint_position += position_pid_controllers_[i].computeCommand( + lbr_command_target.joint_position[i] - lbr_state.measured_joint_position[i], + rclcpp::Duration(std::chrono::milliseconds( + static_cast(robotState().getSampleTime() * 1e3)))); + ++i; + }); } } // end of namespace lbr_fri_ros2 From 234bd305ddf5bffc30696c9ea72ad8c36ad00a91 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 8 Aug 2023 19:36:03 +0100 Subject: [PATCH 06/33] removed include --- .../include/lbr_hardware_interface/lbr_hardware_interface.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/lbr_hardware_interface/include/lbr_hardware_interface/lbr_hardware_interface.hpp b/lbr_hardware_interface/include/lbr_hardware_interface/lbr_hardware_interface.hpp index 587df770..ee1c9c9c 100644 --- a/lbr_hardware_interface/include/lbr_hardware_interface/lbr_hardware_interface.hpp +++ b/lbr_hardware_interface/include/lbr_hardware_interface/lbr_hardware_interface.hpp @@ -1,6 +1,7 @@ #ifndef LBR_HARDWARE_INTERFACE__LBR_HARDWARE_INTERFACE_HPP_ #define LBR_HARDWARE_INTERFACE__LBR_HARDWARE_INTERFACE_HPP_ +#include #include #include #include From b45816a61b9c9edd896f8c1764c6ba56e1c65b2a Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 8 Aug 2023 19:36:14 +0100 Subject: [PATCH 07/33] added static executor --- lbr_hardware_interface/src/lbr_hardware_interface.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/lbr_hardware_interface/src/lbr_hardware_interface.cpp b/lbr_hardware_interface/src/lbr_hardware_interface.cpp index 2a616b3a..e8abea9b 100644 --- a/lbr_hardware_interface/src/lbr_hardware_interface.cpp +++ b/lbr_hardware_interface/src/lbr_hardware_interface.cpp @@ -30,7 +30,6 @@ LBRHardwareInterface::on_init(const hardware_interface::HardwareInfo &system_inf lbr_node_->declare_parameter("robot_name", robot_name_); lbr_node_->declare_parameter("command_guard_variant", "default"); - lbr_node_->declare_parameter("smoothing", 0.8); lbr_app_ = std::make_unique(lbr_node_); @@ -63,7 +62,7 @@ LBRHardwareInterface::on_init(const hardware_interface::HardwareInfo &system_inf } node_thread_ = std::make_unique([this]() { - auto executor = std::make_shared(); + auto executor = std::make_shared(); executor->add_node(lbr_node_); executor->spin(); disconnect_(); From 2f24039dfa1893a140865df417e1d43eaa28d4db Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 8 Aug 2023 19:36:25 +0100 Subject: [PATCH 08/33] changed rate to 100 --- lbr_hardware_interface/config/lbr_controllers.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lbr_hardware_interface/config/lbr_controllers.yml b/lbr_hardware_interface/config/lbr_controllers.yml index 3b57d72a..81a3dd85 100644 --- a/lbr_hardware_interface/config/lbr_controllers.yml +++ b/lbr_hardware_interface/config/lbr_controllers.yml @@ -1,6 +1,6 @@ controller_manager: ros__parameters: - update_rate: 200 + update_rate: 100 joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster From de91e783085da814246af1457703069c4f515fe3 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 8 Aug 2023 19:37:39 +0100 Subject: [PATCH 09/33] added utility draft --- lbr_fri_ros2/CMakeLists.txt | 1 + .../include/lbr_fri_ros2/lbr_utils.hpp | 110 ++++++++++++++++++ lbr_fri_ros2/src/lbr_utils.cpp | 92 +++++++++++++++ 3 files changed, 203 insertions(+) create mode 100644 lbr_fri_ros2/include/lbr_fri_ros2/lbr_utils.hpp create mode 100644 lbr_fri_ros2/src/lbr_utils.cpp diff --git a/lbr_fri_ros2/CMakeLists.txt b/lbr_fri_ros2/CMakeLists.txt index f4d2ed7a..555d7e7b 100644 --- a/lbr_fri_ros2/CMakeLists.txt +++ b/lbr_fri_ros2/CMakeLists.txt @@ -23,6 +23,7 @@ add_library(lbr_fri_ros2 src/lbr_app.cpp src/lbr_client.cpp src/lbr_command_guard.cpp + src/lbr_utils.cpp ) target_include_directories(lbr_fri_ros2 diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/lbr_utils.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/lbr_utils.hpp new file mode 100644 index 00000000..28eeb34a --- /dev/null +++ b/lbr_fri_ros2/include/lbr_fri_ros2/lbr_utils.hpp @@ -0,0 +1,110 @@ +#ifndef LBR_FRI_ROS2__LBR_UTILS_HPP_ +#define LBR_FRI_ROS2__LBR_UTILS_HPP_ + +#include +#include +#include +#include + +#include "control_toolbox/filters.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "friLBRClient.h" + +#include "lbr_fri_msgs/msg/lbr_state.hpp" + +namespace lbr_fri_ros2 { +class ExponentialFilter { +public: + /** + * @brief Construct a new Exponential Smooth object + * + */ + ExponentialFilter(); + + /** + * @brief Construct a new Exponential Smooth object + * + * @param[in] cutoff_frequency + * @param[in] sample_time + */ + ExponentialFilter(const std::uint16_t &cutoff_frequency, const double &sample_time); + + /** + * @brief + * + * @param[in] current + * @param[in] previous + * @return double + */ + inline double compute(const double ¤t, const double &previous); + + /** + * @brief Set the cutoff frequency object + * + * @param[in] cutoff_frequency + * @param[in] sample_time + */ + void set_cutoff_frequency(const std::uint16_t &cutoff_frequency, const double &sample_time); + + inline const double &get_sample_time() const; + inline const double &get_smoothing_factor() const; + +protected: + /** + * @brief + * https://dsp.stackexchange.com/questions/40462/exponential-moving-average-cut-off-frequency + * + * @param[in] cutoff_frequency + * @param[in] sample_time + * @return double + */ + double compute_smoothing_factor(const std::uint16_t &cutoff_frequency, const double &sample_time); + + /** + * @brief + * + * @param[in] smoothing_factor + * @return true + * @return false + */ + bool validate_smoothing_factor(const double &smoothing_factor); + + std::uint16_t cutoff_frequency_; + double sample_time_; + double smoothing_factor_; +}; + +class LBRFilter { + using JointArrayType = lbr_fri_msgs::msg::LBRState::_measured_joint_position_type; + +public: + LBRFilter() = delete; + LBRFilter(const rclcpp::Node::SharedPtr node); + LBRFilter(const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface, + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameter_interface); + + /** + * @brief + * + * @param[in] current + * @param[in, out] previous + */ + void compute(const double *const current, JointArrayType &previous); + + /** + * @brief + * + * @param[in] cutoff_frequency + * @param[in] sample_time + */ + void init(const std::uint16_t &cutoff_frequency, const double &sample_time); + +protected: + ExponentialFilter exponential_filter_; + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_; + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameter_interface_; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_; +}; +} // end of namespace lbr_fri_ros2 +#endif // LBR_FRI_ROS2__LBR_UTILS_HPP_ diff --git a/lbr_fri_ros2/src/lbr_utils.cpp b/lbr_fri_ros2/src/lbr_utils.cpp new file mode 100644 index 00000000..65101d61 --- /dev/null +++ b/lbr_fri_ros2/src/lbr_utils.cpp @@ -0,0 +1,92 @@ +#include "lbr_fri_ros2/lbr_utils.hpp" + +namespace lbr_fri_ros2 { + +ExponentialFilter::ExponentialFilter() : ExponentialFilter::ExponentialFilter(0, 0.0) {} + +ExponentialFilter::ExponentialFilter(const std::uint16_t &cutoff_frequency, + const double &sample_time) { + set_cutoff_frequency(cutoff_frequency, sample_time); +} + +inline double ExponentialFilter::compute(const double ¤t, const double &previous) { + return filters::exponentialSmoothing(current, previous, smoothing_factor_); +} + +void ExponentialFilter::set_cutoff_frequency(const std::uint16_t &cutoff_frequency, + const double &sample_time) { + cutoff_frequency_ = cutoff_frequency; + if (cutoff_frequency_ > static_cast(1. / sample_time)) { + cutoff_frequency_ = static_cast(1. / sample_time); + } + sample_time_ = sample_time; + smoothing_factor_ = compute_smoothing_factor(cutoff_frequency, sample_time); + if (!validate_smoothing_factor(smoothing_factor_)) { + throw std::runtime_error("Smoothing factor is not within [0, 1]."); + } +} + +inline const double &ExponentialFilter::get_sample_time() const { return sample_time_; } + +inline const double &ExponentialFilter::get_smoothing_factor() const { return smoothing_factor_; } + +double ExponentialFilter::compute_smoothing_factor(const std::uint16_t &cutoff_frequency, + const double &sample_time) { + double omega_3db = 2.0 * M_PI * sample_time * cutoff_frequency; + return std::cos(omega_3db) - 1 + + std::sqrt(std::pow(std::cos(omega_3db), 2) - 4 * std::cos(omega_3db) + 3); +} + +bool ExponentialFilter::validate_smoothing_factor(const double &smoothing_factor) { + return smoothing_factor <= 1. && smoothing_factor >= 0.; +} + +LBRFilter::LBRFilter(const rclcpp::Node::SharedPtr node) + : LBRFilter(node->get_node_logging_interface(), node->get_node_parameters_interface()) {} + +LBRFilter::LBRFilter( + const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface, + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameter_interface) + : logging_interface_(logging_interface), parameter_interface_(parameter_interface) {} + +void LBRFilter::compute(const double *const current, JointArrayType &previous) { + int i = 0; + std::for_each(current, current + KUKA::FRI::LBRState::NUMBER_OF_JOINTS, + [&](const auto ¤t_i) { + previous[i] = exponential_filter_.compute(current_i, previous[i]); + ++i; + }); +} + +void LBRFilter::init(const std::uint16_t &cutoff_frequency, const double &sample_time) { + if (!parameter_interface_->has_parameter("cutoff_frequency")) { + parameter_interface_->declare_parameter("cutoff_frequency", + rclcpp::ParameterValue(cutoff_frequency)); + } + exponential_filter_.set_cutoff_frequency(cutoff_frequency, sample_time); + + parameter_callback_handle_ = parameter_interface_->add_on_set_parameters_callback( + [this](const std::vector ¶meters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + for (const auto ¶meter : parameters) { + try { + if (parameter.get_name() == "cutoff_frequency") { + exponential_filter_.set_cutoff_frequency(parameter.as_int(), + exponential_filter_.get_sample_time()); + RCLCPP_INFO(logging_interface_->get_logger(), + "Set %s to: %ld, new smoothing factor: %f.", parameter.get_name().c_str(), + parameter.as_int(), exponential_filter_.get_smoothing_factor()); + } + } catch (const rclcpp::exceptions::InvalidParameterTypeException &e) { + std::string info_msg = "Invalid parameter type: " + std::string(e.what()); + RCLCPP_INFO(logging_interface_->get_logger(), info_msg.c_str()); + result.reason = info_msg; + result.successful = false; + } + } + return result; + }); +} + +} // end of namespace lbr_fri_ros2 From 6b65b71b236fc4a97cfe93ef82b6b2b0272513f7 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 8 Aug 2023 19:57:33 +0100 Subject: [PATCH 10/33] added a prefix --- .../include/lbr_fri_ros2/lbr_utils.hpp | 6 ++++-- lbr_fri_ros2/src/lbr_utils.cpp | 21 ++++++++++++------- 2 files changed, 18 insertions(+), 9 deletions(-) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/lbr_utils.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/lbr_utils.hpp index 28eeb34a..8068167b 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/lbr_utils.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/lbr_utils.hpp @@ -80,9 +80,10 @@ class LBRFilter { public: LBRFilter() = delete; - LBRFilter(const rclcpp::Node::SharedPtr node); + LBRFilter(const rclcpp::Node::SharedPtr node, const std::string ¶m_prefix = ""); LBRFilter(const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface, - const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameter_interface); + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameter_interface, + const std::string ¶m_prefix = ""); /** * @brief @@ -104,6 +105,7 @@ class LBRFilter { ExponentialFilter exponential_filter_; rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_; rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameter_interface_; + std::string param_prefix_; rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_; }; } // end of namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/lbr_utils.cpp b/lbr_fri_ros2/src/lbr_utils.cpp index 65101d61..3a61d528 100644 --- a/lbr_fri_ros2/src/lbr_utils.cpp +++ b/lbr_fri_ros2/src/lbr_utils.cpp @@ -41,13 +41,20 @@ bool ExponentialFilter::validate_smoothing_factor(const double &smoothing_factor return smoothing_factor <= 1. && smoothing_factor >= 0.; } -LBRFilter::LBRFilter(const rclcpp::Node::SharedPtr node) - : LBRFilter(node->get_node_logging_interface(), node->get_node_parameters_interface()) {} +LBRFilter::LBRFilter(const rclcpp::Node::SharedPtr node, const std::string ¶m_prefix) + : LBRFilter(node->get_node_logging_interface(), node->get_node_parameters_interface(), + param_prefix) {} LBRFilter::LBRFilter( const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface, - const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameter_interface) - : logging_interface_(logging_interface), parameter_interface_(parameter_interface) {} + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameter_interface, + const std::string ¶m_prefix) + : logging_interface_(logging_interface), parameter_interface_(parameter_interface), + param_prefix_(param_prefix) { + if (!param_prefix_.empty()) { + param_prefix_ += "."; + } +} void LBRFilter::compute(const double *const current, JointArrayType &previous) { int i = 0; @@ -59,8 +66,8 @@ void LBRFilter::compute(const double *const current, JointArrayType &previous) { } void LBRFilter::init(const std::uint16_t &cutoff_frequency, const double &sample_time) { - if (!parameter_interface_->has_parameter("cutoff_frequency")) { - parameter_interface_->declare_parameter("cutoff_frequency", + if (!parameter_interface_->has_parameter(param_prefix_ + "cutoff_frequency")) { + parameter_interface_->declare_parameter(param_prefix_ + "cutoff_frequency", rclcpp::ParameterValue(cutoff_frequency)); } exponential_filter_.set_cutoff_frequency(cutoff_frequency, sample_time); @@ -71,7 +78,7 @@ void LBRFilter::init(const std::uint16_t &cutoff_frequency, const double &sample result.successful = true; for (const auto ¶meter : parameters) { try { - if (parameter.get_name() == "cutoff_frequency") { + if (parameter.get_name() == param_prefix_ + "cutoff_frequency") { exponential_filter_.set_cutoff_frequency(parameter.as_int(), exponential_filter_.get_sample_time()); RCLCPP_INFO(logging_interface_->get_logger(), From 44147ecd6151496afc3b8c1b38fbc6e560d128d6 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 9 Aug 2023 10:03:07 +0100 Subject: [PATCH 11/33] finished utils draft --- .../include/lbr_fri_ros2/lbr_utils.hpp | 112 ------------------ lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp | 85 +++++++++++++ lbr_fri_ros2/src/{lbr_utils.cpp => utils.cpp} | 44 +++++-- 3 files changed, 122 insertions(+), 119 deletions(-) delete mode 100644 lbr_fri_ros2/include/lbr_fri_ros2/lbr_utils.hpp create mode 100644 lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp rename lbr_fri_ros2/src/{lbr_utils.cpp => utils.cpp} (65%) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/lbr_utils.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/lbr_utils.hpp deleted file mode 100644 index 8068167b..00000000 --- a/lbr_fri_ros2/include/lbr_fri_ros2/lbr_utils.hpp +++ /dev/null @@ -1,112 +0,0 @@ -#ifndef LBR_FRI_ROS2__LBR_UTILS_HPP_ -#define LBR_FRI_ROS2__LBR_UTILS_HPP_ - -#include -#include -#include -#include - -#include "control_toolbox/filters.hpp" -#include "rclcpp/rclcpp.hpp" - -#include "friLBRClient.h" - -#include "lbr_fri_msgs/msg/lbr_state.hpp" - -namespace lbr_fri_ros2 { -class ExponentialFilter { -public: - /** - * @brief Construct a new Exponential Smooth object - * - */ - ExponentialFilter(); - - /** - * @brief Construct a new Exponential Smooth object - * - * @param[in] cutoff_frequency - * @param[in] sample_time - */ - ExponentialFilter(const std::uint16_t &cutoff_frequency, const double &sample_time); - - /** - * @brief - * - * @param[in] current - * @param[in] previous - * @return double - */ - inline double compute(const double ¤t, const double &previous); - - /** - * @brief Set the cutoff frequency object - * - * @param[in] cutoff_frequency - * @param[in] sample_time - */ - void set_cutoff_frequency(const std::uint16_t &cutoff_frequency, const double &sample_time); - - inline const double &get_sample_time() const; - inline const double &get_smoothing_factor() const; - -protected: - /** - * @brief - * https://dsp.stackexchange.com/questions/40462/exponential-moving-average-cut-off-frequency - * - * @param[in] cutoff_frequency - * @param[in] sample_time - * @return double - */ - double compute_smoothing_factor(const std::uint16_t &cutoff_frequency, const double &sample_time); - - /** - * @brief - * - * @param[in] smoothing_factor - * @return true - * @return false - */ - bool validate_smoothing_factor(const double &smoothing_factor); - - std::uint16_t cutoff_frequency_; - double sample_time_; - double smoothing_factor_; -}; - -class LBRFilter { - using JointArrayType = lbr_fri_msgs::msg::LBRState::_measured_joint_position_type; - -public: - LBRFilter() = delete; - LBRFilter(const rclcpp::Node::SharedPtr node, const std::string ¶m_prefix = ""); - LBRFilter(const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface, - const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameter_interface, - const std::string ¶m_prefix = ""); - - /** - * @brief - * - * @param[in] current - * @param[in, out] previous - */ - void compute(const double *const current, JointArrayType &previous); - - /** - * @brief - * - * @param[in] cutoff_frequency - * @param[in] sample_time - */ - void init(const std::uint16_t &cutoff_frequency, const double &sample_time); - -protected: - ExponentialFilter exponential_filter_; - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_; - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameter_interface_; - std::string param_prefix_; - rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_; -}; -} // end of namespace lbr_fri_ros2 -#endif // LBR_FRI_ROS2__LBR_UTILS_HPP_ 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..08f712f1 --- /dev/null +++ b/lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp @@ -0,0 +1,85 @@ +#ifndef LBR_FRI_ROS2__LBR_UTILS_HPP_ +#define LBR_FRI_ROS2__LBR_UTILS_HPP_ + +#include +#include +#include +#include +#include + +#include "control_toolbox/filters.hpp" +#include "control_toolbox/pid_ros.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "friLBRClient.h" + +#include "lbr_fri_msgs/msg/lbr_command.hpp" +#include "lbr_fri_msgs/msg/lbr_state.hpp" + +namespace lbr_fri_ros2 { +class ExponentialFilter { +public: + ExponentialFilter(); + ExponentialFilter(const std::uint16_t &cutoff_frequency, const double &sample_time); + + inline double compute(const double ¤t, const double &previous); + void set_cutoff_frequency(const std::uint16_t &cutoff_frequency, const double &sample_time); + inline const double &get_sample_time() const; + inline const double &get_smoothing_factor() const; + +protected: + // https://dsp.stackexchange.com/questions/40462/exponential-moving-average-cut-off-frequency + double compute_smoothing_factor(const std::uint16_t &cutoff_frequency, const double &sample_time); + bool validate_smoothing_factor(const double &smoothing_factor); + + std::uint16_t cutoff_frequency_; + double sample_time_; + double smoothing_factor_; +}; + +class JointExponentialFilterArrayROS { + using ValueArrayType = std::array; + +public: + JointExponentialFilterArrayROS() = delete; + JointExponentialFilterArrayROS(const rclcpp::Node::SharedPtr node, + const std::string ¶m_prefix = ""); + JointExponentialFilterArrayROS( + const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface, + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameter_interface, + const std::string ¶m_prefix = ""); + + void compute(const double *const current, ValueArrayType &previous); + void init(const std::uint16_t &cutoff_frequency, const double &sample_time); + +protected: + ExponentialFilter exponential_filter_; + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_; + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameter_interface_; + std::string param_prefix_; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_; +}; + +class JointPIDArrayROS { + using ValueArrayType = std::array; + using NameArrayType = std::array; + using PIDArrayType = std::array; + +public: + JointPIDArrayROS() = delete; + JointPIDArrayROS(const rclcpp::Node::SharedPtr node, const NameArrayType &names, + const std::string &prefix = ""); + + void compute(const ValueArrayType &command_target, const ValueArrayType &state, + const rclcpp::Duration &dt, ValueArrayType &command); + + void init(const double &p, const double &i, const double &d, const double &i_max, + const double &i_min, const bool &antiwindup); + +protected: + PIDArrayType pid_controllers_; +}; + +} // end of namespace lbr_fri_ros2 +#endif // LBR_FRI_ROS2__LBR_UTILS_HPP_ diff --git a/lbr_fri_ros2/src/lbr_utils.cpp b/lbr_fri_ros2/src/utils.cpp similarity index 65% rename from lbr_fri_ros2/src/lbr_utils.cpp rename to lbr_fri_ros2/src/utils.cpp index 3a61d528..e30a399e 100644 --- a/lbr_fri_ros2/src/lbr_utils.cpp +++ b/lbr_fri_ros2/src/utils.cpp @@ -1,4 +1,4 @@ -#include "lbr_fri_ros2/lbr_utils.hpp" +#include "lbr_fri_ros2/utils.hpp" namespace lbr_fri_ros2 { @@ -41,11 +41,12 @@ bool ExponentialFilter::validate_smoothing_factor(const double &smoothing_factor return smoothing_factor <= 1. && smoothing_factor >= 0.; } -LBRFilter::LBRFilter(const rclcpp::Node::SharedPtr node, const std::string ¶m_prefix) - : LBRFilter(node->get_node_logging_interface(), node->get_node_parameters_interface(), - param_prefix) {} +JointExponentialFilterArrayROS::JointExponentialFilterArrayROS(const rclcpp::Node::SharedPtr node, + const std::string ¶m_prefix) + : JointExponentialFilterArrayROS(node->get_node_logging_interface(), + node->get_node_parameters_interface(), param_prefix) {} -LBRFilter::LBRFilter( +JointExponentialFilterArrayROS::JointExponentialFilterArrayROS( const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface, const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameter_interface, const std::string ¶m_prefix) @@ -56,7 +57,8 @@ LBRFilter::LBRFilter( } } -void LBRFilter::compute(const double *const current, JointArrayType &previous) { +void JointExponentialFilterArrayROS::compute(const double *const current, + ValueArrayType &previous) { int i = 0; std::for_each(current, current + KUKA::FRI::LBRState::NUMBER_OF_JOINTS, [&](const auto ¤t_i) { @@ -65,7 +67,8 @@ void LBRFilter::compute(const double *const current, JointArrayType &previous) { }); } -void LBRFilter::init(const std::uint16_t &cutoff_frequency, const double &sample_time) { +void JointExponentialFilterArrayROS::init(const std::uint16_t &cutoff_frequency, + const double &sample_time) { if (!parameter_interface_->has_parameter(param_prefix_ + "cutoff_frequency")) { parameter_interface_->declare_parameter(param_prefix_ + "cutoff_frequency", rclcpp::ParameterValue(cutoff_frequency)); @@ -96,4 +99,31 @@ void LBRFilter::init(const std::uint16_t &cutoff_frequency, const double &sample }); } +JointPIDArrayROS::JointPIDArrayROS(const rclcpp::Node::SharedPtr node, const NameArrayType &names, + const std::string &prefix) + : pid_controllers_(PIDArrayType{ + control_toolbox::PidROS{node, prefix + names[0]}, + control_toolbox::PidROS{node, prefix + names[1]}, + control_toolbox::PidROS{node, prefix + names[2]}, + control_toolbox::PidROS{node, prefix + names[3]}, + control_toolbox::PidROS{node, prefix + names[4]}, + control_toolbox::PidROS{node, prefix + names[5]}, + control_toolbox::PidROS{node, prefix + names[6]}, + }) {} + +void JointPIDArrayROS::compute(const ValueArrayType &command_target, const ValueArrayType &state, + const rclcpp::Duration &dt, ValueArrayType &command) { + int i = 0; + std::for_each(command.begin(), command.end(), [&](double &command_i) { + command_i += pid_controllers_[i].computeCommand(command_target[i] - state[i], dt); + ++i; + }); +} + +void JointPIDArrayROS::init(const double &p, const double &i, const double &d, const double &i_max, + const double &i_min, const bool &antiwindup) { + std::for_each(pid_controllers_.begin(), pid_controllers_.end(), + [&](auto &pid) { pid.initPid(p, i, d, i_max, i_min, antiwindup); }); +} + } // end of namespace lbr_fri_ros2 From 38230efb586e491e544c358fdcb1d3dee70f3fef Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 9 Aug 2023 10:03:19 +0100 Subject: [PATCH 12/33] finished utils draft --- lbr_fri_ros2/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lbr_fri_ros2/CMakeLists.txt b/lbr_fri_ros2/CMakeLists.txt index 555d7e7b..72d97d20 100644 --- a/lbr_fri_ros2/CMakeLists.txt +++ b/lbr_fri_ros2/CMakeLists.txt @@ -23,7 +23,7 @@ add_library(lbr_fri_ros2 src/lbr_app.cpp src/lbr_client.cpp src/lbr_command_guard.cpp - src/lbr_utils.cpp + src/utils.cpp ) target_include_directories(lbr_fri_ros2 From cd00107a3944df20eac3df3aca70da01ee1f209b Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 9 Aug 2023 10:09:25 +0100 Subject: [PATCH 13/33] added filters and pid to client --- .../include/lbr_fri_ros2/lbr_client.hpp | 35 +++--- lbr_fri_ros2/src/lbr_client.cpp | 113 +++++++++--------- 2 files changed, 78 insertions(+), 70 deletions(-) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/lbr_client.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/lbr_client.hpp index 5b7a202a..8232cf73 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/lbr_client.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/lbr_client.hpp @@ -17,6 +17,7 @@ #include "lbr_fri_msgs/msg/lbr_command.hpp" #include "lbr_fri_msgs/msg/lbr_state.hpp" #include "lbr_fri_ros2/lbr_command_guard.hpp" +#include "lbr_fri_ros2/utils.hpp" namespace lbr_fri_ros2 { /** @@ -89,6 +90,14 @@ class LBRClient : public KUKA::FRI::LBRClient { */ void init_topics_(); + /** + * @brief Initialize filters, that is #external_torque_filter_, #measured_torque_filter_ and + * #joint_position_pid_. This method may only be called after #robotState is accessible since + * robots's sample time is required. + * + */ + void init_filters_(); + /** * @brief Declare parameters for #node_ that are utilized within LBRClient. * @@ -115,21 +124,22 @@ class LBRClient : public KUKA::FRI::LBRClient { */ void on_lbr_command_(const lbr_fri_msgs::msg::LBRCommand::SharedPtr lbr_command); - /** - * @brief Calculate the command for the robot using the current state and the desired command. - * - * @param[in] lbr_command_target The desired command - * @param[in] lbr_state The current state - * @param[out] lbr_command The command to be written to the robot - */ - void lbr_command_pid_(const lbr_fri_msgs::msg::LBRCommand &lbr_command_target, - const lbr_fri_msgs::msg::LBRState &lbr_state, - lbr_fri_msgs::msg::LBRCommand &lbr_command); - rclcpp::Node::SharedPtr node_; /**< Shared pointer to node.*/ std::unique_ptr lbr_command_guard_; /**< Validating commands prior to writing them to #robotCommand.*/ + JointExponentialFilterArrayROS + external_torque_filter_; /**< JointExponentialFilterArrayROS for smoothing external torques.*/ + JointExponentialFilterArrayROS + measured_torque_filter_; /**< JointExponentialFilterArrayROS for smoothing measured torques.*/ + JointPIDArrayROS + joint_position_pid_; /**< JointPIDArrayROS for smoothing joint positions commands.*/ + + bool topics_init_; /**< Indicates whether topics were initialized. Necessary since #robotState + sample time required.*/ + bool filters_init_; /**< Indicates whether filters were initialized. Necessary since #robotState + sample time required.*/ + uint32_t missed_deadlines_pub_; /**< Counter for states that were not published within specified deadline.*/ uint32_t missed_deadlines_sub_; /**< Counter for commands that were not received within specified @@ -139,9 +149,6 @@ class LBRClient : public KUKA::FRI::LBRClient { lbr_fri_msgs::msg::LBRCommand lbr_command_; /**< Command buffer.*/ lbr_fri_msgs::msg::LBRState lbr_state_; /**< State buffer.*/ - std::array - position_pid_controllers_; /**< Array of PidROS controllers for each joint.*/ - bool open_loop_; /**< Open loop control if true. Best way to command the LBRs.*/ rclcpp::Publisher::SharedPtr diff --git a/lbr_fri_ros2/src/lbr_client.cpp b/lbr_fri_ros2/src/lbr_client.cpp index 1f5c84d9..6ef05b30 100644 --- a/lbr_fri_ros2/src/lbr_client.cpp +++ b/lbr_fri_ros2/src/lbr_client.cpp @@ -4,14 +4,10 @@ namespace lbr_fri_ros2 { LBRClient::LBRClient(const rclcpp::Node::SharedPtr node, std::unique_ptr lbr_command_guard) : node_(node), lbr_command_guard_(std::move(lbr_command_guard)), - position_pid_controllers_( - {control_toolbox::PidROS{node, "A1"}, control_toolbox::PidROS{node, "A2"}, - control_toolbox::PidROS{node, "A3"}, control_toolbox::PidROS{node, "A4"}, - control_toolbox::PidROS{node, "A5"}, control_toolbox::PidROS{node, "A6"}, - control_toolbox::PidROS{node, "A7"}}) { - std::for_each(position_pid_controllers_.begin(), position_pid_controllers_.end(), - [](auto &pid) { pid.initPid(0.02, 0.0, 0.0, 0.0, 0.0, false); }); - + external_torque_filter_(node, "external_torque"), + measured_torque_filter_(node, "measured_torque"), + joint_position_pid_(node, {"A1", "A2", "A3", "A4", "A5", "A6", "A7"}), topics_init_(false), + filters_init_(false) { missed_deadlines_pub_ = 0; missed_deadlines_sub_ = 0; @@ -29,6 +25,7 @@ void LBRClient::log_status() { void LBRClient::onStateChange(KUKA::FRI::ESessionState old_state, KUKA::FRI::ESessionState new_state) { init_topics_(); + init_filters_(); RCLCPP_INFO(node_->get_logger(), "LBR switched from %s to %s.", KUKA_FRI_STATE_MAP[old_state].c_str(), KUKA_FRI_STATE_MAP[new_state].c_str()); } @@ -55,7 +52,11 @@ void LBRClient::command() { pub_lbr_state_(); // compute command - lbr_command_pid_(lbr_command_target_, lbr_state_, lbr_command_); + joint_position_pid_.compute(lbr_command_target_.joint_position, + lbr_state_.measured_joint_position, + rclcpp::Duration(std::chrono::milliseconds( + static_cast(robotState().getSampleTime() * 1e3))), + lbr_command_.joint_position); // validate command if (!lbr_command_guard_->is_valid_command(lbr_command_, robotState())) { @@ -90,38 +91,54 @@ void LBRClient::init_lbr_command_() { } void LBRClient::init_topics_() { - if (!lbr_state_pub_) { - auto pub_options = rclcpp::PublisherOptions(); - pub_options.event_callbacks.deadline_callback = [this](rclcpp::QOSDeadlineOfferedInfo &) { - missed_deadlines_pub_++; - }; - - lbr_state_pub_ = node_->create_publisher( - "~/state", rclcpp::QoS(1) - .deadline(std::chrono::milliseconds( - static_cast(robotState().getSampleTime() * 1e3))) - .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE)); + if (topics_init_) { + return; } - if (!lbr_command_sub_) { - auto memory_strategy = - rclcpp::strategies::message_pool_memory_strategy::MessagePoolMemoryStrategy< - lbr_fri_msgs::msg::LBRCommand, 1>::make_shared(); - - auto sub_options = rclcpp::SubscriptionOptions(); - sub_options.event_callbacks.deadline_callback = [this](rclcpp::QOSDeadlineRequestedInfo &) { - missed_deadlines_sub_++; - }; - - lbr_command_sub_ = node_->create_subscription( - "~/command", - rclcpp::QoS(1) - .deadline( - std::chrono::milliseconds(static_cast(robotState().getSampleTime() * 1e3))) - .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE), - std::bind(&LBRClient::on_lbr_command_, this, std::placeholders::_1), sub_options, - memory_strategy); + auto pub_options = rclcpp::PublisherOptions(); + pub_options.event_callbacks.deadline_callback = [this](rclcpp::QOSDeadlineOfferedInfo &) { + missed_deadlines_pub_++; + }; + + lbr_state_pub_ = node_->create_publisher( + "~/state", rclcpp::QoS(1) + .deadline(std::chrono::milliseconds( + static_cast(robotState().getSampleTime() * 1e3))) + .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE)); + + auto memory_strategy = + rclcpp::strategies::message_pool_memory_strategy::MessagePoolMemoryStrategy< + lbr_fri_msgs::msg::LBRCommand, 1>::make_shared(); + + auto sub_options = rclcpp::SubscriptionOptions(); + sub_options.event_callbacks.deadline_callback = [this](rclcpp::QOSDeadlineRequestedInfo &) { + missed_deadlines_sub_++; + }; + + lbr_command_sub_ = node_->create_subscription( + "~/command", + rclcpp::QoS(1) + .deadline( + std::chrono::milliseconds(static_cast(robotState().getSampleTime() * 1e3))) + .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE), + std::bind(&LBRClient::on_lbr_command_, this, std::placeholders::_1), sub_options, + memory_strategy); + + topics_init_ = true; +} + +void LBRClient::init_filters_() { + if (filters_init_) { + return; } + // initialize PID + joint_position_pid_.init(2. * robotState().getSampleTime(), 0., 0., 0., 0., false); + + // initialize torque filters + external_torque_filter_.init(50, robotState().getSampleTime()); + measured_torque_filter_.init(50, robotState().getSampleTime()); + + filters_init_ = true; } void LBRClient::declare_parameters_() { @@ -141,8 +158,7 @@ void LBRClient::pub_lbr_state_() { lbr_state_.connection_quality = robotState().getConnectionQuality(); lbr_state_.control_mode = robotState().getControlMode(); lbr_state_.drive_state = robotState().getDriveState(); - std::memcpy(lbr_state_.external_torque.data(), robotState().getExternalTorque(), - sizeof(double) * KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + external_torque_filter_.compute(robotState().getExternalTorque(), lbr_state_.external_torque); if (robotState().getSessionState() == KUKA::FRI::ESessionState::COMMANDING_WAIT || robotState().getSessionState() == KUKA::FRI::ESessionState::COMMANDING_ACTIVE) { std::memcpy(lbr_state_.ipo_joint_position.data(), robotState().getIpoJointPosition(), @@ -155,8 +171,7 @@ void LBRClient::pub_lbr_state_() { std::memcpy(lbr_state_.measured_joint_position.data(), robotState().getMeasuredJointPosition(), sizeof(double) * KUKA::FRI::LBRState::NUMBER_OF_JOINTS); } - std::memcpy(lbr_state_.measured_torque.data(), robotState().getMeasuredTorque(), - sizeof(double) * KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + measured_torque_filter_.compute(robotState().getMeasuredTorque(), lbr_state_.measured_torque); lbr_state_.operation_mode = robotState().getOperationMode(); lbr_state_.overlay_type = robotState().getOverlayType(); lbr_state_.safety_state = robotState().getSafetyState(); @@ -173,18 +188,4 @@ void LBRClient::pub_lbr_state_() { void LBRClient::on_lbr_command_(const lbr_fri_msgs::msg::LBRCommand::SharedPtr lbr_command) { lbr_command_target_ = *lbr_command; } - -void LBRClient::lbr_command_pid_(const lbr_fri_msgs::msg::LBRCommand &lbr_command_target, - const lbr_fri_msgs::msg::LBRState &lbr_state, - lbr_fri_msgs::msg::LBRCommand &lbr_command) { - int i = 0; - std::for_each(lbr_command.joint_position.begin(), lbr_command.joint_position.end(), - [&](double &joint_position) { - joint_position += position_pid_controllers_[i].computeCommand( - lbr_command_target.joint_position[i] - lbr_state.measured_joint_position[i], - rclcpp::Duration(std::chrono::milliseconds( - static_cast(robotState().getSampleTime() * 1e3)))); - ++i; - }); -} } // end of namespace lbr_fri_ros2 From 24bdd28ddb9ca79c6de945d5a6770852395e8bf0 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 9 Aug 2023 10:16:30 +0100 Subject: [PATCH 14/33] LBRCommandGuard -> CommandGuard --- lbr_fri_ros2/CMakeLists.txt | 2 +- lbr_fri_ros2/doc/img/lbr_fri_ros2.drawio.svg | 2 +- lbr_fri_ros2/doc/lbr_fri_ros2.rst | 2 +- ...br_command_guard.hpp => command_guard.hpp} | 40 +++++++++---------- lbr_fri_ros2/include/lbr_fri_ros2/lbr_app.hpp | 2 +- .../include/lbr_fri_ros2/lbr_client.hpp | 6 +-- ...br_command_guard.cpp => command_guard.cpp} | 34 ++++++++-------- lbr_fri_ros2/src/lbr_client.cpp | 2 +- 8 files changed, 45 insertions(+), 45 deletions(-) rename lbr_fri_ros2/include/lbr_fri_ros2/{lbr_command_guard.hpp => command_guard.hpp} (79%) rename lbr_fri_ros2/src/{lbr_command_guard.cpp => command_guard.cpp} (80%) diff --git a/lbr_fri_ros2/CMakeLists.txt b/lbr_fri_ros2/CMakeLists.txt index 72d97d20..10e774c2 100644 --- a/lbr_fri_ros2/CMakeLists.txt +++ b/lbr_fri_ros2/CMakeLists.txt @@ -22,7 +22,7 @@ add_library(lbr_fri_ros2 SHARED src/lbr_app.cpp src/lbr_client.cpp - src/lbr_command_guard.cpp + src/command_guard.cpp src/utils.cpp ) diff --git a/lbr_fri_ros2/doc/img/lbr_fri_ros2.drawio.svg b/lbr_fri_ros2/doc/img/lbr_fri_ros2.drawio.svg index e88693b9..5355ad9f 100644 --- a/lbr_fri_ros2/doc/img/lbr_fri_ros2.drawio.svg +++ b/lbr_fri_ros2/doc/img/lbr_fri_ros2.drawio.svg @@ -1,4 +1,4 @@ -
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::LBRCommandGuard
lbr_fri_ros2::LBRCommandGuard
+ LBRCommandGuard(robot_description: const std::string &)
+ is_valid_command(
lbr_command: const lbr_fri_msgs::msg::LBRCommand &,
lbr_state: const KUKA::FRI::LBRState &
): bool {query}
+ LBRCommandGuard(robot_description: const std::string &)...
rclcpp::Node
rclcpp::Node
subscribe lbr_fri_msg::msg::LBRState
to /lbr/state
subscribe lbr_fri_msg::msg::LBRState...
lbr_fri_ros2::LBRApp
lbr_fri_ros2::LBRApp
# 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

# run_(): void
publish lbr_fri_msg::msg::LBRCommand
to /lbr/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::LBRClient
lbr_fri_ros2::LBRClient
# 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::LBRCommandGuard>
# 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 +
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 /lbr/state
subscribe lbr_fri_msg::msg::LBRState...
lbr_fri_ros2::LBRApp
lbr_fri_ros2::LBRApp
# 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

# run_(): void
publish lbr_fri_msg::msg::LBRCommand
to /lbr/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::LBRClient
lbr_fri_ros2::LBRClient
# 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/lbr_fri_ros2.rst b/lbr_fri_ros2/doc/lbr_fri_ros2.rst index 44581188..38ee7ed3 100644 --- a/lbr_fri_ros2/doc/lbr_fri_ros2.rst +++ b/lbr_fri_ros2/doc/lbr_fri_ros2.rst @@ -76,7 +76,7 @@ The :lbr_fri_ros2:`LBRClient ` 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:`LBRCommandGuard `. +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/lbr_command_guard.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp similarity index 79% rename from lbr_fri_ros2/include/lbr_fri_ros2/lbr_command_guard.hpp rename to lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp index fb92b67a..5bbfb625 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/lbr_command_guard.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp @@ -16,18 +16,18 @@ namespace lbr_fri_ros2 { /** - * @brief LBRCommandGuard checks desired commands for limits. + * @brief CommandGuard checks desired commands for limits. * */ -class LBRCommandGuard { +class CommandGuard { protected: using JointArray = lbr_fri_msgs::msg::LBRCommand::_joint_position_type; public: - LBRCommandGuard() = delete; + CommandGuard() = delete; /** - * @brief Construct a new LBRCommandGuard object. + * @brief Construct a new CommandGuard object. * * @param[in] logger_interface Shared node logger interface * @param[in] min_position Minimum joint position [rad] @@ -35,18 +35,18 @@ class LBRCommandGuard { * @param[in] max_velocity Maximum joint velocity [rad/s] * @param[in] max_torque Maximal torque [Nm] */ - LBRCommandGuard(const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface, - const JointArray &min_position, const JointArray &max_position, - const JointArray &max_velocity, const JointArray &max_torque); + CommandGuard(const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface, + const JointArray &min_position, const JointArray &max_position, + const JointArray &max_velocity, const JointArray &max_torque); /** - * @brief Construct a new LBRCommandGuard object. + * @brief Construct a new CommandGuard object. * * @param[in] logger_interface Shared node logger interface * @param robot_description String containing URDF robot rescription */ - LBRCommandGuard(const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface, - const std::string &robot_description); + CommandGuard(const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface, + const std::string &robot_description); /** * @brief Checks for command validity given the current state. @@ -113,23 +113,23 @@ class LBRCommandGuard { }; /** - * @brief Adds early stopping to LBRCommandGuard. + * @brief Adds early stopping to CommandGuard. * */ -class LBRSafeStopCommandGuard : public LBRCommandGuard { +class SafeStopCommandGuard : public CommandGuard { public: - LBRSafeStopCommandGuard() = delete; + SafeStopCommandGuard() = delete; /** - * @brief Construct a new LBRSafeStopCommandGuard object. + * @brief Construct a new SafeStopCommandGuard object. * * @param[in] logger_interface Shared node logger interface * @param robot_description String containing URDF robot rescription */ - LBRSafeStopCommandGuard( + SafeStopCommandGuard( const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface, const std::string &robot_description) - : LBRCommandGuard(logger_interface, robot_description){}; + : CommandGuard(logger_interface, robot_description){}; protected: /** @@ -146,14 +146,14 @@ class LBRSafeStopCommandGuard : public LBRCommandGuard { }; /** - * @brief Creates an LBRCommandGuard object. + * @brief Creates an CommandGuard object. * * @param[in] logger_interface Shared node logger interface * @param[in] robot_description String containing URDF robot rescription - * @param[in] variant Which variant of LBRCommandGuard to create - * @return std::unique_ptr Pointer to LBRCommandGuard object + * @param[in] variant Which variant of CommandGuard to create + * @return std::unique_ptr Pointer to CommandGuard object */ -std::unique_ptr lbr_command_guard_factory( +std::unique_ptr lbr_command_guard_factory( const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface, const std::string &robot_description, const std::string &variant); } // end of namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/lbr_app.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/lbr_app.hpp index 36463b9f..adfe19f4 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/lbr_app.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/lbr_app.hpp @@ -17,8 +17,8 @@ #include "lbr_fri_msgs/srv/app_connect.hpp" #include "lbr_fri_msgs/srv/app_disconnect.hpp" +#include "lbr_fri_ros2/command_guard.hpp" #include "lbr_fri_ros2/lbr_client.hpp" -#include "lbr_fri_ros2/lbr_command_guard.hpp" namespace lbr_fri_ros2 { /** diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/lbr_client.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/lbr_client.hpp index 8232cf73..704a4f8e 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/lbr_client.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/lbr_client.hpp @@ -16,7 +16,7 @@ #include "lbr_fri_msgs/msg/lbr_command.hpp" #include "lbr_fri_msgs/msg/lbr_state.hpp" -#include "lbr_fri_ros2/lbr_command_guard.hpp" +#include "lbr_fri_ros2/command_guard.hpp" #include "lbr_fri_ros2/utils.hpp" namespace lbr_fri_ros2 { @@ -36,7 +36,7 @@ class LBRClient : public KUKA::FRI::LBRClient { * @param[in] lbr_command_guard Command guard for validating incoming commands. * */ - LBRClient(const rclcpp::Node::SharedPtr node, std::unique_ptr lbr_command_guard); + LBRClient(const rclcpp::Node::SharedPtr node, std::unique_ptr lbr_command_guard); /** * @brief Log the status of the robot to terminal. @@ -125,7 +125,7 @@ class LBRClient : public KUKA::FRI::LBRClient { void on_lbr_command_(const lbr_fri_msgs::msg::LBRCommand::SharedPtr lbr_command); rclcpp::Node::SharedPtr node_; /**< Shared pointer to node.*/ - std::unique_ptr + std::unique_ptr lbr_command_guard_; /**< Validating commands prior to writing them to #robotCommand.*/ JointExponentialFilterArrayROS diff --git a/lbr_fri_ros2/src/lbr_command_guard.cpp b/lbr_fri_ros2/src/command_guard.cpp similarity index 80% rename from lbr_fri_ros2/src/lbr_command_guard.cpp rename to lbr_fri_ros2/src/command_guard.cpp index 83473d3e..418d25fd 100644 --- a/lbr_fri_ros2/src/lbr_command_guard.cpp +++ b/lbr_fri_ros2/src/command_guard.cpp @@ -1,14 +1,14 @@ -#include "lbr_fri_ros2/lbr_command_guard.hpp" +#include "lbr_fri_ros2/command_guard.hpp" namespace lbr_fri_ros2 { -LBRCommandGuard::LBRCommandGuard( +CommandGuard::CommandGuard( const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface, const JointArray &min_position, const JointArray &max_position, const JointArray &max_velocity, const JointArray &max_torque) : logger_interface_(logger_interface), min_position_(min_position), max_position_(max_position), max_velocity_(max_velocity), max_torque_(max_torque) {} -LBRCommandGuard::LBRCommandGuard( +CommandGuard::CommandGuard( const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface, const std::string &robot_description) : logger_interface_(logger_interface) { @@ -41,8 +41,8 @@ LBRCommandGuard::LBRCommandGuard( }; } -bool LBRCommandGuard::is_valid_command(const lbr_fri_msgs::msg::LBRCommand &lbr_command, - const KUKA::FRI::LBRState &lbr_state) const { +bool CommandGuard::is_valid_command(const lbr_fri_msgs::msg::LBRCommand &lbr_command, + const KUKA::FRI::LBRState &lbr_state) const { switch (lbr_state.getClientCommandMode()) { case KUKA::FRI::EClientCommandMode::NO_COMMAND_MODE: return false; @@ -87,12 +87,12 @@ bool LBRCommandGuard::is_valid_command(const lbr_fri_msgs::msg::LBRCommand &lbr_ return true; } -bool LBRCommandGuard::is_nan_(const double *begin, const double *end) const { +bool CommandGuard::is_nan_(const double *begin, const double *end) const { return std::find_if(begin, end, [&](const auto &xi) { return std::isnan(xi); }) != end; } -bool LBRCommandGuard::command_in_position_limits_(const lbr_fri_msgs::msg::LBRCommand &lbr_command, - const KUKA::FRI::LBRState & /*lbr_state*/) const { +bool CommandGuard::command_in_position_limits_(const lbr_fri_msgs::msg::LBRCommand &lbr_command, + const KUKA::FRI::LBRState & /*lbr_state*/) const { for (std::size_t i = 0; i < lbr_command.joint_position.size(); ++i) { if (lbr_command.joint_position[i] < min_position_[i] || lbr_command.joint_position[i] > max_position_[i]) { @@ -104,8 +104,8 @@ bool LBRCommandGuard::command_in_position_limits_(const lbr_fri_msgs::msg::LBRCo return true; } -bool LBRCommandGuard::command_in_velocity_limits_(const lbr_fri_msgs::msg::LBRCommand &lbr_command, - const KUKA::FRI::LBRState &lbr_state) const { +bool CommandGuard::command_in_velocity_limits_(const lbr_fri_msgs::msg::LBRCommand &lbr_command, + const KUKA::FRI::LBRState &lbr_state) const { const double &dt = lbr_state.getSampleTime(); 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 > @@ -117,8 +117,8 @@ bool LBRCommandGuard::command_in_velocity_limits_(const lbr_fri_msgs::msg::LBRCo return true; } -bool LBRCommandGuard::command_in_torque_limits_(const lbr_fri_msgs::msg::LBRCommand &lbr_command, - const KUKA::FRI::LBRState &lbr_state) const { +bool CommandGuard::command_in_torque_limits_(const lbr_fri_msgs::msg::LBRCommand &lbr_command, + const KUKA::FRI::LBRState &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]) > max_torque_[i]) { RCLCPP_ERROR(logger_interface_->get_logger(), "Torque command not in limits for joint %ld.", @@ -129,7 +129,7 @@ bool LBRCommandGuard::command_in_torque_limits_(const lbr_fri_msgs::msg::LBRComm return true; } -bool LBRSafeStopCommandGuard::command_in_position_limits_( +bool SafeStopCommandGuard::command_in_position_limits_( const lbr_fri_msgs::msg::LBRCommand &lbr_command, const KUKA::FRI::LBRState &lbr_state) const { for (std::size_t i = 0; i < lbr_command.joint_position.size(); ++i) { if (lbr_command.joint_position[i] < @@ -144,16 +144,16 @@ bool LBRSafeStopCommandGuard::command_in_position_limits_( return true; } -std::unique_ptr lbr_command_guard_factory( +std::unique_ptr lbr_command_guard_factory( const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface, const std::string &robot_description, const std::string &variant) { if (variant == "default") { - return std::make_unique(logger_interface, robot_description); + return std::make_unique(logger_interface, robot_description); } if (variant == "safe_stop") { - return std::make_unique(logger_interface, robot_description); + return std::make_unique(logger_interface, robot_description); } - std::string error_msg = "Invalid LBRCommandGuard variant provided."; + std::string error_msg = "Invalid CommandGuard variant provided."; RCLCPP_ERROR(logger_interface->get_logger(), error_msg.c_str()); throw std::runtime_error(error_msg); } diff --git a/lbr_fri_ros2/src/lbr_client.cpp b/lbr_fri_ros2/src/lbr_client.cpp index 6ef05b30..71dc211d 100644 --- a/lbr_fri_ros2/src/lbr_client.cpp +++ b/lbr_fri_ros2/src/lbr_client.cpp @@ -2,7 +2,7 @@ namespace lbr_fri_ros2 { LBRClient::LBRClient(const rclcpp::Node::SharedPtr node, - std::unique_ptr lbr_command_guard) + std::unique_ptr lbr_command_guard) : node_(node), lbr_command_guard_(std::move(lbr_command_guard)), external_torque_filter_(node, "external_torque"), measured_torque_filter_(node, "measured_torque"), From 939daf0ebcc3b6c62f9176053577eb174ccc7e59 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 9 Aug 2023 10:20:26 +0100 Subject: [PATCH 15/33] LBRApp -> App --- .../src/admittance_control_node.cpp | 4 +-- lbr_fri_ros2/CMakeLists.txt | 2 +- lbr_fri_ros2/doc/img/lbr_fri_ros2.drawio.svg | 2 +- lbr_fri_ros2/doc/lbr_fri_ros2.rst | 6 ++-- .../lbr_fri_ros2/{lbr_app.hpp => app.hpp} | 20 +++++------ lbr_fri_ros2/src/{lbr_app.cpp => app.cpp} | 36 +++++++++---------- lbr_fri_ros2/src/lbr_app_component.cpp | 2 +- lbr_fri_ros2/src/lbr_app_component.hpp | 8 ++--- .../lbr_hardware_interface.hpp | 4 +-- .../src/lbr_hardware_interface.cpp | 2 +- 10 files changed, 43 insertions(+), 43 deletions(-) rename lbr_fri_ros2/include/lbr_fri_ros2/{lbr_app.hpp => app.hpp} (90%) rename lbr_fri_ros2/src/{lbr_app.cpp => app.cpp} (83%) diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_control_node.cpp b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_control_node.cpp index 927cb2ff..07fb3877 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_control_node.cpp +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_control_node.cpp @@ -4,7 +4,7 @@ #include "lbr_fri_msgs/msg/lbr_command.hpp" #include "lbr_fri_msgs/msg/lbr_state.hpp" -#include "lbr_fri_ros2/lbr_app.hpp" +#include "lbr_fri_ros2/app.hpp" #include "admittance_controller.hpp" #include "damped_least_squares.hpp" @@ -78,7 +78,7 @@ int main(int argc, char **argv) { auto lbr_node = std::make_shared("lbr", rclcpp::NodeOptions().use_intra_process_comms(true)); - auto lbr_app = lbr_fri_ros2::LBRApp(lbr_node); + auto lbr_app = lbr_fri_ros2::App(lbr_node); auto admittance_control_node = std::make_shared( "admittance_control_node", rclcpp::NodeOptions().use_intra_process_comms(true)); diff --git a/lbr_fri_ros2/CMakeLists.txt b/lbr_fri_ros2/CMakeLists.txt index 10e774c2..952ca349 100644 --- a/lbr_fri_ros2/CMakeLists.txt +++ b/lbr_fri_ros2/CMakeLists.txt @@ -20,7 +20,7 @@ find_package(urdf REQUIRED) # lbr_fri_ros2 add_library(lbr_fri_ros2 SHARED - src/lbr_app.cpp + src/app.cpp src/lbr_client.cpp src/command_guard.cpp src/utils.cpp diff --git a/lbr_fri_ros2/doc/img/lbr_fri_ros2.drawio.svg b/lbr_fri_ros2/doc/img/lbr_fri_ros2.drawio.svg index 5355ad9f..4f6a26c8 100644 --- a/lbr_fri_ros2/doc/img/lbr_fri_ros2.drawio.svg +++ b/lbr_fri_ros2/doc/img/lbr_fri_ros2.drawio.svg @@ -1,4 +1,4 @@ -
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 /lbr/state
subscribe lbr_fri_msg::msg::LBRState...
lbr_fri_ros2::LBRApp
lbr_fri_ros2::LBRApp
# 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

# run_(): void
publish lbr_fri_msg::msg::LBRCommand
to /lbr/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::LBRClient
lbr_fri_ros2::LBRClient
# 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 +
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 /lbr/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

# run_(): void
publish lbr_fri_msg::msg::LBRCommand
to /lbr/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::LBRClient
lbr_fri_ros2::LBRClient
# 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/lbr_fri_ros2.rst b/lbr_fri_ros2/doc/lbr_fri_ros2.rst index 38ee7ed3..0029fc91 100644 --- a/lbr_fri_ros2/doc/lbr_fri_ros2.rst +++ b/lbr_fri_ros2/doc/lbr_fri_ros2.rst @@ -14,7 +14,7 @@ Quick Start .. thumbnail:: ../../lbr_demos/doc/img/applications_lbr_server.png -#. Run the :lbr_fri_ros2:`LBRApp ` node via `lbr_app.launch.py `_: +#. Run the :lbr_fri_ros2:`App ` node via `lbr_app.launch.py `_: .. code-block:: bash @@ -23,7 +23,7 @@ Quick Start This launch file does 2 things: - Loads the ``robot_description`` (to read joint limits) - - Runs the :lbr_fri_ros2:`LBRAppComponentLBRApp `, which has an instance of :lbr_fri_ros2:`LBRApp ` to + - Runs the :lbr_fri_ros2:`LBRAppComponentLBRApp `, 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:`LBRClient ` @@ -52,7 +52,7 @@ 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_msgs`` 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:`LBRApp `. +- Run stand-alone **and** within ``ros2_control`` -> implemented through :lbr_fri_ros2:`App `. Implementation Details ~~~~~~~~~~~~~~~~~~~~~~ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/lbr_app.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp similarity index 90% rename from lbr_fri_ros2/include/lbr_fri_ros2/lbr_app.hpp rename to lbr_fri_ros2/include/lbr_fri_ros2/app.hpp index adfe19f4..606bc9f9 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/lbr_app.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp @@ -1,5 +1,5 @@ -#ifndef LBR_FRI_ROS2__LBR_APP_HPP_ -#define LBR_FRI_ROS2__LBR_APP_HPP_ +#ifndef LBR_FRI_ROS2__APP_HPP_ +#define LBR_FRI_ROS2__APP_HPP_ #include #include @@ -22,7 +22,7 @@ namespace lbr_fri_ros2 { /** - * @brief The LBRApp has a node for exposing FRI methods to services. It shares this node with the + * @brief The App has a node for exposing FRI methods to services. It shares this node with the * #lbr_client_, which reads commands / write states via realtime safe topics. * * Services: @@ -34,17 +34,17 @@ namespace lbr_fri_ros2 { * #run_. * */ -class LBRApp { +class App { public: /** - * @brief Construct a new LBRApp object. + * @brief Construct a new App object. * * @param node Shared node * * @throws std::runtime error if no robot_description in node parameters */ - LBRApp(const rclcpp::Node::SharedPtr node); - ~LBRApp(); + App(const rclcpp::Node::SharedPtr node); + ~App(); protected: /** @@ -114,7 +114,7 @@ class LBRApp { /** * @brief Exchanges commands / states between ROS and the FRI. * - * Calls step() on #app_, which callbacks #lbr_client_. #lbr_client_ reads commands / write states + * Calls step() on #lbr_app_, which callbacks #lbr_client_. #lbr_client_ reads commands / write states * through realtime safe topics. * */ @@ -142,7 +142,7 @@ class LBRApp { std::unique_ptr connection_; /**< UDP connection for reading states / writing commands.*/ std::unique_ptr - app_; /**< FRI client application that callbacks #lbr_client_ methods.*/ + lbr_app_; /**< FRI client application that callbacks #lbr_client_ methods.*/ }; } // end of namespace lbr_fri_ros2 -#endif // LBR_FRI_ROS2__LBR_APP_HPP_ +#endif // LBR_FRI_ROS2__APP_HPP_ diff --git a/lbr_fri_ros2/src/lbr_app.cpp b/lbr_fri_ros2/src/app.cpp similarity index 83% rename from lbr_fri_ros2/src/lbr_app.cpp rename to lbr_fri_ros2/src/app.cpp index dccf0586..fb2f52ea 100644 --- a/lbr_fri_ros2/src/lbr_app.cpp +++ b/lbr_fri_ros2/src/app.cpp @@ -1,7 +1,7 @@ -#include "lbr_fri_ros2/lbr_app.hpp" +#include "lbr_fri_ros2/app.hpp" namespace lbr_fri_ros2 { -LBRApp::LBRApp(const rclcpp::Node::SharedPtr node) : node_(node) { +App::App(const rclcpp::Node::SharedPtr node) : node_(node) { declare_parameters_(); get_parameters_(); @@ -9,27 +9,27 @@ LBRApp::LBRApp(const rclcpp::Node::SharedPtr node) : node_(node) { app_connect_srv_ = node_->create_service( robot_name_ + "/connect", - std::bind(&LBRApp::on_app_connect_, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&App::on_app_connect_, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default); app_disconnect_srv_ = node_->create_service( robot_name_ + "/disconnect", - std::bind(&LBRApp::on_app_disconnect_, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&App::on_app_disconnect_, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default); lbr_client_ = std::make_shared( node_, lbr_command_guard_factory(node_->get_node_logging_interface(), robot_description_, command_guard_variant_)); connection_ = std::make_unique(); - app_ = std::make_unique(*connection_, *lbr_client_); + lbr_app_ = std::make_unique(*connection_, *lbr_client_); // attempt default connect connect_(port_id_, remote_host_); } -LBRApp::~LBRApp() { disconnect_(); } +App::~App() { disconnect_(); } -void LBRApp::declare_parameters_() { +void App::declare_parameters_() { if (!node_->has_parameter("port_id")) { node_->declare_parameter("port_id", 30200); } @@ -50,7 +50,7 @@ void LBRApp::declare_parameters_() { } } -void LBRApp::get_parameters_() { +void App::get_parameters_() { int port_id = node_->get_parameter("port_id").as_int(); std::string remote_host = node_->get_parameter("remote_host").as_string(); @@ -68,7 +68,7 @@ void LBRApp::get_parameters_() { rt_prio_ = node_->get_parameter("rt_prio").as_int(); } -void LBRApp::on_app_connect_(const lbr_fri_msgs::srv::AppConnect::Request::SharedPtr request, +void App::on_app_connect_(const lbr_fri_msgs::srv::AppConnect::Request::SharedPtr request, lbr_fri_msgs::srv::AppConnect::Response::SharedPtr response) { const char *remote_host = request->remote_host.empty() ? NULL : request->remote_host.c_str(); try { @@ -79,7 +79,7 @@ void LBRApp::on_app_connect_(const lbr_fri_msgs::srv::AppConnect::Request::Share } } -void LBRApp::on_app_disconnect_( +void App::on_app_disconnect_( const lbr_fri_msgs::srv::AppDisconnect::Request::SharedPtr /*request*/, lbr_fri_msgs::srv::AppDisconnect::Response::SharedPtr response) { try { @@ -90,7 +90,7 @@ void LBRApp::on_app_disconnect_( } } -bool LBRApp::valid_port_(const int &port_id) { +bool App::valid_port_(const int &port_id) { if (port_id < 30200 || port_id > 30209) { RCLCPP_ERROR(node_->get_logger(), "Expected port_id in [30200, 30209], got %d.", port_id); return false; @@ -98,18 +98,18 @@ bool LBRApp::valid_port_(const int &port_id) { return true; } -bool LBRApp::connect_(const int &port_id, const char *const remote_host) { +bool App::connect_(const int &port_id, const char *const remote_host) { RCLCPP_INFO(node_->get_logger(), "Attempting to open UDP socket with port_id %d for LBR server...", port_id); if (!connected_) { if (!valid_port_(port_id)) { throw std::range_error("Invalid port_id provided."); } - connected_ = app_->connect(port_id, remote_host); + connected_ = lbr_app_->connect(port_id, remote_host); if (connected_) { port_id_ = port_id; remote_host_ = remote_host; - run_thread_ = std::make_unique(std::bind(&LBRApp::run_, this)); + run_thread_ = std::make_unique(std::bind(&App::run_, this)); } } else { RCLCPP_INFO(node_->get_logger(), "Port already open."); @@ -122,11 +122,11 @@ bool LBRApp::connect_(const int &port_id, const char *const remote_host) { return connected_; } -bool LBRApp::disconnect_() { +bool App::disconnect_() { RCLCPP_INFO(node_->get_logger(), "Attempting to close UDP socket with port_id %d for LBR server...", port_id_); if (connected_) { - app_->disconnect(); + lbr_app_->disconnect(); connected_ = false; } else { RCLCPP_INFO(node_->get_logger(), "Port already closed."); @@ -144,7 +144,7 @@ bool LBRApp::disconnect_() { return !connected_; } -void LBRApp::run_() { +void App::run_() { if (realtime_tools::has_realtime_kernel()) { if (!realtime_tools::configure_sched_fifo(rt_prio_)) { RCLCPP_WARN(node_->get_logger(), "Failed to set FIFO realtime scheduling policy."); @@ -155,7 +155,7 @@ void LBRApp::run_() { bool success = true; while (success && connected_ && rclcpp::ok()) { - success = app_->step(); + success = lbr_app_->step(); if (lbr_client_->robotState().getSessionState() == KUKA::FRI::IDLE) { break; } diff --git a/lbr_fri_ros2/src/lbr_app_component.cpp b/lbr_fri_ros2/src/lbr_app_component.cpp index fa5b7128..c5ec1bef 100644 --- a/lbr_fri_ros2/src/lbr_app_component.cpp +++ b/lbr_fri_ros2/src/lbr_app_component.cpp @@ -3,7 +3,7 @@ namespace lbr_fri_ros2 { LBRAppComponent::LBRAppComponent(const rclcpp::NodeOptions &options) { lbr_node_ = std::make_shared("lbr", options); - lbr_app_ = std::make_unique(lbr_node_); + lbr_app_ = std::make_unique(lbr_node_); } rclcpp::node_interfaces::NodeBaseInterface::SharedPtr diff --git a/lbr_fri_ros2/src/lbr_app_component.hpp b/lbr_fri_ros2/src/lbr_app_component.hpp index debac10d..c759335a 100644 --- a/lbr_fri_ros2/src/lbr_app_component.hpp +++ b/lbr_fri_ros2/src/lbr_app_component.hpp @@ -4,11 +4,11 @@ #include "rclcpp/rclcpp.hpp" -#include "lbr_fri_ros2/lbr_app.hpp" +#include "lbr_fri_ros2/app.hpp" namespace lbr_fri_ros2 { /** - * @brief Component for instantiating #lbr_fri_ros2::LBRApp. + * @brief Component for instantiating #lbr_fri_ros2::App. * */ class LBRAppComponent { @@ -29,8 +29,8 @@ class LBRAppComponent { rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() const; protected: - std::unique_ptr - lbr_app_; /** #lbr_fri_ros2::LBRApp for communicating with the hardware.<*/ + std::unique_ptr + lbr_app_; /** #lbr_fri_ros2::App for communicating with the hardware.<*/ rclcpp::Node::SharedPtr lbr_node_; /** Node for communicating with ROS.<*/ }; } // end of namespace lbr_fri_ros2 diff --git a/lbr_hardware_interface/include/lbr_hardware_interface/lbr_hardware_interface.hpp b/lbr_hardware_interface/include/lbr_hardware_interface/lbr_hardware_interface.hpp index ee1c9c9c..433c6709 100644 --- a/lbr_hardware_interface/include/lbr_hardware_interface/lbr_hardware_interface.hpp +++ b/lbr_hardware_interface/include/lbr_hardware_interface/lbr_hardware_interface.hpp @@ -23,7 +23,7 @@ #include "lbr_fri_msgs/msg/lbr_state.hpp" #include "lbr_fri_msgs/srv/app_connect.hpp" #include "lbr_fri_msgs/srv/app_disconnect.hpp" -#include "lbr_fri_ros2/lbr_app.hpp" +#include "lbr_fri_ros2/app.hpp" #include "lbr_hardware_interface/lbr_hardware_interface_type_values.hpp" namespace lbr_hardware_interface { @@ -83,7 +83,7 @@ class LBRHardwareInterface : public hardware_interface::SystemInterface { // node for handling communication rclcpp::Node::SharedPtr lbr_node_; - std::unique_ptr lbr_app_; + std::unique_ptr lbr_app_; // exposed state interfaces double hw_sample_time_; diff --git a/lbr_hardware_interface/src/lbr_hardware_interface.cpp b/lbr_hardware_interface/src/lbr_hardware_interface.cpp index e8abea9b..05e7520e 100644 --- a/lbr_hardware_interface/src/lbr_hardware_interface.cpp +++ b/lbr_hardware_interface/src/lbr_hardware_interface.cpp @@ -31,7 +31,7 @@ LBRHardwareInterface::on_init(const hardware_interface::HardwareInfo &system_inf lbr_node_->declare_parameter("robot_name", robot_name_); lbr_node_->declare_parameter("command_guard_variant", "default"); - lbr_app_ = std::make_unique(lbr_node_); + lbr_app_ = std::make_unique(lbr_node_); init_command_interfaces_(); init_state_interfaces_(); From ee83aeca5c41560a348bb3a222698f29efa51558 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 9 Aug 2023 10:23:50 +0100 Subject: [PATCH 16/33] LBRClient -> Client --- lbr_fri_ros2/CMakeLists.txt | 2 +- lbr_fri_ros2/doc/img/lbr_fri_ros2.drawio.svg | 2 +- lbr_fri_ros2/doc/lbr_fri_ros2.rst | 18 +++++----- lbr_fri_ros2/include/lbr_fri_ros2/app.hpp | 4 +-- .../{lbr_client.hpp => client.hpp} | 16 ++++----- lbr_fri_ros2/src/app.cpp | 2 +- .../src/{lbr_client.cpp => client.cpp} | 34 +++++++++---------- 7 files changed, 39 insertions(+), 39 deletions(-) rename lbr_fri_ros2/include/lbr_fri_ros2/{lbr_client.hpp => client.hpp} (94%) rename lbr_fri_ros2/src/{lbr_client.cpp => client.cpp} (88%) diff --git a/lbr_fri_ros2/CMakeLists.txt b/lbr_fri_ros2/CMakeLists.txt index 952ca349..88a0d942 100644 --- a/lbr_fri_ros2/CMakeLists.txt +++ b/lbr_fri_ros2/CMakeLists.txt @@ -21,7 +21,7 @@ find_package(urdf REQUIRED) add_library(lbr_fri_ros2 SHARED src/app.cpp - src/lbr_client.cpp + src/client.cpp src/command_guard.cpp src/utils.cpp ) diff --git a/lbr_fri_ros2/doc/img/lbr_fri_ros2.drawio.svg b/lbr_fri_ros2/doc/img/lbr_fri_ros2.drawio.svg index 4f6a26c8..0c955d3b 100644 --- a/lbr_fri_ros2/doc/img/lbr_fri_ros2.drawio.svg +++ b/lbr_fri_ros2/doc/img/lbr_fri_ros2.drawio.svg @@ -1,4 +1,4 @@ -
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 /lbr/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

# run_(): void
publish lbr_fri_msg::msg::LBRCommand
to /lbr/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::LBRClient
lbr_fri_ros2::LBRClient
# 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 +
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 /lbr/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

# run_(): void
publish lbr_fri_msg::msg::LBRCommand
to /lbr/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/lbr_fri_ros2.rst b/lbr_fri_ros2/doc/lbr_fri_ros2.rst index 0029fc91..f62c4fb5 100644 --- a/lbr_fri_ros2/doc/lbr_fri_ros2.rst +++ b/lbr_fri_ros2/doc/lbr_fri_ros2.rst @@ -26,8 +26,8 @@ This launch file does 2 things: - Runs the :lbr_fri_ros2:`LBRAppComponentLBRApp `, 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:`LBRClient ` - - Read robot commands from ``/lbr/command`` via :lbr_fri_ros2:`LBRClient ` + - 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 @@ -59,7 +59,7 @@ 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:`LBRClient ` (interface for reading states / sending commands) +- :fri:`Client ` (interface for reading states / sending commands) The user calls :fri:`step `, which, depending on the robot's state, callbacks: @@ -67,16 +67,16 @@ The user calls :fri:`step `, which, depend - :fri:`waitForCommand ` - :fri:`command ` -The user can implement these callbacks to read states / send commands by implementing an :fri:`LBRClient `. +The user can implement these callbacks to read states / send commands by implementing an :fri:`Client `. -The ``lbr_fri_ros2`` package implements an :fri:`LBRClient ` in :lbr_fri_ros2:`LBRClient `. +The ``lbr_fri_ros2`` package implements an :fri:`Client ` in :lbr_fri_ros2:`Client `. -The :lbr_fri_ros2:`LBRClient ` has +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 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 `. +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/app.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp index 606bc9f9..6a07d51b 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp @@ -18,7 +18,7 @@ #include "lbr_fri_msgs/srv/app_connect.hpp" #include "lbr_fri_msgs/srv/app_disconnect.hpp" #include "lbr_fri_ros2/command_guard.hpp" -#include "lbr_fri_ros2/lbr_client.hpp" +#include "lbr_fri_ros2/client.hpp" namespace lbr_fri_ros2 { /** @@ -138,7 +138,7 @@ class App { rclcpp::Service::SharedPtr app_disconnect_srv_; /**< Service to disconnect from robot via #on_app_disconnect_ callback.*/ - std::shared_ptr lbr_client_; /**< Writes commands to / reads states from robot.*/ + std::shared_ptr lbr_client_; /**< Writes commands to / reads states from robot.*/ std::unique_ptr connection_; /**< UDP connection for reading states / writing commands.*/ std::unique_ptr diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/lbr_client.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/client.hpp similarity index 94% rename from lbr_fri_ros2/include/lbr_fri_ros2/lbr_client.hpp rename to lbr_fri_ros2/include/lbr_fri_ros2/client.hpp index 704a4f8e..54b6621f 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/lbr_client.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/client.hpp @@ -1,5 +1,5 @@ -#ifndef LBR_FRI_ROS2__LBR_CLIENT_HPP_ -#define LBR_FRI_ROS2__LBR_CLIENT_HPP_ +#ifndef LBR_FRI_ROS2__CLIENT_HPP_ +#define LBR_FRI_ROS2__CLIENT_HPP_ #include #include @@ -25,18 +25,18 @@ namespace lbr_fri_ros2 { * from / writing states to topics that follow the robot's QoS. * */ -class LBRClient : public KUKA::FRI::LBRClient { +class Client : public KUKA::FRI::LBRClient { public: - LBRClient() = delete; + Client() = delete; /** - * @brief Construct a new LBRClient object. + * @brief Construct a new Client object. * * @param[in] node Shared node for reading commands from / writing states to topics. * @param[in] lbr_command_guard Command guard for validating incoming commands. * */ - LBRClient(const rclcpp::Node::SharedPtr node, std::unique_ptr lbr_command_guard); + Client(const rclcpp::Node::SharedPtr node, std::unique_ptr lbr_command_guard); /** * @brief Log the status of the robot to terminal. @@ -99,7 +99,7 @@ class LBRClient : public KUKA::FRI::LBRClient { void init_filters_(); /** - * @brief Declare parameters for #node_ that are utilized within LBRClient. + * @brief Declare parameters for #node_ that are utilized within Client. * */ void declare_parameters_(); @@ -166,4 +166,4 @@ class LBRClient : public KUKA::FRI::LBRClient { }; /** Map for converting KUKA::FRI::ESessionState to readable strings.*/ }; } // end of namespace lbr_fri_ros2 -#endif // LBR_FRI_ROS2__LBR_CLIENT_HPP_ +#endif // LBR_FRI_ROS2__CLIENT_HPP_ diff --git a/lbr_fri_ros2/src/app.cpp b/lbr_fri_ros2/src/app.cpp index fb2f52ea..4b1b5ce4 100644 --- a/lbr_fri_ros2/src/app.cpp +++ b/lbr_fri_ros2/src/app.cpp @@ -17,7 +17,7 @@ App::App(const rclcpp::Node::SharedPtr node) : node_(node) { std::bind(&App::on_app_disconnect_, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default); - lbr_client_ = std::make_shared( + lbr_client_ = std::make_shared( node_, lbr_command_guard_factory(node_->get_node_logging_interface(), robot_description_, command_guard_variant_)); connection_ = std::make_unique(); diff --git a/lbr_fri_ros2/src/lbr_client.cpp b/lbr_fri_ros2/src/client.cpp similarity index 88% rename from lbr_fri_ros2/src/lbr_client.cpp rename to lbr_fri_ros2/src/client.cpp index 71dc211d..1328df44 100644 --- a/lbr_fri_ros2/src/lbr_client.cpp +++ b/lbr_fri_ros2/src/client.cpp @@ -1,7 +1,7 @@ -#include "lbr_fri_ros2/lbr_client.hpp" +#include "lbr_fri_ros2/client.hpp" namespace lbr_fri_ros2 { -LBRClient::LBRClient(const rclcpp::Node::SharedPtr node, +Client::Client(const rclcpp::Node::SharedPtr node, std::unique_ptr lbr_command_guard) : node_(node), lbr_command_guard_(std::move(lbr_command_guard)), external_torque_filter_(node, "external_torque"), @@ -15,26 +15,26 @@ LBRClient::LBRClient(const rclcpp::Node::SharedPtr node, get_parameters_(); } -void LBRClient::log_status() { - RCLCPP_INFO(node_->get_logger(), "LBRClient - Publisher missed deadlines: %u", +void Client::log_status() { + RCLCPP_INFO(node_->get_logger(), "Client - Publisher missed deadlines: %u", missed_deadlines_pub_); - RCLCPP_INFO(node_->get_logger(), "LBRClient - Subscription missed deadlines: %u", + RCLCPP_INFO(node_->get_logger(), "Client - Subscription missed deadlines: %u", missed_deadlines_sub_); } -void LBRClient::onStateChange(KUKA::FRI::ESessionState old_state, +void Client::onStateChange(KUKA::FRI::ESessionState old_state, KUKA::FRI::ESessionState new_state) { init_topics_(); init_filters_(); RCLCPP_INFO(node_->get_logger(), "LBR switched from %s to %s.", KUKA_FRI_STATE_MAP[old_state].c_str(), KUKA_FRI_STATE_MAP[new_state].c_str()); } -void LBRClient::monitor() { +void Client::monitor() { pub_lbr_state_(); init_lbr_command_(); } -void LBRClient::waitForCommand() { +void Client::waitForCommand() { KUKA::FRI::LBRClient::waitForCommand(); pub_lbr_state_(); init_lbr_command_(); @@ -48,7 +48,7 @@ void LBRClient::waitForCommand() { } } -void LBRClient::command() { +void Client::command() { pub_lbr_state_(); // compute command @@ -82,7 +82,7 @@ void LBRClient::command() { } } -void LBRClient::init_lbr_command_() { +void Client::init_lbr_command_() { std::memcpy(lbr_command_target_.joint_position.data(), robotState().getMeasuredJointPosition(), sizeof(double) * KUKA::FRI::LBRState::NUMBER_OF_JOINTS); lbr_command_target_.torque.fill(0.); @@ -90,7 +90,7 @@ void LBRClient::init_lbr_command_() { lbr_command_ = lbr_command_target_; } -void LBRClient::init_topics_() { +void Client::init_topics_() { if (topics_init_) { return; } @@ -121,13 +121,13 @@ void LBRClient::init_topics_() { .deadline( std::chrono::milliseconds(static_cast(robotState().getSampleTime() * 1e3))) .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE), - std::bind(&LBRClient::on_lbr_command_, this, std::placeholders::_1), sub_options, + std::bind(&Client::on_lbr_command_, this, std::placeholders::_1), sub_options, memory_strategy); topics_init_ = true; } -void LBRClient::init_filters_() { +void Client::init_filters_() { if (filters_init_) { return; } @@ -141,15 +141,15 @@ void LBRClient::init_filters_() { filters_init_ = true; } -void LBRClient::declare_parameters_() { +void Client::declare_parameters_() { if (!node_->has_parameter("open_loop")) { node_->declare_parameter("open_loop", true); } } -void LBRClient::get_parameters_() { open_loop_ = node_->get_parameter("open_loop").as_bool(); } +void Client::get_parameters_() { open_loop_ = node_->get_parameter("open_loop").as_bool(); } -void LBRClient::pub_lbr_state_() { +void Client::pub_lbr_state_() { lbr_state_.client_command_mode = robotState().getClientCommandMode(); std::memcpy(lbr_state_.commanded_joint_position.data(), robotState().getCommandedJointPosition(), sizeof(double) * KUKA::FRI::LBRState::NUMBER_OF_JOINTS); @@ -185,7 +185,7 @@ void LBRClient::pub_lbr_state_() { lbr_state_pub_->publish(lbr_state_); } -void LBRClient::on_lbr_command_(const lbr_fri_msgs::msg::LBRCommand::SharedPtr lbr_command) { +void Client::on_lbr_command_(const lbr_fri_msgs::msg::LBRCommand::SharedPtr lbr_command) { lbr_command_target_ = *lbr_command; } } // end of namespace lbr_fri_ros2 From f992defd75e75bf3e5f420e2dec8a7f05aa5963f Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 9 Aug 2023 10:32:46 +0100 Subject: [PATCH 17/33] LBRAppComponent -> AppComponent --- lbr_bringup/doc/lbr_bringup.rst | 2 +- lbr_fri_ros2/CMakeLists.txt | 18 +++++++++--------- lbr_fri_ros2/doc/lbr_fri_ros2.rst | 2 +- ...lbr_app_component.cpp => app_component.cpp} | 8 ++++---- ...lbr_app_component.hpp => app_component.hpp} | 10 +++++----- 5 files changed, 20 insertions(+), 20 deletions(-) rename lbr_fri_ros2/src/{lbr_app_component.cpp => app_component.cpp} (61%) rename lbr_fri_ros2/src/{lbr_app_component.hpp => app_component.hpp} (78%) diff --git a/lbr_bringup/doc/lbr_bringup.rst b/lbr_bringup/doc/lbr_bringup.rst index 5836acd5..b049ae89 100644 --- a/lbr_bringup/doc/lbr_bringup.rst +++ b/lbr_bringup/doc/lbr_bringup.rst @@ -69,7 +69,7 @@ Standalone launch is great for research. Only the the real robot is supported. I model:=iiwa7 # [iiwa7, iiwa14, med7, med14] \ robot_name:=lbr # any robot name -This runs the :lbr_fri_ros2:`LBRAppComponentLBRApp `, 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. +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. .. note:: For a list of available parameters, call ``ros2 launch lbr_fri_ros2 lbr_app.launch.py -s``. diff --git a/lbr_fri_ros2/CMakeLists.txt b/lbr_fri_ros2/CMakeLists.txt index 88a0d942..7efd28cc 100644 --- a/lbr_fri_ros2/CMakeLists.txt +++ b/lbr_fri_ros2/CMakeLists.txt @@ -71,33 +71,33 @@ install( DESTINATION share/lbr_fri_ros2 ) -# lbr_app_component -add_library(lbr_app_component +# app_component +add_library(app_component SHARED - src/lbr_app_component.cpp + src/app_component.cpp ) -target_include_directories(lbr_app_component +target_include_directories(app_component PRIVATE src ) -ament_target_dependencies(lbr_app_component +ament_target_dependencies(app_component rclcpp urdf rclcpp_components ) -target_link_libraries(lbr_app_component +target_link_libraries(app_component lbr_fri_ros2 ) -rclcpp_components_register_node(lbr_app_component - PLUGIN lbr_fri_ros2::LBRAppComponent +rclcpp_components_register_node(app_component + PLUGIN lbr_fri_ros2::AppComponent EXECUTABLE lbr_app ) install( - TARGETS lbr_app_component + TARGETS app_component LIBRARY DESTINATION lib RUNTIME DESTINATION lib/lbr_fri_ros2 ) diff --git a/lbr_fri_ros2/doc/lbr_fri_ros2.rst b/lbr_fri_ros2/doc/lbr_fri_ros2.rst index f62c4fb5..657813be 100644 --- a/lbr_fri_ros2/doc/lbr_fri_ros2.rst +++ b/lbr_fri_ros2/doc/lbr_fri_ros2.rst @@ -23,7 +23,7 @@ Quick Start This launch file does 2 things: - Loads the ``robot_description`` (to read joint limits) - - Runs the :lbr_fri_ros2:`LBRAppComponentLBRApp `, which has an instance of :lbr_fri_ros2:`App ` to + - 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 ` diff --git a/lbr_fri_ros2/src/lbr_app_component.cpp b/lbr_fri_ros2/src/app_component.cpp similarity index 61% rename from lbr_fri_ros2/src/lbr_app_component.cpp rename to lbr_fri_ros2/src/app_component.cpp index c5ec1bef..55811519 100644 --- a/lbr_fri_ros2/src/lbr_app_component.cpp +++ b/lbr_fri_ros2/src/app_component.cpp @@ -1,17 +1,17 @@ -#include "lbr_app_component.hpp" +#include "app_component.hpp" namespace lbr_fri_ros2 { -LBRAppComponent::LBRAppComponent(const rclcpp::NodeOptions &options) { +AppComponent::AppComponent(const rclcpp::NodeOptions &options) { lbr_node_ = std::make_shared("lbr", options); lbr_app_ = std::make_unique(lbr_node_); } rclcpp::node_interfaces::NodeBaseInterface::SharedPtr -LBRAppComponent::get_node_base_interface() const { +AppComponent::get_node_base_interface() const { return lbr_node_->get_node_base_interface(); } } // end of namespace lbr_fri_ros2 #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(lbr_fri_ros2::LBRAppComponent) +RCLCPP_COMPONENTS_REGISTER_NODE(lbr_fri_ros2::AppComponent) diff --git a/lbr_fri_ros2/src/lbr_app_component.hpp b/lbr_fri_ros2/src/app_component.hpp similarity index 78% rename from lbr_fri_ros2/src/lbr_app_component.hpp rename to lbr_fri_ros2/src/app_component.hpp index c759335a..88568b64 100644 --- a/lbr_fri_ros2/src/lbr_app_component.hpp +++ b/lbr_fri_ros2/src/app_component.hpp @@ -1,5 +1,5 @@ -#ifndef LBR_FRI_ROS2__LBR_APP_COMPONENT_HPP_ -#define LBR_FRI_ROS2__LBR_APP_COMPONENT_HPP_ +#ifndef LBR_FRI_ROS2__APP_COMPONENT_HPP_ +#define LBR_FRI_ROS2__APP_COMPONENT_HPP_ #include #include "rclcpp/rclcpp.hpp" @@ -11,14 +11,14 @@ namespace lbr_fri_ros2 { * @brief Component for instantiating #lbr_fri_ros2::App. * */ -class LBRAppComponent { +class AppComponent { public: /** - * @brief Construct a new LBRAppComponent object. + * @brief Construct a new AppComponent object. * * @param options Node options */ - LBRAppComponent(const rclcpp::NodeOptions &options); + AppComponent(const rclcpp::NodeOptions &options); /** * @brief Get the node base interface object. Implementing this is necessary for components via From 197545122d54d8e9c22fcdb7e578fa96fef5e785 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 9 Aug 2023 10:32:54 +0100 Subject: [PATCH 18/33] Better logging --- lbr_fri_ros2/src/client.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lbr_fri_ros2/src/client.cpp b/lbr_fri_ros2/src/client.cpp index 1328df44..8145f84c 100644 --- a/lbr_fri_ros2/src/client.cpp +++ b/lbr_fri_ros2/src/client.cpp @@ -26,7 +26,7 @@ void Client::onStateChange(KUKA::FRI::ESessionState old_state, KUKA::FRI::ESessionState new_state) { init_topics_(); init_filters_(); - RCLCPP_INFO(node_->get_logger(), "LBR switched from %s to %s.", + RCLCPP_INFO(node_->get_logger(), "Switched from %s to %s.", KUKA_FRI_STATE_MAP[old_state].c_str(), KUKA_FRI_STATE_MAP[new_state].c_str()); } void Client::monitor() { From 8a7de5d8e23d4b80b4e649e484467d61aec61bb2 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 9 Aug 2023 10:42:33 +0100 Subject: [PATCH 19/33] LBR_UTILS -> UTILS --- lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp index 08f712f1..d8782df0 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp @@ -1,5 +1,5 @@ -#ifndef LBR_FRI_ROS2__LBR_UTILS_HPP_ -#define LBR_FRI_ROS2__LBR_UTILS_HPP_ +#ifndef LBR_FRI_ROS2__UTILS_HPP_ +#define LBR_FRI_ROS2__UTILS_HPP_ #include #include @@ -82,4 +82,4 @@ class JointPIDArrayROS { }; } // end of namespace lbr_fri_ros2 -#endif // LBR_FRI_ROS2__LBR_UTILS_HPP_ +#endif // LBR_FRI_ROS2__UTILS_HPP_ From a761776899346910d5257803a96e76e658a2c756 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 9 Aug 2023 11:00:59 +0100 Subject: [PATCH 20/33] smoohting -> alpha, uint -> double --- lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp | 16 ++++++------- lbr_fri_ros2/src/utils.cpp | 26 ++++++++++----------- 2 files changed, 21 insertions(+), 21 deletions(-) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp index d8782df0..99de4959 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp @@ -21,21 +21,21 @@ namespace lbr_fri_ros2 { class ExponentialFilter { public: ExponentialFilter(); - ExponentialFilter(const std::uint16_t &cutoff_frequency, const double &sample_time); + ExponentialFilter(const double &cutoff_frequency, const double &sample_time); inline double compute(const double ¤t, const double &previous); - void set_cutoff_frequency(const std::uint16_t &cutoff_frequency, const double &sample_time); + void set_cutoff_frequency(const double &cutoff_frequency, const double &sample_time); inline const double &get_sample_time() const; - inline const double &get_smoothing_factor() const; + inline const double &get_alpha() const; protected: // https://dsp.stackexchange.com/questions/40462/exponential-moving-average-cut-off-frequency - double compute_smoothing_factor(const std::uint16_t &cutoff_frequency, const double &sample_time); - bool validate_smoothing_factor(const double &smoothing_factor); + double compute_alpha(const double &cutoff_frequency, const double &sample_time); + bool validate_alpha(const double &alpha); - std::uint16_t cutoff_frequency_; + double cutoff_frequency_; double sample_time_; - double smoothing_factor_; + double alpha_; }; class JointExponentialFilterArrayROS { @@ -51,7 +51,7 @@ class JointExponentialFilterArrayROS { const std::string ¶m_prefix = ""); void compute(const double *const current, ValueArrayType &previous); - void init(const std::uint16_t &cutoff_frequency, const double &sample_time); + void init(const double &cutoff_frequency, const double &sample_time); protected: ExponentialFilter exponential_filter_; diff --git a/lbr_fri_ros2/src/utils.cpp b/lbr_fri_ros2/src/utils.cpp index e30a399e..2db2fe5a 100644 --- a/lbr_fri_ros2/src/utils.cpp +++ b/lbr_fri_ros2/src/utils.cpp @@ -4,41 +4,41 @@ namespace lbr_fri_ros2 { ExponentialFilter::ExponentialFilter() : ExponentialFilter::ExponentialFilter(0, 0.0) {} -ExponentialFilter::ExponentialFilter(const std::uint16_t &cutoff_frequency, +ExponentialFilter::ExponentialFilter(const double &cutoff_frequency, const double &sample_time) { set_cutoff_frequency(cutoff_frequency, sample_time); } inline double ExponentialFilter::compute(const double ¤t, const double &previous) { - return filters::exponentialSmoothing(current, previous, smoothing_factor_); + return filters::exponentialSmoothing(current, previous, alpha_); } -void ExponentialFilter::set_cutoff_frequency(const std::uint16_t &cutoff_frequency, +void ExponentialFilter::set_cutoff_frequency(const double &cutoff_frequency, const double &sample_time) { cutoff_frequency_ = cutoff_frequency; if (cutoff_frequency_ > static_cast(1. / sample_time)) { cutoff_frequency_ = static_cast(1. / sample_time); } sample_time_ = sample_time; - smoothing_factor_ = compute_smoothing_factor(cutoff_frequency, sample_time); - if (!validate_smoothing_factor(smoothing_factor_)) { - throw std::runtime_error("Smoothing factor is not within [0, 1]."); + alpha_ = compute_alpha(cutoff_frequency, sample_time); + if (!validate_alpha(alpha_)) { + throw std::runtime_error("Alpha is not within [0, 1]."); } } inline const double &ExponentialFilter::get_sample_time() const { return sample_time_; } -inline const double &ExponentialFilter::get_smoothing_factor() const { return smoothing_factor_; } +inline const double &ExponentialFilter::get_alpha() const { return alpha_; } -double ExponentialFilter::compute_smoothing_factor(const std::uint16_t &cutoff_frequency, +double ExponentialFilter::compute_alpha(const double &cutoff_frequency, const double &sample_time) { double omega_3db = 2.0 * M_PI * sample_time * cutoff_frequency; return std::cos(omega_3db) - 1 + std::sqrt(std::pow(std::cos(omega_3db), 2) - 4 * std::cos(omega_3db) + 3); } -bool ExponentialFilter::validate_smoothing_factor(const double &smoothing_factor) { - return smoothing_factor <= 1. && smoothing_factor >= 0.; +bool ExponentialFilter::validate_alpha(const double &alpha) { + return alpha <= 1. && alpha >= 0.; } JointExponentialFilterArrayROS::JointExponentialFilterArrayROS(const rclcpp::Node::SharedPtr node, @@ -67,7 +67,7 @@ void JointExponentialFilterArrayROS::compute(const double *const current, }); } -void JointExponentialFilterArrayROS::init(const std::uint16_t &cutoff_frequency, +void JointExponentialFilterArrayROS::init(const double &cutoff_frequency, const double &sample_time) { if (!parameter_interface_->has_parameter(param_prefix_ + "cutoff_frequency")) { parameter_interface_->declare_parameter(param_prefix_ + "cutoff_frequency", @@ -85,8 +85,8 @@ void JointExponentialFilterArrayROS::init(const std::uint16_t &cutoff_frequency, exponential_filter_.set_cutoff_frequency(parameter.as_int(), exponential_filter_.get_sample_time()); RCLCPP_INFO(logging_interface_->get_logger(), - "Set %s to: %ld, new smoothing factor: %f.", parameter.get_name().c_str(), - parameter.as_int(), exponential_filter_.get_smoothing_factor()); + "Set %s to: %ld, new smoothing factor: %f. 0: now smoothing, 1: maximal smoothing", parameter.get_name().c_str(), + parameter.as_int(), 1. - exponential_filter_.get_alpha()); } } catch (const rclcpp::exceptions::InvalidParameterTypeException &e) { std::string info_msg = "Invalid parameter type: " + std::string(e.what()); From 3e6be0f1023a46aca80cef19828cda9ca14025ad Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 9 Aug 2023 11:03:46 +0100 Subject: [PATCH 21/33] formatted --- lbr_fri_ros2/include/lbr_fri_ros2/app.hpp | 6 +++--- lbr_fri_ros2/src/client.cpp | 10 ++++------ lbr_fri_ros2/src/utils.cpp | 16 +++++++--------- 3 files changed, 14 insertions(+), 18 deletions(-) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp index 6a07d51b..b580c1d8 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp @@ -17,8 +17,8 @@ #include "lbr_fri_msgs/srv/app_connect.hpp" #include "lbr_fri_msgs/srv/app_disconnect.hpp" -#include "lbr_fri_ros2/command_guard.hpp" #include "lbr_fri_ros2/client.hpp" +#include "lbr_fri_ros2/command_guard.hpp" namespace lbr_fri_ros2 { /** @@ -114,8 +114,8 @@ class App { /** * @brief Exchanges commands / states between ROS and the FRI. * - * Calls step() on #lbr_app_, which callbacks #lbr_client_. #lbr_client_ reads commands / write states - * through realtime safe topics. + * Calls step() on #lbr_app_, which callbacks #lbr_client_. #lbr_client_ reads commands / write + * states through realtime safe topics. * */ void run_(); diff --git a/lbr_fri_ros2/src/client.cpp b/lbr_fri_ros2/src/client.cpp index 8145f84c..6d165bbd 100644 --- a/lbr_fri_ros2/src/client.cpp +++ b/lbr_fri_ros2/src/client.cpp @@ -1,8 +1,7 @@ #include "lbr_fri_ros2/client.hpp" namespace lbr_fri_ros2 { -Client::Client(const rclcpp::Node::SharedPtr node, - std::unique_ptr lbr_command_guard) +Client::Client(const rclcpp::Node::SharedPtr node, std::unique_ptr lbr_command_guard) : node_(node), lbr_command_guard_(std::move(lbr_command_guard)), external_torque_filter_(node, "external_torque"), measured_torque_filter_(node, "measured_torque"), @@ -22,12 +21,11 @@ void Client::log_status() { missed_deadlines_sub_); } -void Client::onStateChange(KUKA::FRI::ESessionState old_state, - KUKA::FRI::ESessionState new_state) { +void Client::onStateChange(KUKA::FRI::ESessionState old_state, KUKA::FRI::ESessionState new_state) { init_topics_(); init_filters_(); - RCLCPP_INFO(node_->get_logger(), "Switched from %s to %s.", - KUKA_FRI_STATE_MAP[old_state].c_str(), KUKA_FRI_STATE_MAP[new_state].c_str()); + RCLCPP_INFO(node_->get_logger(), "Switched from %s to %s.", KUKA_FRI_STATE_MAP[old_state].c_str(), + KUKA_FRI_STATE_MAP[new_state].c_str()); } void Client::monitor() { pub_lbr_state_(); diff --git a/lbr_fri_ros2/src/utils.cpp b/lbr_fri_ros2/src/utils.cpp index 2db2fe5a..4a4b08ad 100644 --- a/lbr_fri_ros2/src/utils.cpp +++ b/lbr_fri_ros2/src/utils.cpp @@ -4,8 +4,7 @@ namespace lbr_fri_ros2 { ExponentialFilter::ExponentialFilter() : ExponentialFilter::ExponentialFilter(0, 0.0) {} -ExponentialFilter::ExponentialFilter(const double &cutoff_frequency, - const double &sample_time) { +ExponentialFilter::ExponentialFilter(const double &cutoff_frequency, const double &sample_time) { set_cutoff_frequency(cutoff_frequency, sample_time); } @@ -30,16 +29,13 @@ inline const double &ExponentialFilter::get_sample_time() const { return sample_ inline const double &ExponentialFilter::get_alpha() const { return alpha_; } -double ExponentialFilter::compute_alpha(const double &cutoff_frequency, - const double &sample_time) { +double ExponentialFilter::compute_alpha(const double &cutoff_frequency, const double &sample_time) { double omega_3db = 2.0 * M_PI * sample_time * cutoff_frequency; return std::cos(omega_3db) - 1 + std::sqrt(std::pow(std::cos(omega_3db), 2) - 4 * std::cos(omega_3db) + 3); } -bool ExponentialFilter::validate_alpha(const double &alpha) { - return alpha <= 1. && alpha >= 0.; -} +bool ExponentialFilter::validate_alpha(const double &alpha) { return alpha <= 1. && alpha >= 0.; } JointExponentialFilterArrayROS::JointExponentialFilterArrayROS(const rclcpp::Node::SharedPtr node, const std::string ¶m_prefix) @@ -85,8 +81,10 @@ void JointExponentialFilterArrayROS::init(const double &cutoff_frequency, exponential_filter_.set_cutoff_frequency(parameter.as_int(), exponential_filter_.get_sample_time()); RCLCPP_INFO(logging_interface_->get_logger(), - "Set %s to: %ld, new smoothing factor: %f. 0: now smoothing, 1: maximal smoothing", parameter.get_name().c_str(), - parameter.as_int(), 1. - exponential_filter_.get_alpha()); + "Set %s to: %ld, new smoothing factor: %f. 0: no smoothing, 1: maximal " + "smoothing.", + parameter.get_name().c_str(), parameter.as_int(), + 1. - exponential_filter_.get_alpha()); } } catch (const rclcpp::exceptions::InvalidParameterTypeException &e) { std::string info_msg = "Invalid parameter type: " + std::string(e.what()); From 0432f5b0dda92cf685759129ba9bb6df645ca186 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 9 Aug 2023 11:06:47 +0100 Subject: [PATCH 22/33] fixed int --- lbr_fri_ros2/src/utils.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/lbr_fri_ros2/src/utils.cpp b/lbr_fri_ros2/src/utils.cpp index 4a4b08ad..aba0071b 100644 --- a/lbr_fri_ros2/src/utils.cpp +++ b/lbr_fri_ros2/src/utils.cpp @@ -78,12 +78,12 @@ void JointExponentialFilterArrayROS::init(const double &cutoff_frequency, for (const auto ¶meter : parameters) { try { if (parameter.get_name() == param_prefix_ + "cutoff_frequency") { - exponential_filter_.set_cutoff_frequency(parameter.as_int(), + exponential_filter_.set_cutoff_frequency(parameter.as_double(), exponential_filter_.get_sample_time()); RCLCPP_INFO(logging_interface_->get_logger(), - "Set %s to: %ld, new smoothing factor: %f. 0: no smoothing, 1: maximal " + "Set %s to: %f, new smoothing factor: %f. 0: no smoothing, 1: maximal " "smoothing.", - parameter.get_name().c_str(), parameter.as_int(), + parameter.get_name().c_str(), parameter.as_double(), 1. - exponential_filter_.get_alpha()); } } catch (const rclcpp::exceptions::InvalidParameterTypeException &e) { From d4a09c2946e8baa8dc2b27b67c46b670afdb1402 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 9 Aug 2023 11:07:33 +0100 Subject: [PATCH 23/33] removed casts --- lbr_fri_ros2/src/utils.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/lbr_fri_ros2/src/utils.cpp b/lbr_fri_ros2/src/utils.cpp index aba0071b..5fc214ac 100644 --- a/lbr_fri_ros2/src/utils.cpp +++ b/lbr_fri_ros2/src/utils.cpp @@ -15,8 +15,8 @@ inline double ExponentialFilter::compute(const double ¤t, const double &pr void ExponentialFilter::set_cutoff_frequency(const double &cutoff_frequency, const double &sample_time) { cutoff_frequency_ = cutoff_frequency; - if (cutoff_frequency_ > static_cast(1. / sample_time)) { - cutoff_frequency_ = static_cast(1. / sample_time); + if (cutoff_frequency_ > (1. / sample_time)) { + cutoff_frequency_ = (1. / sample_time); } sample_time_ = sample_time; alpha_ = compute_alpha(cutoff_frequency, sample_time); From 66d915e34205b31633c8b39a5876b85f710a4cfc Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 9 Aug 2023 12:05:16 +0100 Subject: [PATCH 24/33] fixed namespaces --- lbr_fri_ros2/include/lbr_fri_ros2/app.hpp | 1 - lbr_fri_ros2/src/app.cpp | 12 +++++------- lbr_fri_ros2/src/app_component.cpp | 3 ++- lbr_fri_ros2/src/utils.cpp | 14 +++++++------- 4 files changed, 14 insertions(+), 16 deletions(-) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp index b580c1d8..e0cc3f27 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp @@ -127,7 +127,6 @@ class App { const char *remote_host_; /**< The remote host's IP address.*/ int port_id_; /**< The UDP port id.*/ std::string robot_description_; /**< The robot description, read from node parameters.*/ - std::string robot_name_; /**< The robot name, read from node parameters.*/ std::string command_guard_variant_; /**< The command guard, read from node parameters.*/ int rt_prio_; /**< The realtime priority, read from node parameters.*/ diff --git a/lbr_fri_ros2/src/app.cpp b/lbr_fri_ros2/src/app.cpp index 4b1b5ce4..c1aa1bd8 100644 --- a/lbr_fri_ros2/src/app.cpp +++ b/lbr_fri_ros2/src/app.cpp @@ -8,12 +8,12 @@ App::App(const rclcpp::Node::SharedPtr node) : node_(node) { connected_ = false; app_connect_srv_ = node_->create_service( - robot_name_ + "/connect", + "~/connect", std::bind(&App::on_app_connect_, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default); app_disconnect_srv_ = node_->create_service( - robot_name_ + "/disconnect", + "~/disconnect", std::bind(&App::on_app_disconnect_, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default); @@ -63,13 +63,12 @@ void App::get_parameters_() { if (!node_->get_parameter("robot_description", robot_description_)) { throw std::runtime_error("Failed to receive robot_description parameter."); } - robot_name_ = node_->get_parameter("robot_name").as_string(); command_guard_variant_ = node_->get_parameter("command_guard_variant").as_string(); rt_prio_ = node_->get_parameter("rt_prio").as_int(); } void App::on_app_connect_(const lbr_fri_msgs::srv::AppConnect::Request::SharedPtr request, - lbr_fri_msgs::srv::AppConnect::Response::SharedPtr response) { + lbr_fri_msgs::srv::AppConnect::Response::SharedPtr response) { const char *remote_host = request->remote_host.empty() ? NULL : request->remote_host.c_str(); try { response->connected = connect_(request->port_id, remote_host); @@ -79,9 +78,8 @@ void App::on_app_connect_(const lbr_fri_msgs::srv::AppConnect::Request::SharedPt } } -void App::on_app_disconnect_( - const lbr_fri_msgs::srv::AppDisconnect::Request::SharedPtr /*request*/, - lbr_fri_msgs::srv::AppDisconnect::Response::SharedPtr response) { +void App::on_app_disconnect_(const lbr_fri_msgs::srv::AppDisconnect::Request::SharedPtr /*request*/, + lbr_fri_msgs::srv::AppDisconnect::Response::SharedPtr response) { try { response->disconnected = disconnect_(); } catch (const std::exception &e) { diff --git a/lbr_fri_ros2/src/app_component.cpp b/lbr_fri_ros2/src/app_component.cpp index 55811519..1e39312f 100644 --- a/lbr_fri_ros2/src/app_component.cpp +++ b/lbr_fri_ros2/src/app_component.cpp @@ -2,7 +2,8 @@ namespace lbr_fri_ros2 { AppComponent::AppComponent(const rclcpp::NodeOptions &options) { - lbr_node_ = std::make_shared("lbr", options); + lbr_node_ = std::make_shared( + "lbr", options); // default node name is "lbr", unless specified in the launch file lbr_app_ = std::make_unique(lbr_node_); } diff --git a/lbr_fri_ros2/src/utils.cpp b/lbr_fri_ros2/src/utils.cpp index 5fc214ac..72700fca 100644 --- a/lbr_fri_ros2/src/utils.cpp +++ b/lbr_fri_ros2/src/utils.cpp @@ -100,13 +100,13 @@ void JointExponentialFilterArrayROS::init(const double &cutoff_frequency, JointPIDArrayROS::JointPIDArrayROS(const rclcpp::Node::SharedPtr node, const NameArrayType &names, const std::string &prefix) : pid_controllers_(PIDArrayType{ - control_toolbox::PidROS{node, prefix + names[0]}, - control_toolbox::PidROS{node, prefix + names[1]}, - control_toolbox::PidROS{node, prefix + names[2]}, - control_toolbox::PidROS{node, prefix + names[3]}, - control_toolbox::PidROS{node, prefix + names[4]}, - control_toolbox::PidROS{node, prefix + names[5]}, - control_toolbox::PidROS{node, prefix + names[6]}, + control_toolbox::PidROS{node, std::string(node->get_name()) + "/" + prefix + names[0]}, + control_toolbox::PidROS{node, std::string(node->get_name()) + "/" + prefix + names[1]}, + control_toolbox::PidROS{node, std::string(node->get_name()) + "/" + prefix + names[2]}, + control_toolbox::PidROS{node, std::string(node->get_name()) + "/" + prefix + names[3]}, + control_toolbox::PidROS{node, std::string(node->get_name()) + "/" + prefix + names[4]}, + control_toolbox::PidROS{node, std::string(node->get_name()) + "/" + prefix + names[5]}, + control_toolbox::PidROS{node, std::string(node->get_name()) + "/" + prefix + names[6]}, }) {} void JointPIDArrayROS::compute(const ValueArrayType &command_target, const ValueArrayType &state, From f01114e3e128c94590ede77573a5855b42b3c4bc Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 9 Aug 2023 12:20:05 +0100 Subject: [PATCH 25/33] amended node name to launch files --- .../launch/joint_sine_overlay.launch.py | 1 - .../launch/torque_sine_overlay.launch.py | 1 - .../launch/wrench_sine_overlay.launch.py | 1 - .../launch/wrench_sine_overlay.launch.py | 1 - lbr_fri_ros2/launch/lbr_app.launch.py | 1 - lbr_fri_ros2/lbr_fri_ros2/launch_mixin.py | 9 +++++++-- 6 files changed, 7 insertions(+), 7 deletions(-) diff --git a/lbr_demos/lbr_demos_fri_ros2_cpp/launch/joint_sine_overlay.launch.py b/lbr_demos/lbr_demos_fri_ros2_cpp/launch/joint_sine_overlay.launch.py index 9d8a7cd0..a95be9d8 100644 --- a/lbr_demos/lbr_demos_fri_ros2_cpp/launch/joint_sine_overlay.launch.py +++ b/lbr_demos/lbr_demos_fri_ros2_cpp/launch/joint_sine_overlay.launch.py @@ -14,7 +14,6 @@ def generate_launch_description() -> LaunchDescription: LBRFRIROS2Mixin.node_lbr_app( parameters=[ robot_description, - LBRDescriptionMixin.param_robot_name(), ] ) ) diff --git a/lbr_demos/lbr_demos_fri_ros2_cpp/launch/torque_sine_overlay.launch.py b/lbr_demos/lbr_demos_fri_ros2_cpp/launch/torque_sine_overlay.launch.py index 7aaef72f..b2f9f9f7 100644 --- a/lbr_demos/lbr_demos_fri_ros2_cpp/launch/torque_sine_overlay.launch.py +++ b/lbr_demos/lbr_demos_fri_ros2_cpp/launch/torque_sine_overlay.launch.py @@ -14,7 +14,6 @@ def generate_launch_description() -> LaunchDescription: LBRFRIROS2Mixin.node_lbr_app( parameters=[ robot_description, - LBRDescriptionMixin.param_robot_name(), ] ) ) diff --git a/lbr_demos/lbr_demos_fri_ros2_cpp/launch/wrench_sine_overlay.launch.py b/lbr_demos/lbr_demos_fri_ros2_cpp/launch/wrench_sine_overlay.launch.py index 118e04c5..e823858d 100644 --- a/lbr_demos/lbr_demos_fri_ros2_cpp/launch/wrench_sine_overlay.launch.py +++ b/lbr_demos/lbr_demos_fri_ros2_cpp/launch/wrench_sine_overlay.launch.py @@ -14,7 +14,6 @@ def generate_launch_description() -> LaunchDescription: LBRFRIROS2Mixin.node_lbr_app( parameters=[ robot_description, - LBRDescriptionMixin.param_robot_name(), ] ) ) diff --git a/lbr_demos/lbr_demos_fri_ros2_python/launch/wrench_sine_overlay.launch.py b/lbr_demos/lbr_demos_fri_ros2_python/launch/wrench_sine_overlay.launch.py index f127da14..f6984290 100644 --- a/lbr_demos/lbr_demos_fri_ros2_python/launch/wrench_sine_overlay.launch.py +++ b/lbr_demos/lbr_demos_fri_ros2_python/launch/wrench_sine_overlay.launch.py @@ -14,7 +14,6 @@ def generate_launch_description() -> LaunchDescription: LBRFRIROS2Mixin.node_lbr_app( parameters=[ robot_description, - LBRDescriptionMixin.param_robot_name(), ] ) ) diff --git a/lbr_fri_ros2/launch/lbr_app.launch.py b/lbr_fri_ros2/launch/lbr_app.launch.py index e9f84543..31f7e6d8 100644 --- a/lbr_fri_ros2/launch/lbr_app.launch.py +++ b/lbr_fri_ros2/launch/lbr_app.launch.py @@ -16,7 +16,6 @@ def generate_launch_description() -> LaunchDescription: LBRFRIROS2Mixin.node_lbr_app( parameters=[ robot_description, - LBRDescriptionMixin.param_robot_name(), LBRFRIROS2Mixin.param_open_loop(), LBRFRIROS2Mixin.param_rt_prio(), ] diff --git a/lbr_fri_ros2/lbr_fri_ros2/launch_mixin.py b/lbr_fri_ros2/lbr_fri_ros2/launch_mixin.py index fe06c69b..79e8ae96 100644 --- a/lbr_fri_ros2/lbr_fri_ros2/launch_mixin.py +++ b/lbr_fri_ros2/lbr_fri_ros2/launch_mixin.py @@ -1,4 +1,4 @@ -from typing import Dict +from typing import Dict, Optional, Union from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration @@ -50,10 +50,15 @@ def param_rt_prio() -> Dict[str, LaunchConfiguration]: return {"rt_prio": LaunchConfiguration("rt_prio", default="80")} @staticmethod - def node_lbr_app(**kwargs) -> DeclareLaunchArgument: + def node_lbr_app( + robot_name: Optional[Union[LaunchConfiguration, str]] = None, **kwargs + ) -> DeclareLaunchArgument: + if robot_name is None: + robot_name = LaunchConfiguration("robot_name", default="lbr") return Node( package="lbr_fri_ros2", executable="lbr_app", + name=robot_name, emulate_tty=True, output="screen", **kwargs, From 37f5c3de83eec8407ecc1576a808e4c329ae47cd Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 9 Aug 2023 12:23:22 +0100 Subject: [PATCH 26/33] updated doc --- lbr_fri_ros2/doc/img/lbr_fri_ros2.drawio.svg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lbr_fri_ros2/doc/img/lbr_fri_ros2.drawio.svg b/lbr_fri_ros2/doc/img/lbr_fri_ros2.drawio.svg index 0c955d3b..4e3c3b6e 100644 --- a/lbr_fri_ros2/doc/img/lbr_fri_ros2.drawio.svg +++ b/lbr_fri_ros2/doc/img/lbr_fri_ros2.drawio.svg @@ -1,4 +1,4 @@ -
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 /lbr/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

# run_(): void
publish lbr_fri_msg::msg::LBRCommand
to /lbr/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 +
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

# 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 From 55f57e08b65a89db10387f2faec219c326d10b29 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 9 Aug 2023 12:41:19 +0100 Subject: [PATCH 27/33] added documentation --- lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp | 140 ++++++++++++++++++-- lbr_fri_ros2/src/utils.cpp | 10 +- 2 files changed, 130 insertions(+), 20 deletions(-) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp index 99de4959..41a47a61 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp @@ -20,22 +20,81 @@ namespace lbr_fri_ros2 { class ExponentialFilter { public: + /** + * @brief Construct a new Exponential Filter object. Performs exponential smoothing with a + * #cutoff_frequency_ according to + * https://dsp.stackexchange.com/questions/40462/exponential-moving-average-cut-off-frequency. + * + */ ExponentialFilter(); + + /** + * @brief Construct a new Exponential Filter object. Performs exponential smoothing with a + * #cutoff_frequency_ according to + * https://dsp.stackexchange.com/questions/40462/exponential-moving-average-cut-off-frequency. + * + * @param[in] cutoff_frequency Frequency in Hz. + * @param[in] sample_time Sample time in seconds. + */ ExponentialFilter(const double &cutoff_frequency, const double &sample_time); + /** + * @brief Compute the exponential smoothing using the control_toolbox + * https://github.com/ros-controls/control_toolbox. + * + * @param[in] current The current value. + * @param[in] previous The previous smoothed value. + * @return double The returned smoothed value. + */ inline double compute(const double ¤t, const double &previous); + + /** + * @brief Set the cutoff frequency object. Internally computes the new #alpha_. + * + * @param[in] cutoff_frequency Frequency in Hz. + * @param[in] sample_time Sample time in seconds. + */ void set_cutoff_frequency(const double &cutoff_frequency, const double &sample_time); + + /** + * @brief Get #sample_time_. + * + * @return const double& + */ inline const double &get_sample_time() const; + + /** + * @brief Get #alpha_. + * + * @return const double& + */ inline const double &get_alpha() const; protected: - // https://dsp.stackexchange.com/questions/40462/exponential-moving-average-cut-off-frequency - double compute_alpha(const double &cutoff_frequency, const double &sample_time); - bool validate_alpha(const double &alpha); - - double cutoff_frequency_; - double sample_time_; - double alpha_; + /** + * @brief Compute alpha given the cutoff frequency and the sample time. + * + * @param[in] cutoff_frequency Frequency in Hz. + * @param[in] sample_time Sample time in seconds. + * @return double Alpha based on + * https://dsp.stackexchange.com/questions/40462/exponential-moving-average-cut-off-frequency. + */ + double compute_alpha_(const double &cutoff_frequency, const double &sample_time); + + /** + * @brief Validate alpha in [0, 1]. + * + * @param[in] alpha Alpha parameter for smoothing. + * @return true if in [0, 1]. + * @return false if outside [0, 1]. + */ + bool validate_alpha_(const double &alpha); + + double cutoff_frequency_; /**< Frequency in Hz.*/ + double sample_time_; /**< Sample time in seconds.*/ + double + alpha_; /**< Alpha parameter based on + https://dsp.stackexchange.com/questions/40462/exponential-moving-average-cut-off-frequency.*/ }; class JointExponentialFilterArrayROS { @@ -43,22 +102,48 @@ class JointExponentialFilterArrayROS { public: JointExponentialFilterArrayROS() = delete; + + /** + * @brief Construct a new JointExponentialFilterArrayROS object. + * + * @param[in] node Shared node for logging and parameter handling. + * @param[in] param_prefix Parameter prefix is e.g. used as: param_prefix + "." + + * "cut_off_frequency". + */ JointExponentialFilterArrayROS(const rclcpp::Node::SharedPtr node, const std::string ¶m_prefix = ""); + + /** + * @brief Construct a new JointExponentialFilterArrayROS object. + * + * @param[in] logging_interface Logging interface. + * @param[in] parameter_interface Parameter interface. + * @param[in] param_prefix Parameter prefix is e.g. used as: param_prefix + "." + + * "cut_off_frequency". + */ JointExponentialFilterArrayROS( const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface, const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameter_interface, const std::string ¶m_prefix = ""); + /** + * @brief Compute the exponential smoothing for each joints using #exponential_filter_. + * + * @param[in] current The current joint values. + * @param[in, out] previous The previous smoothed joint values. Will be updated. + */ void compute(const double *const current, ValueArrayType &previous); void init(const double &cutoff_frequency, const double &sample_time); protected: - ExponentialFilter exponential_filter_; - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_; - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameter_interface_; - std::string param_prefix_; - rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_; + ExponentialFilter exponential_filter_; /**< Exponential filter applied to all joints.*/ + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr + logging_interface_; /**< Logging interface.*/ + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr + parameter_interface_; /** Parameter interface.*/ + std::string param_prefix_; /** Parameter prefix is used as "param_prefix" + "." + "param_name".*/ + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr + parameter_callback_handle_; /**< Parameter callback handle to update parameters.*/ }; class JointPIDArrayROS { @@ -68,18 +153,45 @@ class JointPIDArrayROS { public: JointPIDArrayROS() = delete; + + /** + * @brief Construct a new JointPIDArrayROS object. Used to control the LBRs joints. The parameters + * / topics are prefixed as std::string(node->get_name()) + "/" + prefix + names[i]. + * + * @param[in] node Shared node. + * @param[in] names Names of the joints. + * @param[in] prefix Prefix for the parameters. + */ JointPIDArrayROS(const rclcpp::Node::SharedPtr node, const NameArrayType &names, const std::string &prefix = ""); + /** + * @brief Compute the PID update. + * + * @param[in] command_target The target joint command. + * @param[in] state The current joint state. + * @param[in] dt The time step. + * @param[out] command The returned joint command. + */ void compute(const ValueArrayType &command_target, const ValueArrayType &state, const rclcpp::Duration &dt, ValueArrayType &command); + /** + * @brief Initialize the PID controllers. Sets all #pid_controllers_ to the same parameters, but + * can be overriden individually. + * + * @param[in] p The proportional gain. + * @param[in] i The integral gain. + * @param[in] d The derivative gain. + * @param[in] i_max The maximum integral value. + * @param[in] i_min The minimum integral value. + * @param[in] antiwindup Antiwindup enabled or disabled. + */ void init(const double &p, const double &i, const double &d, const double &i_max, const double &i_min, const bool &antiwindup); protected: - PIDArrayType pid_controllers_; + PIDArrayType pid_controllers_; /**< PID controllers for each joint.*/ }; - } // end of namespace lbr_fri_ros2 #endif // LBR_FRI_ROS2__UTILS_HPP_ diff --git a/lbr_fri_ros2/src/utils.cpp b/lbr_fri_ros2/src/utils.cpp index 72700fca..66296a66 100644 --- a/lbr_fri_ros2/src/utils.cpp +++ b/lbr_fri_ros2/src/utils.cpp @@ -1,7 +1,6 @@ #include "lbr_fri_ros2/utils.hpp" namespace lbr_fri_ros2 { - ExponentialFilter::ExponentialFilter() : ExponentialFilter::ExponentialFilter(0, 0.0) {} ExponentialFilter::ExponentialFilter(const double &cutoff_frequency, const double &sample_time) { @@ -19,8 +18,8 @@ void ExponentialFilter::set_cutoff_frequency(const double &cutoff_frequency, cutoff_frequency_ = (1. / sample_time); } sample_time_ = sample_time; - alpha_ = compute_alpha(cutoff_frequency, sample_time); - if (!validate_alpha(alpha_)) { + alpha_ = compute_alpha_(cutoff_frequency, sample_time); + if (!validate_alpha_(alpha_)) { throw std::runtime_error("Alpha is not within [0, 1]."); } } @@ -29,13 +28,13 @@ inline const double &ExponentialFilter::get_sample_time() const { return sample_ inline const double &ExponentialFilter::get_alpha() const { return alpha_; } -double ExponentialFilter::compute_alpha(const double &cutoff_frequency, const double &sample_time) { +double ExponentialFilter::compute_alpha_(const double &cutoff_frequency, const double &sample_time) { double omega_3db = 2.0 * M_PI * sample_time * cutoff_frequency; return std::cos(omega_3db) - 1 + std::sqrt(std::pow(std::cos(omega_3db), 2) - 4 * std::cos(omega_3db) + 3); } -bool ExponentialFilter::validate_alpha(const double &alpha) { return alpha <= 1. && alpha >= 0.; } +bool ExponentialFilter::validate_alpha_(const double &alpha) { return alpha <= 1. && alpha >= 0.; } JointExponentialFilterArrayROS::JointExponentialFilterArrayROS(const rclcpp::Node::SharedPtr node, const std::string ¶m_prefix) @@ -123,5 +122,4 @@ void JointPIDArrayROS::init(const double &p, const double &i, const double &d, c std::for_each(pid_controllers_.begin(), pid_controllers_.end(), [&](auto &pid) { pid.initPid(p, i, d, i_max, i_min, antiwindup); }); } - } // end of namespace lbr_fri_ros2 From f79a815a05573f5b0c842d3661ccae78c5ffcad4 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 9 Aug 2023 12:45:04 +0100 Subject: [PATCH 28/33] removed legacy param --- lbr_fri_ros2/src/app.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/lbr_fri_ros2/src/app.cpp b/lbr_fri_ros2/src/app.cpp index c1aa1bd8..d9165a27 100644 --- a/lbr_fri_ros2/src/app.cpp +++ b/lbr_fri_ros2/src/app.cpp @@ -39,9 +39,6 @@ void App::declare_parameters_() { if (!node_->has_parameter("robot_description")) { node_->declare_parameter("robot_description", ""); } - if (!node_->has_parameter("robot_name")) { - node_->declare_parameter("robot_name", "lbr"); - } if (!node_->has_parameter("command_guard_variant")) { node_->declare_parameter("command_guard_variant", "safe_stop"); } From 64aa796648d97b9b2e06c5d27dbfa411e770dcec Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 9 Aug 2023 13:51:45 +0100 Subject: [PATCH 29/33] simplified app namespace --- lbr_bringup/doc/lbr_bringup.rst | 4 +- .../src/admittance_control_node.cpp | 8 +- .../lbr_demos_fri_ros2_advanced_python.rst | 2 +- .../launch/joint_sine_overlay.launch.py | 2 +- .../launch/torque_sine_overlay.launch.py | 2 +- .../launch/wrench_sine_overlay.launch.py | 2 +- .../launch/joint_sine_overlay.launch.py | 3 +- .../launch/torque_sine_overlay.launch.py | 3 +- .../launch/wrench_sine_overlay.launch.py | 2 +- lbr_fri_ros2/CMakeLists.txt | 2 +- lbr_fri_ros2/doc/lbr_fri_ros2.rst | 6 +- lbr_fri_ros2/include/lbr_fri_ros2/app.hpp | 4 +- lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp | 2 +- .../{lbr_app.launch.py => app.launch.py} | 2 +- lbr_fri_ros2/lbr_fri_ros2/launch_mixin.py | 6 +- lbr_fri_ros2/src/app.cpp | 12 +-- lbr_fri_ros2/src/app_component.cpp | 7 +- lbr_fri_ros2/src/app_component.hpp | 4 +- lbr_fri_ros2/src/client.cpp | 10 +-- lbr_fri_ros2/src/utils.cpp | 17 ++-- .../lbr_hardware_interface.hpp | 4 +- ...launch.py => hardware_interface.launch.py} | 0 .../src/lbr_hardware_interface.cpp | 83 +++++++++---------- 23 files changed, 92 insertions(+), 95 deletions(-) rename lbr_fri_ros2/launch/{lbr_app.launch.py => app.launch.py} (95%) rename lbr_hardware_interface/launch/{lbr_hardware_interface.launch.py => hardware_interface.launch.py} (100%) diff --git a/lbr_bringup/doc/lbr_bringup.rst b/lbr_bringup/doc/lbr_bringup.rst index b049ae89..3e5437fb 100644 --- a/lbr_bringup/doc/lbr_bringup.rst +++ b/lbr_bringup/doc/lbr_bringup.rst @@ -65,14 +65,14 @@ Standalone launch is great for research. Only the the real robot is supported. I .. code:: bash - ros2 launch lbr_fri_ros2 lbr_app.launch.py \ + ros2 launch lbr_fri_ros2 app.launch.py \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] \ robot_name:=lbr # any robot name 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. .. note:: - For a list of available parameters, call ``ros2 launch lbr_fri_ros2 lbr_app.launch.py -s``. + For a list of available parameters, call ``ros2 launch lbr_fri_ros2 app.launch.py -s``. Troubleshooting --------------- diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_control_node.cpp b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_control_node.cpp index 07fb3877..f3c6d44f 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_control_node.cpp +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_control_node.cpp @@ -75,15 +75,15 @@ int main(int argc, char **argv) { auto executor = std::make_shared(); - auto lbr_node = - std::make_shared("lbr", rclcpp::NodeOptions().use_intra_process_comms(true)); + auto app_node = + std::make_shared("app", rclcpp::NodeOptions().use_intra_process_comms(true)); - auto lbr_app = lbr_fri_ros2::App(lbr_node); + auto app = lbr_fri_ros2::App(app_node); auto admittance_control_node = std::make_shared( "admittance_control_node", rclcpp::NodeOptions().use_intra_process_comms(true)); - executor->add_node(lbr_node); + executor->add_node(app_node); executor->add_node(admittance_control_node); executor->spin(); diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/doc/lbr_demos_fri_ros2_advanced_python.rst b/lbr_demos/lbr_demos_fri_ros2_advanced_python/doc/lbr_demos_fri_ros2_advanced_python.rst index 9e6ab0e1..98d7338d 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_python/doc/lbr_demos_fri_ros2_advanced_python.rst +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_python/doc/lbr_demos_fri_ros2_advanced_python.rst @@ -18,7 +18,7 @@ Admittance Controller .. code-block:: bash - ros2 launch lbr_fri_ros2 lbr_app.launch.py model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + ros2 launch lbr_fri_ros2 app.launch.py model:=iiwa7 # [iiwa7, iiwa14, med7, med14] #. Run the `admittance_control_node `_: diff --git a/lbr_demos/lbr_demos_fri_ros2_cpp/launch/joint_sine_overlay.launch.py b/lbr_demos/lbr_demos_fri_ros2_cpp/launch/joint_sine_overlay.launch.py index a95be9d8..321d3e3f 100644 --- a/lbr_demos/lbr_demos_fri_ros2_cpp/launch/joint_sine_overlay.launch.py +++ b/lbr_demos/lbr_demos_fri_ros2_cpp/launch/joint_sine_overlay.launch.py @@ -11,7 +11,7 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(LBRDescriptionMixin.arg_robot_name()) robot_description = LBRDescriptionMixin.param_robot_description(sim=False) ld.add_action( - LBRFRIROS2Mixin.node_lbr_app( + LBRFRIROS2Mixin.node_app( parameters=[ robot_description, ] diff --git a/lbr_demos/lbr_demos_fri_ros2_cpp/launch/torque_sine_overlay.launch.py b/lbr_demos/lbr_demos_fri_ros2_cpp/launch/torque_sine_overlay.launch.py index b2f9f9f7..ace9a08d 100644 --- a/lbr_demos/lbr_demos_fri_ros2_cpp/launch/torque_sine_overlay.launch.py +++ b/lbr_demos/lbr_demos_fri_ros2_cpp/launch/torque_sine_overlay.launch.py @@ -11,7 +11,7 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(LBRDescriptionMixin.arg_robot_name()) robot_description = LBRDescriptionMixin.param_robot_description(sim=False) ld.add_action( - LBRFRIROS2Mixin.node_lbr_app( + LBRFRIROS2Mixin.node_app( parameters=[ robot_description, ] diff --git a/lbr_demos/lbr_demos_fri_ros2_cpp/launch/wrench_sine_overlay.launch.py b/lbr_demos/lbr_demos_fri_ros2_cpp/launch/wrench_sine_overlay.launch.py index e823858d..1b6e210a 100644 --- a/lbr_demos/lbr_demos_fri_ros2_cpp/launch/wrench_sine_overlay.launch.py +++ b/lbr_demos/lbr_demos_fri_ros2_cpp/launch/wrench_sine_overlay.launch.py @@ -11,7 +11,7 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(LBRDescriptionMixin.arg_robot_name()) robot_description = LBRDescriptionMixin.param_robot_description(sim=False) ld.add_action( - LBRFRIROS2Mixin.node_lbr_app( + LBRFRIROS2Mixin.node_app( parameters=[ robot_description, ] diff --git a/lbr_demos/lbr_demos_fri_ros2_python/launch/joint_sine_overlay.launch.py b/lbr_demos/lbr_demos_fri_ros2_python/launch/joint_sine_overlay.launch.py index a3f0c56d..0eee126d 100644 --- a/lbr_demos/lbr_demos_fri_ros2_python/launch/joint_sine_overlay.launch.py +++ b/lbr_demos/lbr_demos_fri_ros2_python/launch/joint_sine_overlay.launch.py @@ -11,10 +11,9 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(LBRDescriptionMixin.arg_robot_name()) robot_description = LBRDescriptionMixin.param_robot_description(sim=False) ld.add_action( - LBRFRIROS2Mixin.node_lbr_app( + LBRFRIROS2Mixin.node_app( parameters=[ robot_description, - LBRDescriptionMixin.param_robot_name(), ] ) ) diff --git a/lbr_demos/lbr_demos_fri_ros2_python/launch/torque_sine_overlay.launch.py b/lbr_demos/lbr_demos_fri_ros2_python/launch/torque_sine_overlay.launch.py index c7d57742..c4769a14 100644 --- a/lbr_demos/lbr_demos_fri_ros2_python/launch/torque_sine_overlay.launch.py +++ b/lbr_demos/lbr_demos_fri_ros2_python/launch/torque_sine_overlay.launch.py @@ -11,10 +11,9 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(LBRDescriptionMixin.arg_robot_name()) robot_description = LBRDescriptionMixin.param_robot_description(sim=False) ld.add_action( - LBRFRIROS2Mixin.node_lbr_app( + LBRFRIROS2Mixin.node_app( parameters=[ robot_description, - LBRDescriptionMixin.param_robot_name(), ] ) ) diff --git a/lbr_demos/lbr_demos_fri_ros2_python/launch/wrench_sine_overlay.launch.py b/lbr_demos/lbr_demos_fri_ros2_python/launch/wrench_sine_overlay.launch.py index f6984290..c6a3e565 100644 --- a/lbr_demos/lbr_demos_fri_ros2_python/launch/wrench_sine_overlay.launch.py +++ b/lbr_demos/lbr_demos_fri_ros2_python/launch/wrench_sine_overlay.launch.py @@ -11,7 +11,7 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(LBRDescriptionMixin.arg_robot_name()) robot_description = LBRDescriptionMixin.param_robot_description(sim=False) ld.add_action( - LBRFRIROS2Mixin.node_lbr_app( + LBRFRIROS2Mixin.node_app( parameters=[ robot_description, ] diff --git a/lbr_fri_ros2/CMakeLists.txt b/lbr_fri_ros2/CMakeLists.txt index 7efd28cc..0a51037f 100644 --- a/lbr_fri_ros2/CMakeLists.txt +++ b/lbr_fri_ros2/CMakeLists.txt @@ -93,7 +93,7 @@ target_link_libraries(app_component rclcpp_components_register_node(app_component PLUGIN lbr_fri_ros2::AppComponent - EXECUTABLE lbr_app + EXECUTABLE app ) install( diff --git a/lbr_fri_ros2/doc/lbr_fri_ros2.rst b/lbr_fri_ros2/doc/lbr_fri_ros2.rst index 657813be..c420f8a8 100644 --- a/lbr_fri_ros2/doc/lbr_fri_ros2.rst +++ b/lbr_fri_ros2/doc/lbr_fri_ros2.rst @@ -14,11 +14,11 @@ Quick Start .. thumbnail:: ../../lbr_demos/doc/img/applications_lbr_server.png -#. Run the :lbr_fri_ros2:`App ` node via `lbr_app.launch.py `_: +#. Run the :lbr_fri_ros2:`App ` node via `app.launch.py `_: .. code-block:: bash - ros2 launch lbr_fri_ros2 lbr_app.launch.py model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + ros2 launch lbr_fri_ros2 app.launch.py model:=iiwa7 # [iiwa7, iiwa14, med7, med14] This launch file does 2 things: @@ -33,7 +33,7 @@ The topic names change with the robot's name. When running .. code-block:: bash - ros2 launch lbr_fri_ros2 lbr_app.launch.py robot_name:=lbr_1 model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + 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``. diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp index e0cc3f27..c93f500a 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp @@ -114,7 +114,7 @@ class App { /** * @brief Exchanges commands / states between ROS and the FRI. * - * Calls step() on #lbr_app_, which callbacks #lbr_client_. #lbr_client_ reads commands / write + * Calls step() on #app_, which callbacks #lbr_client_. #lbr_client_ reads commands / write * states through realtime safe topics. * */ @@ -141,7 +141,7 @@ class App { std::unique_ptr connection_; /**< UDP connection for reading states / writing commands.*/ std::unique_ptr - lbr_app_; /**< FRI client application that callbacks #lbr_client_ methods.*/ + app_; /**< FRI client application that callbacks #lbr_client_ methods.*/ }; } // end of namespace lbr_fri_ros2 #endif // LBR_FRI_ROS2__APP_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp index 41a47a61..372e0cd8 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp @@ -156,7 +156,7 @@ class JointPIDArrayROS { /** * @brief Construct a new JointPIDArrayROS object. Used to control the LBRs joints. The parameters - * / topics are prefixed as std::string(node->get_name()) + "/" + prefix + names[i]. + * / topics are prefixed as prefix + names[i]. * * @param[in] node Shared node. * @param[in] names Names of the joints. diff --git a/lbr_fri_ros2/launch/lbr_app.launch.py b/lbr_fri_ros2/launch/app.launch.py similarity index 95% rename from lbr_fri_ros2/launch/lbr_app.launch.py rename to lbr_fri_ros2/launch/app.launch.py index 31f7e6d8..e4d4e810 100644 --- a/lbr_fri_ros2/launch/lbr_app.launch.py +++ b/lbr_fri_ros2/launch/app.launch.py @@ -13,7 +13,7 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(LBRFRIROS2Mixin.arg_open_loop()) ld.add_action(LBRFRIROS2Mixin.arg_rt_prio()) ld.add_action( - LBRFRIROS2Mixin.node_lbr_app( + LBRFRIROS2Mixin.node_app( parameters=[ robot_description, LBRFRIROS2Mixin.param_open_loop(), diff --git a/lbr_fri_ros2/lbr_fri_ros2/launch_mixin.py b/lbr_fri_ros2/lbr_fri_ros2/launch_mixin.py index 79e8ae96..ea7507d5 100644 --- a/lbr_fri_ros2/lbr_fri_ros2/launch_mixin.py +++ b/lbr_fri_ros2/lbr_fri_ros2/launch_mixin.py @@ -50,15 +50,15 @@ def param_rt_prio() -> Dict[str, LaunchConfiguration]: return {"rt_prio": LaunchConfiguration("rt_prio", default="80")} @staticmethod - def node_lbr_app( + def node_app( robot_name: Optional[Union[LaunchConfiguration, str]] = None, **kwargs ) -> DeclareLaunchArgument: if robot_name is None: robot_name = LaunchConfiguration("robot_name", default="lbr") return Node( package="lbr_fri_ros2", - executable="lbr_app", - name=robot_name, + executable="app", + namespace=robot_name, emulate_tty=True, output="screen", **kwargs, diff --git a/lbr_fri_ros2/src/app.cpp b/lbr_fri_ros2/src/app.cpp index d9165a27..8a1099b7 100644 --- a/lbr_fri_ros2/src/app.cpp +++ b/lbr_fri_ros2/src/app.cpp @@ -8,12 +8,12 @@ App::App(const rclcpp::Node::SharedPtr node) : node_(node) { connected_ = false; app_connect_srv_ = node_->create_service( - "~/connect", + "connect", std::bind(&App::on_app_connect_, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default); app_disconnect_srv_ = node_->create_service( - "~/disconnect", + "disconnect", std::bind(&App::on_app_disconnect_, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default); @@ -21,7 +21,7 @@ App::App(const rclcpp::Node::SharedPtr node) : node_(node) { node_, lbr_command_guard_factory(node_->get_node_logging_interface(), robot_description_, command_guard_variant_)); connection_ = std::make_unique(); - lbr_app_ = std::make_unique(*connection_, *lbr_client_); + app_ = std::make_unique(*connection_, *lbr_client_); // attempt default connect connect_(port_id_, remote_host_); @@ -100,7 +100,7 @@ bool App::connect_(const int &port_id, const char *const remote_host) { if (!valid_port_(port_id)) { throw std::range_error("Invalid port_id provided."); } - connected_ = lbr_app_->connect(port_id, remote_host); + connected_ = app_->connect(port_id, remote_host); if (connected_) { port_id_ = port_id; remote_host_ = remote_host; @@ -121,7 +121,7 @@ bool App::disconnect_() { RCLCPP_INFO(node_->get_logger(), "Attempting to close UDP socket with port_id %d for LBR server...", port_id_); if (connected_) { - lbr_app_->disconnect(); + app_->disconnect(); connected_ = false; } else { RCLCPP_INFO(node_->get_logger(), "Port already closed."); @@ -150,7 +150,7 @@ void App::run_() { bool success = true; while (success && connected_ && rclcpp::ok()) { - success = lbr_app_->step(); + success = app_->step(); if (lbr_client_->robotState().getSessionState() == KUKA::FRI::IDLE) { break; } diff --git a/lbr_fri_ros2/src/app_component.cpp b/lbr_fri_ros2/src/app_component.cpp index 1e39312f..7bfb0301 100644 --- a/lbr_fri_ros2/src/app_component.cpp +++ b/lbr_fri_ros2/src/app_component.cpp @@ -2,14 +2,13 @@ namespace lbr_fri_ros2 { AppComponent::AppComponent(const rclcpp::NodeOptions &options) { - lbr_node_ = std::make_shared( - "lbr", options); // default node name is "lbr", unless specified in the launch file - lbr_app_ = std::make_unique(lbr_node_); + app_node_ = std::make_shared("app", options); + app_ = std::make_unique(app_node_); } rclcpp::node_interfaces::NodeBaseInterface::SharedPtr AppComponent::get_node_base_interface() const { - return lbr_node_->get_node_base_interface(); + return app_node_->get_node_base_interface(); } } // end of namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/app_component.hpp b/lbr_fri_ros2/src/app_component.hpp index 88568b64..1a4a1f99 100644 --- a/lbr_fri_ros2/src/app_component.hpp +++ b/lbr_fri_ros2/src/app_component.hpp @@ -30,8 +30,8 @@ class AppComponent { protected: std::unique_ptr - lbr_app_; /** #lbr_fri_ros2::App for communicating with the hardware.<*/ - rclcpp::Node::SharedPtr lbr_node_; /** Node for communicating with ROS.<*/ + app_; /** #lbr_fri_ros2::App for communicating with the hardware.<*/ + rclcpp::Node::SharedPtr app_node_; /** Node for communicating with ROS.<*/ }; } // end of namespace lbr_fri_ros2 #endif // LBR_FRI_ROS2__LBR_COMPONENT_HPP_ diff --git a/lbr_fri_ros2/src/client.cpp b/lbr_fri_ros2/src/client.cpp index 6d165bbd..6fc5054b 100644 --- a/lbr_fri_ros2/src/client.cpp +++ b/lbr_fri_ros2/src/client.cpp @@ -99,10 +99,10 @@ void Client::init_topics_() { }; lbr_state_pub_ = node_->create_publisher( - "~/state", rclcpp::QoS(1) - .deadline(std::chrono::milliseconds( - static_cast(robotState().getSampleTime() * 1e3))) - .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE)); + "state", rclcpp::QoS(1) + .deadline(std::chrono::milliseconds( + static_cast(robotState().getSampleTime() * 1e3))) + .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE)); auto memory_strategy = rclcpp::strategies::message_pool_memory_strategy::MessagePoolMemoryStrategy< @@ -114,7 +114,7 @@ void Client::init_topics_() { }; lbr_command_sub_ = node_->create_subscription( - "~/command", + "command", rclcpp::QoS(1) .deadline( std::chrono::milliseconds(static_cast(robotState().getSampleTime() * 1e3))) diff --git a/lbr_fri_ros2/src/utils.cpp b/lbr_fri_ros2/src/utils.cpp index 66296a66..2d474fcb 100644 --- a/lbr_fri_ros2/src/utils.cpp +++ b/lbr_fri_ros2/src/utils.cpp @@ -28,7 +28,8 @@ inline const double &ExponentialFilter::get_sample_time() const { return sample_ inline const double &ExponentialFilter::get_alpha() const { return alpha_; } -double ExponentialFilter::compute_alpha_(const double &cutoff_frequency, const double &sample_time) { +double ExponentialFilter::compute_alpha_(const double &cutoff_frequency, + const double &sample_time) { double omega_3db = 2.0 * M_PI * sample_time * cutoff_frequency; return std::cos(omega_3db) - 1 + std::sqrt(std::pow(std::cos(omega_3db), 2) - 4 * std::cos(omega_3db) + 3); @@ -99,13 +100,13 @@ void JointExponentialFilterArrayROS::init(const double &cutoff_frequency, JointPIDArrayROS::JointPIDArrayROS(const rclcpp::Node::SharedPtr node, const NameArrayType &names, const std::string &prefix) : pid_controllers_(PIDArrayType{ - control_toolbox::PidROS{node, std::string(node->get_name()) + "/" + prefix + names[0]}, - control_toolbox::PidROS{node, std::string(node->get_name()) + "/" + prefix + names[1]}, - control_toolbox::PidROS{node, std::string(node->get_name()) + "/" + prefix + names[2]}, - control_toolbox::PidROS{node, std::string(node->get_name()) + "/" + prefix + names[3]}, - control_toolbox::PidROS{node, std::string(node->get_name()) + "/" + prefix + names[4]}, - control_toolbox::PidROS{node, std::string(node->get_name()) + "/" + prefix + names[5]}, - control_toolbox::PidROS{node, std::string(node->get_name()) + "/" + prefix + names[6]}, + control_toolbox::PidROS{node, prefix + names[0]}, + control_toolbox::PidROS{node, prefix + names[1]}, + control_toolbox::PidROS{node, prefix + names[2]}, + control_toolbox::PidROS{node, prefix + names[3]}, + control_toolbox::PidROS{node, prefix + names[4]}, + control_toolbox::PidROS{node, prefix + names[5]}, + control_toolbox::PidROS{node, prefix + names[6]}, }) {} void JointPIDArrayROS::compute(const ValueArrayType &command_target, const ValueArrayType &state, diff --git a/lbr_hardware_interface/include/lbr_hardware_interface/lbr_hardware_interface.hpp b/lbr_hardware_interface/include/lbr_hardware_interface/lbr_hardware_interface.hpp index 433c6709..c3f4f421 100644 --- a/lbr_hardware_interface/include/lbr_hardware_interface/lbr_hardware_interface.hpp +++ b/lbr_hardware_interface/include/lbr_hardware_interface/lbr_hardware_interface.hpp @@ -82,8 +82,8 @@ class LBRHardwareInterface : public hardware_interface::SystemInterface { const uint8_t LBR_FRI_SENSOR_SIZE = 12; // node for handling communication - rclcpp::Node::SharedPtr lbr_node_; - std::unique_ptr lbr_app_; + rclcpp::Node::SharedPtr app_node_; + std::unique_ptr app_; // exposed state interfaces double hw_sample_time_; diff --git a/lbr_hardware_interface/launch/lbr_hardware_interface.launch.py b/lbr_hardware_interface/launch/hardware_interface.launch.py similarity index 100% rename from lbr_hardware_interface/launch/lbr_hardware_interface.launch.py rename to lbr_hardware_interface/launch/hardware_interface.launch.py diff --git a/lbr_hardware_interface/src/lbr_hardware_interface.cpp b/lbr_hardware_interface/src/lbr_hardware_interface.cpp index 05e7520e..b46194f3 100644 --- a/lbr_hardware_interface/src/lbr_hardware_interface.cpp +++ b/lbr_hardware_interface/src/lbr_hardware_interface.cpp @@ -5,7 +5,7 @@ controller_interface::CallbackReturn LBRHardwareInterface::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(lbr_node_->get_logger(), "Failed to initialize SystemInterface."); + RCLCPP_ERROR(app_node_->get_logger(), "Failed to initialize SystemInterface."); return ret; } @@ -19,19 +19,18 @@ LBRHardwareInterface::on_init(const hardware_interface::HardwareInfo &system_inf "_" + info_.hardware_parameters["robot_name"]; // prefix with underscore to hide node if (port_id_ < 30200 || port_id_ > 30209) { - RCLCPP_ERROR(lbr_node_->get_logger(), "Expected port_id in [30200, 30209]. Found %d.", + RCLCPP_ERROR(app_node_->get_logger(), "Expected port_id in [30200, 30209]. Found %d.", port_id_); return controller_interface::CallbackReturn::ERROR; } // setup node - lbr_node_ = std::make_shared(robot_name_, + app_node_ = std::make_shared("app", robot_name_, rclcpp::NodeOptions().use_intra_process_comms(true)); - lbr_node_->declare_parameter("robot_name", robot_name_); - lbr_node_->declare_parameter("command_guard_variant", "default"); + app_node_->declare_parameter("command_guard_variant", "default"); - lbr_app_ = std::make_unique(lbr_node_); + app_ = std::make_unique(app_node_); init_command_interfaces_(); init_state_interfaces_(); @@ -63,7 +62,7 @@ LBRHardwareInterface::on_init(const hardware_interface::HardwareInfo &system_inf node_thread_ = std::make_unique([this]() { auto executor = std::make_shared(); - executor->add_node(lbr_node_); + executor->add_node(app_node_); executor->spin(); disconnect_(); }); @@ -144,7 +143,7 @@ hardware_interface::return_type LBRHardwareInterface::prepare_command_mode_switc controller_interface::CallbackReturn LBRHardwareInterface::on_activate(const rclcpp_lifecycle::State &) { if (!connect_()) { - RCLCPP_ERROR(lbr_node_->get_logger(), "Failed to connect to robot."); + RCLCPP_ERROR(app_node_->get_logger(), "Failed to connect to robot."); return controller_interface::CallbackReturn::ERROR; } return controller_interface::CallbackReturn::SUCCESS; @@ -153,7 +152,7 @@ LBRHardwareInterface::on_activate(const rclcpp_lifecycle::State &) { controller_interface::CallbackReturn LBRHardwareInterface::on_deactivate(const rclcpp_lifecycle::State &) { if (!disconnect_()) { - RCLCPP_ERROR(lbr_node_->get_logger(), "Failed to disconnect from robot."); + RCLCPP_ERROR(app_node_->get_logger(), "Failed to disconnect from robot."); return controller_interface::CallbackReturn::ERROR; } return controller_interface::CallbackReturn::SUCCESS; @@ -163,7 +162,7 @@ hardware_interface::return_type LBRHardwareInterface::read(const rclcpp::Time & const rclcpp::Duration & /*period*/) { if (exit_commanding_active_(static_cast(hw_session_state_), static_cast(lbr_state_.session_state))) { - RCLCPP_ERROR(lbr_node_->get_logger(), "LBR left COMMANDING_ACTIVE. Please re-run lbr_bringup."); + RCLCPP_ERROR(app_node_->get_logger(), "LBR left COMMANDING_ACTIVE. Please re-run lbr_bringup."); return hardware_interface::return_type::ERROR; } @@ -237,7 +236,7 @@ bool LBRHardwareInterface::wait_for_service_( bool spawned = false; uint8_t attempt = 0; while (attempt < attempts && !spawned && rclcpp::ok()) { - RCLCPP_INFO(lbr_node_->get_logger(), "Waiting for service %s...", client->get_service_name()); + RCLCPP_INFO(app_node_->get_logger(), "Waiting for service %s...", client->get_service_name()); spawned = client->wait_for_service(timeout); ++attempt; } @@ -276,7 +275,7 @@ void LBRHardwareInterface::init_state_interfaces_() { bool LBRHardwareInterface::verify_number_of_joints_() { if (info_.joints.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { - RCLCPP_ERROR(lbr_node_->get_logger(), "Expected %d joints in URDF. Found %ld.", + RCLCPP_ERROR(app_node_->get_logger(), "Expected %d joints in URDF. Found %ld.", KUKA::FRI::LBRState::NUMBER_OF_JOINTS, info_.joints.size()); return false; } @@ -288,7 +287,7 @@ bool LBRHardwareInterface::verify_joint_command_interfaces_() { for (auto &joint : info_.joints) { if (joint.command_interfaces.size() != LBR_FRI_COMMAND_INTERFACE_SIZE) { RCLCPP_ERROR( - lbr_node_->get_logger(), + app_node_->get_logger(), "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); return false; @@ -296,7 +295,7 @@ bool LBRHardwareInterface::verify_joint_command_interfaces_() { for (auto &ci : joint.command_interfaces) { if (ci.name != hardware_interface::HW_IF_POSITION && ci.name != hardware_interface::HW_IF_EFFORT) { - RCLCPP_ERROR(lbr_node_->get_logger(), + RCLCPP_ERROR(app_node_->get_logger(), "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); @@ -312,7 +311,7 @@ bool LBRHardwareInterface::verify_joint_state_interfaces_() { for (auto &joint : info_.joints) { if (joint.state_interfaces.size() != LBR_FRI_STATE_INTERFACE_SIZE) { RCLCPP_ERROR( - lbr_node_->get_logger(), + app_node_->get_logger(), "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); return false; @@ -324,7 +323,7 @@ bool LBRHardwareInterface::verify_joint_state_interfaces_() { si.name != HW_IF_EXTERNAL_TORQUE && si.name != HW_IF_IPO_JOINT_POSITION && si.name != hardware_interface::HW_IF_VELOCITY) { RCLCPP_ERROR( - lbr_node_->get_logger(), + app_node_->get_logger(), "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, @@ -340,14 +339,14 @@ bool LBRHardwareInterface::verify_joint_state_interfaces_() { bool LBRHardwareInterface::verify_sensors_() { // check lbr specific state interfaces if (info_.sensors.size() > 1) { - RCLCPP_ERROR(lbr_node_->get_logger(), "Expected 1 sensor, got %ld", info_.sensors.size()); + RCLCPP_ERROR(app_node_->get_logger(), "Expected 1 sensor, got %ld", info_.sensors.size()); return false; } // check all interfaces are defined in lbr.ros2_control.xacro const auto &lbr_fri_sensor = info_.sensors[0]; if (lbr_fri_sensor.state_interfaces.size() != LBR_FRI_SENSOR_SIZE) { - RCLCPP_ERROR(lbr_node_->get_logger(), + RCLCPP_ERROR(app_node_->get_logger(), "Sensor %s received invalid state interface. Received %ld, expected %d. ", lbr_fri_sensor.name.c_str(), lbr_fri_sensor.state_interfaces.size(), LBR_FRI_SENSOR_SIZE); @@ -364,7 +363,7 @@ bool LBRHardwareInterface::verify_sensors_() { 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(lbr_node_->get_logger(), "Sensor %s received invalid state interface %s.", + RCLCPP_ERROR(app_node_->get_logger(), "Sensor %s received invalid state interface %s.", lbr_fri_sensor.name.c_str(), si.name.c_str()); return false; @@ -374,7 +373,7 @@ bool LBRHardwareInterface::verify_sensors_() { } bool LBRHardwareInterface::spawn_com_layer_() { - if (!lbr_node_) { + if (!app_node_) { printf("No node provided.\n"); return false; } @@ -383,51 +382,51 @@ bool LBRHardwareInterface::spawn_com_layer_() { auto memory_strategy = rclcpp::strategies::message_pool_memory_strategy::MessagePoolMemoryStrategy< lbr_fri_msgs::msg::LBRState, 1>::make_shared(); - lbr_state_sub_ = lbr_node_->create_subscription( - "~/state", + lbr_state_sub_ = app_node_->create_subscription( + "state", rclcpp::QoS(1) .deadline(std::chrono::milliseconds(sample_time_)) .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE), std::bind(&LBRHardwareInterface::on_lbr_state_, this, std::placeholders::_1), rclcpp::SubscriptionOptions(), memory_strategy); - lbr_command_pub_ = lbr_node_->create_publisher( - "~/command", rclcpp::QoS(1) - .deadline(std::chrono::milliseconds(sample_time_)) - .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE)); + lbr_command_pub_ = app_node_->create_publisher( + "command", rclcpp::QoS(1) + .deadline(std::chrono::milliseconds(sample_time_)) + .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE)); } catch (const std::exception &e) { - RCLCPP_ERROR(lbr_node_->get_logger(), "Failed to spawn real time layer.\n%s.", e.what()); + RCLCPP_ERROR(app_node_->get_logger(), "Failed to spawn real time layer.\n%s.", e.what()); return false; } return true; } bool LBRHardwareInterface::spawn_clients_() { - if (!lbr_node_) { + if (!app_node_) { printf("No node provided.\n"); return false; } - std::string app_connect_service_name = "~/connect"; - app_connect_clt_ = lbr_node_->create_client( + std::string app_connect_service_name = "connect"; + app_connect_clt_ = app_node_->create_client( app_connect_service_name, rmw_qos_profile_services_default); - RCLCPP_INFO(lbr_node_->get_logger(), "Waiting for service %s to spawn...", + RCLCPP_INFO(app_node_->get_logger(), "Waiting for service %s to spawn...", app_connect_service_name.c_str()); if (!wait_for_service_(app_connect_clt_)) { - RCLCPP_ERROR(lbr_node_->get_logger(), "Failed."); + RCLCPP_ERROR(app_node_->get_logger(), "Failed."); return false; } - RCLCPP_INFO(lbr_node_->get_logger(), "Done."); + RCLCPP_INFO(app_node_->get_logger(), "Done."); - std::string app_disconnect_service_name = "~/disconnect"; - app_disconnect_clt_ = lbr_node_->create_client( + std::string app_disconnect_service_name = "disconnect"; + app_disconnect_clt_ = app_node_->create_client( app_disconnect_service_name, rmw_qos_profile_services_default); - RCLCPP_INFO(lbr_node_->get_logger(), "Waiting for service %s to spawn...", + RCLCPP_INFO(app_node_->get_logger(), "Waiting for service %s to spawn...", app_disconnect_service_name.c_str()); if (!wait_for_service_(app_disconnect_clt_)) { - RCLCPP_ERROR(lbr_node_->get_logger(), "Failed."); + RCLCPP_ERROR(app_node_->get_logger(), "Failed."); return false; } - RCLCPP_INFO(lbr_node_->get_logger(), "Done."); + RCLCPP_INFO(app_node_->get_logger(), "Done."); return true; } @@ -448,13 +447,13 @@ bool LBRHardwareInterface::connect_() { auto future = app_connect_clt_->async_send_request(connect_request); auto status = future.wait_for(std::chrono::seconds(1)); if (status != std::future_status::ready) { - RCLCPP_ERROR(lbr_node_->get_logger(), "Failed to request connect service %s.", + RCLCPP_ERROR(app_node_->get_logger(), "Failed to request connect service %s.", app_connect_clt_->get_service_name()); return false; } auto response = future.get(); if (!response->connected) { - RCLCPP_ERROR(lbr_node_->get_logger(), "Failed to connect.\n%s", response->message.c_str()); + RCLCPP_ERROR(app_node_->get_logger(), "Failed to connect.\n%s", response->message.c_str()); } return response->connected; } @@ -464,13 +463,13 @@ bool LBRHardwareInterface::disconnect_() { auto future = app_disconnect_clt_->async_send_request(disconnect_request); auto status = future.wait_for(std::chrono::seconds(1)); if (status != std::future_status::ready) { - RCLCPP_ERROR(lbr_node_->get_logger(), "Failed to request disconnect service %s.", + RCLCPP_ERROR(app_node_->get_logger(), "Failed to request disconnect service %s.", app_disconnect_clt_->get_service_name()); return false; } auto response = future.get(); if (!response->disconnected) { - RCLCPP_ERROR(lbr_node_->get_logger(), "Failed to disconnect.\n%s", response->message.c_str()); + RCLCPP_ERROR(app_node_->get_logger(), "Failed to disconnect.\n%s", response->message.c_str()); } return response->disconnected; } From 68d87fa2254ebf00ad4fd6ca9ffdde80a7c68a36 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 9 Aug 2023 13:54:40 +0100 Subject: [PATCH 30/33] udpated doc --- lbr_fri_ros2/doc/img/lbr_fri_ros2.drawio.svg | 2 +- lbr_fri_ros2/include/lbr_fri_ros2/app.hpp | 16 ++++++++-------- lbr_fri_ros2/src/app.cpp | 6 +++--- 3 files changed, 12 insertions(+), 12 deletions(-) diff --git a/lbr_fri_ros2/doc/img/lbr_fri_ros2.drawio.svg b/lbr_fri_ros2/doc/img/lbr_fri_ros2.drawio.svg index 4e3c3b6e..6c207b67 100644 --- a/lbr_fri_ros2/doc/img/lbr_fri_ros2.drawio.svg +++ b/lbr_fri_ros2/doc/img/lbr_fri_ros2.drawio.svg @@ -1,4 +1,4 @@ -
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

# 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 +
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

# 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/include/lbr_fri_ros2/app.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp index c93f500a..08abb81d 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp @@ -23,13 +23,13 @@ namespace lbr_fri_ros2 { /** * @brief The App has a node for exposing FRI methods to services. It shares this node with the - * #lbr_client_, which reads commands / write states via realtime safe topics. + * #client_, which reads commands / write states via realtime safe topics. * * Services: - * - ~/connect (lbr_fri_msgs::srv::AppConnect) + * - connect (lbr_fri_msgs::srv::AppConnect) * Opens UDP port to FRI. Creates #run_thread_ thread via #on_app_connect_ that calls #run_ to * communicate with the robot. - * - ~/disconnect (lbr_fri_msgs::srv::AppDisconnect) + * - disconnect (lbr_fri_msgs::srv::AppDisconnect) * Closes UDP port to FRI. Finishes #run_thread_ thread via #on_app_disconnect_ through ending * #run_. * @@ -61,7 +61,7 @@ class App { void get_parameters_(); /** - * @brief Callback to ~/connect service. Calls #connect_. + * @brief Callback to connect service. Calls #connect_. * * @param[in] request Request containing port_id and remote_host * @param[out] response Response containing connected and message @@ -70,7 +70,7 @@ class App { lbr_fri_msgs::srv::AppConnect::Response::SharedPtr response); /** - * @brief Callback to ~/disconnect service. Calls #disconnect_. + * @brief Callback to disconnect service. Calls #disconnect_. * * @param[in] request Empty request * @param[out] response Response containing disconnected and message @@ -114,7 +114,7 @@ class App { /** * @brief Exchanges commands / states between ROS and the FRI. * - * Calls step() on #app_, which callbacks #lbr_client_. #lbr_client_ reads commands / write + * Calls step() on #app_, which callbacks #client_. #client_ reads commands / write * states through realtime safe topics. * */ @@ -137,11 +137,11 @@ class App { rclcpp::Service::SharedPtr app_disconnect_srv_; /**< Service to disconnect from robot via #on_app_disconnect_ callback.*/ - std::shared_ptr lbr_client_; /**< Writes commands to / reads states from robot.*/ + std::shared_ptr client_; /**< Writes commands to / reads states from robot.*/ std::unique_ptr connection_; /**< UDP connection for reading states / writing commands.*/ std::unique_ptr - app_; /**< FRI client application that callbacks #lbr_client_ methods.*/ + app_; /**< FRI client application that callbacks #client_ methods.*/ }; } // end of namespace lbr_fri_ros2 #endif // LBR_FRI_ROS2__APP_HPP_ diff --git a/lbr_fri_ros2/src/app.cpp b/lbr_fri_ros2/src/app.cpp index 8a1099b7..855f3ecd 100644 --- a/lbr_fri_ros2/src/app.cpp +++ b/lbr_fri_ros2/src/app.cpp @@ -17,11 +17,11 @@ App::App(const rclcpp::Node::SharedPtr node) : node_(node) { std::bind(&App::on_app_disconnect_, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default); - lbr_client_ = std::make_shared( + client_ = std::make_shared( node_, lbr_command_guard_factory(node_->get_node_logging_interface(), robot_description_, command_guard_variant_)); connection_ = std::make_unique(); - app_ = std::make_unique(*connection_, *lbr_client_); + app_ = std::make_unique(*connection_, *client_); // attempt default connect connect_(port_id_, remote_host_); @@ -151,7 +151,7 @@ void App::run_() { bool success = true; while (success && connected_ && rclcpp::ok()) { success = app_->step(); - if (lbr_client_->robotState().getSessionState() == KUKA::FRI::IDLE) { + if (client_->robotState().getSessionState() == KUKA::FRI::IDLE) { break; } } From e298fe383ae740bcf9bc37ca463aa440b362886b Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 9 Aug 2023 14:00:11 +0100 Subject: [PATCH 31/33] added namespace --- .../src/admittance_control_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_control_node.cpp b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_control_node.cpp index f3c6d44f..a743cfb2 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_control_node.cpp +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_control_node.cpp @@ -75,8 +75,8 @@ int main(int argc, char **argv) { auto executor = std::make_shared(); - auto app_node = - std::make_shared("app", rclcpp::NodeOptions().use_intra_process_comms(true)); + auto app_node = std::make_shared( + "app", "lbr", rclcpp::NodeOptions().use_intra_process_comms(true)); auto app = lbr_fri_ros2::App(app_node); From cfff34d07909d3a9c75a9d62942059da47d2be79 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 9 Aug 2023 14:30:21 +0100 Subject: [PATCH 32/33] tuned parameters --- .../src/admittance_controller.hpp | 4 ++-- lbr_fri_ros2/src/client.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_controller.hpp b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_controller.hpp index 4a9b83c3..217a850a 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_controller.hpp +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_controller.hpp @@ -24,8 +24,8 @@ class AdmittanceController { AdmittanceController(const std::string &robot_description, const std::string &base_link = "link_0", const std::string &end_effector_link = "link_ee", - const CartesianVector &f_ext_th = {2., 2., 2., 0.5, 0.5, 0.5}, - const JointVector &dq_gains = {2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0}, + const CartesianVector &f_ext_th = {4., 4., 4., 0.5, 0.5, 0.5}, + const JointVector &dq_gains = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0}, const CartesianVector &dx_gains = {1.5, 1.5, 1.5, 20., 40., 60.}) : dq_gains_(dq_gains), dx_gains_(dx_gains), f_ext_th_(f_ext_th) { if (!kdl_parser::treeFromString(robot_description, tree_)) { diff --git a/lbr_fri_ros2/src/client.cpp b/lbr_fri_ros2/src/client.cpp index 6fc5054b..929191e6 100644 --- a/lbr_fri_ros2/src/client.cpp +++ b/lbr_fri_ros2/src/client.cpp @@ -133,8 +133,8 @@ void Client::init_filters_() { joint_position_pid_.init(2. * robotState().getSampleTime(), 0., 0., 0., 0., false); // initialize torque filters - external_torque_filter_.init(50, robotState().getSampleTime()); - measured_torque_filter_.init(50, robotState().getSampleTime()); + external_torque_filter_.init(3., robotState().getSampleTime()); + measured_torque_filter_.init(3., robotState().getSampleTime()); filters_init_ = true; } From 0fed98ef483d89d35a437189b1685ab366f623b8 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 9 Aug 2023 14:42:20 +0100 Subject: [PATCH 33/33] updated default --- lbr_fri_ros2/src/client.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/lbr_fri_ros2/src/client.cpp b/lbr_fri_ros2/src/client.cpp index 929191e6..4a8e8831 100644 --- a/lbr_fri_ros2/src/client.cpp +++ b/lbr_fri_ros2/src/client.cpp @@ -133,8 +133,8 @@ void Client::init_filters_() { joint_position_pid_.init(2. * robotState().getSampleTime(), 0., 0., 0., 0., false); // initialize torque filters - external_torque_filter_.init(3., robotState().getSampleTime()); - measured_torque_filter_.init(3., robotState().getSampleTime()); + external_torque_filter_.init(10., robotState().getSampleTime()); + measured_torque_filter_.init(10., robotState().getSampleTime()); filters_init_ = true; }