Expliclty pass empty Twist message to realtime buffer.

velocity_controllers/src/integral_sliding_mode_controller.cpp

@@ -39,14 +39,17 @@ namespace velocity_controllers
 namespace
 {
 
-void reset_twist_msg(std::shared_ptr<geometry_msgs::msg::Twist> msg) // NOLINT
+std::shared_ptr<geometry_msgs::msg::Twist> empty_twist_msg() // NOLINT
 {
+ auto msg = std::make_shared<geometry_msgs::msg::Twist>();
  msg->linear.x = std::numeric_limits<double>::quiet_NaN();
  msg->linear.y = std::numeric_limits<double>::quiet_NaN();
  msg->linear.z = std::numeric_limits<double>::quiet_NaN();
  msg->angular.x = std::numeric_limits<double>::quiet_NaN();
  msg->angular.y = std::numeric_limits<double>::quiet_NaN();
  msg->angular.z = std::numeric_limits<double>::quiet_NaN();
+
+ return msg;
 }
 
 } // namespace
@@ -224,8 +227,8 @@ controller_interface::CallbackReturn IntegralSlidingModeController::on_activate(
 
  system_rotation_.writeFromNonRT(Eigen::Quaterniond::Identity());
 
- reset_twist_msg(*(reference_.readFromNonRT()));
- reset_twist_msg(*(system_state_.readFromNonRT()));
+ reference_.writeFromNonRT(empty_twist_msg());
+ system_state_.writeFromNonRT(empty_twist_msg());
 
  reference_interfaces_.assign(reference_interfaces_.size(), std::numeric_limits<double>::quiet_NaN());
  system_state_values_.assign(system_state_values_.size(), std::numeric_limits<double>::quiet_NaN());
@@ -244,41 +247,52 @@ bool IntegralSlidingModeController::on_set_chained_mode(bool /*chained_mode*/) {
 controller_interface::return_type IntegralSlidingModeController::update_reference_from_subscribers(
  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
 {
- // Explicitly create a new shared_ptr to ensure usage it incremented
- auto current_reference(*(reference_.readFromNonRT()));
+ // Explicitly create a new shared_ptr to ensure usage is incremented
+ auto current_reference = reference_.readFromNonRT();
+
+ // no command received yet
+ if (!current_reference || !(*current_reference)) {
+ return controller_interface::return_type::OK;
+ }
 
- const std::vector<double> reference = {current_reference->linear.x, current_reference->linear.y,
- current_reference->linear.z, current_reference->angular.x,
- current_reference->angular.y, current_reference->angular.z};
+ const std::vector<double> reference = {(*current_reference)->linear.x, (*current_reference)->linear.y,
+ (*current_reference)->linear.z, (*current_reference)->angular.x,
+ (*current_reference)->angular.y, (*current_reference)->angular.z};
 
  for (size_t i = 0; i < reference.size(); ++i) {
  if (!std::isnan(reference[i])) {
  reference_interfaces_[i] = reference[i];
  }
  }
 
- reset_twist_msg(current_reference);
+ reference_.writeFromNonRT(empty_twist_msg());
 
  return controller_interface::return_type::OK;
 }
 
 controller_interface::return_type IntegralSlidingModeController::update_system_state_values()
 {
  if (params_.use_external_measured_states) {
- // Explicitly create a new shared_ptr to ensure usage it incremented
- auto current_system_state(*(system_state_.readFromRT()));
+ // Explicitly create a new shared_ptr to ensure usage is incremented
+ auto current_system_state = system_state_.readFromRT();
 
- const std::vector<double> state = {current_system_state->linear.x, current_system_state->linear.y,
- current_system_state->linear.z, current_system_state->angular.x,
- current_system_state->angular.y, current_system_state->angular.z};
+ // no command received yet
+ if (!current_system_state || !(*current_system_state)) {
+ return controller_interface::return_type::OK;
+ }
+
+ const std::vector<double> state = {(*current_system_state)->linear.x, (*current_system_state)->linear.y,
+ (*current_system_state)->linear.z, (*current_system_state)->angular.x,
+ (*current_system_state)->angular.y, (*current_system_state)->angular.z};
 
  for (size_t i = 0; i < state.size(); ++i) {
  if (!std::isnan(state[i])) {
  system_state_values_[i] = state[i];
  }
  }
 
- reset_twist_msg(current_system_state);
+ system_state_.writeFromNonRT(empty_twist_msg());
+
  } else {
  for (size_t i = 0; i < system_state_values_.size(); ++i) {
  system_state_values_[i] = state_interfaces_[i].get_value();