updated app

lbr-stack · Dec 7, 2023 · fa82185 · fa82185
1 parent 5075b7d
commit fa82185
Show file tree

Hide file tree

Showing 3 changed files with 82 additions and 34 deletions.
diff --git a/lbr_fri_ros2/CMakeLists.txt b/lbr_fri_ros2/CMakeLists.txt
@@ -46,7 +46,6 @@ ament_target_dependencies(lbr_fri_ros2
   lbr_fri_msgs
   rclcpp
   realtime_tools
-  urdf
 )
 
 target_link_libraries(lbr_fri_ros2
@@ -63,7 +62,6 @@ ament_export_dependencies(
   lbr_fri_msgs
   rclcpp
   realtime_tools
-  urdf
 )
 
 install(
@@ -82,36 +80,36 @@ install(
   DESTINATION share/lbr_fri_ros2
 )
 
-# # app_component
-# add_library(app_component
-#   SHARED
-#     src/app_component.cpp
-# )
-
-# target_include_directories(app_component
-#   PRIVATE src
-# )
-
-# ament_target_dependencies(app_component
-#   rclcpp
-#   urdf
-#   rclcpp_components
-# )
-
-# target_link_libraries(app_component
-#   lbr_fri_ros2
-# )
-
-# rclcpp_components_register_node(app_component
-#   PLUGIN lbr_fri_ros2::AppComponent
-#   EXECUTABLE app
-# )
-
-# install(
-#   TARGETS app_component
-#   LIBRARY DESTINATION lib
-#   RUNTIME DESTINATION lib/lbr_fri_ros2
-# )
+# app_component
+add_library(app_component
+  SHARED
+    src/app_component.cpp
+)
+
+target_include_directories(app_component
+  PRIVATE src
+)
+
+ament_target_dependencies(app_component
+  rclcpp
+  urdf
+  rclcpp_components
+)
+
+target_link_libraries(app_component
+  lbr_fri_ros2
+)
+
+rclcpp_components_register_node(app_component
+  PLUGIN lbr_fri_ros2::AppComponent
+  EXECUTABLE app
+)
+
+install(
+  TARGETS app_component
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/lbr_fri_ros2
+)
 
 install(
   DIRECTORY config launch

diff --git a/lbr_fri_ros2/src/app_component.cpp b/lbr_fri_ros2/src/app_component.cpp
@@ -7,9 +7,56 @@ AppComponent::AppComponent(const rclcpp::NodeOptions &options) {
   app_node_ptr_->declare_parameter("port_id", 30200);
   app_node_ptr_->declare_parameter("remote_host", std::string(""));
   app_node_ptr_->declare_parameter("rt_prio", 80);
+  app_node_ptr_->declare_parameter("robot_description", std::string(""));
+  app_node_ptr_->declare_parameter("command_guard_variant", std::string("safe_stop"));
+  app_node_ptr_->declare_parameter("external_torque_cutoff_frequency", 10.);
+  app_node_ptr_->declare_parameter("measured_torque_cutoff_frequency", 10.);
+  app_node_ptr_->declare_parameter("open_loop", true);
 
-  async_client_ptr_ = std::make_shared<AsyncClient>(app_node_ptr_);
-  app_ptr_ = std::make_unique<App>(app_node_ptr_, async_client_ptr_);
+  // prepare parameters
+  CommandGuardParameters command_guard_parameters;
+  std::string command_guard_variant =
+      app_node_ptr_->get_parameter("command_guard_variant").as_string();
+  StateInterfaceParameters state_interface_parameters;
+  state_interface_parameters.external_torque_cutoff_frequency =
+      app_node_ptr_->get_parameter("external_torque_cutoff_frequency").as_double();
+  state_interface_parameters.measured_torque_cutoff_frequency =
+      app_node_ptr_->get_parameter("measured_torque_cutoff_frequency").as_double();
+  bool open_loop = app_node_ptr_->get_parameter("open_loop").as_bool();
+
+  // load robot description and parse limits
+  auto robot_description_param = app_node_ptr_->get_parameter("robot_description");
+  urdf::Model model;
+  if (!model.initString(robot_description_param.as_string())) {
+    std::string err = "Failed to intialize urdf model from '" + robot_description_param.get_name() +
+                      "' parameter.";
+    RCLCPP_ERROR(app_node_ptr_->get_logger(), err.c_str());
+    throw std::runtime_error(err);
+  }
+
+  std::size_t jnt_cnt = 0;
+  for (const auto &name_joint_pair : model.joints_) {
+    const auto joint = name_joint_pair.second;
+    if (joint->type == urdf::Joint::REVOLUTE) {
+      if (jnt_cnt >= command_guard_parameters.joint_names.size()) {
+        std::string errs =
+            "Found too many joints in '" + robot_description_param.get_name() + "' parameter.";
+        RCLCPP_ERROR(app_node_ptr_->get_logger(), errs.c_str());
+        throw std::runtime_error(errs);
+      }
+      command_guard_parameters.joint_names[jnt_cnt] = name_joint_pair.first;
+      command_guard_parameters.min_position[jnt_cnt] = joint->limits->lower;
+      command_guard_parameters.max_position[jnt_cnt] = joint->limits->upper;
+      command_guard_parameters.max_velocity[jnt_cnt] = joint->limits->velocity;
+      command_guard_parameters.max_torque[jnt_cnt] = joint->limits->effort;
+      ++jnt_cnt;
+    }
+  }
+
+  // configure client
+  async_client_ptr_ = std::make_shared<AsyncClient>(command_guard_parameters, command_guard_variant,
+                                                    state_interface_parameters, open_loop);
+  app_ptr_ = std::make_unique<App>(async_client_ptr_);
 
   // default connect
   connect_(app_node_ptr_->get_parameter("port_id").as_int(),

diff --git a/lbr_fri_ros2/src/app_component.hpp b/lbr_fri_ros2/src/app_component.hpp
@@ -6,6 +6,7 @@
 #include <thread>
 
 #include "rclcpp/rclcpp.hpp"
+#include "urdf/model.h"
 
 #include "lbr_fri_msgs/msg/lbr_position_command.hpp"
 #include "lbr_fri_msgs/msg/lbr_state.hpp"
@@ -15,7 +16,9 @@
 #include "lbr_fri_msgs/srv/app_disconnect.hpp"
 #include "lbr_fri_ros2/app.hpp"
 #include "lbr_fri_ros2/async_client.hpp"
+#include "lbr_fri_ros2/command_guard.hpp"
 #include "lbr_fri_ros2/enum_maps.hpp"
+#include "lbr_fri_ros2/state_interface.hpp"
 
 namespace lbr_fri_ros2 {
 /**