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 4 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: 8 additions & 6 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 @@ -112,7 +112,8 @@ controller_interface::return_type DiffDriveController::update(
}

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; });
christophfroehlich marked this conversation as resolved.
Show resolved Hide resolved

if (last_command_msg == nullptr)
{
Expand Down Expand Up @@ -326,8 +327,8 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(
}

const TwistStamped empty_twist;
christophfroehlich marked this conversation as resolved.
Show resolved Hide resolved
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); });
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);
Expand All @@ -350,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 @@ -476,7 +478,6 @@ controller_interface::CallbackReturn DiffDriveController::on_cleanup(
return controller_interface::CallbackReturn::ERROR;
}

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

Expand All @@ -503,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