Skip to content

Commit 6e3263e

Browse files
JuliajJuliaj
committed
Merge branch 'master' into pid_controller_update_tests
2 parents 9c0920c + c9ca6f7 commit 6e3263e

File tree

42 files changed

+364
-257
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

42 files changed

+364
-257
lines changed

‎ackermann_steering_controller/test/test_ackermann_steering_controller.cpp

Lines changed: 9 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -12,12 +12,13 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#include "test_ackermann_steering_controller.hpp"
16-
1715
#include <memory>
1816
#include <string>
1917
#include <vector>
2018

19+
#include "hardware_interface/types/hardware_interface_type_values.hpp"
20+
#include "test_ackermann_steering_controller.hpp"
21+
2122
class AckermannSteeringControllerTest
2223
: public AckermannSteeringControllerFixture<TestableAckermannSteeringController>
2324
{
@@ -87,11 +88,13 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces)
8788
ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size());
8889
for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i)
8990
{
90-
const std::string ref_itf_name =
91+
const std::string ref_itf_prefix_name =
9192
std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i];
92-
EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name);
93-
EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name());
94-
EXPECT_EQ(reference_interfaces[i]->get_interface_name(), joint_reference_interfaces_[i]);
93+
EXPECT_EQ(
94+
reference_interfaces[i]->get_name(),
95+
ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY);
96+
EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name);
97+
EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_VELOCITY);
9598
}
9699
}
97100

‎ackermann_steering_controller/test/test_ackermann_steering_controller.hpp

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -295,8 +295,7 @@ class AckermannSteeringControllerFixture : public ::testing::Test
295295

296296
std::array<double, 4> joint_state_values_ = {{0.5, 0.5, 0.0, 0.0}};
297297
std::array<double, 4> joint_command_values_ = {{1.1, 3.3, 2.2, 4.4}};
298-
std::array<std::string, 2> joint_reference_interfaces_ = {
299-
{"linear/velocity", "angular/velocity"}};
298+
std::array<std::string, 2> joint_reference_interfaces_ = {{"linear", "angular"}};
300299
std::string steering_interface_name_ = "position";
301300
// defined in setup
302301
std::string traction_interface_name_ = "";

‎ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp

Lines changed: 9 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -12,12 +12,13 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#include "test_ackermann_steering_controller.hpp"
16-
1715
#include <memory>
1816
#include <string>
1917
#include <vector>
2018

19+
#include "hardware_interface/types/hardware_interface_type_values.hpp"
20+
#include "test_ackermann_steering_controller.hpp"
21+
2122
class AckermannSteeringControllerTest
2223
: public AckermannSteeringControllerFixture<TestableAckermannSteeringController>
2324
{
@@ -89,11 +90,13 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces)
8990
ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size());
9091
for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i)
9192
{
92-
const std::string ref_itf_name =
93+
const std::string ref_itf_prefix_name =
9394
std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i];
94-
EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name);
95-
EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name());
96-
EXPECT_EQ(reference_interfaces[i]->get_interface_name(), joint_reference_interfaces_[i]);
95+
EXPECT_EQ(
96+
reference_interfaces[i]->get_name(),
97+
ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY);
98+
EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name);
99+
EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_VELOCITY);
97100
}
98101
}
99102

‎admittance_controller/src/admittance_controller.cpp

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -147,9 +147,9 @@ AdmittanceController::on_export_reference_interfaces()
147147
{
148148
velocity_reference_.emplace_back(reference_interfaces_[index]);
149149
}
150-
const auto full_name = joint + "/" + interface;
150+
const auto exported_prefix = std::string(get_node()->get_name()) + "/" + joint;
151151
chainable_command_interfaces.emplace_back(hardware_interface::CommandInterface(
152-
std::string(get_node()->get_name()), full_name, reference_interfaces_.data() + index));
152+
exported_prefix, interface, reference_interfaces_.data() + index));
153153

154154
index++;
155155
}

