Skip to content

Commit

Permalink
restrict lbr command guard access to only log
Browse files Browse the repository at this point in the history
  • Loading branch information
@mhubii
mhubii committed Aug 6, 2023
1 parent 9129df1 commit 8b1af90
Show file tree
Hide file tree
Showing 3 changed files with 46 additions and 36 deletions.
32 changes: 18 additions & 14 deletions lbr_fri_ros2/include/lbr_fri_ros2/lbr_command_guard.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,23 +29,24 @@ class LBRCommandGuard {
/**
* @brief Construct a new LBRCommandGuard object.
*
* @param[in] node Shared node for logging
* @param[in] logger_interface Shared node logger interface
* @param[in] min_position Minimum joint position [rad]
* @param[in] max_position Maximum joint position [rad]
* @param[in] max_velocity Maximum joint velocity [rad/s]
* @param[in] max_torque Maximal torque [Nm]
*/
LBRCommandGuard(const rclcpp::Node::SharedPtr node, const JointArray &min_position,
const JointArray &max_position, const JointArray &max_velocity,
const JointArray &max_torque);
LBRCommandGuard(const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface,
const JointArray &min_position, const JointArray &max_position,
const JointArray &max_velocity, const JointArray &max_torque);

/**
* @brief Construct a new LBRCommandGuard object.
*
* @param[in] node Shared node for logging
* @param[in] logger_interface Shared node logger interface
* @param robot_description String containing URDF robot rescription
*/
LBRCommandGuard(const rclcpp::Node::SharedPtr node, const std::string &robot_description);
LBRCommandGuard(const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface,
const std::string &robot_description);

/**
* @brief Checks for command validity given the current state.
Expand Down Expand Up @@ -102,7 +103,8 @@ class LBRCommandGuard {
virtual bool command_in_torque_limits_(const lbr_fri_msgs::msg::LBRCommand &lbr_command,
const KUKA::FRI::LBRState &lbr_state) const;

rclcpp::Node::SharedPtr node_; /**< Shared node for logging.*/
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr
logger_interface_; /**< Shared node logger interface.*/

JointArray min_position_; /**< Minimum joint position [rad].*/
JointArray max_position_; /**< Maximum joint position [rad].*/
Expand All @@ -121,11 +123,13 @@ class LBRSafeStopCommandGuard : public LBRCommandGuard {
/**
* @brief Construct a new LBRSafeStopCommandGuard object.
*
* @param[in] node Shared node for logging
* @param[in] logger_interface Shared node logger interface
* @param robot_description String containing URDF robot rescription
*/
LBRSafeStopCommandGuard(const rclcpp::Node::SharedPtr node, const std::string &robot_description)
: LBRCommandGuard(node, robot_description){};
LBRSafeStopCommandGuard(
const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface,
const std::string &robot_description)
: LBRCommandGuard(logger_interface, robot_description){};

protected:
/**
Expand All @@ -144,13 +148,13 @@ class LBRSafeStopCommandGuard : public LBRCommandGuard {
/**
* @brief Creates an LBRCommandGuard object.
*
* @param[in] node Shared node for logging
* @param[in] logger_interface Shared node logger interface
* @param[in] robot_description String containing URDF robot rescription
* @param[in] variant Which variant of LBRCommandGuard to create
* @return std::unique_ptr<LBRCommandGuard> Pointer to LBRCommandGuard object
*/
std::unique_ptr<LBRCommandGuard> lbr_command_guard_factory(const rclcpp::Node::SharedPtr node,
const std::string &robot_description,
const std::string &variant);
std::unique_ptr<LBRCommandGuard> lbr_command_guard_factory(
const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface,
const std::string &robot_description, const std::string &variant);
} // end of namespace lbr_fri_ros2
#endif // LBR_FRI_ROS2__LBR_COMMAND_GUARD_HPP_
3 changes: 2 additions & 1 deletion lbr_fri_ros2/src/lbr_app.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,8 @@ LBRApp::LBRApp(const rclcpp::Node::SharedPtr node) : node_(node) {
rmw_qos_profile_services_default);

lbr_client_ = std::make_shared<LBRClient>(
node_, lbr_command_guard_factory(node_, robot_description_, command_guard_variant_));
node_, lbr_command_guard_factory(node_->get_node_logging_interface(), robot_description_,
command_guard_variant_));
connection_ = std::make_unique<KUKA::FRI::UdpConnection>();
app_ = std::make_unique<KUKA::FRI::ClientApplication>(*connection_, *lbr_client_);

Expand Down
47 changes: 26 additions & 21 deletions lbr_fri_ros2/src/lbr_command_guard.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,19 +1,21 @@
#include "lbr_fri_ros2/lbr_command_guard.hpp"

