Skip to content

Commit 2cdcf6b

Browse files
mhubiimhubii
committed
move command guard in preparation for state guard (#271)
1 parent 30edf70 commit 2cdcf6b

File tree

9 files changed

+69
-49
lines changed

9 files changed

+69
-49
lines changed

‎lbr_fri_ros2/CMakeLists.txt

+1-1
Original file line numberDiff line numberDiff line change
@@ -25,14 +25,14 @@ find_package(urdf REQUIRED)
2525
# lbr_fri_ros2
2626
add_library(lbr_fri_ros2
2727
SHARED
28+
src/guards/command_guard.cpp
2829
src/interfaces/base_command.cpp
2930
src/interfaces/position_command.cpp
3031
src/interfaces/state.cpp
3132
src/interfaces/torque_command.cpp
3233
src/interfaces/wrench_command.cpp
3334
src/app.cpp
3435
src/async_client.cpp
35-
src/command_guard.cpp
3636
src/control.cpp
3737
src/filters.cpp
3838
src/ft_estimator.cpp

‎lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp renamed to ‎lbr_fri_ros2/include/lbr_fri_ros2/guards/command_guard.hpp

File renamed without changes.

‎lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -12,9 +12,9 @@
1212
#include "friClientVersion.h"
1313

1414
#include "lbr_fri_idl/msg/lbr_command.hpp"
15-
#include "lbr_fri_ros2/command_guard.hpp"
1615
#include "lbr_fri_ros2/filters.hpp"
1716
#include "lbr_fri_ros2/formatting.hpp"
17+
#include "lbr_fri_ros2/guards/command_guard.hpp"
1818
#include "lbr_fri_ros2/types.hpp"
1919

2020
namespace lbr_fri_ros2 {

‎lbr_fri_ros2/src/command_guard.cpp renamed to ‎lbr_fri_ros2/src/guards/command_guard.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
#include "lbr_fri_ros2/command_guard.hpp"
1+
#include "lbr_fri_ros2/guards/command_guard.hpp"
22

33
namespace lbr_fri_ros2 {
44
CommandGuard::CommandGuard(const CommandGuardParameters &command_guard_parameters)

‎lbr_fri_ros2/test/test_position_command.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,7 @@
77

88
#include "lbr_fri_ros2/app.hpp"
99
#include "lbr_fri_ros2/async_client.hpp"
10-
#include "lbr_fri_ros2/command_guard.hpp"
10+
#include "lbr_fri_ros2/guards/command_guard.hpp"
1111

1212
int main() {
1313
rclcpp::init(0, nullptr);

‎lbr_fri_ros2/test/test_torque_command.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,7 @@
77

88
#include "lbr_fri_ros2/app.hpp"
99
#include "lbr_fri_ros2/async_client.hpp"
10-
#include "lbr_fri_ros2/command_guard.hpp"
10+
#include "lbr_fri_ros2/guards/command_guard.hpp"
1111

1212
int main() {
1313
rclcpp::init(0, nullptr);

‎lbr_fri_ros2/test/test_wrench_command.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,7 @@
77

88
#include "lbr_fri_ros2/app.hpp"
99
#include "lbr_fri_ros2/async_client.hpp"
10-
#include "lbr_fri_ros2/command_guard.hpp"
10+
#include "lbr_fri_ros2/guards/command_guard.hpp"
1111

1212
int main() {
1313
rclcpp::init(0, nullptr);

‎lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp

+4-3
Original file line numberDiff line numberDiff line change
@@ -22,9 +22,9 @@
2222
#include "lbr_fri_idl/msg/lbr_state.hpp"
2323
#include "lbr_fri_ros2/app.hpp"
2424
#include "lbr_fri_ros2/async_client.hpp"
25-
#include "lbr_fri_ros2/command_guard.hpp"
2625
#include "lbr_fri_ros2/formatting.hpp"
2726
#include "lbr_fri_ros2/ft_estimator.hpp"
27+
#include "lbr_fri_ros2/guards/command_guard.hpp"
2828
#include "lbr_fri_ros2/interfaces/state.hpp"
2929
#include "lbr_fri_ros2/types.hpp"
3030
#include "lbr_ros2_control/system_interface_type_values.hpp"
@@ -42,11 +42,11 @@ struct SystemInterfaceParameters {
4242
int32_t port_id{30200};
4343
const char *remote_host{nullptr};
4444
int32_t rt_prio{80};
45-
bool open_loop{true};
4645
double joint_position_tau{0.04};
4746
std::string command_guard_variant{"default"};
4847
double external_torque_tau{0.04};
4948
double measured_torque_tau{0.04};
49+
bool open_loop{true};
5050
};
5151

5252
struct EstimatedFTSensorParameters {
@@ -105,7 +105,8 @@ class SystemInterface : public hardware_interface::SystemInterface {
105105

106106
protected:
107107
// setup
108-
bool parse_parameters_(const hardware_interface::HardwareInfo &info);
108+
bool parse_parameters_();
109+
bool parse_ft_parameters_();
109110
void nan_command_interfaces_();
110111
void nan_state_interfaces_();
111112
bool verify_number_of_joints_();

‎lbr_ros2_control/src/system_interface.cpp

+59-40
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,8 @@
33
namespace lbr_ros2_control {
44
controller_interface::CallbackReturn
55
SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) {
6-
auto ret = hardware_interface::SystemInterface::on_init(system_info);
6+
auto ret =
7+
hardware_interface::SystemInterface::on_init(system_info); // parses system_info to info_
78
if (ret != controller_interface::CallbackReturn::SUCCESS) {
89
RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME),
910
lbr_fri_ros2::ColorScheme::ERROR << "Failed to initialize SystemInterface"
@@ -13,23 +14,23 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) {
1314

1415
// parameters_ from lbr_system_interface.xacro (default configurations located in
1516
// lbr_description/ros2_control/lbr_system_interface.xacro)
16-
if (!parse_parameters_(system_info)) {
17+
if (!parse_parameters_()) {
1718
return controller_interface::CallbackReturn::ERROR;
1819
}
1920

2021
// setup driver
2122
lbr_fri_ros2::CommandGuardParameters command_guard_parameters;
2223
lbr_fri_ros2::StateInterfaceParameters state_interface_parameters;
23-
for (std::size_t idx = 0; idx < system_info.joints.size(); ++idx) {
24-
command_guard_parameters.joint_names[idx] = system_info.joints[idx].name;
24+
for (std::size_t idx = 0; idx < info_.joints.size(); ++idx) {
25+
command_guard_parameters.joint_names[idx] = info_.joints[idx].name;
2526
command_guard_parameters.max_positions[idx] =
26-
std::stod(system_info.joints[idx].parameters.at("max_position"));
27+
std::stod(info_.joints[idx].parameters.at("max_position"));
2728
command_guard_parameters.min_positions[idx] =
28-
std::stod(system_info.joints[idx].parameters.at("min_position"));
29+
std::stod(info_.joints[idx].parameters.at("min_position"));
2930
command_guard_parameters.max_velocities[idx] =
30-
std::stod(system_info.joints[idx].parameters.at("max_velocity"));
31+
std::stod(info_.joints[idx].parameters.at("max_velocity"));
3132
command_guard_parameters.max_torques[idx] =
32-
std::stod(system_info.joints[idx].parameters.at("max_torque"));
33+
std::stod(info_.joints[idx].parameters.at("max_torque"));
3334
}
3435
state_interface_parameters.external_torque_tau = parameters_.external_torque_tau;
3536
state_interface_parameters.measured_torque_tau = parameters_.measured_torque_tau;
@@ -52,22 +53,9 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) {
5253
nan_last_hw_states_();
5354

5455
// setup force-torque estimator
55-
std::transform(info_.sensors[1].parameters.at("enabled").begin(),
56-
info_.sensors[1].parameters.at("enabled").end(),
57-
info_.sensors[1].parameters.at("enabled").begin(),
58-
::tolower); // convert to lower case
59-
ft_parameters_.enabled = info_.sensors[1].parameters.at("enabled") == "true";
60-
ft_parameters_.update_rate = std::stoul(info_.sensors[1].parameters.at("update_rate"));
61-
ft_parameters_.rt_prio = std::stoi(info_.sensors[1].parameters.at("rt_prio"));
62-
ft_parameters_.chain_root = info_.sensors[1].parameters.at("chain_root");
63-
ft_parameters_.chain_tip = info_.sensors[1].parameters.at("chain_tip");
64-
ft_parameters_.damping = std::stod(info_.sensors[1].parameters.at("damping"));
65-
ft_parameters_.force_x_th = std::stod(info_.sensors[1].parameters.at("force_x_th"));
66-
ft_parameters_.force_y_th = std::stod(info_.sensors[1].parameters.at("force_y_th"));
67-
ft_parameters_.force_z_th = std::stod(info_.sensors[1].parameters.at("force_z_th"));
68-
ft_parameters_.torque_x_th = std::stod(info_.sensors[1].parameters.at("torque_x_th"));
69-
ft_parameters_.torque_y_th = std::stod(info_.sensors[1].parameters.at("torque_y_th"));
70-
ft_parameters_.torque_z_th = std::stod(info_.sensors[1].parameters.at("torque_z_th"));
56+
if (!parse_ft_parameters_()) {
57+
return controller_interface::CallbackReturn::ERROR;
58+
}
7159
if (ft_parameters_.enabled) {
7260
ft_estimator_impl_ptr_ = std::make_shared<lbr_fri_ros2::FTEstimatorImpl>(
7361
info_.original_xml, ft_parameters_.chain_root, ft_parameters_.chain_tip,
@@ -338,12 +326,12 @@ hardware_interface::return_type SystemInterface::write(const rclcpp::Time & /*ti
338326
return hardware_interface::return_type::OK;
339327
}
340328

341-
bool SystemInterface::parse_parameters_(const hardware_interface::HardwareInfo &system_info) {
329+
bool SystemInterface::parse_parameters_() {
342330
try {
343331
parameters_.fri_client_sdk_major_version =
344-
std::stoul(system_info.hardware_parameters.at("fri_client_sdk_major_version"));
332+
std::stoul(info_.hardware_parameters.at("fri_client_sdk_major_version"));
345333
parameters_.fri_client_sdk_minor_version =
346-
std::stoul(system_info.hardware_parameters.at("fri_client_sdk_minor_version"));
334+
std::stoul(info_.hardware_parameters.at("fri_client_sdk_minor_version"));
347335
if (parameters_.fri_client_sdk_major_version != FRI_CLIENT_VERSION_MAJOR) {
348336
RCLCPP_ERROR_STREAM(
349337
rclcpp::get_logger(LOGGER_NAME),
@@ -354,7 +342,7 @@ bool SystemInterface::parse_parameters_(const hardware_interface::HardwareInfo &
354342
<< lbr_fri_ros2::ColorScheme::ENDC);
355343
return false;
356344
}
357-
std::string client_command_mode = system_info.hardware_parameters.at("client_command_mode");
345+
std::string client_command_mode = info_.hardware_parameters.at("client_command_mode");
358346
if (client_command_mode == "position") {
359347
#if FRI_CLIENT_VERSION_MAJOR == 1
360348
parameters_.client_command_mode = KUKA::FRI::EClientCommandMode::POSITION;
@@ -375,7 +363,7 @@ bool SystemInterface::parse_parameters_(const hardware_interface::HardwareInfo &
375363
<< lbr_fri_ros2::ColorScheme::ENDC);
376364
return false;
377365
}
378-
parameters_.port_id = std::stoul(info_.hardware_parameters["port_id"]);
366+
parameters_.port_id = std::stoul(info_.hardware_parameters.at("port_id"));
379367
if (parameters_.port_id < 30200 || parameters_.port_id > 30209) {
380368
RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME),
381369
lbr_fri_ros2::ColorScheme::ERROR
@@ -384,19 +372,21 @@ bool SystemInterface::parse_parameters_(const hardware_interface::HardwareInfo &
384372
<< lbr_fri_ros2::ColorScheme::ENDC);
385373
return false;
386374
}
387-
info_.hardware_parameters["remote_host"] == "INADDR_ANY"
375+
info_.hardware_parameters.at("remote_host") == "INADDR_ANY"
388376
? parameters_.remote_host = NULL
389-
: parameters_.remote_host = info_.hardware_parameters["remote_host"].c_str();
390-
parameters_.rt_prio = std::stoul(info_.hardware_parameters["rt_prio"]);
391-
std::transform(info_.hardware_parameters["open_loop"].begin(),
392-
info_.hardware_parameters["open_loop"].end(),
393-
info_.hardware_parameters["open_loop"].begin(),
377+
: parameters_.remote_host = info_.hardware_parameters.at("remote_host").c_str();
378+
parameters_.rt_prio = std::stoul(info_.hardware_parameters.at("rt_prio"));
379+
parameters_.joint_position_tau = std::stod(info_.hardware_parameters.at("joint_position_tau"));
380+
parameters_.command_guard_variant = info_.hardware_parameters.at("command_guard_variant");
381+
parameters_.external_torque_tau =
382+
std::stod(info_.hardware_parameters.at("external_torque_tau"));
383+
parameters_.measured_torque_tau =
384+
std::stod(info_.hardware_parameters.at("measured_torque_tau"));
385+
std::transform(info_.hardware_parameters.at("open_loop").begin(),
386+
info_.hardware_parameters.at("open_loop").end(),
387+
info_.hardware_parameters.at("open_loop").begin(),
394388
::tolower); // convert to lower case
395-
parameters_.open_loop = info_.hardware_parameters["open_loop"] == "true";
396-
parameters_.joint_position_tau = std::stod(info_.hardware_parameters["joint_position_tau"]);
397-
parameters_.command_guard_variant = system_info.hardware_parameters.at("command_guard_variant");
398-
parameters_.external_torque_tau = std::stod(info_.hardware_parameters["external_torque_tau"]);
399-
parameters_.measured_torque_tau = std::stod(info_.hardware_parameters["measured_torque_tau"]);
389+
parameters_.open_loop = info_.hardware_parameters.at("open_loop") == "true";
400390
} catch (const std::out_of_range &e) {
401391
RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME),
402392
lbr_fri_ros2::ColorScheme::ERROR
@@ -407,6 +397,35 @@ bool SystemInterface::parse_parameters_(const hardware_interface::HardwareInfo &
407397
return true;
408398
}
409399

400+
bool SystemInterface::parse_ft_parameters_() {
401+
try {
402+
std::transform(info_.sensors[1].parameters.at("enabled").begin(),
403+
info_.sensors[1].parameters.at("enabled").end(),
404+
info_.sensors[1].parameters.at("enabled").begin(),
405+
::tolower); // convert to lower case
406+
const auto &estimated_ft_sensor = info_.sensors[1];
407+
ft_parameters_.enabled = estimated_ft_sensor.parameters.at("enabled") == "true";
408+
ft_parameters_.update_rate = std::stoul(estimated_ft_sensor.parameters.at("update_rate"));
409+
ft_parameters_.rt_prio = std::stoi(estimated_ft_sensor.parameters.at("rt_prio"));
410+
ft_parameters_.chain_root = estimated_ft_sensor.parameters.at("chain_root");
411+
ft_parameters_.chain_tip = estimated_ft_sensor.parameters.at("chain_tip");
412+
ft_parameters_.damping = std::stod(estimated_ft_sensor.parameters.at("damping"));
413+
ft_parameters_.force_x_th = std::stod(estimated_ft_sensor.parameters.at("force_x_th"));
414+
ft_parameters_.force_y_th = std::stod(estimated_ft_sensor.parameters.at("force_y_th"));
415+
ft_parameters_.force_z_th = std::stod(estimated_ft_sensor.parameters.at("force_z_th"));
416+
ft_parameters_.torque_x_th = std::stod(estimated_ft_sensor.parameters.at("torque_x_th"));
417+
ft_parameters_.torque_y_th = std::stod(estimated_ft_sensor.parameters.at("torque_y_th"));
418+
ft_parameters_.torque_z_th = std::stod(estimated_ft_sensor.parameters.at("torque_z_th"));
419+
} catch (const std::out_of_range &e) {
420+
RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME),
421+
lbr_fri_ros2::ColorScheme::ERROR
422+
<< "Failed to parse force-torque sensor parameters with: " << e.what()
423+
<< lbr_fri_ros2::ColorScheme::ENDC);
424+
return false;
425+
}
426+
return true;
427+
}
428+
410429
void SystemInterface::nan_command_interfaces_() {
411430
hw_lbr_command_.joint_position.fill(std::numeric_limits<double>::quiet_NaN());
412431
hw_lbr_command_.torque.fill(std::numeric_limits<double>::quiet_NaN());

0 commit comments

Comments
 (0)