Skip to content

Commit ae1616e

Browse files
mhubiimhubii
committed
updated system interface for new clients
1 parent 7e9ee55 commit ae1616e

File tree

9 files changed

+63
-58
lines changed

9 files changed

+63
-58
lines changed

‎lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp

+2
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,8 @@
11
#ifndef LBR_FRI_ROS2__INTERFACES__POSITION_COMMAND_HPP_
22
#define LBR_FRI_ROS2__INTERFACES__POSITION_COMMAND_HPP_
33

4+
#include <algorithm>
5+
46
#include "lbr_fri_ros2/interfaces/base_command.hpp"
57

68
namespace lbr_fri_ros2 {

‎lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp

+2
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,8 @@
11
#ifndef LBR_FRI_ROS2__INTERFACES__TORQUE_COMMAND_HPP_
22
#define LBR_FRI_ROS2__INTERFACES__TORQUE_COMMAND_HPP_
33

4+
#include <algorithm>
5+
46
#include "lbr_fri_ros2/interfaces/base_command.hpp"
57

68
namespace lbr_fri_ros2 {

‎lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp

+2
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,8 @@
11
#ifndef LBR_FRI_ROS2__INTERFACES__WRENCH_COMMAND_HPP_
22
#define LBR_FRI_ROS2__INTERFACES__WRENCH_COMMAND_HPP_
33

4+
#include <algorithm>
5+
46
#include "lbr_fri_ros2/interfaces/base_command.hpp"
57

68
namespace lbr_fri_ros2 {

‎lbr_fri_ros2/src/interfaces/position_command.cpp

+4
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,10 @@ void PositionCommandInterface::buffered_command_to_fri(fri_command_t_ref command
2929
throw std::runtime_error(err);
3030
}
3131
#endif
32+
if (std::any_of(command_target_.joint_position.cbegin(), command_target_.joint_position.cend(),
33+
[](const double &v) { return std::isnan(v); })) {
34+
this->init_command();
35+
}
3236
if (!command_guard_) {
3337
std::string err = "Uninitialized command guard.";
3438
RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()),

‎lbr_fri_ros2/src/interfaces/torque_command.cpp

+6
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,12 @@ void TorqueCommandInterface::buffered_command_to_fri(fri_command_t_ref command,
1717
ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC);
1818
throw std::runtime_error(err);
1919
}
20+
if (std::any_of(command_target_.joint_position.cbegin(), command_target_.joint_position.cend(),
21+
[](const double &v) { return std::isnan(v); }) ||
22+
std::any_of(command_target_.torque.cbegin(), command_target_.torque.cend(),
23+
[](const double &v) { return std::isnan(v); })) {
24+
this->init_command();
25+
}
2026
if (!command_guard_) {
2127
std::string err = "Uninitialized command guard.";
2228
RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()),

‎lbr_fri_ros2/src/interfaces/wrench_command.cpp

+6
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,12 @@ void WrenchCommandInterface::buffered_command_to_fri(fri_command_t_ref command,
1616
RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME()), err.c_str());
1717
throw std::runtime_error(err);
1818
}
19+
if (std::any_of(command_target_.joint_position.cbegin(), command_target_.joint_position.cend(),
20+
[](const double &v) { return std::isnan(v); }) ||
21+
std::any_of(command_target_.wrench.cbegin(), command_target_.wrench.cend(),
22+
[](const double &v) { return std::isnan(v); })) {
23+
this->init_command();
24+
}
1925
if (!command_guard_) {
2026
std::string err = "Uninitialized command guard.";
2127
RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME()), err.c_str());

‎lbr_ros2_control/config/lbr_system_interface.xacro

+2
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717
A6_velocity_limit
1818
A7_velocity_limit
1919
max_torque
20+
client_command_mode:=^|position
2021
sim:=^|true
2122
port_id:=^|30200
2223
remote_host:=^|INADDR_ANY
@@ -50,6 +51,7 @@
5051
<xacro:unless value="${sim}">
5152
<hardware>
5253
<plugin>lbr_ros2_control::SystemInterface</plugin>
54+
<param name="client_command_mode">${client_command_mode}</param>
5355
<param name="port_id">${port_id}</param>
5456
<param name="remote_host">${remote_host}</param>
5557
<param name="rt_prio">${rt_prio}</param>

‎lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -26,11 +26,12 @@
2626
#include "lbr_fri_ros2/filters.hpp"
2727
#include "lbr_fri_ros2/formatting.hpp"
2828
#include "lbr_fri_ros2/ft_estimator.hpp"
29-
#include "lbr_fri_ros2/state_interface.hpp"
29+
#include "lbr_fri_ros2/interfaces/state.hpp"
3030
#include "lbr_ros2_control/system_interface_type_values.hpp"
3131