namespace lbr_fri_ros2 {
LBRCommandGuard::LBRCommandGuard(const rclcpp::Node::SharedPtr node, const JointArray &min_position,
const JointArray &max_position, const JointArray &max_velocity,
const JointArray &max_torque)
: node_(node), min_position_(min_position), max_position_(max_position),
LBRCommandGuard::LBRCommandGuard(
const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface,
const JointArray &min_position, const JointArray &max_position, const JointArray &max_velocity,
const JointArray &max_torque)
: logger_interface_(logger_interface), min_position_(min_position), max_position_(max_position),
max_velocity_(max_velocity), max_torque_(max_torque) {}

LBRCommandGuard::LBRCommandGuard(const rclcpp::Node::SharedPtr node,
const std::string &robot_description)
: node_(node) {
LBRCommandGuard::LBRCommandGuard(
const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface,
const std::string &robot_description)
: logger_interface_(logger_interface) {
urdf::Model model;
if (!model.initString(robot_description)) {
std::string error_msg = "Failed to intialize urdf model from robot description.";
RCLCPP_ERROR(node_->get_logger(), error_msg.c_str());
RCLCPP_ERROR(logger_interface_->get_logger(), error_msg.c_str());
throw std::runtime_error(error_msg);
}
std::size_t jnt_cnt = 0;
Expand All @@ -22,7 +24,7 @@ LBRCommandGuard::LBRCommandGuard(const rclcpp::Node::SharedPtr node,
if (joint->type == urdf::Joint::REVOLUTE) {
if (jnt_cnt > std::tuple_size<JointArray>()) {
std::string error_msgs = "Found too many joints in robot description.";
RCLCPP_ERROR(node_->get_logger(), error_msgs.c_str());
RCLCPP_ERROR(logger_interface_->get_logger(), error_msgs.c_str());
throw std::runtime_error(error_msgs);
}
min_position_[jnt_cnt] = joint->limits->lower;
Expand All @@ -34,7 +36,7 @@ LBRCommandGuard::LBRCommandGuard(const rclcpp::Node::SharedPtr node,
}
if (jnt_cnt != std::tuple_size<JointArray>()) {
std::string error_msg = "Didn't find expected number of joints in robot description.";
RCLCPP_ERROR(node_->get_logger(), error_msg.c_str());
RCLCPP_ERROR(logger_interface_->get_logger(), error_msg.c_str());
throw std::runtime_error(error_msg);
};
}
Expand Down Expand Up @@ -78,7 +80,7 @@ bool LBRCommandGuard::is_valid_command(const lbr_fri_msgs::msg::LBRCommand &lbr_
}
return true;
default:
RCLCPP_ERROR(node_->get_logger(), "Invalid EClientCommandMode provided.");
RCLCPP_ERROR(logger_interface_->get_logger(), "Invalid EClientCommandMode provided.");
return false;
}

Expand All @@ -94,7 +96,8 @@ bool LBRCommandGuard::command_in_position_limits_(const lbr_fri_msgs::msg::LBRCo
for (std::size_t i = 0; i < lbr_command.joint_position.size(); ++i) {
if (lbr_command.joint_position[i] < min_position_[i] ||
lbr_command.joint_position[i] > max_position_[i]) {
RCLCPP_ERROR(node_->get_logger(), "Position command not in limits for joint %ld.", i);
RCLCPP_ERROR(logger_interface_->get_logger(), "Position command not in limits for joint %ld.",
i);
return false;
}
}
Expand All @@ -107,7 +110,7 @@ bool LBRCommandGuard::command_in_velocity_limits_(const lbr_fri_msgs::msg::LBRCo
for (std::size_t i = 0; i < lbr_command.joint_position[i]; ++i) {
if (std::abs(lbr_command.joint_position[i] - lbr_state.getMeasuredJointPosition()[i]) / dt >
max_velocity_[i]) {
RCLCPP_ERROR(node_->get_logger(), "Velocity not in limits for joint %ld.", i);
RCLCPP_ERROR(logger_interface_->get_logger(), "Velocity not in limits for joint %ld.", i);
return false;
}
}
Expand All @@ -118,7 +121,8 @@ bool LBRCommandGuard::command_in_torque_limits_(const lbr_fri_msgs::msg::LBRComm
const KUKA::FRI::LBRState &lbr_state) const {
for (std::size_t i = 0; i < lbr_command.torque.size(); ++i) {
if (std::abs(lbr_command.torque[i] + lbr_state.getExternalTorque()[i]) > max_torque_[i]) {
RCLCPP_ERROR(node_->get_logger(), "Torque command not in limits for joint %ld.", i);
RCLCPP_ERROR(logger_interface_->get_logger(), "Torque command not in limits for joint %ld.",
i);
return false;
}
}
Expand All @@ -132,24 +136,25 @@ bool LBRSafeStopCommandGuard::command_in_position_limits_(
min_position_[i] + max_velocity_[i] * lbr_state.getSampleTime() ||
lbr_command.joint_position[i] >
max_position_[i] - max_velocity_[i] * lbr_state.getSampleTime()) {
RCLCPP_ERROR(node_->get_logger(), "Position command not in limits for joint %ld.", i);
RCLCPP_ERROR(logger_interface_->get_logger(), "Position command not in limits for joint %ld.",
i);
return false;
}
}
return true;
}

std::unique_ptr<LBRCommandGuard> lbr_command_guard_factory(const rclcpp::Node::SharedPtr node,
const std::string &robot_description,
const std::string &variant) {
std::unique_ptr<LBRCommandGuard> lbr_command_guard_factory(
const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface,
const std::string &robot_description, const std::string &variant) {
if (variant == "default") {
return std::make_unique<LBRCommandGuard>(node, robot_description);
return std::make_unique<LBRCommandGuard>(logger_interface, robot_description);
}
if (variant == "safe_stop") {
return std::make_unique<LBRSafeStopCommandGuard>(node, robot_description);
return std::make_unique<LBRSafeStopCommandGuard>(logger_interface, robot_description);
}
std::string error_msg = "Invalid LBRCommandGuard variant provided.";
RCLCPP_ERROR(node->get_logger(), error_msg.c_str());
RCLCPP_ERROR(logger_interface->get_logger(), error_msg.c_str());
throw std::runtime_error(error_msg);
}
} // end of namespace lbr_fri_ros2

0 comments on commit 8b1af90

Please sign in to comment.