diff --git a/lbr_bringup/package.xml b/lbr_bringup/package.xml index 4907a820..bb2090ae 100644 --- a/lbr_bringup/package.xml +++ b/lbr_bringup/package.xml @@ -2,7 +2,7 @@ lbr_bringup - 1.0.0 + 1.2.0 LBR launch files. mhubii MIT @@ -26,4 +26,4 @@ ament_cmake - + \ No newline at end of file diff --git a/lbr_demos/lbr_fri_ros2_advanced_cpp_demos/package.xml b/lbr_demos/lbr_fri_ros2_advanced_cpp_demos/package.xml index 9907d507..3ca0e207 100644 --- a/lbr_demos/lbr_fri_ros2_advanced_cpp_demos/package.xml +++ b/lbr_demos/lbr_fri_ros2_advanced_cpp_demos/package.xml @@ -2,7 +2,7 @@ lbr_fri_ros2_advanced_cpp_demos - 1.0.0 + 1.2.0 Advanced C++ demos for the lbr_fri_ros2. mhubii MIT diff --git a/lbr_demos/lbr_fri_ros2_advanced_cpp_demos/src/admittance_control_node.cpp b/lbr_demos/lbr_fri_ros2_advanced_cpp_demos/src/admittance_control_node.cpp index dbd559ef..00f162d3 100644 --- a/lbr_demos/lbr_fri_ros2_advanced_cpp_demos/src/admittance_control_node.cpp +++ b/lbr_demos/lbr_fri_ros2_advanced_cpp_demos/src/admittance_control_node.cpp @@ -23,11 +23,11 @@ class AdmittanceControlNode : public rclcpp::Node { this->get_parameter("end_effector_link").as_string()); lbr_command_pub_ = create_publisher( - "/lbr_command", rclcpp::QoS(1) + "/lbr/command", rclcpp::QoS(1) .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE) .deadline(std::chrono::milliseconds(10))); lbr_state_sub_ = create_subscription( - "/lbr_state", + "/lbr/state", rclcpp::QoS(1) .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE) .deadline(std::chrono::milliseconds(10)), @@ -73,17 +73,17 @@ class AdmittanceControlNode : public rclcpp::Node { int main(int argc, char **argv) { rclcpp::init(argc, argv); - auto executor = std::make_shared(); + auto executor = std::make_shared(); - auto lbr_app_node = std::make_shared( - "lbr_app", rclcpp::NodeOptions().use_intra_process_comms(true)); + auto lbr_node = + std::make_shared("lbr", rclcpp::NodeOptions().use_intra_process_comms(true)); - auto lbr_app = lbr_fri_ros2::LBRApp(lbr_app_node); + auto lbr_app = lbr_fri_ros2::LBRApp(lbr_node); auto admittance_control_node = std::make_shared( "admittance_control_node", rclcpp::NodeOptions().use_intra_process_comms(true)); - executor->add_node(lbr_app_node); + executor->add_node(lbr_node); executor->add_node(admittance_control_node); executor->spin(); diff --git a/lbr_demos/lbr_fri_ros2_advanced_python_demos/lbr_fri_ros2_advanced_python_demos/admittance_control_node.py b/lbr_demos/lbr_fri_ros2_advanced_python_demos/lbr_fri_ros2_advanced_python_demos/admittance_control_node.py index ad7ca0a4..5930ec7e 100755 --- a/lbr_demos/lbr_fri_ros2_advanced_python_demos/lbr_fri_ros2_advanced_python_demos/admittance_control_node.py +++ b/lbr_demos/lbr_fri_ros2_advanced_python_demos/lbr_fri_ros2_advanced_python_demos/admittance_control_node.py @@ -30,7 +30,7 @@ def __init__(self, node_name="admittance_control_node") -> None: # publishers and subscribers self.lbr_state_sub_ = self.create_subscription( LBRState, - "/lbr_state", + "/lbr/state", self.on_lbr_state_, QoSProfile( depth=1, @@ -40,7 +40,7 @@ def __init__(self, node_name="admittance_control_node") -> None: ) self.lbr_command_pub_ = self.create_publisher( LBRCommand, - "/lbr_command", + "/lbr/command", QoSProfile( depth=1, reliability=ReliabilityPolicy.RELIABLE, diff --git a/lbr_demos/lbr_fri_ros2_advanced_python_demos/package.xml b/lbr_demos/lbr_fri_ros2_advanced_python_demos/package.xml index 8c541b09..88bddacd 100644 --- a/lbr_demos/lbr_fri_ros2_advanced_python_demos/package.xml +++ b/lbr_demos/lbr_fri_ros2_advanced_python_demos/package.xml @@ -2,7 +2,7 @@ lbr_fri_ros2_advanced_python_demos - 1.1.0 + 1.2.0 Advanced Python demos for the lbr_fri_ros2. mhubii MIT @@ -19,4 +19,4 @@ ament_python - + \ No newline at end of file diff --git a/lbr_demos/lbr_fri_ros2_advanced_python_demos/setup.py b/lbr_demos/lbr_fri_ros2_advanced_python_demos/setup.py index 85d099e5..10348a2a 100644 --- a/lbr_demos/lbr_fri_ros2_advanced_python_demos/setup.py +++ b/lbr_demos/lbr_fri_ros2_advanced_python_demos/setup.py @@ -6,7 +6,7 @@ setup( name=package_name, - version="1.1.0", + version="1.2.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/lbr_demos/lbr_fri_ros2_cpp_demos/package.xml b/lbr_demos/lbr_fri_ros2_cpp_demos/package.xml index 646866b1..4a0de466 100644 --- a/lbr_demos/lbr_fri_ros2_cpp_demos/package.xml +++ b/lbr_demos/lbr_fri_ros2_cpp_demos/package.xml @@ -2,7 +2,7 @@ lbr_fri_ros2_cpp_demos - 1.1.0 + 1.2.0 C++ demos for the lbr_fri_ros2. mhubii MIT @@ -14,11 +14,11 @@ rclcpp lbr_fri_ros2 - + ament_lint_auto ament_lint_common ament_cmake - + \ No newline at end of file diff --git a/lbr_demos/lbr_fri_ros2_cpp_demos/src/joint_sine_overlay_node.cpp b/lbr_demos/lbr_fri_ros2_cpp_demos/src/joint_sine_overlay_node.cpp index 8c83cadc..fdcc8992 100644 --- a/lbr_demos/lbr_fri_ros2_cpp_demos/src/joint_sine_overlay_node.cpp +++ b/lbr_demos/lbr_fri_ros2_cpp_demos/src/joint_sine_overlay_node.cpp @@ -20,13 +20,13 @@ class JointSineOverlayNode : public rclcpp::Node { JointSineOverlayNode(const std::string &node_name) : Node(node_name), phase_(0.) { // create publisher to /lbr_command lbr_command_pub_ = this->create_publisher( - "/lbr_command", rclcpp::QoS(1) + "/lbr/command", rclcpp::QoS(1) .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE) .deadline(std::chrono::milliseconds(10))); // create subscription to /lbr_state lbr_state_sub_ = this->create_subscription( - "/lbr_state", + "/lbr/state", rclcpp::QoS(1) .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE) .deadline(std::chrono::milliseconds(10)), diff --git a/lbr_demos/lbr_fri_ros2_cpp_demos/src/torque_sine_overlay_node.cpp b/lbr_demos/lbr_fri_ros2_cpp_demos/src/torque_sine_overlay_node.cpp index 639b05bf..7b24788b 100644 --- a/lbr_demos/lbr_fri_ros2_cpp_demos/src/torque_sine_overlay_node.cpp +++ b/lbr_demos/lbr_fri_ros2_cpp_demos/src/torque_sine_overlay_node.cpp @@ -20,13 +20,13 @@ class TorqueSineOverlayNode : public rclcpp::Node { TorqueSineOverlayNode(const std::string &node_name) : Node(node_name), phase_(0.) { // create publisher to /lbr_command lbr_command_pub_ = this->create_publisher( - "/lbr_command", rclcpp::QoS(1) + "/lbr/command", rclcpp::QoS(1) .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE) .deadline(std::chrono::milliseconds(10))); // create subscription to /lbr_state lbr_state_sub_ = this->create_subscription( - "/lbr_state", + "/lbr/state", rclcpp::QoS(1) .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE) .deadline(std::chrono::milliseconds(10)), diff --git a/lbr_demos/lbr_fri_ros2_cpp_demos/src/wrench_sine_overlay_node.cpp b/lbr_demos/lbr_fri_ros2_cpp_demos/src/wrench_sine_overlay_node.cpp index ba1a40ce..14e4cd3b 100644 --- a/lbr_demos/lbr_fri_ros2_cpp_demos/src/wrench_sine_overlay_node.cpp +++ b/lbr_demos/lbr_fri_ros2_cpp_demos/src/wrench_sine_overlay_node.cpp @@ -23,13 +23,13 @@ class WrenchSineOverlayNode : public rclcpp::Node { : Node(node_name), phase_x_(0.), phase_y_(0.) { // create publisher to /lbr_command lbr_command_pub_ = this->create_publisher( - "/lbr_command", rclcpp::QoS(1) + "/lbr/command", rclcpp::QoS(1) .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE) .deadline(std::chrono::milliseconds(10))); // create subscription to /lbr_state lbr_state_sub_ = this->create_subscription( - "/lbr_state", + "/lbr/state", rclcpp::QoS(1) .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE) .deadline(std::chrono::milliseconds(10)), diff --git a/lbr_demos/lbr_fri_ros2_python_demos/lbr_fri_ros2_python_demos/joint_sine_overlay_node.py b/lbr_demos/lbr_fri_ros2_python_demos/lbr_fri_ros2_python_demos/joint_sine_overlay_node.py index 99bdffed..2f2c4675 100644 --- a/lbr_demos/lbr_fri_ros2_python_demos/lbr_fri_ros2_python_demos/joint_sine_overlay_node.py +++ b/lbr_demos/lbr_fri_ros2_python_demos/lbr_fri_ros2_python_demos/joint_sine_overlay_node.py @@ -21,7 +21,7 @@ def __init__(self, node_name: str) -> None: # create publisher to /lbr_command self.lbr_command_pub_ = self.create_publisher( LBRCommand, - "/lbr_command", + "/lbr/command", QoSProfile( depth=1, reliability=ReliabilityPolicy.RELIABLE, @@ -32,7 +32,7 @@ def __init__(self, node_name: str) -> None: # create subscription to /lbr_state self.lbr_state_sub_ = self.create_subscription( LBRState, - "/lbr_state", + "/lbr/state", self.on_lbr_state_, QoSProfile( depth=1, diff --git a/lbr_demos/lbr_fri_ros2_python_demos/lbr_fri_ros2_python_demos/torque_sine_overlay_node.py b/lbr_demos/lbr_fri_ros2_python_demos/lbr_fri_ros2_python_demos/torque_sine_overlay_node.py index f8000163..19d0e692 100644 --- a/lbr_demos/lbr_fri_ros2_python_demos/lbr_fri_ros2_python_demos/torque_sine_overlay_node.py +++ b/lbr_demos/lbr_fri_ros2_python_demos/lbr_fri_ros2_python_demos/torque_sine_overlay_node.py @@ -21,7 +21,7 @@ def __init__(self, node_name: str) -> None: # create publisher to /lbr_command self.lbr_command_pub_ = self.create_publisher( LBRCommand, - "/lbr_command", + "/lbr/command", QoSProfile( depth=1, reliability=ReliabilityPolicy.RELIABLE, @@ -32,7 +32,7 @@ def __init__(self, node_name: str) -> None: # create subscription to /lbr_state self.lbr_state_sub_ = self.create_subscription( LBRState, - "/lbr_state", + "/lbr/state", self.on_lbr_state_, QoSProfile( depth=1, diff --git a/lbr_demos/lbr_fri_ros2_python_demos/lbr_fri_ros2_python_demos/wrench_sine_overlay_node.py b/lbr_demos/lbr_fri_ros2_python_demos/lbr_fri_ros2_python_demos/wrench_sine_overlay_node.py index 827ebc9c..4fa3d82c 100644 --- a/lbr_demos/lbr_fri_ros2_python_demos/lbr_fri_ros2_python_demos/wrench_sine_overlay_node.py +++ b/lbr_demos/lbr_fri_ros2_python_demos/lbr_fri_ros2_python_demos/wrench_sine_overlay_node.py @@ -21,7 +21,7 @@ def __init__(self, node_name: str) -> None: # create publisher to /lbr_command self.lbr_command_pub_ = self.create_publisher( LBRCommand, - "/lbr_command", + "/lbr/command", QoSProfile( depth=1, reliability=ReliabilityPolicy.RELIABLE, @@ -32,7 +32,7 @@ def __init__(self, node_name: str) -> None: # create subscription to /lbr_state self.lbr_state_sub_ = self.create_subscription( LBRState, - "/lbr_state", + "/lbr/state", self.on_lbr_state_, QoSProfile( depth=1, diff --git a/lbr_demos/lbr_fri_ros2_python_demos/package.xml b/lbr_demos/lbr_fri_ros2_python_demos/package.xml index 49aaeef4..5bf11800 100644 --- a/lbr_demos/lbr_fri_ros2_python_demos/package.xml +++ b/lbr_demos/lbr_fri_ros2_python_demos/package.xml @@ -2,7 +2,7 @@ lbr_fri_ros2_python_demos - 1.1.0 + 1.2.0 Python demos for the lbr_fri_ros2. mhubii MIT @@ -19,4 +19,4 @@ ament_python - + \ No newline at end of file diff --git a/lbr_demos/lbr_fri_ros2_python_demos/setup.py b/lbr_demos/lbr_fri_ros2_python_demos/setup.py index 3c188bbf..b48987b5 100644 --- a/lbr_demos/lbr_fri_ros2_python_demos/setup.py +++ b/lbr_demos/lbr_fri_ros2_python_demos/setup.py @@ -6,7 +6,7 @@ setup( name=package_name, - version="1.1.0", + version="1.2.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/lbr_demos/lbr_ros2_control_cpp_demos/package.xml b/lbr_demos/lbr_ros2_control_cpp_demos/package.xml index bf6a7f28..bc145837 100644 --- a/lbr_demos/lbr_ros2_control_cpp_demos/package.xml +++ b/lbr_demos/lbr_ros2_control_cpp_demos/package.xml @@ -2,7 +2,7 @@ lbr_ros2_control_cpp_demos - 1.1.0 + 1.2.0 C++ demos for the LBR ros2_control integration. mhubii MIT @@ -26,4 +26,4 @@ ament_cmake - + \ No newline at end of file diff --git a/lbr_demos/lbr_ros2_control_python_demos/package.xml b/lbr_demos/lbr_ros2_control_python_demos/package.xml index 3397c85d..eefd92cf 100644 --- a/lbr_demos/lbr_ros2_control_python_demos/package.xml +++ b/lbr_demos/lbr_ros2_control_python_demos/package.xml @@ -2,7 +2,7 @@ lbr_ros2_control_python_demos - 1.1.0 + 1.2.0 Python demos for the LBR ros2_control integration. mhubii MIT @@ -23,4 +23,4 @@ ament_python - + \ No newline at end of file diff --git a/lbr_demos/lbr_ros2_control_python_demos/setup.py b/lbr_demos/lbr_ros2_control_python_demos/setup.py index cfd0f166..e86a99df 100644 --- a/lbr_demos/lbr_ros2_control_python_demos/setup.py +++ b/lbr_demos/lbr_ros2_control_python_demos/setup.py @@ -4,7 +4,7 @@ setup( name=package_name, - version="1.1.0", + version="1.2.0", 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 aeb64cd4..eb6c4d76 100644 --- a/lbr_description/package.xml +++ b/lbr_description/package.xml @@ -2,7 +2,7 @@ lbr_description - 1.0.0 + 1.2.0 KUKA LBR description files mhubii MIT @@ -15,9 +15,9 @@ robot_state_publisher rviz2 - python3-pytest + python3-pytest ament_cmake - + \ No newline at end of file diff --git a/lbr_description/ros2_control/lbr.ros2_control.xacro b/lbr_description/ros2_control/lbr.ros2_control.xacro index b4796851..01022e8f 100644 --- a/lbr_description/ros2_control/lbr.ros2_control.xacro +++ b/lbr_description/ros2_control/lbr.ros2_control.xacro @@ -26,6 +26,7 @@ ${remote_host} ${port_id} ${sample_time} + ${robot_name} diff --git a/lbr_fri_msgs/package.xml b/lbr_fri_msgs/package.xml index c8277698..5357718e 100644 --- a/lbr_fri_msgs/package.xml +++ b/lbr_fri_msgs/package.xml @@ -2,14 +2,14 @@ lbr_fri_msgs - 1.0.0 + 1.2.0 ROS 2 message for the Fast Robot Interface (FRI) specific states. mhubii MIT ament_cmake rosidl_default_generators - + builtin_interfaces std_msgs @@ -22,4 +22,4 @@ ament_cmake - + \ No newline at end of file diff --git a/lbr_fri_ros2/package.xml b/lbr_fri_ros2/package.xml index d131682c..3bbad41e 100644 --- a/lbr_fri_ros2/package.xml +++ b/lbr_fri_ros2/package.xml @@ -2,8 +2,9 @@ lbr_fri_ros2 - 1.1.0 - The lbr_fri_ros2 package provides the Fast Robot Interface (FRI) integration into ROS 2. Robot states can be extracted and commanded. + 1.2.0 + The lbr_fri_ros2 package provides the Fast Robot Interface (FRI) integration into ROS + 2. Robot states can be extracted and commanded. mhubii MIT @@ -22,4 +23,4 @@ ament_cmake - + \ No newline at end of file diff --git a/lbr_fri_ros2/src/lbr_app_component.cpp b/lbr_fri_ros2/src/lbr_app_component.cpp index b4f72d54..fa5b7128 100644 --- a/lbr_fri_ros2/src/lbr_app_component.cpp +++ b/lbr_fri_ros2/src/lbr_app_component.cpp @@ -2,13 +2,13 @@ namespace lbr_fri_ros2 { LBRAppComponent::LBRAppComponent(const rclcpp::NodeOptions &options) { - node_ = std::make_shared("lbr_component", options); - lbr_app_ = std::make_unique(node_); + 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 { - return node_->get_node_base_interface(); + return lbr_node_->get_node_base_interface(); } } // end of namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/lbr_app_component.hpp b/lbr_fri_ros2/src/lbr_app_component.hpp index 99f0b9a4..79cbce08 100644 --- a/lbr_fri_ros2/src/lbr_app_component.hpp +++ b/lbr_fri_ros2/src/lbr_app_component.hpp @@ -30,8 +30,8 @@ class LBRAppComponent { protected: std::unique_ptr - lbr_app_; /** #LBRApp for communicating with the hardware.<*/ - rclcpp::Node::SharedPtr node_; /** Node for communicating with ROS.<*/ + lbr_app_; /** #LBRApp for communicating with the hardware.<*/ + rclcpp::Node::SharedPtr lbr_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/lbr_client.cpp b/lbr_fri_ros2/src/lbr_client.cpp index 2bf03c63..22c65860 100644 --- a/lbr_fri_ros2/src/lbr_client.cpp +++ b/lbr_fri_ros2/src/lbr_client.cpp @@ -115,10 +115,10 @@ void LBRClient::init_topics_() { void LBRClient::declare_parameters_() { if (!node_->has_parameter("lbr_command_topic")) { - node_->declare_parameter("lbr_command_topic", "/lbr_command"); + node_->declare_parameter("lbr_command_topic", "~/command"); } if (!node_->has_parameter("lbr_state_topic")) { - node_->declare_parameter("lbr_state_topic", "/lbr_state"); + node_->declare_parameter("lbr_state_topic", "~/state"); } if (!node_->has_parameter("smoothing")) { node_->declare_parameter("smoothing", 0.99); diff --git a/lbr_fri_ros2_stack/package.xml b/lbr_fri_ros2_stack/package.xml index d0b14343..ae2d756a 100644 --- a/lbr_fri_ros2_stack/package.xml +++ b/lbr_fri_ros2_stack/package.xml @@ -2,7 +2,7 @@ lbr_fri_ros2_stack - 1.1.0 + 1.2.0 ROS 2 stack for KUKA LBRs. mhubii MIT @@ -18,4 +18,4 @@ ament_cmake - + \ No newline at end of file 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 2dc4e664..fb44b525 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 @@ -81,8 +81,7 @@ class LBRHardwareInterface : public hardware_interface::SystemInterface { const uint8_t LBR_FRI_SENSOR_SIZE = 12; // node for handling communication - rclcpp::Node::SharedPtr hw_node_; - rclcpp::Node::SharedPtr lbr_app_node_; + rclcpp::Node::SharedPtr lbr_node_; std::unique_ptr lbr_app_; // exposed state interfaces @@ -126,6 +125,7 @@ class LBRHardwareInterface : public hardware_interface::SystemInterface { int32_t port_id_; const char *remote_host_; uint8_t sample_time_; + std::string robot_name_; // publisher for sending commands / subscriber to receive goals lbr_fri_msgs::msg::LBRCommand lbr_command_; diff --git a/lbr_hardware_interface/package.xml b/lbr_hardware_interface/package.xml index e36e3b2b..4433151e 100644 --- a/lbr_hardware_interface/package.xml +++ b/lbr_hardware_interface/package.xml @@ -2,7 +2,7 @@ lbr_hardware_interface - 1.1.0 + 1.2.0 ROS 2 hardware hardware_interface for KUKA LBR through Fast Robot Interface (FRI). mhubii MIT @@ -22,4 +22,4 @@ ament_cmake - + \ No newline at end of file diff --git a/lbr_hardware_interface/src/lbr_hardware_interface.cpp b/lbr_hardware_interface/src/lbr_hardware_interface.cpp index 74958f97..533753ed 100644 --- a/lbr_hardware_interface/src/lbr_hardware_interface.cpp +++ b/lbr_hardware_interface/src/lbr_hardware_interface.cpp @@ -3,23 +3,36 @@ namespace lbr_hardware_interface { controller_interface::CallbackReturn LBRHardwareInterface::on_init(const hardware_interface::HardwareInfo &system_info) { - hw_node_ = std::make_shared("fri_hardware_interface_node", - rclcpp::NodeOptions().use_intra_process_comms(true)); - lbr_app_node_ = std::make_shared( - "lbr_app", rclcpp::NodeOptions().use_intra_process_comms(true)); - - lbr_app_node_->declare_parameter("lbr_command_topic", "/_lbr_command"); - lbr_app_node_->declare_parameter("lbr_state_topic", "/_lbr_state"); - lbr_app_node_->declare_parameter("smoothing", 0.8); - - lbr_app_ = std::make_unique(lbr_app_node_); - auto ret = hardware_interface::SystemInterface::on_init(system_info); if (ret != controller_interface::CallbackReturn::SUCCESS) { - RCLCPP_ERROR(hw_node_->get_logger(), "Failed to initialize SystemInterface."); + RCLCPP_ERROR(lbr_node_->get_logger(), "Failed to initialize SystemInterface."); return ret; } + // read port_id and remote_host from lbr.ros2_control.xacro + port_id_ = std::stoul(info_.hardware_parameters["port_id"]); + info_.hardware_parameters["remote_host"] == "INADDR_ANY" + ? remote_host_ = NULL + : remote_host_ = info_.hardware_parameters["remote_host"].c_str(); + sample_time_ = std::stoi(info_.hardware_parameters["sample_time"]); + robot_name_ = info_.hardware_parameters["robot_name"]; + + if (port_id_ < 30200 || port_id_ > 30209) { + RCLCPP_ERROR(lbr_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_, + rclcpp::NodeOptions().use_intra_process_comms(true)); + + lbr_node_->declare_parameter("lbr_command_topic", "~/_lbr_command"); + lbr_node_->declare_parameter("lbr_state_topic", "~/_lbr_state"); + lbr_node_->declare_parameter("smoothing", 0.8); + + lbr_app_ = std::make_unique(lbr_node_); + init_command_interfaces_(); init_state_interfaces_(); init_last_hw_states_(); @@ -40,18 +53,6 @@ LBRHardwareInterface::on_init(const hardware_interface::HardwareInfo &system_inf return controller_interface::CallbackReturn::ERROR; } - // read port_id and remote_host from lbr.ros2_control.xacro - port_id_ = std::stoul(info_.hardware_parameters["port_id"]); - info_.hardware_parameters["remote_host"] == "INADDR_ANY" - ? remote_host_ = NULL - : remote_host_ = info_.hardware_parameters["remote_host"].c_str(); - sample_time_ = std::stoi(info_.hardware_parameters["sample_time"]); - - if (port_id_ < 30200 || port_id_ > 30209) { - RCLCPP_ERROR(hw_node_->get_logger(), "Expected port_id in [30200, 30209]. Found %d.", port_id_); - return controller_interface::CallbackReturn::ERROR; - } - if (!spawn_com_layer_()) { return controller_interface::CallbackReturn::ERROR; } @@ -62,8 +63,7 @@ LBRHardwareInterface::on_init(const hardware_interface::HardwareInfo &system_inf node_thread_ = std::make_unique([this]() { auto executor = std::make_shared(); - executor->add_node(hw_node_); - executor->add_node(lbr_app_node_); + executor->add_node(lbr_node_); executor->spin(); disconnect_(); }); @@ -144,7 +144,7 @@ hardware_interface::return_type LBRHardwareInterface::prepare_command_mode_switc controller_interface::CallbackReturn LBRHardwareInterface::on_activate(const rclcpp_lifecycle::State &) { if (!connect_()) { - RCLCPP_ERROR(hw_node_->get_logger(), "Failed to connect to robot."); + RCLCPP_ERROR(lbr_node_->get_logger(), "Failed to connect to robot."); return controller_interface::CallbackReturn::ERROR; } return controller_interface::CallbackReturn::SUCCESS; @@ -153,7 +153,7 @@ LBRHardwareInterface::on_activate(const rclcpp_lifecycle::State &) { controller_interface::CallbackReturn LBRHardwareInterface::on_deactivate(const rclcpp_lifecycle::State &) { if (!disconnect_()) { - RCLCPP_ERROR(hw_node_->get_logger(), "Failed to disconnect from robot."); + RCLCPP_ERROR(lbr_node_->get_logger(), "Failed to disconnect from robot."); return controller_interface::CallbackReturn::ERROR; } return controller_interface::CallbackReturn::SUCCESS; @@ -163,7 +163,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(hw_node_->get_logger(), "LBR left COMMANDING_ACTIVE. Please re-run lbr_bringup."); + RCLCPP_ERROR(lbr_node_->get_logger(), "LBR left COMMANDING_ACTIVE. Please re-run lbr_bringup."); return hardware_interface::return_type::ERROR; } @@ -232,7 +232,7 @@ bool LBRHardwareInterface::wait_for_service_( bool spawned = false; uint8_t attempt = 0; while (attempt < attempts && !spawned && rclcpp::ok()) { - RCLCPP_INFO(hw_node_->get_logger(), "Waiting for service %s...", client->get_service_name()); + RCLCPP_INFO(lbr_node_->get_logger(), "Waiting for service %s...", client->get_service_name()); spawned = client->wait_for_service(timeout); ++attempt; } @@ -271,7 +271,7 @@ void LBRHardwareInterface::init_state_interfaces_() { bool LBRHardwareInterface::verify_number_of_joints_() { if (info_.joints.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { - RCLCPP_ERROR(hw_node_->get_logger(), "Expected %d joints in URDF. Found %ld.", + RCLCPP_ERROR(lbr_node_->get_logger(), "Expected %d joints in URDF. Found %ld.", KUKA::FRI::LBRState::NUMBER_OF_JOINTS, info_.joints.size()); return false; } @@ -283,7 +283,7 @@ bool LBRHardwareInterface::verify_joint_command_interfaces_() { for (auto &joint : info_.joints) { if (joint.command_interfaces.size() != LBR_FRI_COMMAND_INTERFACE_SIZE) { RCLCPP_ERROR( - hw_node_->get_logger(), + lbr_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; @@ -291,7 +291,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(hw_node_->get_logger(), + RCLCPP_ERROR(lbr_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); @@ -307,7 +307,7 @@ bool LBRHardwareInterface::verify_joint_state_interfaces_() { for (auto &joint : info_.joints) { if (joint.state_interfaces.size() != LBR_FRI_STATE_INTERFACE_SIZE) { RCLCPP_ERROR( - hw_node_->get_logger(), + lbr_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; @@ -319,7 +319,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( - hw_node_->get_logger(), + lbr_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, @@ -335,14 +335,14 @@ bool LBRHardwareInterface::verify_joint_state_interfaces_() { bool LBRHardwareInterface::verify_sensors_() { // check lbr specific state interfaces if (info_.sensors.size() > 1) { - RCLCPP_ERROR(hw_node_->get_logger(), "Expected 1 sensor, got %ld", info_.sensors.size()); + RCLCPP_ERROR(lbr_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(hw_node_->get_logger(), + RCLCPP_ERROR(lbr_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); @@ -359,7 +359,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(hw_node_->get_logger(), "Sensor %s received invalid state interface %s.", + RCLCPP_ERROR(lbr_node_->get_logger(), "Sensor %s received invalid state interface %s.", lbr_fri_sensor.name.c_str(), si.name.c_str()); return false; @@ -369,7 +369,7 @@ bool LBRHardwareInterface::verify_sensors_() { } bool LBRHardwareInterface::spawn_com_layer_() { - if (!hw_node_) { + if (!lbr_node_) { printf("No node provided.\n"); return false; } @@ -378,51 +378,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_ = hw_node_->create_subscription( - "/_lbr_state", + lbr_state_sub_ = lbr_node_->create_subscription( + "~/_lbr_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_ = hw_node_->create_publisher( - "/_lbr_command", rclcpp::QoS(1) - .deadline(std::chrono::milliseconds(sample_time_)) - .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE)); + lbr_command_pub_ = lbr_node_->create_publisher( + "~/_lbr_command", rclcpp::QoS(1) + .deadline(std::chrono::milliseconds(sample_time_)) + .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE)); } catch (const std::exception &e) { - RCLCPP_ERROR(hw_node_->get_logger(), "Failed to spawn real time layer.\n%s.", e.what()); + RCLCPP_ERROR(lbr_node_->get_logger(), "Failed to spawn real time layer.\n%s.", e.what()); return false; } return true; } bool LBRHardwareInterface::spawn_clients_() { - if (!hw_node_) { + if (!lbr_node_) { printf("No node provided.\n"); return false; } - std::string app_connect_service_name = "/lbr_app/connect"; - app_connect_clt_ = hw_node_->create_client( + 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(hw_node_->get_logger(), "Waiting for service %s to spawn...", + RCLCPP_INFO(lbr_node_->get_logger(), "Waiting for service %s to spawn...", app_connect_service_name.c_str()); if (!wait_for_service_(app_connect_clt_)) { - RCLCPP_ERROR(hw_node_->get_logger(), "Failed."); + RCLCPP_ERROR(lbr_node_->get_logger(), "Failed."); return false; } - RCLCPP_INFO(hw_node_->get_logger(), "Done."); + RCLCPP_INFO(lbr_node_->get_logger(), "Done."); - std::string app_disconnect_service_name = "/lbr_app/disconnect"; - app_disconnect_clt_ = hw_node_->create_client( + 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(hw_node_->get_logger(), "Waiting for service %s to spawn...", + RCLCPP_INFO(lbr_node_->get_logger(), "Waiting for service %s to spawn...", app_disconnect_service_name.c_str()); if (!wait_for_service_(app_disconnect_clt_)) { - RCLCPP_ERROR(hw_node_->get_logger(), "Failed."); + RCLCPP_ERROR(lbr_node_->get_logger(), "Failed."); return false; } - RCLCPP_INFO(hw_node_->get_logger(), "Done."); + RCLCPP_INFO(lbr_node_->get_logger(), "Done."); return true; } @@ -443,13 +443,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(hw_node_->get_logger(), "Failed to request connect service %s.", + RCLCPP_ERROR(lbr_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(hw_node_->get_logger(), "Failed to connect.\n%s", response->message.c_str()); + RCLCPP_ERROR(lbr_node_->get_logger(), "Failed to connect.\n%s", response->message.c_str()); } return response->connected; } @@ -459,13 +459,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(hw_node_->get_logger(), "Failed to request disconnect service %s.", + RCLCPP_ERROR(lbr_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(hw_node_->get_logger(), "Failed to disconnect.\n%s", response->message.c_str()); + RCLCPP_ERROR(lbr_node_->get_logger(), "Failed to disconnect.\n%s", response->message.c_str()); } return response->disconnected; }