updated messages in demos

lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_control_node.cpp

@@ -2,7 +2,7 @@
 
 #include "rclcpp/rclcpp.hpp"
 
-#include "lbr_fri_msgs/msg/lbr_command.hpp"
+#include "lbr_fri_msgs/msg/lbr_position_command.hpp"
 #include "lbr_fri_msgs/msg/lbr_state.hpp"
 #include "lbr_fri_ros2/app.hpp"
 
@@ -22,15 +22,10 @@ class AdmittanceControlNode : public rclcpp::Node {
         this->get_parameter("base_link").as_string(),
         this->get_parameter("end_effector_link").as_string());
 
-    lbr_command_pub_ = create_publisher<lbr_fri_msgs::msg::LBRCommand>(
-        "/lbr/command", rclcpp::QoS(1)
-                            .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE)
-                            .deadline(std::chrono::milliseconds(10)));
+    lbr_position_command_pub_ =
+        create_publisher<lbr_fri_msgs::msg::LBRPositionCommand>("/lbr/command", 1);
     lbr_state_sub_ = create_subscription<lbr_fri_msgs::msg::LBRState>(
-        "/lbr/state",
-        rclcpp::QoS(1)
-            .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE)
-            .deadline(std::chrono::milliseconds(10)),
+        "/lbr/state", 1,
         std::bind(&AdmittanceControlNode::on_lbr_state, this, std::placeholders::_1));
   }
 
