Skip to content

Commit f9edc41

Browse files
christophfroehlichchristophfroehlich
authored
Fix RealtimeBox API changes (ros-controls#1385)
1 parent 36068e1 commit f9edc41

File tree

6 files changed

+35
-28
lines changed

6 files changed

+35
-28
lines changed

‎diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -131,6 +131,7 @@ class DiffDriveController : public controller_interface::ControllerInterface
131131
rclcpp::Subscription<TwistStamped>::SharedPtr velocity_command_subscriber_ = nullptr;
132132

133133
realtime_tools::RealtimeBox<std::shared_ptr<TwistStamped>> received_velocity_msg_ptr_{nullptr};
134+
std::shared_ptr<TwistStamped> last_command_msg_;
134135

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

‎diff_drive_controller/src/diff_drive_controller.cpp‎

Lines changed: 15 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -111,26 +111,27 @@ controller_interface::return_type DiffDriveController::update(
111111
return controller_interface::return_type::OK;
112112
}
113113

114-
std::shared_ptr<TwistStamped> last_command_msg;
115-
received_velocity_msg_ptr_.get(last_command_msg);
114+
// if the mutex is unable to lock, last_command_msg_ won't be updated
115+
received_velocity_msg_ptr_.try_get([this](const std::shared_ptr<TwistStamped> & msg)
116+
{ last_command_msg_ = msg; });
116117

117-
if (last_command_msg == nullptr)
118+
if (last_command_msg_ == nullptr)
118119
{
119120
RCLCPP_WARN(logger, "Velocity message received was a nullptr.");
120121
return controller_interface::return_type::ERROR;
121122
}
122123

123-
const auto age_of_last_command = time - last_command_msg->header.stamp;
124+
const auto age_of_last_command = time - last_command_msg_->header.stamp;
124125
// Brake if cmd_vel has timeout, override the stored command
125126
if (age_of_last_command > cmd_vel_timeout_)
126127
{
127-
last_command_msg->twist.linear.x = 0.0;
128-
last_command_msg->twist.angular.z = 0.0;
128+
last_command_msg_->twist.linear.x = 0.0;
129+
last_command_msg_->twist.angular.z = 0.0;
129130
}
130131

131132
// command may be limited further by SpeedLimit,
132133
// without affecting the stored twist command
133-
TwistStamped command = *last_command_msg;
134+
TwistStamped command = *last_command_msg_;
134135
double & linear_command = command.twist.linear.x;
135136
double & angular_command = command.twist.angular.z;
136137

@@ -325,12 +326,12 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(
325326
limited_velocity_publisher_);
326327
}
327328

328-
const TwistStamped empty_twist;
329-
received_velocity_msg_ptr_.set(std::make_shared<TwistStamped>(empty_twist));
330-
329+
last_command_msg_ = std::make_shared<TwistStamped>();
330+
received_velocity_msg_ptr_.set([this](std::shared_ptr<TwistStamped> & stored_value)
331+
{ stored_value = last_command_msg_; });
331332
// Fill last two commands with default constructed commands
332-
previous_commands_.emplace(empty_twist);
333-
previous_commands_.emplace(empty_twist);
333+
previous_commands_.emplace(*last_command_msg_);
334+
previous_commands_.emplace(*last_command_msg_);
334335

335336
// initialize command subscriber
336337
velocity_command_subscriber_ = get_node()->create_subscription<TwistStamped>(
@@ -350,7 +351,8 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(
350351
"time, this message will only be shown once");
351352
msg->header.stamp = get_node()->get_clock()->now();
352353
}
353-
received_velocity_msg_ptr_.set(std::move(msg));
354+
received_velocity_msg_ptr_.set([msg](std::shared_ptr<TwistStamped> & stored_value)
355+
{ stored_value = std::move(msg); });
354356
});
355357

356358
// initialize odometry publisher and message
@@ -476,7 +478,6 @@ controller_interface::CallbackReturn DiffDriveController::on_cleanup(
476478
return controller_interface::CallbackReturn::ERROR;
477479
}
478480

479-
received_velocity_msg_ptr_.set(std::make_shared<TwistStamped>());
480481
return controller_interface::CallbackReturn::SUCCESS;
481482
}
482483

‎diff_drive_controller/test/test_diff_drive_controller.cpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -49,7 +49,8 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr
4949
std::shared_ptr<geometry_msgs::msg::TwistStamped> getLastReceivedTwist()
5050
{
5151
std::shared_ptr<geometry_msgs::msg::TwistStamped> ret;
52-
received_velocity_msg_ptr_.get(ret);
52+
received_velocity_msg_ptr_.get(
53+
[&ret](const std::shared_ptr<geometry_msgs::msg::TwistStamped> & msg) { ret = msg; });
5354
return ret;
5455
}
5556

