Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix RealtimeBox API changes #1385

Merged
merged 8 commits into from
Dec 4, 2024
Merged

Fix RealtimeBox API changes #1385

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
07ddb35
Fix diff_drive_controller
christophfroehlich Nov 23, 2024
90a3dc7
Rename Twist->TwistStamped
christophfroehlich Nov 23, 2024
96462c1
Fix tricycle_controller
christophfroehlich Nov 23, 2024
b584ec7
Merge branch 'master' into fix/realtimebox
christophfroehlich Nov 28, 2024
2edbe33
less code is better
bmagyar Dec 2, 2024
cc5beec
Merge branch 'master' into fix/realtimebox
bmagyar Dec 2, 2024
0ad818a
Store the old message in case of a locked realtime box
christophfroehlich Dec 3, 2024
67e1093
Initialize last_command_msg_
christophfroehlich Dec 3, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 7 additions & 7 deletions diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,7 @@ namespace diff_drive_controller
{
class DiffDriveController : public controller_interface::ControllerInterface
{
using Twist = geometry_msgs::msg::TwistStamped;
using TwistStamped = geometry_msgs::msg::TwistStamped;

public:
DIFF_DRIVE_CONTROLLER_PUBLIC
Expand Down Expand Up @@ -128,20 +128,20 @@ class DiffDriveController : public controller_interface::ControllerInterface
realtime_odometry_transform_publisher_ = nullptr;

bool subscriber_is_active_ = false;
rclcpp::Subscription<Twist>::SharedPtr velocity_command_subscriber_ = nullptr;
rclcpp::Subscription<TwistStamped>::SharedPtr velocity_command_subscriber_ = nullptr;

realtime_tools::RealtimeBox<std::shared_ptr<Twist>> received_velocity_msg_ptr_{nullptr};
realtime_tools::RealtimeBox<std::shared_ptr<TwistStamped>> received_velocity_msg_ptr_{nullptr};

std::queue<Twist> previous_commands_; // last two commands
std::queue<TwistStamped> previous_commands_; // last two commands

// speed limiters
SpeedLimiter limiter_linear_;
SpeedLimiter limiter_angular_;

bool publish_limited_velocity_ = false;
std::shared_ptr<rclcpp::Publisher<Twist>> limited_velocity_publisher_ = nullptr;
std::shared_ptr<realtime_tools::RealtimePublisher<Twist>> realtime_limited_velocity_publisher_ =
nullptr;
std::shared_ptr<rclcpp::Publisher<TwistStamped>> limited_velocity_publisher_ = nullptr;
std::shared_ptr<realtime_tools::RealtimePublisher<TwistStamped>>
realtime_limited_velocity_publisher_ = nullptr;

rclcpp::Time previous_update_timestamp_{0};

Expand Down
33 changes: 18 additions & 15 deletions diff_drive_controller/src/diff_drive_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -111,8 +111,9 @@ controller_interface::return_type DiffDriveController::update(
return controller_interface::return_type::OK;
}

std::shared_ptr<Twist> last_command_msg;
received_velocity_msg_ptr_.get(last_command_msg);
std::shared_ptr<TwistStamped> last_command_msg;
received_velocity_msg_ptr_.try_get([&last_command_msg](const std::shared_ptr<TwistStamped> & msg)
{ last_command_msg = msg; });
christophfroehlich marked this conversation as resolved.
Show resolved Hide resolved

if (last_command_msg == nullptr)
{
Expand All @@ -130,7 +131,7 @@ controller_interface::return_type DiffDriveController::update(

// command may be limited further by SpeedLimit,
// without affecting the stored twist command
Twist command = *last_command_msg;
TwistStamped command = *last_command_msg;
double & linear_command = command.twist.linear.x;
double & angular_command = command.twist.angular.z;

Expand Down Expand Up @@ -318,23 +319,24 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(

if (publish_limited_velocity_)
{
limited_velocity_publisher_ =
get_node()->create_publisher<Twist>(DEFAULT_COMMAND_OUT_TOPIC, rclcpp::SystemDefaultsQoS());
limited_velocity_publisher_ = get_node()->create_publisher<TwistStamped>(
DEFAULT_COMMAND_OUT_TOPIC, rclcpp::SystemDefaultsQoS());
realtime_limited_velocity_publisher_ =
std::make_shared<realtime_tools::RealtimePublisher<Twist>>(limited_velocity_publisher_);
std::make_shared<realtime_tools::RealtimePublisher<TwistStamped>>(
limited_velocity_publisher_);
}

const Twist empty_twist;
received_velocity_msg_ptr_.set(std::make_shared<Twist>(empty_twist));

const TwistStamped empty_twist;
christophfroehlich marked this conversation as resolved.
Show resolved Hide resolved
received_velocity_msg_ptr_.set([empty_twist](std::shared_ptr<TwistStamped> & stored_value)
{ stored_value = std::make_shared<TwistStamped>(empty_twist); });
christophfroehlich marked this conversation as resolved.
Show resolved Hide resolved
// Fill last two commands with default constructed commands
previous_commands_.emplace(empty_twist);
previous_commands_.emplace(empty_twist);

// initialize command subscriber
velocity_command_subscriber_ = get_node()->create_subscription<Twist>(
velocity_command_subscriber_ = get_node()->create_subscription<TwistStamped>(
DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(),
[this](const std::shared_ptr<Twist> msg) -> void
[this](const std::shared_ptr<TwistStamped> msg) -> void
{
if (!subscriber_is_active_)
{
Expand All @@ -349,7 +351,8 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(
"time, this message will only be shown once");
msg->header.stamp = get_node()->get_clock()->now();
}
received_velocity_msg_ptr_.set(std::move(msg));
received_velocity_msg_ptr_.set([msg](std::shared_ptr<TwistStamped> & stored_value)
{ stored_value = std::move(msg); });
});

// initialize odometry publisher and message
Expand Down Expand Up @@ -475,7 +478,6 @@ controller_interface::CallbackReturn DiffDriveController::on_cleanup(
return controller_interface::CallbackReturn::ERROR;
}

received_velocity_msg_ptr_.set(std::make_shared<Twist>());
return controller_interface::CallbackReturn::SUCCESS;
}

Expand All @@ -493,7 +495,7 @@ bool DiffDriveController::reset()
odometry_.resetOdometry();

// release the old queue
std::queue<Twist> empty;
std::queue<TwistStamped> empty;
std::swap(previous_commands_, empty);

registered_left_wheel_handles_.clear();
Expand All @@ -502,7 +504,8 @@ bool DiffDriveController::reset()
subscriber_is_active_ = false;
velocity_command_subscriber_.reset();

received_velocity_msg_ptr_.set(nullptr);
received_velocity_msg_ptr_.set([](std::shared_ptr<TwistStamped> & stored_value)
{ stored_value = nullptr; });
bmagyar marked this conversation as resolved.
Show resolved Hide resolved
is_halted = false;
return true;
}
Expand Down
3 changes: 2 additions & 1 deletion diff_drive_controller/test/test_diff_drive_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,8 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr
std::shared_ptr<geometry_msgs::msg::TwistStamped> getLastReceivedTwist()
{
std::shared_ptr<geometry_msgs::msg::TwistStamped> ret;
received_velocity_msg_ptr_.get(ret);
received_velocity_msg_ptr_.get(
[&ret](const std::shared_ptr<geometry_msgs::msg::TwistStamped> & msg) { ret = msg; });
return ret;
}

Expand Down
14 changes: 8 additions & 6 deletions tricycle_controller/src/tricycle_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -96,7 +96,8 @@ controller_interface::return_type TricycleController::update(
return controller_interface::return_type::OK;
}
std::shared_ptr<TwistStamped> last_command_msg;
received_velocity_msg_ptr_.get(last_command_msg);
received_velocity_msg_ptr_.try_get([&last_command_msg](const std::shared_ptr<TwistStamped> & msg)
{ last_command_msg = msg; });
if (last_command_msg == nullptr)
{
RCLCPP_WARN(get_node()->get_logger(), "Velocity message received was a nullptr.");
Expand Down Expand Up @@ -272,8 +273,8 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State &
}

const TwistStamped empty_twist;
received_velocity_msg_ptr_.set(std::make_shared<TwistStamped>(empty_twist));

received_velocity_msg_ptr_.set([empty_twist](std::shared_ptr<TwistStamped> & stored_value)
{ stored_value = std::make_shared<TwistStamped>(empty_twist); });
// Fill last two commands with default constructed commands
const AckermannDrive empty_ackermann_drive;
previous_commands_.emplace(empty_ackermann_drive);
Expand Down Expand Up @@ -307,7 +308,8 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State &
"time, this message will only be shown once");
msg->header.stamp = get_node()->get_clock()->now();
}
received_velocity_msg_ptr_.set(std::move(msg));
received_velocity_msg_ptr_.set([msg](std::shared_ptr<TwistStamped> & stored_value)
{ stored_value = std::move(msg); });
});