@@ -43,7 +38,7 @@ class AdmittanceControlNode : public rclcpp::Node {
     smooth_lbr_state_(lbr_state, 0.95);
 
     auto lbr_command = admittance_controller_->update(lbr_state_);
-    lbr_command_pub_->publish(lbr_command);
+    lbr_position_command_pub_->publish(lbr_command);
   };
 
   void smooth_lbr_state_(const lbr_fri_msgs::msg::LBRState::SharedPtr lbr_state, double alpha) {
@@ -64,7 +59,7 @@ class AdmittanceControlNode : public rclcpp::Node {
   bool init_{false};
   lbr_fri_msgs::msg::LBRState lbr_state_;
 
-  rclcpp::Publisher<lbr_fri_msgs::msg::LBRCommand>::SharedPtr lbr_command_pub_;
+  rclcpp::Publisher<lbr_fri_msgs::msg::LBRPositionCommand>::SharedPtr lbr_position_command_pub_;
   rclcpp::Subscription<lbr_fri_msgs::msg::LBRState>::SharedPtr lbr_state_sub_;
 
   std::unique_ptr<AdmittanceController> admittance_controller_;

lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_controller.hpp

@@ -11,7 +11,7 @@
 
 #include "friLBRState.h"
 
-#include "lbr_fri_msgs/msg/lbr_command.hpp"
+#include "lbr_fri_msgs/msg/lbr_position_command.hpp"
 #include "lbr_fri_msgs/msg/lbr_state.hpp"
 
 #include "damped_least_squares.hpp"
@@ -40,7 +40,8 @@ class AdmittanceController {
     q_.resize(chain_.getNrOfJoints());
   };
 
-  const lbr_fri_msgs::msg::LBRCommand &update(const lbr_fri_msgs::msg::LBRState &lbr_state) {
+  const lbr_fri_msgs::msg::LBRPositionCommand &
+  update(const lbr_fri_msgs::msg::LBRState &lbr_state) {
     std::memcpy(q_.data.data(), lbr_state.measured_joint_position.data(),
                 sizeof(double) * KUKA::FRI::LBRState::NUMBER_OF_JOINTS);
     std::memcpy(tau_ext_.data(), lbr_state.external_torque.data(),
@@ -62,15 +63,15 @@ class AdmittanceController {
     dq_ = dq_gains_.asDiagonal() * jacobian_inv_ * f_ext_;
 
     for (int i = 0; i < 7; i++) {
-      lbr_command_.joint_position[i] =
+      lbr_position_command_.joint_position[i] =
           lbr_state.measured_joint_position[i] + dq_[i] * lbr_state.sample_time;
     }
 
-    return lbr_command_;
+    return lbr_position_command_;
   };
 
 protected:
-  lbr_fri_msgs::msg::LBRCommand lbr_command_;
+  lbr_fri_msgs::msg::LBRPositionCommand lbr_position_command_;
   KDL::Tree tree_;
   KDL::Chain chain_;
   std::unique_ptr<KDL::ChainJntToJacSolver> jacobian_solver_;

lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/damped_least_squares.hpp

@@ -1,5 +1,6 @@
 #ifndef LBR_DEMOS_FRI_ROS2_ADVANCED_CPP__DAMPED_LEAST_SQUARES_HPP_
 #define LBR_DEMOS_FRI_ROS2_ADVANCED_CPP__DAMPED_LEAST_SQUARES_HPP_
+
 #include <Eigen/Core>
 #include <Eigen/SVD>
 

lbr_demos/lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/admittance_control_node.py

@@ -1,10 +1,8 @@
 import numpy as np
 import rclpy
-from rclpy.duration import Duration
 from rclpy.node import Node
-from rclpy.qos import QoSProfile, ReliabilityPolicy
 
-from lbr_fri_msgs.msg import LBRCommand, LBRState
+from lbr_fri_msgs.msg import LBRPositionCommand, LBRState
 
 from .admittance_controller import AdmittanceController
 
@@ -29,30 +27,17 @@ def __init__(self, node_name="admittance_control_node") -> None:
 
         # publishers and subscribers
         self.lbr_state_sub_ = self.create_subscription(
-            LBRState,
-            "/lbr/state",
-            self.on_lbr_state_,
-            QoSProfile(
-                depth=1,
-                reliability=ReliabilityPolicy.RELIABLE,
-                deadline=Duration(nanoseconds=10 * 1e6),  # 10 milliseconds
-            ),
+            LBRState, "/lbr/state", self.on_lbr_state_, 1
         )
-        self.lbr_command_pub_ = self.create_publisher(
-            LBRCommand,
-            "/lbr/command",
-            QoSProfile(
-                depth=1,
-                reliability=ReliabilityPolicy.RELIABLE,
-                deadline=Duration(nanoseconds=10 * 1e6),  # 10 milliseconds
-            ),
+        self.lbr_position_command_pub_ = self.create_publisher(
+            LBRPositionCommand, "/lbr/command", 1
         )
 
     def on_lbr_state_(self, lbr_state: LBRState) -> None:
         self.smooth_lbr_state_(lbr_state, 0.95)
 
         lbr_command = self.controller_(self.lbr_state_)
-        self.lbr_command_pub_.publish(lbr_command)
+        self.lbr_position_command_pub_.publish(lbr_command)
 
     def smooth_lbr_state_(self, lbr_state: LBRState, alpha: float):
         if not self.init_:

lbr_demos/lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/admittance_controller.py

@@ -1,7 +1,7 @@
 import kinpy
 import numpy as np
 
-from lbr_fri_msgs.msg import LBRCommand, LBRState
+from lbr_fri_msgs.msg import LBRPositionCommand, LBRState
 
 
 class AdmittanceController(object):
@@ -14,7 +14,7 @@ def __init__(
         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]),
     ) -> None:
-        self.lbr_command_ = LBRCommand()
+        self.lbr_position_command_ = LBRPositionCommand()
 
         self.chain_ = kinpy.build_serial_chain_from_urdf(
             data=robot_description,
@@ -34,7 +34,7 @@ def __init__(
         self.f_ext_th_ = f_ext_th
         self.alpha_ = 0.99
 
-    def __call__(self, lbr_state: LBRState) -> LBRCommand:
+    def __call__(self, lbr_state: LBRState) -> LBRPositionCommand:
         self.q_ = np.array(lbr_state.measured_joint_position.tolist())
         self.tau_ext_ = np.array(lbr_state.external_torque.tolist())
 
@@ -55,9 +55,9 @@ def __call__(self, lbr_state: LBRState) -> LBRCommand:
             + (1 - self.alpha_) * self.dq_gain_ @ self.jacobian_inv_ @ self.f_ext_
         )
 
-        self.lbr_command_.joint_position = (
+        self.lbr_position_command_.joint_position = (
             np.array(lbr_state.measured_joint_position.tolist())
             + lbr_state.sample_time * self.dq_
         ).data
 
-        return self.lbr_command_
+        return self.lbr_position_command_

lbr_demos/lbr_demos_fri_ros2_cpp/src/joint_sine_overlay_node.cpp

@@ -9,7 +9,7 @@
 #include "rclcpp/rclcpp.hpp"
 
 // include lbr_fri_msgs
-#include "lbr_fri_msgs/msg/lbr_command.hpp"
+#include "lbr_fri_msgs/msg/lbr_position_command.hpp"
 #include "lbr_fri_msgs/msg/lbr_state.hpp"
 
 class JointSineOverlayNode : public rclcpp::Node {
@@ -19,30 +19,25 @@ class JointSineOverlayNode : public rclcpp::Node {
 public:
   JointSineOverlayNode(const std::string &node_name) : Node(node_name), phase_(0.) {
     // create publisher to /lbr/command
-    lbr_command_pub_ = this->create_publisher<lbr_fri_msgs::msg::LBRCommand>(
-        "/lbr/command", rclcpp::QoS(1)
-                            .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE)
-                            .deadline(std::chrono::milliseconds(10)));
+    lbr_position_command_pub_ =
+        this->create_publisher<lbr_fri_msgs::msg::LBRPositionCommand>("/lbr/command", 1);
 
     // create subscription to /lbr/state
     lbr_state_sub_ = this->create_subscription<lbr_fri_msgs::msg::LBRState>(
-        "/lbr/state",
-        rclcpp::QoS(1)
-            .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE)
-            .deadline(std::chrono::milliseconds(10)),
+        "/lbr/state", 1,
         std::bind(&JointSineOverlayNode::on_lbr_state_, this, std::placeholders::_1));
   };
 
 protected:
   void on_lbr_state_(const lbr_fri_msgs::msg::LBRState::SharedPtr lbr_state) {
-    lbr_command_.joint_position = lbr_state->ipo_joint_position;
+    lbr_position_command_.joint_position = lbr_state->ipo_joint_position;
 
     if (lbr_state->session_state == KUKA::FRI::COMMANDING_ACTIVE) {
       // overlay sine wave on 4th joint
-      lbr_command_.joint_position[3] += amplitude_ * sin(phase_);
+      lbr_position_command_.joint_position[3] += amplitude_ * sin(phase_);
       phase_ += 2 * M_PI * frequency_ * lbr_state->sample_time;
 
-      lbr_command_pub_->publish(lbr_command_);
+      lbr_position_command_pub_->publish(lbr_position_command_);
     } else {
       // reset phase
       phase_ = 0.;
@@ -51,10 +46,10 @@ class JointSineOverlayNode : public rclcpp::Node {
 
   double phase_;
 
-  rclcpp::Publisher<lbr_fri_msgs::msg::LBRCommand>::SharedPtr lbr_command_pub_;
+  rclcpp::Publisher<lbr_fri_msgs::msg::LBRPositionCommand>::SharedPtr lbr_position_command_pub_;
   rclcpp::Subscription<lbr_fri_msgs::msg::LBRState>::SharedPtr lbr_state_sub_;
 
-  lbr_fri_msgs::msg::LBRCommand lbr_command_;
+  lbr_fri_msgs::msg::LBRPositionCommand lbr_position_command_;
 };
 
 int main(int argc, char **argv) {

lbr_demos/lbr_demos_fri_ros2_cpp/src/torque_sine_overlay_node.cpp

@@ -9,8 +9,8 @@
 #include "rclcpp/rclcpp.hpp"
 
 // include lbr_fri_msgs
-#include "lbr_fri_msgs/msg/lbr_command.hpp"
 #include "lbr_fri_msgs/msg/lbr_state.hpp"
+#include "lbr_fri_msgs/msg/lbr_torque_command.hpp"
 
 class TorqueSineOverlayNode : public rclcpp::Node {
   static constexpr double amplitude_ = 15.;  // Nm
@@ -19,30 +19,25 @@ class TorqueSineOverlayNode : public rclcpp::Node {
 public:
   TorqueSineOverlayNode(const std::string &node_name) : Node(node_name), phase_(0.) {
     // create publisher to /lbr/command
-    lbr_command_pub_ = this->create_publisher<lbr_fri_msgs::msg::LBRCommand>(
-        "/lbr/command", rclcpp::QoS(1)
-                            .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE)
-                            .deadline(std::chrono::milliseconds(10)));
+    lbr_torque_command_pub_ =
+        this->create_publisher<lbr_fri_msgs::msg::LBRTorqueCommand>("/lbr/command", 1);
 
     // create subscription to /lbr/state
     lbr_state_sub_ = this->create_subscription<lbr_fri_msgs::msg::LBRState>(
-        "/lbr/state",
-        rclcpp::QoS(1)
-            .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE)
-            .deadline(std::chrono::milliseconds(10)),
+        "/lbr/state", 1,
         std::bind(&TorqueSineOverlayNode::on_lbr_state_, this, std::placeholders::_1));
   };
 
 protected:
   void on_lbr_state_(const lbr_fri_msgs::msg::LBRState::SharedPtr lbr_state) {
-    lbr_command_.joint_position = lbr_state->ipo_joint_position;
+    lbr_torque_command_.joint_position = lbr_state->ipo_joint_position;
 
     if (lbr_state->session_state == KUKA::FRI::COMMANDING_ACTIVE) {
       // overlay torque sine wave on 4th joint
-      lbr_command_.torque[3] = amplitude_ * sin(phase_);
+      lbr_torque_command_.torque[3] = amplitude_ * sin(phase_);
       phase_ += 2 * M_PI * frequency_ * lbr_state->sample_time;
 
-      lbr_command_pub_->publish(lbr_command_);
+      lbr_torque_command_pub_->publish(lbr_torque_command_);
     } else {
       // reset phase
       phase_ = 0.;
@@ -51,10 +46,10 @@ class TorqueSineOverlayNode : public rclcpp::Node {
 
   double phase_;
 
-  rclcpp::Publisher<lbr_fri_msgs::msg::LBRCommand>::SharedPtr lbr_command_pub_;
+  rclcpp::Publisher<lbr_fri_msgs::msg::LBRTorqueCommand>::SharedPtr lbr_torque_command_pub_;
   rclcpp::Subscription<lbr_fri_msgs::msg::LBRState>::SharedPtr lbr_state_sub_;
 
-  lbr_fri_msgs::msg::LBRCommand lbr_command_;
+  lbr_fri_msgs::msg::LBRTorqueCommand lbr_torque_command_;
 };
 
 int main(int argc, char **argv) {

lbr_demos/lbr_demos_fri_ros2_cpp/src/wrench_sine_overlay_node.cpp

@@ -9,8 +9,8 @@
 #include "rclcpp/rclcpp.hpp"
 
 // include lbr_fri_msgs
-#include "lbr_fri_msgs/msg/lbr_command.hpp"
 #include "lbr_fri_msgs/msg/lbr_state.hpp"
+#include "lbr_fri_msgs/msg/lbr_wrench_command.hpp"
 
 class WrenchSineOverlayNode : public rclcpp::Node {
   static constexpr double amplitude_x_ = 5.;   // N
@@ -22,32 +22,27 @@ class WrenchSineOverlayNode : public rclcpp::Node {
   WrenchSineOverlayNode(const std::string &node_name)
       : Node(node_name), phase_x_(0.), phase_y_(0.) {
     // create publisher to /lbr/command
-    lbr_command_pub_ = this->create_publisher<lbr_fri_msgs::msg::LBRCommand>(
-        "/lbr/command", rclcpp::QoS(1)
-                            .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE)
-                            .deadline(std::chrono::milliseconds(10)));
+    lbr_wrench_command_pub_ =
+        this->create_publisher<lbr_fri_msgs::msg::LBRWrenchCommand>("/lbr/command", 1);
 
     // create subscription to /lbr/state
     lbr_state_sub_ = this->create_subscription<lbr_fri_msgs::msg::LBRState>(
-        "/lbr/state",
-        rclcpp::QoS(1)
-            .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE)
-            .deadline(std::chrono::milliseconds(10)),
+        "/lbr/state", 1,
         std::bind(&WrenchSineOverlayNode::on_lbr_state_, this, std::placeholders::_1));
   };
 
 protected:
   void on_lbr_state_(const lbr_fri_msgs::msg::LBRState::SharedPtr lbr_state) {
-    lbr_command_.joint_position = lbr_state->ipo_joint_position;
+    lbr_wrench_command_.joint_position = lbr_state->ipo_joint_position;
 
     if (lbr_state->session_state == KUKA::FRI::COMMANDING_ACTIVE) {
       // overlay wrench sine wave on x / y direction
-      lbr_command_.wrench[0] = amplitude_x_ * sin(phase_x_);
-      lbr_command_.wrench[1] = amplitude_y_ * sin(phase_y_);
+      lbr_wrench_command_.wrench[0] = amplitude_x_ * sin(phase_x_);
+      lbr_wrench_command_.wrench[1] = amplitude_y_ * sin(phase_y_);
       phase_x_ += 2 * M_PI * frequency_x_ * lbr_state->sample_time;
       phase_y_ += 2 * M_PI * frequency_y_ * lbr_state->sample_time;
 
-      lbr_command_pub_->publish(lbr_command_);
+      lbr_wrench_command_pub_->publish(lbr_wrench_command_);
     } else {
       // reset phase
       phase_x_ = 0.;
@@ -57,10 +52,10 @@ class WrenchSineOverlayNode : public rclcpp::Node {
 
   double phase_x_, phase_y_;
 
-  rclcpp::Publisher<lbr_fri_msgs::msg::LBRCommand>::SharedPtr lbr_command_pub_;
+  rclcpp::Publisher<lbr_fri_msgs::msg::LBRWrenchCommand>::SharedPtr lbr_wrench_command_pub_;
   rclcpp::Subscription<lbr_fri_msgs::msg::LBRState>::SharedPtr lbr_state_sub_;
 
-  lbr_fri_msgs::msg::LBRCommand lbr_command_;
+  lbr_fri_msgs::msg::LBRWrenchCommand lbr_wrench_command_;
 };
 
 int main(int argc, char **argv) {

lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/joint_sine_overlay_node.py

@@ -1,9 +1,7 @@
 import math
 
 import rclpy
-from rclpy.duration import Duration
 from rclpy.node import Node
-from rclpy.qos import QoSProfile, ReliabilityPolicy
 
 # import lbr_fri_msgs
 from lbr_fri_msgs.msg import LBRCommand, LBRState
@@ -19,26 +17,11 @@ def __init__(self, node_name: str) -> None:
         self.lbr_command_ = LBRCommand()
 
         # create publisher to /lbr/command
-        self.lbr_command_pub_ = self.create_publisher(
-            LBRCommand,
-            "/lbr/command",
-            QoSProfile(
-                depth=1,
-                reliability=ReliabilityPolicy.RELIABLE,
-                deadline=Duration(nanoseconds=10 * 1e6),  # 10 milliseconds
-            ),
-        )
+        self.lbr_command_pub_ = self.create_publisher(LBRCommand, "/lbr/command", 1)
 
         # create subscription to /lbr_state
         self.lbr_state_sub_ = self.create_subscription(
-            LBRState,
-            "/lbr/state",
-            self.on_lbr_state_,
-            QoSProfile(
-                depth=1,
-                reliability=ReliabilityPolicy.RELIABLE,
-                deadline=Duration(nanoseconds=10 * 1e6),  # 10 milliseconds
-            ),
+            LBRState, "/lbr/state", self.on_lbr_state_, 1
         )
 
     def on_lbr_state_(self, lbr_state: LBRState) -> None: