Skip to content

Commit d01dd0f

Browse files
wittenatorwittenator
committed
Merge branch 'master' of github.com:wittenator/ros2_controllers
1 parent f150a9b commit d01dd0f

9 files changed

+80
-93
lines changed

‎steering_controllers_library/CMakeLists.txt‎

Lines changed: 4 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -21,7 +21,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
2121
tf2
2222
tf2_msgs
2323
tf2_geometry_msgs
24-
ackermann_msgs
2524
)
2625

2726
find_package(ament_cmake REQUIRED)
@@ -56,7 +55,6 @@ target_link_libraries(steering_controllers_library PUBLIC
5655
realtime_tools::realtime_tools
5756
tf2::tf2
5857
tf2_geometry_msgs::tf2_geometry_msgs
59-
${ackermann_msgs_TARGETS}
6058
${tf2_msgs_TARGETS}
6159
${geometry_msgs_TARGETS}
6260
${control_msgs_TARGETS}
@@ -75,10 +73,10 @@ if(BUILD_TESTING)
7573
target_link_libraries(test_steering_controllers_library steering_controllers_library)
7674

7775
add_rostest_with_parameters_gmock(
78-
test_steering_controllers_library_ackermann_input test/test_steering_controllers_library_ackermann_input.cpp
79-
${CMAKE_CURRENT_SOURCE_DIR}/test/steering_controllers_library_ackermann_params.yaml)
80-
target_include_directories(test_steering_controllers_library_ackermann_input PRIVATE include)
81-
target_link_libraries(test_steering_controllers_library_ackermann_input steering_controllers_library)
76+
test_steering_controllers_library_steering_input test/test_steering_controllers_library_steering_input.cpp
77+
${CMAKE_CURRENT_SOURCE_DIR}/test/steering_controllers_library_steering_input_params.yaml)
78+
target_include_directories(test_steering_controllers_library_steering_input PRIVATE include)
79+
target_link_libraries(test_steering_controllers_library_steering_input steering_controllers_library)
8280

8381
ament_add_gmock(test_steering_odometry test/test_steering_odometry.cpp)
8482
target_link_libraries(test_steering_odometry steering_controllers_library)

‎steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -28,7 +28,7 @@
2828
#include "realtime_tools/realtime_publisher.hpp"
2929

3030
// TODO(anyone): Replace with controller specific messages
31-
#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp"
31+
#include "control_msgs/msg/steering_controller_command.hpp"
3232
#include "control_msgs/msg/steering_controller_status.hpp"
3333
#include "geometry_msgs/msg/twist_stamped.hpp"
3434
#include "nav_msgs/msg/odometry.hpp"
@@ -72,7 +72,7 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl
7272
const rclcpp::Time & time, const rclcpp::Duration & period) override;
7373

7474
using ControllerTwistReferenceMsg = geometry_msgs::msg::TwistStamped;
75-
using ControllerAckermannReferenceMsg = ackermann_msgs::msg::AckermannDriveStamped;
75+
using ControllerSteeringReferenceMsg = control_msgs::msg::SteeringControllerCommand;
7676
using ControllerStateMsgOdom = nav_msgs::msg::Odometry;
7777
using ControllerStateMsgTf = tf2_msgs::msg::TFMessage;
7878
using SteeringControllerStateMsg = control_msgs::msg::SteeringControllerStatus;
@@ -86,11 +86,11 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl
8686

8787
// Command subscribers and Controller State publisher
8888
rclcpp::Subscription<ControllerTwistReferenceMsg>::SharedPtr ref_subscriber_twist_ = nullptr;
89-
rclcpp::Subscription<ControllerAckermannReferenceMsg>::SharedPtr ref_subscriber_ackermann_ =
89+
rclcpp::Subscription<ControllerSteeringReferenceMsg>::SharedPtr ref_subscriber_steering_ =
9090
nullptr;
9191
realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerTwistReferenceMsg>> input_ref_twist_;
92-
realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerAckermannReferenceMsg>>
93-
input_ref_ackermann_;
92+
realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerSteeringReferenceMsg>>
93+
input_ref_steering_;
9494
rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // 0ms
9595

9696
using ControllerStatePublisherOdom = realtime_tools::RealtimePublisher<ControllerStateMsgOdom>;
@@ -133,7 +133,7 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl
133133
private:
134134
// callback for topic interface
135135
void reference_callback_twist(const std::shared_ptr<ControllerTwistReferenceMsg> msg);
136-
void reference_callback_ackermann(const std::shared_ptr<ControllerAckermannReferenceMsg> msg);
136+
void reference_callback_steering(const std::shared_ptr<ControllerSteeringReferenceMsg> msg);
137137
};
138138

139139
} // namespace steering_controllers_library

‎steering_controllers_library/package.xml‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -40,7 +40,6 @@
4040
<depend>tf2</depend>
4141
<depend>tf2_msgs</depend>
4242
<depend>tf2_geometry_msgs</depend>
43-
<depend>ackermann_msgs</depend>
4443

4544
<test_depend>ament_cmake_gmock</test_depend>
4645
<test_depend>controller_manager</test_depend>

‎steering_controllers_library/src/steering_controllers_library.cpp‎

Lines changed: 25 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -28,8 +28,8 @@ namespace
2828

2929
using ControllerTwistReferenceMsg =
3030
steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg;
31-
using ControllerAckermannReferenceMsg =
32-
steering_controllers_library::SteeringControllersLibrary::ControllerAckermannReferenceMsg;
31+
using ControllerSteeringReferenceMsg =
32+
steering_controllers_library::SteeringControllersLibrary::ControllerSteeringReferenceMsg;
3333

