diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp index 757f22a740..7159adc49b 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp +++ b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp @@ -130,6 +130,7 @@ class DiffDriveController : public controller_interface::ControllerInterface rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; + std::shared_ptr last_command_msg_; std::queue previous_commands_; // last two commands diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 1eaf187964..937e8a93d9 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -118,26 +118,27 @@ controller_interface::return_type DiffDriveController::update( return controller_interface::return_type::OK; } - std::shared_ptr last_command_msg; - received_velocity_msg_ptr_.get(last_command_msg); + // if the mutex is unable to lock, last_command_msg_ won't be updated + received_velocity_msg_ptr_.try_get([this](const std::shared_ptr & msg) + { last_command_msg_ = msg; }); - if (last_command_msg == nullptr) + if (last_command_msg_ == nullptr) { RCLCPP_WARN(logger, "Velocity message received was a nullptr."); return controller_interface::return_type::ERROR; } - const auto age_of_last_command = time - last_command_msg->header.stamp; + const auto age_of_last_command = time - last_command_msg_->header.stamp; // Brake if cmd_vel has timeout, override the stored command if (age_of_last_command > cmd_vel_timeout_) { - last_command_msg->twist.linear.x = 0.0; - last_command_msg->twist.angular.z = 0.0; + last_command_msg_->twist.linear.x = 0.0; + last_command_msg_->twist.angular.z = 0.0; } // command may be limited further by SpeedLimit, // without affecting the stored twist command - TwistStamped command = *last_command_msg; + TwistStamped command = *last_command_msg_; double & linear_command = command.twist.linear.x; double & angular_command = command.twist.angular.z; @@ -387,12 +388,12 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( limited_velocity_publisher_); } - const TwistStamped empty_twist; - received_velocity_msg_ptr_.set(std::make_shared(empty_twist)); - + last_command_msg_ = std::make_shared(); + received_velocity_msg_ptr_.set([this](std::shared_ptr & stored_value) + { stored_value = last_command_msg_; }); // Fill last two commands with default constructed commands - previous_commands_.emplace(empty_twist); - previous_commands_.emplace(empty_twist); + previous_commands_.emplace(*last_command_msg_); + previous_commands_.emplace(*last_command_msg_); // initialize command subscriber velocity_command_subscriber_ = get_node()->create_subscription( @@ -412,7 +413,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 & stored_value) + { stored_value = std::move(msg); }); }); // initialize odometry publisher and message @@ -538,7 +540,6 @@ controller_interface::CallbackReturn DiffDriveController::on_cleanup( return controller_interface::CallbackReturn::ERROR; } - received_velocity_msg_ptr_.set(std::make_shared()); return controller_interface::CallbackReturn::SUCCESS; } diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 6ebbb9f464..b7665c9fc2 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -14,7 +14,207 @@ #include -#include "test_diff_drive_controller.hpp" +#include +#include +#include +#include + +#include "diff_drive_controller/diff_drive_controller.hpp" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" + +using CallbackReturn = controller_interface::CallbackReturn; +using hardware_interface::HW_IF_POSITION; +using hardware_interface::HW_IF_VELOCITY; +using hardware_interface::LoanedCommandInterface; +using hardware_interface::LoanedStateInterface; +using lifecycle_msgs::msg::State; +using testing::SizeIs; + +namespace +{ +const std::vector left_wheel_names = {"left_wheel_joint"}; +const std::vector right_wheel_names = {"right_wheel_joint"}; +} // namespace + +class TestableDiffDriveController : public diff_drive_controller::DiffDriveController +{ +public: + using DiffDriveController::DiffDriveController; + std::shared_ptr getLastReceivedTwist() + { + std::shared_ptr ret; + received_velocity_msg_ptr_.get( + [&ret](const std::shared_ptr & msg) { ret = msg; }); + return ret; + } + + /** + * @brief wait_for_twist block until a new twist is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function + */ + void wait_for_twist( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds(500)) + { + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + } + + /** + * @brief Used to get the real_time_odometry_publisher to verify its contents + * + * @return Copy of realtime_odometry_publisher_ object + */ + std::shared_ptr> + get_rt_odom_publisher() + { + return realtime_odometry_publisher_; + } +}; + +class TestDiffDriveController : public ::testing::Test +{ +protected: + void SetUp() override + { + // use the name of the test as the controller name (i.e, the node name) to be able to set + // parameters from yaml for each test + controller_name = ::testing::UnitTest::GetInstance()->current_test_info()->name(); + controller_ = std::make_unique(); + + pub_node = std::make_shared("velocity_publisher"); + velocity_publisher = pub_node->create_publisher( + controller_name + "/cmd_vel", rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() { rclcpp::shutdown(); } + + /// Publish velocity msgs + /** + * linear - magnitude of the linear command in the geometry_msgs::twist message + * angular - the magnitude of the angular command in geometry_msgs::twist message + */ + void publish(double linear, double angular) + { + int wait_count = 0; + auto topic = velocity_publisher->get_topic_name(); + while (pub_node->count_subscribers(topic) == 0) + { + if (wait_count >= 5) + { + auto error_msg = std::string("publishing to ") + topic + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + + geometry_msgs::msg::TwistStamped velocity_message; + velocity_message.header.stamp = pub_node->get_clock()->now(); + velocity_message.twist.linear.x = linear; + velocity_message.twist.angular.z = angular; + velocity_publisher->publish(velocity_message); + } + + /// \brief wait for the subscriber and publisher to completely setup + void waitForSetup() + { + constexpr std::chrono::seconds TIMEOUT{2}; + auto clock = pub_node->get_clock(); + auto start = clock->now(); + while (velocity_publisher->get_subscription_count() <= 0) + { + if ((clock->now() - start) > TIMEOUT) + { + FAIL(); + } + rclcpp::spin_some(pub_node); + } + } + + void assignResourcesPosFeedback() + { + std::vector state_ifs; + state_ifs.emplace_back(left_wheel_pos_state_); + state_ifs.emplace_back(right_wheel_pos_state_); + + std::vector command_ifs; + command_ifs.emplace_back(left_wheel_vel_cmd_); + command_ifs.emplace_back(right_wheel_vel_cmd_); + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void assignResourcesVelFeedback() + { + std::vector state_ifs; + state_ifs.emplace_back(left_wheel_vel_state_); + state_ifs.emplace_back(right_wheel_vel_state_); + + std::vector command_ifs; + command_ifs.emplace_back(left_wheel_vel_cmd_); + command_ifs.emplace_back(right_wheel_vel_cmd_); + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + controller_interface::return_type InitController( + const std::vector left_wheel_joints_init = left_wheel_names, + const std::vector right_wheel_joints_init = right_wheel_names, + const std::vector & parameters = {}, const std::string ns = "") + { + auto node_options = rclcpp::NodeOptions(); + std::vector parameter_overrides; + + parameter_overrides.push_back( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_joints_init))); + parameter_overrides.push_back( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_joints_init))); + // default parameters + parameter_overrides.push_back( + rclcpp::Parameter("wheel_separation", rclcpp::ParameterValue(1.0))); + parameter_overrides.push_back(rclcpp::Parameter("wheel_radius", rclcpp::ParameterValue(0.1))); + + parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); + node_options.parameter_overrides(parameter_overrides); + + return controller_->init(controller_name, urdf_, 0, ns, node_options); + } + + std::string controller_name; + std::unique_ptr controller_; + + std::vector position_values_ = {0.1, 0.2}; + std::vector velocity_values_ = {0.01, 0.02}; + + hardware_interface::StateInterface left_wheel_pos_state_{ + left_wheel_names[0], HW_IF_POSITION, &position_values_[0]}; + hardware_interface::StateInterface right_wheel_pos_state_{ + right_wheel_names[0], HW_IF_POSITION, &position_values_[1]}; + hardware_interface::StateInterface left_wheel_vel_state_{ + left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]}; + hardware_interface::StateInterface right_wheel_vel_state_{ + right_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[1]}; + hardware_interface::CommandInterface left_wheel_vel_cmd_{ + left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]}; + hardware_interface::CommandInterface right_wheel_vel_cmd_{ + right_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[1]}; + + rclcpp::Node::SharedPtr pub_node; + rclcpp::Publisher::SharedPtr velocity_publisher; + + const std::string urdf_ = ""; +}; TEST_F(TestDiffDriveController, init_fails_without_parameters) { diff --git a/diff_drive_controller/test/test_diff_drive_controller.hpp b/diff_drive_controller/test/test_diff_drive_controller.hpp deleted file mode 100644 index b5d1b88422..0000000000 --- a/diff_drive_controller/test/test_diff_drive_controller.hpp +++ /dev/null @@ -1,221 +0,0 @@ -// Copyright 2020 PAL Robotics SL. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef TEST_DIFF_DRIVE_CONTROLLER_HPP_ -#define TEST_DIFF_DRIVE_CONTROLLER_HPP_ - -#include - -#include -#include -#include -#include - -#include "diff_drive_controller/diff_drive_controller.hpp" -#include "hardware_interface/loaned_command_interface.hpp" -#include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/executor.hpp" -#include "rclcpp/executors.hpp" - -using CallbackReturn = controller_interface::CallbackReturn; -using hardware_interface::HW_IF_POSITION; -using hardware_interface::HW_IF_VELOCITY; -using hardware_interface::LoanedCommandInterface; -using hardware_interface::LoanedStateInterface; -using lifecycle_msgs::msg::State; -using testing::SizeIs; - -namespace -{ -const std::vector left_wheel_names = {"left_wheel_joint"}; -const std::vector right_wheel_names = {"right_wheel_joint"}; -} // namespace - -class TestableDiffDriveController : public diff_drive_controller::DiffDriveController -{ -public: - using DiffDriveController::DiffDriveController; - std::shared_ptr getLastReceivedTwist() - { - std::shared_ptr ret; - received_velocity_msg_ptr_.get(ret); - return ret; - } - - /** - * @brief wait_for_twist block until a new twist is received. - * Requires that the executor is not spinned elsewhere between the - * message publication and the call to this function - */ - void wait_for_twist( - rclcpp::Executor & executor, - const std::chrono::milliseconds & timeout = std::chrono::milliseconds(500)) - { - auto until = get_node()->get_clock()->now() + timeout; - while (get_node()->get_clock()->now() < until) - { - executor.spin_some(); - std::this_thread::sleep_for(std::chrono::microseconds(10)); - } - } - - /** - * @brief Used to get the real_time_odometry_publisher to verify its contents - * - * @return Copy of realtime_odometry_publisher_ object - */ - std::shared_ptr> - get_rt_odom_publisher() - { - return realtime_odometry_publisher_; - } -}; - -class TestDiffDriveController : public ::testing::Test -{ -protected: - void SetUp() override - { - // use the name of the test as the controller name (i.e, the node name) to be able to set - // parameters from yaml for each test - controller_name = ::testing::UnitTest::GetInstance()->current_test_info()->name(); - controller_ = std::make_unique(); - - pub_node = std::make_shared("velocity_publisher"); - velocity_publisher = pub_node->create_publisher( - controller_name + "/cmd_vel", rclcpp::SystemDefaultsQoS()); - } - - static void TearDownTestCase() { rclcpp::shutdown(); } - - /// Publish velocity msgs - /** - * linear - magnitude of the linear command in the geometry_msgs::twist message - * angular - the magnitude of the angular command in geometry_msgs::twist message - */ - void publish(double linear, double angular) - { - int wait_count = 0; - auto topic = velocity_publisher->get_topic_name(); - while (pub_node->count_subscribers(topic) == 0) - { - if (wait_count >= 5) - { - auto error_msg = std::string("publishing to ") + topic + " but no node subscribes to it"; - throw std::runtime_error(error_msg); - } - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - ++wait_count; - } - - geometry_msgs::msg::TwistStamped velocity_message; - velocity_message.header.stamp = pub_node->get_clock()->now(); - velocity_message.twist.linear.x = linear; - velocity_message.twist.angular.z = angular; - velocity_publisher->publish(velocity_message); - } - - /// \brief wait for the subscriber and publisher to completely setup - void waitForSetup() - { - constexpr std::chrono::seconds TIMEOUT{2}; - auto clock = pub_node->get_clock(); - auto start = clock->now(); - while (velocity_publisher->get_subscription_count() <= 0) - { - if ((clock->now() - start) > TIMEOUT) - { - FAIL(); - } - rclcpp::spin_some(pub_node); - } - } - - void assignResourcesPosFeedback() - { - std::vector state_ifs; - state_ifs.emplace_back(left_wheel_pos_state_); - state_ifs.emplace_back(right_wheel_pos_state_); - - std::vector command_ifs; - command_ifs.emplace_back(left_wheel_vel_cmd_); - command_ifs.emplace_back(right_wheel_vel_cmd_); - - controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); - } - - void assignResourcesVelFeedback() - { - std::vector state_ifs; - state_ifs.emplace_back(left_wheel_vel_state_); - state_ifs.emplace_back(right_wheel_vel_state_); - - std::vector command_ifs; - command_ifs.emplace_back(left_wheel_vel_cmd_); - command_ifs.emplace_back(right_wheel_vel_cmd_); - - controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); - } - - controller_interface::return_type InitController( - const std::vector left_wheel_joints_init = left_wheel_names, - const std::vector right_wheel_joints_init = right_wheel_names, - const std::vector & parameters = {}, const std::string ns = "") - { - auto node_options = rclcpp::NodeOptions(); - std::vector parameter_overrides; - - parameter_overrides.push_back( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_joints_init))); - parameter_overrides.push_back( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_joints_init))); - // default parameters - parameter_overrides.push_back( - rclcpp::Parameter("wheel_separation", rclcpp::ParameterValue(1.0))); - parameter_overrides.push_back(rclcpp::Parameter("wheel_radius", rclcpp::ParameterValue(0.1))); - - parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); - node_options.parameter_overrides(parameter_overrides); - - return controller_->init(controller_name, urdf_, 0, ns, node_options); - } - - std::string controller_name; - std::unique_ptr controller_; - - std::vector position_values_ = {0.1, 0.2}; - std::vector velocity_values_ = {0.01, 0.02}; - - hardware_interface::StateInterface left_wheel_pos_state_{ - left_wheel_names[0], HW_IF_POSITION, &position_values_[0]}; - hardware_interface::StateInterface right_wheel_pos_state_{ - right_wheel_names[0], HW_IF_POSITION, &position_values_[1]}; - hardware_interface::StateInterface left_wheel_vel_state_{ - left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]}; - hardware_interface::StateInterface right_wheel_vel_state_{ - right_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[1]}; - hardware_interface::CommandInterface left_wheel_vel_cmd_{ - left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]}; - hardware_interface::CommandInterface right_wheel_vel_cmd_{ - right_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[1]}; - - rclcpp::Node::SharedPtr pub_node; - rclcpp::Publisher::SharedPtr velocity_publisher; - - const std::string urdf_ = ""; -}; - -#endif // TEST_DIFF_DRIVE_CONTROLLER_HPP_ diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index 010a890f52..1f9361dadd 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -139,6 +139,7 @@ class TricycleController : public controller_interface::ControllerInterface rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; + std::shared_ptr last_command_msg_; rclcpp::Service::SharedPtr reset_odom_service_; diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index ec7ca7bd5e..1d6daba958 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -95,25 +95,27 @@ controller_interface::return_type TricycleController::update( } return controller_interface::return_type::OK; } - std::shared_ptr last_command_msg; - received_velocity_msg_ptr_.get(last_command_msg); - if (last_command_msg == nullptr) + + // if the mutex is unable to lock, last_command_msg_ won't be updated + received_velocity_msg_ptr_.try_get([this](const std::shared_ptr & msg) + { last_command_msg_ = msg; }); + if (last_command_msg_ == nullptr) { RCLCPP_WARN(get_node()->get_logger(), "Velocity message received was a nullptr."); return controller_interface::return_type::ERROR; } - const auto age_of_last_command = time - last_command_msg->header.stamp; + const auto age_of_last_command = time - last_command_msg_->header.stamp; // Brake if cmd_vel has timeout, override the stored command if (age_of_last_command > cmd_vel_timeout_) { - last_command_msg->twist.linear.x = 0.0; - last_command_msg->twist.angular.z = 0.0; + last_command_msg_->twist.linear.x = 0.0; + last_command_msg_->twist.angular.z = 0.0; } // command may be limited further by Limiters, // without affecting the stored twist command - TwistStamped command = *last_command_msg; + TwistStamped command = *last_command_msg_; double & linear_command = command.twist.linear.x; double & angular_command = command.twist.angular.z; 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 & return CallbackReturn::ERROR; } - const TwistStamped empty_twist; - received_velocity_msg_ptr_.set(std::make_shared(empty_twist)); - + last_command_msg_ = std::make_shared(); + received_velocity_msg_ptr_.set([this](std::shared_ptr & stored_value) + { stored_value = last_command_msg_; }); // Fill last two commands with default constructed commands const AckermannDrive empty_ackermann_drive; previous_commands_.emplace(empty_ackermann_drive); @@ -307,7 +309,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 & stored_value) + { stored_value = std::move(msg); }); }); // initialize odometry publisher and message @@ -397,7 +400,6 @@ CallbackReturn TricycleController::on_cleanup(const rclcpp_lifecycle::State &) return CallbackReturn::ERROR; } - received_velocity_msg_ptr_.set(std::make_shared()); return CallbackReturn::SUCCESS; } diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index 9d43c2590d..5e868ebeea 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -54,7 +54,8 @@ class TestableTricycleController : public tricycle_controller::TricycleControlle std::shared_ptr getLastReceivedTwist() { std::shared_ptr ret; - received_velocity_msg_ptr_.get(ret); + received_velocity_msg_ptr_.get( + [&ret](const std::shared_ptr & msg) { ret = msg; }); return ret; }