Update main branch with ros-controls changes (#160)

* Add on_init in ur_controllers.

* Suppress warnings.

* Remove unneeded methods in controllers.

* Update formatting.

Co-authored-by: Denis Štogl <[email protected]>

Author: livanov93

ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp

@@ -23,12 +23,15 @@
 #ifndef UR_CONTROLLERS__SCALED_JOINT_TRAJECTORY_CONTROLLER_HPP_
 #define UR_CONTROLLERS__SCALED_JOINT_TRAJECTORY_CONTROLLER_HPP_
 
+#include "angles/angles.h"
 #include "joint_trajectory_controller/joint_trajectory_controller.hpp"
 #include "joint_trajectory_controller/trajectory.hpp"
-#include "angles/angles.h"
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 
 namespace ur_controllers
 {
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
 class ScaledJointTrajectoryController : public joint_trajectory_controller::JointTrajectoryController
 {
 public:
@@ -37,8 +40,7 @@ class ScaledJointTrajectoryController : public joint_trajectory_controller::Join
 
   controller_interface::InterfaceConfiguration state_interface_configuration() const override;
 
-  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
-  on_activate(const rclcpp_lifecycle::State& state) override;
+  CallbackReturn on_activate(const rclcpp_lifecycle::State& state) override;
 
   controller_interface::return_type update() override;
 

ur_controllers/include/ur_controllers/speed_scaling_state_broadcaster.hpp

@@ -35,6 +35,8 @@
 
 namespace ur_controllers
 {
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
 class SpeedScalingStateBroadcaster : public controller_interface::ControllerInterface
 {
 public:
@@ -46,14 +48,13 @@ class SpeedScalingStateBroadcaster : public controller_interface::ControllerInte
 
   controller_interface::return_type update() override;
 
-  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
-  on_configure(const rclcpp_lifecycle::State& previous_state) override;
+  CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
+
+  CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
 
-  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
-  on_activate(const rclcpp_lifecycle::State& previous_state) override;
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
 
-  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
-  on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
+  CallbackReturn on_init() override;
 
 protected:
   std::vector<std::string> sensor_names_;

ur_controllers/src/scaled_joint_trajectory_controller.cpp

@@ -36,8 +36,7 @@ controller_interface::InterfaceConfiguration ScaledJointTrajectoryController::st
   return conf;
 }
 
-rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
-ScaledJointTrajectoryController::on_activate(const rclcpp_lifecycle::State& state)
+CallbackReturn ScaledJointTrajectoryController::on_activate(const rclcpp_lifecycle::State& state)
 {
   TimeData time_data;
   time_data.time = node_->now();
@@ -137,7 +136,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update()
     rcl_duration_value_t period = (time_data.time - time_data_.readFromRT()->time).nanoseconds();
     time_data.period = rclcpp::Duration::from_nanoseconds(scaling_factor_ * period);
     time_data.uptime = time_data_.readFromRT()->uptime + time_data.period;
-    rclcpp::Time traj_time = time_data_.readFromRT()->uptime + rclcpp::Duration(period);
+    rclcpp::Time traj_time = time_data_.readFromRT()->uptime + rclcpp::Duration::from_nanoseconds(period);
     time_data_.writeFromNonRT(time_data);
 
     // if sampling the first time, set the point before you sample

ur_controllers/src/speed_scaling_state_broadcaster.cpp

@@ -44,6 +44,18 @@ SpeedScalingStateBroadcaster::SpeedScalingStateBroadcaster()
 {
 }
 
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn SpeedScalingStateBroadcaster::on_init()
+{
+  try {
+    auto_declare<double>("state_publish_rate", 100.0);
+  } catch (std::exception& e) {
+    fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
+    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
+  }
+
+  return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
 controller_interface::InterfaceConfiguration SpeedScalingStateBroadcaster::command_interface_configuration() const
 {
   return controller_interface::InterfaceConfiguration{ controller_interface::interface_configuration_type::NONE };
@@ -60,8 +72,6 @@ controller_interface::InterfaceConfiguration SpeedScalingStateBroadcaster::state
 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
 SpeedScalingStateBroadcaster::on_configure(const rclcpp_lifecycle::State& /*previous_state*/)
 {
-  auto_declare<double>("state_publish_rate", 100.0);
-
   if (!node_->get_parameter("state_publish_rate", publish_rate_)) {
     RCLCPP_INFO(get_node()->get_logger(), "Parameter 'state_publish_rate' not set");
     return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;