3434
// called from RT control loop
3535
void reset_controller_reference_msg(
@@ -46,15 +46,12 @@ void reset_controller_reference_msg(
4646
}
4747

4848
void reset_controller_reference_msg(
49-
const std::shared_ptr<ControllerAckermannReferenceMsg> & msg,
49+
const std::shared_ptr<ControllerSteeringReferenceMsg> & msg,
5050
const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node)
5151
{
5252
msg->header.stamp = node->now();
53-
msg->drive.speed = std::numeric_limits<double>::quiet_NaN();
54-
msg->drive.acceleration = std::numeric_limits<double>::quiet_NaN();
55-
msg->drive.jerk = std::numeric_limits<double>::quiet_NaN();
56-
msg->drive.steering_angle = std::numeric_limits<double>::quiet_NaN();
57-
msg->drive.steering_angle_velocity = std::numeric_limits<double>::quiet_NaN();
53+
msg->linear_velocity = std::numeric_limits<double>::quiet_NaN();
54+
msg->steering_angle = std::numeric_limits<double>::quiet_NaN();
5855
}
5956

6057
} // namespace
@@ -128,7 +125,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure(
128125
{
129126
RCLCPP_ERROR(
130127
get_node()->get_logger(),
131-
"Ackermann configuration requires exactly two traction joints, but %zu were provided",
128+
"Steering configuration requires exactly two traction joints, but %zu were provided",
132129
params_.traction_joints_names.size());
133130
return controller_interface::CallbackReturn::ERROR;
134131
}
@@ -162,7 +159,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure(
162159
{
163160
RCLCPP_ERROR(
164161
get_node()->get_logger(),
165-
"Ackermann configuration requires exactly two steering joints, but %zu were provided",
162+
"Steering configuration requires exactly two steering joints, but %zu were provided",
166163
params_.steering_joints_names.size());
167164
return controller_interface::CallbackReturn::ERROR;
168165
}
@@ -203,7 +200,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure(
203200
{
204201
RCLCPP_ERROR(
205202
get_node()->get_logger(),
206-
"Ackermann configuration requires exactly two traction joints, but %zu state interface "
203+
"Steering configuration requires exactly two traction joints, but %zu state interface "
207204
"names were provided",
208205
params_.traction_joints_state_names.size());
209206
return controller_interface::CallbackReturn::ERROR;
@@ -248,7 +245,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure(
248245
{
249246
RCLCPP_ERROR(
250247
get_node()->get_logger(),
251-
"Ackermann configuration requires exactly two steering joints, but %zu state interface "
248+
"Steering configuration requires exactly two steering joints, but %zu state interface "
252249
"names were provided",
253250
params_.steering_joints_state_names.size());
254251
return controller_interface::CallbackReturn::ERROR;
@@ -282,14 +279,14 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure(
282279
}
283280
else
284281
{
285-
ref_subscriber_ackermann_ = get_node()->create_subscription<ControllerAckermannReferenceMsg>(
282+
ref_subscriber_steering_ = get_node()->create_subscription<ControllerSteeringReferenceMsg>(
286283
"~/reference", subscribers_qos,
287284
std::bind(
288-
&SteeringControllersLibrary::reference_callback_ackermann, this, std::placeholders::_1));
289-
std::shared_ptr<ControllerAckermannReferenceMsg> msg =
290-
std::make_shared<ControllerAckermannReferenceMsg>();
285+
&SteeringControllersLibrary::reference_callback_steering, this, std::placeholders::_1));
286+
std::shared_ptr<ControllerSteeringReferenceMsg> msg =
287+
std::make_shared<ControllerSteeringReferenceMsg>();
291288
reset_controller_reference_msg(msg, get_node());
292-
input_ref_ackermann_.writeFromNonRT(msg);
289+
input_ref_steering_.writeFromNonRT(msg);
293290
}
294291

295292
try
@@ -400,8 +397,8 @@ void SteeringControllersLibrary::reference_callback_twist(
400397
}
401398
}
402399

403-
void SteeringControllersLibrary::reference_callback_ackermann(
404-
const std::shared_ptr<ControllerAckermannReferenceMsg> msg)
400+
void SteeringControllersLibrary::reference_callback_steering(
401+
const std::shared_ptr<ControllerSteeringReferenceMsg> msg)
405402
{
406403
// if no timestamp provided use current time for command timestamp
407404
if (msg->header.stamp.sec == 0 && msg->header.stamp.nanosec == 0u)
@@ -415,7 +412,7 @@ void SteeringControllersLibrary::reference_callback_ackermann(
415412

416413
if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_)
417414
{
418-
input_ref_ackermann_.writeFromNonRT(msg);
415+
input_ref_steering_.writeFromNonRT(msg);
419416
}
420417
else
421418
{
@@ -509,7 +506,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_activate(
509506
}
510507
else
511508
{
512-
reset_controller_reference_msg(*(input_ref_ackermann_.readFromRT()), get_node());
509+
reset_controller_reference_msg(*(input_ref_steering_.readFromRT()), get_node());
513510
}
514511

515512
return controller_interface::CallbackReturn::SUCCESS;
@@ -539,11 +536,11 @@ controller_interface::return_type SteeringControllersLibrary::update_reference_f
539536
}
540537
else
541538
{
542-
auto current_ref = *(input_ref_ackermann_.readFromRT());
543-
if (!std::isnan(current_ref->drive.speed) && !std::isnan(current_ref->drive.steering_angle))
539+
auto current_ref = *(input_ref_steering_.readFromRT());
540+
if (!std::isnan(current_ref->linear_velocity) && !std::isnan(current_ref->steering_angle))
544541
{
545-
reference_interfaces_[0] = current_ref->drive.speed;
546-
reference_interfaces_[1] = current_ref->drive.steering_angle;
542+
reference_interfaces_[0] = current_ref->linear_velocity;
543+
reference_interfaces_[1] = current_ref->steering_angle;
547544
}
548545
}
549546

@@ -562,9 +559,9 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c
562559

563560
if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1]))
564561
{
565-
const auto age_of_last_command =
566-
params_.use_twist_input ? time - (*(input_ref_twist_.readFromRT()))->header.stamp
567-
: time - (*(input_ref_ackermann_.readFromRT()))->header.stamp;
562+
const auto age_of_last_command = params_.use_twist_input
563+
? time - (*(input_ref_twist_.readFromRT()))->header.stamp
564+
: time - (*(input_ref_steering_.readFromRT()))->header.stamp;
568565

569566
const auto timeout =
570567
age_of_last_command > ref_timeout_ && ref_timeout_ != rclcpp::Duration::from_seconds(0);

‎steering_controllers_library/src/steering_controllers_library.yaml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -118,6 +118,6 @@ steering_controllers_library:
118118
use_twist_input: {
119119
type: bool,
120120
default_value: true,
121-
description: "Choose whether a TwistStamped or a AckermannDriveStamped message is used as input.",
121+
description: "Choose whether a TwistStamped or a SteeringControllerCommand message is used as input.",
122122
read_only: true,
123123
}

‎steering_controllers_library/test/steering_controllers_library_ackermann_params.yaml‎ renamed to ‎steering_controllers_library/test/steering_controllers_library_steering_input_params.yaml‎

Lines changed: 3 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,14 +1,13 @@
1-
test_steering_controllers_library:
1+
test_steering_controllers_steering_input_library:
22
ros__parameters:
33

44
reference_timeout: 0.1
5-
front_steering: true
65
open_loop: false
76
velocity_rolling_window_size: 10
87
position_feedback: false
98
use_twist_input: false
10-
rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint]
11-
front_wheels_names: [front_right_steering_joint, front_left_steering_joint]
9+
traction_joints_names: [rear_right_wheel_joint, rear_left_wheel_joint]
10+
steering_joints_names: [front_right_steering_joint, front_left_steering_joint]
1211

1312
wheelbase: 3.24644
1413
steering_track_width: 2.12321

‎steering_controllers_library/test/test_steering_controllers_library.hpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -35,8 +35,8 @@ using ControllerStateMsg =
3535
steering_controllers_library::SteeringControllersLibrary::SteeringControllerStateMsg;
3636
using ControllerTwistReferenceMsg =
3737
steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg;
38-
using ControllerAckermannReferenceMsg =
39-
steering_controllers_library::SteeringControllersLibrary::ControllerAckermannReferenceMsg;
38+
using ControllerSteeringReferenceMsg =
39+
steering_controllers_library::SteeringControllersLibrary::ControllerSteeringReferenceMsg;
4040

4141
// NOTE: Testing steering_controllers_library for Ackermann vehicle configuration only
4242

‎steering_controllers_library/test/test_steering_controllers_library_ackermann_input.cpp‎ renamed to ‎steering_controllers_library/test/test_steering_controllers_library_steering_input.cpp‎

Lines changed: 20 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -18,15 +18,15 @@
1818
#include <vector>
1919

2020
#include "hardware_interface/types/hardware_interface_type_values.hpp"
21-
#include "test_steering_controllers_library_ackermann_input.hpp"
21+
#include "test_steering_controllers_library_steering_input.hpp"
2222

23-
class SteeringControllersLibraryTest
24-
: public SteeringControllersLibraryFixture<TestableSteeringControllersLibrary>
23+
class SteeringControllersLibrarySteeringInputTest
24+
: public SteeringControllersSteeringInputLibraryFixture<TestableSteeringControllersSteeringInputLibrary>
2525
{
2626
};
2727

2828
// checking if all interfaces, command, state and reference are exported as expected
29-
TEST_F(SteeringControllersLibraryTest, check_exported_interfaces)
29+
TEST_F(SteeringControllersLibrarySteeringInputTest, check_exported_interfaces)
3030
{
3131
SetUpController();
3232

@@ -82,7 +82,7 @@ TEST_F(SteeringControllersLibraryTest, check_exported_interfaces)
8282
// Tests controller update_reference_from_subscribers and
8383
// its two cases for position_feedback true/false behavior
8484
// when too old msg is sent i.e age_of_last_command > ref_timeout case
85-
TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout)
85+
TEST_F(SteeringControllersLibrarySteeringInputTest, test_both_update_methods_for_ref_timeout)
8686
{
8787
SetUpController();
8888

@@ -103,30 +103,27 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout)
103103
const float TEST_LINEAR_VELOCITY_X = static_cast<float>(1.5);
104104
const float TEST_STEERING_ANGLE = static_cast<float>(0.575875);
105105

106-
std::shared_ptr<ControllerAckermannReferenceMsg> msg =
107-
std::make_shared<ControllerAckermannReferenceMsg>();
106+
std::shared_ptr<ControllerSteeringReferenceMsg> msg =
107+
std::make_shared<ControllerSteeringReferenceMsg>();
108108

109109
// adjusting to achieve age_of_last_command > ref_timeout
110110
msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ -
111111
rclcpp::Duration::from_seconds(0.1);
112-
msg->drive.speed = TEST_LINEAR_VELOCITY_X;
113-
msg->drive.steering_angle = TEST_STEERING_ANGLE;
114-
msg->drive.steering_angle_velocity = std::numeric_limits<double>::quiet_NaN();
115-
msg->drive.acceleration = std::numeric_limits<double>::quiet_NaN();
116-
msg->drive.jerk = std::numeric_limits<double>::quiet_NaN();
117-
controller_->input_ref_ackermann_.writeFromNonRT(msg);
112+
msg->linear_velocity = TEST_LINEAR_VELOCITY_X;
113+
msg->steering_angle = TEST_STEERING_ANGLE;
114+
controller_->input_ref_steering_.writeFromNonRT(msg);
118115

119116
const auto age_of_last_command =
120117
controller_->get_node()->now() -
121-
(*(controller_->input_ref_ackermann_.readFromNonRT()))->header.stamp;
118+
(*(controller_->input_ref_steering_.readFromNonRT()))->header.stamp;
122119

123120
// case 1 position_feedback = false
124121
controller_->params_.position_feedback = false;
125122

126123
// age_of_last_command > ref_timeout_
127124
ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_);
128125
ASSERT_EQ(
129-
(*(controller_->input_ref_ackermann_.readFromRT()))->drive.speed, TEST_LINEAR_VELOCITY_X);
126+
(*(controller_->input_ref_steering_.readFromRT()))->linear_velocity, TEST_LINEAR_VELOCITY_X);
130127
ASSERT_EQ(
131128
controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)),
132129
controller_interface::return_type::OK);
@@ -138,9 +135,9 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout)
138135
EXPECT_TRUE(std::isnan(interface));
139136
}
140137
ASSERT_EQ(
141-
(*(controller_->input_ref_ackermann_.readFromNonRT()))->drive.speed, TEST_LINEAR_VELOCITY_X);
138+
(*(controller_->input_ref_steering_.readFromNonRT()))->linear_velocity, TEST_LINEAR_VELOCITY_X);
142139
ASSERT_EQ(
143-
(*(controller_->input_ref_ackermann_.readFromNonRT()))->drive.steering_angle,
140+
(*(controller_->input_ref_steering_.readFromNonRT()))->steering_angle,
144141
TEST_STEERING_ANGLE);
145142