3232
namespace lbr_ros2_control {
3333
struct SystemInterfaceParameters {
34+
KUKA::FRI::EClientCommandMode client_command_mode{KUKA::FRI::EClientCommandMode::POSITION};
3435
int32_t port_id{30200};
3536
const char *remote_host{nullptr};
3637
int32_t rt_prio{80};

‎lbr_ros2_control/src/system_interface.cpp

+37-57
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,27 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) {
1212
}
1313

1414
// parameters_ from config/lbr_system_interface.xacro
15+
std::string client_command_mode = system_info.hardware_parameters.at("client_command_mode");
16+
if (client_command_mode == "position") {
17+
#if FRICLIENT_VERSION_MAJOR == 1
18+
parameters_.client_command_mode = KUKA::FRI::EClientCommandMode::POSITION;
19+
#endif
20+
#if FRICLIENT_VERSION_MAJOR == 2
21+
parameters_.client_command_mode = KUKA::FRI::EClientCommandMode::JOINT_POSITION;
22+
#endif
23+
} else if (client_command_mode == "torque") {
24+
parameters_.client_command_mode = KUKA::FRI::EClientCommandMode::TORQUE;
25+
} else if (client_command_mode == "wrench") {
26+
parameters_.client_command_mode = KUKA::FRI::EClientCommandMode::WRENCH;
27+
} else {
28+
RCLCPP_ERROR_STREAM(
29+
rclcpp::get_logger(LOGGER_NAME),
30+
lbr_fri_ros2::ColorScheme::ERROR
31+
<< "Expected client_command_mode 'position', 'torque' or 'wrench', got '"
32+
<< lbr_fri_ros2::ColorScheme::BOLD << parameters_.client_command_mode << "'"
33+
<< lbr_fri_ros2::ColorScheme::ENDC);
34+
return controller_interface::CallbackReturn::ERROR;
35+
}
1536
parameters_.port_id = std::stoul(info_.hardware_parameters["port_id"]);
1637
if (parameters_.port_id < 30200 || parameters_.port_id > 30209) {
1738
RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME),
@@ -72,8 +93,8 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) {
7293

7394
try {
7495
async_client_ptr_ = std::make_shared<lbr_fri_ros2::AsyncClient>(
75-
pid_parameters, command_guard_parameters, parameters_.command_guard_variant,
76-
state_interface_parameters, parameters_.open_loop);
96+
parameters_.client_command_mode, pid_parameters, command_guard_parameters,
97+
parameters_.command_guard_variant, state_interface_parameters, parameters_.open_loop);
7798
app_ptr_ = std::make_unique<lbr_fri_ros2::App>(async_client_ptr_);
7899
} catch (const std::exception &e) {
79100
RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME),
@@ -231,9 +252,9 @@ controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_l
231252
if (!app_ptr_->open_udp_socket(parameters_.port_id, parameters_.remote_host)) {
232253
return controller_interface::CallbackReturn::ERROR;
233254
}
234-
app_ptr_->run(parameters_.rt_prio);
255+
app_ptr_->run_async(parameters_.rt_prio);
235256
int attempt = 0;
236-
while (!async_client_ptr_->get_state_interface().is_initialized() && rclcpp::ok()) {
257+
while (!async_client_ptr_->get_state_interface()->is_initialized() && rclcpp::ok()) {
237258
RCLCPP_INFO_STREAM(
238259
rclcpp::get_logger(LOGGER_NAME),
239260
"Awaiting robot heartbeat. Attempt "
@@ -246,7 +267,7 @@ controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_l
246267
std::this_thread::sleep_for(std::chrono::seconds(1));
247268
}
248269
if (!async_client_ptr_->get_state_interface()
249-
.is_initialized()) { // check connection should rclcpp::ok() fail
270+
->is_initialized()) { // check connection should rclcpp::ok() fail
250271
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Failed to connect");
251272
return controller_interface::CallbackReturn::ERROR;
252273
}
@@ -256,13 +277,13 @@ controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_l
256277
RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME),
257278
"Control mode '"
258279
<< lbr_fri_ros2::EnumMaps::control_mode_map(
259-
async_client_ptr_->get_state_interface().get_state().control_mode)
280+
async_client_ptr_->get_state_interface()->get_state().control_mode)
260281
.c_str()
261282
<< "'");
262283
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Sample time %.3f s / %.1f Hz",
263-
async_client_ptr_->get_state_interface().get_state().sample_time,
264-
1. / async_client_ptr_->get_state_interface().get_state().sample_time);
265-
while (!(async_client_ptr_->get_state_interface().get_state().session_state >=
284+
async_client_ptr_->get_state_interface()->get_state().sample_time,
285+
1. / async_client_ptr_->get_state_interface()->get_state().sample_time);
286+
while (!(async_client_ptr_->get_state_interface()->get_state().session_state >=
266287
KUKA::FRI::ESessionState::COMMANDING_WAIT) &&
267288
rclcpp::ok()) {
268289
RCLCPP_INFO_STREAM(
@@ -273,7 +294,7 @@ controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_l
273294
<< lbr_fri_ros2::ColorScheme::ENDC << "' state. Current state '"
274295
<< lbr_fri_ros2::ColorScheme::BOLD << lbr_fri_ros2::ColorScheme::OKBLUE
275296
<< lbr_fri_ros2::EnumMaps::session_state_map(
276-
async_client_ptr_->get_state_interface().get_state().session_state)
297+
async_client_ptr_->get_state_interface()->get_state().session_state)
277298
<< lbr_fri_ros2::ColorScheme::ENDC << "'.");
278299
std::this_thread::sleep_for(std::chrono::seconds(1));
279300
}
@@ -285,18 +306,18 @@ controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_l
285306