‎admittance_controller/test/test_admittance_controller.cpp

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -168,6 +168,27 @@ TEST_F(AdmittanceControllerTest, check_interfaces)
168168
ASSERT_EQ(
169169
controller_->state_interfaces_.size(),
170170
state_interface_types_.size() * joint_names_.size() + fts_state_values_.size());
171+
172+
const auto reference_interfaces = controller_->ordered_exported_reference_interfaces_;
173+
ASSERT_EQ(reference_interfaces.size(), 2 * joint_names_.size());
174+
for (auto i = 0ul; i < joint_names_.size(); i++)
175+
{
176+
const std::string ref_itf_prefix_name =
177+
std::string(controller_->get_node()->get_name()) + "/" + joint_names_[i];
178+
EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name);
179+
EXPECT_EQ(
180+
reference_interfaces[i]->get_name(),
181+
ref_itf_prefix_name + "/" + hardware_interface::HW_IF_POSITION);
182+
EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_POSITION);
183+
EXPECT_EQ(
184+
reference_interfaces[i + joint_names_.size()]->get_prefix_name(), ref_itf_prefix_name);
185+
EXPECT_EQ(
186+
reference_interfaces[i + joint_names_.size()]->get_name(),
187+
ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY);
188+
EXPECT_EQ(
189+
reference_interfaces[i + joint_names_.size()]->get_interface_name(),
190+
hardware_interface::HW_IF_VELOCITY);
191+
}
171192
}
172193

173194
TEST_F(AdmittanceControllerTest, activate_success)

‎bicycle_steering_controller/test/test_bicycle_steering_controller.cpp

Lines changed: 9 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -12,12 +12,13 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#include "test_bicycle_steering_controller.hpp"
16-
1715
#include <memory>
1816
#include <string>
1917
#include <vector>
2018

19+
#include "hardware_interface/types/hardware_interface_type_values.hpp"
20+
#include "test_bicycle_steering_controller.hpp"
21+
2122
class BicycleSteeringControllerTest
2223
: public BicycleSteeringControllerFixture<TestableBicycleSteeringController>
2324
{
@@ -71,11 +72,13 @@ TEST_F(BicycleSteeringControllerTest, check_exported_interfaces)
7172
ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size());
7273
for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i)
7374
{
74-
const std::string ref_itf_name =
75+
const std::string ref_itf_prefix_name =
7576
std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i];
76-
EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name);
77-
EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name());
78-
EXPECT_EQ(reference_interfaces[i]->get_interface_name(), joint_reference_interfaces_[i]);
77+
EXPECT_EQ(
78+
reference_interfaces[i]->get_name(),
79+
ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY);
80+
EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name);
81+
EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_VELOCITY);
7982
}
8083
}
8184

‎bicycle_steering_controller/test/test_bicycle_steering_controller.hpp

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -263,8 +263,7 @@ class BicycleSteeringControllerFixture : public ::testing::Test
263263

264264
std::array<double, 2> joint_state_values_ = {{3.3, 0.5}};
265265
std::array<double, 2> joint_command_values_ = {{1.1, 2.2}};
266-
std::array<std::string, 2> joint_reference_interfaces_ = {
267-
{"linear/velocity", "angular/velocity"}};
266+
std::array<std::string, 2> joint_reference_interfaces_ = {{"linear", "angular"}};
268267
std::string steering_interface_name_ = "position";
269268

270269
// defined in setup

‎bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp

Lines changed: 9 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -12,12 +12,13 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#include "test_bicycle_steering_controller.hpp"
16-
1715
#include <memory>
1816
#include <string>
1917
#include <vector>
2018

19+
#include "hardware_interface/types/hardware_interface_type_values.hpp"
20+
#include "test_bicycle_steering_controller.hpp"
21+
2122
class BicycleSteeringControllerTest
2223
: public BicycleSteeringControllerFixture<TestableBicycleSteeringController>
2324
{
@@ -75,11 +76,13 @@ TEST_F(BicycleSteeringControllerTest, check_exported_interfaces)
7576
ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size());
7677
for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i)
7778
{
78-
const std::string ref_itf_name =
79+
const std::string ref_itf_prefix_name =
7980
std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i];
80-
EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name);
81-
EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name());
82-
EXPECT_EQ(reference_interfaces[i]->get_interface_name(), joint_reference_interfaces_[i]);
81+
EXPECT_EQ(
82+
reference_interfaces[i]->get_name(),
83+
ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY);
84+
EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name);
85+
EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_VELOCITY);
8386
}
8487
}
8588

