Fix after kinematics_interface API break (v1.1.0 -> v1.2.0) (#46)

* use latest kinematics_interface version * temporary point to dynamics_interface version for kinematics_interface 1.2.0 * use new "kyn->initialize()" API * make CI format happier * use main branch of "dynamics_interface" pkg * use realtime_publisher.hpp instead of obsolete realtime_publisher.h * fix call to moveit_servo::Servo::getCurrentRobotState() * ignore deps during tests --------- Co-authored-by: Thibault Poignonec <[email protected]>
ICube-Robotics · Jan 18, 2025 · 815b394 · 815b394
1 parent 3a0867d
commit 815b394
Show file tree

Hide file tree

Showing 10 changed files with 33 additions and 12 deletions.
diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
@@ -51,7 +51,8 @@ jobs:
             rosdep install --ignore-src --from-paths . -y -r
             colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install
             source install/setup.bash
-            colcon test --event-handlers console_direct+
+            DEPS_PKG_NAMES=$(colcon list -n --base-paths src/external)
+            colcon test --event-handlers console_direct+ console_package_list+ --packages-skip $DEPS_PKG_NAMES
             colcon test-result
 
       - name: Upload Tests to Artifacts

diff --git a/cartesian_controllers_ros2.repos b/cartesian_controllers_ros2.repos
@@ -7,4 +7,4 @@ repositories:
     external/ros-controls/kinematics_interface:
         type: git
         url: https://github.com/ros-controls/kinematics_interface.git
-        version: 1.1.0
+        version: master
diff --git a/...ian_state_broadcaster/include/cartesian_state_broadcaster/cartesian_state_broadcaster.hpp b/...ian_state_broadcaster/include/cartesian_state_broadcaster/cartesian_state_broadcaster.hpp
@@ -21,7 +21,7 @@
 #include <string>
 #include <vector>
 
-#include "realtime_tools/realtime_publisher.h"
+#include "realtime_tools/realtime_publisher.hpp"
 #include "cartesian_state_broadcaster/visibility_control.h"
 
 #include "controller_interface/controller_interface.hpp"

diff --git a/cartesian_state_broadcaster/src/cartesian_state_broadcaster.cpp b/cartesian_state_broadcaster/src/cartesian_state_broadcaster.cpp
@@ -53,6 +53,16 @@ CallbackReturn CartesianStateBroadcaster::on_configure(
   auto kinematics_plugin_package =
     get_node()->get_parameter("kinematics_plugin_package").as_string();
 
+  // Load URDF
+  auto robot_param = rclcpp::Parameter();
+  if (!get_node()->get_parameter("robot_description", robot_param)) {
+    RCLCPP_ERROR(
+          rclcpp::get_logger("CartesianStateBroadcaster"),
+          "parameter robot_description not set");
+    return CallbackReturn::ERROR;
+  }
+  auto robot_description = robot_param.as_string();
+
   // Load the differential IK plugin
   if (!kinematics_plugin_name.empty()) {
     try {
@@ -62,8 +72,7 @@ CallbackReturn CartesianStateBroadcaster::on_configure(
       kinematics_ = std::unique_ptr<kinematics_interface::KinematicsInterface>(
         kinematics_loader_->createUnmanagedInstance(kinematics_plugin_name));
       if (!kinematics_->initialize(
-          get_node()->get_node_parameters_interface(),
-          end_effector_name_))
+        robot_description, get_node()->get_node_parameters_interface(), "kinematics"))
       {
         RCLCPP_ERROR(
           rclcpp::get_logger("CartesianStateBroadcaster"),

diff --git a/cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_controller.hpp b/cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_controller.hpp
@@ -38,7 +38,7 @@
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 #include "rclcpp_lifecycle/state.hpp"
 #include "realtime_tools/realtime_buffer.h"
-#include "realtime_tools/realtime_publisher.h"
+#include "realtime_tools/realtime_publisher.hpp"
 #include "semantic_components/force_torque_sensor.hpp"
 
 // Ros msgs

diff --git a/cartesian_vic_controller/src/cartesian_vic_rule.cpp b/cartesian_vic_controller/src/cartesian_vic_rule.cpp
@@ -61,6 +61,15 @@ CartesianVicRule::configure(
   num_joints_ = num_joints;
   // reset vic state
   reset(num_joints);
+
+  // Load URDF
+  auto robot_param = rclcpp::Parameter();
+  if (!parameters_interface->get_parameter("robot_description", robot_param)) {
+    RCLCPP_ERROR(logger_, "parameter robot_description not set");
+    return controller_interface::return_type::ERROR;
+  }
+  auto robot_description = robot_param.as_string();
+
   // Load the dynamics (also used for IK) plugin
   if (!parameters_.dynamics.plugin_name.empty()) {
     try {
@@ -70,7 +79,9 @@ CartesianVicRule::configure(
         "dynamics_interface::DynamicsInterface");
       dynamics_ = std::unique_ptr<dynamics_interface::DynamicsInterface>(
         dynamics_loader_->createUnmanagedInstance(parameters_.dynamics.plugin_name));
-      if (!dynamics_->initialize(parameters_interface, parameters_.dynamics.tip)) {
+      if (!dynamics_->initialize(
+        robot_description, parameters_interface, "dynamics"))
+      {
         return controller_interface::return_type::ERROR;
       }
     } catch (pluginlib::PluginlibException & ex) {

diff --git a/cartesian_vic_servo/include/cartesian_vic_servo/vic_servo_node.hpp b/cartesian_vic_servo/include/cartesian_vic_servo/vic_servo_node.hpp
@@ -23,7 +23,7 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "realtime_tools/realtime_buffer.h"
-#include "realtime_tools/realtime_publisher.h"
+#include "realtime_tools/realtime_publisher.hpp"
 
 #include "geometry_msgs/msg/twist_stamped.hpp"
 #include "geometry_msgs/msg/wrench_stamped.hpp"

diff --git a/cartesian_vic_servo/src/demo_twist.cpp b/cartesian_vic_servo/src/demo_twist.cpp
@@ -101,7 +101,7 @@ int main(int argc, char * argv[])
 
   // create command queue to build trajectory message and add current robot state
   std::deque<KinematicState> joint_cmd_rolling_window;
-  KinematicState current_state = servo.getCurrentRobotState();
+  KinematicState current_state = servo.getCurrentRobotState(false);
   updateSlidingWindow(current_state, joint_cmd_rolling_window, servo_params.max_expected_latency,
     demo_node->now());
 

diff --git a/cartesian_vic_servo/src/vic_servo_node.cpp b/cartesian_vic_servo/src/vic_servo_node.cpp
@@ -271,7 +271,7 @@ bool CartesianVicServo::start()
   }
 
   // Reset command queue
-  auto current_state = servo_->getCurrentRobotState();
+  auto current_state = servo_->getCurrentRobotState(false);
   moveit_servo::updateSlidingWindow(
     current_state, joint_cmd_rolling_window_, max_expected_latency_, this->now());
 
@@ -651,7 +651,7 @@ bool CartesianVicServo::send_twist_command(
   } else {
     // if joint_cmd_rolling_window_ is empty or all commands are outdated, use current robot state
     joint_cmd_rolling_window_.clear();
-    current_state = servo_->getCurrentRobotState();
+    current_state = servo_->getCurrentRobotState(false);
     current_state.velocities *= 0.0;
   }
 

diff --git a/...op_controller/include/cartesian_vic_teleop_controller/cartesian_vic_teleop_controller.hpp b/...op_controller/include/cartesian_vic_teleop_controller/cartesian_vic_teleop_controller.hpp
@@ -29,7 +29,7 @@
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 #include "rclcpp_lifecycle/state.hpp"
 #include "realtime_tools/realtime_buffer.h"
-#include "realtime_tools/realtime_publisher.h"
+#include "realtime_tools/realtime_publisher.hpp"
 
 // Base class for VIC controller
 #include "cartesian_vic_controller/cartesian_vic_controller.hpp"