286307
controller_interface::CallbackReturn
287308
SystemInterface::on_deactivate(const rclcpp_lifecycle::State &) {
288-
app_ptr_->stop_run();
309+
app_ptr_->request_stop();
289310
app_ptr_->close_udp_socket();
290311
return controller_interface::CallbackReturn::SUCCESS;
291312
}
292313

293314
hardware_interface::return_type SystemInterface::read(const rclcpp::Time & /*time*/,
294315
const rclcpp::Duration &period) {
295-
if (!async_client_ptr_->get_state_interface().is_initialized()) {
316+
if (!async_client_ptr_->get_state_interface()->is_initialized()) {
296317
return hardware_interface::return_type::OK;
297318
}
298319

299-
hw_lbr_state_ = async_client_ptr_->get_state_interface().get_state();
320+
hw_lbr_state_ = async_client_ptr_->get_state_interface()->get_state();
300321

301322
if (period.seconds() - hw_lbr_state_.sample_time * 0.2 > hw_lbr_state_.sample_time) {
302323
RCLCPP_WARN_STREAM(rclcpp::get_logger(LOGGER_NAME),
@@ -313,7 +334,7 @@ hardware_interface::return_type SystemInterface::read(const rclcpp::Time & /*tim
313334
lbr_fri_ros2::ColorScheme::ERROR
314335
<< "LBR left COMMANDING_ACTIVE. Please re-run lbr_bringup"
315336
<< lbr_fri_ros2::ColorScheme::ENDC);
316-
app_ptr_->stop_run();
337+
app_ptr_->request_stop();
317338
app_ptr_->close_udp_socket();
318339
return hardware_interface::return_type::ERROR;
319340
}
@@ -345,49 +366,8 @@ hardware_interface::return_type SystemInterface::write(const rclcpp::Time & /*ti
345366
if (hw_session_state_ != KUKA::FRI::COMMANDING_ACTIVE) {
346367
return hardware_interface::return_type::OK;
347368
}
348-
#if FRICLIENT_VERSION_MAJOR == 1
349-
if (hw_client_command_mode_ == KUKA::FRI::EClientCommandMode::POSITION)
350-
#endif
351-
#if FRICLIENT_VERSION_MAJOR == 2
352-
if (hw_client_command_mode_ == KUKA::FRI::EClientCommandMode::JOINT_POSITION)
353-
#endif
354-
{
355-
if (std::any_of(hw_lbr_command_.joint_position.cbegin(),
356-
hw_lbr_command_.joint_position.cend(),
357-
[](const double &v) { return std::isnan(v); })) {
358-
return hardware_interface::return_type::OK;
359-
}
360-
async_client_ptr_->get_command_interface().set_command_target(hw_lbr_command_);
361-
return hardware_interface::return_type::OK;
362-
}
363-
if (hw_client_command_mode_ == KUKA::FRI::EClientCommandMode::TORQUE) {
364-
if (std::any_of(hw_lbr_command_.joint_position.cbegin(), hw_lbr_command_.joint_position.cend(),
365-
[](const double &v) { return std::isnan(v); }) ||
366-
std::any_of(hw_lbr_command_.torque.cbegin(), hw_lbr_command_.torque.cend(),
367-
[](const double &v) { return std::isnan(v); })) {
368-
return hardware_interface::return_type::OK;
369-
}
370-
async_client_ptr_->get_command_interface().set_command_target(hw_lbr_command_);
371-
return hardware_interface::return_type::OK;
372-
}
373-
if (hw_client_command_mode_ == KUKA::FRI::EClientCommandMode::WRENCH) {
374-
if (std::any_of(hw_lbr_command_.wrench.cbegin(), hw_lbr_command_.wrench.cend(),
375-
[](const double &v) { return std::isnan(v); }) ||
376-
std::any_of(hw_lbr_command_.joint_position.cbegin(), hw_lbr_command_.joint_position.cend(),
377-
[](const double &v) { return std::isnan(v); })) {
378-
return hardware_interface::return_type::OK;
379-
}
380-
async_client_ptr_->get_command_interface().set_command_target(hw_lbr_command_);
381-
return hardware_interface::return_type::OK;
382-
}
383-
#if FRICLIENT_VERSION_MAJOR == 2
384-
if (hw_client_command_mode_ == KUKA::FRI::EClientCommandMode::CARTESIAN_POSE) {
385-
throw std::runtime_error(
386-
lbr_fri_ros2::EnumMaps::client_command_mode_map(hw_client_command_mode_) +
387-
" command mode currently not implemented.");
388-
}
389-
#endif
390-
return hardware_interface::return_type::ERROR;
369+
async_client_ptr_->get_command_interface()->buffer_command_target(hw_lbr_command_);
370+
return hardware_interface::return_type::OK;
391371
}
392372

393373
void SystemInterface::nan_command_interfaces_() {

0 commit comments

Comments
 (0)