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;
}