From c700f0c26e8997d2ab0dd6ade6a53ea07893f718 Mon Sep 17 00:00:00 2001 From: mhubii Date: Sat, 30 Sep 2023 19:11:59 +0100 Subject: [PATCH] updated messages in demos --- .../src/admittance_control_node.cpp | 17 +++++-------- .../src/admittance_controller.hpp | 11 ++++---- .../src/damped_least_squares.hpp | 1 + .../admittance_control_node.py | 25 ++++--------------- .../admittance_controller.py | 10 ++++---- .../src/joint_sine_overlay_node.cpp | 23 +++++++---------- .../src/torque_sine_overlay_node.cpp | 23 +++++++---------- .../src/wrench_sine_overlay_node.cpp | 25 ++++++++----------- .../joint_sine_overlay_node.py | 21 ++-------------- .../torque_sine_overlay_node.py | 21 ++-------------- .../wrench_sine_overlay_node.py | 23 ++--------------- 11 files changed, 57 insertions(+), 143 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 a743cfb2..1f0656e4 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 @@ -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/command", rclcpp::QoS(1) - .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE) - .deadline(std::chrono::milliseconds(10))); + lbr_position_command_pub_ = + create_publisher("/lbr/command", 1); lbr_state_sub_ = create_subscription( - "/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::SharedPtr lbr_command_pub_; + rclcpp::Publisher::SharedPtr lbr_position_command_pub_; rclcpp::Subscription::SharedPtr lbr_state_sub_; std::unique_ptr admittance_controller_; 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 217a850a..e21d5dd5 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 @@ -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 jacobian_solver_; diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/damped_least_squares.hpp b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/damped_least_squares.hpp index dda7ab5e..c983f7cf 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/damped_least_squares.hpp +++ b/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 #include 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 fa0b4867..c3d9e576 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 @@ -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_: 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 5a3463f4..f2c1eb9c 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 @@ -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_ diff --git a/lbr_demos/lbr_demos_fri_ros2_cpp/src/joint_sine_overlay_node.cpp b/lbr_demos/lbr_demos_fri_ros2_cpp/src/joint_sine_overlay_node.cpp index 5d737ce1..460bf5c2 100644 --- a/lbr_demos/lbr_demos_fri_ros2_cpp/src/joint_sine_overlay_node.cpp +++ b/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/command", rclcpp::QoS(1) - .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE) - .deadline(std::chrono::milliseconds(10))); + lbr_position_command_pub_ = + this->create_publisher("/lbr/command", 1); // create subscription to /lbr/state lbr_state_sub_ = this->create_subscription( - "/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::SharedPtr lbr_command_pub_; + rclcpp::Publisher::SharedPtr lbr_position_command_pub_; rclcpp::Subscription::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) { diff --git a/lbr_demos/lbr_demos_fri_ros2_cpp/src/torque_sine_overlay_node.cpp b/lbr_demos/lbr_demos_fri_ros2_cpp/src/torque_sine_overlay_node.cpp index b8a49c97..364926ad 100644 --- a/lbr_demos/lbr_demos_fri_ros2_cpp/src/torque_sine_overlay_node.cpp +++ b/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/command", rclcpp::QoS(1) - .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE) - .deadline(std::chrono::milliseconds(10))); + lbr_torque_command_pub_ = + this->create_publisher("/lbr/command", 1); // create subscription to /lbr/state lbr_state_sub_ = this->create_subscription( - "/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::SharedPtr lbr_command_pub_; + rclcpp::Publisher::SharedPtr lbr_torque_command_pub_; rclcpp::Subscription::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) { diff --git a/lbr_demos/lbr_demos_fri_ros2_cpp/src/wrench_sine_overlay_node.cpp b/lbr_demos/lbr_demos_fri_ros2_cpp/src/wrench_sine_overlay_node.cpp index 0f477018..c4bfaf9b 100644 --- a/lbr_demos/lbr_demos_fri_ros2_cpp/src/wrench_sine_overlay_node.cpp +++ b/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/command", rclcpp::QoS(1) - .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE) - .deadline(std::chrono::milliseconds(10))); + lbr_wrench_command_pub_ = + this->create_publisher("/lbr/command", 1); // create subscription to /lbr/state lbr_state_sub_ = this->create_subscription( - "/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::SharedPtr lbr_command_pub_; + rclcpp::Publisher::SharedPtr lbr_wrench_command_pub_; rclcpp::Subscription::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) { diff --git a/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/joint_sine_overlay_node.py b/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/joint_sine_overlay_node.py index 234ba81a..7b3bd706 100644 --- a/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/joint_sine_overlay_node.py +++ b/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: diff --git a/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/torque_sine_overlay_node.py b/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/torque_sine_overlay_node.py index fa955eed..c7fd3791 100644 --- a/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/torque_sine_overlay_node.py +++ b/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/torque_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: diff --git a/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/wrench_sine_overlay_node.py b/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/wrench_sine_overlay_node.py index 3cfe3fa8..b1463c68 100644 --- a/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/wrench_sine_overlay_node.py +++ b/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/wrench_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,27 +17,10 @@ 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 - ), - ) + self.lbr_state_sub_ = self.create_subscription(LBRState, "/lbr/state", 1) def on_lbr_state_(self, lbr_state: LBRState) -> None: self.lbr_command_.joint_position = lbr_state.ipo_joint_position