‎diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -141,8 +141,6 @@ class DiffDriveController : public controller_interface::ChainableControllerInte
141141
rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0);
142142
rclcpp::Time previous_publish_timestamp_{0, 0, RCL_CLOCK_UNINITIALIZED};
143143

144-
bool is_halted = false;
145-
146144
bool reset();
147145
void halt();
148146

‎diff_drive_controller/src/diff_drive_controller.cpp

Lines changed: 3 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -101,15 +101,6 @@ controller_interface::return_type DiffDriveController::update_reference_from_sub
101101
const rclcpp::Time & time, const rclcpp::Duration & /*period*/)
102102
{
103103
auto logger = get_node()->get_logger();
104-
if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE)
105-
{
106-
if (!is_halted)
107-
{
108-
halt();
109-
is_halted = true;
110-
}
111-
return controller_interface::return_type::OK;
112-
}
113104

114105
const std::shared_ptr<TwistStamped> command_msg_ptr = *(received_velocity_msg_ptr_.readFromRT());
115106

@@ -149,15 +140,6 @@ controller_interface::return_type DiffDriveController::update_and_write_commands
149140
const rclcpp::Time & time, const rclcpp::Duration & period)
150141
{
151142
auto logger = get_node()->get_logger();
152-
if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE)
153-
{
154-
if (!is_halted)
155-
{
156-
halt();
157-
is_halted = true;
158-
}
159-
return controller_interface::return_type::OK;
160-
}
161143

162144
// command may be limited further by SpeedLimit,
163145
// without affecting the stored twist command
@@ -562,7 +544,6 @@ controller_interface::CallbackReturn DiffDriveController::on_activate(
562544
return controller_interface::CallbackReturn::ERROR;
563545
}
564546

565-
is_halted = false;
566547
subscriber_is_active_ = true;
567548

568549
RCLCPP_DEBUG(get_node()->get_logger(), "Subscriber and publisher are now active.");
@@ -573,11 +554,7 @@ controller_interface::CallbackReturn DiffDriveController::on_deactivate(
573554
const rclcpp_lifecycle::State &)
574555
{
575556
subscriber_is_active_ = false;
576-
if (!is_halted)
577-
{
578-
halt();
579-
is_halted = true;
580-
}
557+
halt();
581558
reset_buffers();
582559
registered_left_wheel_handles_.clear();
583560
registered_right_wheel_handles_.clear();
@@ -616,7 +593,6 @@ bool DiffDriveController::reset()
616593
subscriber_is_active_ = false;
617594
velocity_command_subscriber_.reset();
618595

619-
is_halted = false;
620596
return true;
621597
}
622598

@@ -724,11 +700,11 @@ DiffDriveController::on_export_reference_interfaces()
724700
reference_interfaces.reserve(reference_interfaces_.size());
725701

726702
reference_interfaces.push_back(hardware_interface::CommandInterface(
727-
get_node()->get_name(), std::string("linear/") + hardware_interface::HW_IF_VELOCITY,
703+
get_node()->get_name() + std::string("/linear"), hardware_interface::HW_IF_VELOCITY,
728704
&reference_interfaces_[0]));
729705

730706
reference_interfaces.push_back(hardware_interface::CommandInterface(
731-
get_node()->get_name(), std::string("angular/") + hardware_interface::HW_IF_VELOCITY,
707+
get_node()->get_name() + std::string("/angular"), hardware_interface::HW_IF_VELOCITY,
732708
&reference_interfaces_[1]));
733709

734710
return reference_interfaces;

0 commit comments

Comments
 (0)