// initialize odometry publisher and message
Expand Down Expand Up @@ -397,7 +399,6 @@ CallbackReturn TricycleController::on_cleanup(const rclcpp_lifecycle::State &)
return CallbackReturn::ERROR;
}

received_velocity_msg_ptr_.set(std::make_shared<TwistStamped>());
return CallbackReturn::SUCCESS;
}

Expand Down Expand Up @@ -433,7 +434,8 @@ bool TricycleController::reset()
subscriber_is_active_ = false;
velocity_command_subscriber_.reset();

received_velocity_msg_ptr_.set(nullptr);
received_velocity_msg_ptr_.set([](std::shared_ptr<TwistStamped> & stored_value)
{ stored_value = nullptr; });
bmagyar marked this conversation as resolved.
Show resolved Hide resolved
is_halted = false;
return true;
}
Expand Down
3 changes: 2 additions & 1 deletion tricycle_controller/test/test_tricycle_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,8 @@ class TestableTricycleController : public tricycle_controller::TricycleControlle
std::shared_ptr<geometry_msgs::msg::TwistStamped> getLastReceivedTwist()
{
std::shared_ptr<geometry_msgs::msg::TwistStamped> ret;
received_velocity_msg_ptr_.get(ret);
received_velocity_msg_ptr_.get(
[&ret](const std::shared_ptr<geometry_msgs::msg::TwistStamped> & msg) { ret = msg; });
return ret;
}

Expand Down
Loading