fix: controllers for rolling

Author: danielsanjosepro

src/cartesian_controller.cpp

@@ -65,7 +65,8 @@ CartesianController::update(const rclcpp::Time &time,
 
     /*q[i] = exponential_moving_average(q[i], state_interfaces_[i].get_value(),*/
     /*                                  params_.filter.q);*/
-    q[i] = state_interfaces_[i].get_value();
+    auto q_opt = state_interfaces_[i].get_optional();
+    q[i] = q_opt.value_or(0.0);
     if (continous_joint_types.count(
                    joint.shortname())) { // Then we are handling a continous
                                          // joint that is SO(2)
@@ -77,7 +78,8 @@ CartesianController::update(const rclcpp::Time &time,
     /*dq[i] = exponential_moving_average(*/
     /*    dq[i], state_interfaces_[num_joints + i].get_value(),*/
     /*    params_.filter.dq);*/
-    dq[i] = state_interfaces_[num_joints + i].get_value();
+    auto dq_opt = state_interfaces_[num_joints + i].get_optional();
+    dq[i] = dq_opt.value_or(0.0);
   }
 
   if (new_target_pose_) {parse_target_pose_(); new_target_pose_ = false;}
@@ -202,7 +204,9 @@ CartesianController::update(const rclcpp::Time &time,
 
   if (not params_.stop_commands) {
     for (size_t i = 0; i < num_joints; ++i) {
-      command_interfaces_[i].set_value(tau_d[i]);
+      if (!command_interfaces_[i].set_value(tau_d[i])) {
+        RCLCPP_WARN(get_node()->get_logger(), "Failed to set command value for joint %d", i);
+      }
     }
   }
 
@@ -456,7 +460,9 @@ CallbackReturn CartesianController::on_activate(
         model_.getJointId(joint_name); // pinocchio joind id might be different
     auto joint = model_.joints[joint_id];
 
-    q[i] = state_interfaces_[i].get_value();
+    auto q_opt = state_interfaces_[i].get_optional();
+    auto dq_opt = state_interfaces_[num_joints + i].get_optional();
+    q[i] = q_opt.value_or(0.0);
     if (joint.shortname() == "JointModelRZ") { // simple revolute joint case
       q_pin[joint.idx_q()] = q[i];
     } else if (continous_joint_types.count(
@@ -466,10 +472,10 @@ CallbackReturn CartesianController::on_activate(
       q_pin[joint.idx_q() + 1] = std::sin(q[i]);
     }
 
-    q_ref[i] = state_interfaces_[i].get_value();
+    q_ref[i] = q_opt.value_or(0.0);
 
-    dq[i] = state_interfaces_[num_joints + i].get_value();
-    dq_ref[i] = state_interfaces_[num_joints + i].get_value();
+    dq[i] = dq_opt.value_or(0.0);
+    dq_ref[i] = dq_opt.value_or(0.0);
   }
 
   pinocchio::forwardKinematics(model_, data_, q_pin, dq);

src/pose_broadcaster.cpp

@@ -46,7 +46,8 @@ PoseBroadcaster::update(const rclcpp::Time &time,
     auto joint_id = model_.getJointId(joint_name);
     auto joint = model_.joints[joint_id];
 
-    q[i] = state_interfaces_[i].get_value();
+    auto q_opt = state_interfaces_[i].get_optional();
+    q[i] = q_opt.value_or(0.0);
     if (continous_joint_types.count(joint.shortname())) {  // Then we are handling a continous joint that is SO(2)
       q_pin[joint.idx_q()] = std::cos(q[i]);
       q_pin[joint.idx_q()+1] = std::sin(q[i]);
@@ -70,10 +71,9 @@ PoseBroadcaster::update(const rclcpp::Time &time,
     should_publish = time_since_last >= min_interval;
   }
 
-  if (should_publish && realtime_pose_publisher_ && realtime_pose_publisher_->trylock())
+  if (should_publish && realtime_pose_publisher_)
   {
-    auto & pose_msg = realtime_pose_publisher_->msg_;
-
+    geometry_msgs::msg::PoseStamped pose_msg;
     pose_msg.header.stamp = time;
     pose_msg.header.frame_id = params_.base_frame;
     pose_msg.pose.position.x = current_pose.translation()[0];
@@ -83,8 +83,10 @@ PoseBroadcaster::update(const rclcpp::Time &time,
     pose_msg.pose.orientation.y = current_quaternion.y();
     pose_msg.pose.orientation.z = current_quaternion.z();
     pose_msg.pose.orientation.w = current_quaternion.w();
-    realtime_pose_publisher_->unlockAndPublish();
-    last_publish_time_ = time;
+    
+    if (realtime_pose_publisher_->try_publish(pose_msg)) {
+      last_publish_time_ = time;
+    }
   }
 
   return controller_interface::return_type::OK;

src/torque_feedback_controller.cpp

@@ -48,8 +48,10 @@ TorqueFeedbackController::update(const rclcpp::Time & /*time*/,
                                  const rclcpp::Duration & /*period*/) {
   // Update joint states
   for (int i = 0; i < num_joints_; i++) {
-    q_[i] = state_interfaces_[i].get_value();
-    dq_[i] = state_interfaces_[num_joints_ + i].get_value();
+    auto q_opt = state_interfaces_[i].get_optional();
+    auto dq_opt = state_interfaces_[num_joints_ + i].get_optional();
+    q_[i] = q_opt.value_or(0.0);
+    dq_[i] = dq_opt.value_or(0.0);
   }
 
   // Compute forward kinematics and jacobian
@@ -109,7 +111,9 @@ TorqueFeedbackController::update(const rclcpp::Time & /*time*/,
   tau_commanded_ = tau_d + tau_f + tau_nullspace;
 
   for (int i = 0; i < num_joints_; i++) {
-    command_interfaces_[i].set_value(tau_commanded_[i]);
+    if (!command_interfaces_[i].set_value(tau_commanded_[i])) {
+      RCLCPP_WARN(get_node()->get_logger(), "Failed to set command value for joint %d", i);
+    }
   }
 
   params_listener_->refresh_dynamic_parameters();
@@ -248,8 +252,10 @@ CallbackReturn TorqueFeedbackController::on_configure(
 CallbackReturn TorqueFeedbackController::on_activate(
     const rclcpp_lifecycle::State & /*previous_state*/) {
   for (int i = 0; i < num_joints_; i++) {
-    q_[i] = state_interfaces_[i].get_value();
-    dq_[i] = state_interfaces_[num_joints_ + i].get_value();
+    auto q_opt = state_interfaces_[i].get_optional();
+    auto dq_opt = state_interfaces_[num_joints_ + i].get_optional();
+    q_[i] = q_opt.value_or(0.0);
+    dq_[i] = dq_opt.value_or(0.0);
     tau_ext_[i] = 0.0;
     q_init_[i] = q_[i];
   }

src/twist_broadcaster.cpp

@@ -48,8 +48,10 @@ TwistBroadcaster::update(const rclcpp::Time &time,
         auto joint_id = model_.getJointId(joint_name);
         auto joint = model_.joints[joint_id];
 
-        q[i] = state_interfaces_[i*2].get_value();
-        q_dot[i] = state_interfaces_[i*2+1].get_value();
+        auto q_opt = state_interfaces_[i*2].get_optional();
+        auto q_dot_opt = state_interfaces_[i*2+1].get_optional();
+        q[i] = q_opt.value_or(0.0);
+        q_dot[i] = q_dot_opt.value_or(0.0);
 
         if (continous_joint_types.count(joint.shortname())) {  // Then we are handling a continous joint that is SO(2)
             q_pin[joint.idx_q()] = std::cos(q[i]);
@@ -74,10 +76,9 @@ TwistBroadcaster::update(const rclcpp::Time &time,
         should_publish = time_since_last >= min_interval;
     }
 
-    if (should_publish && realtime_twist_publisher_ && realtime_twist_publisher_->trylock())
+    if (should_publish && realtime_twist_publisher_)
     {
-        auto & twist_msg = realtime_twist_publisher_->msg_;
-
+        geometry_msgs::msg::TwistStamped twist_msg;
         twist_msg.header.stamp = time;
         twist_msg.header.frame_id = params_.end_effector_frame;
         twist_msg.twist.linear.x = current_velocity.linear()[0];
@@ -86,8 +87,10 @@ TwistBroadcaster::update(const rclcpp::Time &time,
         twist_msg.twist.angular.x = current_velocity.angular()[0];
         twist_msg.twist.angular.y = current_velocity.angular()[1];
         twist_msg.twist.angular.z = current_velocity.angular()[2];
-        realtime_twist_publisher_->unlockAndPublish();
-        last_publish_time_ = time;
+        
+        if (realtime_twist_publisher_->try_publish(twist_msg)) {
+            last_publish_time_ = time;
+        }
     }
 
     return controller_interface::return_type::OK;