‎tricycle_controller/include/tricycle_controller/tricycle_controller.hpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -139,6 +139,7 @@ class TricycleController : public controller_interface::ControllerInterface
139139
rclcpp::Subscription<TwistStamped>::SharedPtr velocity_command_subscriber_ = nullptr;
140140

141141
realtime_tools::RealtimeBox<std::shared_ptr<TwistStamped>> received_velocity_msg_ptr_{nullptr};
142+
std::shared_ptr<TwistStamped> last_command_msg_;
142143

143144
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reset_odom_service_;
144145

‎tricycle_controller/src/tricycle_controller.cpp‎

Lines changed: 14 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -95,25 +95,27 @@ controller_interface::return_type TricycleController::update(
9595
}
9696
return controller_interface::return_type::OK;
9797
}
98-
std::shared_ptr<TwistStamped> last_command_msg;
99-
received_velocity_msg_ptr_.get(last_command_msg);
100-
if (last_command_msg == nullptr)
98+
99+
// if the mutex is unable to lock, last_command_msg_ won't be updated
100+
received_velocity_msg_ptr_.try_get([this](const std::shared_ptr<TwistStamped> & msg)
101+
{ last_command_msg_ = msg; });
102+
if (last_command_msg_ == nullptr)
101103
{
102104
RCLCPP_WARN(get_node()->get_logger(), "Velocity message received was a nullptr.");
103105
return controller_interface::return_type::ERROR;
104106
}
105107

106-
const auto age_of_last_command = time - last_command_msg->header.stamp;
108+
const auto age_of_last_command = time - last_command_msg_->header.stamp;
107109
// Brake if cmd_vel has timeout, override the stored command
108110
if (age_of_last_command > cmd_vel_timeout_)
109111
{
110-
last_command_msg->twist.linear.x = 0.0;
111-
last_command_msg->twist.angular.z = 0.0;
112+
last_command_msg_->twist.linear.x = 0.0;
113+
last_command_msg_->twist.angular.z = 0.0;
112114
}
113115

114116
// command may be limited further by Limiters,
115117
// without affecting the stored twist command
116-
TwistStamped command = *last_command_msg;
118+
TwistStamped command = *last_command_msg_;
117119
double & linear_command = command.twist.linear.x;
118120
double & angular_command = command.twist.angular.z;
119121
double Ws_read = traction_joint_[0].velocity_state.get().get_value(); // in radians/s
@@ -271,9 +273,9 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State &
271273
return CallbackReturn::ERROR;
272274
}
273275

274-
const TwistStamped empty_twist;
275-
received_velocity_msg_ptr_.set(std::make_shared<TwistStamped>(empty_twist));
276-
276+
last_command_msg_ = std::make_shared<TwistStamped>();
277+
received_velocity_msg_ptr_.set([this](std::shared_ptr<TwistStamped> & stored_value)
278+
{ stored_value = last_command_msg_; });
277279
// Fill last two commands with default constructed commands
278280
const AckermannDrive empty_ackermann_drive;
279281
previous_commands_.emplace(empty_ackermann_drive);
@@ -307,7 +309,8 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State &
307309
"time, this message will only be shown once");
308310
msg->header.stamp = get_node()->get_clock()->now();
309311
}
310-
received_velocity_msg_ptr_.set(std::move(msg));
312+
received_velocity_msg_ptr_.set([msg](std::shared_ptr<TwistStamped> & stored_value)
313+
{ stored_value = std::move(msg); });
311314
});
312315

313316
// initialize odometry publisher and message
@@ -397,7 +400,6 @@ CallbackReturn TricycleController::on_cleanup(const rclcpp_lifecycle::State &)
397400
return CallbackReturn::ERROR;
398401
}
399402

400-
received_velocity_msg_ptr_.set(std::make_shared<TwistStamped>());
401403
return CallbackReturn::SUCCESS;
402404
}
403405

‎tricycle_controller/test/test_tricycle_controller.cpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -54,7 +54,8 @@ class TestableTricycleController : public tricycle_controller::TricycleControlle
5454
std::shared_ptr<geometry_msgs::msg::TwistStamped> getLastReceivedTwist()
5555
{
5656
std::shared_ptr<geometry_msgs::msg::TwistStamped> ret;
57-
received_velocity_msg_ptr_.get(ret);
57+
received_velocity_msg_ptr_.get(
58+
[&ret](const std::shared_ptr<geometry_msgs::msg::TwistStamped> & msg) { ret = msg; });
5859
return ret;
5960
}
6061

0 commit comments

Comments
 (0)