Skip to content

Commit

Permalink
Remove reference_callback_unstamped
Browse files Browse the repository at this point in the history
  • Loading branch information
christophfroehlich committed May 26, 2024
1 parent 0934b60 commit c254f91
Show file tree
Hide file tree
Showing 2 changed files with 0 additions and 37 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -142,7 +142,6 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl
// callback for topic interface
STEERING_CONTROLLERS__VISIBILITY_LOCAL void reference_callback(
const std::shared_ptr<ControllerTwistReferenceMsg> msg);
void reference_callback_unstamped(const std::shared_ptr<geometry_msgs::msg::Twist> msg);
};

} // namespace steering_controllers_library
Expand Down
36 changes: 0 additions & 36 deletions steering_controllers_library/src/steering_controllers_library.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -234,42 +234,6 @@ void SteeringControllersLibrary::reference_callback(
}
}

void SteeringControllersLibrary::reference_callback_unstamped(
const std::shared_ptr<geometry_msgs::msg::Twist> msg)
{
RCLCPP_WARN(
get_node()->get_logger(),
"Use of Twist message without stamped is deprecated and it will be removed in ROS 2 J-Turtle "
"version. Use '~/reference' topic with 'geometry_msgs::msg::TwistStamped' message type in the "
"future.");
auto twist_stamped = *(input_ref_.readFromNonRT());
twist_stamped->header.stamp = get_node()->now();
// if no timestamp provided use current time for command timestamp
if (twist_stamped->header.stamp.sec == 0 && twist_stamped->header.stamp.nanosec == 0u)
{
RCLCPP_WARN(
get_node()->get_logger(),
"Timestamp in header is missing, using current time as command timestamp.");
twist_stamped->header.stamp = get_node()->now();
}

const auto age_of_last_command = get_node()->now() - twist_stamped->header.stamp;

if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_)
{
twist_stamped->twist = *msg;
}
else
{
RCLCPP_ERROR(
get_node()->get_logger(),
"Received message has timestamp %.10f older for %.10f which is more then allowed timeout "
"(%.4f).",
rclcpp::Time(twist_stamped->header.stamp).seconds(), age_of_last_command.seconds(),
ref_timeout_.seconds());
}
}

controller_interface::InterfaceConfiguration
SteeringControllersLibrary::command_interface_configuration() const
{
Expand Down

0 comments on commit c254f91

Please sign in to comment.