146143
EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0]));
@@ -163,17 +160,14 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout)
163160
// adjusting to achieve age_of_last_command > ref_timeout
164161
msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ -
165162
rclcpp::Duration::from_seconds(0.1);
166-
msg->drive.speed = TEST_LINEAR_VELOCITY_X;
167-
msg->drive.steering_angle = TEST_STEERING_ANGLE;
168-
msg->drive.steering_angle_velocity = std::numeric_limits<double>::quiet_NaN();
169-
msg->drive.acceleration = std::numeric_limits<double>::quiet_NaN();
170-
msg->drive.jerk = std::numeric_limits<double>::quiet_NaN();
171-
controller_->input_ref_ackermann_.writeFromNonRT(msg);
163+
msg->linear_velocity = TEST_LINEAR_VELOCITY_X;
164+
msg->steering_angle = TEST_STEERING_ANGLE;
165+
controller_->input_ref_steering_.writeFromNonRT(msg);
172166

173167
// age_of_last_command > ref_timeout_
174168
ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_);
175169
ASSERT_EQ(
176-
(*(controller_->input_ref_ackermann_.readFromRT()))->drive.speed, TEST_LINEAR_VELOCITY_X);
170+
(*(controller_->input_ref_steering_.readFromRT()))->linear_velocity, TEST_LINEAR_VELOCITY_X);
177171
ASSERT_EQ(
178172
controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)),
179173
controller_interface::return_type::OK);
@@ -185,9 +179,9 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout)
185179
EXPECT_TRUE(std::isnan(interface));
186180
}
187181
ASSERT_EQ(
188-
(*(controller_->input_ref_ackermann_.readFromNonRT()))->drive.speed, TEST_LINEAR_VELOCITY_X);
182+
(*(controller_->input_ref_steering_.readFromNonRT()))->linear_velocity, TEST_LINEAR_VELOCITY_X);
189183
ASSERT_EQ(
190-
(*(controller_->input_ref_ackermann_.readFromNonRT()))->drive.steering_angle,
184+
(*(controller_->input_ref_steering_.readFromNonRT()))->steering_angle,
191185
TEST_STEERING_ANGLE);
192186

193187
EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0]));

0 commit comments

Comments
 (0)