Update controller compatibility to latest ros2_controllers (#71)

* update Black version to pass CI

Co-authored-by: JafarAbdi <[email protected]>

Author: MarqRazz

fault_controller/kortex2_controllers/CMakeLists.txt

@@ -12,11 +12,13 @@ find_package(controller_interface REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(example_interfaces REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(realtime_tools REQUIRED)
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
   controller_interface
   geometry_msgs
   example_interfaces
+  realtime_tools
 )
 
 include_directories(include)

fault_controller/kortex2_controllers/package.xml

@@ -13,6 +13,7 @@
   <depend>geometry_msgs</depend>
   <depend>example_interfaces</depend>
   <depend>rclcpp</depend>
+  <depend>realtime_tools</depend>
 
   <export>
     <build_type>ament_cmake</build_type>

fault_controller/kortex2_controllers/src/fault_controller.cpp

@@ -82,7 +82,7 @@ CallbackReturn FaultController::on_activate(const rclcpp_lifecycle::State & /*pr
   command_interfaces_[CommandInterfaces::RESET_FAULT_ASYNC_SUCCESS].set_value(NO_CMD);
   try
   {
-    fault_pub_ = node_->create_publisher<FbkType>("~/internal_fault", 1);
+    fault_pub_ = get_node()->create_publisher<FbkType>("~/internal_fault", 1);
     realtime_publisher_ = std::make_unique<StatePublisher>(fault_pub_);
   }
   catch (const std::exception & e)
@@ -92,7 +92,7 @@ CallbackReturn FaultController::on_activate(const rclcpp_lifecycle::State & /*pr
       e.what());
     return CallbackReturn::ERROR;
   }
-  trigger_command_srv_ = node_->create_service<example_interfaces::srv::Trigger>(
+  trigger_command_srv_ = get_node()->create_service<example_interfaces::srv::Trigger>(
     "~/reset_fault",
     std::bind(&FaultController::resetFault, this, std::placeholders::_1, std::placeholders::_2));
 
@@ -114,7 +114,7 @@ bool FaultController::resetFault(
   command_interfaces_[CommandInterfaces::RESET_FAULT_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
   command_interfaces_[CommandInterfaces::RESET_FAULT_CMD].set_value(ISSUE_CMD);
 
-  RCLCPP_INFO(node_->get_logger(), "Trying to reset faults on kinova controller.");
+  RCLCPP_INFO(get_node()->get_logger(), "Trying to reset faults on kinova controller.");
 
   while (command_interfaces_[CommandInterfaces::RESET_FAULT_ASYNC_SUCCESS].get_value() ==
          ASYNC_WAITING)
@@ -126,7 +126,7 @@ bool FaultController::resetFault(
     command_interfaces_[CommandInterfaces::RESET_FAULT_ASYNC_SUCCESS].get_value());
 
   RCLCPP_INFO(
-    node_->get_logger(), "Resetting fault on kinova controller '%s'!",
+    get_node()->get_logger(), "Resetting fault on kinova controller '%s'!",
     resp->success ? "succeeded" : "failed");
 
   return resp->success;

fault_controller/kortex2_controllers/src/twist_controller.cpp

@@ -80,7 +80,7 @@ CallbackReturn TwistController::on_init()
 
 CallbackReturn TwistController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/)
 {
-  joint_name_ = node_->get_parameter("joint").as_string();
+  joint_name_ = get_node()->get_parameter("joint").as_string();
 
   if (joint_name_.empty())
   {
@@ -91,7 +91,7 @@ CallbackReturn TwistController::on_configure(const rclcpp_lifecycle::State & /*p
   // Specialized, child controllers set interfaces before calling configure function.
   if (interface_names_.empty())
   {
-    interface_names_ = node_->get_parameter("interface_names").as_string_array();
+    interface_names_ = get_node()->get_parameter("interface_names").as_string_array();
   }
 
   if (interface_names_.empty())
@@ -136,7 +136,7 @@ controller_interface::return_type TwistController::update(
   if (command_interfaces_.size() != 6)
   {
     RCLCPP_ERROR_THROTTLE(
-      get_node()->get_logger(), *node_->get_clock(), 1000,
+      get_node()->get_logger(), *get_node()->get_clock(), 1000,
       "Twist controller needs does not match number of interfaces needed 6, given (%zu) interfaces",
       command_interfaces_.size());
     return controller_interface::return_type::ERROR;