From 4343c7a4632a97f52fc664b5dcdcad258ffb5e2a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 4 Nov 2024 12:50:08 +0100 Subject: [PATCH 01/84] [ForceTorqueSensorBroadcaster] added force and torque offsets to the parameters + export state interfaces (#1215) --- .../force_torque_sensor_broadcaster.xml | 2 +- .../force_torque_sensor_broadcaster.hpp | 16 +++- .../src/force_torque_sensor_broadcaster.cpp | 80 ++++++++++++++++++- ..._torque_sensor_broadcaster_parameters.yaml | 33 ++++++++ .../test_force_torque_sensor_broadcaster.cpp | 78 ++++++++++++++++++ 5 files changed, 202 insertions(+), 7 deletions(-) diff --git a/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.xml b/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.xml index 10d19a93c5..8e61e013de 100644 --- a/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.xml +++ b/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.xml @@ -1,6 +1,6 @@ + type="force_torque_sensor_broadcaster::ForceTorqueSensorBroadcaster" base_class_type="controller_interface::ChainableControllerInterface"> This controller publishes the readings of force-torque interfaces as geometry_msgs/WrenchStamped message. diff --git a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp index bd477ed68a..e5a5349c32 100644 --- a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp @@ -20,8 +20,9 @@ #define FORCE_TORQUE_SENSOR_BROADCASTER__FORCE_TORQUE_SENSOR_BROADCASTER_HPP_ #include +#include -#include "controller_interface/controller_interface.hpp" +#include "controller_interface/chainable_controller_interface.hpp" #include "force_torque_sensor_broadcaster/visibility_control.h" // auto-generated by generate_parameter_library #include "force_torque_sensor_broadcaster_parameters.hpp" @@ -32,7 +33,7 @@ namespace force_torque_sensor_broadcaster { -class ForceTorqueSensorBroadcaster : public controller_interface::ControllerInterface +class ForceTorqueSensorBroadcaster : public controller_interface::ChainableControllerInterface { public: FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC @@ -59,10 +60,19 @@ class ForceTorqueSensorBroadcaster : public controller_interface::ControllerInte const rclcpp_lifecycle::State & previous_state) override; FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC - controller_interface::return_type update( + controller_interface::return_type update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) override; + FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC + controller_interface::return_type update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC + std::vector on_export_state_interfaces() override; + protected: + void apply_sensor_offset(const Params & params, geometry_msgs::msg::WrenchStamped & msg); + std::shared_ptr param_listener_; Params params_; diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index 9b570d353f..ae105a511c 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -24,7 +24,7 @@ namespace force_torque_sensor_broadcaster { ForceTorqueSensorBroadcaster::ForceTorqueSensorBroadcaster() -: controller_interface::ControllerInterface() +: controller_interface::ChainableControllerInterface() { } @@ -141,23 +141,97 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_deactivate return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::return_type ForceTorqueSensorBroadcaster::update( +controller_interface::return_type ForceTorqueSensorBroadcaster::update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { + if (param_listener_->is_old(params_)) + { + params_ = param_listener_->get_params(); + } if (realtime_publisher_ && realtime_publisher_->trylock()) { realtime_publisher_->msg_.header.stamp = time; force_torque_sensor_->get_values_as_message(realtime_publisher_->msg_.wrench); + this->apply_sensor_offset(params_, realtime_publisher_->msg_); realtime_publisher_->unlockAndPublish(); } return controller_interface::return_type::OK; } +controller_interface::return_type ForceTorqueSensorBroadcaster::update_reference_from_subscribers( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + return controller_interface::return_type::OK; +} + +std::vector +ForceTorqueSensorBroadcaster::on_export_state_interfaces() +{ + std::vector exported_state_interfaces; + + std::vector force_names( + {params_.interface_names.force.x, params_.interface_names.force.y, + params_.interface_names.force.z}); + std::vector torque_names( + {params_.interface_names.torque.x, params_.interface_names.torque.y, + params_.interface_names.torque.z}); + if (!params_.sensor_name.empty()) + { + const auto semantic_comp_itf_names = force_torque_sensor_->get_state_interface_names(); + std::copy( + semantic_comp_itf_names.begin(), semantic_comp_itf_names.begin() + 3, force_names.begin()); + std::copy( + semantic_comp_itf_names.begin() + 3, semantic_comp_itf_names.end(), torque_names.begin()); + } + const std::string controller_name = get_node()->get_name(); + if (!force_names[0].empty()) + { + exported_state_interfaces.emplace_back(hardware_interface::StateInterface( + controller_name, force_names[0], &realtime_publisher_->msg_.wrench.force.x)); + } + if (!force_names[1].empty()) + { + exported_state_interfaces.emplace_back(hardware_interface::StateInterface( + controller_name, force_names[1], &realtime_publisher_->msg_.wrench.force.y)); + } + if (!force_names[2].empty()) + { + exported_state_interfaces.emplace_back(hardware_interface::StateInterface( + controller_name, force_names[2], &realtime_publisher_->msg_.wrench.force.z)); + } + if (!torque_names[0].empty()) + { + exported_state_interfaces.emplace_back(hardware_interface::StateInterface( + controller_name, torque_names[0], &realtime_publisher_->msg_.wrench.torque.x)); + } + if (!torque_names[1].empty()) + { + exported_state_interfaces.emplace_back(hardware_interface::StateInterface( + controller_name, torque_names[1], &realtime_publisher_->msg_.wrench.torque.y)); + } + if (!torque_names[2].empty()) + { + exported_state_interfaces.emplace_back(hardware_interface::StateInterface( + controller_name, torque_names[2], &realtime_publisher_->msg_.wrench.torque.z)); + } + return exported_state_interfaces; +} + +void ForceTorqueSensorBroadcaster::apply_sensor_offset( + const Params & params, geometry_msgs::msg::WrenchStamped & msg) +{ + msg.wrench.force.x += params.offset.force.x; + msg.wrench.force.y += params.offset.force.y; + msg.wrench.force.z += params.offset.force.z; + msg.wrench.torque.x += params.offset.torque.x; + msg.wrench.torque.y += params.offset.torque.y; + msg.wrench.torque.z += params.offset.torque.z; +} } // namespace force_torque_sensor_broadcaster #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( force_torque_sensor_broadcaster::ForceTorqueSensorBroadcaster, - controller_interface::ControllerInterface) + controller_interface::ChainableControllerInterface) diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml index 3e75ab6012..0869f5cf3c 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml @@ -46,3 +46,36 @@ force_torque_sensor_broadcaster: default_value: "", description: "Name of the state interface with torque values around 'z' axis.", } + offset: + force: + x: { + type: double, + default_value: 0.0, + description: "The offset of force values on 'x' axis.", + } + y: { + type: double, + default_value: 0.0, + description: "The offset of force values on 'y' axis.", + } + z: { + type: double, + default_value: 0.0, + description: "The offset of force values on 'z' axis.", + } + torque: + x: { + type: double, + default_value: 0.0, + description: "The offset of torque values around 'x' axis.", + } + y: { + type: double, + default_value: 0.0, + description: "The offset of torque values around 'y' axis.", + } + z: { + type: double, + default_value: 0.0, + description: "The offset of torque values around 'z' axis.", + } diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index 1ea25520cc..e436beb2e5 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -278,6 +278,60 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success) ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5]); } +TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Offsets) +{ + SetUpFTSBroadcaster(); + + std::array force_offsets = {10.0, 30.0, -50.0}; + std::array torque_offsets = {1.0, -1.2, -5.2}; + // set the params 'sensor_name' and 'frame_id' + fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + fts_broadcaster_->get_node()->set_parameter({"offset.force.x", force_offsets[0]}); + fts_broadcaster_->get_node()->set_parameter({"offset.force.y", force_offsets[1]}); + fts_broadcaster_->get_node()->set_parameter({"offset.force.z", force_offsets[2]}); + fts_broadcaster_->get_node()->set_parameter({"offset.torque.x", torque_offsets[0]}); + fts_broadcaster_->get_node()->set_parameter({"offset.torque.y", torque_offsets[1]}); + fts_broadcaster_->get_node()->set_parameter({"offset.torque.z", torque_offsets[2]}); + + ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + geometry_msgs::msg::WrenchStamped wrench_msg; + subscribe_and_get_message(wrench_msg); + + ASSERT_EQ(wrench_msg.header.frame_id, frame_id_); + ASSERT_EQ(wrench_msg.wrench.force.x, sensor_values_[0] + force_offsets[0]); + ASSERT_EQ(wrench_msg.wrench.force.y, sensor_values_[1] + force_offsets[1]); + ASSERT_EQ(wrench_msg.wrench.force.z, sensor_values_[2] + force_offsets[2]); + ASSERT_EQ(wrench_msg.wrench.torque.x, sensor_values_[3] + torque_offsets[0]); + ASSERT_EQ(wrench_msg.wrench.torque.y, sensor_values_[4] + torque_offsets[1]); + ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5] + torque_offsets[2]); + + // Check the exported state interfaces + const auto exported_state_interfaces = fts_broadcaster_->export_state_interfaces(); + ASSERT_EQ(exported_state_interfaces.size(), 6u); + const std::string controller_name = fts_broadcaster_->get_node()->get_name(); + ASSERT_EQ( + exported_state_interfaces[0]->get_name(), controller_name + "/" + sensor_name_ + "/force.x"); + ASSERT_EQ( + exported_state_interfaces[1]->get_name(), controller_name + "/" + sensor_name_ + "/force.y"); + ASSERT_EQ( + exported_state_interfaces[2]->get_name(), controller_name + "/" + sensor_name_ + "/force.z"); + ASSERT_EQ( + exported_state_interfaces[3]->get_name(), controller_name + "/" + sensor_name_ + "/torque.x"); + ASSERT_EQ( + exported_state_interfaces[4]->get_name(), controller_name + "/" + sensor_name_ + "/torque.y"); + ASSERT_EQ( + exported_state_interfaces[5]->get_name(), controller_name + "/" + sensor_name_ + "/torque.z"); + for (size_t i = 0; i < 6; ++i) + { + ASSERT_EQ( + exported_state_interfaces[i]->get_value(), + sensor_values_[i] + (i < 3 ? force_offsets[i] : torque_offsets[i - 3])); + } +} + TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success) { SetUpFTSBroadcaster(); @@ -300,6 +354,15 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success) ASSERT_TRUE(std::isnan(wrench_msg.wrench.torque.x)); ASSERT_TRUE(std::isnan(wrench_msg.wrench.torque.y)); ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5]); + + // Check the exported state interfaces + const auto exported_state_interfaces = fts_broadcaster_->export_state_interfaces(); + ASSERT_EQ(exported_state_interfaces.size(), 2u); + const std::string controller_name = fts_broadcaster_->get_node()->get_name(); + ASSERT_EQ(exported_state_interfaces[0]->get_name(), controller_name + "/fts_sensor/force.x"); + ASSERT_EQ(exported_state_interfaces[1]->get_name(), controller_name + "/fts_sensor/torque.z"); + ASSERT_EQ(exported_state_interfaces[0]->get_value(), sensor_values_[0]); + ASSERT_EQ(exported_state_interfaces[1]->get_value(), sensor_values_[5]); } TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success) @@ -328,6 +391,21 @@ TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success) ASSERT_EQ(wrench_msg.wrench.torque.x, sensor_values_[3]); ASSERT_EQ(wrench_msg.wrench.torque.y, sensor_values_[4]); ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5]); + + // Check the exported state interfaces + const auto exported_state_interfaces = fts_broadcaster_->export_state_interfaces(); + ASSERT_EQ(exported_state_interfaces.size(), 6u); + const std::string controller_name = fts_broadcaster_->get_node()->get_name(); + ASSERT_EQ(exported_state_interfaces[0]->get_name(), controller_name + "/fts_sensor/force.x"); + ASSERT_EQ(exported_state_interfaces[1]->get_name(), controller_name + "/fts_sensor/force.y"); + ASSERT_EQ(exported_state_interfaces[2]->get_name(), controller_name + "/fts_sensor/force.z"); + ASSERT_EQ(exported_state_interfaces[3]->get_name(), controller_name + "/fts_sensor/torque.x"); + ASSERT_EQ(exported_state_interfaces[4]->get_name(), controller_name + "/fts_sensor/torque.y"); + ASSERT_EQ(exported_state_interfaces[5]->get_name(), controller_name + "/fts_sensor/torque.z"); + for (size_t i = 0; i < 6; ++i) + { + ASSERT_EQ(exported_state_interfaces[i]->get_value(), sensor_values_[i]); + } } int main(int argc, char ** argv) From f2da66240082c1abf2e25a624640b148171e861d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 5 Nov 2024 16:50:59 +0100 Subject: [PATCH 02/84] Update ros2_controllers.humble.repos (#1350) --- ros2_controllers.humble.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_controllers.humble.repos b/ros2_controllers.humble.repos index 2a3b96551d..a0432f65b3 100644 --- a/ros2_controllers.humble.repos +++ b/ros2_controllers.humble.repos @@ -10,7 +10,7 @@ repositories: kinematics_interface: type: git url: https://github.com/ros-controls/kinematics_interface.git - version: master + version: humble control_msgs: type: git url: https://github.com/ros-controls/control_msgs.git From 9439764015ef4a8b245ed07381459c19883e5a2b Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 6 Nov 2024 22:47:17 +0100 Subject: [PATCH 03/84] Adding use of robot description parameter in the Admittance Controller (#1247) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --------- Co-authored-by: Kevin DeMarco Co-authored-by: Nikola Banovic Co-authored-by: Bence Magyar Co-authored-by: Christoph Fröhlich --- .../include/admittance_controller/admittance_rule.hpp | 3 ++- .../admittance_controller/admittance_rule_impl.hpp | 6 ++++-- admittance_controller/src/admittance_controller.cpp | 4 +++- .../test/test_admittance_controller.hpp | 9 +++------ 4 files changed, 12 insertions(+), 10 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 7223dbe9d1..a326b663d0 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -102,7 +102,8 @@ class AdmittanceRule /// Configure admittance rule memory using number of joints. controller_interface::return_type configure( - const std::shared_ptr & node, const size_t num_joint); + const std::shared_ptr & node, const size_t num_joint, + const std::string & robot_description); /// Reset all values back to default controller_interface::return_type reset(const size_t num_joints); diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index cab8b4cf45..13d4e67fbc 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -20,6 +20,7 @@ #include "admittance_controller/admittance_rule.hpp" #include +#include #include #include @@ -34,7 +35,8 @@ constexpr auto NUM_CARTESIAN_DOF = 6; // (3 translation + 3 rotation) /// Configure admittance rule memory for num joints and load kinematics interface controller_interface::return_type AdmittanceRule::configure( - const std::shared_ptr & node, const size_t num_joints) + const std::shared_ptr & node, const size_t num_joints, + const std::string & robot_description) { num_joints_ = num_joints; @@ -58,7 +60,7 @@ controller_interface::return_type AdmittanceRule::configure( kinematics_loader_->createUnmanagedInstance(parameters_.kinematics.plugin_name)); if (!kinematics_->initialize( - node->get_node_parameters_interface(), parameters_.kinematics.tip)) + robot_description, node->get_node_parameters_interface(), "kinematics")) { return controller_interface::return_type::ERROR; } diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index a4b56d739c..6e0e38140a 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -280,7 +280,9 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( semantic_components::ForceTorqueSensor(admittance_->parameters_.ft_sensor.name)); // configure admittance rule - if (admittance_->configure(get_node(), num_joints_) == controller_interface::return_type::ERROR) + if ( + admittance_->configure(get_node(), num_joints_, this->get_robot_description()) == + controller_interface::return_type::ERROR) { return controller_interface::CallbackReturn::ERROR; } diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index b2a95c12fa..4f4635d861 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -102,7 +102,6 @@ class TestableAdmittanceController : public admittance_controller::AdmittanceCon } } -private: const std::string robot_description_ = ros2_control_test_assets::valid_6d_robot_urdf; const std::string robot_description_semantic_ = ros2_control_test_assets::valid_6d_robot_srdf; }; @@ -110,10 +109,7 @@ class TestableAdmittanceController : public admittance_controller::AdmittanceCon class AdmittanceControllerTest : public ::testing::Test { public: - static void SetUpTestCase() - { - // rclcpp::init(0, nullptr); - } + static void SetUpTestCase() {} void SetUp() { @@ -163,7 +159,8 @@ class AdmittanceControllerTest : public ::testing::Test controller_interface::return_type SetUpControllerCommon( const std::string & controller_name, const rclcpp::NodeOptions & options) { - auto result = controller_->init(controller_name, "", 0, "", options); + auto result = + controller_->init(controller_name, controller_->robot_description_, 0, "", options); controller_->export_reference_interfaces(); assign_interfaces(); From b0391e2069c8ec9a94ce35cb3e395bac3ca62eb6 Mon Sep 17 00:00:00 2001 From: RobertWilbrandt Date: Thu, 7 Nov 2024 09:06:38 +0100 Subject: [PATCH 04/84] [jtc] Improve trajectory sampling efficiency (#1297) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --------- Co-authored-by: Christoph Fröhlich Co-authored-by: Bence Magyar --- .../trajectory.hpp | 21 +++- .../src/trajectory.cpp | 5 +- .../test/test_trajectory.cpp | 115 ++++++++++++++++++ 3 files changed, 137 insertions(+), 4 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index b00d79481c..14373b006e 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -69,6 +69,9 @@ class Trajectory * acceleration respectively. Deduction assumes that the provided velocity or acceleration have to * be reached at the time defined in the segment. * + * This function assumes that sampling is only done at monotonically increasing \p sample_time + * for any trajectory. + * * Specific case returns for start_segment_itr and end_segment_itr: * - Sampling before the trajectory start: * start_segment_itr = begin(), end_segment_itr = begin() @@ -85,9 +88,12 @@ class Trajectory * * \param[in] sample_time Time at which trajectory will be sampled. * \param[in] interpolation_method Specify whether splines, another method, or no interpolation at - * all. \param[out] expected_state Calculated new at \p sample_time. \param[out] start_segment_itr - * Iterator to the start segment for given \p sample_time. See description above. \param[out] - * end_segment_itr Iterator to the end segment for given \p sample_time. See description above. + * all. + * \param[out] output_state Calculated new at \p sample_time. + * \param[out] start_segment_itr Iterator to the start segment for given \p sample_time. See + * description above. + * \param[out] end_segment_itr Iterator to the end segment for given \p sample_time. See + * description above. */ JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool sample( @@ -147,6 +153,14 @@ class Trajectory JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool is_sampled_already() const { return sampled_already_; } + /// Get the index of the segment start returned by the last \p sample() operation. + /** + * As the trajectory is only accessed at monotonically increasing sampling times, this index is + * used to speed up the selection of relevant trajectory points. + */ + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + size_t last_sample_index() const { return last_sample_idx_; } + private: void deduce_from_derivatives( trajectory_msgs::msg::JointTrajectoryPoint & first_state, @@ -160,6 +174,7 @@ class Trajectory trajectory_msgs::msg::JointTrajectoryPoint state_before_traj_msg_; bool sampled_already_ = false; + size_t last_sample_idx_ = 0; }; /** diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index 54f785a070..512b16f3c5 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -84,6 +84,7 @@ void Trajectory::update(std::shared_ptr j trajectory_msg_ = joint_trajectory; trajectory_start_time_ = static_cast(joint_trajectory->header.stamp); sampled_already_ = false; + last_sample_idx_ = 0; } bool Trajectory::sample( @@ -149,7 +150,7 @@ bool Trajectory::sample( // time_from_start + trajectory time is the expected arrival time of trajectory const auto last_idx = trajectory_msg_->points.size() - 1; - for (size_t i = 0; i < last_idx; ++i) + for (size_t i = last_sample_idx_; i < last_idx; ++i) { auto & point = trajectory_msg_->points[i]; auto & next_point = trajectory_msg_->points[i + 1]; @@ -175,6 +176,7 @@ bool Trajectory::sample( } start_segment_itr = begin() + i; end_segment_itr = begin() + (i + 1); + last_sample_idx_ = i; return true; } } @@ -182,6 +184,7 @@ bool Trajectory::sample( // whole animation has played out start_segment_itr = --end(); end_segment_itr = end(); + last_sample_idx_ = last_idx; output_state = (*start_segment_itr); // the trajectories in msg may have empty velocities/accel, so resize them if (output_state.velocities.empty()) diff --git a/joint_trajectory_controller/test/test_trajectory.cpp b/joint_trajectory_controller/test/test_trajectory.cpp index b0d7ad8e18..44f511aa31 100644 --- a/joint_trajectory_controller/test/test_trajectory.cpp +++ b/joint_trajectory_controller/test/test_trajectory.cpp @@ -44,10 +44,12 @@ TEST(TestTrajectory, initialize_trajectory) empty_msg->header.stamp.nanosec = 2; const rclcpp::Time empty_time = empty_msg->header.stamp; auto traj = joint_trajectory_controller::Trajectory(empty_msg); + EXPECT_EQ(0, traj.last_sample_index()); trajectory_msgs::msg::JointTrajectoryPoint expected_point; joint_trajectory_controller::TrajectoryPointConstIter start, end; traj.sample(clock.now(), DEFAULT_INTERPOLATION, expected_point, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.end(), start); EXPECT_EQ(traj.end(), end); @@ -64,6 +66,7 @@ TEST(TestTrajectory, initialize_trajectory) trajectory_msgs::msg::JointTrajectoryPoint expected_point; joint_trajectory_controller::TrajectoryPointConstIter start, end; traj.sample(clock.now(), DEFAULT_INTERPOLATION, expected_point, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.end(), start); EXPECT_EQ(traj.end(), end); @@ -109,6 +112,7 @@ TEST(TestTrajectory, sample_trajectory_positions) // sample at trajectory starting time { traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -121,6 +125,7 @@ TEST(TestTrajectory, sample_trajectory_positions) bool result = traj.sample( time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(result, false); } @@ -129,6 +134,7 @@ TEST(TestTrajectory, sample_trajectory_positions) traj.sample( time_now + rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ(traj.begin(), end); double half_current_to_p1 = (point_before_msg.positions[0] + p1.positions[0]) * 0.5; @@ -142,6 +148,7 @@ TEST(TestTrajectory, sample_trajectory_positions) traj.sample( time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ((++traj.begin()), end); EXPECT_NEAR(p1.positions[0], expected_state.positions[0], EPS); @@ -154,6 +161,7 @@ TEST(TestTrajectory, sample_trajectory_positions) traj.sample( time_now + rclcpp::Duration::from_seconds(1.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ((++traj.begin()), end); double half_p1_to_p2 = (p1.positions[0] + p2.positions[0]) * 0.5; @@ -165,6 +173,7 @@ TEST(TestTrajectory, sample_trajectory_positions) traj.sample( time_now + rclcpp::Duration::from_seconds(2.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(1, traj.last_sample_index()); double half_p2_to_p3 = (p2.positions[0] + p3.positions[0]) * 0.5; EXPECT_NEAR(half_p2_to_p3, expected_state.positions[0], EPS); } @@ -174,6 +183,7 @@ TEST(TestTrajectory, sample_trajectory_positions) traj.sample( time_now + rclcpp::Duration::from_seconds(3.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); } @@ -182,6 +192,7 @@ TEST(TestTrajectory, sample_trajectory_positions) traj.sample( time_now + rclcpp::Duration::from_seconds(3.125), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); ASSERT_EQ((--traj.end()), start); ASSERT_EQ(traj.end(), end); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); @@ -193,6 +204,7 @@ TEST(TestTrajectory, sample_trajectory_positions) traj.sample( time_now + rclcpp::Duration::from_seconds(30.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); ASSERT_EQ((--traj.end()), start); ASSERT_EQ(traj.end(), end); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); @@ -350,6 +362,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) // sample at trajectory starting time { traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(traj.last_sample_index(), 0); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -362,6 +375,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) bool result = traj.sample( time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(result, false); } @@ -370,6 +384,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); double half_current_to_p1 = @@ -390,6 +405,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ((++traj.begin()), end); EXPECT_NEAR(position_first_seg, expected_state.positions[0], EPS); @@ -404,6 +420,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(1.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ((++traj.begin()), end); double half_p1_to_p2 = @@ -424,6 +441,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(2), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(1, traj.last_sample_index()); EXPECT_EQ((++traj.begin()), start); EXPECT_EQ((--traj.end()), end); EXPECT_NEAR(position_second_seg, expected_state.positions[0], EPS); @@ -438,6 +456,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(2.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(1, traj.last_sample_index()); EXPECT_EQ((++traj.begin()), start); EXPECT_EQ((--traj.end()), end); double half_p2_to_p3 = @@ -458,6 +477,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(3.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); EXPECT_EQ((--traj.end()), start); EXPECT_EQ(traj.end(), end); EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); @@ -471,6 +491,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(3.125), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); EXPECT_EQ((--traj.end()), start); EXPECT_EQ(traj.end(), end); EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); @@ -523,6 +544,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation_strange_witho // sample at trajectory starting time { traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -536,6 +558,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation_strange_witho bool result = traj.sample( time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(result, false); } @@ -544,6 +567,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation_strange_witho traj.sample( time_now + rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); // double half_current_to_p1 = point_before_msg.positions[0] + @@ -564,6 +588,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation_strange_witho traj.sample( time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ((++traj.begin()), end); EXPECT_NEAR(position_first_seg, expected_state.positions[0], EPS); @@ -615,6 +640,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) // sample at trajectory starting time { traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -628,6 +654,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) bool result = traj.sample( time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(result, false); } @@ -642,6 +669,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ((++traj.begin()), end); EXPECT_NEAR(position_first_seg, expected_state.positions[0], EPS); @@ -658,6 +686,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(2), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(1, traj.last_sample_index()); EXPECT_EQ((++traj.begin()), start); EXPECT_EQ((--traj.end()), end); EXPECT_NEAR(position_second_seg, expected_state.positions[0], EPS); @@ -674,6 +703,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(3.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); EXPECT_EQ((--traj.end()), start); EXPECT_EQ(traj.end(), end); EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); @@ -686,6 +716,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(3.125), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); EXPECT_EQ((--traj.end()), start); EXPECT_EQ(traj.end(), end); EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); @@ -732,6 +763,7 @@ TEST(TestTrajectory, skip_interpolation) // sample at trajectory starting time { traj.sample(time_now, no_interpolation, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -747,6 +779,7 @@ TEST(TestTrajectory, skip_interpolation) bool result = traj.sample( time_now - rclcpp::Duration::from_seconds(0.5), no_interpolation, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(result, false); } @@ -755,6 +788,7 @@ TEST(TestTrajectory, skip_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(0.5), no_interpolation, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ(traj.begin(), end); // For passthrough, this should just return the first waypoint @@ -771,6 +805,7 @@ TEST(TestTrajectory, skip_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(1.0), no_interpolation, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ((++traj.begin()), end); EXPECT_NEAR(p2.positions[0], expected_state.positions[0], EPS); @@ -786,6 +821,7 @@ TEST(TestTrajectory, skip_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(1.5), no_interpolation, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ((++traj.begin()), end); EXPECT_NEAR(p2.positions[0], expected_state.positions[0], EPS); @@ -796,6 +832,7 @@ TEST(TestTrajectory, skip_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(2.5), no_interpolation, expected_state, start, end); + EXPECT_EQ(1, traj.last_sample_index()); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); } @@ -804,6 +841,7 @@ TEST(TestTrajectory, skip_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(3.0), no_interpolation, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); } @@ -812,6 +850,7 @@ TEST(TestTrajectory, skip_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(3.125), no_interpolation, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); ASSERT_EQ((--traj.end()), start); ASSERT_EQ(traj.end(), end); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); @@ -819,6 +858,82 @@ TEST(TestTrajectory, skip_interpolation) } } +TEST(TestTrajectory, update_trajectory) +{ + // Verify that sampling works correctly after updating with a new trajectory + auto first_msg = std::make_shared(); + first_msg->header.stamp = rclcpp::Time(0); + + trajectory_msgs::msg::JointTrajectoryPoint p1; + p1.positions.push_back(1.0); + p1.time_from_start = rclcpp::Duration::from_seconds(1.0); + first_msg->points.push_back(p1); + + trajectory_msgs::msg::JointTrajectoryPoint p2; + p2.positions.push_back(2.0); + p2.time_from_start = rclcpp::Duration::from_seconds(2.0); + first_msg->points.push_back(p2); + + trajectory_msgs::msg::JointTrajectoryPoint p3; + p3.positions.push_back(3.0); + p3.time_from_start = rclcpp::Duration::from_seconds(3.0); + first_msg->points.push_back(p3); + + trajectory_msgs::msg::JointTrajectoryPoint point_before_msg; + point_before_msg.time_from_start = rclcpp::Duration::from_seconds(0.0); + point_before_msg.positions.push_back(0.0); + + const rclcpp::Time time_now = rclcpp::Clock().now(); + auto traj = joint_trajectory_controller::Trajectory(time_now, point_before_msg, first_msg); + EXPECT_EQ(0, traj.last_sample_index()); + + trajectory_msgs::msg::JointTrajectoryPoint expected_state; + joint_trajectory_controller::TrajectoryPointConstIter start, end; + + // Sample at starting time + traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); + + // Sample 2.5s after msg + traj.sample( + time_now + rclcpp::Duration::from_seconds(2.5), DEFAULT_INTERPOLATION, expected_state, start, + end); + EXPECT_EQ(1, traj.last_sample_index()); + + // Update trajectory + auto snd_msg = std::make_shared(); + snd_msg->header.stamp = rclcpp::Time(0); + + snd_msg->points.push_back(p1); + snd_msg->points.push_back(p2); + snd_msg->points.push_back(p3); + + traj.update(snd_msg); + + // Sample at starting time + { + traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); + EXPECT_EQ(traj.begin(), start); + EXPECT_EQ(traj.begin(), end); + EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); + EXPECT_NEAR( + (p1.positions[0] - point_before_msg.positions[0]), expected_state.velocities[0], EPS); + EXPECT_NEAR(0.0, expected_state.accelerations[0], EPS); + } + + // Sample 1.5s after msg + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(1.5), DEFAULT_INTERPOLATION, expected_state, start, + end); + EXPECT_EQ(0, traj.last_sample_index()); + EXPECT_EQ(traj.begin(), start); + EXPECT_EQ(std::next(traj.begin()), end); + EXPECT_NEAR((p1.positions[0] + p2.positions[0]) / 2, expected_state.positions[0], EPS); + } +} + TEST(TestWrapAroundJoint, no_wraparound) { const std::vector initial_position(3, 0.); From 7ed1a0ee49187139bcf2b574b0b7c992fe0d06a3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 8 Nov 2024 11:43:45 +0100 Subject: [PATCH 05/84] [JTC] Fix the JTC length_error exceptions in the tests (#1360) --- .../test/test_trajectory_controller_utils.hpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 796503c036..a850891331 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -26,6 +26,7 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/joint_trajectory_controller.hpp" #include "joint_trajectory_controller/tolerances.hpp" +#include "lifecycle_msgs/msg/state.hpp" #include "ros2_control_test_assets/descriptions.hpp" namespace @@ -391,6 +392,21 @@ class TrajectoryControllerTest : public ::testing::Test return traj_controller_->get_node()->activate(); } + void DeactivateTrajectoryController() + { + if (traj_controller_) + { + if ( + traj_controller_->get_lifecycle_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + EXPECT_EQ( + traj_controller_->get_node()->deactivate().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + } + } + } + static void TearDownTestCase() { rclcpp::shutdown(); } void subscribeToState(rclcpp::Executor & executor) @@ -776,6 +792,8 @@ class TrajectoryControllerTestParameterized state_interface_types_ = std::get<1>(GetParam()); } + virtual void TearDown() { TrajectoryControllerTest::DeactivateTrajectoryController(); } + static void TearDownTestCase() { TrajectoryControllerTest::TearDownTestCase(); } }; From 55ed331f761113be5f3acf3018ffde2f78872f6c Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 8 Nov 2024 10:56:32 +0000 Subject: [PATCH 06/84] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 3 + admittance_controller/CHANGELOG.rst | 5 + bicycle_steering_controller/CHANGELOG.rst | 3 + diff_drive_controller/CHANGELOG.rst | 3 + effort_controllers/CHANGELOG.rst | 3 + force_torque_sensor_broadcaster/CHANGELOG.rst | 5 + forward_command_controller/CHANGELOG.rst | 3 + gripper_controllers/CHANGELOG.rst | 3 + imu_sensor_broadcaster/CHANGELOG.rst | 3 + joint_state_broadcaster/CHANGELOG.rst | 5 + joint_trajectory_controller/CHANGELOG.rst | 8 + parallel_gripper_controller/CHANGELOG.rst | 3 + pid_controller/CHANGELOG.rst | 5 + pose_broadcaster/CHANGELOG.rst | 210 ++++++++++++++++++ pose_broadcaster/package.xml | 2 +- position_controllers/CHANGELOG.rst | 3 + range_sensor_broadcaster/CHANGELOG.rst | 3 + ros2_controllers/CHANGELOG.rst | 3 + ros2_controllers_test_nodes/CHANGELOG.rst | 3 + rqt_joint_trajectory_controller/CHANGELOG.rst | 3 + steering_controllers_library/CHANGELOG.rst | 3 + tricycle_controller/CHANGELOG.rst | 3 + tricycle_steering_controller/CHANGELOG.rst | 3 + velocity_controllers/CHANGELOG.rst | 3 + 24 files changed, 290 insertions(+), 1 deletion(-) create mode 100644 pose_broadcaster/CHANGELOG.rst diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index bc2997b8a9..8f13e3cb6f 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- * Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 46899f9cd6..bff4354f4a 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Adding use of robot description parameter in the Admittance Controller (`#1247 `_) +* Contributors: Dr. Denis, Kevin DeMarco, Nikola Banovic, Bence Magyar, Christoph Fröhlich + 4.15.0 (2024-10-07) ------------------- diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 0d096951ca..82a4e610bb 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- * Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 4c48fb234d..e806e497af 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index ea2efdcf4e..2cc63d7c90 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 7d2a7a1a8a..258b1d52a3 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [ForceTorqueSensorBroadcaster] added force and torque offsets to the parameters + export state interfaces (`#1215 `_) +* Contributors: Sai Kishor Kothakota + 4.15.0 (2024-10-07) ------------------- diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 54caa9d764..8130b880b3 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 11c61f86cf..5feab361b1 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 40af7a5360..214b914e08 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index cf422e3eb8..47ce981e63 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [JSB] Fix the behaviour of publishing unavailable state interfaces when they are previously available (`#1331 `_) +* Contributors: Sai Kishor Kothakota + 4.15.0 (2024-10-07) ------------------- diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 81696246b1..e873080f17 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [JTC] Fix the JTC length_error exceptions in the tests (`#1360 `_) +* [jtc] Improve trajectory sampling efficiency (`#1297 `_) +* fixes for windows compilation (`#1330 `_) +* [JTC] Add Parameter to Toggle State Setting on Activation (`#1231 `_) +* Contributors: Gilmar Correia, Kenta Kato, RobertWilbrandt, Sai Kishor Kothakota + 4.15.0 (2024-10-07) ------------------- diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst index a3024fbf62..da2098ecd0 100644 --- a/parallel_gripper_controller/CHANGELOG.rst +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package parallel_gripper_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 5a27471199..c4ce2430f6 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fixes for windows compilation (`#1330 `_) +* Contributors: Gilmar Correia + 4.15.0 (2024-10-07) ------------------- * Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) diff --git a/pose_broadcaster/CHANGELOG.rst b/pose_broadcaster/CHANGELOG.rst new file mode 100644 index 0000000000..729df5a2f9 --- /dev/null +++ b/pose_broadcaster/CHANGELOG.rst @@ -0,0 +1,210 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package pose_broadcaster +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Add hardware_interface_testing dependency (`#1335 `_) +* Implement new PoseBroadcaster controller (`#1311 `_) +* Contributors: Christoph Fröhlich, RobertWilbrandt + +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- + +4.12.1 (2024-08-14) +------------------- + +4.12.0 (2024-07-23) +------------------- + +4.11.0 (2024-07-09) +------------------- + +4.10.0 (2024-07-01) +------------------- + +4.9.0 (2024-06-05) +------------------ + +4.8.0 (2024-05-14) +------------------ + +4.7.0 (2024-03-22) +------------------ + +4.6.0 (2024-02-12) +------------------ + +4.5.0 (2024-01-31) +------------------ + +4.4.0 (2024-01-11) +------------------ + +4.3.0 (2024-01-08) +------------------ + +4.2.0 (2023-12-12) +------------------ + +4.1.0 (2023-12-01) +------------------ + +4.0.0 (2023-11-21) +------------------ + +3.17.0 (2023-10-31) +------------------- + +3.16.0 (2023-09-20) +------------------- + +3.15.0 (2023-09-11) +------------------- + +3.14.0 (2023-08-16) +------------------- + +3.13.0 (2023-08-04) +------------------- + +3.12.0 (2023-07-18) +------------------- + +3.11.0 (2023-06-24) +------------------- + +3.10.1 (2023-06-06) +------------------- + +3.10.0 (2023-06-04) +------------------- + +3.9.0 (2023-05-28) +------------------ + +3.8.0 (2023-05-14) +------------------ + +3.7.0 (2023-05-02) +------------------ + +3.6.0 (2023-04-29) +------------------ + +3.5.0 (2023-04-14) +------------------ + +3.4.0 (2023-04-02) +------------------ + +3.3.0 (2023-03-07) +------------------ + +3.2.0 (2023-02-10) +------------------ + +3.1.0 (2023-01-26) +------------------ + +3.0.0 (2023-01-19) +------------------ + +2.15.0 (2022-12-06) +------------------- + +2.14.0 (2022-11-18) +------------------- + +2.13.0 (2022-10-05) +------------------- + +2.12.0 (2022-09-01) +------------------- + +2.11.0 (2022-08-04) +------------------- + +2.10.0 (2022-08-01) +------------------- + +2.9.0 (2022-07-14) +------------------ + +2.8.0 (2022-07-09) +------------------ + +2.7.0 (2022-07-03) +------------------ + +2.6.0 (2022-06-18) +------------------ + +2.5.0 (2022-05-13) +------------------ + +2.4.0 (2022-04-29) +------------------ + +2.3.0 (2022-04-21) +------------------ + +2.2.0 (2022-03-25) +------------------ + +2.1.0 (2022-02-23) +------------------ + +2.0.1 (2022-02-01) +------------------ + +2.0.0 (2022-01-28) +------------------ + +1.3.0 (2022-01-11) +------------------ + +1.2.0 (2021-12-29) +------------------ + +1.1.0 (2021-10-25) +------------------ + +1.0.0 (2021-09-29) +------------------ + +0.5.0 (2021-08-30) +------------------ + +0.4.1 (2021-07-08) +------------------ + +0.4.0 (2021-06-28) +------------------ + +0.3.1 (2021-05-23) +------------------ + +0.3.0 (2021-05-21) +------------------ + +0.2.1 (2021-05-03) +------------------ + +0.2.0 (2021-02-06) +------------------ + +0.1.2 (2021-01-07) +------------------ + +0.1.1 (2021-01-06) +------------------ + +0.1.0 (2020-12-23) +------------------ diff --git a/pose_broadcaster/package.xml b/pose_broadcaster/package.xml index 4c3506f1cd..6a71e11c7e 100644 --- a/pose_broadcaster/package.xml +++ b/pose_broadcaster/package.xml @@ -2,7 +2,7 @@ pose_broadcaster - 0.0.0 + 4.15.0 Broadcaster to publish cartesian states. Robert Wilbrandt diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index d52201f2b4..eb6d8df132 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 77eae62d2a..c485b49efe 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index bd9e0a8b38..51c989d4f2 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 29ca6cd84d..7a9ab97dc6 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 043930c536..5db2db7bad 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 39360d6921..44fed2a58c 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- * Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 34520eeb83..c867767b9a 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 4e1f9abd1e..3542beded0 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- * Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index a04bf1c4ae..82ceab063a 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- From f519170c776649de11431addbfe5bba84ac3603d Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 8 Nov 2024 10:56:52 +0000 Subject: [PATCH 07/84] 4.16.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- parallel_gripper_controller/CHANGELOG.rst | 4 ++-- parallel_gripper_controller/package.xml | 2 +- pid_controller/CHANGELOG.rst | 4 ++-- pid_controller/package.xml | 2 +- pose_broadcaster/CHANGELOG.rst | 4 ++-- pose_broadcaster/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 48 files changed, 71 insertions(+), 71 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 8f13e3cb6f..1f275bd7ba 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 5764de56c1..128eabae4c 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.15.0 + 4.16.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Apache License 2.0 Bence Magyar diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index bff4354f4a..0d7daa5f5d 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- * Adding use of robot description parameter in the Admittance Controller (`#1247 `_) * Contributors: Dr. Denis, Kevin DeMarco, Nikola Banovic, Bence Magyar, Christoph Fröhlich diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 06fde3cb59..1a08bff889 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.15.0 + 4.16.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 82a4e610bb..57d23a977a 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index f58cf42dff..c87e4e6642 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.15.0 + 4.16.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index e806e497af..e79f1faf1d 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 45ab80d5cb..84f51a0750 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.15.0 + 4.16.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 2cc63d7c90..0abc5feee9 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index fab6d671ad..36ab9ce79e 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.15.0 + 4.16.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 258b1d52a3..6bd9e2febd 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- * [ForceTorqueSensorBroadcaster] added force and torque offsets to the parameters + export state interfaces (`#1215 `_) * Contributors: Sai Kishor Kothakota diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 1383342246..237cb2c888 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.15.0 + 4.16.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 8130b880b3..ae920d9f83 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index f9f30dd053..8a02785a3a 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.15.0 + 4.16.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 5feab361b1..291cab691a 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index d46c98f608..76877ff6fa 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 4.15.0 + 4.16.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 214b914e08..2219d53fe4 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index ed1f794afa..9de0f9c316 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.15.0 + 4.16.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 47ce981e63..ed2f4b31eb 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- * [JSB] Fix the behaviour of publishing unavailable state interfaces when they are previously available (`#1331 `_) * Contributors: Sai Kishor Kothakota diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index c3a1e66574..9e5412881c 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.15.0 + 4.16.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index e873080f17..87089f700a 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- * [JTC] Fix the JTC length_error exceptions in the tests (`#1360 `_) * [jtc] Improve trajectory sampling efficiency (`#1297 `_) * fixes for windows compilation (`#1330 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 112c4ad9a1..c21db502ec 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.15.0 + 4.16.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Dr. Denis Štogl diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst index da2098ecd0..6ee5646ac4 100644 --- a/parallel_gripper_controller/CHANGELOG.rst +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package parallel_gripper_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/parallel_gripper_controller/package.xml b/parallel_gripper_controller/package.xml index b396ba7b85..5fbb69303e 100644 --- a/parallel_gripper_controller/package.xml +++ b/parallel_gripper_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> parallel_gripper_controller - 4.15.0 + 4.16.0 The parallel_gripper_controller package Bence Magyar diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index c4ce2430f6..9c20af4e97 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- * fixes for windows compilation (`#1330 `_) * Contributors: Gilmar Correia diff --git a/pid_controller/package.xml b/pid_controller/package.xml index 79a66d3cc3..0a22ecfe7b 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.15.0 + 4.16.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar Denis Štogl diff --git a/pose_broadcaster/CHANGELOG.rst b/pose_broadcaster/CHANGELOG.rst index 729df5a2f9..c12bc9da80 100644 --- a/pose_broadcaster/CHANGELOG.rst +++ b/pose_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pose_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- * Add hardware_interface_testing dependency (`#1335 `_) * Implement new PoseBroadcaster controller (`#1311 `_) * Contributors: Christoph Fröhlich, RobertWilbrandt diff --git a/pose_broadcaster/package.xml b/pose_broadcaster/package.xml index 6a71e11c7e..01c91f21f2 100644 --- a/pose_broadcaster/package.xml +++ b/pose_broadcaster/package.xml @@ -2,7 +2,7 @@ pose_broadcaster - 4.15.0 + 4.16.0 Broadcaster to publish cartesian states. Robert Wilbrandt diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index eb6d8df132..e5478f518a 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/position_controllers/package.xml b/position_controllers/package.xml index c03888eaf8..17bf0b736e 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.15.0 + 4.16.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index c485b49efe..5d97df0e0b 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 92283fe3ad..3b4276951f 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.15.0 + 4.16.0 Controller to publish readings of Range sensors. Bence Magyar Florent Chretien diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 51c989d4f2..a45a425f24 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 0886b489c7..6eb4144647 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.15.0 + 4.16.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 7a9ab97dc6..d3c3059abf 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index f61a51e34e..1ad38581a2 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.15.0 + 4.16.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index 59aee66875..fb26e9c5f7 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.15.0", + version="4.16.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 5db2db7bad..555a636d7e 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 7920e63246..5954bcd03d 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 4.15.0 + 4.16.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index a70342e154..f2ee71c3d7 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -21,7 +21,7 @@ setup( name=package_name, - version="4.15.0", + version="4.16.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 44fed2a58c..417c4c9f05 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 5c96c4624c..93d304637d 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.15.0 + 4.16.0 Package for steering robot configurations including odometry and interfaces. Apache License 2.0 Bence Magyar diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index c867767b9a..31258cac7c 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 4e6ab510f3..568bd5cb46 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.15.0 + 4.16.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 3542beded0..975c48303d 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 27aac82dd1..66c5456b9b 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.15.0 + 4.16.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 82ceab063a..90b143f675 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index ea295db0ed..60ca9c693c 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.15.0 + 4.16.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From de77fd0fad6cd85ab23027c1b4d685b2f8085526 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 12 Nov 2024 16:48:15 +0100 Subject: [PATCH 08/84] Update maintainers and add url tags (#1363) --- ackermann_steering_controller/package.xml | 13 ++++++++++--- admittance_controller/package.xml | 15 +++++++++++++-- bicycle_steering_controller/package.xml | 13 ++++++++++--- diff_drive_controller/package.xml | 16 ++++++++++++++-- effort_controllers/package.xml | 11 ++++++++++- force_torque_sensor_broadcaster/package.xml | 14 ++++++++++++-- forward_command_controller/package.xml | 11 ++++++++++- gripper_controllers/package.xml | 9 +++++++++ imu_sensor_broadcaster/package.xml | 13 +++++++++++-- joint_state_broadcaster/package.xml | 14 +++++++++++--- joint_trajectory_controller/package.xml | 8 +++++++- parallel_gripper_controller/package.xml | 9 +++++++++ pid_controller/package.xml | 13 +++++++++++-- pose_broadcaster/package.xml | 12 +++++++++++- position_controllers/package.xml | 11 ++++++++++- range_sensor_broadcaster/package.xml | 14 ++++++++++++-- ros2_controllers/package.xml | 9 +++++++-- ros2_controllers_test_nodes/package.xml | 10 ++++++++-- rqt_joint_trajectory_controller/package.xml | 9 ++++++++- steering_controllers_library/package.xml | 14 ++++++++++---- tricycle_controller/package.xml | 11 ++++++++++- tricycle_steering_controller/package.xml | 14 ++++++++++---- velocity_controllers/package.xml | 11 ++++++++++- 23 files changed, 233 insertions(+), 41 deletions(-) diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 128eabae4c..6ae2f0a438 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -4,10 +4,17 @@ ackermann_steering_controller 4.16.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. - Apache License 2.0 + Bence Magyar - Dr.-Ing. Denis Štogl - dr. sc. Tomislav Petkovic + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ Dr.-Ing. Denis Štogl dr. sc. Tomislav Petkovic diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 1a08bff889..5b48c69695 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -4,11 +4,22 @@ admittance_controller 4.16.0 Implementation of admittance controllers for different input and output interface. - Denis Štogl + Bence Magyar - Andy Zelenak + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Denis Štogl + Andy Zelenak + Paul Gesel + ament_cmake backward_ros diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index c87e4e6642..3fafbda077 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -4,10 +4,17 @@ bicycle_steering_controller 4.16.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. - Apache License 2.0 + Bence Magyar - Dr.-Ing. Denis Štogl - dr. sc. Tomislav Petkovic + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ Dr.-Ing. Denis Štogl dr. sc. Tomislav Petkovic diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 84f51a0750..0602d91898 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -2,12 +2,24 @@ diff_drive_controller 4.16.0 - Controller for a differential drive mobile base. + Controller for a differential-drive mobile base. + Bence Magyar - Jordan Palacios + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Bence Magyar + Enrique Fernández + Manuel Meraz + Jordan Palacios + ament_cmake generate_parameter_library diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 36ab9ce79e..b33224db9b 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -3,11 +3,20 @@ effort_controllers 4.16.0 Generic controller for forwarding commands. + Bence Magyar - Jordan Palacios + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Jordan Palacios + ament_cmake backward_ros diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 237cb2c888..f2f5124001 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -4,11 +4,21 @@ force_torque_sensor_broadcaster 4.16.0 Controller to publish state of force-torque sensors. + Bence Magyar - Denis Štogl - Subhas Das + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Denis Štogl + Subhas Das + ament_cmake backward_ros diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 8a02785a3a..be71c32052 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -3,11 +3,20 @@ forward_command_controller 4.16.0 Generic controller for forwarding commands. + Bence Magyar - Jordan Palacios + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Jordan Palacios + ament_cmake backward_ros diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 76877ff6fa..33f06f226a 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -6,11 +6,20 @@ gripper_controllers 4.16.0 The gripper_controllers package + Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + Sachin Chitta + Jafar Abdi ament_cmake diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 9de0f9c316..a09f09dee2 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -4,11 +4,20 @@ imu_sensor_broadcaster 4.16.0 Controller to publish readings of IMU sensors. + Bence Magyar - Denis Štogl - Victor Lopez + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Victor Lopez + ament_cmake backward_ros diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 9e5412881c..575c17c5a0 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -3,13 +3,21 @@ joint_state_broadcaster 4.16.0 Broadcaster to publish joint state + Bence Magyar - Denis Stogl - Jordan Palacios - Karsten Knese + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Jordan Palacios + Karsten Knese + ament_cmake backward_ros diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index c21db502ec..830321e7b6 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -3,12 +3,18 @@ joint_trajectory_controller 4.16.0 Controller for executing joint-space trajectories on a group of joints + Bence Magyar - Dr. Denis Štogl + Denis Štogl Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + ament_cmake angles diff --git a/parallel_gripper_controller/package.xml b/parallel_gripper_controller/package.xml index 5fbb69303e..7e712b632a 100644 --- a/parallel_gripper_controller/package.xml +++ b/parallel_gripper_controller/package.xml @@ -6,11 +6,20 @@ parallel_gripper_controller 4.16.0 The parallel_gripper_controller package + Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + Sachin Chitta + Paul Gesel ament_cmake diff --git a/pid_controller/package.xml b/pid_controller/package.xml index 0a22ecfe7b..fb3f40184a 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -4,10 +4,19 @@ pid_controller 4.16.0 Controller based on PID implememenation from control_toolbox package. + Bence Magyar - Denis Štogl + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + Denis Štogl - Apache-2.0 ament_cmake diff --git a/pose_broadcaster/package.xml b/pose_broadcaster/package.xml index 01c91f21f2..dc5d629b50 100644 --- a/pose_broadcaster/package.xml +++ b/pose_broadcaster/package.xml @@ -4,10 +4,20 @@ pose_broadcaster 4.16.0 Broadcaster to publish cartesian states. - Robert Wilbrandt + + Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Robert Wilbrandt + ament_cmake backward_ros diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 17bf0b736e..8aa7bf8117 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -3,11 +3,20 @@ position_controllers 4.16.0 Generic controller for forwarding commands. + Bence Magyar - Jordan Palacios + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Jordan Palacios + ament_cmake backward_ros diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 3b4276951f..a81823a6a2 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -3,11 +3,21 @@ range_sensor_broadcaster 4.16.0 - Controller to publish readings of Range sensors. + Controller to publish readings of range sensors. + Bence Magyar - Florent Chretien + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Florent Chretien + ament_cmake backward_ros diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 6eb4144647..ccde6cf812 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -2,13 +2,18 @@ ros2_controllers 4.16.0 - Metapackage for ROS2 controllers related packages + Metapackage for ros2_controllers related packages + Bence Magyar - Jordan Palacios + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ ament_cmake diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 1ad38581a2..c53f1b9971 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -5,10 +5,16 @@ 4.16.0 Demo nodes for showing and testing functionalities of the ros2_control framework. - Denis Štogl Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Apache License 2.0 - Apache-2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ rclpy std_msgs diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 5954bcd03d..da92d9d663 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -8,8 +8,15 @@ Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota - Apache-2.0 + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ Adolfo Rodriguez Tsouroukdissian Noel Jimenez Garcia diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 93d304637d..efe9f93a67 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -4,11 +4,17 @@ steering_controllers_library 4.16.0 Package for steering robot configurations including odometry and interfaces. - Apache License 2.0 + Bence Magyar - Dr.-Ing. Denis Štogl - dr. sc. Tomislav Petkovic - Tony Najjar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ Dr.-Ing. Denis Štogl dr. sc. Tomislav Petkovic diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 568bd5cb46..516adef248 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -4,9 +4,18 @@ tricycle_controller 4.16.0 Controller for a tricycle drive mobile base + Bence Magyar - Tony Najjar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + Tony Najjar ament_cmake diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 66c5456b9b..7c26e09ba0 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -4,11 +4,17 @@ tricycle_steering_controller 4.16.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. - Apache License 2.0 + Bence Magyar - Dr.-Ing. Denis Štogl - dr. sc. Tomislav Petkovic - Tony Najjar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ Dr.-Ing. Denis Štogl dr. sc. Tomislav Petkovic diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 60ca9c693c..50e662cc58 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -3,11 +3,20 @@ velocity_controllers 4.16.0 Generic controller for forwarding commands. + Bence Magyar - Jordan Palacios + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Jordan Palacios + ament_cmake backward_ros From 28dd7e12987bad86e131c0f2c5684f8c84a61c59 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 12 Nov 2024 16:53:32 +0100 Subject: [PATCH 09/84] TractionLimiter: Fix wrong input checks (#1341) --- tricycle_controller/CMakeLists.txt | 6 + .../tricycle_controller/traction_limiter.hpp | 15 +- tricycle_controller/src/traction_limiter.cpp | 30 +- .../test/test_traction_limiter.cpp | 546 ++++++++++++++++++ 4 files changed, 590 insertions(+), 7 deletions(-) create mode 100644 tricycle_controller/test/test_traction_limiter.cpp diff --git a/tricycle_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt index 6834b753a0..7afe150c43 100644 --- a/tricycle_controller/CMakeLists.txt +++ b/tricycle_controller/CMakeLists.txt @@ -85,6 +85,12 @@ if(BUILD_TESTING) controller_manager ros2_control_test_assets ) + + ament_add_gmock(test_traction_limiter + test/test_traction_limiter.cpp) + target_link_libraries(test_traction_limiter + tricycle_controller + ) endif() install( diff --git a/tricycle_controller/include/tricycle_controller/traction_limiter.hpp b/tricycle_controller/include/tricycle_controller/traction_limiter.hpp index ea0bb16025..437f6cd4c2 100644 --- a/tricycle_controller/include/tricycle_controller/traction_limiter.hpp +++ b/tricycle_controller/include/tricycle_controller/traction_limiter.hpp @@ -28,14 +28,25 @@ class TractionLimiter public: /** * \brief Constructor + * + * Parameters are applied symmetrically for both directions, i.e., are applied + * to the absolute value of the corresponding quantity. + * + * \warning + * - Setting min_velocity: the robot can't stand still + * + * - Setting min_deceleration/min_acceleration: the robot can't move with constant velocity + * + * - Setting min_jerk: the robot can't move with constant acceleration + * * \param [in] min_velocity Minimum velocity [m/s] or [rad/s] * \param [in] max_velocity Maximum velocity [m/s] or [rad/s] * \param [in] min_acceleration Minimum acceleration [m/s^2] or [rad/s^2] * \param [in] max_acceleration Maximum acceleration [m/s^2] or [rad/s^2] * \param [in] min_deceleration Minimum deceleration [m/s^2] or [rad/s^2] * \param [in] max_deceleration Maximum deceleration [m/s^2] or [rad/s^2] - * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0 - * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0 + * \param [in] min_jerk Minimum jerk [m/s^3] + * \param [in] max_jerk Maximum jerk [m/s^3] */ TractionLimiter( double min_velocity = NAN, double max_velocity = NAN, double min_acceleration = NAN, diff --git a/tricycle_controller/src/traction_limiter.cpp b/tricycle_controller/src/traction_limiter.cpp index a8019714e1..e31e5e1e82 100644 --- a/tricycle_controller/src/traction_limiter.cpp +++ b/tricycle_controller/src/traction_limiter.cpp @@ -44,13 +44,13 @@ TractionLimiter::TractionLimiter( if (!std::isnan(max_acceleration_) && std::isnan(min_acceleration_)) min_acceleration_ = 0.0; if (!std::isnan(min_deceleration_) && std::isnan(max_deceleration_)) max_deceleration_ = 1000.0; - if (!std::isnan(max_deceleration_) && std::isnan(min_acceleration_)) min_deceleration_ = 0.0; + if (!std::isnan(max_deceleration_) && std::isnan(min_deceleration_)) min_deceleration_ = 0.0; if (!std::isnan(min_jerk_) && std::isnan(max_jerk_)) max_jerk_ = 1000.0; if (!std::isnan(max_jerk_) && std::isnan(min_jerk_)) min_jerk_ = 0.0; const std::string error = - "The positive limit will be applied to both directions. Setting different limits for positive " + " The positive limit will be applied to both directions. Setting different limits for positive " "and negative directions is not supported. Actuators are " "assumed to have the same constraints in both directions"; if (min_velocity_ < 0 || max_velocity_ < 0) @@ -58,19 +58,39 @@ TractionLimiter::TractionLimiter( throw std::invalid_argument("Velocity cannot be negative." + error); } + if (min_velocity_ > max_velocity_) + { + throw std::invalid_argument("Min velocity cannot be greater than max velocity."); + } + if (min_acceleration_ < 0 || max_acceleration_ < 0) { - throw std::invalid_argument("Acceleration cannot be negative." + error); + throw std::invalid_argument("Acceleration limits cannot be negative." + error); + } + + if (min_acceleration_ > max_acceleration_) + { + throw std::invalid_argument("Min acceleration cannot be greater than max acceleration."); } if (min_deceleration_ < 0 || max_deceleration_ < 0) { - throw std::invalid_argument("Deceleration cannot be negative." + error); + throw std::invalid_argument("Deceleration limits cannot be negative." + error); + } + + if (min_deceleration_ > max_deceleration_) + { + throw std::invalid_argument("Min deceleration cannot be greater than max deceleration."); } if (min_jerk_ < 0 || max_jerk_ < 0) { - throw std::invalid_argument("Jerk cannot be negative." + error); + throw std::invalid_argument("Jerk limits cannot be negative." + error); + } + + if (min_jerk_ > max_jerk_) + { + throw std::invalid_argument("Min jerk cannot be greater than max jerk."); } } diff --git a/tricycle_controller/test/test_traction_limiter.cpp b/tricycle_controller/test/test_traction_limiter.cpp new file mode 100644 index 0000000000..4afed7f217 --- /dev/null +++ b/tricycle_controller/test/test_traction_limiter.cpp @@ -0,0 +1,546 @@ +// Copyright 2024 AIT - Austrian Institute of Technology GmbH +// +// 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. + +#include +#include + +#include "tricycle_controller/traction_limiter.hpp" + +TEST(SpeedLimiterTest, testWrongParams) +{ + EXPECT_NO_THROW(tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + )); + + // velocity + { + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + -10., // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + -10., // min_velocity + -20., // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + -10., // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + 20., // min_velocity + 10., // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + } + + // acceleration + { + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + -10., // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + -10., // min_acceleration + -20., // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + -10., // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + 20., // min_acceleration + 10., // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + } + + // deceleration + { + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + -10., // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + -10., // min_deceleration + -20., // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + -10., // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + 20., // min_deceleration + 10., // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + } + + // jerk + { + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + -10., // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + -10., // min_jerk + -20. // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + -10. // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + 20., // min_jerk + 10. // max_jerk + ), + std::invalid_argument); + } +} + +TEST(SpeedLimiterTest, testNoLimits) +{ + tricycle_controller::TractionLimiter limiter; + double v = 10.0; + double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // check if the velocity is not limited + EXPECT_DOUBLE_EQ(v, 10.0); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); + v = -10.0; + limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // check if the velocity is not limited + EXPECT_DOUBLE_EQ(v, -10.0); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); +} + +TEST(SpeedLimiterTest, testVelocityLimits) +{ + tricycle_controller::TractionLimiter limiter(0.5, 1.0, 0.5, 1.0, 2.0, 3.0, 0.5, 5.0); + { + double v = 10.0; + double limiting_factor = limiter.limit_velocity(v); + // check if the robot speed is now 1.0 m.s-1, the limit + EXPECT_DOUBLE_EQ(v, 1.0); + EXPECT_DOUBLE_EQ(limiting_factor, 0.1); + + v = 0.1; + limiting_factor = limiter.limit_velocity(v); + // check if the robot speed is now 0.5 m.s-1, the limit + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 0.1); + + // TODO(christophfroehlich): does this behavior make sense? + v = 0.0; + limiting_factor = limiter.limit_velocity(v); + // check if the robot speed is now 0.5 m.s-1, the limit + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); // div by 0! + + v = -10.0; + limiting_factor = limiter.limit_velocity(v); + // check if the robot speed is now -1.0 m.s-1, the limit + EXPECT_DOUBLE_EQ(v, -1.0); + EXPECT_DOUBLE_EQ(limiting_factor, -1.0 / -10.0); + + v = -0.1; + limiting_factor = limiter.limit_velocity(v); + // check if the robot speed is now -0.5 m.s-1, the limit + EXPECT_DOUBLE_EQ(v, -0.5); + EXPECT_DOUBLE_EQ(limiting_factor, -0.5 / -0.1); + } + + { + double v = 10.0; + double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // acceleration is now limiting, not velocity + // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + + v = -10.0; + limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // acceleration is now limiting, not velocity + // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + } +} + +TEST(SpeedLimiterTest, testVelocityNoLimits) +{ + { + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, 0.5, 5.0); + double v = 10.0; + double limiting_factor = limiter.limit_velocity(v); + // check if the velocity is not limited + EXPECT_DOUBLE_EQ(v, 10.0); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); + + v = -10.0; + limiting_factor = limiter.limit_velocity(v); + // check if the velocity is not limited + EXPECT_DOUBLE_EQ(v, -10.0); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); + } + + { + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, 0.5, 5.0); + double v = 10.0; + double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // acceleration is now limiting, not velocity + // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + + v = -10.0; + limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // acceleration is now limiting, not velocity + // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + } + + { + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 5.0); + double v = 10.0; + double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // jerk is now limiting, not velocity + EXPECT_DOUBLE_EQ(v, 2.5); + EXPECT_DOUBLE_EQ(limiting_factor, 2.5 / 10.0); + + v = -10.0; + limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // jerk is now limiting, not velocity + EXPECT_DOUBLE_EQ(v, -2.5); + EXPECT_DOUBLE_EQ(limiting_factor, 2.5 / 10.0); + } +} + +TEST(SpeedLimiterTest, testAccelerationLimits) +{ + { + // test max_acceleration + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, 0.5, 5.0); + + double v = 10.0; + double limiting_factor = limiter.limit_acceleration(v, 0.0, 0.5); + // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + + v = -10.0; + limiting_factor = limiter.limit_acceleration(v, 0.0, 0.5); + // check if the robot speed is now -0.5 m.s-1, which is -1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + } + { + // test min_acceleration + // TODO(christophfroehlich): does this behavior make sense? + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()); + double v = 0.0; + double limiting_factor = limiter.limit_acceleration(v, 0.0, 0.5); + // check if the robot speed is now 0.25m.s-1 = 0.5m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 0.25); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); // div by 0! + + v = -std::numeric_limits::epsilon(); + limiting_factor = limiter.limit_acceleration(v, 0.0, 0.5); + // check if the robot speed is now -0.25m.s-1 = -0.5m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -0.25); + } + + { + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, 0.5, 5.0); + + double v = 10.0; + double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + + v = -10.0; + limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // check if the robot speed is now -0.5 m.s-1, which is -1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + } +} + +TEST(SpeedLimiterTest, testDecelerationLimits) +{ + { + // test max_deceleration + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, 0.5, 5.0); + + double v = 0.0; + double limiting_factor = limiter.limit_acceleration(v, 10.0, 0.5); + // check if the robot speed is now 8.5 m.s-1, which is 10.0 - 3.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 8.5); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); // div by 0! + + v = 0.0; + limiting_factor = limiter.limit_acceleration(v, -10.0, 0.5); + // check if the robot speed is now -8.5 m.s-1, which is -10.0 + 3.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -8.5); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); // div by 0! + } + { + // test min_deceleration + // TODO(christophfroehlich): does this behavior make sense? + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()); + double v = 9.9; + limiter.limit_acceleration(v, 10.0, 0.5); + // check if the robot speed is now 9.0m.s-1 = 10 - 2.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 9.0); + + v = -9.9; + limiter.limit_acceleration(v, -10., 0.5); + // check if the robot speed is now -9.0m.s-1 = -10 + 2.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -9.0); + } + { + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, 0.5, 5.0); + double v = 0.0; + double limiting_factor = limiter.limit(v, 10.0, 10.0, 0.5); + // check if the robot speed is now 8.5 m.s-1, which is 10.0 - 3.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 8.5); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); // div by 0! + + v = 0.0; + limiting_factor = limiter.limit(v, -10.0, -10.0, 0.5); + // check if the robot speed is now -8.5 m.s-1, which is -10.0 + 3.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -8.5); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); // div by 0! + } +} + +TEST(SpeedLimiterTest, testJerkLimits) +{ + { + // test max_jerk + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 5.0); + double v = 10.0; + double limiting_factor = limiter.limit_jerk(v, 0.0, 0.0, 0.5); + // check if the robot speed is now 2.5m.s-1 = 5.0m.s-3 * 2 * 0.5s * 0.5s + EXPECT_DOUBLE_EQ(v, 2.5); + EXPECT_DOUBLE_EQ(limiting_factor, 2.5 / 10.0); + + v = -10.0; + limiting_factor = limiter.limit_jerk(v, 0.0, 0.0, 0.5); + // check if the robot speed is now -2.5m.s-1 = -5.0m.s-3 * 2 * 0.5s * 0.5s + EXPECT_DOUBLE_EQ(v, -2.5); + EXPECT_DOUBLE_EQ(limiting_factor, 2.5 / 10.0); + } + { + // test min_jerk + // TODO(christophfroehlich): does this behavior make sense? + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 5.0); + double v = 0.0; + double limiting_factor = limiter.limit_jerk(v, 0.0, 0.0, 0.5); + // check if the robot speed is now 0.25m.s-1 = 0.5m.s-3 * 2 * 0.5s * 0.5s + EXPECT_DOUBLE_EQ(v, 0.25); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); // div by 0! + + v = -std::numeric_limits::epsilon(); + limiting_factor = limiter.limit_jerk(v, 0.0, 0.0, 0.5); + // check if the robot speed is now -0.25m.s-1 = -0.5m.s-3 * 2 * 0.5s * 0.5s + EXPECT_DOUBLE_EQ(v, -0.25); + } + { + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, 0.5, 5.0); + // acceleration is limiting, not jerk + + double v = 10.0; + double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // check if the robot speed is now 0.5 m.s-1, which is -1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + + v = -10.0; + limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // check if the robot speed is now -0.5 m.s-1, which is -1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + } +} From 2e5791759c1f02d831766fc60a7af37d65c78ecb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 18 Nov 2024 13:17:13 +0100 Subject: [PATCH 10/84] test_nodes: catch keyboard interrupt and add simple launch tests (#1369) --- .../publisher_forward_position_controller.py | 13 ++- .../publisher_joint_trajectory_controller.py | 13 ++- ros2_controllers_test_nodes/setup.py | 3 +- .../rrbot_forward_position_publisher.yaml | 11 ++ .../rrbot_joint_trajectory_publisher.yaml | 24 ++++ ...sher_forward_position_controller_launch.py | 107 ++++++++++++++++++ ...sher_joint_trajectory_controller_launch.py | 106 +++++++++++++++++ 7 files changed, 269 insertions(+), 8 deletions(-) create mode 100644 ros2_controllers_test_nodes/test/rrbot_forward_position_publisher.yaml create mode 100644 ros2_controllers_test_nodes/test/rrbot_joint_trajectory_publisher.yaml create mode 100644 ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py create mode 100644 ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py diff --git a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py index bb6add77ef..51582a90d4 100644 --- a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py +++ b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py @@ -68,9 +68,16 @@ def main(args=None): publisher_forward_position = PublisherForwardPosition() - rclpy.spin(publisher_forward_position) - publisher_forward_position.destroy_node() - rclpy.shutdown() + try: + rclpy.spin(publisher_forward_position) + except KeyboardInterrupt: + print("Keyboard interrupt received. Shutting down node.") + except Exception as e: + print(f"Unhandled exception: {e}") + finally: + if rclpy.ok(): + publisher_forward_position.destroy_node() + rclpy.shutdown() if __name__ == "__main__": diff --git a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py index 27f28da1be..91d39855aa 100644 --- a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py +++ b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py @@ -184,9 +184,16 @@ def main(args=None): publisher_joint_trajectory = PublisherJointTrajectory() - rclpy.spin(publisher_joint_trajectory) - publisher_joint_trajectory.destroy_node() - rclpy.shutdown() + try: + rclpy.spin(publisher_joint_trajectory) + except KeyboardInterrupt: + print("Keyboard interrupt received. Shutting down node.") + except Exception as e: + print(f"Unhandled exception: {e}") + finally: + if rclpy.ok(): + publisher_joint_trajectory.destroy_node() + rclpy.shutdown() if __name__ == "__main__": diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index fb26e9c5f7..3900392a10 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -25,8 +25,7 @@ data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), ("share/" + package_name, ["package.xml"]), - ("share/" + package_name, glob("launch/*.launch.py")), - ("share/" + package_name + "/configs", glob("configs/*.*")), + ("share/" + package_name + "/test", glob("test/*.yaml")), ], install_requires=["setuptools"], zip_safe=True, diff --git a/ros2_controllers_test_nodes/test/rrbot_forward_position_publisher.yaml b/ros2_controllers_test_nodes/test/rrbot_forward_position_publisher.yaml new file mode 100644 index 0000000000..879ad34ab9 --- /dev/null +++ b/ros2_controllers_test_nodes/test/rrbot_forward_position_publisher.yaml @@ -0,0 +1,11 @@ +publisher_forward_position_controller: + ros__parameters: + + wait_sec_between_publish: 5 + publish_topic: "/forward_position_controller/commands" + + goal_names: ["pos1", "pos2", "pos3", "pos4"] + pos1: [0.785, 0.785] + pos2: [0.0, 0.0] + pos3: [-0.785, -0.785] + pos4: [0.0, 0.0] diff --git a/ros2_controllers_test_nodes/test/rrbot_joint_trajectory_publisher.yaml b/ros2_controllers_test_nodes/test/rrbot_joint_trajectory_publisher.yaml new file mode 100644 index 0000000000..7dd8304134 --- /dev/null +++ b/ros2_controllers_test_nodes/test/rrbot_joint_trajectory_publisher.yaml @@ -0,0 +1,24 @@ +publisher_position_trajectory_controller: + ros__parameters: + + controller_name: "joint_trajectory_position_controller" + wait_sec_between_publish: 6 + + goal_names: ["pos1", "pos2", "pos3", "pos4"] + pos1: + positions: [0.785, 0.785] + pos2: + positions: [0.0, 0.0] + pos3: + positions: [-0.785, -0.785] + pos4: + positions: [0.0, 0.0] + + joints: + - joint1 + - joint2 + + check_starting_point: false + starting_point_limits: + joint1: [-0.1,0.1] + joint2: [-0.1,0.1] diff --git a/ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py b/ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py new file mode 100644 index 0000000000..8ebb2df7b3 --- /dev/null +++ b/ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py @@ -0,0 +1,107 @@ +# Copyright (c) 2024 AIT - Austrian Institute of Technology GmbH +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: Christoph Froehlich + +import pytest +import unittest +import time + +from launch import LaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from launch_testing.actions import ReadyToTest +from launch_testing_ros import WaitForTopics + +import launch_testing.markers +import rclpy +import launch_ros.actions +from rclpy.node import Node + +from std_msgs.msg import Float64MultiArray + + +# Executes the given launch file and checks if all nodes can be started +@pytest.mark.launch_test +def generate_test_description(): + + params = PathJoinSubstitution( + [ + FindPackageShare("ros2_controllers_test_nodes"), + "test", + "rrbot_forward_position_publisher.yaml", + ] + ) + + pub_node = launch_ros.actions.Node( + package="ros2_controllers_test_nodes", + executable="publisher_forward_position_controller", + parameters=[params], + output="both", + ) + + return LaunchDescription([pub_node, ReadyToTest()]) + + +# This is our test fixture. Each method is a test case. +# These run alongside the processes specified in generate_test_description() +class TestFixture(unittest.TestCase): + + def setUp(self): + rclpy.init() + self.node = Node("test_node") + + def tearDown(self): + self.node.destroy_node() + rclpy.shutdown() + + def test_node_start(self): + start = time.time() + found = False + while time.time() - start < 2.0 and not found: + found = "publisher_forward_position_controller" in self.node.get_node_names() + time.sleep(0.1) + assert found, "publisher_forward_position_controller not found!" + + def test_check_if_topic_published(self): + topic = "/forward_position_controller/commands" + wait_for_topics = WaitForTopics([(topic, Float64MultiArray)], timeout=20.0) + assert wait_for_topics.wait(), f"Topic '{topic}' not found!" + msgs = wait_for_topics.received_messages(topic) + msg = msgs[0] + assert len(msg.data) == 2, "Wrong number of joints in message" + wait_for_topics.shutdown() + + +@launch_testing.post_shutdown_test() +# These tests are run after the processes in generate_test_description() have shut down. +class TestPublisherShutdown(unittest.TestCase): + + def test_exit_codes(self, proc_info): + """Check if the processes exited normally.""" + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py b/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py new file mode 100644 index 0000000000..d8d6e2a577 --- /dev/null +++ b/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py @@ -0,0 +1,106 @@ +# Copyright (c) 2024 AIT - Austrian Institute of Technology GmbH +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: Christoph Froehlich + +import pytest +import unittest +import time + +from launch import LaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from launch_testing.actions import ReadyToTest +from launch_testing_ros import WaitForTopics + +import launch_testing.markers +import rclpy +import launch_ros.actions +from rclpy.node import Node +from trajectory_msgs.msg import JointTrajectory + + +# Executes the given launch file and checks if all nodes can be started +@pytest.mark.launch_test +def generate_test_description(): + + params = PathJoinSubstitution( + [ + FindPackageShare("ros2_controllers_test_nodes"), + "test", + "rrbot_joint_trajectory_publisher.yaml", + ] + ) + + pub_node = launch_ros.actions.Node( + package="ros2_controllers_test_nodes", + executable="publisher_joint_trajectory_controller", + parameters=[params], + output="both", + ) + + return LaunchDescription([pub_node, ReadyToTest()]) + + +# This is our test fixture. Each method is a test case. +# These run alongside the processes specified in generate_test_description() +class TestFixture(unittest.TestCase): + + def setUp(self): + rclpy.init() + self.node = Node("test_node") + + def tearDown(self): + self.node.destroy_node() + rclpy.shutdown() + + def test_node_start(self): + start = time.time() + found = False + while time.time() - start < 2.0 and not found: + found = "publisher_position_trajectory_controller" in self.node.get_node_names() + time.sleep(0.1) + assert found, "publisher_position_trajectory_controller not found!" + + def test_check_if_topic_published(self): + topic = "/joint_trajectory_position_controller/joint_trajectory" + wait_for_topics = WaitForTopics([(topic, JointTrajectory)], timeout=20.0) + assert wait_for_topics.wait(), f"Topic '{topic}' not found!" + msgs = wait_for_topics.received_messages(topic) + msg = msgs[0] + assert len(msg.joint_names) == 2, "Wrong number of joints in message" + wait_for_topics.shutdown() + + +@launch_testing.post_shutdown_test() +# These tests are run after the processes in generate_test_description() have shut down. +class TestPublisherShutdown(unittest.TestCase): + + def test_exit_codes(self, proc_info): + """Check if the processes exited normally.""" + launch_testing.asserts.assertExitCodes(proc_info) From 0590c6ac2f916b13ce5842913b8811a32fd4ffab Mon Sep 17 00:00:00 2001 From: Wiktor Bajor <69388767+Wiktor-99@users.noreply.github.com> Date: Mon, 18 Nov 2024 15:07:19 +0100 Subject: [PATCH 11/84] Gpio command controller (#1251) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --------- Co-authored-by: m.bednarczyk Co-authored-by: Maciej Bednarczyk Co-authored-by: Bence Magyar Co-authored-by: Christoph Fröhlich Co-authored-by: Christoph Froehlich Co-authored-by: Sai Kishor Kothakota --- doc/controllers_index.rst | 1 + doc/release_notes.rst | 4 + gpio_controllers/CMakeLists.txt | 115 +++ gpio_controllers/doc/userdoc.rst | 57 ++ gpio_controllers/gpio_controllers_plugin.xml | 7 + .../gpio_command_controller.hpp | 114 +++ .../gpio_controllers/visibility_control.h | 49 ++ gpio_controllers/package.xml | 30 + .../src/gpio_command_controller.cpp | 416 +++++++++++ .../gpio_command_controller_parameters.yaml | 34 + .../test/test_gpio_command_controller.cpp | 693 ++++++++++++++++++ .../test_load_gpio_command_controller.cpp | 37 + ros2_controllers/package.xml | 1 + 13 files changed, 1558 insertions(+) create mode 100644 gpio_controllers/CMakeLists.txt create mode 100644 gpio_controllers/doc/userdoc.rst create mode 100644 gpio_controllers/gpio_controllers_plugin.xml create mode 100644 gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp create mode 100644 gpio_controllers/include/gpio_controllers/visibility_control.h create mode 100644 gpio_controllers/package.xml create mode 100644 gpio_controllers/src/gpio_command_controller.cpp create mode 100644 gpio_controllers/src/gpio_command_controller_parameters.yaml create mode 100644 gpio_controllers/test/test_gpio_command_controller.cpp create mode 100644 gpio_controllers/test/test_load_gpio_command_controller.cpp diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index d5b956aa8a..0a9c6ec485 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -56,6 +56,7 @@ The controllers are using `common hardware interface definitions`_, and may use PID Controller <../pid_controller/doc/userdoc.rst> Position Controllers <../position_controllers/doc/userdoc.rst> Velocity Controllers <../velocity_controllers/doc/userdoc.rst> + Gpio Command Controller <../gpio_controllers/doc/userdoc.rst> Broadcasters diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 688c35724d..b1db026df2 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -63,3 +63,7 @@ steering_controllers_library tricycle_controller ************************ * tricycle_controller now uses generate_parameter_library (`#957 `_). + +gpio_controllers +************************ +* The GPIO command controller was added 🎉 (`#1251 `_). diff --git a/gpio_controllers/CMakeLists.txt b/gpio_controllers/CMakeLists.txt new file mode 100644 index 0000000000..6ea97e04ca --- /dev/null +++ b/gpio_controllers/CMakeLists.txt @@ -0,0 +1,115 @@ +cmake_minimum_required(VERSION 3.8) +project(gpio_controllers) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(controller_interface REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(realtime_tools REQUIRED) +find_package(generate_parameter_library REQUIRED) +find_package(control_msgs REQUIRED) + + +generate_parameter_library(gpio_command_controller_parameters + src/gpio_command_controller_parameters.yaml +) + +add_library(gpio_controllers + SHARED + src/gpio_command_controller.cpp +) +target_include_directories(gpio_controllers PRIVATE include) +target_link_libraries(gpio_controllers PUBLIC gpio_command_controller_parameters) +ament_target_dependencies(gpio_controllers PUBLIC + builtin_interfaces + controller_interface + hardware_interface + pluginlib + rclcpp_lifecycle + rcutils + realtime_tools + control_msgs +) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(gpio_controllers PRIVATE "GPIO_COMMAND_CONTROLLER_BUILDING_DLL") +pluginlib_export_plugin_description_file(controller_interface gpio_controllers_plugin.xml) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS + gpio_controllers + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + ament_add_gmock( + test_load_gpio_command_controller + test/test_load_gpio_command_controller.cpp + ) + + target_include_directories(test_load_gpio_command_controller PRIVATE include) + ament_target_dependencies(test_load_gpio_command_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + + ament_add_gmock( + test_gpio_command_controller + test/test_gpio_command_controller.cpp + ) + target_include_directories(test_gpio_command_controller PRIVATE include) + target_link_libraries(test_gpio_command_controller + gpio_controllers + ) + ament_target_dependencies(test_gpio_command_controller + controller_interface + hardware_interface + rclcpp + rclcpp_lifecycle + realtime_tools + ros2_control_test_assets + control_msgs + ) +endif() + +ament_export_dependencies( + controller_interface + hardware_interface + rclcpp + rclcpp_lifecycle + realtime_tools +) +ament_export_include_directories( + include +) +ament_export_libraries( + gpio_controllers +) +ament_package() diff --git a/gpio_controllers/doc/userdoc.rst b/gpio_controllers/doc/userdoc.rst new file mode 100644 index 0000000000..199a4bd208 --- /dev/null +++ b/gpio_controllers/doc/userdoc.rst @@ -0,0 +1,57 @@ +.. _gpio_controllers_userdoc: + +gpio_controllers +===================== + +This is a collection of controllers for hardware interfaces of type GPIO (```` tag in the URDF). + +gpio_command_controller +----------------------------- +gpio_command_controller let the user expose command interfaces of given GPIO interfaces and publishes state interfaces of the configured command interfaces. + +Description of controller's interfaces +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +- ``//gpio_states`` [``control_msgs/msg/DynamicJointState``]: Publishes all state interfaces of the given GPIO interfaces. +- ``//commands`` [``control_msgs/msg/DynamicJointState``]: A subscriber for configured command interfaces. + + +Parameters +^^^^^^^^^^^^^^^^^^^^^^^^ + +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. + +.. generate_parameter_library_details:: + ../src/gpio_command_controller_parameters.yaml + +The controller expects at least one GPIO interface and the corresponding command interface names or state interface. However, these Command and State interfaces are optional. The controller behaves as a broadcaster when no Command Interface is present, thereby publishing the configured GPIO state interfaces if set, else the one present in the URDF. + +.. note:: + + When no state interface is provided in the param file, the controller will try to use state_interfaces from ros2_control's config placed in the URDF for configured gpio interfaces. + However, command interfaces will not be configured based on the available URDF setup. + +.. code-block:: yaml + + gpio_command_controller: + ros__parameters: + type: gpio_controllers/GpioCommandController + gpios: + - Gpio1 + - Gpio2 + command_interfaces: + Gpio1: + - interfaces: + - dig.1 + - dig.2 + - dig.3 + Gpio2: + - interfaces: + - ana.1 + - ana.2 + state_interfaces: + Gpio2: + - interfaces: + - ana.1 + - ana.2 + +With the above-defined controller configuration, the controller will accept commands for all gpios' interfaces and will only publish the state of Gpio2. diff --git a/gpio_controllers/gpio_controllers_plugin.xml b/gpio_controllers/gpio_controllers_plugin.xml new file mode 100644 index 0000000000..03fd3e1ee0 --- /dev/null +++ b/gpio_controllers/gpio_controllers_plugin.xml @@ -0,0 +1,7 @@ + + + + The gpio command controller commands a group of gpios using multiple interfaces. + + + diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp new file mode 100644 index 0000000000..5149c74a1c --- /dev/null +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -0,0 +1,114 @@ +// Copyright 2022 ICUBE Laboratory, University of Strasbourg +// +// 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 GPIO_CONTROLLERS__GPIO_COMMAND_CONTROLLER_HPP_ +#define GPIO_CONTROLLERS__GPIO_COMMAND_CONTROLLER_HPP_ + +#include +#include +#include +#include + +#include "control_msgs/msg/dynamic_interface_group_values.hpp" +#include "controller_interface/controller_interface.hpp" +#include "gpio_command_controller_parameters.hpp" +#include "gpio_controllers/visibility_control.h" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" + +namespace gpio_controllers +{ +using CmdType = control_msgs::msg::DynamicInterfaceGroupValues; +using StateType = control_msgs::msg::DynamicInterfaceGroupValues; +using CallbackReturn = controller_interface::CallbackReturn; +using InterfacesNames = std::vector; +using MapOfReferencesToCommandInterfaces = std::unordered_map< + std::string, std::reference_wrapper>; +using MapOfReferencesToStateInterfaces = + std::unordered_map>; +using StateInterfaces = + std::vector>; + +class GpioCommandController : public controller_interface::ControllerInterface +{ +public: + GPIO_COMMAND_CONTROLLER_PUBLIC + GpioCommandController(); + + GPIO_COMMAND_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + CallbackReturn on_init() override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +private: + void store_command_interface_types(); + void store_state_interface_types(); + void initialize_gpio_state_msg(); + void update_gpios_states(); + controller_interface::return_type update_gpios_commands(); + template + std::unordered_map> create_map_of_references_to_interfaces( + const InterfacesNames & interfaces_from_params, std::vector & configured_interfaces); + template + bool check_if_configured_interfaces_matches_received( + const InterfacesNames & interfaces_from_params, const T & configured_interfaces); + void apply_state_value( + StateType & state_msg, std::size_t gpio_index, std::size_t interface_index) const; + void apply_command( + const CmdType & gpio_commands, std::size_t gpio_index, + std::size_t command_interface_index) const; + bool should_broadcast_all_interfaces_of_configured_gpios() const; + void set_all_state_interfaces_of_configured_gpios(); + InterfacesNames get_gpios_state_interfaces_names(const std::string & gpio_name) const; + bool update_dynamic_map_parameters(); + std::vector get_gpios_from_urdf() const; + +protected: + InterfacesNames command_interface_types_; + InterfacesNames state_interface_types_; + MapOfReferencesToCommandInterfaces command_interfaces_map_; + MapOfReferencesToStateInterfaces state_interfaces_map_; + + realtime_tools::RealtimeBuffer> rt_command_ptr_{}; + rclcpp::Subscription::SharedPtr gpios_command_subscriber_{}; + + std::shared_ptr> gpio_state_publisher_{}; + std::shared_ptr> realtime_gpio_state_publisher_{}; + + std::shared_ptr param_listener_{}; + gpio_command_controller_parameters::Params params_; +}; + +} // namespace gpio_controllers + +#endif // GPIO_CONTROLLERS__GPIO_COMMAND_CONTROLLER_HPP_ diff --git a/gpio_controllers/include/gpio_controllers/visibility_control.h b/gpio_controllers/include/gpio_controllers/visibility_control.h new file mode 100644 index 0000000000..a735a1621c --- /dev/null +++ b/gpio_controllers/include/gpio_controllers/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright 2022 ICUBE Laboratory, University of Strasbourg +// +// 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 GPIO_CONTROLLERS__VISIBILITY_CONTROL_H_ +#define GPIO_CONTROLLERS__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define GPIO_COMMAND_CONTROLLER_EXPORT __attribute__((dllexport)) +#define GPIO_COMMAND_CONTROLLER_IMPORT __attribute__((dllimport)) +#else +#define GPIO_COMMAND_CONTROLLER_EXPORT __declspec(dllexport) +#define GPIO_COMMAND_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef GPIO_COMMAND_CONTROLLER_BUILDING_LIBRARY +#define GPIO_COMMAND_CONTROLLER_PUBLIC GPIO_COMMAND_CONTROLLER_EXPORT +#else +#define GPIO_COMMAND_CONTROLLER_PUBLIC GPIO_COMMAND_CONTROLLER_IMPORT +#endif +#define GPIO_COMMAND_CONTROLLER_PUBLIC_TYPE GPIO_COMMAND_CONTROLLER_PUBLIC +#define GPIO_COMMAND_CONTROLLER_LOCAL +#else +#define GPIO_COMMAND_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define GPIO_COMMAND_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define GPIO_COMMAND_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define GPIO_COMMAND_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define GPIO_COMMAND_CONTROLLER_PUBLIC +#define GPIO_COMMAND_CONTROLLER_LOCAL +#endif +#define GPIO_COMMAND_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // GPIO_CONTROLLERS__VISIBILITY_CONTROL_H_ diff --git a/gpio_controllers/package.xml b/gpio_controllers/package.xml new file mode 100644 index 0000000000..9b98746545 --- /dev/null +++ b/gpio_controllers/package.xml @@ -0,0 +1,30 @@ + + + + gpio_controllers + 4.15.0 + Controllers to interact with gpios. + Maciej Bednarczyk + Wiktor Bajor + Apache License 2.0 + + ament_cmake + + controller_interface + hardware_interface + rclcpp + rclcpp_lifecycle + control_msgs + realtime_tools + generate_parameter_library + + pluginlib + + ament_cmake_gmock + controller_manager + ros2_control_test_assets + + + ament_cmake + + diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp new file mode 100644 index 0000000000..a509eae774 --- /dev/null +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -0,0 +1,416 @@ +// Copyright 2022 ICUBE Laboratory, University of Strasbourg +// +// 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. + +#include "gpio_controllers/gpio_command_controller.hpp" + +#include + +#include "controller_interface/helpers.hpp" +#include "hardware_interface/component_parser.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/subscription.hpp" + +namespace +{ +template +void print_interface(const rclcpp::Logger & logger, const T & command_interfaces) +{ + for (const auto & [interface_name, value] : command_interfaces) + { + RCLCPP_ERROR(logger, "Got %s", interface_name.c_str()); + } +} + +std::vector extract_gpios_from_hardware_info( + const std::vector & hardware_infos) +{ + std::vector result; + for (const auto & hardware_info : hardware_infos) + { + std::copy( + hardware_info.gpios.begin(), hardware_info.gpios.end(), std::back_insert_iterator(result)); + } + return result; +} +} // namespace + +namespace gpio_controllers +{ + +GpioCommandController::GpioCommandController() : controller_interface::ControllerInterface() {} + +CallbackReturn GpioCommandController::on_init() +try +{ + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + return CallbackReturn::SUCCESS; +} +catch (const std::exception & e) +{ + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; +} + +CallbackReturn GpioCommandController::on_configure(const rclcpp_lifecycle::State &) +try +{ + if (!update_dynamic_map_parameters()) + { + return controller_interface::CallbackReturn::ERROR; + } + + store_command_interface_types(); + store_state_interface_types(); + + if (command_interface_types_.empty() && state_interface_types_.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "No command or state interfaces are configured"); + return CallbackReturn::ERROR; + } + + if (!command_interface_types_.empty()) + { + gpios_command_subscriber_ = get_node()->create_subscription( + "~/commands", rclcpp::SystemDefaultsQoS(), + [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); + } + + gpio_state_publisher_ = + get_node()->create_publisher("~/gpio_states", rclcpp::SystemDefaultsQoS()); + + realtime_gpio_state_publisher_ = + std::make_shared>(gpio_state_publisher_); + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return CallbackReturn::SUCCESS; +} +catch (const std::exception & e) +{ + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; +} + +controller_interface::InterfaceConfiguration +GpioCommandController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + command_interfaces_config.names = command_interface_types_; + + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration GpioCommandController::state_interface_configuration() + const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_interfaces_config.names = state_interface_types_; + + return state_interfaces_config; +} + +CallbackReturn GpioCommandController::on_activate(const rclcpp_lifecycle::State &) +{ + command_interfaces_map_ = + create_map_of_references_to_interfaces(command_interface_types_, command_interfaces_); + state_interfaces_map_ = + create_map_of_references_to_interfaces(state_interface_types_, state_interfaces_); + if ( + !check_if_configured_interfaces_matches_received( + command_interface_types_, command_interfaces_map_) || + !check_if_configured_interfaces_matches_received(state_interface_types_, state_interfaces_map_)) + { + return CallbackReturn::ERROR; + } + + initialize_gpio_state_msg(); + rt_command_ptr_.reset(); + RCLCPP_INFO(get_node()->get_logger(), "activate successful"); + return CallbackReturn::SUCCESS; +} + +CallbackReturn GpioCommandController::on_deactivate(const rclcpp_lifecycle::State &) +{ + rt_command_ptr_.reset(); + return CallbackReturn::SUCCESS; +} + +controller_interface::return_type GpioCommandController::update( + const rclcpp::Time &, const rclcpp::Duration &) +{ + update_gpios_states(); + return update_gpios_commands(); +} + +bool GpioCommandController::update_dynamic_map_parameters() +{ + auto logger = get_node()->get_logger(); + if (!param_listener_) + { + RCLCPP_ERROR(logger, "Error encountered during init"); + return false; + } + // update the dynamic map parameters + param_listener_->refresh_dynamic_parameters(); + // get parameters from the listener in case they were updated + params_ = param_listener_->get_params(); + return true; +} + +void GpioCommandController::store_command_interface_types() +{ + for (const auto & [gpio_name, interfaces] : params_.command_interfaces.gpios_map) + { + std::transform( + interfaces.interfaces.cbegin(), interfaces.interfaces.cend(), + std::back_inserter(command_interface_types_), + [&](const auto & interface_name) { return gpio_name + "/" + interface_name; }); + } +} + +bool GpioCommandController::should_broadcast_all_interfaces_of_configured_gpios() const +{ + auto are_interfaces_empty = [](const auto & interfaces) + { return interfaces.second.interfaces.empty(); }; + return std::all_of( + params_.state_interfaces.gpios_map.cbegin(), params_.state_interfaces.gpios_map.cend(), + are_interfaces_empty); +} + +std::vector GpioCommandController::get_gpios_from_urdf() const +try +{ + return extract_gpios_from_hardware_info( + hardware_interface::parse_control_resources_from_urdf(get_robot_description())); +} +catch (const std::exception & e) +{ + fprintf(stderr, "Exception thrown during extracting gpios info from urdf %s \n", e.what()); + return {}; +} + +void GpioCommandController::set_all_state_interfaces_of_configured_gpios() +{ + const auto gpios{get_gpios_from_urdf()}; + for (const auto & gpio_name : params_.gpios) + { + for (const auto & gpio : gpios) + { + if (gpio_name == gpio.name) + { + std::transform( + gpio.state_interfaces.begin(), gpio.state_interfaces.end(), + std::back_insert_iterator(state_interface_types_), + [&gpio_name](const auto & interface_name) + { return gpio_name + '/' + interface_name.name; }); + } + } + } +} + +void GpioCommandController::store_state_interface_types() +{ + if (should_broadcast_all_interfaces_of_configured_gpios()) + { + RCLCPP_INFO( + get_node()->get_logger(), + "State interfaces are not configured. All available interfaces of configured GPIOs will be " + "broadcasted."); + set_all_state_interfaces_of_configured_gpios(); + return; + } + + for (const auto & [gpio_name, interfaces] : params_.state_interfaces.gpios_map) + { + std::transform( + interfaces.interfaces.cbegin(), interfaces.interfaces.cend(), + std::back_inserter(state_interface_types_), + [&](const auto & interface_name) { return gpio_name + "/" + interface_name; }); + } +} + +void GpioCommandController::initialize_gpio_state_msg() +{ + auto & gpio_state_msg = realtime_gpio_state_publisher_->msg_; + gpio_state_msg.header.stamp = get_node()->now(); + gpio_state_msg.interface_groups.resize(params_.gpios.size()); + gpio_state_msg.interface_values.resize(params_.gpios.size()); + + for (std::size_t gpio_index = 0; gpio_index < params_.gpios.size(); ++gpio_index) + { + const auto gpio_name = params_.gpios[gpio_index]; + gpio_state_msg.interface_groups[gpio_index] = gpio_name; + gpio_state_msg.interface_values[gpio_index].interface_names = + get_gpios_state_interfaces_names(gpio_name); + gpio_state_msg.interface_values[gpio_index].values = std::vector( + gpio_state_msg.interface_values[gpio_index].interface_names.size(), + std::numeric_limits::quiet_NaN()); + } +} + +InterfacesNames GpioCommandController::get_gpios_state_interfaces_names( + const std::string & gpio_name) const +{ + InterfacesNames result; + for (const auto & interface_name : state_interface_types_) + { + const auto it = state_interfaces_map_.find(interface_name); + if (it != state_interfaces_map_.cend() && it->second.get().get_prefix_name() == gpio_name) + { + result.emplace_back(it->second.get().get_interface_name()); + } + } + return result; +} + +template +std::unordered_map> +GpioCommandController::create_map_of_references_to_interfaces( + const InterfacesNames & interfaces_from_params, std::vector & configured_interfaces) +{ + std::unordered_map> map; + for (const auto & interface_name : interfaces_from_params) + { + auto interface = std::find_if( + configured_interfaces.begin(), configured_interfaces.end(), + [&](const auto & configured_interface) + { + const auto full_name_interface_name = configured_interface.get_name(); + return full_name_interface_name == interface_name; + }); + if (interface != configured_interfaces.end()) + { + map.emplace(interface_name, std::ref(*interface)); + } + } + return map; +} + +template +bool GpioCommandController::check_if_configured_interfaces_matches_received( + const InterfacesNames & interfaces_from_params, const T & configured_interfaces) +{ + if (!(configured_interfaces.size() == interfaces_from_params.size())) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Expected %ld interfaces, got %ld", interfaces_from_params.size(), + configured_interfaces.size()); + for (const auto & interface : interfaces_from_params) + { + RCLCPP_ERROR(get_node()->get_logger(), "Expected %s", interface.c_str()); + } + print_interface(get_node()->get_logger(), configured_interfaces); + return false; + } + return true; +} + +controller_interface::return_type GpioCommandController::update_gpios_commands() +{ + auto gpio_commands_ptr = rt_command_ptr_.readFromRT(); + if (!gpio_commands_ptr || !(*gpio_commands_ptr)) + { + return controller_interface::return_type::OK; + } + + const auto gpio_commands = *(*gpio_commands_ptr); + for (std::size_t gpio_index = 0; gpio_index < gpio_commands.interface_groups.size(); ++gpio_index) + { + const auto & gpio_name = gpio_commands.interface_groups[gpio_index]; + if ( + gpio_commands.interface_values[gpio_index].values.size() != + gpio_commands.interface_values[gpio_index].interface_names.size()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "For gpio %s interfaces_names do not match values", + gpio_name.c_str()); + return controller_interface::return_type::ERROR; + } + for (std::size_t command_interface_index = 0; + command_interface_index < gpio_commands.interface_values[gpio_index].values.size(); + ++command_interface_index) + { + apply_command(gpio_commands, gpio_index, command_interface_index); + } + } + return controller_interface::return_type::OK; +} + +void GpioCommandController::apply_command( + const CmdType & gpio_commands, std::size_t gpio_index, std::size_t command_interface_index) const +{ + const auto full_command_interface_name = + gpio_commands.interface_groups[gpio_index] + '/' + + gpio_commands.interface_values[gpio_index].interface_names[command_interface_index]; + try + { + command_interfaces_map_.at(full_command_interface_name) + .get() + .set_value(gpio_commands.interface_values[gpio_index].values[command_interface_index]); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during applying command stage of %s with message: %s \n", + full_command_interface_name.c_str(), e.what()); + } +} + +void GpioCommandController::update_gpios_states() +{ + if (!realtime_gpio_state_publisher_ || !realtime_gpio_state_publisher_->trylock()) + { + return; + } + + auto & gpio_state_msg = realtime_gpio_state_publisher_->msg_; + gpio_state_msg.header.stamp = get_node()->now(); + for (std::size_t gpio_index = 0; gpio_index < gpio_state_msg.interface_groups.size(); + ++gpio_index) + { + for (std::size_t interface_index = 0; + interface_index < gpio_state_msg.interface_values[gpio_index].interface_names.size(); + ++interface_index) + { + apply_state_value(gpio_state_msg, gpio_index, interface_index); + } + } + realtime_gpio_state_publisher_->unlockAndPublish(); +} + +void GpioCommandController::apply_state_value( + StateType & state_msg, std::size_t gpio_index, std::size_t interface_index) const +{ + const auto interface_name = + state_msg.interface_groups[gpio_index] + '/' + + state_msg.interface_values[gpio_index].interface_names[interface_index]; + try + { + state_msg.interface_values[gpio_index].values[interface_index] = + state_interfaces_map_.at(interface_name).get().get_value(); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during reading state of: %s \n", interface_name.c_str()); + } +} + +} // namespace gpio_controllers + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + gpio_controllers::GpioCommandController, controller_interface::ControllerInterface) diff --git a/gpio_controllers/src/gpio_command_controller_parameters.yaml b/gpio_controllers/src/gpio_command_controller_parameters.yaml new file mode 100644 index 0000000000..0c6aade35f --- /dev/null +++ b/gpio_controllers/src/gpio_command_controller_parameters.yaml @@ -0,0 +1,34 @@ +gpio_command_controller_parameters: + gpios: { + type: string_array, + description: "List of gpios", + read_only: true, + validation: { + size_gt<>: [0], + unique<>: null + } + } + + command_interfaces: + __map_gpios: + interfaces: { + type: string_array, + description: "List of command interfaces for each gpio.", + read_only: true, + default_value: [], + validation: { + unique<>: null + } + } + + state_interfaces: + __map_gpios: + interfaces: { + type: string_array, + description: "List of state interfaces for each gpio. If empty all available gpios' states are used.", + read_only: true, + default_value: [], + validation: { + unique<>: null + } + } diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp new file mode 100644 index 0000000000..ec0e0f1228 --- /dev/null +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -0,0 +1,693 @@ +// Copyright 2022 ICUBE Laboratory, University of Strasbourg +// +// 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. +#include +#include +#include + +#include "control_msgs/msg/dynamic_interface_group_values.hpp" +#include "gmock/gmock.h" +#include "gpio_controllers/gpio_command_controller.hpp" +#include "hardware_interface/handle.hpp" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp/wait_result.hpp" +#include "rclcpp/wait_set.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +namespace +{ +const auto hardware_resources_with_gpio = + R"( + + + + + +)"; + +const auto minimal_robot_urdf_with_gpio = std::string(ros2_control_test_assets::urdf_head) + + std::string(hardware_resources_with_gpio) + + std::string(ros2_control_test_assets::urdf_tail); +} // namespace + +using CallbackReturn = controller_interface::CallbackReturn; +using hardware_interface::LoanedCommandInterface; +using hardware_interface::LoanedStateInterface; +using CmdType = control_msgs::msg::DynamicInterfaceGroupValues; +using StateType = control_msgs::msg::DynamicInterfaceGroupValues; +using hardware_interface::CommandInterface; +using hardware_interface::StateInterface; + +namespace +{ +rclcpp::NodeOptions create_node_options_with_overriden_parameters( + std::vector parameters) +{ + auto node_options = rclcpp::NodeOptions(); + node_options.parameter_overrides(parameters); + return node_options; +} +} // namespace + +class FriendGpioCommandController : public gpio_controllers::GpioCommandController +{ + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenGivenCommandWithValuesForAllInterfacesThenValueOfInterfacesShouldBeUpdated); + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenGivenCommandWithOnlyOneGpioThenInterfacesValuesShouldBeUpdated); + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenCommandContainsMoreValuesThenInterfacesNameForGpioUpdateShouldReturnFalse); + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenCommandContainsMoreInterfacesNameThenValuesForGpioUpdateShouldReturnFalse); + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenGivenCommandInterfacesInDifferentOrderThenValueOfInterfacesShouldBeUpdated); + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenGivenCmdContainsWrongGpioInterfacesOrWrongGpioNameThenCommandInterfacesShouldNotBeUpdated); +}; + +class GpioCommandControllerTestSuite : public ::testing::Test +{ +public: + GpioCommandControllerTestSuite() + { + rclcpp::init(0, nullptr); + node = std::make_unique("node"); + } + ~GpioCommandControllerTestSuite() { rclcpp::shutdown(); } + void setup_command_and_state_interfaces() + { + std::vector command_interfaces; + command_interfaces.emplace_back(gpio_1_1_dig_cmd); + command_interfaces.emplace_back(gpio_1_2_dig_cmd); + command_interfaces.emplace_back(gpio_2_ana_cmd); + + std::vector state_interfaces; + state_interfaces.emplace_back(gpio_1_1_dig_state); + state_interfaces.emplace_back(gpio_1_2_dig_state); + state_interfaces.emplace_back(gpio_2_ana_state); + + controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); + } + + void move_to_activate_state(controller_interface::return_type result_of_initialization) + { + ASSERT_EQ(result_of_initialization, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + setup_command_and_state_interfaces(); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + } + + void stop_test_when_message_cannot_be_published(int max_sub_check_loop_count) + { + ASSERT_GE(max_sub_check_loop_count, 0) + << "Test was unable to publish a message through controller/broadcaster update loop"; + } + + void assert_default_command_and_state_values() + { + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), gpio_commands.at(0)); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), gpio_commands.at(1)); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), gpio_commands.at(2)); + ASSERT_EQ(gpio_1_1_dig_state.get_value(), gpio_states.at(0)); + ASSERT_EQ(gpio_1_2_dig_state.get_value(), gpio_states.at(1)); + ASSERT_EQ(gpio_2_ana_state.get_value(), gpio_states.at(2)); + } + + void update_controller_loop() + { + ASSERT_EQ( + controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + } + + CmdType createGpioCommand( + const std::vector & gpios_names, + const std::vector & interface_values) + { + CmdType command; + command.interface_groups = gpios_names; + command.interface_values = interface_values; + return command; + } + + control_msgs::msg::InterfaceValue createInterfaceValue( + const std::vector & interfaces_names, + const std::vector & interfaces_values) + { + control_msgs::msg::InterfaceValue output; + output.interface_names = interfaces_names; + output.values = interfaces_values; + return output; + } + + void wait_one_miliseconds_for_timeout() + { + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller.get_node()->get_node_base_interface()); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = controller.get_node()->get_clock()->now() + timeout; + while (controller.get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + } + + int wait_for_subscription( + std::shared_ptr subscription, int max_sub_check_loop_count = 5) + { + rclcpp::WaitSet wait_set; + wait_set.add_subscription(subscription); + while (max_sub_check_loop_count--) + { + controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + { + break; + } + } + return max_sub_check_loop_count; + } + + FriendGpioCommandController controller; + + const std::vector gpio_names{"gpio1", "gpio2"}; + std::vector gpio_commands{1.0, 0.0, 3.1}; + std::vector gpio_states{1.0, 0.0, 3.1}; + + CommandInterface gpio_1_1_dig_cmd{gpio_names.at(0), "dig.1", &gpio_commands.at(0)}; + CommandInterface gpio_1_2_dig_cmd{gpio_names.at(0), "dig.2", &gpio_commands.at(1)}; + CommandInterface gpio_2_ana_cmd{gpio_names.at(1), "ana.1", &gpio_commands.at(2)}; + + StateInterface gpio_1_1_dig_state{gpio_names.at(0), "dig.1", &gpio_states.at(0)}; + StateInterface gpio_1_2_dig_state{gpio_names.at(0), "dig.2", &gpio_states.at(1)}; + StateInterface gpio_2_ana_state{gpio_names.at(1), "ana.1", &gpio_states.at(2)}; + std::unique_ptr node; +}; + +TEST_F(GpioCommandControllerTestSuite, WhenNoParametersAreSetInitShouldFail) +{ + const auto result = controller.init( + "test_gpio_command_controller", ros2_control_test_assets::minimal_robot_urdf, 0, "", + controller.define_custom_node_options()); + ASSERT_EQ(result, controller_interface::return_type::ERROR); +} + +TEST_F(GpioCommandControllerTestSuite, WhenGpiosParameterIsEmptyInitShouldFail) +{ + const auto node_options = + create_node_options_with_overriden_parameters({{"gpios", std::vector{}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::ERROR); +} + +TEST_F(GpioCommandControllerTestSuite, WhenInterfacesParameterForGpioIsEmptyInitShouldNotFail) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", std::vector{"gpio1"}}, + {"command_interfaces.gpio1.interfaces", std::vector{}}, + {"state_interfaces.gpio1.interfaces", std::vector{}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); +} + +TEST_F(GpioCommandControllerTestSuite, WhenInterfacesParameterForGpioIsNotSetInitShouldNotFail) +{ + const auto node_options = + create_node_options_with_overriden_parameters({{"gpios", std::vector{"gpio1"}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenGpiosAreSetAndInterfacesAreSetForAllGpiosThenInitShouldSuccess) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + ASSERT_EQ(result, controller_interface::return_type::OK); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenCommandAndStateInterfacesAreEmptyAndNoInterfacesAreSetInUrdfConfigurationShouldFail) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", std::vector{"gpio1"}}, + {"command_interfaces.gpio1.interfaces", std::vector{}}, + {"state_interfaces.gpio1.interfaces", std::vector{}}}); + const auto result = controller.init( + "test_gpio_command_controller", ros2_control_test_assets::minimal_robot_urdf, 0, "", + node_options); + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenStateInterfaceAreNotConfiguredButAreSetInUrdfForConfiguredGpiosThenConfigureShouldSucceed) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", std::vector{"gpio1"}}, + {"command_interfaces.gpio1.interfaces", std::vector{}}, + {"state_interfaces.gpio1.interfaces", std::vector{}}}); + const auto result = controller.init( + "test_gpio_command_controller", minimal_robot_urdf_with_gpio, 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenStateInterfaceAreNotConfiguredAndProvidedUrdfIsEmptyThenConfigureShouldFail) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", std::vector{"gpio1"}}, + {"command_interfaces.gpio1.interfaces", std::vector{}}, + {"state_interfaces.gpio1.interfaces", std::vector{}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F(GpioCommandControllerTestSuite, ConfigureAndActivateParamsSuccess) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + setup_command_and_state_interfaces(); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenAssignedCommandInterfacesDoNotMatchInterfacesFromParamsThenControllerShouldFailOnActivation) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + std::vector command_interfaces; + command_interfaces.emplace_back(gpio_1_1_dig_cmd); + command_interfaces.emplace_back(gpio_1_2_dig_cmd); + + std::vector state_interfaces; + state_interfaces.emplace_back(gpio_1_1_dig_state); + state_interfaces.emplace_back(gpio_1_2_dig_state); + state_interfaces.emplace_back(gpio_2_ana_state); + + controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenAssignedStateInterfacesDoNotMatchInterfacesFromParamsThenControllerShouldFailOnActivation) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + std::vector command_interfaces; + command_interfaces.emplace_back(gpio_1_1_dig_cmd); + command_interfaces.emplace_back(gpio_1_2_dig_cmd); + command_interfaces.emplace_back(gpio_2_ana_cmd); + + std::vector state_interfaces; + state_interfaces.emplace_back(gpio_1_1_dig_state); + state_interfaces.emplace_back(gpio_1_2_dig_state); + + controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); + + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenCommandInterfacesDontMatchStatesButBothMatchAssignedOnesThenOnActivationShouldSucceed) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + std::vector command_interfaces; + command_interfaces.emplace_back(gpio_1_1_dig_cmd); + command_interfaces.emplace_back(gpio_1_2_dig_cmd); + command_interfaces.emplace_back(gpio_2_ana_cmd); + + std::vector state_interfaces; + state_interfaces.emplace_back(gpio_1_1_dig_state); + state_interfaces.emplace_back(gpio_2_ana_state); + + controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); + + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenThereWasNoCommandForGpiosThenCommandInterfacesShouldHaveDefaultValues) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + assert_default_command_and_state_values(); + update_controller_loop(); + assert_default_command_and_state_values(); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenCommandContainsMoreValuesThenInterfacesNameForGpioUpdateShouldReturnFalse) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + + const auto command = createGpioCommand( + {"gpio1", "gpio2"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0, 1.0}), + createInterfaceValue({"ana.1"}, {30.0})}); + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + ASSERT_EQ( + controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::ERROR); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenCommandContainsMoreInterfacesNameThenValuesForGpioUpdateShouldReturnFalse) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + + const auto command = createGpioCommand( + {"gpio1", "gpio2"}, + {createInterfaceValue({"dig.1", "dig.2"}, {0.0}), createInterfaceValue({"ana.1"}, {30.0})}); + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + ASSERT_EQ( + controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::ERROR); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenGivenCommandWithValuesForAllInterfacesThenValueOfInterfacesShouldBeUpdated) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + + const auto command = createGpioCommand( + {"gpio1", "gpio2"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0}), + createInterfaceValue({"ana.1"}, {30.0})}); + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + update_controller_loop(); + + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), 30.0); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenGivenCommandInterfacesInDifferentOrderThenValueOfInterfacesShouldBeUpdated) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + + const auto command = createGpioCommand( + {"gpio2", "gpio1"}, {createInterfaceValue({"ana.1"}, {30.0}), + createInterfaceValue({"dig.2", "dig.1"}, {1.0, 0.0})}); + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + update_controller_loop(); + + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), 30.0); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenGivenCommandWithOnlyOneGpioThenInterfacesValuesShouldBeUpdated) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + + const auto command = + createGpioCommand({"gpio1"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0})}); + + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + update_controller_loop(); + + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), gpio_commands[2]); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenGivenCmdContainsWrongGpioInterfacesOrWrongGpioNameThenCommandInterfacesShouldNotBeUpdated) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + + const auto command = createGpioCommand( + {"gpio1", "gpio3"}, {createInterfaceValue({"dig.3", "dig.4"}, {20.0, 25.0}), + createInterfaceValue({"ana.1"}, {21.0})}); + + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + update_controller_loop(); + + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), gpio_commands.at(0)); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), gpio_commands.at(1)); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), gpio_commands.at(2)); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenGivenCommandWithValuesForAllInterfacesViaTopicThenValueOfInterfacesShouldBeUpdated) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + + auto command_pub = node->create_publisher( + std::string(controller.get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); + const auto command = createGpioCommand( + {"gpio1", "gpio2"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0}), + createInterfaceValue({"ana.1"}, {30.0})}); + command_pub->publish(command); + wait_one_miliseconds_for_timeout(); + update_controller_loop(); + + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), 30.0); +} + +TEST_F(GpioCommandControllerTestSuite, ControllerShouldPublishGpioStatesWithCurrentValues) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + + auto subscription = node->create_subscription( + std::string(controller.get_node()->get_name()) + "/gpio_states", 10, + [&](const StateType::SharedPtr) {}); + + stop_test_when_message_cannot_be_published(wait_for_subscription(subscription)); + + StateType gpio_state_msg; + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(gpio_state_msg, msg_info)); + + ASSERT_EQ(gpio_state_msg.interface_groups.size(), gpio_names.size()); + ASSERT_EQ(gpio_state_msg.interface_groups.at(0), "gpio1"); + ASSERT_EQ(gpio_state_msg.interface_groups.at(1), "gpio2"); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).interface_names.at(0), "dig.1"); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).interface_names.at(1), "dig.2"); + ASSERT_EQ(gpio_state_msg.interface_values.at(1).interface_names.at(0), "ana.1"); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).values.at(0), 1.0); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).values.at(1), 0.0); + ASSERT_EQ(gpio_state_msg.interface_values.at(1).values.at(0), 3.1); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenStateInterfaceAreNotConfiguredButSetInUrdfForConfiguredGpiosThenThatStatesShouldBePublished) +{ + const std::vector single_gpio{"gpio1"}; + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", single_gpio}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1"}}}); + + std::vector command_interfaces; + command_interfaces.emplace_back(gpio_1_1_dig_cmd); + + std::vector state_interfaces; + state_interfaces.emplace_back(gpio_1_1_dig_state); + state_interfaces.emplace_back(gpio_2_ana_state); + + const auto result_of_initialization = controller.init( + "test_gpio_command_controller", minimal_robot_urdf_with_gpio, 0, "", node_options); + ASSERT_EQ(result_of_initialization, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + auto subscription = node->create_subscription( + std::string(controller.get_node()->get_name()) + "/gpio_states", 10, + [&](const StateType::SharedPtr) {}); + + stop_test_when_message_cannot_be_published(wait_for_subscription(subscription)); + + StateType gpio_state_msg; + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(gpio_state_msg, msg_info)); + + ASSERT_EQ(gpio_state_msg.interface_groups.size(), single_gpio.size()); + ASSERT_EQ(gpio_state_msg.interface_groups.at(0), "gpio1"); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).interface_names.at(0), "dig.1"); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).values.at(0), 1.0); +} + +TEST_F( + GpioCommandControllerTestSuite, + ControllerShouldPublishGpioStatesWithCurrentValuesWhenOnlyStateInterfacesAreSet) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + + std::vector state_interfaces; + state_interfaces.emplace_back(gpio_1_1_dig_state); + state_interfaces.emplace_back(gpio_2_ana_state); + + const auto result_of_initialization = + controller.init("test_gpio_command_controller", "", 0, "", node_options); + ASSERT_EQ(result_of_initialization, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + controller.assign_interfaces({}, std::move(state_interfaces)); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + auto subscription = node->create_subscription( + std::string(controller.get_node()->get_name()) + "/gpio_states", 10, + [&](const StateType::SharedPtr) {}); + + stop_test_when_message_cannot_be_published(wait_for_subscription(subscription)); + + StateType gpio_state_msg; + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(gpio_state_msg, msg_info)); + + ASSERT_EQ(gpio_state_msg.interface_groups.size(), gpio_names.size()); + ASSERT_EQ(gpio_state_msg.interface_groups.at(0), "gpio1"); + ASSERT_EQ(gpio_state_msg.interface_groups.at(1), "gpio2"); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).interface_names.at(0), "dig.1"); + ASSERT_EQ(gpio_state_msg.interface_values.at(1).interface_names.at(0), "ana.1"); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).values.at(0), 1.0); + ASSERT_EQ(gpio_state_msg.interface_values.at(1).values.at(0), 3.1); +} diff --git a/gpio_controllers/test/test_load_gpio_command_controller.cpp b/gpio_controllers/test/test_load_gpio_command_controller.cpp new file mode 100644 index 0000000000..379a53b048 --- /dev/null +++ b/gpio_controllers/test/test_load_gpio_command_controller.cpp @@ -0,0 +1,37 @@ +// Copyright 2022 ICUBE Laboratory, University of Strasbourg +// +// 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. + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadGpioCommandController, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + + ASSERT_NO_THROW( + cm.load_controller("test_gpio_command_controller", "gpio_controllers/GpioCommandController")); + + rclcpp::shutdown(); +} diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index ccde6cf812..4969658788 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -24,6 +24,7 @@ effort_controllers force_torque_sensor_broadcaster forward_command_controller + gpio_controllers imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller From 4f447f4be74da751462a9f8e74e1b0b45c2adb67 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 18 Nov 2024 19:50:08 +0100 Subject: [PATCH 12/84] Add few warning flags to error in all ros2_controllers packages and fix tests (#1370) --- ackermann_steering_controller/CMakeLists.txt | 3 +- .../test_ackermann_steering_controller.hpp | 7 +-- admittance_controller/CMakeLists.txt | 3 +- .../test/test_admittance_controller.hpp | 18 +++---- bicycle_steering_controller/CMakeLists.txt | 3 +- .../test/test_bicycle_steering_controller.hpp | 18 ++++--- diff_drive_controller/CMakeLists.txt | 4 +- effort_controllers/CMakeLists.txt | 3 +- .../CMakeLists.txt | 3 +- .../test_force_torque_sensor_broadcaster.cpp | 4 +- .../test_force_torque_sensor_broadcaster.hpp | 2 +- forward_command_controller/CMakeLists.txt | 3 +- gripper_controllers/CMakeLists.txt | 4 +- imu_sensor_broadcaster/CMakeLists.txt | 3 +- .../test/test_imu_sensor_broadcaster.hpp | 2 +- joint_state_broadcaster/CMakeLists.txt | 3 +- joint_trajectory_controller/CMakeLists.txt | 4 +- parallel_gripper_controller/CMakeLists.txt | 3 +- pid_controller/CMakeLists.txt | 4 +- pose_broadcaster/CMakeLists.txt | 3 +- .../test/test_pose_broadcaster.hpp | 2 +- position_controllers/CMakeLists.txt | 3 +- range_sensor_broadcaster/CMakeLists.txt | 4 +- .../src/range_sensor_broadcaster.cpp | 8 +-- .../test/test_range_sensor_broadcaster.cpp | 50 +++++++++---------- .../test/test_range_sensor_broadcaster.hpp | 1 + steering_controllers_library/CMakeLists.txt | 3 +- .../test_steering_controllers_library.hpp | 7 +-- tricycle_controller/CMakeLists.txt | 3 +- tricycle_steering_controller/CMakeLists.txt | 3 +- .../test_tricycle_steering_controller.hpp | 18 +++---- velocity_controllers/CMakeLists.txt | 3 +- 32 files changed, 116 insertions(+), 86 deletions(-) diff --git a/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt index 83edf7c157..2ffc413633 100644 --- a/ackermann_steering_controller/CMakeLists.txt +++ b/ackermann_steering_controller/CMakeLists.txt @@ -3,7 +3,8 @@ project(ackermann_steering_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() # find dependencies diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 59637a072f..c45601d701 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -293,9 +293,10 @@ class AckermannSteeringControllerFixture : public ::testing::Test double front_wheels_radius_ = 0.45; double rear_wheels_radius_ = 0.45; - std::array joint_state_values_ = {0.5, 0.5, 0.0, 0.0}; - std::array joint_command_values_ = {1.1, 3.3, 2.2, 4.4}; - std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; + std::array joint_state_values_ = {{0.5, 0.5, 0.0, 0.0}}; + std::array joint_command_values_ = {{1.1, 3.3, 2.2, 4.4}}; + std::array joint_reference_interfaces_ = { + {"linear/velocity", "angular/velocity"}}; std::string steering_interface_name_ = "position"; // defined in setup std::string traction_interface_name_ = ""; diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index cd16714a31..477a343776 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -3,7 +3,8 @@ project(admittance_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 4f4635d861..7ee56b8c11 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -371,15 +371,15 @@ class AdmittanceControllerTest : public ::testing::Test const std::string fixed_world_frame_ = "fixed_world_frame"; const std::string sensor_frame_ = "link_6"; - std::array admittance_selected_axes_ = {true, true, true, true, true, true}; - std::array admittance_mass_ = {5.5, 6.6, 7.7, 8.8, 9.9, 10.10}; - std::array admittance_damping_ratio_ = {2.828427, 2.828427, 2.828427, - 2.828427, 2.828427, 2.828427}; - std::array admittance_stiffness_ = {214.1, 214.2, 214.3, 214.4, 214.5, 214.6}; - - std::array joint_command_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - std::array joint_state_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6}; - std::array fts_state_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + std::array admittance_selected_axes_ = {{true, true, true, true, true, true}}; + std::array admittance_mass_ = {{5.5, 6.6, 7.7, 8.8, 9.9, 10.10}}; + std::array admittance_damping_ratio_ = { + {2.828427, 2.828427, 2.828427, 2.828427, 2.828427, 2.828427}}; + std::array admittance_stiffness_ = {{214.1, 214.2, 214.3, 214.4, 214.5, 214.6}}; + + std::array joint_command_values_ = {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; + std::array joint_state_values_ = {{1.1, 2.2, 3.3, 4.4, 5.5, 6.6}}; + std::array fts_state_values_ = {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; std::vector fts_state_names_; std::vector state_itfs_; diff --git a/bicycle_steering_controller/CMakeLists.txt b/bicycle_steering_controller/CMakeLists.txt index db1f5cf4ba..1f3a4599cc 100644 --- a/bicycle_steering_controller/CMakeLists.txt +++ b/bicycle_steering_controller/CMakeLists.txt @@ -3,7 +3,8 @@ project(bicycle_steering_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() # find dependencies diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 65f1691a3b..0253078bb9 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -247,12 +247,13 @@ class BicycleSteeringControllerFixture : public ::testing::Test bool open_loop_ = false; unsigned int velocity_rolling_window_size_ = 10; bool position_feedback_ = false; - std::vector rear_wheels_names_ = {"rear_wheel_joint"}; - std::vector front_wheels_names_ = {"steering_axis_joint"}; - std::vector joint_names_ = {rear_wheels_names_[0], front_wheels_names_[0]}; + std::vector rear_wheels_names_ = {{"rear_wheel_joint"}}; + std::vector front_wheels_names_ = {{"steering_axis_joint"}}; + std::vector joint_names_ = {{rear_wheels_names_[0], front_wheels_names_[0]}}; - std::vector rear_wheels_preceeding_names_ = {"pid_controller/rear_wheel_joint"}; - std::vector front_wheels_preceeding_names_ = {"pid_controller/steering_axis_joint"}; + std::vector rear_wheels_preceeding_names_ = {{"pid_controller/rear_wheel_joint"}}; + std::vector front_wheels_preceeding_names_ = { + {"pid_controller/steering_axis_joint"}}; std::vector preceeding_joint_names_ = { rear_wheels_preceeding_names_[0], front_wheels_preceeding_names_[0]}; @@ -260,9 +261,10 @@ class BicycleSteeringControllerFixture : public ::testing::Test double front_wheels_radius_ = 0.45; double rear_wheels_radius_ = 0.45; - std::array joint_state_values_ = {3.3, 0.5}; - std::array joint_command_values_ = {1.1, 2.2}; - std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; + std::array joint_state_values_ = {{3.3, 0.5}}; + std::array joint_command_values_ = {{1.1, 2.2}}; + std::array joint_reference_interfaces_ = { + {"linear/velocity", "angular/velocity"}}; std::string steering_interface_name_ = "position"; // defined in setup diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index 4eeb98b9d2..4b3ff4f77f 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -2,7 +2,9 @@ cmake_minimum_required(VERSION 3.16) project(diff_drive_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format) + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/effort_controllers/CMakeLists.txt b/effort_controllers/CMakeLists.txt index eae8642fb6..e79015fbbf 100644 --- a/effort_controllers/CMakeLists.txt +++ b/effort_controllers/CMakeLists.txt @@ -3,7 +3,8 @@ project(effort_controllers LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index 02c343f050..d9b005e650 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -3,7 +3,8 @@ project(force_torque_sensor_broadcaster LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index e436beb2e5..26f959e88f 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -282,8 +282,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Offsets { SetUpFTSBroadcaster(); - std::array force_offsets = {10.0, 30.0, -50.0}; - std::array torque_offsets = {1.0, -1.2, -5.2}; + std::array force_offsets = {{10.0, 30.0, -50.0}}; + std::array torque_offsets = {{1.0, -1.2, -5.2}}; // set the params 'sensor_name' and 'frame_id' fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp index fe7fb0c174..5d485d9d12 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp @@ -55,7 +55,7 @@ class ForceTorqueSensorBroadcasterTest : public ::testing::Test protected: const std::string sensor_name_ = "fts_sensor"; const std::string frame_id_ = "fts_sensor_frame"; - std::array sensor_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6}; + std::array sensor_values_ = {{1.1, 2.2, 3.3, 4.4, 5.5, 6.6}}; hardware_interface::StateInterface fts_force_x_{sensor_name_, "force.x", &sensor_values_[0]}; hardware_interface::StateInterface fts_force_y_{sensor_name_, "force.y", &sensor_values_[1]}; diff --git a/forward_command_controller/CMakeLists.txt b/forward_command_controller/CMakeLists.txt index 4885c69c8a..bf027866d6 100644 --- a/forward_command_controller/CMakeLists.txt +++ b/forward_command_controller/CMakeLists.txt @@ -3,7 +3,8 @@ project(forward_command_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/gripper_controllers/CMakeLists.txt b/gripper_controllers/CMakeLists.txt index c4a85dd453..4e9c72f79b 100644 --- a/gripper_controllers/CMakeLists.txt +++ b/gripper_controllers/CMakeLists.txt @@ -7,7 +7,9 @@ if(APPLE OR WIN32) endif() if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/imu_sensor_broadcaster/CMakeLists.txt b/imu_sensor_broadcaster/CMakeLists.txt index 5539dea9ff..38f0adbb54 100644 --- a/imu_sensor_broadcaster/CMakeLists.txt +++ b/imu_sensor_broadcaster/CMakeLists.txt @@ -3,7 +3,8 @@ project(imu_sensor_broadcaster LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp index 0f3286c302..70e01d593c 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp @@ -54,7 +54,7 @@ class IMUSensorBroadcasterTest : public ::testing::Test protected: const std::string sensor_name_ = "imu_sensor"; const std::string frame_id_ = "imu_sensor_frame"; - std::array sensor_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7, 8.8, 9.9, 10.10}; + std::array sensor_values_ = {{1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7, 8.8, 9.9, 10.10}}; hardware_interface::StateInterface imu_orientation_x_{ sensor_name_, "orientation.x", &sensor_values_[0]}; hardware_interface::StateInterface imu_orientation_y_{ diff --git a/joint_state_broadcaster/CMakeLists.txt b/joint_state_broadcaster/CMakeLists.txt index cc8dc18bf6..a75bbfa8f9 100644 --- a/joint_state_broadcaster/CMakeLists.txt +++ b/joint_state_broadcaster/CMakeLists.txt @@ -3,7 +3,8 @@ project(joint_state_broadcaster LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 438661f7bf..ec142c72f3 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -2,7 +2,9 @@ cmake_minimum_required(VERSION 3.16) project(joint_trajectory_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/parallel_gripper_controller/CMakeLists.txt b/parallel_gripper_controller/CMakeLists.txt index cb0f4fafc9..3d9be8dfac 100644 --- a/parallel_gripper_controller/CMakeLists.txt +++ b/parallel_gripper_controller/CMakeLists.txt @@ -3,7 +3,8 @@ project(parallel_gripper_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/pid_controller/CMakeLists.txt b/pid_controller/CMakeLists.txt index f9a9abce89..ed9bdcd8cf 100644 --- a/pid_controller/CMakeLists.txt +++ b/pid_controller/CMakeLists.txt @@ -2,7 +2,9 @@ cmake_minimum_required(VERSION 3.16) project(pid_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() if(WIN32) diff --git a/pose_broadcaster/CMakeLists.txt b/pose_broadcaster/CMakeLists.txt index 46028cf258..3cc434d2d0 100644 --- a/pose_broadcaster/CMakeLists.txt +++ b/pose_broadcaster/CMakeLists.txt @@ -6,7 +6,8 @@ project(pose_broadcaster if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/pose_broadcaster/test/test_pose_broadcaster.hpp b/pose_broadcaster/test/test_pose_broadcaster.hpp index a164b2c6ac..10b9c03d1c 100644 --- a/pose_broadcaster/test/test_pose_broadcaster.hpp +++ b/pose_broadcaster/test/test_pose_broadcaster.hpp @@ -39,7 +39,7 @@ class PoseBroadcasterTest : public ::testing::Test const std::string frame_id_ = "pose_base_frame"; const std::string tf_child_frame_id_ = "pose_frame"; - std::array pose_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7}; + std::array pose_values_ = {{1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7}}; hardware_interface::StateInterface pose_position_x_{pose_name_, "position.x", &pose_values_[0]}; hardware_interface::StateInterface pose_position_y_{pose_name_, "position.x", &pose_values_[1]}; diff --git a/position_controllers/CMakeLists.txt b/position_controllers/CMakeLists.txt index 18f3cb313a..e76b76555e 100644 --- a/position_controllers/CMakeLists.txt +++ b/position_controllers/CMakeLists.txt @@ -3,7 +3,8 @@ project(position_controllers LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/range_sensor_broadcaster/CMakeLists.txt b/range_sensor_broadcaster/CMakeLists.txt index 6f21607c9c..d70614ea53 100644 --- a/range_sensor_broadcaster/CMakeLists.txt +++ b/range_sensor_broadcaster/CMakeLists.txt @@ -8,7 +8,9 @@ if(NOT CMAKE_CXX_STANDARD) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp b/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp index 7c6d714be3..4ff817b2d3 100644 --- a/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp @@ -75,10 +75,10 @@ controller_interface::CallbackReturn RangeSensorBroadcaster::on_configure( realtime_publisher_->lock(); realtime_publisher_->msg_.header.frame_id = params_.frame_id; - realtime_publisher_->msg_.radiation_type = params_.radiation_type; - realtime_publisher_->msg_.field_of_view = params_.field_of_view; - realtime_publisher_->msg_.min_range = params_.min_range; - realtime_publisher_->msg_.max_range = params_.max_range; + realtime_publisher_->msg_.radiation_type = static_cast(params_.radiation_type); + realtime_publisher_->msg_.field_of_view = static_cast(params_.field_of_view); + realtime_publisher_->msg_.min_range = static_cast(params_.min_range); + realtime_publisher_->msg_.max_range = static_cast(params_.max_range); // \note The versions conditioning is added here to support the source-compatibility with Humble #if SENSOR_MSGS_VERSION_MAJOR >= 5 realtime_publisher_->msg_.variance = params_.variance; diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp index 59d27ebc0c..052c0384d3 100644 --- a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp @@ -208,11 +208,11 @@ TEST_F(RangeSensorBroadcasterTest, Publish_RangeBroadcaster_Success) subscribe_and_get_message(range_msg); EXPECT_EQ(range_msg.header.frame_id, frame_id_); - EXPECT_THAT(range_msg.range, ::testing::FloatEq(sensor_range_)); + EXPECT_THAT(range_msg.range, ::testing::FloatEq(static_cast(sensor_range_))); EXPECT_EQ(range_msg.radiation_type, radiation_type_); - EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); - EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); - EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); + EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(static_cast(field_of_view_))); + EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(static_cast(min_range_))); + EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(static_cast(max_range_))); #if SENSOR_MSGS_VERSION_MAJOR >= 5 EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); #endif @@ -227,30 +227,30 @@ TEST_F(RangeSensorBroadcasterTest, Publish_Bandaries_RangeBroadcaster_Success) sensor_msgs::msg::Range range_msg; - sensor_range_ = 0.10; + sensor_range_ = 0.10f; subscribe_and_get_message(range_msg); EXPECT_EQ(range_msg.header.frame_id, frame_id_); - EXPECT_THAT(range_msg.range, ::testing::FloatEq(sensor_range_)); + EXPECT_THAT(range_msg.range, ::testing::FloatEq(static_cast(sensor_range_))); EXPECT_EQ(range_msg.radiation_type, radiation_type_); - EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); - EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); - EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); + EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(static_cast(field_of_view_))); + EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(static_cast(min_range_))); + EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(static_cast(max_range_))); #if SENSOR_MSGS_VERSION_MAJOR >= 5 - EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); + EXPECT_THAT(range_msg.variance, ::testing::FloatEq(static_cast(variance_))); #endif sensor_range_ = 4.0; subscribe_and_get_message(range_msg); EXPECT_EQ(range_msg.header.frame_id, frame_id_); - EXPECT_THAT(range_msg.range, ::testing::FloatEq(sensor_range_)); + EXPECT_THAT(range_msg.range, ::testing::FloatEq(static_cast(sensor_range_))); EXPECT_EQ(range_msg.radiation_type, radiation_type_); - EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); - EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); - EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); + EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(static_cast(field_of_view_))); + EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(static_cast(min_range_))); + EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(static_cast(max_range_))); #if SENSOR_MSGS_VERSION_MAJOR >= 5 - EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); + EXPECT_THAT(range_msg.variance, ::testing::FloatEq(static_cast(variance_))); #endif } @@ -268,13 +268,13 @@ TEST_F(RangeSensorBroadcasterTest, Publish_OutOfBandaries_RangeBroadcaster_Succe EXPECT_EQ(range_msg.header.frame_id, frame_id_); // Even out of boundaries you will get the out_of_range range value - EXPECT_THAT(range_msg.range, ::testing::FloatEq(sensor_range_)); + EXPECT_THAT(range_msg.range, ::testing::FloatEq(static_cast(sensor_range_))); EXPECT_EQ(range_msg.radiation_type, radiation_type_); - EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); - EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); - EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); + EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(static_cast(field_of_view_))); + EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(static_cast(min_range_))); + EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(static_cast(max_range_))); #if SENSOR_MSGS_VERSION_MAJOR >= 5 - EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); + EXPECT_THAT(range_msg.variance, ::testing::FloatEq(static_cast(variance_))); #endif sensor_range_ = 6.0; @@ -282,13 +282,13 @@ TEST_F(RangeSensorBroadcasterTest, Publish_OutOfBandaries_RangeBroadcaster_Succe EXPECT_EQ(range_msg.header.frame_id, frame_id_); // Even out of boundaries you will get the out_of_range range value - EXPECT_THAT(range_msg.range, ::testing::FloatEq(sensor_range_)); + EXPECT_THAT(range_msg.range, ::testing::FloatEq(static_cast(sensor_range_))); EXPECT_EQ(range_msg.radiation_type, radiation_type_); - EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); - EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); - EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); + EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(static_cast(field_of_view_))); + EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(static_cast(min_range_))); + EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(static_cast(max_range_))); #if SENSOR_MSGS_VERSION_MAJOR >= 5 - EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); + EXPECT_THAT(range_msg.variance, ::testing::FloatEq(static_cast(variance_))); #endif } diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.hpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.hpp index 10696d071f..b7ffa7fe4a 100644 --- a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.hpp +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.hpp @@ -38,6 +38,7 @@ class RangeSensorBroadcasterTest : public ::testing::Test // defining the parameter names same as in test/range_sensor_broadcaster_params.yaml const std::string sensor_name_ = "range_sensor"; const std::string frame_id_ = "range_sensor_frame"; + const std::string interface_name_ = "range"; const double field_of_view_ = 0.1; const int radiation_type_ = 1; diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt index e2bfdbab71..fc79d54b7c 100644 --- a/steering_controllers_library/CMakeLists.txt +++ b/steering_controllers_library/CMakeLists.txt @@ -3,7 +3,8 @@ project(steering_controllers_library LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() # find dependencies diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 93ee823e0f..ff0ab972d5 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -314,10 +314,11 @@ class SteeringControllersLibraryFixture : public ::testing::Test double front_wheels_radius_ = 0.45; double rear_wheels_radius_ = 0.45; - std::array joint_state_values_ = {0.5, 0.5, 0.0, 0.0}; - std::array joint_command_values_ = {1.1, 3.3, 2.2, 4.4}; + std::array joint_state_values_ = {{0.5, 0.5, 0.0, 0.0}}; + std::array joint_command_values_ = {{1.1, 3.3, 2.2, 4.4}}; - std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; + std::array joint_reference_interfaces_ = { + {"linear/velocity", "angular/velocity"}}; std::string steering_interface_name_ = "position"; // defined in setup std::string traction_interface_name_ = ""; diff --git a/tricycle_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt index 7afe150c43..ee877272ab 100644 --- a/tricycle_controller/CMakeLists.txt +++ b/tricycle_controller/CMakeLists.txt @@ -3,7 +3,8 @@ project(tricycle_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/tricycle_steering_controller/CMakeLists.txt b/tricycle_steering_controller/CMakeLists.txt index 24b4cd1a22..9c82ee3574 100644 --- a/tricycle_steering_controller/CMakeLists.txt +++ b/tricycle_steering_controller/CMakeLists.txt @@ -3,7 +3,8 @@ project(tricycle_steering_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() # find dependencies diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index 3b7a053937..c5dd71099f 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -260,15 +260,15 @@ class TricycleSteeringControllerFixture : public ::testing::Test bool open_loop_ = false; unsigned int velocity_rolling_window_size_ = 10; bool position_feedback_ = false; - std::vector rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; - std::vector front_wheels_names_ = {"steering_axis_joint"}; - std::vector joint_names_ = { + std::vector rear_wheels_names_{"rear_right_wheel_joint", "rear_left_wheel_joint"}; + std::vector front_wheels_names_{"steering_axis_joint"}; + std::vector joint_names_{ rear_wheels_names_[0], rear_wheels_names_[1], front_wheels_names_[0]}; - std::vector rear_wheels_preceeding_names_ = { + std::vector rear_wheels_preceeding_names_{ "pid_controller/rear_right_wheel_joint", "pid_controller/rear_left_wheel_joint"}; - std::vector front_wheels_preceeding_names_ = {"pid_controller/steering_axis_joint"}; - std::vector preceeding_joint_names_ = { + std::vector front_wheels_preceeding_names_{"pid_controller/steering_axis_joint"}; + std::vector preceeding_joint_names_{ rear_wheels_preceeding_names_[0], rear_wheels_preceeding_names_[1], front_wheels_preceeding_names_[0]}; @@ -278,9 +278,9 @@ class TricycleSteeringControllerFixture : public ::testing::Test double front_wheels_radius_ = 0.45; double rear_wheels_radius_ = 0.45; - std::array joint_state_values_ = {0.5, 0.5, 0.0}; - std::array joint_command_values_ = {1.1, 3.3, 2.2}; - std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; + std::array joint_state_values_{{0.5, 0.5, 0.0}}; + std::array joint_command_values_{{1.1, 3.3, 2.2}}; + std::array joint_reference_interfaces_{{"linear/velocity", "angular/velocity"}}; std::string steering_interface_name_ = "position"; // defined in setup std::string traction_interface_name_ = ""; diff --git a/velocity_controllers/CMakeLists.txt b/velocity_controllers/CMakeLists.txt index a39cd162fd..feb4eae74f 100644 --- a/velocity_controllers/CMakeLists.txt +++ b/velocity_controllers/CMakeLists.txt @@ -3,7 +3,8 @@ project(velocity_controllers LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS From 3b600f28067f2a2146861db58f3d7c10925e492b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 18 Nov 2024 20:22:09 +0100 Subject: [PATCH 13/84] Add compatibility build for humble+jazzy distro (#1368) --- .../workflows/humble-semi-binary-build.yml | 2 +- .github/workflows/iron-semi-binary-build.yml | 2 +- .github/workflows/jazzy-semi-binary-build.yml | 2 +- ...ling-compatibility-humble-binary-build.yml | 48 +++++++++++++++++++ ...lling-compatibility-jazzy-binary-build.yml | 48 +++++++++++++++++++ .../workflows/rolling-semi-binary-build.yml | 2 +- 6 files changed, 100 insertions(+), 4 deletions(-) create mode 100644 .github/workflows/rolling-compatibility-humble-binary-build.yml create mode 100644 .github/workflows/rolling-compatibility-jazzy-binary-build.yml diff --git a/.github/workflows/humble-semi-binary-build.yml b/.github/workflows/humble-semi-binary-build.yml index 4246d18160..c53c46032a 100644 --- a/.github/workflows/humble-semi-binary-build.yml +++ b/.github/workflows/humble-semi-binary-build.yml @@ -39,7 +39,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_REPO: [main, testing] + ROS_REPO: [testing] with: ros_distro: humble ros_repo: ${{ matrix.ROS_REPO }} diff --git a/.github/workflows/iron-semi-binary-build.yml b/.github/workflows/iron-semi-binary-build.yml index 3aca5e5b70..a0782e50bb 100644 --- a/.github/workflows/iron-semi-binary-build.yml +++ b/.github/workflows/iron-semi-binary-build.yml @@ -40,7 +40,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_REPO: [main, testing] + ROS_REPO: [testing] with: ros_distro: iron ros_repo: ${{ matrix.ROS_REPO }} diff --git a/.github/workflows/jazzy-semi-binary-build.yml b/.github/workflows/jazzy-semi-binary-build.yml index ffa4f914b9..a462d9040a 100644 --- a/.github/workflows/jazzy-semi-binary-build.yml +++ b/.github/workflows/jazzy-semi-binary-build.yml @@ -41,7 +41,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_REPO: [main, testing] + ROS_REPO: [testing] with: ros_distro: jazzy ros_repo: ${{ matrix.ROS_REPO }} diff --git a/.github/workflows/rolling-compatibility-humble-binary-build.yml b/.github/workflows/rolling-compatibility-humble-binary-build.yml new file mode 100644 index 0000000000..7aab02fa62 --- /dev/null +++ b/.github/workflows/rolling-compatibility-humble-binary-build.yml @@ -0,0 +1,48 @@ +name: Check Rolling Compatibility on Humble +# author: Christoph Froehlich +# description: 'Build & test the rolling version on Humble distro.' + +on: + workflow_dispatch: + pull_request: + branches: + - master + - '*feature*' + - '*feature/**' + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-compatibility-humble-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' + push: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-compatibility-humble-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' + +jobs: + build-on-humble: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [humble] + ROS_REPO: [testing] + with: + ros_distro: ${{ matrix.ROS_DISTRO }} + ros_repo: ${{ matrix.ROS_REPO }} + upstream_workspace: ros2_controllers.rolling.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-compatibility-jazzy-binary-build.yml b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml new file mode 100644 index 0000000000..37ffbb84a4 --- /dev/null +++ b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml @@ -0,0 +1,48 @@ +name: Check Rolling Compatibility on Jazzy +# author: Christoph Froehlich +# description: 'Build & test the rolling version on Jazzy distro.' + +on: + workflow_dispatch: + pull_request: + branches: + - master + - '*feature*' + - '*feature/**' + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-compatibility-jazzy-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' + push: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-compatibility-jazzy-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' + +jobs: + build-on-jazzy: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [jazzy] + ROS_REPO: [testing] + with: + ros_distro: ${{ matrix.ROS_DISTRO }} + ros_repo: ${{ matrix.ROS_REPO }} + upstream_workspace: ros2_controllers.rolling.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-semi-binary-build.yml b/.github/workflows/rolling-semi-binary-build.yml index 872c509931..db0dd1fe64 100644 --- a/.github/workflows/rolling-semi-binary-build.yml +++ b/.github/workflows/rolling-semi-binary-build.yml @@ -41,7 +41,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_REPO: [main, testing] + ROS_REPO: [testing] with: ros_distro: rolling ros_repo: ${{ matrix.ROS_REPO }} From 29e84b977be4b8fda791e17fd1ba47ec5b2ef3ef Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 18 Nov 2024 22:11:03 +0100 Subject: [PATCH 14/84] Remove iron workflows and update readme (#1338) Co-authored-by: Dr. Denis --- .github/dependabot.yml | 7 --- .github/mergify.yml | 9 ---- .github/workflows/iron-abi-compatibility.yml | 27 ---------- .github/workflows/iron-binary-build.yml | 50 ------------------- .github/workflows/iron-check-docs.yml | 18 ------- .github/workflows/iron-coverage-build.yml | 38 -------------- .github/workflows/iron-debian-build.yml | 30 ----------- .github/workflows/iron-pre-commit.yml | 13 ----- .../workflows/iron-rhel-semi-binary-build.yml | 29 ----------- .github/workflows/iron-semi-binary-build.yml | 48 ------------------ .github/workflows/iron-source-build.yml | 28 ----------- README.md | 1 - ros2_controllers-not-released.iron.repos | 6 --- ros2_controllers.iron.repos | 21 -------- 14 files changed, 325 deletions(-) delete mode 100644 .github/workflows/iron-abi-compatibility.yml delete mode 100644 .github/workflows/iron-binary-build.yml delete mode 100644 .github/workflows/iron-check-docs.yml delete mode 100644 .github/workflows/iron-coverage-build.yml delete mode 100644 .github/workflows/iron-debian-build.yml delete mode 100644 .github/workflows/iron-pre-commit.yml delete mode 100644 .github/workflows/iron-rhel-semi-binary-build.yml delete mode 100644 .github/workflows/iron-semi-binary-build.yml delete mode 100644 .github/workflows/iron-source-build.yml delete mode 100644 ros2_controllers-not-released.iron.repos delete mode 100644 ros2_controllers.iron.repos diff --git a/.github/dependabot.yml b/.github/dependabot.yml index aafd67c236..f5e9921f23 100644 --- a/.github/dependabot.yml +++ b/.github/dependabot.yml @@ -18,10 +18,3 @@ updates: schedule: interval: "weekly" target-branch: "humble" - - package-ecosystem: "github-actions" - # Workflow files stored in the - # default location of `.github/workflows` - directory: "/" - schedule: - interval: "weekly" - target-branch: "iron" diff --git a/.github/mergify.yml b/.github/mergify.yml index 39ee6b6bc0..c2eda7385b 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -8,15 +8,6 @@ pull_request_rules: branches: - humble - - name: Backport to iron at reviewers discretion - conditions: - - base=master - - "label=backport-iron" - actions: - backport: - branches: - - iron - - name: Ask to resolve conflict conditions: - conflict diff --git a/.github/workflows/iron-abi-compatibility.yml b/.github/workflows/iron-abi-compatibility.yml deleted file mode 100644 index be1aec6680..0000000000 --- a/.github/workflows/iron-abi-compatibility.yml +++ /dev/null @@ -1,27 +0,0 @@ -name: Iron - ABI Compatibility Check -on: - workflow_dispatch: - pull_request: - branches: - - iron - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '.github/workflows/iron-abi-compatibility.yml' - - '**.yaml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_controllers-not-released.iron.repos' - -jobs: - abi_check: - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v4 - - uses: ros-industrial/industrial_ci@master - env: - ROS_DISTRO: iron - ROS_REPO: testing - ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} - NOT_TEST_BUILD: true diff --git a/.github/workflows/iron-binary-build.yml b/.github/workflows/iron-binary-build.yml deleted file mode 100644 index 0d2f0ff7f3..0000000000 --- a/.github/workflows/iron-binary-build.yml +++ /dev/null @@ -1,50 +0,0 @@ -name: Iron Binary Build -# author: Denis Štogl -# description: 'Build & test all dependencies from released (binary) packages.' - -on: - workflow_dispatch: - pull_request: - branches: - - iron - - '*feature*' - - '*feature/**' - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '**.yaml' - - '.github/workflows/iron-binary-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_controllers-not-released.iron.repos' - push: - branches: - - iron - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '**.yaml' - - '.github/workflows/iron-binary-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_controllers-not-released.iron.repos' - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '03 1 * * *' - -jobs: - binary: - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master - strategy: - fail-fast: false - matrix: - ROS_REPO: [main, testing] - with: - ros_distro: iron - ros_repo: ${{ matrix.ROS_REPO }} - upstream_workspace: ros2_controllers-not-released.iron.repos - ref_for_scheduled_build: iron diff --git a/.github/workflows/iron-check-docs.yml b/.github/workflows/iron-check-docs.yml deleted file mode 100644 index f842902055..0000000000 --- a/.github/workflows/iron-check-docs.yml +++ /dev/null @@ -1,18 +0,0 @@ -name: Iron Check Docs - -on: - workflow_dispatch: - pull_request: - branches: - - iron - paths: - - '**.rst' - - '**.md' - - '**.yaml' - -jobs: - check-docs: - name: Check Docs - uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@iron - with: - ROS2_CONTROLLERS_PR: ${{ github.ref }} diff --git a/.github/workflows/iron-coverage-build.yml b/.github/workflows/iron-coverage-build.yml deleted file mode 100644 index e1d2e84bc8..0000000000 --- a/.github/workflows/iron-coverage-build.yml +++ /dev/null @@ -1,38 +0,0 @@ -name: Coverage Build - Iron -on: - workflow_dispatch: - push: - branches: - - iron - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '**.yaml' - - '.github/workflows/iron-coverage-build.yml' - - 'codecov.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_controllers.iron.repos' - pull_request: - branches: - - iron - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '**.yaml' - - '.github/workflows/iron-coverage-build.yml' - - 'codecov.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_controllers.iron.repos' - -jobs: - coverage_iron: - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master - secrets: inherit - with: - ros_distro: iron diff --git a/.github/workflows/iron-debian-build.yml b/.github/workflows/iron-debian-build.yml deleted file mode 100644 index ebd2f46c18..0000000000 --- a/.github/workflows/iron-debian-build.yml +++ /dev/null @@ -1,30 +0,0 @@ -name: Debian Iron Source Build -on: - workflow_dispatch: - pull_request: - branches: - - iron - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '**.yaml' - - '.github/workflows/iron-debian-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_controllers.iron.repos' - schedule: - # Run every day to detect flakiness and broken dependencies - - cron: '03 1 * * *' - - -jobs: - iron_debian: - name: Iron debian build - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-debian-build.yml@master - with: - ros_distro: iron - upstream_workspace: ros2_controllers.iron.repos - ref_for_scheduled_build: iron - skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/iron-pre-commit.yml b/.github/workflows/iron-pre-commit.yml deleted file mode 100644 index a128958031..0000000000 --- a/.github/workflows/iron-pre-commit.yml +++ /dev/null @@ -1,13 +0,0 @@ -name: Pre-Commit - Iron - -on: - workflow_dispatch: - pull_request: - branches: - - iron - -jobs: - pre-commit: - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master - with: - ros_distro: iron diff --git a/.github/workflows/iron-rhel-semi-binary-build.yml b/.github/workflows/iron-rhel-semi-binary-build.yml deleted file mode 100644 index 109dac7550..0000000000 --- a/.github/workflows/iron-rhel-semi-binary-build.yml +++ /dev/null @@ -1,29 +0,0 @@ -name: RHEL Iron Binary Build -on: - workflow_dispatch: - pull_request: - branches: - - iron - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.yaml' - - '.github/workflows/iron-rhel-semi-binary-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_controllers.iron.repos' - schedule: - # Run every day to detect flakiness and broken dependencies - - cron: '03 1 * * *' - - -jobs: - iron_rhel_binary: - name: Iron RHEL binary build - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-rhel-binary-build.yml@master - with: - ros_distro: iron - upstream_workspace: ros2_controllers.iron.repos - ref_for_scheduled_build: iron - skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/iron-semi-binary-build.yml b/.github/workflows/iron-semi-binary-build.yml deleted file mode 100644 index a0782e50bb..0000000000 --- a/.github/workflows/iron-semi-binary-build.yml +++ /dev/null @@ -1,48 +0,0 @@ -name: Iron Semi-Binary Build -# description: 'Build & test that compiles the main dependencies from source.' - -on: - workflow_dispatch: - pull_request: - branches: - - iron - - '*feature*' - - '*feature/**' - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '**.yaml' - - '.github/workflows/iron-semi-binary-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_controllers.iron.repos' - push: - branches: - - iron - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.yaml' - - '.github/workflows/iron-semi-binary-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_controllers.iron.repos' - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '33 1 * * *' - -jobs: - semi_binary: - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master - strategy: - fail-fast: false - matrix: - ROS_REPO: [testing] - with: - ros_distro: iron - ros_repo: ${{ matrix.ROS_REPO }} - upstream_workspace: ros2_controllers.iron.repos - ref_for_scheduled_build: iron diff --git a/.github/workflows/iron-source-build.yml b/.github/workflows/iron-source-build.yml deleted file mode 100644 index c54ead7877..0000000000 --- a/.github/workflows/iron-source-build.yml +++ /dev/null @@ -1,28 +0,0 @@ -name: Iron Source Build -on: - workflow_dispatch: - push: - branches: - - iron - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '**.yaml' - - '.github/workflows/iron-source-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_controllers.iron.repos' - schedule: - # Run every day to detect flakiness and broken dependencies - - cron: '03 3 * * *' - -jobs: - source: - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-ros-tooling-source-build.yml@master - with: - ros_distro: iron - ref: iron - ros2_repo_branch: iron - os_name: ubuntu-22.04 diff --git a/README.md b/README.md index 57a9ecdda1..28a501dcdc 100644 --- a/README.md +++ b/README.md @@ -11,7 +11,6 @@ ROS2 Distro | Branch | Build status | Documentation | Released packages :---------: | :----: | :----------: | :-----------: | :---------------: **Rolling** | [`master`](https://github.com/ros-controls/ros2_controllers/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build.yml?branch=master) | [control.ros.org](https://control.ros.org/master/doc/ros2_controllers/doc/controllers_index.html) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#rolling) **Jazzy** | [`master`](https://github.com/ros-controls/ros2_controllers/tree/master) | [![Jazzy Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/jazzy-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/jazzy-binary-build.yml?branch=master)
[![Jazzy Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/jazzy-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/jazzy-semi-binary-build.yml?branch=master) | [control.ros.org](https://control.ros.org/jazzy/doc/ros2_controllers/doc/controllers_index.html) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#jazzy) -**Iron** | [`iron`](https://github.com/ros-controls/ros2_controllers/tree/iron) | [![Iron Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/iron-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/iron-binary-build.yml?branch=master)
[![Iron Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/iron-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/iron-semi-binary-build.yml?branch=master) | [control.ros.org](https://control.ros.org/iron/doc/ros2_controllers/doc/controllers_index.html) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#iron) **Humble** | [`humble`](https://github.com/ros-controls/ros2_controllers/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-binary-build.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build.yml?branch=master) | [control.ros.org](https://control.ros.org/humble/doc/ros2_controllers/doc/controllers_index.html) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#humble) ### Explanation of different build types diff --git a/ros2_controllers-not-released.iron.repos b/ros2_controllers-not-released.iron.repos deleted file mode 100644 index 1b3910e7e7..0000000000 --- a/ros2_controllers-not-released.iron.repos +++ /dev/null @@ -1,6 +0,0 @@ -repositories: - ## EXAMPLE DEPENDENCY -# : -# type: git -# url: git@github.com:/.git -# version: master diff --git a/ros2_controllers.iron.repos b/ros2_controllers.iron.repos deleted file mode 100644 index 7f2db052cb..0000000000 --- a/ros2_controllers.iron.repos +++ /dev/null @@ -1,21 +0,0 @@ -repositories: - ros2_control: - type: git - url: https://github.com/ros-controls/ros2_control.git - version: iron - realtime_tools: - type: git - url: https://github.com/ros-controls/realtime_tools.git - version: master - control_msgs: - type: git - url: https://github.com/ros-controls/control_msgs.git - version: master - control_toolbox: - type: git - url: https://github.com/ros-controls/control_toolbox.git - version: ros2-master - kinematics_interface: - type: git - url: https://github.com/ros-controls/kinematics_interface.git - version: master From 79358e38100139cd1d91ba8d090a388677d464b3 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 19 Nov 2024 09:04:51 +0100 Subject: [PATCH 15/84] Add Mecanum Drive Controller (#512) --- doc/controllers_index.rst | 1 + doc/release_notes.rst | 4 + mecanum_drive_controller/CMakeLists.txt | 110 ++++ mecanum_drive_controller/doc/userdoc.rst | 61 ++ .../mecanum_drive_controller.hpp | 162 +++++ .../mecanum_drive_controller/odometry.hpp | 104 ++++ .../visibility_control.h | 49 ++ .../mecanum_drive_controller.xml | 7 + mecanum_drive_controller/package.xml | 50 ++ .../src/mecanum_drive_controller.cpp | 527 ++++++++++++++++ .../src/mecanum_drive_controller.yaml | 148 +++++ mecanum_drive_controller/src/odometry.cpp | 124 ++++ .../test/mecanum_drive_controller_params.yaml | 19 + ...um_drive_controller_preceeding_params.yaml | 24 + .../test_load_mecanum_drive_controller.cpp | 52 ++ .../test/test_mecanum_drive_controller.cpp | 576 ++++++++++++++++++ .../test/test_mecanum_drive_controller.hpp | 317 ++++++++++ ...st_mecanum_drive_controller_preceeding.cpp | 99 +++ ros2_controllers/package.xml | 1 + steering_controllers_library/doc/userdoc.rst | 2 +- 20 files changed, 2436 insertions(+), 1 deletion(-) create mode 100644 mecanum_drive_controller/CMakeLists.txt create mode 100644 mecanum_drive_controller/doc/userdoc.rst create mode 100644 mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp create mode 100644 mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp create mode 100644 mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h create mode 100644 mecanum_drive_controller/mecanum_drive_controller.xml create mode 100644 mecanum_drive_controller/package.xml create mode 100644 mecanum_drive_controller/src/mecanum_drive_controller.cpp create mode 100644 mecanum_drive_controller/src/mecanum_drive_controller.yaml create mode 100644 mecanum_drive_controller/src/odometry.cpp create mode 100644 mecanum_drive_controller/test/mecanum_drive_controller_params.yaml create mode 100644 mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml create mode 100644 mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp create mode 100644 mecanum_drive_controller/test/test_mecanum_drive_controller.cpp create mode 100644 mecanum_drive_controller/test/test_mecanum_drive_controller.hpp create mode 100644 mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 0a9c6ec485..fe09839ce5 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -28,6 +28,7 @@ Controllers for Wheeled Mobile Robots :titlesonly: Differential Drive Controller <../diff_drive_controller/doc/userdoc.rst> + Mecanum Drive Controllers <../mecanum_drive_controller/doc/userdoc.rst> Steering Controllers Library <../steering_controllers_library/doc/userdoc.rst> Tricycle Controller <../tricycle_controller/doc/userdoc.rst> diff --git a/doc/release_notes.rst b/doc/release_notes.rst index b1db026df2..974bca880f 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -50,6 +50,10 @@ joint_trajectory_controller * Add the boolean parameter ``set_last_command_interface_value_as_state_on_activation``. When set to ``true``, the last command interface value is used as both the current state and the last commanded state upon activation. When set to ``false``, the current state is used for both (`#1231 `_). +mecanum_drive_controller +************************ +* 🚀 The mecanum_drive_controller was added 🎉 (`#512 `_). + pid_controller ************************ * 🚀 The PID controller was added 🎉 (`#434 `_). diff --git a/mecanum_drive_controller/CMakeLists.txt b/mecanum_drive_controller/CMakeLists.txt new file mode 100644 index 0000000000..48bac58fe7 --- /dev/null +++ b/mecanum_drive_controller/CMakeLists.txt @@ -0,0 +1,110 @@ +cmake_minimum_required(VERSION 3.16) +project(mecanum_drive_controller LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + hardware_interface + generate_parameter_library + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs + tf2 + tf2_geometry_msgs + tf2_msgs +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(mecanum_drive_controller_parameters + src/mecanum_drive_controller.yaml +) + +add_library( + mecanum_drive_controller + SHARED + src/mecanum_drive_controller.cpp + src/odometry.cpp +) +target_compile_features(mecanum_drive_controller PUBLIC cxx_std_17) +target_include_directories(mecanum_drive_controller PUBLIC + "$" + "$") +target_link_libraries(mecanum_drive_controller PUBLIC + mecanum_drive_controller_parameters) +ament_target_dependencies(mecanum_drive_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(mecanum_drive_controller PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL") + +pluginlib_export_plugin_description_file( + controller_interface mecanum_drive_controller.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_mecanum_drive_controller test/test_load_mecanum_drive_controller.cpp) + ament_target_dependencies(test_load_mecanum_drive_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + + add_rostest_with_parameters_gmock( + test_mecanum_drive_controller test/test_mecanum_drive_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/mecanum_drive_controller_params.yaml) + target_include_directories(test_mecanum_drive_controller PRIVATE include) + target_link_libraries(test_mecanum_drive_controller mecanum_drive_controller) + ament_target_dependencies( + test_mecanum_drive_controller + controller_interface + hardware_interface + ) + + add_rostest_with_parameters_gmock( + test_mecanum_drive_controller_preceeding test/test_mecanum_drive_controller_preceeding.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/mecanum_drive_controller_preceeding_params.yaml) + target_include_directories(test_mecanum_drive_controller_preceeding PRIVATE include) + target_link_libraries(test_mecanum_drive_controller_preceeding mecanum_drive_controller) + ament_target_dependencies( + test_mecanum_drive_controller_preceeding + controller_interface + hardware_interface + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include/mecanum_drive_controller +) + +install( + TARGETS mecanum_drive_controller mecanum_drive_controller_parameters + EXPORT export_mecanum_drive_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_mecanum_drive_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/mecanum_drive_controller/doc/userdoc.rst b/mecanum_drive_controller/doc/userdoc.rst new file mode 100644 index 0000000000..37c8d7d0e7 --- /dev/null +++ b/mecanum_drive_controller/doc/userdoc.rst @@ -0,0 +1,61 @@ +.. _mecanum_drive_controller_userdoc: + +mecanum_drive_controller +========================= + +Library with shared functionalities for mobile robot controllers with mecanum drive (four wheels). +The library implements generic odometry and update methods and defines the main interfaces. + +Execution logic of the controller +---------------------------------- + +The controller uses velocity input, i.e., stamped Twist messages where linear ``x``, ``y``, and angular ``z`` components are used. +Values in other components are ignored. +In the chain mode, the controller provides three reference interfaces, one for linear velocity and one for steering angle position. +Other relevant features are: + + - odometry publishing as Odometry and TF message; + - input command timeout based on a parameter. + +Note about odometry calculation: +In the DiffDRiveController, the velocity is filtered out, but we prefer to return it raw and let the user perform post-processing at will. +We prefer this way of doing so as filtering introduces delay (which makes it difficult to interpret and compare behavior curves). + + +Description of controller's interfaces +-------------------------------------- + +References (from a preceding controller) +,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, +- / [double] # in [rad] or [rad/s] + +Commands +,,,,,,,,, +- / [double] # in [rad] or [rad/s] + +States +,,,,,,, +- / [double] # in [rad] or [rad/s] + ..note :: + + ``joint_names[i]`` can be of ``state_joint_names`` parameter (if used), ``command_joint_names`` otherwise. + + +Subscribers +,,,,,,,,,,,, +Used when the controller is not in chained mode (``in_chained_mode == false``). + +- /reference [geometry_msgs/msg/TwistStamped] + +Publishers +,,,,,,,,,,, +- /odometry [nav_msgs/msg/Odometry] +- /tf_odometry [tf2_msgs/msg/TFMessage] +- /controller_state [control_msgs/msg/MecanumDriveControllerState] + +Parameters +,,,,,,,,,,, + +For a list of parameters and their meaning, see the YAML file in the ``src`` folder of the controller's package. + +For an exemplary parameterization, see the ``test`` folder of the controller's package. diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp new file mode 100644 index 0000000000..0fd7f6af29 --- /dev/null +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -0,0 +1,162 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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 MECANUM_DRIVE_CONTROLLER__MECANUM_DRIVE_CONTROLLER_HPP_ +#define MECANUM_DRIVE_CONTROLLER__MECANUM_DRIVE_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "controller_interface/chainable_controller_interface.hpp" +#include "mecanum_drive_controller/odometry.hpp" +#include "mecanum_drive_controller/visibility_control.h" +#include "mecanum_drive_controller_parameters.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" +#include "std_srvs/srv/set_bool.hpp" + +#include "control_msgs/msg/mecanum_drive_controller_state.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "tf2_msgs/msg/tf_message.hpp" +namespace mecanum_drive_controller +{ +// name constants for state interfaces +static constexpr size_t NR_STATE_ITFS = 4; + +// name constants for command interfaces +static constexpr size_t NR_CMD_ITFS = 4; + +// name constants for reference interfaces +static constexpr size_t NR_REF_ITFS = 3; + +class MecanumDriveController : public controller_interface::ChainableControllerInterface +{ +public: + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + MecanumDriveController(); + + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_init() override; + + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::return_type update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::return_type update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + using ControllerReferenceMsg = geometry_msgs::msg::TwistStamped; + using OdomStateMsg = nav_msgs::msg::Odometry; + using TfStateMsg = tf2_msgs::msg::TFMessage; + using ControllerStateMsg = control_msgs::msg::MecanumDriveControllerState; + +protected: + std::shared_ptr param_listener_; + mecanum_drive_controller::Params params_; + + /** + * The list is sorted in the following order: + * - front left wheel + * - front right wheel + * - back right wheel + * - back left wheel + */ + enum WheelIndex : std::size_t + { + FRONT_LEFT = 0, + FRONT_RIGHT = 1, + REAR_RIGHT = 2, + REAR_LEFT = 3 + }; + + /** + * Internal lists with joint names sorted as in `WheelIndex` enum. + */ + std::vector command_joint_names_; + + /** + * Internal lists with joint names sorted as in `WheelIndex` enum. + * If parameters for state joint names are *not* defined, this list is the same as + * `command_joint_names_`. + */ + std::vector state_joint_names_; + + // Names of the references, ex: high level vel commands from MoveIt, Nav2, etc. + // used for preceding controller + std::vector reference_names_; + + // Command subscribers and Controller State, odom state, tf state publishers + rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; + realtime_tools::RealtimeBuffer> input_ref_; + rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); + + using OdomStatePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr odom_s_publisher_; + std::unique_ptr rt_odom_state_publisher_; + + using TfStatePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr tf_odom_s_publisher_; + std::unique_ptr rt_tf_odom_state_publisher_; + + using ControllerStatePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr controller_s_publisher_; + std::unique_ptr controller_state_publisher_; + + // override methods from ChainableControllerInterface + std::vector on_export_reference_interfaces() override; + + bool on_set_chained_mode(bool chained_mode) override; + + Odometry odometry_; + +private: + // callback for topic interface + MECANUM_DRIVE_CONTROLLER__VISIBILITY_LOCAL + void reference_callback(const std::shared_ptr msg); + + double velocity_in_center_frame_linear_x_; // [m/s] + double velocity_in_center_frame_linear_y_; // [m/s] + double velocity_in_center_frame_angular_z_; // [rad/s] +}; + +} // namespace mecanum_drive_controller + +#endif // MECANUM_DRIVE_CONTROLLER__MECANUM_DRIVE_CONTROLLER_HPP_ diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp new file mode 100644 index 0000000000..ac1ad060dd --- /dev/null +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp @@ -0,0 +1,104 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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 MECANUM_DRIVE_CONTROLLER__ODOMETRY_HPP_ +#define MECANUM_DRIVE_CONTROLLER__ODOMETRY_HPP_ + +#include "geometry_msgs/msg/twist.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" + +#define PLANAR_POINT_DIM 3 + +namespace mecanum_drive_controller +{ +/// \brief The Odometry class handles odometry readings +/// (2D pose and velocity with related timestamp) +class Odometry +{ +public: + /// Integration function, used to integrate the odometry: + typedef std::function IntegrationFunction; + + /// \brief Constructor + /// Timestamp will get the current time value + /// Value will be set to zero + Odometry(); + + /// \brief Initialize the odometry + /// \param time Current time + void init(const rclcpp::Time & time, std::array base_frame_offset); + + /// \brief Updates the odometry class with latest wheels position + /// \param wheel_front_left_vel Wheel velocity [rad/s] + /// \param wheel_rear_left_vel Wheel velocity [rad/s] + /// \param wheel_rear_right_vel Wheel velocity [rad/s] + /// \param wheel_front_right_vel Wheel velocity [rad/s] + /// \param time Current time + /// \return true if the odometry is actually updated + bool update( + const double wheel_front_left_vel, const double wheel_rear_left_vel, + const double wheel_rear_right_vel, const double wheel_front_right_vel, const double dt); + + /// \return position (x component) [m] + double getX() const { return position_x_in_base_frame_; } + /// \return position (y component) [m] + double getY() const { return position_y_in_base_frame_; } + /// \return orientation (z component) [m] + double getRz() const { return orientation_z_in_base_frame_; } + /// \return body velocity of the base frame (linear x component) [m/s] + double getVx() const { return velocity_in_base_frame_linear_x; } + /// \return body velocity of the base frame (linear y component) [m/s] + double getVy() const { return velocity_in_base_frame_linear_y; } + /// \return body velocity of the base frame (angular z component) [m/s] + double getWz() const + { + return velocity_in_base_frame_angular_z; + ; + } + + /// \brief Sets the wheels parameters: mecanum geometric param and radius + /// \param sum_of_robot_center_projection_on_X_Y_axis Wheels geometric param + /// (used in mecanum wheels' ik) [m] + /// \param wheels_radius Wheels radius [m] + void setWheelsParams( + const double sum_of_robot_center_projection_on_X_Y_axis, const double wheels_radius); + +private: + /// Current timestamp: + rclcpp::Time timestamp_; + + /// Reference frame (wrt to center frame). [x, y, theta] + std::array base_frame_offset_; + + /// Current pose: + double position_x_in_base_frame_; // [m] + double position_y_in_base_frame_; // [m] + double orientation_z_in_base_frame_; // [rad] + + double velocity_in_base_frame_linear_x; // [m/s] + double velocity_in_base_frame_linear_y; // [m/s] + double velocity_in_base_frame_angular_z; // [rad/s] + + /// Wheels kinematic parameters [m]: + /// lx and ly represent the distance from the robot's center to the wheels + /// projected on the x and y axis with origin at robots center respectively, + /// sum_of_robot_center_projection_on_X_Y_axis_ = lx+ly + double sum_of_robot_center_projection_on_X_Y_axis_; + double wheels_radius_; // [m] +}; + +} // namespace mecanum_drive_controller + +#endif // MECANUM_DRIVE_CONTROLLER__ODOMETRY_HPP_ */ diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h b/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h new file mode 100644 index 0000000000..3222b2fa52 --- /dev/null +++ b/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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 MECANUM_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_ +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef MECANUM_DRIVE_CONTROLLER__VISIBILITY_BUILDING_DLL +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC MECANUM_DRIVE_CONTROLLER__VISIBILITY_EXPORT +#else +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC MECANUM_DRIVE_CONTROLLER__VISIBILITY_IMPORT +#endif +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC_TYPE MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_LOCAL +#else +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_LOCAL +#endif +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // MECANUM_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/mecanum_drive_controller/mecanum_drive_controller.xml b/mecanum_drive_controller/mecanum_drive_controller.xml new file mode 100644 index 0000000000..c012ad53ca --- /dev/null +++ b/mecanum_drive_controller/mecanum_drive_controller.xml @@ -0,0 +1,7 @@ + + + + The mecanum drive controller transforms linear and angular velocity messages into signals for each wheel(s) for a 4 mecanum wheeled robot. + + diff --git a/mecanum_drive_controller/package.xml b/mecanum_drive_controller/package.xml new file mode 100644 index 0000000000..97aadf4327 --- /dev/null +++ b/mecanum_drive_controller/package.xml @@ -0,0 +1,50 @@ + + + + mecanum_drive_controller + 0.0.0 + Implementation of mecanum drive controller for 4 wheel drive. + + + Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Dr.-Ing. Denis Štogl + Giridhar Bukka + + ament_cmake + + generate_parameter_library + + control_msgs + controller_interface + geometry_msgs + hardware_interface + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + rcpputils + std_srvs + tf2 + tf2_geometry_msgs + tf2_msgs + + ament_cmake_gmock + controller_manager + hardware_interface_testing + ros2_control_test_assets + + + ament_cmake + + diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp new file mode 100644 index 0000000000..2fd5fdbdcb --- /dev/null +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -0,0 +1,527 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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. + +#include "mecanum_drive_controller/mecanum_drive_controller.hpp" + +#include +#include +#include +#include + +#include "controller_interface/helpers.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "tf2/transform_datatypes.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace +{ // utility + +using ControllerReferenceMsg = + mecanum_drive_controller::MecanumDriveController::ControllerReferenceMsg; + +// called from RT control loop +void reset_controller_reference_msg( + const std::shared_ptr & msg, + const std::shared_ptr & node) +{ + msg->header.stamp = node->now(); + msg->twist.linear.x = std::numeric_limits::quiet_NaN(); + msg->twist.linear.y = std::numeric_limits::quiet_NaN(); + msg->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg->twist.angular.z = std::numeric_limits::quiet_NaN(); +} + +} // namespace + +namespace mecanum_drive_controller +{ +MecanumDriveController::MecanumDriveController() +: controller_interface::ChainableControllerInterface() +{ +} + +controller_interface::CallbackReturn MecanumDriveController::on_init() +{ + try + { + param_listener_ = std::make_shared(get_node()); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during controller's init with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn MecanumDriveController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + params_ = param_listener_->get_params(); + + auto prepare_lists_with_joint_names = + [&command_joints = this->command_joint_names_, &state_joints = this->state_joint_names_]( + const std::size_t index, const std::string & command_joint_name, + const std::string & state_joint_name) + { + command_joints[index] = command_joint_name; + if (state_joint_name.empty()) + { + state_joints[index] = command_joint_name; + } + else + { + state_joints[index] = state_joint_name; + } + }; + + command_joint_names_.resize(4); + state_joint_names_.resize(4); + + // The joint names are sorted according to the order documented in the header file! + prepare_lists_with_joint_names( + FRONT_LEFT, params_.front_left_wheel_command_joint_name, + params_.front_left_wheel_state_joint_name); + prepare_lists_with_joint_names( + FRONT_RIGHT, params_.front_right_wheel_command_joint_name, + params_.front_right_wheel_state_joint_name); + prepare_lists_with_joint_names( + REAR_RIGHT, params_.rear_right_wheel_command_joint_name, + params_.rear_right_wheel_state_joint_name); + prepare_lists_with_joint_names( + REAR_LEFT, params_.rear_left_wheel_command_joint_name, + params_.rear_left_wheel_state_joint_name); + + // Set wheel params for the odometry computation + odometry_.setWheelsParams( + params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis, + params_.kinematics.wheels_radius); + + // topics QoS + auto subscribers_qos = rclcpp::SystemDefaultsQoS(); + subscribers_qos.keep_last(1); + subscribers_qos.best_effort(); + + // Reference Subscriber + ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout); + ref_subscriber_ = get_node()->create_subscription( + "~/reference", subscribers_qos, + std::bind(&MecanumDriveController::reference_callback, this, std::placeholders::_1)); + + std::shared_ptr msg = std::make_shared(); + reset_controller_reference_msg(msg, get_node()); + input_ref_.writeFromNonRT(msg); + + try + { + // Odom state publisher + odom_s_publisher_ = + get_node()->create_publisher("~/odometry", rclcpp::SystemDefaultsQoS()); + rt_odom_state_publisher_ = std::make_unique(odom_s_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + rt_odom_state_publisher_->lock(); + rt_odom_state_publisher_->msg_.header.stamp = get_node()->now(); + rt_odom_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; + rt_odom_state_publisher_->msg_.child_frame_id = params_.base_frame_id; + rt_odom_state_publisher_->msg_.pose.pose.position.z = 0; + + auto & covariance = rt_odom_state_publisher_->msg_.twist.covariance; + constexpr size_t NUM_DIMENSIONS = 6; + for (size_t index = 0; index < 6; ++index) + { + const size_t diagonal_index = NUM_DIMENSIONS * index + index; + covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; + covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; + } + rt_odom_state_publisher_->unlock(); + + try + { + // Tf State publisher + tf_odom_s_publisher_ = + get_node()->create_publisher("~/tf_odometry", rclcpp::SystemDefaultsQoS()); + rt_tf_odom_state_publisher_ = std::make_unique(tf_odom_s_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + rt_tf_odom_state_publisher_->lock(); + rt_tf_odom_state_publisher_->msg_.transforms.resize(1); + rt_tf_odom_state_publisher_->msg_.transforms[0].header.stamp = get_node()->now(); + rt_tf_odom_state_publisher_->msg_.transforms[0].header.frame_id = params_.odom_frame_id; + rt_tf_odom_state_publisher_->msg_.transforms[0].child_frame_id = params_.base_frame_id; + rt_tf_odom_state_publisher_->msg_.transforms[0].transform.translation.z = 0.0; + rt_tf_odom_state_publisher_->unlock(); + + try + { + // controller State publisher + controller_s_publisher_ = get_node()->create_publisher( + "~/controller_state", rclcpp::SystemDefaultsQoS()); + controller_state_publisher_ = + std::make_unique(controller_s_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, + "Exception thrown during publisher creation at configure stage " + "with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + controller_state_publisher_->lock(); + controller_state_publisher_->msg_.header.stamp = get_node()->now(); + controller_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; + controller_state_publisher_->unlock(); + + RCLCPP_INFO(get_node()->get_logger(), "MecanumDriveController configured successfully"); + + return controller_interface::CallbackReturn::SUCCESS; +} + +void MecanumDriveController::reference_callback(const std::shared_ptr msg) +{ + // If no timestamp provided, use current time for command timestamp + if (msg->header.stamp.sec == 0 && msg->header.stamp.nanosec == 0u) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Timestamp in header is missing, using current time as command timestamp."); + msg->header.stamp = get_node()->now(); + } + + const auto age_of_last_command = get_node()->now() - msg->header.stamp; + + // Check the timeout condition + if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) + { + input_ref_.writeFromNonRT(msg); + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Received message has timestamp %.10f older by %.10f than allowed timeout (%.4f).", + rclcpp::Time(msg->header.stamp).seconds(), age_of_last_command.seconds(), + ref_timeout_.seconds()); + + reset_controller_reference_msg(msg, get_node()); + } +} + +controller_interface::InterfaceConfiguration +MecanumDriveController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + command_interfaces_config.names.reserve(command_joint_names_.size()); + for (const auto & joint : command_joint_names_) + { + command_interfaces_config.names.push_back(joint + "/" + hardware_interface::HW_IF_VELOCITY); + } + + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration MecanumDriveController::state_interface_configuration() + const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + state_interfaces_config.names.reserve(state_joint_names_.size()); + + for (const auto & joint : state_joint_names_) + { + state_interfaces_config.names.push_back(joint + "/" + hardware_interface::HW_IF_VELOCITY); + } + + return state_interfaces_config; +} + +std::vector +MecanumDriveController::on_export_reference_interfaces() +{ + reference_interfaces_.resize(NR_REF_ITFS, std::numeric_limits::quiet_NaN()); + + std::vector reference_interfaces; + + reference_interfaces.reserve(reference_interfaces_.size()); + + std::vector reference_interface_names = { + "linear/x/velocity", "linear/y/velocity", "angular/z/velocity"}; + + for (size_t i = 0; i < reference_interfaces_.size(); ++i) + { + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name(), reference_interface_names[i], &reference_interfaces_[i])); + } + + return reference_interfaces; +} + +bool MecanumDriveController::on_set_chained_mode(bool chained_mode) +{ + // Always accept switch to/from chained mode + return true || chained_mode; +} + +controller_interface::CallbackReturn MecanumDriveController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // Set default value in command + reset_controller_reference_msg(*(input_ref_.readFromRT()), get_node()); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn MecanumDriveController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + bool value_set_no_error = true; + for (size_t i = 0; i < NR_CMD_ITFS; ++i) + { + value_set_no_error &= + command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); + } + if (!value_set_no_error) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Setting values to command interfaces has failed! " + "This means that you are maybe blocking the interface in your hardware for too long."); + return controller_interface::CallbackReturn::FAILURE; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type MecanumDriveController::update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) +{ + auto current_ref = *(input_ref_.readFromRT()); + const auto age_of_last_command = time - (current_ref)->header.stamp; + + // send message only if there is no timeout + if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) + { + if ( + !std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.linear.y) && + !std::isnan(current_ref->twist.angular.z)) + { + reference_interfaces_[0] = current_ref->twist.linear.x; + reference_interfaces_[1] = current_ref->twist.linear.y; + reference_interfaces_[2] = current_ref->twist.angular.z; + + if (ref_timeout_ == rclcpp::Duration::from_seconds(0)) + { + current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref->twist.linear.y = std::numeric_limits::quiet_NaN(); + current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); + } + } + } + else + { + if ( + !std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.linear.y) && + !std::isnan(current_ref->twist.angular.z)) + { + reference_interfaces_[0] = 0.0; + reference_interfaces_[1] = 0.0; + reference_interfaces_[2] = 0.0; + + current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref->twist.linear.y = std::numeric_limits::quiet_NaN(); + current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); + } + } + + return controller_interface::return_type::OK; +} + +controller_interface::return_type MecanumDriveController::update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + // FORWARD KINEMATICS (odometry). + const double wheel_front_left_state_vel = state_interfaces_[FRONT_LEFT].get_value(); + const double wheel_front_right_state_vel = state_interfaces_[FRONT_RIGHT].get_value(); + const double wheel_rear_right_state_vel = state_interfaces_[REAR_RIGHT].get_value(); + const double wheel_rear_left_state_vel = state_interfaces_[REAR_LEFT].get_value(); + + if ( + !std::isnan(wheel_front_left_state_vel) && !std::isnan(wheel_rear_left_state_vel) && + !std::isnan(wheel_rear_right_state_vel) && !std::isnan(wheel_front_right_state_vel)) + { + // Estimate twist (using joint information) and integrate + odometry_.update( + wheel_front_left_state_vel, wheel_rear_left_state_vel, wheel_rear_right_state_vel, + wheel_front_right_state_vel, period.seconds()); + } + + // INVERSE KINEMATICS (move robot). + // Compute wheels velocities (this is the actual ik): + // NOTE: the input desired twist (from topic `~/reference`) is a body twist. + if ( + !std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1]) && + !std::isnan(reference_interfaces_[2])) + { + tf2::Quaternion quaternion; + quaternion.setRPY(0.0, 0.0, params_.kinematics.base_frame_offset.theta); + /// \note The variables meaning: + /// rotation_from_base_to_center: Rotation transformation matrix, to transform from + /// base frame to center frame + /// linear_trans_from_base_to_center: offset/linear transformation matrix, to + /// transform from base frame to center frame + + tf2::Matrix3x3 rotation_from_base_to_center = tf2::Matrix3x3((quaternion)); + tf2::Vector3 velocity_in_base_frame_w_r_t_center_frame_ = + rotation_from_base_to_center * + tf2::Vector3(reference_interfaces_[0], reference_interfaces_[1], 0.0); + tf2::Vector3 linear_trans_from_base_to_center = tf2::Vector3( + params_.kinematics.base_frame_offset.x, params_.kinematics.base_frame_offset.y, 0.0); + + velocity_in_center_frame_linear_x_ = + velocity_in_base_frame_w_r_t_center_frame_.x() + + linear_trans_from_base_to_center.y() * reference_interfaces_[2]; + velocity_in_center_frame_linear_y_ = + velocity_in_base_frame_w_r_t_center_frame_.y() - + linear_trans_from_base_to_center.x() * reference_interfaces_[2]; + velocity_in_center_frame_angular_z_ = reference_interfaces_[2]; + + const double wheel_front_left_vel = + 1.0 / params_.kinematics.wheels_radius * + (velocity_in_center_frame_linear_x_ - velocity_in_center_frame_linear_y_ - + params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + velocity_in_center_frame_angular_z_); + const double wheel_front_right_vel = + 1.0 / params_.kinematics.wheels_radius * + (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ + + params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + velocity_in_center_frame_angular_z_); + const double wheel_rear_right_vel = + 1.0 / params_.kinematics.wheels_radius * + (velocity_in_center_frame_linear_x_ - velocity_in_center_frame_linear_y_ + + params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + velocity_in_center_frame_angular_z_); + const double wheel_rear_left_vel = + 1.0 / params_.kinematics.wheels_radius * + (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ - + params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + velocity_in_center_frame_angular_z_); + + // Set wheels velocities - The joint names are sorted according to the order documented in the + // header file! + const bool value_set_error = + command_interfaces_[FRONT_LEFT].set_value(wheel_front_left_vel) && + command_interfaces_[FRONT_RIGHT].set_value(wheel_front_right_vel) && + command_interfaces_[REAR_RIGHT].set_value(wheel_rear_right_vel) && + command_interfaces_[REAR_LEFT].set_value(wheel_rear_left_vel); + RCLCPP_ERROR_EXPRESSION( + get_node()->get_logger(), !value_set_error, + "Setting values to command interfaces has failed! " + "This means that you are maybe blocking the interface in your hardware for too long."); + } + else + { + const bool value_set_error = command_interfaces_[FRONT_LEFT].set_value(0.0) && + command_interfaces_[FRONT_RIGHT].set_value(0.0) && + command_interfaces_[REAR_RIGHT].set_value(0.0) && + command_interfaces_[REAR_LEFT].set_value(0.0); + RCLCPP_ERROR_EXPRESSION( + get_node()->get_logger(), !value_set_error, + "Setting values to command interfaces has failed! " + "This means that you are maybe blocking the interface in your hardware for too long."); + } + + // Publish odometry message + // Compute and store orientation info + tf2::Quaternion orientation; + orientation.setRPY(0.0, 0.0, odometry_.getRz()); + + // Populate odom message and publish + if (rt_odom_state_publisher_->trylock()) + { + rt_odom_state_publisher_->msg_.header.stamp = time; + rt_odom_state_publisher_->msg_.pose.pose.position.x = odometry_.getX(); + rt_odom_state_publisher_->msg_.pose.pose.position.y = odometry_.getY(); + rt_odom_state_publisher_->msg_.pose.pose.orientation = tf2::toMsg(orientation); + rt_odom_state_publisher_->msg_.twist.twist.linear.x = odometry_.getVx(); + rt_odom_state_publisher_->msg_.twist.twist.linear.y = odometry_.getVy(); + rt_odom_state_publisher_->msg_.twist.twist.angular.z = odometry_.getWz(); + rt_odom_state_publisher_->unlockAndPublish(); + } + + // Publish tf /odom frame + if (params_.enable_odom_tf && rt_tf_odom_state_publisher_->trylock()) + { + rt_tf_odom_state_publisher_->msg_.transforms.front().header.stamp = time; + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.x = odometry_.getX(); + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.y = odometry_.getY(); + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.rotation = + tf2::toMsg(orientation); + rt_tf_odom_state_publisher_->unlockAndPublish(); + } + + if (controller_state_publisher_->trylock()) + { + controller_state_publisher_->msg_.header.stamp = get_node()->now(); + controller_state_publisher_->msg_.front_left_wheel_velocity = + state_interfaces_[FRONT_LEFT].get_value(); + controller_state_publisher_->msg_.front_right_wheel_velocity = + state_interfaces_[FRONT_RIGHT].get_value(); + controller_state_publisher_->msg_.back_right_wheel_velocity = + state_interfaces_[REAR_RIGHT].get_value(); + controller_state_publisher_->msg_.back_left_wheel_velocity = + state_interfaces_[REAR_LEFT].get_value(); + controller_state_publisher_->msg_.reference_velocity.linear.x = reference_interfaces_[0]; + controller_state_publisher_->msg_.reference_velocity.linear.y = reference_interfaces_[1]; + controller_state_publisher_->msg_.reference_velocity.angular.z = reference_interfaces_[2]; + controller_state_publisher_->unlockAndPublish(); + } + + reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[2] = std::numeric_limits::quiet_NaN(); + + return controller_interface::return_type::OK; +} + +} // namespace mecanum_drive_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + mecanum_drive_controller::MecanumDriveController, + controller_interface::ChainableControllerInterface) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.yaml b/mecanum_drive_controller/src/mecanum_drive_controller.yaml new file mode 100644 index 0000000000..896b731953 --- /dev/null +++ b/mecanum_drive_controller/src/mecanum_drive_controller.yaml @@ -0,0 +1,148 @@ +mecanum_drive_controller: + reference_timeout: { + type: double, + default_value: 0.0, + description: "Timeout for controller references after which they will be reset. This is especially useful for controllers that can cause unwanted and dangerous behavior if reference is not reset, e.g., velocity controllers. If value is 0 the reference is reset after each run.", + } + + # Command joint names + front_left_wheel_command_joint_name: { + type: string, + default_value: "", + description: "Name of the joints for commanding front left wheel.", + read_only: true, + validation: { + not_empty<>: null, + } + } + + front_right_wheel_command_joint_name: { + type: string, + default_value: "", + description: "Name of the joints for commanding front right wheel.", + read_only: true, + validation: { + not_empty<>: null, + } + } + + rear_right_wheel_command_joint_name: { + type: string, + default_value: "", + description: "Name of the joints for commanding rear right wheel.", + read_only: true, + validation: { + not_empty<>: null, + } + } + + rear_left_wheel_command_joint_name: { + type: string, + default_value: "", + description: "Name of the joints for commanding rear left wheel.", + read_only: true, + validation: { + not_empty<>: null, + } + } + + # State joint names + front_left_wheel_state_joint_name: { + type: string, + default_value: "", + description: "(optional) Specifies a joint name for reading states of front left wheel. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", + read_only: true, + } + + front_right_wheel_state_joint_name: { + type: string, + default_value: "", + description: "(optional) Specifies a joint name for reading states of front right wheel. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", + read_only: true, + } + + rear_right_wheel_state_joint_name: { + type: string, + default_value: "", + description: "(optional) Specifies a joint name for reading states of rear right wheel. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", + read_only: true, + } + + rear_left_wheel_state_joint_name: { + type: string, + default_value: "", + description: "(optional) Specifies a joint name for reading states of rear left wheel. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", + read_only: true, + } + + kinematics: + base_frame_offset: + x: { + type: double, + default_value: 0.0, + description: "Base frame offset along X axis of base_frame (base_link frame)." , + read_only: true, + } + y: { + type: double, + default_value: 0.0, + description: "Base frame offset along Y axis of base_frame (base_link frame)." , + read_only: true, + } + theta: { + type: double, + default_value: 0.0, + description: "Base frame offset along Theta axis of base_frame (base_link frame).", + read_only: true, + } + + wheels_radius: { + type: double, + default_value: 0.0, + description: "Wheel's radius.", + read_only: false, + validation: { + gt<>: [0.0] + } + } + + sum_of_robot_center_projection_on_X_Y_axis: { + type: double, + default_value: 0.0, + description: "Wheels geometric param used in mecanum wheels' IK. lx and ly represent the distance from the robot's center to the wheels projected on the x and y axis with origin at robots center respectively, sum_of_robot_center_projection_on_X_Y_axis = lx+ly", + read_only: false, + } + + base_frame_id: { + type: string, + default_value: "base_link", + description: "Base frame_id set to value of base_frame_id.", + read_only: false, + + } + odom_frame_id: { + type: string, + default_value: "odom", + description: "Odometry frame_id set to value of odom_frame_id.", + read_only: false, + } + enable_odom_tf: { + type: bool, + default_value: true, + description: "Publishing to tf is enabled or disabled?", + read_only: false, + } + + twist_covariance_diagonal: { + type: double_array, + default_value: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], + description: "Diagonal values of twist covariance matrix.", + read_only: false, + } + + pose_covariance_diagonal: { + type: double_array, + default_value: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], + description: "Diagonal values of pose covariance matrix.", + read_only: false, + } diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp new file mode 100644 index 0000000000..bb873fcfdb --- /dev/null +++ b/mecanum_drive_controller/src/odometry.cpp @@ -0,0 +1,124 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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. + +#include "mecanum_drive_controller/odometry.hpp" + +#include "tf2/transform_datatypes.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace mecanum_drive_controller +{ +Odometry::Odometry() +: timestamp_(0.0), + position_x_in_base_frame_(0.0), + position_y_in_base_frame_(0.0), + orientation_z_in_base_frame_(0.0), + velocity_in_base_frame_linear_x(0.0), + velocity_in_base_frame_linear_y(0.0), + velocity_in_base_frame_angular_z(0.0), + sum_of_robot_center_projection_on_X_Y_axis_(0.0), + wheels_radius_(0.0) +{ +} + +void Odometry::init( + const rclcpp::Time & time, std::array base_frame_offset) +{ + // Reset timestamp: + timestamp_ = time; + + // Base frame offset (wrt to center frame). + base_frame_offset_[0] = base_frame_offset[0]; + base_frame_offset_[1] = base_frame_offset[1]; + base_frame_offset_[2] = base_frame_offset[2]; +} + +bool Odometry::update( + const double wheel_front_left_vel, const double wheel_rear_left_vel, + const double wheel_rear_right_vel, const double wheel_front_right_vel, const double dt) +{ + /// We cannot estimate the speed with very small time intervals: + // const double dt = (time - timestamp_).toSec(); + if (dt < 0.0001) return false; // Interval too small to integrate with + + /// Compute FK (i.e. compute mobile robot's body twist out of its wheels velocities): + /// NOTE: the mecanum IK gives the body speed at the center frame, we then offset this velocity + /// at the base frame. + /// NOTE: in the diff drive the velocity is filtered out, but we prefer to return it raw and + /// let the user perform post-processing at will. + /// We prefer this way of doing as filtering introduces delay (which makes it difficult + /// to interpret and compare behavior curves). + + /// \note The variables meaning: + /// angular_transformation_from_center_2_base: Rotation transformation matrix, to transform + /// from center frame to base frame + /// linear_transformation_from_center_2_base: offset/linear transformation matrix, + /// to transform from center frame to base frame + + double velocity_in_center_frame_linear_x = + 0.25 * wheels_radius_ * + (wheel_front_left_vel + wheel_rear_left_vel + wheel_rear_right_vel + wheel_front_right_vel); + double velocity_in_center_frame_linear_y = + 0.25 * wheels_radius_ * + (-wheel_front_left_vel + wheel_rear_left_vel - wheel_rear_right_vel + wheel_front_right_vel); + double velocity_in_center_frame_angular_z = + 0.25 * wheels_radius_ / sum_of_robot_center_projection_on_X_Y_axis_ * + (-wheel_front_left_vel - wheel_rear_left_vel + wheel_rear_right_vel + wheel_front_right_vel); + + tf2::Quaternion orientation_R_c_b; + orientation_R_c_b.setRPY(0.0, 0.0, -base_frame_offset_[2]); + + tf2::Matrix3x3 angular_transformation_from_center_2_base = tf2::Matrix3x3((orientation_R_c_b)); + tf2::Vector3 velocity_in_center_frame_w_r_t_base_frame_ = + angular_transformation_from_center_2_base * + tf2::Vector3(velocity_in_center_frame_linear_x, velocity_in_center_frame_linear_y, 0.0); + tf2::Vector3 linear_transformation_from_center_2_base = + angular_transformation_from_center_2_base * + tf2::Vector3(-base_frame_offset_[0], -base_frame_offset_[1], 0.0); + + velocity_in_base_frame_linear_x = + velocity_in_center_frame_w_r_t_base_frame_.x() + + linear_transformation_from_center_2_base.y() * velocity_in_center_frame_angular_z; + velocity_in_base_frame_linear_y = + velocity_in_center_frame_w_r_t_base_frame_.y() - + linear_transformation_from_center_2_base.x() * velocity_in_center_frame_angular_z; + velocity_in_base_frame_angular_z = velocity_in_center_frame_angular_z; + + /// Integration. + /// NOTE: the position is expressed in the odometry frame , unlike the twist which is + /// expressed in the body frame. + orientation_z_in_base_frame_ += velocity_in_base_frame_angular_z * dt; + + tf2::Quaternion orientation_R_b_odom; + orientation_R_b_odom.setRPY(0.0, 0.0, orientation_z_in_base_frame_); + + tf2::Matrix3x3 angular_transformation_from_base_2_odom = tf2::Matrix3x3((orientation_R_b_odom)); + tf2::Vector3 velocity_in_base_frame_w_r_t_odom_frame_ = + angular_transformation_from_base_2_odom * + tf2::Vector3(velocity_in_base_frame_linear_x, velocity_in_base_frame_linear_y, 0.0); + + position_x_in_base_frame_ += velocity_in_base_frame_w_r_t_odom_frame_.x() * dt; + position_y_in_base_frame_ += velocity_in_base_frame_w_r_t_odom_frame_.y() * dt; + + return true; +} + +void Odometry::setWheelsParams( + const double sum_of_robot_center_projection_on_X_Y_axis, const double wheels_radius) +{ + sum_of_robot_center_projection_on_X_Y_axis_ = sum_of_robot_center_projection_on_X_Y_axis; + wheels_radius_ = wheels_radius; +} + +} // namespace mecanum_drive_controller diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml new file mode 100644 index 0000000000..580b4058d7 --- /dev/null +++ b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml @@ -0,0 +1,19 @@ +test_mecanum_drive_controller: + ros__parameters: + reference_timeout: 0.9 + + front_left_wheel_command_joint_name: "front_left_wheel_joint" + front_right_wheel_command_joint_name: "front_right_wheel_joint" + rear_right_wheel_command_joint_name: "back_right_wheel_joint" + rear_left_wheel_command_joint_name: "back_left_wheel_joint" + + kinematics: + base_frame_offset: { x: 0.0, y: 0.0, theta: 0.0 } + wheels_radius: 0.5 + sum_of_robot_center_projection_on_X_Y_axis: 1.0 + + base_frame_id: "base_link" + odom_frame_id: "odom" + enable_odom_tf: true + twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] + pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml new file mode 100644 index 0000000000..c4e5bb391a --- /dev/null +++ b/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml @@ -0,0 +1,24 @@ +test_mecanum_drive_controller: + ros__parameters: + reference_timeout: 0.1 + + front_left_wheel_command_joint_name: "front_left_wheel_joint" + front_right_wheel_command_joint_name: "front_right_wheel_joint" + rear_right_wheel_command_joint_name: "back_right_wheel_joint" + rear_left_wheel_command_joint_name: "back_left_wheel_joint" + + front_left_wheel_state_joint_name: "state_front_left_wheel_joint" + front_right_wheel_state_joint_name: "state_front_right_wheel_joint" + rear_right_wheel_state_joint_name: "state_back_right_wheel_joint" + rear_left_wheel_state_joint_name: "state_back_left_wheel_joint" + + kinematics: + base_frame_offset: { x: 0.0, y: 0.0, theta: 0.0 } + wheels_radius: 0.5 + sum_of_robot_center_projection_on_X_Y_axis: 1.0 + + base_frame_id: "base_link" + odom_frame_id: "odom" + enable_odom_tf: true + twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] + pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] diff --git a/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp new file mode 100644 index 0000000000..16fa3e815a --- /dev/null +++ b/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp @@ -0,0 +1,52 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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. + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadMecanumDriveController, load_controller) +{ + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/mecanum_drive_controller_params.yaml"; + + cm.set_parameter({"test_mecanum_drive_controller.params_file", test_file_path}); + cm.set_parameter( + {"test_mecanum_drive_controller.type", "mecanum_drive_controller/MecanumDriveController"}); + + ASSERT_NE(cm.load_controller("test_mecanum_drive_controller"), nullptr); + + rclcpp::shutdown(); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp new file mode 100644 index 0000000000..5327e890e9 --- /dev/null +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -0,0 +1,576 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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. + +#include "test_mecanum_drive_controller.hpp" + +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +using mecanum_drive_controller::NR_CMD_ITFS; +using mecanum_drive_controller::NR_REF_ITFS; +using mecanum_drive_controller::NR_STATE_ITFS; + +class MecanumDriveControllerTest +: public MecanumDriveControllerFixture +{ +}; + +namespace +{ +// Floating-point value comparison threshold +const double EPS = 1e-3; +} // namespace + +TEST_F(MecanumDriveControllerTest, when_controller_is_configured_expect_all_parameters_set) +{ + SetUpController(); + + ASSERT_EQ(controller_->params_.reference_timeout, 0.0); + ASSERT_EQ(controller_->params_.kinematics.wheels_radius, 0.0); + ASSERT_EQ(controller_->params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis, 0.0); + + ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.x, 0.0); + ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.y, 0.0); + ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.theta, 0.0); + + ASSERT_TRUE(controller_->params_.front_left_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_right_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_right_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_left_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_left_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_left_wheel_state_joint_name.empty()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ(controller_->params_.reference_timeout, 0.9); + ASSERT_EQ(controller_->params_.kinematics.wheels_radius, 0.5); + ASSERT_EQ(controller_->params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis, 1.0); + + ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.x, 0.0); + ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.y, 0.0); + ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.theta, 0.0); + + ASSERT_EQ( + controller_->params_.front_left_wheel_command_joint_name, TEST_FRONT_LEFT_CMD_JOINT_NAME); + ASSERT_EQ( + controller_->params_.front_right_wheel_command_joint_name, TEST_FRONT_RIGHT_CMD_JOINT_NAME); + ASSERT_EQ( + controller_->params_.rear_right_wheel_command_joint_name, TEST_REAR_RIGHT_CMD_JOINT_NAME); + ASSERT_EQ(controller_->params_.rear_left_wheel_command_joint_name, TEST_REAR_LEFT_CMD_JOINT_NAME); + ASSERT_THAT(controller_->command_joint_names_, testing::ElementsAreArray(command_joint_names_)); + + // When state joint names are empty they are the same as the command joint names + ASSERT_TRUE(controller_->params_.front_left_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_left_wheel_state_joint_name.empty()); + ASSERT_THAT(controller_->state_joint_names_, testing::ElementsAreArray(command_joint_names_)); +} + +TEST_F(MecanumDriveControllerTest, when_controller_configured_expect_properly_exported_interfaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check command itfs configuration + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + for (size_t i = 0; i < command_intefaces.names.size(); ++i) + { + EXPECT_EQ(command_intefaces.names[i], command_joint_names_[i] + "/" + interface_name_); + } + + // check state itfs configuration + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + for (size_t i = 0; i < state_intefaces.names.size(); ++i) + { + EXPECT_EQ(state_intefaces.names[i], command_joint_names_[i] + "/" + interface_name_); + } + + // check ref itfs configuration, + + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), NR_REF_ITFS); + + for (size_t i = 0; i < reference_interface_names.size(); ++i) + { + const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + + std::string("/") + reference_interface_names[i]; + EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), reference_interface_names[i]); + } +} + +TEST_F(MecanumDriveControllerTest, when_controller_is_activated_expect_reference_reset) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check that the message is reset + auto msg = controller_->input_ref_.readFromNonRT(); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.x)); + + ASSERT_TRUE(std::isnan((*msg)->twist.angular.z)); +} + +TEST_F(MecanumDriveControllerTest, when_controller_active_and_update_called_expect_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(MecanumDriveControllerTest, when_active_controller_is_deactivated_expect_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F( + MecanumDriveControllerTest, + when_controller_is_reactivated_expect_cmd_itfs_not_set_and_update_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_value(), 101.101); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_value())); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_value())); + + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +// when update is called expect the previously set reference before calling update, +// inside the controller state message +TEST_F(MecanumDriveControllerTest, when_update_is_called_expect_status_message) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + controller_->reference_interfaces_[0] = 1.5; + + ControllerStateMsg msg; + subscribe_to_controller_status_execute_update_and_get_messages(msg); + + EXPECT_EQ(msg.reference_velocity.linear.x, 1.5); +} + +// reference_interfaces and command_interfaces values depend on the reference_msg, +// the below test shows two cases when reference_msg is not received and when it is received. +TEST_F( + MecanumDriveControllerTest, + when_reference_msg_received_expect_updated_commands_and_status_message) +{ + SetUpController(); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + // no reference_msg sent + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ControllerStateMsg msg; + subscribe_to_controller_status_execute_update_and_get_messages(msg); + joint_command_values_[controller_->get_rear_left_wheel_index()] = command_lin_x; + + EXPECT_TRUE(std::isnan(msg.reference_velocity.linear.x)); + + // reference_callback() is implicitly called when publish_commands() is called + // reference_msg is published with provided time stamp when publish_commands( time_stamp) + // is called + publish_commands(controller_->get_node()->now()); + controller_->wait_for_commands(executor); + + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // REAR LEFT vel = + // 1.0 / params_.kinematics.wheels_radius * + // (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ - + // params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + // velocity_in_center_frame_angular_z_); + // joint_command_values_[REAR_LEFT] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) + EXPECT_EQ(joint_command_values_[controller_->get_rear_left_wheel_index()], 3.0); + + subscribe_to_controller_status_execute_update_and_get_messages(msg); + + ASSERT_EQ(msg.reference_velocity.linear.x, 1.5); + ASSERT_EQ(msg.back_left_wheel_velocity, 0.1); + ASSERT_EQ(msg.back_right_wheel_velocity, 0.1); +} + +// when too old msg is sent expect reference msg reset +TEST_F(MecanumDriveControllerTest, when_reference_msg_is_too_old_expect_unset_reference) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto reference = *(controller_->input_ref_.readFromNonRT()); + auto old_timestamp = reference->header.stamp; + EXPECT_TRUE(std::isnan(reference->twist.linear.x)); + EXPECT_TRUE(std::isnan(reference->twist.linear.y)); + EXPECT_TRUE(std::isnan(reference->twist.angular.z)); + + // reference_callback() is implicitly called when publish_commands() is called + // reference_msg is published with provided time stamp when publish_commands( time_stamp) + // is called + publish_commands( + controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1)); + controller_->wait_for_commands(executor); + ASSERT_EQ(old_timestamp, (*(controller_->input_ref_.readFromNonRT()))->header.stamp); + EXPECT_TRUE(std::isnan((reference)->twist.linear.x)); + EXPECT_TRUE(std::isnan((reference)->twist.linear.y)); + EXPECT_TRUE(std::isnan((reference)->twist.angular.z)); +} + +// when time stamp is zero expect that time stamp is set to current time stamp +TEST_F( + MecanumDriveControllerTest, + when_reference_msg_has_timestamp_zero_expect_reference_set_and_timestamp_set_to_current_time) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto reference = controller_->input_ref_.readFromNonRT(); + auto old_timestamp = (*reference)->header.stamp; + EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); + EXPECT_TRUE(std::isnan((*reference)->twist.linear.y)); + EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); + + // reference_callback() is implicitly called when publish_commands() is called + // reference_msg is published with provided time stamp when publish_commands( time_stamp) + // is called + publish_commands(rclcpp::Time(0)); + + controller_->wait_for_commands(executor); + ASSERT_EQ(old_timestamp.sec, (*(controller_->input_ref_.readFromNonRT()))->header.stamp.sec); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 1.5); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y, 0.0); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); + EXPECT_NE((*(controller_->input_ref_.readFromNonRT()))->header.stamp.sec, 0.0); +} + +// when the reference_msg has valid timestamp then the timeout check in reference_callback() +// shall pass and reference_msg is not reset +TEST_F(MecanumDriveControllerTest, when_message_has_valid_timestamp_expect_reference_set) +{ + SetUpController(); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto reference = controller_->input_ref_.readFromNonRT(); + EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); + EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); + + // reference_callback() is implicitly called when publish_commands() is called + // reference_msg is published with provided time stamp when publish_commands( time_stamp) + // is called + publish_commands(controller_->get_node()->now()); + + controller_->wait_for_commands(executor); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 1.5); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y, 0.0); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); +} + +// when not in chainable mode and ref_msg_timedout expect +// command_interfaces are set to 0.0 and when ref_msg is not timedout expect +// command_interfaces are set to valid command values +TEST_F( + MecanumDriveControllerTest, + when_ref_msg_old_expect_cmnd_itfs_set_to_zero_otherwise_to_valid_cmnds) +{ + // 1. age>ref_timeout 2. ageget_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(false); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + // set command statically + joint_command_values_[controller_->get_rear_left_wheel_index()] = command_lin_x; + + std::shared_ptr msg = std::make_shared(); + + msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1); + msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg->twist.linear.y = TEST_LINEAR_VELOCITY_y; + msg->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.writeFromNonRT(msg); + const auto age_of_last_command = + controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + + // age_of_last_command > ref_timeout_ + ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + + EXPECT_EQ(joint_command_values_[controller_->get_rear_left_wheel_index()], 0); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) + { + EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0.0); + } + + std::shared_ptr msg_2 = std::make_shared(); + msg_2->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.01); + msg_2->twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg_2->twist.linear.y = TEST_LINEAR_VELOCITY_y; + msg_2->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg_2->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg_2->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg_2->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.writeFromNonRT(msg_2); + const auto age_of_last_command_2 = + controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + + // age_of_last_command_2 < ref_timeout_ + ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NE(joint_command_values_[controller_->get_rear_left_wheel_index()], command_lin_x); + // BACK Left vel = + // 1.0 / params_.kinematics.wheels_radius * + // (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ - + // params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + // velocity_in_center_frame_angular_z_); + // joint_command_values_[controller_->get_rear_left_wheel_index()] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * + // 0.0) + EXPECT_EQ(joint_command_values_[controller_->get_rear_left_wheel_index()], 3.0); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) + { + EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 3.0); + } +} + +// when in chained mode the reference_interfaces of chained controller and command_interfaces +// of preceding controller point to same memory location, hence reference_interfaces are not +// exclusively set by the update method of chained controller +TEST_F( + MecanumDriveControllerTest, + when_controller_in_chainable_mode_expect_receiving_commands_from_reference_interfaces_directly) +{ + SetUpController(); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + // set command statically + joint_command_values_[controller_->get_rear_left_wheel_index()] = command_lin_x; + // imitating preceding controllers command_interfaces setting reference_interfaces of chained + // controller. + controller_->reference_interfaces_[0] = 3.0; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + + // reference_callback() is implicitly called when publish_commands() is called + // reference_msg is published with provided time stamp when publish_commands( time_stamp) + // is called + publish_commands(controller_->get_node()->now()); + + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NE(joint_command_values_[controller_->get_rear_left_wheel_index()], command_lin_x); + + // REAR LEFT vel = + // 1.0 / params_.kinematics.wheels_radius * + // (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ - + // params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + // velocity_in_center_frame_angular_z_); + + // joint_command_values_[REAR_LEFT] = 1.0 / 0.5 * (3.0 - 0.0 - 1 * 0.0) + EXPECT_EQ(joint_command_values_[controller_->get_rear_left_wheel_index()], 6.0); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) + { + EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 6.0); + } +} + +// when ref_timeout = 0 expect reference_msg is accepted only once and command_interfaces +// are calculated to valid values and reference_interfaces are unset +TEST_F( + MecanumDriveControllerTest, + when_reference_timeout_is_zero_expect_reference_msg_being_used_only_once) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // set command statically + joint_command_values_[controller_->get_rear_left_wheel_index()] = command_lin_x; + + controller_->ref_timeout_ = rclcpp::Duration::from_seconds(0.0); + std::shared_ptr msg = std::make_shared(); + + msg->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.0); + msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg->twist.linear.y = TEST_LINEAR_VELOCITY_y; + msg->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.writeFromNonRT(msg); + const auto age_of_last_command = + controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + + ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_FALSE(std::isnan(joint_command_values_[1])); + EXPECT_NE(joint_command_values_[controller_->get_rear_left_wheel_index()], command_lin_x); + // REAR LEFT vel = + // 1.0 / params_.kinematics.wheels_radius * + // (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ - + // params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + // velocity_in_center_frame_angular_z_); + // joint_command_values_[REAR_LEFT] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) + EXPECT_EQ(joint_command_values_[controller_->get_rear_left_wheel_index()], 3.0); + ASSERT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); +} + +TEST_F( + MecanumDriveControllerTest, + when_ref_timeout_zero_for_reference_callback_expect_reference_msg_being_used_only_once) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y)); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + controller_->ref_timeout_ = rclcpp::Duration::from_seconds(0.0); + + // reference_callback() is called implicitly when publish_commands() is called. + // reference_msg is published with provided time stamp when publish_commands( time_stamp) + // is called + publish_commands(controller_->get_node()->now()); + + controller_->wait_for_commands(executor); + + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y)); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 1.5); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y, 0.0); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp new file mode 100644 index 0000000000..5f7cf36dc7 --- /dev/null +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -0,0 +1,317 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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_MECANUM_DRIVE_CONTROLLER_HPP_ +#define TEST_MECANUM_DRIVE_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "gmock/gmock.h" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "mecanum_drive_controller/mecanum_drive_controller.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +using ControllerStateMsg = mecanum_drive_controller::MecanumDriveController::ControllerStateMsg; +using ControllerReferenceMsg = + mecanum_drive_controller::MecanumDriveController::ControllerReferenceMsg; +using TfStateMsg = mecanum_drive_controller::MecanumDriveController::TfStateMsg; +using OdomStateMsg = mecanum_drive_controller::MecanumDriveController::OdomStateMsg; + +namespace +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +} // namespace +// namespace + +// subclassing and friending so we can access member variables +class TestableMecanumDriveController : public mecanum_drive_controller::MecanumDriveController +{ + FRIEND_TEST(MecanumDriveControllerTest, when_controller_is_configured_expect_all_parameters_set); + FRIEND_TEST( + MecanumDriveControllerTest, when_controller_configured_expect_properly_exported_interfaces); + FRIEND_TEST(MecanumDriveControllerTest, when_controller_is_activated_expect_reference_reset); + FRIEND_TEST(MecanumDriveControllerTest, when_controller_active_and_update_called_expect_success); + FRIEND_TEST(MecanumDriveControllerTest, when_active_controller_is_deactivated_expect_success); + FRIEND_TEST( + MecanumDriveControllerTest, + when_controller_is_reactivated_expect_cmd_itfs_not_set_and_update_success); + FRIEND_TEST(MecanumDriveControllerTest, when_update_is_called_expect_status_message); + FRIEND_TEST( + MecanumDriveControllerTest, + when_reference_msg_received_expect_updated_commands_and_status_message); + FRIEND_TEST(MecanumDriveControllerTest, when_reference_msg_is_too_old_expect_unset_reference); + FRIEND_TEST( + MecanumDriveControllerTest, + when_reference_msg_has_timestamp_zero_expect_reference_set_and_timestamp_set_to_current_time); + FRIEND_TEST(MecanumDriveControllerTest, when_message_has_valid_timestamp_expect_reference_set); + FRIEND_TEST( + MecanumDriveControllerTest, + when_ref_msg_old_expect_cmnd_itfs_set_to_zero_otherwise_to_valid_cmnds); + FRIEND_TEST( + MecanumDriveControllerTest, + when_controller_in_chainable_mode_expect_receiving_commands_from_reference_interfaces_directly); + FRIEND_TEST( + MecanumDriveControllerTest, + when_reference_timeout_is_zero_expect_reference_msg_being_used_only_once); + FRIEND_TEST( + MecanumDriveControllerTest, + when_ref_timeout_zero_for_reference_callback_expect_reference_msg_being_used_only_once); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + return mecanum_drive_controller::MecanumDriveController::on_configure(previous_state); + } + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override + { + auto ref_itfs = on_export_reference_interfaces(); + return mecanum_drive_controller::MecanumDriveController::on_activate(previous_state); + } + + /** + * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + */ + void wait_for_command( + 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)); + } + } + + void wait_for_commands( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + wait_for_command(executor, timeout); + } + + size_t get_front_left_wheel_index() { return WheelIndex::FRONT_LEFT; } + + size_t get_front_right_wheel_index() { return WheelIndex::FRONT_RIGHT; } + + size_t get_rear_right_wheel_index() { return WheelIndex::REAR_RIGHT; } + + size_t get_rear_left_wheel_index() { return WheelIndex::REAR_LEFT; } +}; + +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class MecanumDriveControllerFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() {} + + void SetUp() + { + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + command_publisher_ = command_publisher_node_->create_publisher( + "/test_mecanum_drive_controller/reference", rclcpp::SystemDefaultsQoS()); + + odom_s_publisher_node_ = std::make_shared("odom_s_publisher"); + odom_s_publisher_ = odom_s_publisher_node_->create_publisher( + "/test_mecanum_drive_controller/odometry", rclcpp::SystemDefaultsQoS()); + + tf_odom_s_publisher_node_ = std::make_shared("tf_odom_s_publisher"); + tf_odom_s_publisher_ = tf_odom_s_publisher_node_->create_publisher( + "/test_mecanum_drive_controller/tf_odometry", rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() {} + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController(const std::string controller_name = "test_mecanum_drive_controller") + { + const auto urdf = ""; + const auto ns = ""; + ASSERT_EQ( + controller_->init(controller_name, urdf, 0, ns, controller_->define_custom_node_options()), + controller_interface::return_type::OK); + + std::vector command_ifs; + command_itfs_.reserve(joint_command_values_.size()); + command_ifs.reserve(joint_command_values_.size()); + + for (size_t i = 0; i < joint_command_values_.size(); ++i) + { + command_itfs_.emplace_back(hardware_interface::CommandInterface( + command_joint_names_[i], interface_name_, &joint_command_values_[i])); + command_ifs.emplace_back(command_itfs_.back()); + } + + std::vector state_ifs; + state_itfs_.reserve(joint_state_values_.size()); + state_ifs.reserve(joint_state_values_.size()); + + for (size_t i = 0; i < joint_state_values_.size(); ++i) + { + state_itfs_.emplace_back(hardware_interface::StateInterface( + command_joint_names_[i], interface_name_, &joint_state_values_[i])); + state_ifs.emplace_back(state_itfs_.back()); + } + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void subscribe_to_controller_status_execute_update_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + ControllerStateMsg::SharedPtr received_msg; + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; + auto subscription = test_subscription_node.create_subscription( + "/test_mecanum_drive_controller/controller_state", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); + // call update to publish the test value + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + while (max_sub_check_loop_count--) + { + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{5}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + // check if message has been received + if (received_msg.get()) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); + + // take message from subscription + msg = *received_msg; + } + + void publish_commands( + const rclcpp::Time & stamp, const double & twist_linear_x = 1.5, + const double & twist_linear_y = 0.0, const double & twist_angular_z = 0.0) + { + auto wait_for_topic = [&](const auto topic_name) + { + size_t wait_count = 0; + while (command_publisher_node_->count_subscribers(topic_name) == 0) + { + if (wait_count >= 5) + { + auto error_msg = + std::string("publishing to ") + topic_name + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + ++wait_count; + } + }; + + wait_for_topic(command_publisher_->get_topic_name()); + + ControllerReferenceMsg msg; + msg.header.stamp = stamp; + msg.twist.linear.x = twist_linear_x; + msg.twist.linear.y = twist_linear_y; + msg.twist.linear.z = std::numeric_limits::quiet_NaN(); + msg.twist.angular.x = std::numeric_limits::quiet_NaN(); + msg.twist.angular.y = std::numeric_limits::quiet_NaN(); + msg.twist.angular.z = twist_angular_z; + + command_publisher_->publish(msg); + } + +protected: + std::vector reference_interface_names = { + "linear/x/velocity", "linear/y/velocity", "angular/z/velocity"}; + + static constexpr char TEST_FRONT_LEFT_CMD_JOINT_NAME[] = "front_left_wheel_joint"; + static constexpr char TEST_FRONT_RIGHT_CMD_JOINT_NAME[] = "front_right_wheel_joint"; + static constexpr char TEST_REAR_RIGHT_CMD_JOINT_NAME[] = "back_right_wheel_joint"; + static constexpr char TEST_REAR_LEFT_CMD_JOINT_NAME[] = "back_left_wheel_joint"; + std::vector command_joint_names_ = { + TEST_FRONT_LEFT_CMD_JOINT_NAME, TEST_FRONT_RIGHT_CMD_JOINT_NAME, TEST_REAR_RIGHT_CMD_JOINT_NAME, + TEST_REAR_LEFT_CMD_JOINT_NAME}; + + static constexpr char TEST_FRONT_LEFT_STATE_JOINT_NAME[] = "state_front_left_wheel_joint"; + static constexpr char TEST_FRONT_RIGHT_STATE_JOINT_NAME[] = "state_front_right_wheel_joint"; + static constexpr char TEST_REAR_RIGHT_STATE_JOINT_NAME[] = "state_back_right_wheel_joint"; + static constexpr char TEST_REAR_LEFT_STATE_JOINT_NAME[] = "state_back_left_wheel_joint"; + std::vector state_joint_names_ = { + TEST_FRONT_LEFT_STATE_JOINT_NAME, TEST_FRONT_RIGHT_STATE_JOINT_NAME, + TEST_REAR_RIGHT_STATE_JOINT_NAME, TEST_REAR_LEFT_STATE_JOINT_NAME}; + std::string interface_name_ = hardware_interface::HW_IF_VELOCITY; + + // Controller-related parameters + + std::array joint_state_values_ = {{0.1, 0.1, 0.1, 0.1}}; + std::array joint_command_values_ = {{101.101, 101.101, 101.101, 101.101}}; + + static constexpr double TEST_LINEAR_VELOCITY_X = 1.5; + static constexpr double TEST_LINEAR_VELOCITY_y = 0.0; + static constexpr double TEST_ANGULAR_VELOCITY_Z = 0.0; + double command_lin_x = 111; + + std::vector state_itfs_; + std::vector command_itfs_; + + double ref_timeout_ = 0.1; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr command_publisher_; + rclcpp::Node::SharedPtr odom_s_publisher_node_; + rclcpp::Publisher::SharedPtr odom_s_publisher_; + rclcpp::Node::SharedPtr tf_odom_s_publisher_node_; + rclcpp::Publisher::SharedPtr tf_odom_s_publisher_; +}; + +#endif // TEST_MECANUM_DRIVE_CONTROLLER_HPP_ diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp new file mode 100644 index 0000000000..edbee8a702 --- /dev/null +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp @@ -0,0 +1,99 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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. + +#include "test_mecanum_drive_controller.hpp" + +#include +#include +#include +#include +#include + +using mecanum_drive_controller::NR_CMD_ITFS; +using mecanum_drive_controller::NR_REF_ITFS; +using mecanum_drive_controller::NR_STATE_ITFS; + +class MecanumDriveControllerTest +: public MecanumDriveControllerFixture +{ +}; + +TEST_F(MecanumDriveControllerTest, when_controller_is_configured_expect_all_parameters_set) +{ + SetUpController(); + + ASSERT_EQ(controller_->params_.reference_timeout, 0.0); + + ASSERT_TRUE(controller_->params_.front_left_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_right_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_right_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_left_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_left_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_left_wheel_state_joint_name.empty()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ(controller_->params_.reference_timeout, 0.1); + + ASSERT_EQ( + controller_->params_.front_left_wheel_command_joint_name, TEST_FRONT_LEFT_CMD_JOINT_NAME); + ASSERT_EQ( + controller_->params_.front_right_wheel_command_joint_name, TEST_FRONT_RIGHT_CMD_JOINT_NAME); + ASSERT_EQ( + controller_->params_.rear_right_wheel_command_joint_name, TEST_REAR_RIGHT_CMD_JOINT_NAME); + ASSERT_EQ(controller_->params_.rear_left_wheel_command_joint_name, TEST_REAR_LEFT_CMD_JOINT_NAME); + ASSERT_THAT(controller_->command_joint_names_, testing::ElementsAreArray(command_joint_names_)); + + ASSERT_EQ( + controller_->params_.front_left_wheel_state_joint_name, TEST_FRONT_LEFT_STATE_JOINT_NAME); + ASSERT_EQ( + controller_->params_.front_right_wheel_state_joint_name, TEST_FRONT_RIGHT_STATE_JOINT_NAME); + ASSERT_EQ( + controller_->params_.rear_right_wheel_state_joint_name, TEST_REAR_RIGHT_STATE_JOINT_NAME); + ASSERT_EQ(controller_->params_.rear_left_wheel_state_joint_name, TEST_REAR_LEFT_STATE_JOINT_NAME); + ASSERT_THAT(controller_->state_joint_names_, testing::ElementsAreArray(state_joint_names_)); +} + +// checking if all interfaces, command and state interfaces are exported as expected +TEST_F(MecanumDriveControllerTest, when_controller_configured_expect_properly_exported_interfaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + for (size_t i = 0; i < command_intefaces.names.size(); ++i) + { + EXPECT_EQ(command_intefaces.names[i], command_joint_names_[i] + "/" + interface_name_); + } + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + for (size_t i = 0; i < state_intefaces.names.size(); ++i) + { + EXPECT_EQ(state_intefaces.names[i], state_joint_names_[i] + "/" + interface_name_); + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 4969658788..99ca06c77b 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -28,6 +28,7 @@ imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller + mecanum_drive_controller pid_controller position_controllers range_sensor_broadcaster diff --git a/steering_controllers_library/doc/userdoc.rst b/steering_controllers_library/doc/userdoc.rst index 44b180162e..8889824d62 100644 --- a/steering_controllers_library/doc/userdoc.rst +++ b/steering_controllers_library/doc/userdoc.rst @@ -10,7 +10,7 @@ steering_controllers_library .. _twist_msg: https://github.com/ros2/common_interfaces/blob/{DISTRO}/geometry_msgs/msg/TwistStamped.msg .. _tf_msg: https://github.com/ros2/geometry2/blob/{DISTRO}/tf2_msgs/msg/TFMessage.msg -Library with shared functionalities for mobile robot controllers with steering drives, with so-called non-holonomic constraints. +Library with shared functionalities for mobile robot controllers with steering drives (2 degrees of freedom), with so-called non-holonomic constraints. The library implements generic odometry and update methods and defines the main interfaces. From a5bb11bf1402c9d2e50e21e4ce7380f2b4f14218 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 22 Nov 2024 11:15:31 +0100 Subject: [PATCH 16/84] Add missing deps for test_nodes (#1378) --- ros2_controllers_test_nodes/package.xml | 2 ++ .../test_publisher_forward_position_controller_launch.py | 6 ++---- .../test_publisher_joint_trajectory_controller_launch.py | 5 ++--- 3 files changed, 6 insertions(+), 7 deletions(-) diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index c53f1b9971..0b97d6bb0c 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -21,6 +21,8 @@ trajectory_msgs python3-pytest + launch_testing_ros + launch_ros ament_python diff --git a/ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py b/ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py index 8ebb2df7b3..3f4c9da21f 100644 --- a/ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py +++ b/ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py @@ -34,15 +34,13 @@ from launch import LaunchDescription from launch.substitutions import PathJoinSubstitution +import launch_ros.actions from launch_ros.substitutions import FindPackageShare from launch_testing.actions import ReadyToTest -from launch_testing_ros import WaitForTopics - import launch_testing.markers +from launch_testing_ros import WaitForTopics import rclpy -import launch_ros.actions from rclpy.node import Node - from std_msgs.msg import Float64MultiArray diff --git a/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py b/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py index d8d6e2a577..62ad25550d 100644 --- a/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py +++ b/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py @@ -34,13 +34,12 @@ from launch import LaunchDescription from launch.substitutions import PathJoinSubstitution +import launch_ros.actions from launch_ros.substitutions import FindPackageShare from launch_testing.actions import ReadyToTest -from launch_testing_ros import WaitForTopics - import launch_testing.markers +from launch_testing_ros import WaitForTopics import rclpy -import launch_ros.actions from rclpy.node import Node from trajectory_msgs.msg import JointTrajectory From 1bd6870a1115a0551506bbd45afd88c6ce16c2a8 Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Fri, 22 Nov 2024 11:22:20 +0100 Subject: [PATCH 17/84] [JTC] Sample at t + dT instead of the beginning of the control cycle (#1306) --- .../joint_trajectory_controller.hpp | 2 + .../trajectory.hpp | 11 +- .../src/joint_trajectory_controller.cpp | 29 ++- .../src/trajectory.cpp | 8 +- .../test/test_trajectory_controller.cpp | 166 ++++++++++++------ .../test/test_trajectory_controller_utils.hpp | 4 +- 6 files changed, 147 insertions(+), 73 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 2f0a1f4df0..67954b8378 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -112,6 +112,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // Preallocate variables used in the realtime update() function trajectory_msgs::msg::JointTrajectoryPoint state_current_; trajectory_msgs::msg::JointTrajectoryPoint command_current_; + trajectory_msgs::msg::JointTrajectoryPoint command_next_; trajectory_msgs::msg::JointTrajectoryPoint state_desired_; trajectory_msgs::msg::JointTrajectoryPoint state_error_; @@ -124,6 +125,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // Parameters from ROS for joint_trajectory_controller std::shared_ptr param_listener_; Params params_; + rclcpp::Duration update_period_{0, 0}; trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; /// Specify interpolation method. Default to splines. diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index 14373b006e..d650c369d0 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -69,8 +69,10 @@ class Trajectory * acceleration respectively. Deduction assumes that the provided velocity or acceleration have to * be reached at the time defined in the segment. * - * This function assumes that sampling is only done at monotonically increasing \p sample_time - * for any trajectory. + * This function by default assumes that sampling is only done at monotonically increasing \p + * sample_time for any trajectory. That means, it will only search for a point matching the sample + * time after the point it has been called before. If this isn't desired, set \p + * search_monotonically_increasing to false. * * Specific case returns for start_segment_itr and end_segment_itr: * - Sampling before the trajectory start: @@ -94,13 +96,16 @@ class Trajectory * description above. * \param[out] end_segment_itr Iterator to the end segment for given \p sample_time. See * description above. + * \param[in] search_monotonically_increasing If set to true, the next sample call will start + * searching in the trajectory at the index of this call's result. */ JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool sample( const rclcpp::Time & sample_time, const interpolation_methods::InterpolationMethod interpolation_method, trajectory_msgs::msg::JointTrajectoryPoint & output_state, - TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr); + TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr, + const bool search_monotonically_increasing = true); /** * Do interpolation between 2 states given a time in between their respective timestamps diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index bea37e02be..0fc39ecaaf 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #include @@ -180,6 +181,7 @@ controller_interface::return_type JointTrajectoryController::update( if (has_active_trajectory()) { bool first_sample = false; + TrajectoryPointConstIter start_segment_itr, end_segment_itr; // if sampling the first time, set the point before you sample if (!traj_external_point_ptr_->is_sampled_already()) { @@ -196,11 +198,15 @@ controller_interface::return_type JointTrajectoryController::update( } } - // find segment for current timestamp - TrajectoryPointConstIter start_segment_itr, end_segment_itr; - const bool valid_point = traj_external_point_ptr_->sample( + // Sample expected state from the trajectory + traj_external_point_ptr_->sample( time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr); + // Sample setpoint for next control cycle + const bool valid_point = traj_external_point_ptr_->sample( + time + update_period_, interpolation_method_, command_next_, start_segment_itr, + end_segment_itr, false); + if (valid_point) { const rclcpp::Time traj_start = traj_external_point_ptr_->time_from_start(); @@ -276,7 +282,7 @@ controller_interface::return_type JointTrajectoryController::update( // Update PIDs for (auto i = 0ul; i < dof_; ++i) { - tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) + + tmp_command_[i] = (command_next_.velocities[i] * ff_velocity_scale_[i]) + pids_[i]->computeCommand( state_error_.positions[i], state_error_.velocities[i], (uint64_t)period.nanoseconds()); @@ -286,7 +292,7 @@ controller_interface::return_type JointTrajectoryController::update( // set values for next hardware write() if (has_position_command_interface_) { - assign_interface_from_point(joint_command_interface_[0], state_desired_.positions); + assign_interface_from_point(joint_command_interface_[0], command_next_.positions); } if (has_velocity_command_interface_) { @@ -296,12 +302,12 @@ controller_interface::return_type JointTrajectoryController::update( } else { - assign_interface_from_point(joint_command_interface_[1], state_desired_.velocities); + assign_interface_from_point(joint_command_interface_[1], command_next_.velocities); } } if (has_acceleration_command_interface_) { - assign_interface_from_point(joint_command_interface_[2], state_desired_.accelerations); + assign_interface_from_point(joint_command_interface_[2], command_next_.accelerations); } if (has_effort_command_interface_) { @@ -309,7 +315,7 @@ controller_interface::return_type JointTrajectoryController::update( } // store the previous command. Used in open-loop control mode - last_commanded_state_ = state_desired_; + last_commanded_state_ = command_next_; } if (active_goal) @@ -867,6 +873,13 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( std::string(get_node()->get_name()) + "/query_state", std::bind(&JointTrajectoryController::query_state_service, this, _1, _2)); + if (get_update_rate() == 0) + { + throw std::runtime_error("Controller's update rate is set to 0. This should not happen!"); + } + update_period_ = + rclcpp::Duration(0.0, static_cast(1.0e9 / static_cast(get_update_rate()))); + return CallbackReturn::SUCCESS; } diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index 512b16f3c5..1c50d26c62 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -91,7 +91,8 @@ bool Trajectory::sample( const rclcpp::Time & sample_time, const interpolation_methods::InterpolationMethod interpolation_method, trajectory_msgs::msg::JointTrajectoryPoint & output_state, - TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr) + TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr, + const bool search_monotonically_increasing) { THROW_ON_NULLPTR(trajectory_msg_) @@ -176,7 +177,10 @@ bool Trajectory::sample( } start_segment_itr = begin() + i; end_segment_itr = begin() + (i + 1); - last_sample_idx_ = i; + if (search_monotonically_increasing) + { + last_sample_idx_ = i; + } return true; } } diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index f188c0f04b..21d0277541 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -53,7 +53,7 @@ TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) traj_controller_->get_node()->set_parameter( rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)); - const auto state = traj_controller_->get_node()->configure(); + const auto state = traj_controller_->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); // send msg @@ -84,7 +84,7 @@ TEST_P(TrajectoryControllerTestParameterized, check_interface_names) rclcpp::executors::MultiThreadedExecutor executor; SetUpTrajectoryController(executor); - const auto state = traj_controller_->get_node()->configure(); + const auto state = traj_controller_->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); compare_joints(joint_names_, joint_names_); @@ -97,7 +97,7 @@ TEST_P(TrajectoryControllerTestParameterized, check_interface_names_with_command const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names_); SetUpTrajectoryController(executor, {command_joint_names_param}); - const auto state = traj_controller_->get_node()->configure(); + const auto state = traj_controller_->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); compare_joints(joint_names_, command_joint_names_); @@ -108,7 +108,7 @@ TEST_P(TrajectoryControllerTestParameterized, activate) rclcpp::executors::MultiThreadedExecutor executor; SetUpTrajectoryController(executor); - auto state = traj_controller_->get_node()->configure(); + auto state = traj_controller_->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); auto cmd_if_conf = traj_controller_->command_interface_configuration(); @@ -162,7 +162,7 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup_after_configure) SetUpTrajectoryController(executor); // configure controller - auto state = traj_controller_->get_node()->configure(); + auto state = traj_controller_->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); // cleanup controller @@ -178,6 +178,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param SetUpTrajectoryController(executor); traj_controller_->get_node()->set_parameter( rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)); + traj_controller_->get_node()->set_parameter(rclcpp::Parameter("update_rate", 10)); // This call is replacing the way parameters are set via launch auto state = traj_controller_->configure(); @@ -206,8 +207,10 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param // wait for reaching the first point // controller would process the second point when deactivated below + // Since the trajectory will be sampled at time + update_period, we need to pass the time 0.15 + // seconds to sample at t=0.25 sec traj_controller_->update( - rclcpp::Time(static_cast(0.25 * 1e9)), rclcpp::Duration::from_seconds(0.15)); + rclcpp::Time(static_cast(0.15 * 1e9)), rclcpp::Duration::from_seconds(0.15)); EXPECT_TRUE(traj_controller_->has_active_traj()); if (traj_controller_->has_position_command_interface()) { @@ -644,7 +647,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun size_t n_joints = joint_names_.size(); // send msg - constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(300); builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; // *INDENT-OFF* std::vector> points{ @@ -653,7 +656,23 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun publish(time_from_start, points, rclcpp::Time()); traj_controller_->wait_for_trajectory(executor); - updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); + const rclcpp::Duration controller_period = + rclcpp::Duration::from_seconds(1.0 / traj_controller_->get_update_rate()); + + auto end_time = updateControllerAsync( + rclcpp::Duration(FIRST_POINT_TIME) - controller_period, rclcpp::Time(0, 0, RCL_STEADY_TIME), + controller_period); + if (traj_controller_->has_position_command_interface()) + { + // check command interface + // One step before the first point, the target should hit the setpoint + EXPECT_NEAR(points[0][0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], joint_pos_[2], COMMON_THRESHOLD); + } + + // Propagate to actual setpoint time + traj_controller_->update(end_time + controller_period, controller_period); // get states from class variables auto state_feedback = traj_controller_->get_state_feedback(); @@ -678,14 +697,6 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun EXPECT_NEAR(state_error.positions[1], state_reference.positions[1] - INITIAL_POS_JOINTS[1], EPS); EXPECT_NEAR(state_error.positions[2], state_reference.positions[2] - INITIAL_POS_JOINTS[2], EPS); - if (traj_controller_->has_position_command_interface()) - { - // check command interface - EXPECT_NEAR(points[0][0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(points[0][2], joint_pos_[2], COMMON_THRESHOLD); - } - if (traj_controller_->has_velocity_command_interface()) { // use_closed_loop_pid_adapter_ @@ -745,7 +756,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) size_t n_joints = joint_names_.size(); // send msg - constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(300); builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; // *INDENT-OFF* std::vector> points{ @@ -756,7 +767,23 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); traj_controller_->wait_for_trajectory(executor); - updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); + const rclcpp::Duration controller_period = + rclcpp::Duration::from_seconds(1.0 / traj_controller_->get_update_rate()); + auto end_time = updateControllerAsync( + rclcpp::Duration(FIRST_POINT_TIME) - controller_period, rclcpp::Time(0, 0, RCL_STEADY_TIME), + controller_period); + + if (traj_controller_->has_position_command_interface()) + { + // check command interface + // One step before the first point, the target should hit the setpoint + EXPECT_NEAR(points[0][0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], joint_pos_[2], COMMON_THRESHOLD); + } + + // Propagate to actual setpoint time + traj_controller_->update(end_time + controller_period, controller_period); // get states from class variables auto state_feedback = traj_controller_->get_state_feedback(); @@ -782,14 +809,6 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) EXPECT_NEAR( state_error.positions[2], state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI, EPS); - if (traj_controller_->has_position_command_interface()) - { - // check command interface - EXPECT_NEAR(points[0][0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(points[0][2], joint_pos_[2], COMMON_THRESHOLD); - } - if (traj_controller_->has_velocity_command_interface()) { // use_closed_loop_pid_adapter_ @@ -1548,16 +1567,27 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; expected_desired = expected_actual; + traj_controller_->wait_for_trajectory(executor); + auto end_time = updateControllerAsync( + rclcpp::Duration(delay), rclcpp::Time(0, 0, RCL_STEADY_TIME), + rclcpp::Duration::from_seconds(1.0 / traj_controller_->get_update_rate())); + // Check that we reached end of points_old[0] trajectory - auto end_time = - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + if (traj_controller_->has_position_command_interface()) + { + auto state_feedback = traj_controller_->get_state_feedback(); + for (size_t i = 0; i < expected_actual.positions.size(); ++i) + { + EXPECT_NEAR(expected_actual.positions[i], state_feedback.positions[i], 0.01); + } + } RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the past"); // New trajectory will end before current time rclcpp::Time new_traj_start = rclcpp::Clock(RCL_STEADY_TIME).now() - delay - std::chrono::milliseconds(100); expected_actual.positions = {points_old[1].begin(), points_old[1].end()}; - expected_desired = expected_actual; + expected_desired = expected_actual; // robot should be standing still by now publish(time_from_start, points_new, new_traj_start); waitAndCompareState( expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1, end_time); @@ -1577,10 +1607,20 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory // send points_old and wait to reach first point publish(time_from_start, points_old, rclcpp::Time()); expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; - expected_desired = expected_actual; - // Check that we reached end of points_old[0]trajectory - auto end_time = - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + traj_controller_->wait_for_trajectory(executor); + auto end_time = updateControllerAsync( + rclcpp::Duration(delay), rclcpp::Time(0, 0, RCL_STEADY_TIME), + rclcpp::Duration::from_seconds(1.0 / traj_controller_->get_update_rate())); + + if (traj_controller_->has_position_command_interface()) + { + // Check that we reached end of points_old[0] trajectory + auto state_feedback = traj_controller_->get_state_feedback(); + for (size_t i = 0; i < expected_actual.positions.size(); ++i) + { + EXPECT_NEAR(expected_actual.positions[i], state_feedback.positions[i], 0.01); + } + } // send points_new before the old trajectory is finished RCLCPP_INFO( @@ -1653,6 +1693,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro // only makes sense with position command interface return; } + auto controller_period = + rclcpp::Duration::from_seconds(1.0 / traj_controller_->get_update_rate()); // goal setup std::vector first_goal = {3.3, 4.4, 5.5}; @@ -1665,8 +1707,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro builtin_interfaces::msg::Duration time_from_start; time_from_start.sec = 1; time_from_start.nanosec = 0; - double trajectory_frac = rclcpp::Duration::from_seconds(0.01).seconds() / - (time_from_start.sec + time_from_start.nanosec * 1e-9); + double trajectory_frac = + controller_period.seconds() / (time_from_start.sec + time_from_start.nanosec * 1e-9); std::vector> points{{first_goal}}; publish( time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME), {}, first_goal_velocities); @@ -1690,7 +1732,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro // One the first update(s) there should be a "jump" in opposite direction from command // (towards the state value) EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - auto end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + auto end_time = updateControllerAsync(controller_period); // Expect backward commands at first, consider advancement of the trajectory // exact value is not directly predictable, because of the spline interpolation -> increase // tolerance @@ -1699,10 +1741,10 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro 0.1); EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); EXPECT_LT(joint_pos_[0], first_goal[0]); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); EXPECT_LT(joint_pos_[0], first_goal[0]); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); EXPECT_LT(joint_pos_[0], first_goal[0]); @@ -1721,17 +1763,19 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro // One the first update(s) there should be a "jump" in the goal direction from command // (towards the state value) EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + end_time = updateControllerAsync(controller_period); // Expect backward commands at first, consider advancement of the trajectory + // exact value is not directly predictable, because of the spline interpolation -> increase + // tolerance EXPECT_NEAR( joint_state_pos_[0] + (first_goal[0] - joint_state_pos_[0]) * trajectory_frac, joint_pos_[0], - COMMON_THRESHOLD); + 0.1); EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); EXPECT_GT(joint_pos_[0], first_goal[0]); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); EXPECT_GT(joint_pos_[0], first_goal[0]); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); EXPECT_GT(joint_pos_[0], first_goal[0]); @@ -1747,13 +1791,17 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e rclcpp::executors::SingleThreadedExecutor executor; // set open loop to true, this should change behavior from above rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); - SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); + rclcpp::Parameter update_rate_param("update_rate", 100); + SetUpAndActivateTrajectoryController( + executor, {is_open_loop_parameters, update_rate_param}, true); if (traj_controller_->has_position_command_interface() == false) { // only makes sense with position command interface return; } + auto controller_period = + rclcpp::Duration::from_seconds(1.0 / traj_controller_->get_update_rate()); // goal setup std::vector first_goal = {3.3, 4.4, 5.5}; @@ -1764,8 +1812,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e builtin_interfaces::msg::Duration time_from_start; time_from_start.sec = 1; time_from_start.nanosec = 0; - double trajectory_frac = rclcpp::Duration::from_seconds(0.01).seconds() / - (time_from_start.sec + time_from_start.nanosec * 1e-9); + double trajectory_frac = + controller_period.seconds() / (time_from_start.sec + time_from_start.nanosec * 1e-9); std::vector> points{{first_goal}}; publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); traj_controller_->wait_for_trajectory(executor); @@ -1788,17 +1836,18 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e // One the first update(s) there **should not** be a "jump" in opposite direction from // command (towards the state value) EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - auto end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + auto end_time = updateControllerAsync(controller_period); // There should not be backward commands + // exact value is not directly predictable, because of the spline interpolation -> increase + // tolerance EXPECT_NEAR( - first_goal[0] + (second_goal[0] - first_goal[0]) * trajectory_frac, joint_pos_[0], - COMMON_THRESHOLD); + first_goal[0] + (second_goal[0] - first_goal[0]) * trajectory_frac, joint_pos_[0], 0.1); EXPECT_GT(joint_pos_[0], first_goal[0]); EXPECT_LT(joint_pos_[0], second_goal[0]); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); EXPECT_GT(joint_pos_[0], first_goal[0]); EXPECT_LT(joint_pos_[0], second_goal[0]); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); EXPECT_GT(joint_pos_[0], first_goal[0]); EXPECT_LT(joint_pos_[0], second_goal[0]); @@ -1817,17 +1866,18 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e // One the first update(s) there **should not** be a "jump" in the goal direction from // command (towards the state value) EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); // There should not be a jump toward commands + // exact value is not directly predictable, because of the spline interpolation -> increase + // tolerance EXPECT_NEAR( - second_goal[0] + (first_goal[0] - second_goal[0]) * trajectory_frac, joint_pos_[0], - COMMON_THRESHOLD); + second_goal[0] + (first_goal[0] - second_goal[0]) * trajectory_frac, joint_pos_[0], 0.1); EXPECT_LT(joint_pos_[0], second_goal[0]); EXPECT_GT(joint_pos_[0], first_goal[0]); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); EXPECT_GT(joint_pos_[0], first_goal[0]); EXPECT_LT(joint_pos_[0], second_goal[0]); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); EXPECT_GT(joint_pos_[0], first_goal[0]); EXPECT_LT(joint_pos_[0], second_goal[0]); @@ -2098,7 +2148,7 @@ TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parame // command interfaces: empty command_interface_types_ = {}; EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK); - auto state = traj_controller_->get_node()->configure(); + auto state = traj_controller_->configure(); EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); // command interfaces: bad_name @@ -2143,7 +2193,7 @@ TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parame command_interface_types_ = {"velocity"}; state_interface_types_ = {"position"}; EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK); - state = traj_controller_->get_node()->configure(); + state = traj_controller_->configure(); EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); state_interface_types_ = {"velocity"}; EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); @@ -2152,7 +2202,7 @@ TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parame command_interface_types_ = {"effort"}; state_interface_types_ = {"position"}; EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK); - state = traj_controller_->get_node()->configure(); + state = traj_controller_->configure(); EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); state_interface_types_ = {"velocity"}; EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index a850891331..7dff81f08f 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -278,7 +278,7 @@ class TrajectoryControllerTest : public ::testing::Test traj_controller_->set_node_options(node_options); return traj_controller_->init( - controller_name_, urdf, 0, "", traj_controller_->define_custom_node_options()); + controller_name_, urdf, 100, "", traj_controller_->define_custom_node_options()); } void SetPidParameters(double p_value = 0.0, double ff_value = 1.0) @@ -324,7 +324,7 @@ class TrajectoryControllerTest : public ::testing::Test // set pid parameters before configure SetPidParameters(k_p, ff); - traj_controller_->get_node()->configure(); + traj_controller_->configure(); ActivateTrajectoryController( separate_cmd_and_state_values, initial_pos_joints, initial_vel_joints, initial_acc_joints, From 9b344c7b5c06879b85cd4bcc135e35b5f7eedc2f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 22 Nov 2024 14:22:49 +0100 Subject: [PATCH 18/84] Add another dependency (#1382) --- ros2_controllers_test_nodes/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 0b97d6bb0c..21eb31f6df 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -17,6 +17,7 @@ https://github.com/ros-controls/ros2_controllers/ rclpy + sensor_msgs std_msgs trajectory_msgs From d8b30f864ea70126e989cc451ddab482be8850f7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 27 Nov 2024 22:47:43 +0100 Subject: [PATCH 19/84] Rename Twist->TwistStamped (#1393) --- .../diff_drive_controller.hpp | 14 +++++------ .../src/diff_drive_controller.cpp | 23 ++++++++++--------- 2 files changed, 19 insertions(+), 18 deletions(-) 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 ac34b81eca..74526b199c 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 @@ -45,7 +45,7 @@ namespace diff_drive_controller { class DiffDriveController : public controller_interface::ControllerInterface { - using Twist = geometry_msgs::msg::TwistStamped; + using TwistStamped = geometry_msgs::msg::TwistStamped; public: DIFF_DRIVE_CONTROLLER_PUBLIC @@ -128,20 +128,20 @@ class DiffDriveController : public controller_interface::ControllerInterface realtime_odometry_transform_publisher_ = nullptr; bool subscriber_is_active_ = false; - rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; + rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; - realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; + realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; - std::queue previous_commands_; // last two commands + std::queue previous_commands_; // last two commands // speed limiters SpeedLimiter limiter_linear_; SpeedLimiter limiter_angular_; bool publish_limited_velocity_ = false; - std::shared_ptr> limited_velocity_publisher_ = nullptr; - std::shared_ptr> realtime_limited_velocity_publisher_ = - nullptr; + std::shared_ptr> limited_velocity_publisher_ = nullptr; + std::shared_ptr> + realtime_limited_velocity_publisher_ = nullptr; rclcpp::Time previous_update_timestamp_{0}; diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 66da6d6738..d21cac602d 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -111,7 +111,7 @@ controller_interface::return_type DiffDriveController::update( return controller_interface::return_type::OK; } - std::shared_ptr last_command_msg; + std::shared_ptr last_command_msg; received_velocity_msg_ptr_.get(last_command_msg); if (last_command_msg == nullptr) @@ -130,7 +130,7 @@ controller_interface::return_type DiffDriveController::update( // command may be limited further by SpeedLimit, // without affecting the stored twist command - Twist command = *last_command_msg; + TwistStamped command = *last_command_msg; double & linear_command = command.twist.linear.x; double & angular_command = command.twist.angular.z; @@ -318,23 +318,24 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( if (publish_limited_velocity_) { - limited_velocity_publisher_ = - get_node()->create_publisher(DEFAULT_COMMAND_OUT_TOPIC, rclcpp::SystemDefaultsQoS()); + limited_velocity_publisher_ = get_node()->create_publisher( + DEFAULT_COMMAND_OUT_TOPIC, rclcpp::SystemDefaultsQoS()); realtime_limited_velocity_publisher_ = - std::make_shared>(limited_velocity_publisher_); + std::make_shared>( + limited_velocity_publisher_); } - const Twist empty_twist; - received_velocity_msg_ptr_.set(std::make_shared(empty_twist)); + const TwistStamped empty_twist; + received_velocity_msg_ptr_.set(std::make_shared(empty_twist)); // Fill last two commands with default constructed commands previous_commands_.emplace(empty_twist); previous_commands_.emplace(empty_twist); // initialize command subscriber - velocity_command_subscriber_ = get_node()->create_subscription( + velocity_command_subscriber_ = get_node()->create_subscription( DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void + [this](const std::shared_ptr msg) -> void { if (!subscriber_is_active_) { @@ -475,7 +476,7 @@ controller_interface::CallbackReturn DiffDriveController::on_cleanup( return controller_interface::CallbackReturn::ERROR; } - received_velocity_msg_ptr_.set(std::make_shared()); + received_velocity_msg_ptr_.set(std::make_shared()); return controller_interface::CallbackReturn::SUCCESS; } @@ -493,7 +494,7 @@ bool DiffDriveController::reset() odometry_.resetOdometry(); // release the old queue - std::queue empty; + std::queue empty; std::swap(previous_commands_, empty); registered_left_wheel_handles_.clear(); From f64c964b2ed772d7d75e8d7e5d46a1f35c8159bb Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Fri, 29 Nov 2024 16:21:01 +0100 Subject: [PATCH 20/84] JTC: sum periods (#1395) --- .../joint_trajectory_controller.hpp | 2 ++ .../src/joint_trajectory_controller.cpp | 11 ++++++++--- .../test/test_trajectory_actions.cpp | 9 ++++++--- 3 files changed, 16 insertions(+), 6 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 67954b8378..2377b2c0b6 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -127,6 +127,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa Params params_; rclcpp::Duration update_period_{0, 0}; + rclcpp::Time traj_time_; + trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; /// Specify interpolation method. Default to splines. interpolation_methods::InterpolationMethod interpolation_method_{ diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 0fc39ecaaf..82d9c94aab 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -196,15 +196,20 @@ controller_interface::return_type JointTrajectoryController::update( traj_external_point_ptr_->set_point_before_trajectory_msg( time, state_current_, joints_angle_wraparound_); } + traj_time_ = time; + } + else + { + traj_time_ += period; } // Sample expected state from the trajectory traj_external_point_ptr_->sample( - time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr); + traj_time_, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr); // Sample setpoint for next control cycle const bool valid_point = traj_external_point_ptr_->sample( - time + update_period_, interpolation_method_, command_next_, start_segment_itr, + traj_time_ + update_period_, interpolation_method_, command_next_, start_segment_itr, end_segment_itr, false); if (valid_point) @@ -217,7 +222,7 @@ controller_interface::return_type JointTrajectoryController::update( // time_difference is // - negative until first point is reached // - counting from zero to time_from_start of next point - double time_difference = time.seconds() - segment_time_from_start.seconds(); + double time_difference = traj_time_.seconds() - segment_time_from_start.seconds(); bool tolerance_violated_while_moving = false; bool outside_goal_tolerance = false; bool within_goal_time = true; diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 164f7d0e11..d4979deef8 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -83,12 +83,15 @@ class TestTrajectoryActions : public TrajectoryControllerTest { // controller hardware cycle update loop auto clock = rclcpp::Clock(RCL_STEADY_TIME); - auto start_time = clock.now(); + auto now_time = clock.now(); + auto last_time = now_time; rclcpp::Duration wait = rclcpp::Duration::from_seconds(2.0); - auto end_time = start_time + wait; + auto end_time = last_time + wait; while (clock.now() < end_time) { - traj_controller_->update(clock.now(), clock.now() - start_time); + now_time = clock.now(); + traj_controller_->update(now_time, now_time - last_time); + last_time = now_time; } }); From ff3c8a0242bb65e00f7bd65b85add08275c2bcfa Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 1 Dec 2024 11:09:50 +0100 Subject: [PATCH 21/84] Bump version of pre-commit hooks (#1401) Co-authored-by: christophfroehlich <3367244+christophfroehlich@users.noreply.github.com> --- .pre-commit-config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 205e0f63ab..75c5402ffe 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -63,7 +63,7 @@ repos: # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v19.1.3 + rev: v19.1.4 hooks: - id: clang-format args: ['-fallback-style=none', '-i'] @@ -133,7 +133,7 @@ repos: exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ - repo: https://github.com/python-jsonschema/check-jsonschema - rev: 0.29.4 + rev: 0.30.0 hooks: - id: check-github-workflows args: ["--verbose"] From 36068e180aa510ab79a9c19a91f9de8ad5e74f06 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 1 Dec 2024 23:02:56 +0100 Subject: [PATCH 22/84] Add explicit cast to period.count() (#1404) --- .../include/gripper_controllers/hardware_interface_adapter.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp index 2cc24794b8..98fd5a56b1 100644 --- a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp +++ b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp @@ -165,7 +165,8 @@ class HardwareInterfaceAdapter // Time since the last call to update const auto period = std::chrono::steady_clock::now() - last_update_time_; // Update PIDs - double command = pid_->computeCommand(error_position, error_velocity, period.count()); + double command = + pid_->computeCommand(error_position, error_velocity, static_cast(period.count())); command = std::min( fabs(max_allowed_effort), std::max(-fabs(max_allowed_effort), command)); joint_handle_->get().set_value(command); From f9edc41a451877e292b206069aa2e8bfd25148c0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 4 Dec 2024 11:16:28 +0100 Subject: [PATCH 23/84] Fix RealtimeBox API changes (#1385) --- .../diff_drive_controller.hpp | 1 + .../src/diff_drive_controller.cpp | 29 ++++++++++--------- .../test/test_diff_drive_controller.cpp | 3 +- .../tricycle_controller.hpp | 1 + .../src/tricycle_controller.cpp | 26 +++++++++-------- .../test/test_tricycle_controller.cpp | 3 +- 6 files changed, 35 insertions(+), 28 deletions(-) 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 74526b199c..84712fe748 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 @@ -131,6 +131,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 d21cac602d..d710c81257 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -111,26 +111,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; @@ -325,12 +326,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( @@ -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 & stored_value) + { stored_value = std::move(msg); }); }); // initialize odometry publisher and message @@ -476,7 +478,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 72ae4dbfd7..e29c6bbfee 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -49,7 +49,8 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr 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; } 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; } From cd730558b2c449568c627f066cdf8bfa8bc4c3bf Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 4 Dec 2024 15:06:45 +0100 Subject: [PATCH 24/84] Use the .hpp headers from `realtime_tools` package (#1406) --- .../include/admittance_controller/admittance_controller.hpp | 4 ++-- .../include/diff_drive_controller/diff_drive_controller.hpp | 4 ++-- .../force_torque_sensor_broadcaster.hpp | 2 +- .../forward_command_controller/forward_controllers_base.hpp | 2 +- .../include/gpio_controllers/gpio_command_controller.hpp | 4 ++-- .../gripper_controllers/gripper_action_controller.hpp | 4 ++-- .../imu_sensor_broadcaster/imu_sensor_broadcaster.hpp | 2 +- .../joint_state_broadcaster/joint_state_broadcaster.hpp | 2 +- .../joint_trajectory_controller.hpp | 6 +++--- .../mecanum_drive_controller/mecanum_drive_controller.hpp | 4 ++-- .../include/mecanum_drive_controller/odometry.hpp | 4 ++-- .../parallel_gripper_action_controller.hpp | 4 ++-- pid_controller/include/pid_controller/pid_controller.hpp | 4 ++-- .../include/pose_broadcaster/pose_broadcaster.hpp | 2 +- .../range_sensor_broadcaster/range_sensor_broadcaster.hpp | 2 +- .../steering_controllers_library.hpp | 4 ++-- .../include/tricycle_controller/tricycle_controller.hpp | 4 ++-- 17 files changed, 29 insertions(+), 29 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 9be6c3298c..89cd195ce4 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -32,8 +32,8 @@ #include "rclcpp/duration.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" #include "semantic_components/force_torque_sensor.hpp" namespace admittance_controller 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 84712fe748..4fc4d37d4b 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 @@ -34,8 +34,8 @@ #include "nav_msgs/msg/odometry.hpp" #include "odometry.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_box.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_box.hpp" +#include "realtime_tools/realtime_publisher.hpp" #include "tf2_msgs/msg/tf_message.hpp" // auto-generated by generate_parameter_library diff --git a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp index e5a5349c32..2364dd7c8b 100644 --- a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp @@ -28,7 +28,7 @@ #include "force_torque_sensor_broadcaster_parameters.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_publisher.hpp" #include "semantic_components/force_torque_sensor.hpp" namespace force_torque_sensor_broadcaster diff --git a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp index 4858e5ab64..fce7941d8a 100644 --- a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp @@ -23,7 +23,7 @@ #include "forward_command_controller/visibility_control.h" #include "rclcpp/subscription.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_buffer.hpp" #include "std_msgs/msg/float64_multi_array.hpp" namespace forward_command_controller diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp index 5149c74a1c..febac1294e 100644 --- a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -26,8 +26,8 @@ #include "gpio_controllers/visibility_control.h" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" namespace gpio_controllers { diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp index 38e0bd8191..478168391b 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp @@ -37,8 +37,8 @@ #include "gripper_controllers/visibility_control.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_server_goal_handle.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_server_goal_handle.hpp" // Project #include "gripper_controllers/hardware_interface_adapter.hpp" diff --git a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp index 1ba0b032ac..449020b6e2 100644 --- a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp @@ -26,7 +26,7 @@ // auto-generated by generate_parameter_library #include "imu_sensor_broadcaster_parameters.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_publisher.hpp" #include "semantic_components/imu_sensor.hpp" #include "sensor_msgs/msg/imu.hpp" diff --git a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp index ecc3c767f6..7ac98eccfb 100644 --- a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp @@ -25,7 +25,7 @@ #include "joint_state_broadcaster/visibility_control.h" // auto-generated by generate_parameter_library #include "joint_state_broadcaster_parameters.hpp" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_publisher.hpp" #include "sensor_msgs/msg/joint_state.hpp" #include "urdf/model.h" diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 2377b2c0b6..324ccfe4f1 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -36,9 +36,9 @@ #include "rclcpp/timer.hpp" #include "rclcpp_action/server.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" -#include "realtime_tools/realtime_server_goal_handle.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" +#include "realtime_tools/realtime_server_goal_handle.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp index 0fd7f6af29..f835752d43 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -29,8 +29,8 @@ #include "mecanum_drive_controller_parameters.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" #include "std_srvs/srv/set_bool.hpp" #include "control_msgs/msg/mecanum_drive_controller_state.hpp" diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp index ac1ad060dd..241809e8c2 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp @@ -16,8 +16,8 @@ #define MECANUM_DRIVE_CONTROLLER__ODOMETRY_HPP_ #include "geometry_msgs/msg/twist.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" #define PLANAR_POINT_DIM 3 diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp index d303990b89..739d1c570a 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp @@ -37,8 +37,8 @@ #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" #include "parallel_gripper_controller/visibility_control.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_server_goal_handle.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_server_goal_handle.hpp" // Project #include "parallel_gripper_action_controller_parameters.hpp" diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index ce66fd06d4..0aba6cf849 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -29,8 +29,8 @@ #include "pid_controller/visibility_control.h" #include "pid_controller_parameters.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" #include "std_srvs/srv/set_bool.hpp" namespace pid_controller diff --git a/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp index 621a90cc85..9317fce5e1 100644 --- a/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp +++ b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp @@ -25,7 +25,7 @@ #include "pose_broadcaster_parameters.hpp" #include "rclcpp/publisher.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_publisher.hpp" #include "semantic_components/pose_sensor.hpp" #include "tf2_msgs/msg/tf_message.hpp" diff --git a/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp b/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp index 5a93f95982..2e4e47b018 100644 --- a/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp +++ b/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp @@ -25,7 +25,7 @@ #include "range_sensor_broadcaster/visibility_control.h" #include "range_sensor_broadcaster_parameters.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_publisher.hpp" #include "semantic_components/range_sensor.hpp" #include "sensor_msgs/msg/range.hpp" diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index 84a892d79e..49236986ee 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -23,8 +23,8 @@ #include "controller_interface/chainable_controller_interface.hpp" #include "hardware_interface/handle.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" #include "steering_controllers_library/steering_odometry.hpp" #include "steering_controllers_library/visibility_control.h" #include "steering_controllers_library_parameters.hpp" diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index 1f9361dadd..4d64549613 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -33,8 +33,8 @@ #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_box.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_box.hpp" +#include "realtime_tools/realtime_publisher.hpp" #include "std_srvs/srv/empty.hpp" #include "tf2_msgs/msg/tf_message.hpp" #include "tricycle_controller/odometry.hpp" From b9a7cfc7dcdd4cb6c49971ae906344d45ac2fb15 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 6 Dec 2024 10:15:25 +0100 Subject: [PATCH 25/84] Don't call shutdown() after an exception (#1400) --- .../publisher_forward_position_controller.py | 6 +----- .../publisher_joint_trajectory_controller.py | 6 +----- 2 files changed, 2 insertions(+), 10 deletions(-) diff --git a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py index 51582a90d4..27fb4535df 100644 --- a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py +++ b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py @@ -70,14 +70,10 @@ def main(args=None): try: rclpy.spin(publisher_forward_position) - except KeyboardInterrupt: + except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException): print("Keyboard interrupt received. Shutting down node.") except Exception as e: print(f"Unhandled exception: {e}") - finally: - if rclpy.ok(): - publisher_forward_position.destroy_node() - rclpy.shutdown() if __name__ == "__main__": diff --git a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py index 91d39855aa..cf38890407 100644 --- a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py +++ b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py @@ -186,14 +186,10 @@ def main(args=None): try: rclpy.spin(publisher_joint_trajectory) - except KeyboardInterrupt: + except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException): print("Keyboard interrupt received. Shutting down node.") except Exception as e: print(f"Unhandled exception: {e}") - finally: - if rclpy.ok(): - publisher_joint_trajectory.destroy_node() - rclpy.shutdown() if __name__ == "__main__": From dc60f6fb888096b90a81cae4564c86653a95e4de Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 6 Dec 2024 10:38:09 +0100 Subject: [PATCH 26/84] Add missing dependency to gpio_controllers (#1410) --- gpio_controllers/package.xml | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/gpio_controllers/package.xml b/gpio_controllers/package.xml index 9b98746545..c8c60fe522 100644 --- a/gpio_controllers/package.xml +++ b/gpio_controllers/package.xml @@ -4,10 +4,21 @@ gpio_controllers 4.15.0 Controllers to interact with gpios. - Maciej Bednarczyk - Wiktor Bajor + + Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Maciej Bednarczyk + Wiktor Bajor + ament_cmake controller_interface @@ -22,6 +33,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets From 27f59461b9506e32d26b46d86a79c2264c5b0498 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 7 Dec 2024 09:56:47 +0000 Subject: [PATCH 27/84] Update changelogs & align version numbers --- ackermann_steering_controller/CHANGELOG.rst | 6 + admittance_controller/CHANGELOG.rst | 7 + bicycle_steering_controller/CHANGELOG.rst | 6 + diff_drive_controller/CHANGELOG.rst | 9 + effort_controllers/CHANGELOG.rst | 6 + force_torque_sensor_broadcaster/CHANGELOG.rst | 7 + forward_command_controller/CHANGELOG.rst | 7 + gpio_controllers/CHANGELOG.rst | 214 ++++++++++++++++++ gpio_controllers/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 8 + imu_sensor_broadcaster/CHANGELOG.rst | 7 + joint_state_broadcaster/CHANGELOG.rst | 7 + joint_trajectory_controller/CHANGELOG.rst | 9 + mecanum_drive_controller/CHANGELOG.rst | 213 +++++++++++++++++ mecanum_drive_controller/package.xml | 2 +- parallel_gripper_controller/CHANGELOG.rst | 7 + pid_controller/CHANGELOG.rst | 7 + pose_broadcaster/CHANGELOG.rst | 7 + position_controllers/CHANGELOG.rst | 6 + range_sensor_broadcaster/CHANGELOG.rst | 7 + ros2_controllers/CHANGELOG.rst | 7 + ros2_controllers_test_nodes/CHANGELOG.rst | 9 + rqt_joint_trajectory_controller/CHANGELOG.rst | 5 + steering_controllers_library/CHANGELOG.rst | 8 + tricycle_controller/CHANGELOG.rst | 9 + tricycle_steering_controller/CHANGELOG.rst | 6 + velocity_controllers/CHANGELOG.rst | 6 + 27 files changed, 592 insertions(+), 2 deletions(-) create mode 100644 gpio_controllers/CHANGELOG.rst create mode 100644 mecanum_drive_controller/CHANGELOG.rst diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 1f275bd7ba..504617d317 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 0d7daa5f5d..aa2736002f 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use the .hpp headers from `realtime_tools` package (`#1406 `_) +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- * Adding use of robot description parameter in the Admittance Controller (`#1247 `_) diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 57d23a977a..27c0e3f27d 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index e79f1faf1d..cb6c718ffa 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use the .hpp headers from `realtime_tools` package (`#1406 `_) +* Fix RealtimeBox API changes (`#1385 `_) +* Rename Twist->TwistStamped (`#1393 `_) +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 0abc5feee9..09dc92a195 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 6bd9e2febd..d3a7c3d0de 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use the .hpp headers from `realtime_tools` package (`#1406 `_) +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- * [ForceTorqueSensorBroadcaster] added force and torque offsets to the parameters + export state interfaces (`#1215 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index ae920d9f83..730a7a3c6a 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use the .hpp headers from `realtime_tools` package (`#1406 `_) +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- diff --git a/gpio_controllers/CHANGELOG.rst b/gpio_controllers/CHANGELOG.rst new file mode 100644 index 0000000000..02ade18ca6 --- /dev/null +++ b/gpio_controllers/CHANGELOG.rst @@ -0,0 +1,214 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package gpio_controllers +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Add missing dependency to gpio_controllers (`#1410 `_) +* Use the .hpp headers from `realtime_tools` package (`#1406 `_) +* Gpio command controller (`#1251 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota, Wiktor Bajor + +4.16.0 (2024-11-08) +------------------- + +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- + +4.12.1 (2024-08-14) +------------------- + +4.12.0 (2024-07-23) +------------------- + +4.11.0 (2024-07-09) +------------------- + +4.10.0 (2024-07-01) +------------------- + +4.9.0 (2024-06-05) +------------------ + +4.8.0 (2024-05-14) +------------------ + +4.7.0 (2024-03-22) +------------------ + +4.6.0 (2024-02-12) +------------------ + +4.5.0 (2024-01-31) +------------------ + +4.4.0 (2024-01-11) +------------------ + +4.3.0 (2024-01-08) +------------------ + +4.2.0 (2023-12-12) +------------------ + +4.1.0 (2023-12-01) +------------------ + +4.0.0 (2023-11-21) +------------------ + +3.17.0 (2023-10-31) +------------------- + +3.16.0 (2023-09-20) +------------------- + +3.15.0 (2023-09-11) +------------------- + +3.14.0 (2023-08-16) +------------------- + +3.13.0 (2023-08-04) +------------------- + +3.12.0 (2023-07-18) +------------------- + +3.11.0 (2023-06-24) +------------------- + +3.10.1 (2023-06-06) +------------------- + +3.10.0 (2023-06-04) +------------------- + +3.9.0 (2023-05-28) +------------------ + +3.8.0 (2023-05-14) +------------------ + +3.7.0 (2023-05-02) +------------------ + +3.6.0 (2023-04-29) +------------------ + +3.5.0 (2023-04-14) +------------------ + +3.4.0 (2023-04-02) +------------------ + +3.3.0 (2023-03-07) +------------------ + +3.2.0 (2023-02-10) +------------------ + +3.1.0 (2023-01-26) +------------------ + +3.0.0 (2023-01-19) +------------------ + +2.15.0 (2022-12-06) +------------------- + +2.14.0 (2022-11-18) +------------------- + +2.13.0 (2022-10-05) +------------------- + +2.12.0 (2022-09-01) +------------------- + +2.11.0 (2022-08-04) +------------------- + +2.10.0 (2022-08-01) +------------------- + +2.9.0 (2022-07-14) +------------------ + +2.8.0 (2022-07-09) +------------------ + +2.7.0 (2022-07-03) +------------------ + +2.6.0 (2022-06-18) +------------------ + +2.5.0 (2022-05-13) +------------------ + +2.4.0 (2022-04-29) +------------------ + +2.3.0 (2022-04-21) +------------------ + +2.2.0 (2022-03-25) +------------------ + +2.1.0 (2022-02-23) +------------------ + +2.0.1 (2022-02-01) +------------------ + +2.0.0 (2022-01-28) +------------------ + +1.3.0 (2022-01-11) +------------------ + +1.2.0 (2021-12-29) +------------------ + +1.1.0 (2021-10-25) +------------------ + +1.0.0 (2021-09-29) +------------------ + +0.5.0 (2021-08-30) +------------------ + +0.4.1 (2021-07-08) +------------------ + +0.4.0 (2021-06-28) +------------------ + +0.3.1 (2021-05-23) +------------------ + +0.3.0 (2021-05-21) +------------------ + +0.2.1 (2021-05-03) +------------------ + +0.2.0 (2021-02-06) +------------------ + +0.1.2 (2021-01-07) +------------------ + +0.1.1 (2021-01-06) +------------------ + +0.1.0 (2020-12-23) +------------------ diff --git a/gpio_controllers/package.xml b/gpio_controllers/package.xml index c8c60fe522..4f800a4cad 100644 --- a/gpio_controllers/package.xml +++ b/gpio_controllers/package.xml @@ -2,7 +2,7 @@ gpio_controllers - 4.15.0 + 4.16.0 Controllers to interact with gpios. Bence Magyar diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 291cab691a..512a481bcd 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use the .hpp headers from `realtime_tools` package (`#1406 `_) +* Add explicit cast to period.count() (`#1404 `_) +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 2219d53fe4..ba19caac8c 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use the .hpp headers from `realtime_tools` package (`#1406 `_) +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index ed2f4b31eb..c89bbc6737 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use the .hpp headers from `realtime_tools` package (`#1406 `_) +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- * [JSB] Fix the behaviour of publishing unavailable state interfaces when they are previously available (`#1331 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 87089f700a..3003ce830b 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use the .hpp headers from `realtime_tools` package (`#1406 `_) +* JTC: sum periods (`#1395 `_) +* [JTC] Sample at t + dT instead of the beginning of the control cycle (`#1306 `_) +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Felix Exner (fexner), Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- * [JTC] Fix the JTC length_error exceptions in the tests (`#1360 `_) diff --git a/mecanum_drive_controller/CHANGELOG.rst b/mecanum_drive_controller/CHANGELOG.rst new file mode 100644 index 0000000000..82e43faea5 --- /dev/null +++ b/mecanum_drive_controller/CHANGELOG.rst @@ -0,0 +1,213 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package mecanum_drive_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Use the .hpp headers from `realtime_tools` package (`#1406 `_) +* Add Mecanum Drive Controller (`#512 `_) +* Contributors: Dr. Denis, Sai Kishor Kothakota + +4.16.0 (2024-11-08) +------------------- + +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- + +4.12.1 (2024-08-14) +------------------- + +4.12.0 (2024-07-23) +------------------- + +4.11.0 (2024-07-09) +------------------- + +4.10.0 (2024-07-01) +------------------- + +4.9.0 (2024-06-05) +------------------ + +4.8.0 (2024-05-14) +------------------ + +4.7.0 (2024-03-22) +------------------ + +4.6.0 (2024-02-12) +------------------ + +4.5.0 (2024-01-31) +------------------ + +4.4.0 (2024-01-11) +------------------ + +4.3.0 (2024-01-08) +------------------ + +4.2.0 (2023-12-12) +------------------ + +4.1.0 (2023-12-01) +------------------ + +4.0.0 (2023-11-21) +------------------ + +3.17.0 (2023-10-31) +------------------- + +3.16.0 (2023-09-20) +------------------- + +3.15.0 (2023-09-11) +------------------- + +3.14.0 (2023-08-16) +------------------- + +3.13.0 (2023-08-04) +------------------- + +3.12.0 (2023-07-18) +------------------- + +3.11.0 (2023-06-24) +------------------- + +3.10.1 (2023-06-06) +------------------- + +3.10.0 (2023-06-04) +------------------- + +3.9.0 (2023-05-28) +------------------ + +3.8.0 (2023-05-14) +------------------ + +3.7.0 (2023-05-02) +------------------ + +3.6.0 (2023-04-29) +------------------ + +3.5.0 (2023-04-14) +------------------ + +3.4.0 (2023-04-02) +------------------ + +3.3.0 (2023-03-07) +------------------ + +3.2.0 (2023-02-10) +------------------ + +3.1.0 (2023-01-26) +------------------ + +3.0.0 (2023-01-19) +------------------ + +2.15.0 (2022-12-06) +------------------- + +2.14.0 (2022-11-18) +------------------- + +2.13.0 (2022-10-05) +------------------- + +2.12.0 (2022-09-01) +------------------- + +2.11.0 (2022-08-04) +------------------- + +2.10.0 (2022-08-01) +------------------- + +2.9.0 (2022-07-14) +------------------ + +2.8.0 (2022-07-09) +------------------ + +2.7.0 (2022-07-03) +------------------ + +2.6.0 (2022-06-18) +------------------ + +2.5.0 (2022-05-13) +------------------ + +2.4.0 (2022-04-29) +------------------ + +2.3.0 (2022-04-21) +------------------ + +2.2.0 (2022-03-25) +------------------ + +2.1.0 (2022-02-23) +------------------ + +2.0.1 (2022-02-01) +------------------ + +2.0.0 (2022-01-28) +------------------ + +1.3.0 (2022-01-11) +------------------ + +1.2.0 (2021-12-29) +------------------ + +1.1.0 (2021-10-25) +------------------ + +1.0.0 (2021-09-29) +------------------ + +0.5.0 (2021-08-30) +------------------ + +0.4.1 (2021-07-08) +------------------ + +0.4.0 (2021-06-28) +------------------ + +0.3.1 (2021-05-23) +------------------ + +0.3.0 (2021-05-21) +------------------ + +0.2.1 (2021-05-03) +------------------ + +0.2.0 (2021-02-06) +------------------ + +0.1.2 (2021-01-07) +------------------ + +0.1.1 (2021-01-06) +------------------ + +0.1.0 (2020-12-23) +------------------ diff --git a/mecanum_drive_controller/package.xml b/mecanum_drive_controller/package.xml index 97aadf4327..e446098de6 100644 --- a/mecanum_drive_controller/package.xml +++ b/mecanum_drive_controller/package.xml @@ -2,7 +2,7 @@ mecanum_drive_controller - 0.0.0 + 4.16.0 Implementation of mecanum drive controller for 4 wheel drive. diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst index 6ee5646ac4..7d12f514e1 100644 --- a/parallel_gripper_controller/CHANGELOG.rst +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package parallel_gripper_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use the .hpp headers from `realtime_tools` package (`#1406 `_) +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 9c20af4e97..dc6b61831d 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use the .hpp headers from `realtime_tools` package (`#1406 `_) +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- * fixes for windows compilation (`#1330 `_) diff --git a/pose_broadcaster/CHANGELOG.rst b/pose_broadcaster/CHANGELOG.rst index c12bc9da80..44af47b18c 100644 --- a/pose_broadcaster/CHANGELOG.rst +++ b/pose_broadcaster/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package pose_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use the .hpp headers from `realtime_tools` package (`#1406 `_) +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- * Add hardware_interface_testing dependency (`#1335 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index e5478f518a..0c6460ff34 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 5d97df0e0b..ab9dbb2f09 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use the .hpp headers from `realtime_tools` package (`#1406 `_) +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index a45a425f24..62235a2440 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add Mecanum Drive Controller (`#512 `_) +* Gpio command controller (`#1251 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Dr. Denis, Wiktor Bajor + 4.16.0 (2024-11-08) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index d3c3059abf..782f49edb9 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Don't call shutdown() after an exception (`#1400 `_) +* Add another dependency (`#1382 `_) +* Add missing deps for test_nodes (`#1378 `_) +* test_nodes: catch keyboard interrupt and add simple launch tests (`#1369 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich + 4.16.0 (2024-11-08) ------------------- diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 555a636d7e..36c587c364 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich + 4.16.0 (2024-11-08) ------------------- diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 417c4c9f05..e46f9db9f8 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use the .hpp headers from `realtime_tools` package (`#1406 `_) +* Add Mecanum Drive Controller (`#512 `_) +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Dr. Denis, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 31258cac7c..5b7e16fc1f 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use the .hpp headers from `realtime_tools` package (`#1406 `_) +* Fix RealtimeBox API changes (`#1385 `_) +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* TractionLimiter: Fix wrong input checks (`#1341 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 975c48303d..db1532b6a3 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 90b143f675..1c4f38f430 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- From 2c7047e527b09916ca58c6031e4cb6ee665416d8 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 7 Dec 2024 09:57:16 +0000 Subject: [PATCH 28/84] 4.17.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gpio_controllers/CHANGELOG.rst | 4 ++-- gpio_controllers/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- mecanum_drive_controller/CHANGELOG.rst | 4 ++-- mecanum_drive_controller/package.xml | 2 +- parallel_gripper_controller/CHANGELOG.rst | 4 ++-- parallel_gripper_controller/package.xml | 2 +- pid_controller/CHANGELOG.rst | 4 ++-- pid_controller/package.xml | 2 +- pose_broadcaster/CHANGELOG.rst | 4 ++-- pose_broadcaster/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 52 files changed, 77 insertions(+), 77 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 504617d317..fe06a8aaa2 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) * Update maintainers and add url tags (`#1363 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 6ae2f0a438..dbb466cead 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.16.0 + 4.17.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Bence Magyar diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index aa2736002f..900bb68242 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) * Update maintainers and add url tags (`#1363 `_) diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 5b48c69695..fbd0dbc8dd 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.16.0 + 4.17.0 Implementation of admittance controllers for different input and output interface. Bence Magyar diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 27c0e3f27d..37a975fd0d 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) * Update maintainers and add url tags (`#1363 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 3fafbda077..186818c71a 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.16.0 + 4.17.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index cb6c718ffa..5789720d29 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) * Fix RealtimeBox API changes (`#1385 `_) * Rename Twist->TwistStamped (`#1393 `_) diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 0602d91898..6147cfcc84 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.16.0 + 4.17.0 Controller for a differential-drive mobile base. Bence Magyar diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 09dc92a195..6ecf9eefc0 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) * Update maintainers and add url tags (`#1363 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index b33224db9b..28943d2984 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.16.0 + 4.17.0 Generic controller for forwarding commands. Bence Magyar diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index d3a7c3d0de..765aad14ee 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) * Update maintainers and add url tags (`#1363 `_) diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index f2f5124001..446376ebb7 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.16.0 + 4.17.0 Controller to publish state of force-torque sensors. Bence Magyar diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 730a7a3c6a..668700a056 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) * Update maintainers and add url tags (`#1363 `_) diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index be71c32052..a332d5525e 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.16.0 + 4.17.0 Generic controller for forwarding commands. Bence Magyar diff --git a/gpio_controllers/CHANGELOG.rst b/gpio_controllers/CHANGELOG.rst index 02ade18ca6..7af5d2bd31 100644 --- a/gpio_controllers/CHANGELOG.rst +++ b/gpio_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gpio_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Add missing dependency to gpio_controllers (`#1410 `_) * Use the .hpp headers from `realtime_tools` package (`#1406 `_) * Gpio command controller (`#1251 `_) diff --git a/gpio_controllers/package.xml b/gpio_controllers/package.xml index 4f800a4cad..a0560389aa 100644 --- a/gpio_controllers/package.xml +++ b/gpio_controllers/package.xml @@ -2,7 +2,7 @@ gpio_controllers - 4.16.0 + 4.17.0 Controllers to interact with gpios. Bence Magyar diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 512a481bcd..f2f2fcb4a9 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) * Add explicit cast to period.count() (`#1404 `_) * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 33f06f226a..0de8e1b14d 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 4.16.0 + 4.17.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index ba19caac8c..cc97a2d28d 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) * Update maintainers and add url tags (`#1363 `_) diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index a09f09dee2..905f6c9c97 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.16.0 + 4.17.0 Controller to publish readings of IMU sensors. Bence Magyar diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index c89bbc6737..d9a94bd335 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) * Update maintainers and add url tags (`#1363 `_) diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 575c17c5a0..61915ab2b0 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.16.0 + 4.17.0 Broadcaster to publish joint state Bence Magyar diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 3003ce830b..9c609ed561 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) * JTC: sum periods (`#1395 `_) * [JTC] Sample at t + dT instead of the beginning of the control cycle (`#1306 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 830321e7b6..886d37b97a 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.16.0 + 4.17.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar diff --git a/mecanum_drive_controller/CHANGELOG.rst b/mecanum_drive_controller/CHANGELOG.rst index 82e43faea5..22b7afb932 100644 --- a/mecanum_drive_controller/CHANGELOG.rst +++ b/mecanum_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package mecanum_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) * Add Mecanum Drive Controller (`#512 `_) * Contributors: Dr. Denis, Sai Kishor Kothakota diff --git a/mecanum_drive_controller/package.xml b/mecanum_drive_controller/package.xml index e446098de6..149601e2d8 100644 --- a/mecanum_drive_controller/package.xml +++ b/mecanum_drive_controller/package.xml @@ -2,7 +2,7 @@ mecanum_drive_controller - 4.16.0 + 4.17.0 Implementation of mecanum drive controller for 4 wheel drive. diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst index 7d12f514e1..b085543641 100644 --- a/parallel_gripper_controller/CHANGELOG.rst +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package parallel_gripper_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) * Update maintainers and add url tags (`#1363 `_) diff --git a/parallel_gripper_controller/package.xml b/parallel_gripper_controller/package.xml index 7e712b632a..219ce0b15a 100644 --- a/parallel_gripper_controller/package.xml +++ b/parallel_gripper_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> parallel_gripper_controller - 4.16.0 + 4.17.0 The parallel_gripper_controller package Bence Magyar diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index dc6b61831d..3ef0927df3 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) * Update maintainers and add url tags (`#1363 `_) diff --git a/pid_controller/package.xml b/pid_controller/package.xml index fb3f40184a..b8912e24bf 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.16.0 + 4.17.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar diff --git a/pose_broadcaster/CHANGELOG.rst b/pose_broadcaster/CHANGELOG.rst index 44af47b18c..3abf140f40 100644 --- a/pose_broadcaster/CHANGELOG.rst +++ b/pose_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pose_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) * Update maintainers and add url tags (`#1363 `_) diff --git a/pose_broadcaster/package.xml b/pose_broadcaster/package.xml index dc5d629b50..3802a7495e 100644 --- a/pose_broadcaster/package.xml +++ b/pose_broadcaster/package.xml @@ -2,7 +2,7 @@ pose_broadcaster - 4.16.0 + 4.17.0 Broadcaster to publish cartesian states. Bence Magyar diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 0c6460ff34..3a843e74bd 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) * Update maintainers and add url tags (`#1363 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 8aa7bf8117..56fbf02b64 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.16.0 + 4.17.0 Generic controller for forwarding commands. Bence Magyar diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index ab9dbb2f09..716fbbbaf6 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) * Update maintainers and add url tags (`#1363 `_) diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index a81823a6a2..a368683dd2 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.16.0 + 4.17.0 Controller to publish readings of range sensors. Bence Magyar diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 62235a2440..b59b7f93a2 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Add Mecanum Drive Controller (`#512 `_) * Gpio command controller (`#1251 `_) * Update maintainers and add url tags (`#1363 `_) diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 99ca06c77b..ec612a3139 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.16.0 + 4.17.0 Metapackage for ros2_controllers related packages Bence Magyar diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 782f49edb9..365994cede 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Don't call shutdown() after an exception (`#1400 `_) * Add another dependency (`#1382 `_) * Add missing deps for test_nodes (`#1378 `_) diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 21eb31f6df..12164d8e3a 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.16.0 + 4.17.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Bence Magyar diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index 3900392a10..cb7332f67e 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.16.0", + version="4.17.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 36c587c364..6d8fd768cc 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Update maintainers and add url tags (`#1363 `_) * Contributors: Christoph Fröhlich diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index da92d9d663..570e0a707e 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 4.16.0 + 4.17.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index f2ee71c3d7..01ae56e876 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -21,7 +21,7 @@ setup( name=package_name, - version="4.16.0", + version="4.17.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index e46f9db9f8..962a44e99a 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) * Add Mecanum Drive Controller (`#512 `_) * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index efe9f93a67..ddeabdf96c 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.16.0 + 4.17.0 Package for steering robot configurations including odometry and interfaces. Bence Magyar diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 5b7e16fc1f..1e10cc42b3 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) * Fix RealtimeBox API changes (`#1385 `_) * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 516adef248..d365725518 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.16.0 + 4.17.0 Controller for a tricycle drive mobile base Bence Magyar diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index db1532b6a3..92a416eb68 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) * Update maintainers and add url tags (`#1363 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 7c26e09ba0..c49822629b 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.16.0 + 4.17.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Bence Magyar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 1c4f38f430..975e8a2afd 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) * Update maintainers and add url tags (`#1363 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 50e662cc58..362be2fe27 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.16.0 + 4.17.0 Generic controller for forwarding commands. Bence Magyar From a6c1c0c1c209fcfee8ed81825e7c7e7784d54eee Mon Sep 17 00:00:00 2001 From: Lennart Nachtigall Date: Mon, 9 Dec 2024 11:56:32 +0100 Subject: [PATCH 29/84] Add wrench offset for admittance controller (#1249) --- admittance_controller/doc/userdoc.rst | 3 ++ .../admittance_controller.hpp | 3 ++ .../src/admittance_controller.cpp | 40 ++++++++++++++++++- doc/release_notes.rst | 1 + 4 files changed, 46 insertions(+), 1 deletion(-) diff --git a/admittance_controller/doc/userdoc.rst b/admittance_controller/doc/userdoc.rst index 8056a017d7..c624269d86 100644 --- a/admittance_controller/doc/userdoc.rst +++ b/admittance_controller/doc/userdoc.rst @@ -32,6 +32,9 @@ Topics ~/joint_references (input topic) [trajectory_msgs::msg::JointTrajectoryPoint] Target joint commands when controller is not in chained mode. +~/wrench_reference (input topic) [geometry_msgs::msg::WrenchStamped] + Target wrench offset (WrenchStamped has to be in the frame of the FT-sensor). + ~/state (output topic) [control_msgs::msg::AdmittanceControllerState] Topic publishing internal states. diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 89cd195ce4..17b70b264d 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -133,6 +133,8 @@ class AdmittanceController : public controller_interface::ChainableControllerInt // ROS subscribers rclcpp::Subscription::SharedPtr input_joint_command_subscriber_; + rclcpp::Subscription::SharedPtr + input_wrench_command_subscriber_; rclcpp::Publisher::SharedPtr s_publisher_; // admittance parameters @@ -144,6 +146,7 @@ class AdmittanceController : public controller_interface::ChainableControllerInt // real-time buffer realtime_tools::RealtimeBuffer> input_joint_command_; + realtime_tools::RealtimeBuffer input_wrench_command_; std::unique_ptr> state_publisher_; trajectory_msgs::msg::JointTrajectoryPoint last_commanded_; diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 6e0e38140a..58a5bcd6e2 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -27,6 +27,23 @@ namespace admittance_controller { + +geometry_msgs::msg::Wrench add_wrenches( + const geometry_msgs::msg::Wrench & a, const geometry_msgs::msg::Wrench & b) +{ + geometry_msgs::msg::Wrench res; + + res.force.x = a.force.x + b.force.x; + res.force.y = a.force.y + b.force.y; + res.force.z = a.force.z + b.force.z; + + res.torque.x = a.torque.x + b.torque.x; + res.torque.y = a.torque.y + b.torque.y; + res.torque.z = a.torque.z + b.torque.z; + + return res; +} + controller_interface::CallbackReturn AdmittanceController::on_init() { // initialize controller config @@ -116,6 +133,7 @@ AdmittanceController::on_export_reference_interfaces() reference_interfaces_.resize(num_chainable_interfaces, std::numeric_limits::quiet_NaN()); position_reference_ = {}; velocity_reference_ = {}; + input_wrench_command_.reset(); // assign reference interfaces auto index = 0ul; @@ -265,6 +283,24 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( input_joint_command_subscriber_ = get_node()->create_subscription( "~/joint_references", rclcpp::SystemDefaultsQoS(), joint_command_callback); + + input_wrench_command_subscriber_ = + get_node()->create_subscription( + "~/wrench_reference", rclcpp::SystemDefaultsQoS(), + [&](const geometry_msgs::msg::WrenchStamped & msg) + { + if ( + msg.header.frame_id != admittance_->parameters_.ft_sensor.frame.id && + !msg.header.frame_id.empty()) + { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), "Ignoring wrench reference as it is on the wrong frame: " + << msg.header.frame_id << ". Expected reference frame: " + << admittance_->parameters_.ft_sensor.frame.id); + return; + } + input_wrench_command_.writeFromNonRT(msg); + }); s_publisher_ = get_node()->create_publisher( "~/status", rclcpp::SystemDefaultsQoS()); state_publisher_ = @@ -398,8 +434,10 @@ controller_interface::return_type AdmittanceController::update_and_write_command // get all controller inputs read_state_from_hardware(joint_state_, ft_values_); + auto offsetted_ft_values = add_wrenches(ft_values_, input_wrench_command_.readFromRT()->wrench); + // apply admittance control to reference to determine desired state - admittance_->update(joint_state_, ft_values_, reference_, period, reference_admittance_); + admittance_->update(joint_state_, offsetted_ft_values, reference_, period, reference_admittance_); // write calculated values to joint interfaces write_state_to_hardware(reference_admittance_); diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 974bca880f..c7149faf03 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -7,6 +7,7 @@ This list summarizes the changes between Iron (previous) and Jazzy (current) rel admittance_controller ************************ * Remove ``robot_description`` parameter from parameter YAML, because it is not used at all (`#963 `_). +* Added ``~/wrench_reference`` input topic which allows to provide a force-torque offset as WrenchStamped (`#1249 `_). diff_drive_controller ***************************** From de42962cdab61c110cf913846fde087c119e76de Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 9 Dec 2024 11:57:24 +0100 Subject: [PATCH 30/84] Add missing plugins to ros2_controllers dependencies (#1413) --- ros2_controllers/package.xml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index ec612a3139..294d851e40 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -25,11 +25,14 @@ force_torque_sensor_broadcaster forward_command_controller gpio_controllers + gripper_controllers imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller mecanum_drive_controller + parallel_gripper_controller pid_controller + pose_broadcaster position_controllers range_sensor_broadcaster steering_controllers_library From 330b9e4e4ea18b423ff1cc0f8c8f7f0724ee33f4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 9 Dec 2024 13:04:12 +0100 Subject: [PATCH 31/84] Add sai (#1416) --- .github/mergify.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/mergify.yml b/.github/mergify.yml index c2eda7385b..84ad380e90 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -23,7 +23,7 @@ pull_request_rules: - author=mergify[bot] actions: comment: - message: This pull request is in conflict. Could you fix it @bmagyar @destogl @christophfroehlich? + message: This pull request is in conflict. Could you fix it @bmagyar @destogl @christophfroehlich @saikishor? - name: development targets master branch conditions: @@ -31,6 +31,7 @@ pull_request_rules: - author!=bmagyar - author!=destogl - author!=christophfroehlich + - author!=saikishor - author!=mergify[bot] - author!=dependabot[bot] actions: From 66d160c6ae2b596dd3749e658cb01e2ac00d35a7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 9 Dec 2024 17:15:51 +0100 Subject: [PATCH 32/84] [CI] Add clang job and setup concurrency (#1407) --- .../workflows/humble-abi-compatibility.yml | 5 +++ .github/workflows/humble-binary-build.yml | 5 +++ .github/workflows/humble-check-docs.yml | 4 +++ .github/workflows/humble-coverage-build.yml | 5 +++ .github/workflows/humble-debian-build.yml | 4 +++ .github/workflows/humble-pre-commit.yml | 4 +++ .../humble-rhel-semi-binary-build.yml | 5 +++ .../workflows/humble-semi-binary-build.yml | 22 +++++++++--- .github/workflows/jazzy-abi-compatibility.yml | 5 +++ .github/workflows/jazzy-binary-build.yml | 5 +++ .github/workflows/jazzy-check-docs.yml | 4 +++ .github/workflows/jazzy-coverage-build.yml | 5 +++ .github/workflows/jazzy-debian-build.yml | 4 +++ .github/workflows/jazzy-pre-commit.yml | 4 +++ .../jazzy-rhel-semi-binary-build.yml | 4 +++ .github/workflows/jazzy-semi-binary-build.yml | 22 +++++++++--- .../workflows/rolling-abi-compatibility.yml | 5 +++ .github/workflows/rolling-binary-build.yml | 5 +++ .github/workflows/rolling-check-docs.yml | 4 +++ ...ling-compatibility-humble-binary-build.yml | 14 ++++---- ...lling-compatibility-jazzy-binary-build.yml | 14 ++++---- .github/workflows/rolling-coverage-build.yml | 5 +++ .github/workflows/rolling-debian-build.yml | 4 +++ .github/workflows/rolling-pre-commit.yml | 4 +++ .../rolling-rhel-semi-binary-build.yml | 4 +++ .../workflows/rolling-semi-binary-build.yml | 22 +++++++++--- .github/workflows/rosdoc2.yml | 3 ++ .../admittance_controller/admittance_rule.hpp | 9 ++--- .../admittance_rule_impl.hpp | 35 +++++++++++-------- .../src/admittance_controller.cpp | 4 +-- .../src/diff_drive_controller.cpp | 2 +- .../test_load_forward_command_controller.cpp | 2 +- .../test/test_joint_state_broadcaster.cpp | 4 +-- .../tolerances.hpp | 4 +-- .../trajectory.hpp | 4 +-- .../src/joint_trajectory_controller.cpp | 6 ++-- .../src/trajectory.cpp | 4 +-- .../test/test_trajectory_controller_utils.hpp | 4 ++- pid_controller/src/pid_controller.cpp | 2 +- ros2_controllers.humble.repos | 2 +- .../src/steering_controllers_library.cpp | 3 +- .../src/steering_odometry.cpp | 5 ++- .../src/tricycle_controller.cpp | 2 +- 43 files changed, 214 insertions(+), 69 deletions(-) diff --git a/.github/workflows/humble-abi-compatibility.yml b/.github/workflows/humble-abi-compatibility.yml index 247371ba7d..6cfd2c6068 100644 --- a/.github/workflows/humble-abi-compatibility.yml +++ b/.github/workflows/humble-abi-compatibility.yml @@ -15,6 +15,11 @@ on: - '**/CMakeLists.txt' - 'ros2_controllers-not-released.humble.repos' +concurrency: + # cancel previous runs of the same workflow, except for pushes on humble branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: abi_check: runs-on: ubuntu-latest diff --git a/.github/workflows/humble-binary-build.yml b/.github/workflows/humble-binary-build.yml index a031d823cf..f013f18880 100644 --- a/.github/workflows/humble-binary-build.yml +++ b/.github/workflows/humble-binary-build.yml @@ -34,6 +34,11 @@ on: # Run every morning to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on humble branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master diff --git a/.github/workflows/humble-check-docs.yml b/.github/workflows/humble-check-docs.yml index 0eeeaedf40..448ff4084b 100644 --- a/.github/workflows/humble-check-docs.yml +++ b/.github/workflows/humble-check-docs.yml @@ -10,6 +10,10 @@ on: - '**.md' - '**.yaml' +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + jobs: check-docs: name: Check Docs diff --git a/.github/workflows/humble-coverage-build.yml b/.github/workflows/humble-coverage-build.yml index d0dcb9c350..40d1de7052 100644 --- a/.github/workflows/humble-coverage-build.yml +++ b/.github/workflows/humble-coverage-build.yml @@ -30,6 +30,11 @@ on: - '**/CMakeLists.txt' - 'ros2_controllers.humble.repos' +concurrency: + # cancel previous runs of the same workflow, except for pushes on humble branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: coverage_humble: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master diff --git a/.github/workflows/humble-debian-build.yml b/.github/workflows/humble-debian-build.yml index cd25248caf..e52ffe6842 100644 --- a/.github/workflows/humble-debian-build.yml +++ b/.github/workflows/humble-debian-build.yml @@ -18,6 +18,10 @@ on: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on humble branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} jobs: humble_debian: diff --git a/.github/workflows/humble-pre-commit.yml b/.github/workflows/humble-pre-commit.yml index 5bb2408578..38a76ee025 100644 --- a/.github/workflows/humble-pre-commit.yml +++ b/.github/workflows/humble-pre-commit.yml @@ -6,6 +6,10 @@ on: branches: - humble +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + jobs: pre-commit: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master diff --git a/.github/workflows/humble-rhel-semi-binary-build.yml b/.github/workflows/humble-rhel-semi-binary-build.yml index 25c755ada5..6019d08e46 100644 --- a/.github/workflows/humble-rhel-semi-binary-build.yml +++ b/.github/workflows/humble-rhel-semi-binary-build.yml @@ -18,6 +18,11 @@ on: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on humble branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: humble_rhel_binary: name: Humble RHEL binary build diff --git a/.github/workflows/humble-semi-binary-build.yml b/.github/workflows/humble-semi-binary-build.yml index c53c46032a..2d0437782b 100644 --- a/.github/workflows/humble-semi-binary-build.yml +++ b/.github/workflows/humble-semi-binary-build.yml @@ -33,15 +33,27 @@ on: # Run every morning to detect flakiness and broken dependencies - cron: '33 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on humble branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: semi_binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master - strategy: - fail-fast: false - matrix: - ROS_REPO: [testing] with: ros_distro: humble - ros_repo: ${{ matrix.ROS_REPO }} + ros_repo: testing upstream_workspace: ros2_controllers.humble.repos ref_for_scheduled_build: humble + semi_binary_clang: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: humble + ros_repo: testing + upstream_workspace: ros2_controllers-not-released.humble.repos + ref_for_scheduled_build: humble + additional_debs: clang + c_compiler: clang + cxx_compiler: clang++ + not_test_build: true diff --git a/.github/workflows/jazzy-abi-compatibility.yml b/.github/workflows/jazzy-abi-compatibility.yml index 8e557b5ee3..5da5fb9d00 100644 --- a/.github/workflows/jazzy-abi-compatibility.yml +++ b/.github/workflows/jazzy-abi-compatibility.yml @@ -14,6 +14,11 @@ on: - '**/CMakeLists.txt' - 'ros2_controllers-not-released.jazzy.repos' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: abi_check: runs-on: ubuntu-latest diff --git a/.github/workflows/jazzy-binary-build.yml b/.github/workflows/jazzy-binary-build.yml index cda4969abf..7dd294a55b 100644 --- a/.github/workflows/jazzy-binary-build.yml +++ b/.github/workflows/jazzy-binary-build.yml @@ -35,6 +35,11 @@ on: # Run every morning to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master diff --git a/.github/workflows/jazzy-check-docs.yml b/.github/workflows/jazzy-check-docs.yml index 2f9eaf18bd..cfc67a3eac 100644 --- a/.github/workflows/jazzy-check-docs.yml +++ b/.github/workflows/jazzy-check-docs.yml @@ -10,6 +10,10 @@ on: - '**.md' - '**.yaml' +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + jobs: check-docs: name: Check Docs diff --git a/.github/workflows/jazzy-coverage-build.yml b/.github/workflows/jazzy-coverage-build.yml index c96ac88c32..2f587c7003 100644 --- a/.github/workflows/jazzy-coverage-build.yml +++ b/.github/workflows/jazzy-coverage-build.yml @@ -33,6 +33,11 @@ on: # - '**/CMakeLists.txt' # - 'ros2_controllers.jazzy.repos' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: coverage_jazzy: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master diff --git a/.github/workflows/jazzy-debian-build.yml b/.github/workflows/jazzy-debian-build.yml index e3e3b8a353..b1a60f1528 100644 --- a/.github/workflows/jazzy-debian-build.yml +++ b/.github/workflows/jazzy-debian-build.yml @@ -18,6 +18,10 @@ on: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} jobs: jazzy_debian: diff --git a/.github/workflows/jazzy-pre-commit.yml b/.github/workflows/jazzy-pre-commit.yml index d9ec610bbc..aab5ba52ac 100644 --- a/.github/workflows/jazzy-pre-commit.yml +++ b/.github/workflows/jazzy-pre-commit.yml @@ -6,6 +6,10 @@ on: branches: - master +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + jobs: pre-commit: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master diff --git a/.github/workflows/jazzy-rhel-semi-binary-build.yml b/.github/workflows/jazzy-rhel-semi-binary-build.yml index 1c62fcf2ac..f39c9cc570 100644 --- a/.github/workflows/jazzy-rhel-semi-binary-build.yml +++ b/.github/workflows/jazzy-rhel-semi-binary-build.yml @@ -18,6 +18,10 @@ on: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} jobs: jazzy_rhel: diff --git a/.github/workflows/jazzy-semi-binary-build.yml b/.github/workflows/jazzy-semi-binary-build.yml index a462d9040a..e38fd5b7ba 100644 --- a/.github/workflows/jazzy-semi-binary-build.yml +++ b/.github/workflows/jazzy-semi-binary-build.yml @@ -35,15 +35,27 @@ on: # Run every morning to detect flakiness and broken dependencies - cron: '33 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: semi_binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master - strategy: - fail-fast: false - matrix: - ROS_REPO: [testing] with: ros_distro: jazzy - ros_repo: ${{ matrix.ROS_REPO }} + ros_repo: testing upstream_workspace: ros2_controllers.jazzy.repos ref_for_scheduled_build: master + semi_binary_clang: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: jazzy + ros_repo: testing + upstream_workspace: ros2_controllers.jazzy.repos + ref_for_scheduled_build: jazzy + additional_debs: clang + c_compiler: clang + cxx_compiler: clang++ + not_test_build: true diff --git a/.github/workflows/rolling-abi-compatibility.yml b/.github/workflows/rolling-abi-compatibility.yml index da3337554c..1f0ec61d40 100644 --- a/.github/workflows/rolling-abi-compatibility.yml +++ b/.github/workflows/rolling-abi-compatibility.yml @@ -14,6 +14,11 @@ on: - '**/CMakeLists.txt' - 'ros2_controllers-not-released.rolling.repos' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: abi_check: runs-on: ubuntu-latest diff --git a/.github/workflows/rolling-binary-build.yml b/.github/workflows/rolling-binary-build.yml index b512eb9db7..a3686144f3 100644 --- a/.github/workflows/rolling-binary-build.yml +++ b/.github/workflows/rolling-binary-build.yml @@ -36,6 +36,11 @@ on: # Run every morning to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master diff --git a/.github/workflows/rolling-check-docs.yml b/.github/workflows/rolling-check-docs.yml index 15b7eff57e..1144c4e6c0 100644 --- a/.github/workflows/rolling-check-docs.yml +++ b/.github/workflows/rolling-check-docs.yml @@ -11,6 +11,10 @@ on: - '**.yaml' - '.github/workflows/rolling-check-docs.yml' +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + jobs: check-docs: name: Check Docs diff --git a/.github/workflows/rolling-compatibility-humble-binary-build.yml b/.github/workflows/rolling-compatibility-humble-binary-build.yml index 7aab02fa62..6370db30d1 100644 --- a/.github/workflows/rolling-compatibility-humble-binary-build.yml +++ b/.github/workflows/rolling-compatibility-humble-binary-build.yml @@ -33,16 +33,16 @@ on: - '**/CMakeLists.txt' - 'ros2_controllers.rolling.repos' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: build-on-humble: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master - strategy: - fail-fast: false - matrix: - ROS_DISTRO: [humble] - ROS_REPO: [testing] with: - ros_distro: ${{ matrix.ROS_DISTRO }} - ros_repo: ${{ matrix.ROS_REPO }} + ros_distro: humble + ros_repo: testing upstream_workspace: ros2_controllers.rolling.repos ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-compatibility-jazzy-binary-build.yml b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml index 37ffbb84a4..4da98f2d09 100644 --- a/.github/workflows/rolling-compatibility-jazzy-binary-build.yml +++ b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml @@ -33,16 +33,16 @@ on: - '**/CMakeLists.txt' - 'ros2_controllers.rolling.repos' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: build-on-jazzy: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master - strategy: - fail-fast: false - matrix: - ROS_DISTRO: [jazzy] - ROS_REPO: [testing] with: - ros_distro: ${{ matrix.ROS_DISTRO }} - ros_repo: ${{ matrix.ROS_REPO }} + ros_distro: jazzy + ros_repo: testing upstream_workspace: ros2_controllers.rolling.repos ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-coverage-build.yml b/.github/workflows/rolling-coverage-build.yml index 058ff9bb33..abea8a90db 100644 --- a/.github/workflows/rolling-coverage-build.yml +++ b/.github/workflows/rolling-coverage-build.yml @@ -30,6 +30,11 @@ on: - '**/CMakeLists.txt' - 'ros2_controllers.rolling.repos' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: coverage_rolling: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master diff --git a/.github/workflows/rolling-debian-build.yml b/.github/workflows/rolling-debian-build.yml index d7efd3ea0a..8f9394762b 100644 --- a/.github/workflows/rolling-debian-build.yml +++ b/.github/workflows/rolling-debian-build.yml @@ -18,6 +18,10 @@ on: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} jobs: rolling_debian: diff --git a/.github/workflows/rolling-pre-commit.yml b/.github/workflows/rolling-pre-commit.yml index 7bc07ba802..792278d6d2 100644 --- a/.github/workflows/rolling-pre-commit.yml +++ b/.github/workflows/rolling-pre-commit.yml @@ -6,6 +6,10 @@ on: branches: - master +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + jobs: pre-commit: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master diff --git a/.github/workflows/rolling-rhel-semi-binary-build.yml b/.github/workflows/rolling-rhel-semi-binary-build.yml index 29a53b810a..0f03ad02c2 100644 --- a/.github/workflows/rolling-rhel-semi-binary-build.yml +++ b/.github/workflows/rolling-rhel-semi-binary-build.yml @@ -18,6 +18,10 @@ on: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} jobs: rolling_rhel: diff --git a/.github/workflows/rolling-semi-binary-build.yml b/.github/workflows/rolling-semi-binary-build.yml index db0dd1fe64..06f4c55612 100644 --- a/.github/workflows/rolling-semi-binary-build.yml +++ b/.github/workflows/rolling-semi-binary-build.yml @@ -35,15 +35,27 @@ on: # Run every morning to detect flakiness and broken dependencies - cron: '33 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: semi_binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master - strategy: - fail-fast: false - matrix: - ROS_REPO: [testing] with: ros_distro: rolling - ros_repo: ${{ matrix.ROS_REPO }} + ros_repo: testing upstream_workspace: ros2_controllers.rolling.repos ref_for_scheduled_build: master + semi_binary_clang: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: rolling + ros_repo: testing + upstream_workspace: ros2_controllers.rolling.repos + ref_for_scheduled_build: rolling + additional_debs: clang + c_compiler: clang + cxx_compiler: clang++ + not_test_build: true diff --git a/.github/workflows/rosdoc2.yml b/.github/workflows/rosdoc2.yml index 0771248e79..434344fcaa 100644 --- a/.github/workflows/rosdoc2.yml +++ b/.github/workflows/rosdoc2.yml @@ -8,6 +8,9 @@ on: - ros2_controllers/rosdoc2.yaml - ros2_controllers/package.xml +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true jobs: check: diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index a326b663d0..bfb3a612c7 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -63,10 +63,11 @@ struct AdmittanceState mass_inv.setZero(); stiffness.setZero(); selected_axes.setZero(); - current_joint_pos = Eigen::VectorXd::Zero(num_joints); - joint_pos = Eigen::VectorXd::Zero(num_joints); - joint_vel = Eigen::VectorXd::Zero(num_joints); - joint_acc = Eigen::VectorXd::Zero(num_joints); + auto idx = static_cast(num_joints); + current_joint_pos = Eigen::VectorXd::Zero(idx); + joint_pos = Eigen::VectorXd::Zero(idx); + joint_vel = Eigen::VectorXd::Zero(idx); + joint_acc = Eigen::VectorXd::Zero(idx); } Eigen::VectorXd current_joint_pos; diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 13d4e67fbc..859c7e3336 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -136,9 +136,11 @@ void AdmittanceRule::apply_parameters_update() for (size_t i = 0; i < NUM_CARTESIAN_DOF; ++i) { - admittance_state_.mass_inv[i] = 1.0 / parameters_.admittance.mass[i]; - admittance_state_.damping[i] = parameters_.admittance.damping_ratio[i] * 2 * - sqrt(admittance_state_.mass[i] * admittance_state_.stiffness[i]); + auto idx = static_cast(i); + admittance_state_.mass_inv[idx] = 1.0 / parameters_.admittance.mass[i]; + admittance_state_.damping[idx] = + parameters_.admittance.damping_ratio[i] * 2 * + sqrt(admittance_state_.mass[idx] * admittance_state_.stiffness[idx]); } } @@ -216,12 +218,13 @@ controller_interface::return_type AdmittanceRule::update( // update joint desired joint state for (size_t i = 0; i < num_joints_; ++i) { + auto idx = static_cast(i); desired_joint_state.positions[i] = - reference_joint_state.positions[i] + admittance_state_.joint_pos[i]; + reference_joint_state.positions[i] + admittance_state_.joint_pos[idx]; desired_joint_state.velocities[i] = - reference_joint_state.velocities[i] + admittance_state_.joint_vel[i]; + reference_joint_state.velocities[i] + admittance_state_.joint_vel[idx]; desired_joint_state.accelerations[i] = - reference_joint_state.accelerations[i] + admittance_state_.joint_acc[i]; + reference_joint_state.accelerations[i] + admittance_state_.joint_acc[idx]; } return controller_interface::return_type::OK; @@ -334,7 +337,7 @@ void AdmittanceRule::process_wrench_measurements( new_wrench_base.block<3, 1>(0, 1) -= (cog_world_rot * cog_pos_).cross(end_effector_weight_); // apply smoothing filter - for (size_t i = 0; i < 6; ++i) + for (Eigen::Index i = 0; i < 6; ++i) { wrench_world_(i) = filters::exponentialSmoothing( new_wrench_base(i), wrench_world_(i), parameters_.ft_sensor.filter_coefficient); @@ -345,18 +348,20 @@ const control_msgs::msg::AdmittanceControllerState & AdmittanceRule::get_control { for (size_t i = 0; i < NUM_CARTESIAN_DOF; ++i) { - state_message_.stiffness.data[i] = admittance_state_.stiffness[i]; - state_message_.damping.data[i] = admittance_state_.damping[i]; - state_message_.selected_axes.data[i] = static_cast(admittance_state_.selected_axes[i]); - state_message_.mass.data[i] = admittance_state_.mass[i]; + auto idx = static_cast(i); + state_message_.stiffness.data[i] = admittance_state_.stiffness[idx]; + state_message_.damping.data[i] = admittance_state_.damping[idx]; + state_message_.selected_axes.data[i] = static_cast(admittance_state_.selected_axes[idx]); + state_message_.mass.data[i] = admittance_state_.mass[idx]; } for (size_t i = 0; i < parameters_.joints.size(); ++i) { + auto idx = static_cast(i); state_message_.joint_state.name[i] = parameters_.joints[i]; - state_message_.joint_state.position[i] = admittance_state_.joint_pos[i]; - state_message_.joint_state.velocity[i] = admittance_state_.joint_vel[i]; - state_message_.joint_state.effort[i] = admittance_state_.joint_acc[i]; + state_message_.joint_state.position[i] = admittance_state_.joint_pos[idx]; + state_message_.joint_state.velocity[i] = admittance_state_.joint_vel[idx]; + state_message_.joint_state.effort[i] = admittance_state_.joint_acc[idx]; } state_message_.wrench_base.wrench.force.x = admittance_state_.wrench_base[0]; @@ -411,7 +416,7 @@ void AdmittanceRule::vec_to_eigen(const std::vector & data, T2 & matrix) { for (auto row = 0; row < matrix.rows(); row++) { - matrix(row, col) = data[row + col * matrix.rows()]; + matrix(row, col) = data[static_cast(row + col * matrix.rows())]; } } } diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 58a5bcd6e2..468c02624b 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -340,7 +340,7 @@ controller_interface::CallbackReturn AdmittanceController::on_activate( { auto it = std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - auto index = std::distance(allowed_interface_types_.begin(), it); + auto index = static_cast(std::distance(allowed_interface_types_.begin(), it)); if (!controller_interface::get_ordered_interfaces( state_interfaces_, admittance_->parameters_.joints, interface, joint_state_interface_[index])) @@ -355,7 +355,7 @@ controller_interface::CallbackReturn AdmittanceController::on_activate( { auto it = std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - auto index = std::distance(allowed_interface_types_.begin(), it); + auto index = static_cast(std::distance(allowed_interface_types_.begin(), it)); if (!controller_interface::get_ordered_interfaces( command_interfaces_, command_joint_names_, interface, joint_command_interface_[index])) { diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index d710c81257..a93d3d3812 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -292,7 +292,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( const double right_wheel_radius = params_.right_wheel_radius_multiplier * params_.wheel_radius; odometry_.setWheelParams(wheel_separation, left_wheel_radius, right_wheel_radius); - odometry_.setVelocityRollingWindowSize(params_.velocity_rolling_window_size); + odometry_.setVelocityRollingWindowSize(static_cast(params_.velocity_rolling_window_size)); cmd_vel_timeout_ = std::chrono::milliseconds{static_cast(params_.cmd_vel_timeout * 1000.0)}; publish_limited_velocity_ = params_.publish_limited_velocity; diff --git a/forward_command_controller/test/test_load_forward_command_controller.cpp b/forward_command_controller/test/test_load_forward_command_controller.cpp index c192f1eb5f..ad2e53162a 100644 --- a/forward_command_controller/test/test_load_forward_command_controller.cpp +++ b/forward_command_controller/test/test_load_forward_command_controller.cpp @@ -30,7 +30,7 @@ TEST(TestLoadForwardCommandController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index de534c00b6..3ec7ff4ac2 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -1091,10 +1091,10 @@ void JointStateBroadcasterTest::test_published_dynamic_joint_state_message( ASSERT_THAT( dynamic_joint_state_msg->interface_values[i].interface_names, ElementsAreArray(INTERFACE_NAMES)); - const auto index_in_original_order = std::distance( + const auto index_in_original_order = static_cast(std::distance( joint_names_.cbegin(), std::find( - joint_names_.cbegin(), joint_names_.cend(), dynamic_joint_state_msg->joint_names[i])); + joint_names_.cbegin(), joint_names_.cend(), dynamic_joint_state_msg->joint_names[i]))); ASSERT_THAT( dynamic_joint_state_msg->interface_values[i].values, Each(joint_values_[index_in_original_order])); diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index 4902fd1dcc..b33a2bf5c5 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -199,7 +199,7 @@ SegmentTolerances get_segment_tolerances( .c_str()); return default_tolerances; } - auto i = std::distance(joints.cbegin(), it); + auto i = static_cast(std::distance(joints.cbegin(), it)); std::string interface = ""; try { @@ -246,7 +246,7 @@ SegmentTolerances get_segment_tolerances( .c_str()); return default_tolerances; } - auto i = std::distance(joints.cbegin(), it); + auto i = static_cast(std::distance(joints.cbegin(), it)); std::string interface = ""; try { diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index d650c369d0..74d4e28f3a 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -206,8 +206,8 @@ inline std::vector mapping(const T & t1, const T & t2) } else { - const size_t t1_dist = std::distance(t1.begin(), t1_it); - const size_t t2_dist = std::distance(t2.begin(), t2_it); + const size_t t1_dist = static_cast(std::distance(t1.begin(), t1_it)); + const size_t t2_dist = static_cast(std::distance(t2.begin(), t2_it)); mapping_vector[t1_dist] = t2_dist; } } diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 82d9c94aab..365ce993d0 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -385,7 +385,7 @@ controller_interface::return_type JointTrajectoryController::update( rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); rt_has_pending_goal_.writeFromNonRT(false); - RCLCPP_WARN(logger, error_string.c_str()); + RCLCPP_WARN(logger, "%s", error_string.c_str()); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_hold_position()); @@ -907,7 +907,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( { auto it = std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - auto index = std::distance(allowed_interface_types_.begin(), it); + auto index = static_cast(std::distance(allowed_interface_types_.begin(), it)); if (!controller_interface::get_ordered_interfaces( command_interfaces_, command_joint_names_, interface, joint_command_interface_[index])) { @@ -921,7 +921,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( { auto it = std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - auto index = std::distance(allowed_interface_types_.begin(), it); + auto index = static_cast(std::distance(allowed_interface_types_.begin(), it)); if (!controller_interface::get_ordered_interfaces( state_interfaces_, params_.joints, interface, joint_state_interface_[index])) { diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index 1c50d26c62..57366f88a8 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -175,8 +175,8 @@ bool Trajectory::sample( interpolate_between_points(t0, point, t1, next_point, sample_time, output_state); } - start_segment_itr = begin() + i; - end_segment_itr = begin() + (i + 1); + start_segment_itr = begin() + static_cast(i); + end_segment_itr = begin() + static_cast(i + 1); if (search_monotonically_increasing) { last_sample_idx_ = i; diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 7dff81f08f..b0e66394d1 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -467,7 +467,9 @@ class TrajectoryControllerTest : public ::testing::Test if (joint_names.empty()) { traj_msg.joint_names = { - joint_names_.begin(), joint_names_.begin() + points_positions[0].size()}; + joint_names_.begin(), + joint_names_.begin() + + static_cast::difference_type>(points_positions[0].size())}; } else { diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 032dc1d666..f3b2ba33a0 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -320,7 +320,7 @@ void PidController::reference_callback(const std::shared_ptrdof_names.begin(), found_it); + auto position = static_cast(std::distance(ref_msg->dof_names.begin(), found_it)); ref_msg->values[position] = msg->values[i]; ref_msg->values_dot[position] = msg->values_dot[i]; } diff --git a/ros2_controllers.humble.repos b/ros2_controllers.humble.repos index a0432f65b3..548c95b44a 100644 --- a/ros2_controllers.humble.repos +++ b/ros2_controllers.humble.repos @@ -6,7 +6,7 @@ repositories: realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git - version: master + version: humble kinematics_interface: type: git url: https://github.com/ros-controls/kinematics_interface.git diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 836574f150..3c14013f40 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -81,7 +81,8 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { params_ = param_listener_->get_params(); - odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); + odometry_.set_velocity_rolling_window_size( + static_cast(params_.velocity_rolling_window_size)); configure_odometry(); diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index dbe210ed41..8b8bb8bfd8 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -204,7 +204,10 @@ void SteeringOdometry::set_velocity_rolling_window_size(size_t velocity_rolling_ reset_accumulators(); } -void SteeringOdometry::set_odometry_type(const unsigned int type) { config_type_ = type; } +void SteeringOdometry::set_odometry_type(const unsigned int type) +{ + config_type_ = static_cast(type); +} double SteeringOdometry::convert_twist_to_steering_angle(double v_bx, double omega_bz) { diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 1d6daba958..bc04451ad6 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -236,7 +236,7 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & } odometry_.setWheelParams(params_.wheelbase, params_.wheel_radius); - odometry_.setVelocityRollingWindowSize(params_.velocity_rolling_window_size); + odometry_.setVelocityRollingWindowSize(static_cast(params_.velocity_rolling_window_size)); cmd_vel_timeout_ = std::chrono::milliseconds{params_.cmd_vel_timeout}; params_.publish_ackermann_command = From 950c9c1a056c8d8861000a8f11facb7e1cd9f8c9 Mon Sep 17 00:00:00 2001 From: Rafal Gorecki <126687345+rafal-gorecki@users.noreply.github.com> Date: Tue, 10 Dec 2024 17:57:39 +0100 Subject: [PATCH 33/84] Improve tf_prefix based on namespace (#1420) Co-authored-by: Kees van Teeffelen <107179662+kjvanteeffelen@users.noreply.github.com> --- diff_drive_controller/src/diff_drive_controller.cpp | 9 +++++---- .../test/test_diff_drive_controller.cpp | 6 +++--- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index a93d3d3812..7c3c9864d2 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -375,13 +375,14 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( tf_prefix = std::string(get_node()->get_namespace()); } - if (tf_prefix == "/") + // Make sure prefix does not start with '/' and always ends with '/' + if (tf_prefix.back() != '/') { - tf_prefix = ""; + tf_prefix = tf_prefix + "/"; } - else + if (tf_prefix.front() == '/') { - tf_prefix = tf_prefix + "/"; + tf_prefix.erase(0, 1); } } diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index e29c6bbfee..a984c5a5c3 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -392,7 +392,6 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_names TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_namespace) { std::string test_namespace = "/test_namespace"; - std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = ""; @@ -412,10 +411,11 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_name auto odometry_message = controller_->get_rt_odom_publisher()->msg_; std::string test_odom_frame_id = odometry_message.header.frame_id; std::string test_base_frame_id = odometry_message.child_frame_id; + std::string ns_prefix = test_namespace.erase(0, 1) + "/"; /* tf_frame_prefix_enable is true but frame_prefix is blank so namespace should be appended to the * frame id's */ - ASSERT_EQ(test_odom_frame_id, test_namespace + "/" + odom_id); - ASSERT_EQ(test_base_frame_id, test_namespace + "/" + base_link_id); + ASSERT_EQ(test_odom_frame_id, ns_prefix + odom_id); + ASSERT_EQ(test_base_frame_id, ns_prefix + base_link_id); } TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) From d32665a8cc0584682e28c6dc8efa392c83df654c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 16 Dec 2024 11:52:36 +0100 Subject: [PATCH 34/84] Update command limiter of diff_drive_controller (#1315) --- diff_drive_controller/CMakeLists.txt | 10 +- .../doc/parameters_context.yaml | 4 - .../custom_validators.hpp | 64 ++++++ .../diff_drive_controller.hpp | 1 - .../diff_drive_controller/odometry.hpp | 2 - .../diff_drive_controller/speed_limiter.hpp | 97 ++++++--- diff_drive_controller/package.xml | 1 + .../src/diff_drive_controller.cpp | 86 +++++++- .../src/diff_drive_controller_parameter.yaml | 100 ++++++++- diff_drive_controller/src/odometry.cpp | 10 +- diff_drive_controller/src/speed_limiter.cpp | 139 ------------ .../config/test_diff_drive_controller.yaml | 33 ++- .../test/test_diff_drive_controller.cpp | 205 +++++++++++++++++- doc/migration.rst | 1 + doc/release_notes.rst | 2 + 15 files changed, 540 insertions(+), 215 deletions(-) create mode 100644 diff_drive_controller/include/diff_drive_controller/custom_validators.hpp delete mode 100644 diff_drive_controller/src/speed_limiter.cpp diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index 4b3ff4f77f..d94b6e3ce0 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -8,6 +8,7 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") endif() set(THIS_PACKAGE_INCLUDE_DEPENDS + control_toolbox controller_interface generate_parameter_library geometry_msgs @@ -32,19 +33,21 @@ add_compile_definitions(RCPPUTILS_VERSION_MINOR=${rcpputils_VERSION_MINOR}) generate_parameter_library(diff_drive_controller_parameters src/diff_drive_controller_parameter.yaml + include/diff_drive_controller/custom_validators.hpp ) add_library(diff_drive_controller SHARED src/diff_drive_controller.cpp src/odometry.cpp - src/speed_limiter.cpp ) target_compile_features(diff_drive_controller PUBLIC cxx_std_17) target_include_directories(diff_drive_controller PUBLIC $ $ ) -target_link_libraries(diff_drive_controller PUBLIC diff_drive_controller_parameters) +target_link_libraries(diff_drive_controller + PUBLIC + diff_drive_controller_parameters) ament_target_dependencies(diff_drive_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. @@ -57,7 +60,8 @@ if(BUILD_TESTING) find_package(ros2_control_test_assets REQUIRED) ament_add_gmock(test_diff_drive_controller - test/test_diff_drive_controller.cpp) + test/test_diff_drive_controller.cpp + ) target_link_libraries(test_diff_drive_controller diff_drive_controller ) diff --git a/diff_drive_controller/doc/parameters_context.yaml b/diff_drive_controller/doc/parameters_context.yaml index 81e92806f5..d950f7f7e9 100644 --- a/diff_drive_controller/doc/parameters_context.yaml +++ b/diff_drive_controller/doc/parameters_context.yaml @@ -1,9 +1,5 @@ linear.x: | Joint limits structure for the linear ``x``-axis. - The limiter ignores position limits. - For details see ``joint_limits`` package from ros2_control repository. angular.z: | Joint limits structure for the rotation about ``z``-axis. - The limiter ignores position limits. - For details see ``joint_limits`` package from ros2_control repository. diff --git a/diff_drive_controller/include/diff_drive_controller/custom_validators.hpp b/diff_drive_controller/include/diff_drive_controller/custom_validators.hpp new file mode 100644 index 0000000000..53fae54560 --- /dev/null +++ b/diff_drive_controller/include/diff_drive_controller/custom_validators.hpp @@ -0,0 +1,64 @@ +// Copyright 2024 AIT - Austrian Institute of Technology GmbH +// +// 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. + +// TODO(christophfroehlich) remove this file and use it from control_toolbox once +// https://github.com/PickNikRobotics/generate_parameter_library/pull/213 is merged and released + +#ifndef DIFF_DRIVE_CONTROLLER__CUSTOM_VALIDATORS_HPP_ +#define DIFF_DRIVE_CONTROLLER__CUSTOM_VALIDATORS_HPP_ + +#include + +#include + +#include +#include +#include + +namespace diff_drive_controller +{ + +/** + * @brief gt_eq, but check only if the value is not NaN + */ +template +tl::expected gt_eq_or_nan(rclcpp::Parameter const & parameter, T expected_value) +{ + auto param_value = parameter.as_double(); + if (!std::isnan(param_value)) + { + // check only if the value is not NaN + return rsl::gt_eq(parameter, expected_value); + } + return {}; +} + +/** + * @brief lt_eq, but check only if the value is not NaN + */ +template +tl::expected lt_eq_or_nan(rclcpp::Parameter const & parameter, T expected_value) +{ + auto param_value = parameter.as_double(); + if (!std::isnan(param_value)) + { + // check only if the value is not NaN + return rsl::lt_eq(parameter, expected_value); + } + return {}; +} + +} // namespace diff_drive_controller + +#endif // DIFF_DRIVE_CONTROLLER__CUSTOM_VALIDATORS_HPP_ 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 4fc4d37d4b..85f4fb23b0 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 @@ -20,7 +20,6 @@ #define DIFF_DRIVE_CONTROLLER__DIFF_DRIVE_CONTROLLER_HPP_ #include -#include #include #include #include diff --git a/diff_drive_controller/include/diff_drive_controller/odometry.hpp b/diff_drive_controller/include/diff_drive_controller/odometry.hpp index edca431d3d..ae4417a788 100644 --- a/diff_drive_controller/include/diff_drive_controller/odometry.hpp +++ b/diff_drive_controller/include/diff_drive_controller/odometry.hpp @@ -22,8 +22,6 @@ #ifndef DIFF_DRIVE_CONTROLLER__ODOMETRY_HPP_ #define DIFF_DRIVE_CONTROLLER__ODOMETRY_HPP_ -#include - #include "rclcpp/time.hpp" // \note The versions conditioning is added here to support the source-compatibility with Humble #if RCPPUTILS_VERSION_MAJOR >= 2 && RCPPUTILS_VERSION_MINOR >= 6 diff --git a/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp b/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp index f6fe439f5d..4fa08fcdba 100644 --- a/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp +++ b/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp @@ -19,7 +19,9 @@ #ifndef DIFF_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_ #define DIFF_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_ -#include +#include + +#include "control_toolbox/rate_limiter.hpp" namespace diff_drive_controller { @@ -33,16 +35,65 @@ class SpeedLimiter * \param [in] has_jerk_limits if true, applies jerk limits * \param [in] min_velocity Minimum velocity [m/s], usually <= 0 * \param [in] max_velocity Maximum velocity [m/s], usually >= 0 - * \param [in] min_acceleration Minimum acceleration [m/s^2], usually <= 0 + * \param [in] max_deceleration Maximum deceleration [m/s^2], usually <= 0 * \param [in] max_acceleration Maximum acceleration [m/s^2], usually >= 0 * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0 * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0 */ + [[deprecated]] SpeedLimiter( + bool has_velocity_limits = true, bool has_acceleration_limits = true, + bool has_jerk_limits = true, double min_velocity = std::numeric_limits::quiet_NaN(), + double max_velocity = std::numeric_limits::quiet_NaN(), + double max_deceleration = std::numeric_limits::quiet_NaN(), + double max_acceleration = std::numeric_limits::quiet_NaN(), + double min_jerk = std::numeric_limits::quiet_NaN(), + double max_jerk = std::numeric_limits::quiet_NaN()) + { + if (!has_velocity_limits) + { + min_velocity = max_velocity = std::numeric_limits::quiet_NaN(); + } + if (!has_acceleration_limits) + { + max_deceleration = max_acceleration = std::numeric_limits::quiet_NaN(); + } + if (!has_jerk_limits) + { + min_jerk = max_jerk = std::numeric_limits::quiet_NaN(); + } + speed_limiter_ = control_toolbox::RateLimiter( + min_velocity, max_velocity, max_deceleration, max_acceleration, + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), min_jerk, + max_jerk); + } + + /** + * \brief Constructor + * \param [in] min_velocity Minimum velocity [m/s], usually <= 0 + * \param [in] max_velocity Maximum velocity [m/s], usually >= 0 + * \param [in] max_acceleration_reverse Maximum acceleration in reverse direction [m/s^2], usually + * <= 0 + * \param [in] max_acceleration Maximum acceleration [m/s^2], usually >= 0 + * \param [in] max_deceleration Maximum deceleration [m/s^2], usually <= 0 + * \param [in] max_deceleration_reverse Maximum deceleration in reverse direction [m/s^2], usually + * >= 0 + * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0 + * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0 + */ SpeedLimiter( - bool has_velocity_limits = false, bool has_acceleration_limits = false, - bool has_jerk_limits = false, double min_velocity = NAN, double max_velocity = NAN, - double min_acceleration = NAN, double max_acceleration = NAN, double min_jerk = NAN, - double max_jerk = NAN); + double min_velocity = std::numeric_limits::quiet_NaN(), + double max_velocity = std::numeric_limits::quiet_NaN(), + double max_acceleration_reverse = std::numeric_limits::quiet_NaN(), + double max_acceleration = std::numeric_limits::quiet_NaN(), + double max_deceleration = std::numeric_limits::quiet_NaN(), + double max_deceleration_reverse = std::numeric_limits::quiet_NaN(), + double min_jerk = std::numeric_limits::quiet_NaN(), + double max_jerk = std::numeric_limits::quiet_NaN()) + { + speed_limiter_ = control_toolbox::RateLimiter( + min_velocity, max_velocity, max_acceleration_reverse, max_acceleration, max_deceleration, + max_deceleration_reverse, min_jerk, max_jerk); + } /** * \brief Limit the velocity and acceleration @@ -52,14 +103,17 @@ class SpeedLimiter * \param [in] dt Time step [s] * \return Limiting factor (1.0 if none) */ - double limit(double & v, double v0, double v1, double dt); + double limit(double & v, double v0, double v1, double dt) + { + return speed_limiter_.limit(v, v0, v1, dt); + } /** * \brief Limit the velocity * \param [in, out] v Velocity [m/s] * \return Limiting factor (1.0 if none) */ - double limit_velocity(double & v); + double limit_velocity(double & v) { return speed_limiter_.limit_value(v); } /** * \brief Limit the acceleration @@ -68,7 +122,10 @@ class SpeedLimiter * \param [in] dt Time step [s] * \return Limiting factor (1.0 if none) */ - double limit_acceleration(double & v, double v0, double dt); + double limit_acceleration(double & v, double v0, double dt) + { + return speed_limiter_.limit_first_derivative(v, v0, dt); + } /** * \brief Limit the jerk @@ -79,25 +136,13 @@ class SpeedLimiter * \return Limiting factor (1.0 if none) * \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control */ - double limit_jerk(double & v, double v0, double v1, double dt); + double limit_jerk(double & v, double v0, double v1, double dt) + { + return speed_limiter_.limit_second_derivative(v, v0, v1, dt); + } private: - // Enable/Disable velocity/acceleration/jerk limits: - bool has_velocity_limits_; - bool has_acceleration_limits_; - bool has_jerk_limits_; - - // Velocity limits: - double min_velocity_; - double max_velocity_; - - // Acceleration limits: - double min_acceleration_; - double max_acceleration_; - - // Jerk limits: - double min_jerk_; - double max_jerk_; + control_toolbox::RateLimiter speed_limiter_; // Instance of the new RateLimiter }; } // namespace diff_drive_controller diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 6147cfcc84..1ed9673c79 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -24,6 +24,7 @@ generate_parameter_library backward_ros + control_toolbox controller_interface geometry_msgs hardware_interface diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 7c3c9864d2..6957864321 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -45,7 +45,14 @@ using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; using lifecycle_msgs::msg::State; -DiffDriveController::DiffDriveController() : controller_interface::ControllerInterface() {} +DiffDriveController::DiffDriveController() +: controller_interface::ControllerInterface(), + // dummy limiter, will be created in on_configure + // could be done with shared_ptr instead -> but will break ABI + limiter_angular_(std::numeric_limits::quiet_NaN()), + limiter_linear_(std::numeric_limits::quiet_NaN()) +{ +} const char * DiffDriveController::feedback_type() const { @@ -297,17 +304,78 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( cmd_vel_timeout_ = std::chrono::milliseconds{static_cast(params_.cmd_vel_timeout * 1000.0)}; publish_limited_velocity_ = params_.publish_limited_velocity; + // TODO(christophfroehlich) remove deprecated parameters + // START DEPRECATED + if (!params_.linear.x.has_velocity_limits) + { + RCLCPP_WARN( + logger, + "[deprecated] has_velocity_limits parameter is deprecated, instead set the respective limits " + "to NAN"); + params_.linear.x.min_velocity = params_.linear.x.max_velocity = + std::numeric_limits::quiet_NaN(); + } + if (!params_.linear.x.has_acceleration_limits) + { + RCLCPP_WARN( + logger, + "[deprecated] has_acceleration_limits parameter is deprecated, instead set the respective " + "limits to " + "NAN"); + params_.linear.x.max_deceleration = params_.linear.x.max_acceleration = + params_.linear.x.max_deceleration_reverse = params_.linear.x.max_acceleration_reverse = + std::numeric_limits::quiet_NaN(); + } + if (!params_.linear.x.has_jerk_limits) + { + RCLCPP_WARN( + logger, + "[deprecated] has_jerk_limits parameter is deprecated, instead set the respective limits to " + "NAN"); + params_.linear.x.min_jerk = params_.linear.x.max_jerk = + std::numeric_limits::quiet_NaN(); + } + if (!params_.angular.z.has_velocity_limits) + { + RCLCPP_WARN( + logger, + "[deprecated] has_velocity_limits parameter is deprecated, instead set the respective limits " + "to NAN"); + params_.angular.z.min_velocity = params_.angular.z.max_velocity = + std::numeric_limits::quiet_NaN(); + } + if (!params_.angular.z.has_acceleration_limits) + { + RCLCPP_WARN( + logger, + "[deprecated] has_acceleration_limits parameter is deprecated, instead set the respective " + "limits to " + "NAN"); + params_.angular.z.max_deceleration = params_.angular.z.max_acceleration = + params_.angular.z.max_deceleration_reverse = params_.angular.z.max_acceleration_reverse = + std::numeric_limits::quiet_NaN(); + } + if (!params_.angular.z.has_jerk_limits) + { + RCLCPP_WARN( + logger, + "[deprecated] has_jerk_limits parameter is deprecated, instead set the respective limits to " + "NAN"); + params_.angular.z.min_jerk = params_.angular.z.max_jerk = + std::numeric_limits::quiet_NaN(); + } + // END DEPRECATED limiter_linear_ = SpeedLimiter( - params_.linear.x.has_velocity_limits, params_.linear.x.has_acceleration_limits, - params_.linear.x.has_jerk_limits, params_.linear.x.min_velocity, params_.linear.x.max_velocity, - params_.linear.x.min_acceleration, params_.linear.x.max_acceleration, params_.linear.x.min_jerk, - params_.linear.x.max_jerk); + params_.linear.x.min_velocity, params_.linear.x.max_velocity, + params_.linear.x.max_acceleration_reverse, params_.linear.x.max_acceleration, + params_.linear.x.max_deceleration, params_.linear.x.max_deceleration_reverse, + params_.linear.x.min_jerk, params_.linear.x.max_jerk); limiter_angular_ = SpeedLimiter( - params_.angular.z.has_velocity_limits, params_.angular.z.has_acceleration_limits, - params_.angular.z.has_jerk_limits, params_.angular.z.min_velocity, - params_.angular.z.max_velocity, params_.angular.z.min_acceleration, - params_.angular.z.max_acceleration, params_.angular.z.min_jerk, params_.angular.z.max_jerk); + params_.angular.z.min_velocity, params_.angular.z.max_velocity, + params_.angular.z.max_acceleration_reverse, params_.angular.z.max_acceleration, + params_.angular.z.max_deceleration, params_.angular.z.max_deceleration_reverse, + params_.angular.z.min_jerk, params_.angular.z.max_jerk); if (!reset()) { diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index 755259cc2a..dc1109e043 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -115,75 +115,163 @@ diff_drive_controller: x: has_velocity_limits: { type: bool, - default_value: false, + default_value: true, + description: "deprecated, set the respective limits to ``.NAN`` for deactivation" } has_acceleration_limits: { type: bool, - default_value: false, + default_value: true, + description: "deprecated, set the respective limits to ``.NAN`` for deactivation" } has_jerk_limits: { type: bool, - default_value: false, + default_value: true, + description: "deprecated, set the respective limits to ``.NAN`` for deactivation" } max_velocity: { type: double, default_value: .NAN, + validation: { + "diff_drive_controller::gt_eq_or_nan<>": [0.0] + } } min_velocity: { type: double, default_value: .NAN, + validation: { + "diff_drive_controller::lt_eq_or_nan<>": [0.0] + } } max_acceleration: { type: double, default_value: .NAN, + description: "Maximum acceleration in forward direction.", + validation: { + "diff_drive_controller::gt_eq_or_nan<>": [0.0] + } + } + max_deceleration: { + type: double, + default_value: .NAN, + description: "Maximum deceleration in forward direction.", + validation: { + "diff_drive_controller::lt_eq_or_nan<>": [0.0] + } } min_acceleration: { type: double, default_value: .NAN, + description: "deprecated, use max_deceleration" + } + max_acceleration_reverse: { + type: double, + default_value: .NAN, + description: "Maximum acceleration in reverse direction. If not set, -max_acceleration will be used.", + validation: { + "diff_drive_controller::lt_eq_or_nan<>": [0.0] + } + } + max_deceleration_reverse: { + type: double, + default_value: .NAN, + description: "Maximum deceleration in reverse direction. If not set, -max_deceleration will be used.", + validation: { + "diff_drive_controller::gt_eq_or_nan<>": [0.0] + } } max_jerk: { type: double, default_value: .NAN, + validation: { + "diff_drive_controller::gt_eq_or_nan<>": [0.0] + } } min_jerk: { type: double, default_value: .NAN, + validation: { + "diff_drive_controller::lt_eq_or_nan<>": [0.0] + } } angular: z: has_velocity_limits: { type: bool, - default_value: false, + default_value: true, + description: "deprecated, set the respective limits to ``.NAN`` for deactivation" } has_acceleration_limits: { type: bool, - default_value: false, + default_value: true, + description: "deprecated, set the respective limits to ``.NAN`` for deactivation" } has_jerk_limits: { type: bool, - default_value: false, + default_value: true, + description: "deprecated, set the respective limits to ``.NAN`` for deactivation" } max_velocity: { type: double, default_value: .NAN, + validation: { + "diff_drive_controller::gt_eq_or_nan<>": [0.0] + } } min_velocity: { type: double, default_value: .NAN, + validation: { + "diff_drive_controller::lt_eq_or_nan<>": [0.0] + } } max_acceleration: { type: double, default_value: .NAN, + description: "Maximum acceleration in positive direction.", + validation: { + "diff_drive_controller::gt_eq_or_nan<>": [0.0] + } + } + max_deceleration: { + type: double, + default_value: .NAN, + description: "Maximum deceleration in positive direction.", + validation: { + "diff_drive_controller::lt_eq_or_nan<>": [0.0] + } } min_acceleration: { type: double, default_value: .NAN, + description: "deprecated, use max_deceleration" + } + max_acceleration_reverse: { + type: double, + default_value: .NAN, + description: "Maximum acceleration in negative direction. If not set, -max_acceleration will be used.", + validation: { + "diff_drive_controller::lt_eq_or_nan<>": [0.0] + } + } + max_deceleration_reverse: { + type: double, + default_value: .NAN, + description: "Maximum deceleration in negative direction. If not set, -max_deceleration will be used.", + validation: { + "diff_drive_controller::gt_eq_or_nan<>": [0.0] + } } max_jerk: { type: double, default_value: .NAN, + validation: { + "diff_drive_controller::gt_eq_or_nan<>": [0.0] + } } min_jerk: { type: double, default_value: .NAN, + validation: { + "diff_drive_controller::lt_eq_or_nan<>": [0.0] + } } diff --git a/diff_drive_controller/src/odometry.cpp b/diff_drive_controller/src/odometry.cpp index 99bb22890d..a4a1dbd6ca 100644 --- a/diff_drive_controller/src/odometry.cpp +++ b/diff_drive_controller/src/odometry.cpp @@ -16,6 +16,8 @@ * Author: Enrique Fernández */ +#include + #include "diff_drive_controller/odometry.hpp" namespace diff_drive_controller @@ -134,8 +136,8 @@ void Odometry::integrateRungeKutta2(double linear, double angular) const double direction = heading_ + angular * 0.5; /// Runge-Kutta 2nd order integration: - x_ += linear * cos(direction); - y_ += linear * sin(direction); + x_ += linear * std::cos(direction); + y_ += linear * std::sin(direction); heading_ += angular; } @@ -151,8 +153,8 @@ void Odometry::integrateExact(double linear, double angular) const double heading_old = heading_; const double r = linear / angular; heading_ += angular; - x_ += r * (sin(heading_) - sin(heading_old)); - y_ += -r * (cos(heading_) - cos(heading_old)); + x_ += r * (std::sin(heading_) - std::sin(heading_old)); + y_ += -r * (std::cos(heading_) - std::cos(heading_old)); } } diff --git a/diff_drive_controller/src/speed_limiter.cpp b/diff_drive_controller/src/speed_limiter.cpp deleted file mode 100644 index 0f82c2cc3b..0000000000 --- a/diff_drive_controller/src/speed_limiter.cpp +++ /dev/null @@ -1,139 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// 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. - -/* - * Author: Enrique Fernández - */ - -#include -#include - -#include "diff_drive_controller/speed_limiter.hpp" - -namespace diff_drive_controller -{ -SpeedLimiter::SpeedLimiter( - bool has_velocity_limits, bool has_acceleration_limits, bool has_jerk_limits, double min_velocity, - double max_velocity, double min_acceleration, double max_acceleration, double min_jerk, - double max_jerk) -: has_velocity_limits_(has_velocity_limits), - has_acceleration_limits_(has_acceleration_limits), - has_jerk_limits_(has_jerk_limits), - min_velocity_(min_velocity), - max_velocity_(max_velocity), - min_acceleration_(min_acceleration), - max_acceleration_(max_acceleration), - min_jerk_(min_jerk), - max_jerk_(max_jerk) -{ - // Check if limits are valid, max must be specified, min defaults to -max if unspecified - if (has_velocity_limits_) - { - if (std::isnan(max_velocity_)) - { - throw std::runtime_error("Cannot apply velocity limits if max_velocity is not specified"); - } - if (std::isnan(min_velocity_)) - { - min_velocity_ = -max_velocity_; - } - } - if (has_acceleration_limits_) - { - if (std::isnan(max_acceleration_)) - { - throw std::runtime_error( - "Cannot apply acceleration limits if max_acceleration is not specified"); - } - if (std::isnan(min_acceleration_)) - { - min_acceleration_ = -max_acceleration_; - } - } - if (has_jerk_limits_) - { - if (std::isnan(max_jerk_)) - { - throw std::runtime_error("Cannot apply jerk limits if max_jerk is not specified"); - } - if (std::isnan(min_jerk_)) - { - min_jerk_ = -max_jerk_; - } - } -} - -double SpeedLimiter::limit(double & v, double v0, double v1, double dt) -{ - const double tmp = v; - - limit_jerk(v, v0, v1, dt); - limit_acceleration(v, v0, dt); - limit_velocity(v); - - return tmp != 0.0 ? v / tmp : 1.0; -} - -double SpeedLimiter::limit_velocity(double & v) -{ - const double tmp = v; - - if (has_velocity_limits_) - { - v = std::clamp(v, min_velocity_, max_velocity_); - } - - return tmp != 0.0 ? v / tmp : 1.0; -} - -double SpeedLimiter::limit_acceleration(double & v, double v0, double dt) -{ - const double tmp = v; - - if (has_acceleration_limits_) - { - const double dv_min = min_acceleration_ * dt; - const double dv_max = max_acceleration_ * dt; - - const double dv = std::clamp(v - v0, dv_min, dv_max); - - v = v0 + dv; - } - - return tmp != 0.0 ? v / tmp : 1.0; -} - -double SpeedLimiter::limit_jerk(double & v, double v0, double v1, double dt) -{ - const double tmp = v; - - if (has_jerk_limits_) - { - const double dv = v - v0; - const double dv0 = v0 - v1; - - const double dt2 = 2. * dt * dt; - - const double da_min = min_jerk_ * dt2; - const double da_max = max_jerk_ * dt2; - - const double da = std::clamp(dv - dv0, da_min, da_max); - - v = v0 + dv0 + da; - } - - return tmp != 0.0 ? v / tmp : 1.0; -} - -} // namespace diff_drive_controller diff --git a/diff_drive_controller/test/config/test_diff_drive_controller.yaml b/diff_drive_controller/test/config/test_diff_drive_controller.yaml index 606bacbf4f..7707f2050f 100644 --- a/diff_drive_controller/test/config/test_diff_drive_controller.yaml +++ b/diff_drive_controller/test/config/test_diff_drive_controller.yaml @@ -23,21 +23,20 @@ test_diff_drive_controller: publish_limited_velocity: true velocity_rolling_window_size: 10 - linear.x.has_velocity_limits: false - linear.x.has_acceleration_limits: false - linear.x.has_jerk_limits: false - linear.x.max_velocity: 0.0 - linear.x.min_velocity: 0.0 - linear.x.max_acceleration: 0.0 - linear.x.max_jerk: 0.0 - linear.x.min_jerk: 0.0 + linear.x.max_velocity: .NAN + linear.x.min_velocity: .NAN + linear.x.max_acceleration: .NAN + linear.x.max_deceleration: .NAN + linear.x.max_acceleration_reverse: .NAN + linear.x.max_deceleration_reverse: .NAN + linear.x.max_jerk: .NAN + linear.x.min_jerk: .NAN - angular.z.has_velocity_limits: false - angular.z.has_acceleration_limits: false - angular.z.has_jerk_limits: false - angular.z.max_velocity: 0.0 - angular.z.min_velocity: 0.0 - angular.z.max_acceleration: 0.0 - angular.z.min_acceleration: 0.0 - angular.z.max_jerk: 0.0 - angular.z.min_jerk: 0.0 + angular.z.max_velocity: .NAN + angular.z.min_velocity: .NAN + angular.z.max_acceleration: .NAN + angular.z.max_deceleration: .NAN + angular.z.max_acceleration_reverse: .NAN + angular.z.max_deceleration_reverse: .NAN + angular.z.max_jerk: .NAN + angular.z.min_jerk: .NAN diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index a984c5a5c3..f2fc671920 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -16,7 +16,6 @@ #include #include -#include #include #include @@ -86,10 +85,11 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr class TestDiffDriveController : public ::testing::Test { protected: - static void SetUpTestCase() { rclcpp::init(0, nullptr); } - 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"); @@ -191,7 +191,7 @@ class TestDiffDriveController : public ::testing::Test return controller_->init(controller_name, urdf_, 0, ns, node_options); } - const std::string controller_name = "test_diff_drive_controller"; + std::string controller_name; std::unique_ptr controller_; std::vector position_values_ = {0.1, 0.2}; @@ -449,6 +449,194 @@ TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); } +TEST_F(TestDiffDriveController, test_speed_limiter) +{ + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + { + rclcpp::Parameter("linear.x.max_acceleration", rclcpp::ParameterValue(2.0)), + rclcpp::Parameter("linear.x.max_deceleration", rclcpp::ParameterValue(-4.0)), + rclcpp::Parameter("linear.x.max_acceleration_reverse", rclcpp::ParameterValue(-8.0)), + rclcpp::Parameter("linear.x.max_deceleration_reverse", rclcpp::ParameterValue(10.0)), + }), + controller_interface::return_type::OK); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + auto state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + assignResourcesPosFeedback(); + + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + waitForSetup(); + + // send msg + publish(0.0, 0.0); + // wait for msg is be published to the system + controller_->wait_for_twist(executor); + + // wait for the speed limiter to fill the queue + for (int i = 0; i < 3; ++i) + { + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_NEAR(0.0, left_wheel_vel_cmd_.get_value(), 1e-3); + EXPECT_NEAR(0.0, right_wheel_vel_cmd_.get_value(), 1e-3); + } + + const double dt = 0.001; + const double wheel_radius = 0.1; + + // we send four steps of acceleration, deceleration, reverse acceleration and reverse deceleration + { + const double linear = 1.0; + // send msg + publish(linear, 0.0); + // wait for msg is be published to the system + controller_->wait_for_twist(executor); + + // should be in acceleration limit + const double time_acc = linear / 2.0; + for (int i = 0; i < floor(time_acc / dt) - 1; ++i) + { + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_GT(linear / wheel_radius, left_wheel_vel_cmd_.get_value()) + << "at t: " << i * dt << "s, but should be t: " << time_acc; + EXPECT_GT(linear / wheel_radius, right_wheel_vel_cmd_.get_value()) + << "at t: " << i * dt << "s, but should be t: " << time_acc; + } + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_.get_value(), 1e-3); + EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_.get_value(), 1e-3); + + // wait for the speed limiter to fill the queue + for (int i = 0; i < 3; ++i) + { + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_EQ(linear / wheel_radius, left_wheel_vel_cmd_.get_value()); + EXPECT_EQ(linear / wheel_radius, right_wheel_vel_cmd_.get_value()); + } + } + + { + const double linear = 0.0; + // send msg + publish(linear, 0.0); + // wait for msg is be published to the system + controller_->wait_for_twist(executor); + + // should be in acceleration limit + const double time_acc = -1.0 / (-4.0); + for (int i = 0; i < floor(time_acc / dt) - 1; ++i) + { + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_LT(linear / wheel_radius, left_wheel_vel_cmd_.get_value()) + << "at t: " << i * dt << "s, but should be t: " << time_acc; + EXPECT_LT(linear / wheel_radius, right_wheel_vel_cmd_.get_value()) + << "at t: " << i * dt << "s, but should be t: " << time_acc; + } + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_.get_value(), 1e-3); + EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_.get_value(), 1e-3); + + // wait for the speed limiter to fill the queue + for (int i = 0; i < 3; ++i) + { + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_.get_value(), 1e-3); + EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_.get_value(), 1e-3); + } + } + + { + const double linear = -1.0; + // send msg + publish(linear, 0.0); + // wait for msg is be published to the system + controller_->wait_for_twist(executor); + + // should be in acceleration limit + const double time_acc = -1.0 / (-8.0); + for (int i = 0; i < floor(time_acc / dt) - 1; ++i) + { + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_LT(linear / wheel_radius, left_wheel_vel_cmd_.get_value()) + << "at t: " << i * dt << "s, but should be t: " << time_acc; + EXPECT_LT(linear / wheel_radius, right_wheel_vel_cmd_.get_value()) + << "at t: " << i * dt << "s, but should be t: " << time_acc; + } + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_.get_value(), 1e-3); + EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_.get_value(), 1e-3); + + // wait for the speed limiter to fill the queue + for (int i = 0; i < 3; ++i) + { + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_.get_value(), 1e-3); + EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_.get_value(), 1e-3); + } + } + + { + const double linear = 0.0; + // send msg + publish(linear, 0.0); + // wait for msg is be published to the system + controller_->wait_for_twist(executor); + + // should be in acceleration limit + const double time_acc = 1.0 / (10.0); + for (int i = 0; i < floor(time_acc / dt) - 1; ++i) + { + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_GT(linear / wheel_radius, left_wheel_vel_cmd_.get_value()) + << "at t: " << i * dt << "s, but should be t: " << time_acc; + EXPECT_GT(linear / wheel_radius, right_wheel_vel_cmd_.get_value()) + << "at t: " << i * dt << "s, but should be t: " << time_acc; + } + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_.get_value(), 1e-3); + EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_.get_value(), 1e-3); + + // wait for the speed limiter to fill the queue + for (int i = 0; i < 3; ++i) + { + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_.get_value(), 1e-3); + EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_.get_value(), 1e-3); + } + } +} + TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1) { ASSERT_EQ( @@ -576,3 +764,12 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); executor.cancel(); } + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/doc/migration.rst b/doc/migration.rst index 4c0e4608d6..4f603880bb 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -8,6 +8,7 @@ This list summarizes important changes between Iron (previous) and Jazzy (curren diff_drive_controller ***************************** * The twist message on ``~/cmd_vel`` is now required to be of stamped type (`#812 `_). +* Parameters ``has_velocity_limits``, ``has_acceleration_limits``, and ``has_jerk_limits`` are removed. Instead, set the respective limits to ``.NAN``. (`#1315 `_). joint_trajectory_controller ***************************** diff --git a/doc/release_notes.rst b/doc/release_notes.rst index c7149faf03..0275230fdc 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -13,6 +13,8 @@ diff_drive_controller ***************************** * The twist message on ``~/cmd_vel`` is now required to be of stamped type (`#812 `_). * Remove unused parameter ``wheels_per_side`` (`#958 `_). +* Parameters ``has_velocity_limits``, ``has_acceleration_limits``, and ``has_jerk_limits`` are removed. Instead, set the respective limits to ``.NAN``. (`#1315 `_). +* Parameters ``max_acceleration_reverse`` and ``max_deceleration_reverse`` were added to configure asymmetric acceleration/deceleration behavior. (`#1315 `_). joint_trajectory_controller ***************************** From c9c834379b0115c50b54d2267e28aca756aadb01 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 16 Dec 2024 21:49:39 +0100 Subject: [PATCH 35/84] [CI] Add downstream jobs (#1422) --- .../humble-semi-binary-downstream-build.yml | 49 ++++++++++++++++ .../jazzy-semi-binary-downstream-build.yml | 49 ++++++++++++++++ .../rolling-semi-binary-downstream-build.yml | 57 +++++++++++++++++++ downstream.humble.repos | 5 ++ downstream.jazzy.repos | 5 ++ downstream.rolling.repos | 5 ++ ros_controls.humble.repos | 13 +++++ ros_controls.jazzy.repos | 9 +++ ros_controls.rolling.repos | 9 +++ 9 files changed, 201 insertions(+) create mode 100644 .github/workflows/humble-semi-binary-downstream-build.yml create mode 100644 .github/workflows/jazzy-semi-binary-downstream-build.yml create mode 100644 .github/workflows/rolling-semi-binary-downstream-build.yml create mode 100644 downstream.humble.repos create mode 100644 downstream.jazzy.repos create mode 100644 downstream.rolling.repos create mode 100644 ros_controls.humble.repos create mode 100644 ros_controls.jazzy.repos create mode 100644 ros_controls.rolling.repos diff --git a/.github/workflows/humble-semi-binary-downstream-build.yml b/.github/workflows/humble-semi-binary-downstream-build.yml new file mode 100644 index 0000000000..fc185032e1 --- /dev/null +++ b/.github/workflows/humble-semi-binary-downstream-build.yml @@ -0,0 +1,49 @@ +name: Humble Downstream Build +# description: 'Build & test downstream packages from source.' +# author: Christoph Froehlich + +on: + workflow_dispatch: + pull_request: + branches: + - humble + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/humble-semi-binary-downstream-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros_controls.humble.repos' + +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +jobs: + build-downstream: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: humble + ros_repo: testing + ref_for_scheduled_build: humble + upstream_workspace: ros2_controllers.humble.repos + # we don't test this repository, we just build it + not_test_build: true + # we test the downstream packages, which are part of our organization + downstream_workspace: ros_controls.humble.repos + not_test_downstream: false + build-downstream-3rd-party: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: humble + ros_repo: testing + ref_for_scheduled_build: humble + upstream_workspace: ros2_controllers.humble.repos + # we don't test this repository, we just build it + not_test_build: true + # we don't test the downstream packages, which are outside of our organization + downstream_workspace: downstream.humble.repos + not_test_downstream: true diff --git a/.github/workflows/jazzy-semi-binary-downstream-build.yml b/.github/workflows/jazzy-semi-binary-downstream-build.yml new file mode 100644 index 0000000000..91ec818e91 --- /dev/null +++ b/.github/workflows/jazzy-semi-binary-downstream-build.yml @@ -0,0 +1,49 @@ +name: Jazzy Downstream Build +# description: 'Build & test downstream packages from source.' +# author: Christoph Froehlich + +on: + workflow_dispatch: + pull_request: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/jazzy-semi-binary-downstream-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros_controls.jazzy.repos' + +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +jobs: + build-downstream: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: jazzy + ros_repo: testing + ref_for_scheduled_build: master + upstream_workspace: ros2_controllers.jazzy.repos + # we don't test this repository, we just build it + not_test_build: true + # we test the downstream packages, which are part of our organization + downstream_workspace: ros_controls.jazzy.repos + not_test_downstream: false + build-downstream-3rd-party: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: jazzy + ros_repo: testing + ref_for_scheduled_build: master + upstream_workspace: ros2_controllers.jazzy.repos + # we don't test this repository, we just build it + not_test_build: true + # we don't test the downstream packages, which are outside of our organization + downstream_workspace: downstream.jazzy.repos + not_test_downstream: true diff --git a/.github/workflows/rolling-semi-binary-downstream-build.yml b/.github/workflows/rolling-semi-binary-downstream-build.yml new file mode 100644 index 0000000000..0cf978e528 --- /dev/null +++ b/.github/workflows/rolling-semi-binary-downstream-build.yml @@ -0,0 +1,57 @@ +name: Rolling Downstream Build +# description: 'Build & test downstream packages from source.' +# author: Christoph Froehlich + +on: + workflow_dispatch: + pull_request: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-semi-binary-downstream-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros_controls.rolling.repos' + +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +jobs: + build-downstream: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [rolling] + with: + ros_distro: ${{ matrix.ROS_DISTRO }} + ros_repo: testing + ref_for_scheduled_build: master + upstream_workspace: ros2_controllers.${{ matrix.ROS_DISTRO }}.repos + # we don't test this repository, we just build it + not_test_build: true + # we test the downstream packages, which are part of our organization + downstream_workspace: ros_controls.${{ matrix.ROS_DISTRO }}.repos + not_test_downstream: false + build-downstream-3rd-party: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [rolling] + with: + ros_distro: ${{ matrix.ROS_DISTRO }} + ros_repo: testing + ref_for_scheduled_build: master + upstream_workspace: ros2_controllers.${{ matrix.ROS_DISTRO }}.repos + # we don't test this repository, we just build it + not_test_build: true + # we don't test the downstream packages, which are outside of our organization + downstream_workspace: downstream.${{ matrix.ROS_DISTRO }}.repos + not_test_downstream: true diff --git a/downstream.humble.repos b/downstream.humble.repos new file mode 100644 index 0000000000..81feb3723b --- /dev/null +++ b/downstream.humble.repos @@ -0,0 +1,5 @@ +repositories: + UniversalRobots/Universal_Robots_ROS2_Driver: + type: git + url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver.git + version: humble diff --git a/downstream.jazzy.repos b/downstream.jazzy.repos new file mode 100644 index 0000000000..e5a26574b7 --- /dev/null +++ b/downstream.jazzy.repos @@ -0,0 +1,5 @@ +repositories: + UniversalRobots/Universal_Robots_ROS2_Driver: + type: git + url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver.git + version: main diff --git a/downstream.rolling.repos b/downstream.rolling.repos new file mode 100644 index 0000000000..e5a26574b7 --- /dev/null +++ b/downstream.rolling.repos @@ -0,0 +1,5 @@ +repositories: + UniversalRobots/Universal_Robots_ROS2_Driver: + type: git + url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver.git + version: main diff --git a/ros_controls.humble.repos b/ros_controls.humble.repos new file mode 100644 index 0000000000..f57a077307 --- /dev/null +++ b/ros_controls.humble.repos @@ -0,0 +1,13 @@ +repositories: + ros-controls/gazebo_ros2_control: + type: git + url: https://github.com/ros-controls/gazebo_ros2_control.git + version: humble + ros-controls/gz_ros2_control: + type: git + url: https://github.com/ros-controls/gz_ros2_control.git + version: humble + ros-controls/ros2_control_demos: + type: git + url: https://github.com/ros-controls/ros2_control_demos.git + version: humble diff --git a/ros_controls.jazzy.repos b/ros_controls.jazzy.repos new file mode 100644 index 0000000000..e7cfd385f8 --- /dev/null +++ b/ros_controls.jazzy.repos @@ -0,0 +1,9 @@ +repositories: + ros-controls/gz_ros2_control: + type: git + url: https://github.com/ros-controls/gz_ros2_control.git + version: jazzy + ros-controls/ros2_control_demos: + type: git + url: https://github.com/ros-controls/ros2_control_demos.git + version: master diff --git a/ros_controls.rolling.repos b/ros_controls.rolling.repos new file mode 100644 index 0000000000..da64a510fd --- /dev/null +++ b/ros_controls.rolling.repos @@ -0,0 +1,9 @@ +repositories: + ros-controls/gz_ros2_control: + type: git + url: https://github.com/ros-controls/gz_ros2_control.git + version: rolling + ros-controls/ros2_control_demos: + type: git + url: https://github.com/ros-controls/ros2_control_demos.git + version: master From 337127a1d928055cbcc0a0d7d72e0725c16cf0db Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 17 Dec 2024 07:31:41 +0100 Subject: [PATCH 36/84] Add an error msg if empty message is received (#1424) --- .../src/joint_trajectory_controller.cpp | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 365ce993d0..763d2acd07 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1457,8 +1457,15 @@ bool JointTrajectoryController::validate_trajectory_msg( // This currently supports only position, velocity and acceleration inputs if (params_.allow_integration_in_goal_trajectories) { - const bool all_empty = points[i].positions.empty() && points[i].velocities.empty() && - points[i].accelerations.empty(); + if ( + points[i].positions.empty() && points[i].velocities.empty() && + points[i].accelerations.empty()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "The given trajectory has no position, velocity, or acceleration points."); + return false; + } const bool position_error = !points[i].positions.empty() && !validate_trajectory_point_field(joint_count, points[i].positions, "positions", i, false); @@ -1469,7 +1476,7 @@ bool JointTrajectoryController::validate_trajectory_msg( !points[i].accelerations.empty() && !validate_trajectory_point_field( joint_count, points[i].accelerations, "accelerations", i, false); - if (all_empty || position_error || velocity_error || acceleration_error) + if (position_error || velocity_error || acceleration_error) { return false; } From 0438a1e886386035295da90c3e3dc0436217ddcc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 17 Dec 2024 07:44:10 +0100 Subject: [PATCH 37/84] steering_controllers_library: Add `reduce_wheel_speed_until_steering_reached` parameter (#1314) --- doc/release_notes.rst | 1 + .../steering_odometry.hpp | 5 +- .../src/steering_controllers_library.cpp | 8 +- .../src/steering_controllers_library.yaml | 7 + .../src/steering_odometry.cpp | 26 ++- .../test/test_steering_odometry.cpp | 149 +++++++++++++++++- 6 files changed, 189 insertions(+), 7 deletions(-) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 0275230fdc..74cd0402f1 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -66,6 +66,7 @@ steering_controllers_library * Changing default int values to double in steering controller's yaml file. The controllers should now initialize successfully without specifying these parameters (`#927 `_). * A fix for Ackermann steering odometry was added (`#921 `_). * Do not reset the steering wheels to ``0.0`` on timeout (`#1289 `_). +* New parameter ``reduce_wheel_speed_until_steering_reached`` was added. If set to true, then the wheel speed(s) is reduced until the steering angle has been reached. Only considered if ``open_loop = false`` (`#1314 `_). tricycle_controller ************************ diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp index 5b67797b79..ddf9fcdec8 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -192,10 +192,13 @@ class SteeringOdometry * \param v_bx Desired linear velocity of the robot in x_b-axis direction * \param omega_bz Desired angular velocity of the robot around x_z-axis * \param open_loop If false, the IK will be calculated using measured steering angle + * \param reduce_wheel_speed_until_steering_reached Reduce wheel speed until the steering angle + * has been reached * \return Tuple of velocity commands and steering commands */ std::tuple, std::vector> get_commands( - const double v_bx, const double omega_bz, const bool open_loop = true); + const double v_bx, const double omega_bz, const bool open_loop = true, + const bool reduce_wheel_speed_until_steering_reached = false); /** * \brief Reset poses, heading, and accumulators diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 3c14013f40..3d4164080c 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -353,7 +353,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_deactivate( } controller_interface::return_type SteeringControllersLibrary::update_reference_from_subscribers( - const rclcpp::Time & time, const rclcpp::Duration & /*period*/) + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { auto current_ref = *(input_ref_.readFromRT()); @@ -386,8 +386,10 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c const auto timeout = age_of_last_command > ref_timeout_ && ref_timeout_ != rclcpp::Duration::from_seconds(0); - auto [traction_commands, steering_commands] = - odometry_.get_commands(last_linear_velocity_, last_angular_velocity_, params_.open_loop); + auto [traction_commands, steering_commands] = odometry_.get_commands( + last_linear_velocity_, last_angular_velocity_, params_.open_loop, + params_.reduce_wheel_speed_until_steering_reached); + if (params_.front_steering) { for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) diff --git a/steering_controllers_library/src/steering_controllers_library.yaml b/steering_controllers_library/src/steering_controllers_library.yaml index 711a780458..4e7deef698 100644 --- a/steering_controllers_library/src/steering_controllers_library.yaml +++ b/steering_controllers_library/src/steering_controllers_library.yaml @@ -63,6 +63,13 @@ steering_controllers_library: read_only: false, } + reduce_wheel_speed_until_steering_reached: { + type: bool, + default_value: false, + description: "Reduce wheel speed until the steering angle has been reached.", + read_only: false, + } + velocity_rolling_window_size: { type: int, default_value: 10, diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 8b8bb8bfd8..b2bf100255 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -217,7 +217,8 @@ double SteeringOdometry::convert_twist_to_steering_angle(double v_bx, double ome } std::tuple, std::vector> SteeringOdometry::get_commands( - const double v_bx, const double omega_bz, const bool open_loop) + const double v_bx, const double omega_bz, const bool open_loop, + const bool reduce_wheel_speed_until_steering_reached) { // desired wheel speed and steering angle of the middle of traction and steering axis double Ws, phi, phi_IK = steer_pos_; @@ -244,6 +245,29 @@ std::tuple, std::vector> SteeringOdometry::get_comma // wheel speed Ws = v_bx / wheel_radius_; + if (!open_loop && reduce_wheel_speed_until_steering_reached) + { + // Reduce wheel speed until the target angle has been reached + double phi_delta = abs(steer_pos_ - phi); + double scale; + const double min_phi_delta = M_PI / 6.; + if (phi_delta < min_phi_delta) + { + scale = 1; + } + else if (phi_delta >= 1.5608) + { + // cos(1.5608) = 0.01 + scale = 0.01 / cos(min_phi_delta); + } + else + { + // TODO(anyone): find the best function, e.g convex power functions + scale = cos(phi_delta) / cos(min_phi_delta); + } + Ws *= scale; + } + if (config_type_ == BICYCLE_CONFIG) { std::vector traction_commands = {Ws}; diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp index e3c8db6c15..3d6d97f15f 100644 --- a/steering_controllers_library/test/test_steering_odometry.cpp +++ b/steering_controllers_library/test/test_steering_odometry.cpp @@ -116,13 +116,59 @@ TEST(TestSteeringOdometry, ackermann_IK_right) odom.update_from_position(0., -0.2, 1.); // assume already turn auto cmd = odom.get_commands(1., -0.1, false); auto cmd0 = std::get<0>(cmd); // vel - EXPECT_LT(cmd0[0], cmd0[1]); // right (inner) < left outer) + EXPECT_LT(cmd0[0], cmd0[1]); // right (inner) < left (outer) EXPECT_GT(cmd0[0], 0); auto cmd1 = std::get<1>(cmd); // steer EXPECT_GT(std::abs(cmd1[0]), std::abs(cmd1[1])); // abs right (inner) > abs left (outer) EXPECT_LT(cmd1[0], 0); } +TEST(TestSteeringOdometry, ackermann_IK_right_steering_limited) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + + { + odom.update_from_position(0., -0.785, 1.); // already steered + auto cmd = odom.get_commands(1., -0.5, false, true); + auto vel_cmd_steered = std::get<0>(cmd); // vel + EXPECT_LT(vel_cmd_steered[0], vel_cmd_steered[1]); // right (inner) < left (outer) + EXPECT_GT(vel_cmd_steered[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_GT(std::abs(cmd1[0]), std::abs(cmd1[1])); // abs right (inner) > abs left (outer) + EXPECT_LT(cmd1[0], 0); + } + + std::vector vel_cmd_not_steered; + { + odom.update_from_position(0., -0.1, 1.); // not fully steered + auto cmd = odom.get_commands(1., -0.5, false, false); + vel_cmd_not_steered = std::get<0>(cmd); // vel + EXPECT_LT(vel_cmd_not_steered[0], vel_cmd_not_steered[1]); // right (inner) < left (outer) + EXPECT_GT(vel_cmd_not_steered[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_GT(std::abs(cmd1[0]), std::abs(cmd1[1])); // abs right (inner) > abs left (outer) + EXPECT_LT(cmd1[0], 0); + } + + { + odom.update_from_position(0., -0.1, 1.); // not fully steered + auto cmd = odom.get_commands(1., -0.5, false, true); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_LT(cmd0[0], cmd0[1]); // right (inner) < left (outer) + EXPECT_GT(cmd0[0], 0); + // vel should be less than vel_cmd_not_steered now + for (size_t i = 0; i < cmd0.size(); ++i) + { + EXPECT_LT(cmd0[i], vel_cmd_not_steered[i]); + } + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_GT(std::abs(cmd1[0]), std::abs(cmd1[1])); // abs right (inner) > abs left (outer) + EXPECT_LT(cmd1[0], 0); + } +} + // ----------------- bicycle ----------------- TEST(TestSteeringOdometry, bicycle_IK_linear) @@ -164,6 +210,62 @@ TEST(TestSteeringOdometry, bicycle_IK_right) EXPECT_LT(cmd1[0], 0); // left steering } +TEST(TestSteeringOdometry, bicycle_IK_right_steering_limited) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + + { + odom.update_from_position(0., -0.785, 1.); // already steered + auto cmd = odom.get_commands(1., -0.5, false, true); + auto vel_cmd_steered = std::get<0>(cmd); // vel + EXPECT_DOUBLE_EQ(vel_cmd_steered[0], 1.0); // equals linear + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], 0); + } + + std::vector vel_cmd_not_steered; + { + odom.update_from_position(0., -0.1, 1.); // not fully steered + auto cmd = odom.get_commands(1., -0.5, false, false); + vel_cmd_not_steered = std::get<0>(cmd); // vel + EXPECT_DOUBLE_EQ(vel_cmd_not_steered[0], 1.0); // equals linear + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], 0); + } + + std::vector vel_cmd_not_steered_limited; + { + odom.update_from_position(0., -0.1, 1.); // not fully steered + auto cmd = odom.get_commands(1., -0.5, false, true); + vel_cmd_not_steered_limited = std::get<0>(cmd); // vel + EXPECT_GT(vel_cmd_not_steered_limited[0], 0); + // vel should be less than vel_cmd_not_steered now + for (size_t i = 0; i < vel_cmd_not_steered_limited.size(); ++i) + { + EXPECT_LT(vel_cmd_not_steered_limited[i], vel_cmd_not_steered[i]); + } + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], 0); + } + + { + // larger error -> check min of scale + odom.update_from_position(0., M_PI, 1.); // not fully steered + auto cmd = odom.get_commands(1., -0.5, false, true); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_GT(cmd0[0], 0); + // vel should be less than vel_cmd_not_steered_limited now + for (size_t i = 0; i < cmd0.size(); ++i) + { + EXPECT_LT(cmd0[i], vel_cmd_not_steered_limited[i]); + } + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], 0); + } +} + TEST(TestSteeringOdometry, bicycle_odometry) { steering_odometry::SteeringOdometry odom(1); @@ -214,12 +316,55 @@ TEST(TestSteeringOdometry, tricycle_IK_right) odom.update_from_position(0., -0.2, 1.); // assume already turn auto cmd = odom.get_commands(1., -0.1, false); auto cmd0 = std::get<0>(cmd); // vel - EXPECT_LT(cmd0[0], cmd0[1]); // right (inner) < left outer) + EXPECT_LT(cmd0[0], cmd0[1]); // right (inner) < left (outer) EXPECT_GT(cmd0[0], 0); auto cmd1 = std::get<1>(cmd); // steer EXPECT_LT(cmd1[0], 0); // right steering } +TEST(TestSteeringOdometry, tricycle_IK_right_steering_limited) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + + { + odom.update_from_position(0., -0.785, 1.); // already steered + auto cmd = odom.get_commands(1., -0.5, false, true); + auto vel_cmd_steered = std::get<0>(cmd); // vel + EXPECT_LT(vel_cmd_steered[0], vel_cmd_steered[1]); // right (inner) < left (outer) + EXPECT_GT(vel_cmd_steered[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], 0); + } + + std::vector vel_cmd_not_steered; + { + odom.update_from_position(0., -0.1, 1.); // not fully steered + auto cmd = odom.get_commands(1., -0.5, false, false); + vel_cmd_not_steered = std::get<0>(cmd); // vel + EXPECT_LT(vel_cmd_not_steered[0], vel_cmd_not_steered[1]); // right (inner) < left (outer) + EXPECT_GT(vel_cmd_not_steered[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], 0); + } + + { + odom.update_from_position(0., -0.1, 1.); // not fully steered + auto cmd = odom.get_commands(1., -0.5, false, true); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_LT(cmd0[0], cmd0[1]); // right (inner) < left (outer) + EXPECT_GT(cmd0[0], 0); + // vel should be less than vel_cmd_not_steered now + for (size_t i = 0; i < cmd0.size(); ++i) + { + EXPECT_LT(cmd0[i], vel_cmd_not_steered[i]); + } + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], 0); + } +} + TEST(TestSteeringOdometry, tricycle_odometry) { steering_odometry::SteeringOdometry odom(1); From a09e7af552c873ce29940b10167af99568252fb2 Mon Sep 17 00:00:00 2001 From: "Jakub \"Deli\" Delicat" Date: Wed, 18 Dec 2024 12:54:24 +0100 Subject: [PATCH 38/84] Update position controller package.xml (#1431) --- position_controllers/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 56fbf02b64..a7e46344bb 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -2,7 +2,7 @@ position_controllers 4.17.0 - Generic controller for forwarding commands. + Generic position controller for forwarding position commands. Bence Magyar Denis Štogl From a22c99a503fc9181cdef4000a43207de404ec91e Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 19 Dec 2024 08:38:59 +0000 Subject: [PATCH 39/84] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 3 +++ admittance_controller/CHANGELOG.rst | 6 ++++++ bicycle_steering_controller/CHANGELOG.rst | 3 +++ diff_drive_controller/CHANGELOG.rst | 7 +++++++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 3 +++ forward_command_controller/CHANGELOG.rst | 5 +++++ gpio_controllers/CHANGELOG.rst | 3 +++ gripper_controllers/CHANGELOG.rst | 3 +++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 5 +++++ joint_trajectory_controller/CHANGELOG.rst | 6 ++++++ mecanum_drive_controller/CHANGELOG.rst | 3 +++ parallel_gripper_controller/CHANGELOG.rst | 3 +++ pid_controller/CHANGELOG.rst | 5 +++++ pose_broadcaster/CHANGELOG.rst | 3 +++ position_controllers/CHANGELOG.rst | 5 +++++ range_sensor_broadcaster/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 5 +++++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ steering_controllers_library/CHANGELOG.rst | 6 ++++++ tricycle_controller/CHANGELOG.rst | 5 +++++ tricycle_steering_controller/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 3 +++ 25 files changed, 100 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index fe06a8aaa2..3a7bd23326 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.17.0 (2024-12-07) ------------------- * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 900bb68242..c34ea4f91b 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [CI] Add clang job and setup concurrency (`#1407 `_) +* Add wrench offset for admittance controller (`#1249 `_) +* Contributors: Christoph Fröhlich, Lennart Nachtigall + 4.17.0 (2024-12-07) ------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 37a975fd0d..39e459fcf0 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.17.0 (2024-12-07) ------------------- * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 5789720d29..cba27f35e7 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update command limiter of diff_drive_controller (`#1315 `_) +* Improve tf_prefix based on namespace (`#1420 `_) +* [CI] Add clang job and setup concurrency (`#1407 `_) +* Contributors: Christoph Fröhlich, Rafal Gorecki + 4.17.0 (2024-12-07) ------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 6ecf9eefc0..20f8bab886 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.17.0 (2024-12-07) ------------------- * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 765aad14ee..4407feadd7 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.17.0 (2024-12-07) ------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 668700a056..763ea574e8 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [CI] Add clang job and setup concurrency (`#1407 `_) +* Contributors: Christoph Fröhlich + 4.17.0 (2024-12-07) ------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) diff --git a/gpio_controllers/CHANGELOG.rst b/gpio_controllers/CHANGELOG.rst index 7af5d2bd31..ccd5ee7349 100644 --- a/gpio_controllers/CHANGELOG.rst +++ b/gpio_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gpio_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.17.0 (2024-12-07) ------------------- * Add missing dependency to gpio_controllers (`#1410 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index f2f2fcb4a9..549da12a7c 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.17.0 (2024-12-07) ------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index cc97a2d28d..3fe1bf2d60 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.17.0 (2024-12-07) ------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index d9a94bd335..2f5f32dee1 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [CI] Add clang job and setup concurrency (`#1407 `_) +* Contributors: Christoph Fröhlich + 4.17.0 (2024-12-07) ------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 9c609ed561..b00fd217a3 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add an error msg if empty message is received (`#1424 `_) +* [CI] Add clang job and setup concurrency (`#1407 `_) +* Contributors: Christoph Fröhlich + 4.17.0 (2024-12-07) ------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) diff --git a/mecanum_drive_controller/CHANGELOG.rst b/mecanum_drive_controller/CHANGELOG.rst index 22b7afb932..425a3dea33 100644 --- a/mecanum_drive_controller/CHANGELOG.rst +++ b/mecanum_drive_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package mecanum_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.17.0 (2024-12-07) ------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst index b085543641..9ceb05320b 100644 --- a/parallel_gripper_controller/CHANGELOG.rst +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package parallel_gripper_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.17.0 (2024-12-07) ------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 3ef0927df3..169da431c7 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [CI] Add clang job and setup concurrency (`#1407 `_) +* Contributors: Christoph Fröhlich + 4.17.0 (2024-12-07) ------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) diff --git a/pose_broadcaster/CHANGELOG.rst b/pose_broadcaster/CHANGELOG.rst index 3abf140f40..f46fc61ec2 100644 --- a/pose_broadcaster/CHANGELOG.rst +++ b/pose_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package pose_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.17.0 (2024-12-07) ------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 3a843e74bd..22324faed1 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update position controller package.xml (`#1431 `_) +* Contributors: Jakub "Deli" Delicat + 4.17.0 (2024-12-07) ------------------- * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 716fbbbaf6..053705704e 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.17.0 (2024-12-07) ------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index b59b7f93a2..b1295fb63d 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing plugins to ros2_controllers dependencies (`#1413 `_) +* Contributors: Sai Kishor Kothakota + 4.17.0 (2024-12-07) ------------------- * Add Mecanum Drive Controller (`#512 `_) diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 365994cede..2c3a2e5567 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.17.0 (2024-12-07) ------------------- * Don't call shutdown() after an exception (`#1400 `_) diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 6d8fd768cc..dcfb075ceb 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.17.0 (2024-12-07) ------------------- * Update maintainers and add url tags (`#1363 `_) diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 962a44e99a..fb3da4d37c 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* steering_controllers_library: Add `reduce_wheel_speed_until_steering_reached` parameter (`#1314 `_) +* [CI] Add clang job and setup concurrency (`#1407 `_) +* Contributors: Christoph Fröhlich + 4.17.0 (2024-12-07) ------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 1e10cc42b3..0dd75bcf36 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [CI] Add clang job and setup concurrency (`#1407 `_) +* Contributors: Christoph Fröhlich + 4.17.0 (2024-12-07) ------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 92a416eb68..6ed6c4e163 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.17.0 (2024-12-07) ------------------- * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 975e8a2afd..7664d602fa 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.17.0 (2024-12-07) ------------------- * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) From eaeefdfbf18a73bcc1dac62141e3da3b009858df Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 19 Dec 2024 08:40:14 +0000 Subject: [PATCH 40/84] 4.18.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gpio_controllers/CHANGELOG.rst | 4 ++-- gpio_controllers/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- mecanum_drive_controller/CHANGELOG.rst | 4 ++-- mecanum_drive_controller/package.xml | 2 +- parallel_gripper_controller/CHANGELOG.rst | 4 ++-- parallel_gripper_controller/package.xml | 2 +- pid_controller/CHANGELOG.rst | 4 ++-- pid_controller/package.xml | 2 +- pose_broadcaster/CHANGELOG.rst | 4 ++-- pose_broadcaster/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 52 files changed, 77 insertions(+), 77 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 3a7bd23326..b8f12995aa 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-12-19) +------------------- 4.17.0 (2024-12-07) ------------------- diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index dbb466cead..21e9158a60 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.17.0 + 4.18.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Bence Magyar diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index c34ea4f91b..7e0e2d00c3 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-12-19) +------------------- * [CI] Add clang job and setup concurrency (`#1407 `_) * Add wrench offset for admittance controller (`#1249 `_) * Contributors: Christoph Fröhlich, Lennart Nachtigall diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index fbd0dbc8dd..fedc56426f 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.17.0 + 4.18.0 Implementation of admittance controllers for different input and output interface. Bence Magyar diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 39e459fcf0..86833e9ca0 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-12-19) +------------------- 4.17.0 (2024-12-07) ------------------- diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 186818c71a..42e0a0e137 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.17.0 + 4.18.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index cba27f35e7..eb32050dda 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-12-19) +------------------- * Update command limiter of diff_drive_controller (`#1315 `_) * Improve tf_prefix based on namespace (`#1420 `_) * [CI] Add clang job and setup concurrency (`#1407 `_) diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 1ed9673c79..1579e1b31f 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.17.0 + 4.18.0 Controller for a differential-drive mobile base. Bence Magyar diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 20f8bab886..76125e7a08 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-12-19) +------------------- 4.17.0 (2024-12-07) ------------------- diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 28943d2984..51a8e869aa 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.17.0 + 4.18.0 Generic controller for forwarding commands. Bence Magyar diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 4407feadd7..33c741ad3f 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-12-19) +------------------- 4.17.0 (2024-12-07) ------------------- diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 446376ebb7..17a794b200 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.17.0 + 4.18.0 Controller to publish state of force-torque sensors. Bence Magyar diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 763ea574e8..b72327a18f 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-12-19) +------------------- * [CI] Add clang job and setup concurrency (`#1407 `_) * Contributors: Christoph Fröhlich diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index a332d5525e..e6e666c5f4 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.17.0 + 4.18.0 Generic controller for forwarding commands. Bence Magyar diff --git a/gpio_controllers/CHANGELOG.rst b/gpio_controllers/CHANGELOG.rst index ccd5ee7349..0bd9254a3c 100644 --- a/gpio_controllers/CHANGELOG.rst +++ b/gpio_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gpio_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-12-19) +------------------- 4.17.0 (2024-12-07) ------------------- diff --git a/gpio_controllers/package.xml b/gpio_controllers/package.xml index a0560389aa..108e951ffa 100644 --- a/gpio_controllers/package.xml +++ b/gpio_controllers/package.xml @@ -2,7 +2,7 @@ gpio_controllers - 4.17.0 + 4.18.0 Controllers to interact with gpios. Bence Magyar diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 549da12a7c..f2d262c30d 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-12-19) +------------------- 4.17.0 (2024-12-07) ------------------- diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 0de8e1b14d..3dc9ab83c0 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 4.17.0 + 4.18.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 3fe1bf2d60..1d2379a9e5 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-12-19) +------------------- 4.17.0 (2024-12-07) ------------------- diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 905f6c9c97..2bce9aa92d 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.17.0 + 4.18.0 Controller to publish readings of IMU sensors. Bence Magyar diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 2f5f32dee1..d1cfe32fe5 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-12-19) +------------------- * [CI] Add clang job and setup concurrency (`#1407 `_) * Contributors: Christoph Fröhlich diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 61915ab2b0..fe40531c15 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.17.0 + 4.18.0 Broadcaster to publish joint state Bence Magyar diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index b00fd217a3..e4f26fa2f0 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-12-19) +------------------- * Add an error msg if empty message is received (`#1424 `_) * [CI] Add clang job and setup concurrency (`#1407 `_) * Contributors: Christoph Fröhlich diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 886d37b97a..9acd8bfdcd 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.17.0 + 4.18.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar diff --git a/mecanum_drive_controller/CHANGELOG.rst b/mecanum_drive_controller/CHANGELOG.rst index 425a3dea33..f45c653f27 100644 --- a/mecanum_drive_controller/CHANGELOG.rst +++ b/mecanum_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package mecanum_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-12-19) +------------------- 4.17.0 (2024-12-07) ------------------- diff --git a/mecanum_drive_controller/package.xml b/mecanum_drive_controller/package.xml index 149601e2d8..076d215529 100644 --- a/mecanum_drive_controller/package.xml +++ b/mecanum_drive_controller/package.xml @@ -2,7 +2,7 @@ mecanum_drive_controller - 4.17.0 + 4.18.0 Implementation of mecanum drive controller for 4 wheel drive. diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst index 9ceb05320b..8c889e82b2 100644 --- a/parallel_gripper_controller/CHANGELOG.rst +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package parallel_gripper_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-12-19) +------------------- 4.17.0 (2024-12-07) ------------------- diff --git a/parallel_gripper_controller/package.xml b/parallel_gripper_controller/package.xml index 219ce0b15a..4db99f7537 100644 --- a/parallel_gripper_controller/package.xml +++ b/parallel_gripper_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> parallel_gripper_controller - 4.17.0 + 4.18.0 The parallel_gripper_controller package Bence Magyar diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 169da431c7..d2bdaf679d 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-12-19) +------------------- * [CI] Add clang job and setup concurrency (`#1407 `_) * Contributors: Christoph Fröhlich diff --git a/pid_controller/package.xml b/pid_controller/package.xml index b8912e24bf..a422e2254d 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.17.0 + 4.18.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar diff --git a/pose_broadcaster/CHANGELOG.rst b/pose_broadcaster/CHANGELOG.rst index f46fc61ec2..43fd04c3b3 100644 --- a/pose_broadcaster/CHANGELOG.rst +++ b/pose_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pose_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-12-19) +------------------- 4.17.0 (2024-12-07) ------------------- diff --git a/pose_broadcaster/package.xml b/pose_broadcaster/package.xml index 3802a7495e..02581f5aa2 100644 --- a/pose_broadcaster/package.xml +++ b/pose_broadcaster/package.xml @@ -2,7 +2,7 @@ pose_broadcaster - 4.17.0 + 4.18.0 Broadcaster to publish cartesian states. Bence Magyar diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 22324faed1..5d939991a0 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-12-19) +------------------- * Update position controller package.xml (`#1431 `_) * Contributors: Jakub "Deli" Delicat diff --git a/position_controllers/package.xml b/position_controllers/package.xml index a7e46344bb..12873809bf 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.17.0 + 4.18.0 Generic position controller for forwarding position commands. Bence Magyar diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 053705704e..c6be8e44ac 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-12-19) +------------------- 4.17.0 (2024-12-07) ------------------- diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index a368683dd2..3cf633e87a 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.17.0 + 4.18.0 Controller to publish readings of range sensors. Bence Magyar diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index b1295fb63d..19d8e12c68 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-12-19) +------------------- * Add missing plugins to ros2_controllers dependencies (`#1413 `_) * Contributors: Sai Kishor Kothakota diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 294d851e40..491c88f395 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.17.0 + 4.18.0 Metapackage for ros2_controllers related packages Bence Magyar diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 2c3a2e5567..fa20e80af1 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-12-19) +------------------- 4.17.0 (2024-12-07) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 12164d8e3a..ab5d69e2c3 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.17.0 + 4.18.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Bence Magyar diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index cb7332f67e..ca37c49797 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.17.0", + version="4.18.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index dcfb075ceb..1210b917ef 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-12-19) +------------------- 4.17.0 (2024-12-07) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 570e0a707e..34cc0f82ae 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 4.17.0 + 4.18.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index 01ae56e876..a8c9ed1503 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -21,7 +21,7 @@ setup( name=package_name, - version="4.17.0", + version="4.18.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index fb3da4d37c..cd91a8b8f9 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-12-19) +------------------- * steering_controllers_library: Add `reduce_wheel_speed_until_steering_reached` parameter (`#1314 `_) * [CI] Add clang job and setup concurrency (`#1407 `_) * Contributors: Christoph Fröhlich diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index ddeabdf96c..c48d22cb05 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.17.0 + 4.18.0 Package for steering robot configurations including odometry and interfaces. Bence Magyar diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 0dd75bcf36..45d8531c29 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-12-19) +------------------- * [CI] Add clang job and setup concurrency (`#1407 `_) * Contributors: Christoph Fröhlich diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index d365725518..ce5fcfce6a 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.17.0 + 4.18.0 Controller for a tricycle drive mobile base Bence Magyar diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 6ed6c4e163..18fef2ec79 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-12-19) +------------------- 4.17.0 (2024-12-07) ------------------- diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index c49822629b..1cb79f5af2 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.17.0 + 4.18.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Bence Magyar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 7664d602fa..02440b920a 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-12-19) +------------------- 4.17.0 (2024-12-07) ------------------- diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 362be2fe27..0b27796fc4 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.17.0 + 4.18.0 Generic controller for forwarding commands. Bence Magyar From 81c0d41e4feebfa7e7c81b01d0ac1101eaf10e5d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 21 Dec 2024 11:36:15 +0100 Subject: [PATCH 41/84] Fix label of good first issue template (#1438) --- .github/ISSUE_TEMPLATE/good-first-issue.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/ISSUE_TEMPLATE/good-first-issue.md b/.github/ISSUE_TEMPLATE/good-first-issue.md index 4de9ad8d30..5e80e595d3 100644 --- a/.github/ISSUE_TEMPLATE/good-first-issue.md +++ b/.github/ISSUE_TEMPLATE/good-first-issue.md @@ -2,7 +2,7 @@ name: Good first issue about: Create an issue to welcome a new contributor into the community. title: '' -labels: good-first-issue +labels: ["good first issue"] assignees: '' --- From 765b371d56d8a918cd78dca2453bed189ddf3bb2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 25 Dec 2024 11:07:56 +0100 Subject: [PATCH 42/84] Fix ref for scheduled build (#1445) --- .github/workflows/jazzy-semi-binary-build.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/jazzy-semi-binary-build.yml b/.github/workflows/jazzy-semi-binary-build.yml index e38fd5b7ba..f6585f6a02 100644 --- a/.github/workflows/jazzy-semi-binary-build.yml +++ b/.github/workflows/jazzy-semi-binary-build.yml @@ -54,7 +54,7 @@ jobs: ros_distro: jazzy ros_repo: testing upstream_workspace: ros2_controllers.jazzy.repos - ref_for_scheduled_build: jazzy + ref_for_scheduled_build: master additional_debs: clang c_compiler: clang cxx_compiler: clang++ From f8bf4ed84cdc52ece855d2aafd60b11ab3722eac Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 25 Dec 2024 17:46:35 +0100 Subject: [PATCH 43/84] Fix ref for scheduled build (#1447) --- .github/workflows/rolling-semi-binary-build.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/rolling-semi-binary-build.yml b/.github/workflows/rolling-semi-binary-build.yml index 06f4c55612..ecb462bad0 100644 --- a/.github/workflows/rolling-semi-binary-build.yml +++ b/.github/workflows/rolling-semi-binary-build.yml @@ -54,7 +54,7 @@ jobs: ros_distro: rolling ros_repo: testing upstream_workspace: ros2_controllers.rolling.repos - ref_for_scheduled_build: rolling + ref_for_scheduled_build: master additional_debs: clang c_compiler: clang cxx_compiler: clang++ From f23b7d833a41bff6e2125ff5243e0ed95133e6d8 Mon Sep 17 00:00:00 2001 From: Shankar-Balajee Date: Fri, 27 Dec 2024 00:46:21 +0530 Subject: [PATCH 44/84] Clean up unused variable EPS in mecanum_drive_controller (#1444) --- .../test/test_mecanum_drive_controller.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 5327e890e9..bbddcec06a 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -31,12 +31,6 @@ class MecanumDriveControllerTest { }; -namespace -{ -// Floating-point value comparison threshold -const double EPS = 1e-3; -} // namespace - TEST_F(MecanumDriveControllerTest, when_controller_is_configured_expect_all_parameters_set) { SetUpController(); From a1d8da49325900e1901406e28a1b2a0e271ef602 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 30 Dec 2024 13:33:28 +0000 Subject: [PATCH 45/84] Remove visibility macros (#1451) --- ackermann_steering_controller/CMakeLists.txt | 10 ++-- .../ackermann_steering_controller.hpp | 10 +--- .../visibility_control.h | 52 ----------------- admittance_controller/CMakeLists.txt | 10 ++-- .../admittance_controller.hpp | 10 ---- .../visibility_control.h | 49 ---------------- bicycle_steering_controller/CMakeLists.txt | 10 ++-- .../bicycle_steering_controller.hpp | 10 +--- .../visibility_control.h | 52 ----------------- diff_drive_controller/CMakeLists.txt | 9 +-- .../diff_drive_controller.hpp | 12 ---- .../visibility_control.h | 56 ------------------- effort_controllers/CMakeLists.txt | 9 +-- .../joint_group_effort_controller.hpp | 4 -- .../effort_controllers/visibility_control.h | 56 ------------------- .../CMakeLists.txt | 10 ++-- .../force_torque_sensor_broadcaster.hpp | 13 +---- .../visibility_control.h | 53 ------------------ forward_command_controller/CMakeLists.txt | 10 ++-- .../forward_command_controller.hpp | 2 - .../forward_controllers_base.hpp | 10 ---- ...i_interface_forward_command_controller.hpp | 2 - .../visibility_control.h | 56 ------------------- gpio_controllers/CMakeLists.txt | 13 ++--- .../gpio_command_controller.hpp | 9 --- .../gpio_controllers/visibility_control.h | 49 ---------------- gripper_controllers/CMakeLists.txt | 6 +- .../gripper_action_controller.hpp | 11 +--- .../visibility_control.hpp | 56 ------------------- imu_sensor_broadcaster/CMakeLists.txt | 10 ++-- .../imu_sensor_broadcaster.hpp | 10 +--- .../visibility_control.h | 53 ------------------ joint_state_broadcaster/CMakeLists.txt | 9 +-- .../joint_state_broadcaster.hpp | 9 --- .../visibility_control.h | 56 ------------------- joint_trajectory_controller/CMakeLists.txt | 9 +-- .../joint_trajectory_controller.hpp | 28 ---------- .../trajectory.hpp | 16 ------ .../visibility_control.h | 56 ------------------- mecanum_drive_controller/CMakeLists.txt | 10 ++-- .../mecanum_drive_controller.hpp | 11 ---- .../visibility_control.h | 49 ---------------- parallel_gripper_controller/CMakeLists.txt | 2 +- .../parallel_gripper_action_controller.hpp | 11 +--- .../visibility_control.hpp | 56 ------------------- pid_controller/CMakeLists.txt | 10 ++-- .../include/pid_controller/pid_controller.hpp | 12 ---- .../pid_controller/visibility_control.h | 49 ---------------- pose_broadcaster/CMakeLists.txt | 13 ++--- .../pose_broadcaster/pose_broadcaster.hpp | 17 +++--- .../pose_broadcaster/visibility_control.h | 49 ---------------- position_controllers/CMakeLists.txt | 9 +-- .../joint_group_position_controller.hpp | 4 +- .../position_controllers/visibility_control.h | 56 ------------------- range_sensor_broadcaster/CMakeLists.txt | 7 ++- .../range_sensor_broadcaster.hpp | 10 +--- .../visibility_control.h | 53 ------------------ steering_controllers_library/CMakeLists.txt | 10 ++-- .../steering_controllers_library.hpp | 36 +++++------- .../visibility_control.h | 50 ----------------- tricycle_controller/CMakeLists.txt | 9 +-- .../tricycle_controller.hpp | 12 ---- .../tricycle_controller/visibility_control.h | 53 ------------------ tricycle_steering_controller/CMakeLists.txt | 10 ++-- .../tricycle_steering_controller.hpp | 10 +--- .../visibility_control.h | 52 ----------------- velocity_controllers/CMakeLists.txt | 10 ++-- .../joint_group_velocity_controller.hpp | 5 +- .../velocity_controllers/visibility_control.h | 56 ------------------- 69 files changed, 137 insertions(+), 1519 deletions(-) delete mode 100644 ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h delete mode 100644 admittance_controller/include/admittance_controller/visibility_control.h delete mode 100644 bicycle_steering_controller/include/bicycle_steering_controller/visibility_control.h delete mode 100644 diff_drive_controller/include/diff_drive_controller/visibility_control.h delete mode 100644 effort_controllers/include/effort_controllers/visibility_control.h delete mode 100644 force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/visibility_control.h delete mode 100644 forward_command_controller/include/forward_command_controller/visibility_control.h delete mode 100644 gpio_controllers/include/gpio_controllers/visibility_control.h delete mode 100644 gripper_controllers/include/gripper_controllers/visibility_control.hpp delete mode 100644 imu_sensor_broadcaster/include/imu_sensor_broadcaster/visibility_control.h delete mode 100644 joint_state_broadcaster/include/joint_state_broadcaster/visibility_control.h delete mode 100644 joint_trajectory_controller/include/joint_trajectory_controller/visibility_control.h delete mode 100644 mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h delete mode 100644 parallel_gripper_controller/include/parallel_gripper_controller/visibility_control.hpp delete mode 100644 pid_controller/include/pid_controller/visibility_control.h delete mode 100644 pose_broadcaster/include/pose_broadcaster/visibility_control.h delete mode 100644 position_controllers/include/position_controllers/visibility_control.h delete mode 100644 range_sensor_broadcaster/include/range_sensor_broadcaster/visibility_control.h delete mode 100644 steering_controllers_library/include/steering_controllers_library/visibility_control.h delete mode 100644 tricycle_controller/include/tricycle_controller/visibility_control.h delete mode 100644 tricycle_steering_controller/include/tricycle_steering_controller/visibility_control.h delete mode 100644 velocity_controllers/include/velocity_controllers/visibility_control.h diff --git a/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt index 2ffc413633..2f160fd4c5 100644 --- a/ackermann_steering_controller/CMakeLists.txt +++ b/ackermann_steering_controller/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.16) -project(ackermann_steering_controller LANGUAGES CXX) +project(ackermann_steering_controller) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable @@ -7,6 +7,10 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") -Werror=missing-braces) endif() +# using this instead of visibility macros +# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053 +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) + # find dependencies set(THIS_PACKAGE_INCLUDE_DEPENDS controller_interface @@ -45,10 +49,6 @@ target_link_libraries(ackermann_steering_controller PUBLIC ackermann_steering_controller_parameters) ament_target_dependencies(ackermann_steering_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(ackermann_steering_controller PRIVATE "ACKERMANN_STEERING_CONTROLLER__VISIBILITY_BUILDING_DLL") - pluginlib_export_plugin_description_file( controller_interface ackermann_steering_controller.xml) diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp index 0cb6bcd016..c1eacb40bb 100644 --- a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp @@ -20,7 +20,6 @@ #include -#include "ackermann_steering_controller/visibility_control.h" #include "ackermann_steering_controller_parameters.hpp" #include "steering_controllers_library/steering_controllers_library.hpp" @@ -47,14 +46,11 @@ class AckermannSteeringController : public steering_controllers_library::Steerin public: AckermannSteeringController(); - ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn - configure_odometry() override; + controller_interface::CallbackReturn configure_odometry() override; - ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC bool update_odometry( - const rclcpp::Duration & period) override; + bool update_odometry(const rclcpp::Duration & period) override; - ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC void - initialize_implementation_parameter_listener() override; + void initialize_implementation_parameter_listener() override; protected: std::shared_ptr ackermann_param_listener_; diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h b/ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h deleted file mode 100644 index 177f0bf87c..0000000000 --- a/ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h +++ /dev/null @@ -1,52 +0,0 @@ -// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// 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 ACKERMANN_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) -#else -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) -#endif -#ifdef ACKERMANN_STEERING_CONTROLLER__VISIBILITY_BUILDING_DLL -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ - ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT -#else -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ - ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT -#endif -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE \ - ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL -#else -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT -#if __GNUC__ >= 4 -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) -#else -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL -#endif -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE -#endif - -#endif // ACKERMANN_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index 477a343776..a068239c17 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.16) -project(admittance_controller LANGUAGES CXX) +project(admittance_controller) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable @@ -7,6 +7,10 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") -Werror=missing-braces) endif() +# using this instead of visibility macros +# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053 +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) + set(THIS_PACKAGE_INCLUDE_DEPENDS angles control_msgs @@ -54,10 +58,6 @@ target_link_libraries(admittance_controller PUBLIC ) ament_target_dependencies(admittance_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(admittance_controller PRIVATE "ADMITTANCE_CONTROLLER_BUILDING_DLL") - pluginlib_export_plugin_description_file(controller_interface admittance_controller.xml) if(BUILD_TESTING) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 17b70b264d..35b072f8d7 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -25,7 +25,6 @@ #include "admittance_controller_parameters.hpp" #include "admittance_controller/admittance_rule.hpp" -#include "admittance_controller/visibility_control.h" #include "control_msgs/msg/admittance_controller_state.hpp" #include "controller_interface/chainable_controller_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -43,7 +42,6 @@ using ControllerStateMsg = control_msgs::msg::AdmittanceControllerState; class AdmittanceController : public controller_interface::ChainableControllerInterface { public: - ADMITTANCE_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_init() override; /// Export configuration of required state interfaces. @@ -51,7 +49,6 @@ class AdmittanceController : public controller_interface::ChainableControllerInt * Allowed types of state interfaces are \ref hardware_interface::POSITION, * \ref hardware_interface::VELOCITY, \ref hardware_interface::ACCELERATION. */ - ADMITTANCE_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; /// Export configuration of required state interfaces. @@ -59,30 +56,23 @@ class AdmittanceController : public controller_interface::ChainableControllerInt * Allowed types of state interfaces are \ref hardware_interface::POSITION, * \ref hardware_interface::VELOCITY, \ref hardware_interface::ACCELERATION. */ - ADMITTANCE_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; - ADMITTANCE_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; - ADMITTANCE_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; - ADMITTANCE_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; - ADMITTANCE_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_cleanup( const rclcpp_lifecycle::State & previous_state) override; - ADMITTANCE_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_error( const rclcpp_lifecycle::State & previous_state) override; - ADMITTANCE_CONTROLLER_PUBLIC controller_interface::return_type update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) override; diff --git a/admittance_controller/include/admittance_controller/visibility_control.h b/admittance_controller/include/admittance_controller/visibility_control.h deleted file mode 100644 index 24f17a5c2c..0000000000 --- a/admittance_controller/include/admittance_controller/visibility_control.h +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright (c) 2021, PickNik, Inc. -// -// 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 ADMITTANCE_CONTROLLER__VISIBILITY_CONTROL_H_ -#define ADMITTANCE_CONTROLLER__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define ADMITTANCE_CONTROLLER_EXPORT __attribute__((dllexport)) -#define ADMITTANCE_CONTROLLER_IMPORT __attribute__((dllimport)) -#else -#define ADMITTANCE_CONTROLLER_EXPORT __declspec(dllexport) -#define ADMITTANCE_CONTROLLER_IMPORT __declspec(dllimport) -#endif -#ifdef ADMITTANCE_CONTROLLER_BUILDING_DLL -#define ADMITTANCE_CONTROLLER_PUBLIC ADMITTANCE_CONTROLLER_EXPORT -#else -#define ADMITTANCE_CONTROLLER_PUBLIC ADMITTANCE_CONTROLLER_IMPORT -#endif -#define ADMITTANCE_CONTROLLER_PUBLIC_TYPE ADMITTANCE_CONTROLLER_PUBLIC -#define ADMITTANCE_CONTROLLER_LOCAL -#else -#define ADMITTANCE_CONTROLLER_EXPORT __attribute__((visibility("default"))) -#define ADMITTANCE_CONTROLLER_IMPORT -#if __GNUC__ >= 4 -#define ADMITTANCE_CONTROLLER_PUBLIC __attribute__((visibility("default"))) -#define ADMITTANCE_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) -#else -#define ADMITTANCE_CONTROLLER_PUBLIC -#define ADMITTANCE_CONTROLLER_LOCAL -#endif -#define ADMITTANCE_CONTROLLER_PUBLIC_TYPE -#endif - -#endif // ADMITTANCE_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/bicycle_steering_controller/CMakeLists.txt b/bicycle_steering_controller/CMakeLists.txt index 1f3a4599cc..2e88e9e70a 100644 --- a/bicycle_steering_controller/CMakeLists.txt +++ b/bicycle_steering_controller/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.16) -project(bicycle_steering_controller LANGUAGES CXX) +project(bicycle_steering_controller) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable @@ -7,6 +7,10 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") -Werror=missing-braces) endif() +# using this instead of visibility macros +# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053 +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) + # find dependencies set(THIS_PACKAGE_INCLUDE_DEPENDS controller_interface @@ -45,10 +49,6 @@ target_link_libraries(bicycle_steering_controller PUBLIC bicycle_steering_controller_parameters) ament_target_dependencies(bicycle_steering_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(bicycle_steering_controller PRIVATE "BICYCLE_STEERING_CONTROLLER__VISIBILITY_BUILDING_DLL") - pluginlib_export_plugin_description_file( controller_interface bicycle_steering_controller.xml) diff --git a/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp b/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp index 1b3e050a37..efb893f2bb 100644 --- a/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp @@ -20,7 +20,6 @@ #include -#include "bicycle_steering_controller/visibility_control.h" #include "bicycle_steering_controller_parameters.hpp" #include "steering_controllers_library/steering_controllers_library.hpp" @@ -43,14 +42,11 @@ class BicycleSteeringController : public steering_controllers_library::SteeringC public: BicycleSteeringController(); - BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn - configure_odometry() override; + controller_interface::CallbackReturn configure_odometry() override; - BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC bool update_odometry( - const rclcpp::Duration & period) override; + bool update_odometry(const rclcpp::Duration & period) override; - BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC void initialize_implementation_parameter_listener() - override; + void initialize_implementation_parameter_listener() override; protected: std::shared_ptr bicycle_param_listener_; diff --git a/bicycle_steering_controller/include/bicycle_steering_controller/visibility_control.h b/bicycle_steering_controller/include/bicycle_steering_controller/visibility_control.h deleted file mode 100644 index b076a00215..0000000000 --- a/bicycle_steering_controller/include/bicycle_steering_controller/visibility_control.h +++ /dev/null @@ -1,52 +0,0 @@ -// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// 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 BICYCLE_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ -#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) -#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) -#else -#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) -#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) -#endif -#ifdef BICYCLE_STEERING_CONTROLLER__VISIBILITY_BUILDING_DLL -#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ - BICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT -#else -#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ - BICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT -#endif -#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE \ - BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC -#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_LOCAL -#else -#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) -#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT -#if __GNUC__ >= 4 -#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) -#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) -#else -#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC -#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_LOCAL -#endif -#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE -#endif - -#endif // BICYCLE_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index d94b6e3ce0..c5f4ade8a6 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.16) -project(diff_drive_controller LANGUAGES CXX) +project(diff_drive_controller) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable @@ -7,6 +7,10 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") -Werror=missing-braces) endif() +# using this instead of visibility macros +# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053 +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) + set(THIS_PACKAGE_INCLUDE_DEPENDS control_toolbox controller_interface @@ -49,9 +53,6 @@ target_link_libraries(diff_drive_controller PUBLIC diff_drive_controller_parameters) ament_target_dependencies(diff_drive_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(diff_drive_controller PRIVATE "DIFF_DRIVE_CONTROLLER_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface diff_drive_plugin.xml) if(BUILD_TESTING) 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 85f4fb23b0..36ba2d6874 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 @@ -28,7 +28,6 @@ #include "controller_interface/controller_interface.hpp" #include "diff_drive_controller/odometry.hpp" #include "diff_drive_controller/speed_limiter.hpp" -#include "diff_drive_controller/visibility_control.h" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "odometry.hpp" @@ -47,43 +46,32 @@ class DiffDriveController : public controller_interface::ControllerInterface using TwistStamped = geometry_msgs::msg::TwistStamped; public: - DIFF_DRIVE_CONTROLLER_PUBLIC DiffDriveController(); - DIFF_DRIVE_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; - DIFF_DRIVE_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; - DIFF_DRIVE_CONTROLLER_PUBLIC controller_interface::return_type update( const rclcpp::Time & time, const rclcpp::Duration & period) override; - DIFF_DRIVE_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_init() override; - DIFF_DRIVE_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; - DIFF_DRIVE_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; - DIFF_DRIVE_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; - DIFF_DRIVE_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_cleanup( const rclcpp_lifecycle::State & previous_state) override; - DIFF_DRIVE_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_error( const rclcpp_lifecycle::State & previous_state) override; - DIFF_DRIVE_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_shutdown( const rclcpp_lifecycle::State & previous_state) override; diff --git a/diff_drive_controller/include/diff_drive_controller/visibility_control.h b/diff_drive_controller/include/diff_drive_controller/visibility_control.h deleted file mode 100644 index 94d78153eb..0000000000 --- a/diff_drive_controller/include/diff_drive_controller/visibility_control.h +++ /dev/null @@ -1,56 +0,0 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. -// -// 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. - -/* This header must be included by all rclcpp headers which declare symbols - * which are defined in the rclcpp library. When not building the rclcpp - * library, i.e. when using the headers in other package's code, the contents - * of this header change the visibility of certain symbols which the rclcpp - * library cannot have, but the consuming code must have inorder to link. - */ - -#ifndef DIFF_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_ -#define DIFF_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define DIFF_DRIVE_CONTROLLER_EXPORT __attribute__((dllexport)) -#define DIFF_DRIVE_CONTROLLER_IMPORT __attribute__((dllimport)) -#else -#define DIFF_DRIVE_CONTROLLER_EXPORT __declspec(dllexport) -#define DIFF_DRIVE_CONTROLLER_IMPORT __declspec(dllimport) -#endif -#ifdef DIFF_DRIVE_CONTROLLER_BUILDING_DLL -#define DIFF_DRIVE_CONTROLLER_PUBLIC DIFF_DRIVE_CONTROLLER_EXPORT -#else -#define DIFF_DRIVE_CONTROLLER_PUBLIC DIFF_DRIVE_CONTROLLER_IMPORT -#endif -#define DIFF_DRIVE_CONTROLLER_PUBLIC_TYPE DIFF_DRIVE_CONTROLLER_PUBLIC -#define DIFF_DRIVE_CONTROLLER_LOCAL -#else -#define DIFF_DRIVE_CONTROLLER_EXPORT __attribute__((visibility("default"))) -#define DIFF_DRIVE_CONTROLLER_IMPORT -#if __GNUC__ >= 4 -#define DIFF_DRIVE_CONTROLLER_PUBLIC __attribute__((visibility("default"))) -#define DIFF_DRIVE_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) -#else -#define DIFF_DRIVE_CONTROLLER_PUBLIC -#define DIFF_DRIVE_CONTROLLER_LOCAL -#endif -#define DIFF_DRIVE_CONTROLLER_PUBLIC_TYPE -#endif - -#endif // DIFF_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/effort_controllers/CMakeLists.txt b/effort_controllers/CMakeLists.txt index e79015fbbf..60cce95e28 100644 --- a/effort_controllers/CMakeLists.txt +++ b/effort_controllers/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.16) -project(effort_controllers LANGUAGES CXX) +project(effort_controllers) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable @@ -7,6 +7,10 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") -Werror=missing-braces) endif() +# using this instead of visibility macros +# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053 +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) + set(THIS_PACKAGE_INCLUDE_DEPENDS forward_command_controller pluginlib @@ -29,9 +33,6 @@ target_include_directories(effort_controllers PUBLIC ) ament_target_dependencies(effort_controllers PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(effort_controllers PRIVATE "EFFORT_CONTROLLERS_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface effort_controllers_plugins.xml) if(BUILD_TESTING) diff --git a/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp b/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp index b07e8a630c..2855570cfd 100644 --- a/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp +++ b/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp @@ -15,7 +15,6 @@ #ifndef EFFORT_CONTROLLERS__JOINT_GROUP_EFFORT_CONTROLLER_HPP_ #define EFFORT_CONTROLLERS__JOINT_GROUP_EFFORT_CONTROLLER_HPP_ -#include "effort_controllers/visibility_control.h" #include "forward_command_controller/forward_command_controller.hpp" namespace effort_controllers @@ -33,13 +32,10 @@ namespace effort_controllers class JointGroupEffortController : public forward_command_controller::ForwardCommandController { public: - EFFORT_CONTROLLERS_PUBLIC JointGroupEffortController(); - EFFORT_CONTROLLERS_PUBLIC controller_interface::CallbackReturn on_init() override; - EFFORT_CONTROLLERS_PUBLIC controller_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; }; diff --git a/effort_controllers/include/effort_controllers/visibility_control.h b/effort_controllers/include/effort_controllers/visibility_control.h deleted file mode 100644 index e3ca28355d..0000000000 --- a/effort_controllers/include/effort_controllers/visibility_control.h +++ /dev/null @@ -1,56 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// 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. - -/* This header must be included by all rclcpp headers which declare symbols - * which are defined in the rclcpp library. When not building the rclcpp - * library, i.e. when using the headers in other package's code, the contents - * of this header change the visibility of certain symbols which the rclcpp - * library cannot have, but the consuming code must have inorder to link. - */ - -#ifndef EFFORT_CONTROLLERS__VISIBILITY_CONTROL_H_ -#define EFFORT_CONTROLLERS__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define EFFORT_CONTROLLERS_EXPORT __attribute__((dllexport)) -#define EFFORT_CONTROLLERS_IMPORT __attribute__((dllimport)) -#else -#define EFFORT_CONTROLLERS_EXPORT __declspec(dllexport) -#define EFFORT_CONTROLLERS_IMPORT __declspec(dllimport) -#endif -#ifdef EFFORT_CONTROLLERS_BUILDING_DLL -#define EFFORT_CONTROLLERS_PUBLIC EFFORT_CONTROLLERS_EXPORT -#else -#define EFFORT_CONTROLLERS_PUBLIC EFFORT_CONTROLLERS_IMPORT -#endif -#define EFFORT_CONTROLLERS_PUBLIC_TYPE EFFORT_CONTROLLERS_PUBLIC -#define EFFORT_CONTROLLERS_LOCAL -#else -#define EFFORT_CONTROLLERS_EXPORT __attribute__((visibility("default"))) -#define EFFORT_CONTROLLERS_IMPORT -#if __GNUC__ >= 4 -#define EFFORT_CONTROLLERS_PUBLIC __attribute__((visibility("default"))) -#define EFFORT_CONTROLLERS_LOCAL __attribute__((visibility("hidden"))) -#else -#define EFFORT_CONTROLLERS_PUBLIC -#define EFFORT_CONTROLLERS_LOCAL -#endif -#define EFFORT_CONTROLLERS_PUBLIC_TYPE -#endif - -#endif // EFFORT_CONTROLLERS__VISIBILITY_CONTROL_H_ diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index d9b005e650..be54c3bd58 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.16) -project(force_torque_sensor_broadcaster LANGUAGES CXX) +project(force_torque_sensor_broadcaster) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable @@ -7,6 +7,10 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") -Werror=missing-braces) endif() +# using this instead of visibility macros +# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053 +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) + set(THIS_PACKAGE_INCLUDE_DEPENDS controller_interface generate_parameter_library @@ -41,10 +45,6 @@ target_link_libraries(force_torque_sensor_broadcaster PUBLIC ) ament_target_dependencies(force_torque_sensor_broadcaster PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(force_torque_sensor_broadcaster PRIVATE "FORCE_TORQUE_SENSOR_BROADCASTER_BUILDING_DLL") - pluginlib_export_plugin_description_file( controller_interface force_torque_sensor_broadcaster.xml) diff --git a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp index 2364dd7c8b..0861715678 100644 --- a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp @@ -23,7 +23,6 @@ #include #include "controller_interface/chainable_controller_interface.hpp" -#include "force_torque_sensor_broadcaster/visibility_control.h" // auto-generated by generate_parameter_library #include "force_torque_sensor_broadcaster_parameters.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" @@ -36,38 +35,28 @@ namespace force_torque_sensor_broadcaster class ForceTorqueSensorBroadcaster : public controller_interface::ChainableControllerInterface { public: - FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC ForceTorqueSensorBroadcaster(); - FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; - FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; + controller_interface::CallbackReturn on_init() override; - FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_init() override; - - FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; - FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; - FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; - FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC controller_interface::return_type update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) override; - FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC controller_interface::return_type update_reference_from_subscribers( const rclcpp::Time & time, const rclcpp::Duration & period) override; - FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC std::vector on_export_state_interfaces() override; protected: diff --git a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/visibility_control.h b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/visibility_control.h deleted file mode 100644 index 1f5295e886..0000000000 --- a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/visibility_control.h +++ /dev/null @@ -1,53 +0,0 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// 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. - -/* - * Author: Subhas Das, Denis Stogl - */ - -#ifndef FORCE_TORQUE_SENSOR_BROADCASTER__VISIBILITY_CONTROL_H_ -#define FORCE_TORQUE_SENSOR_BROADCASTER__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define FORCE_TORQUE_SENSOR_BROADCASTER_EXPORT __attribute__((dllexport)) -#define FORCE_TORQUE_SENSOR_BROADCASTER_IMPORT __attribute__((dllimport)) -#else -#define FORCE_TORQUE_SENSOR_BROADCASTER_EXPORT __declspec(dllexport) -#define FORCE_TORQUE_SENSOR_BROADCASTER_IMPORT __declspec(dllimport) -#endif -#ifdef FORCE_TORQUE_SENSOR_BROADCASTER_BUILDING_DLL -#define FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC FORCE_TORQUE_SENSOR_BROADCASTER_EXPORT -#else -#define FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC FORCE_TORQUE_SENSOR_BROADCASTER_IMPORT -#endif -#define FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC_TYPE FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC -#define FORCE_TORQUE_SENSOR_BROADCASTER_LOCAL -#else -#define FORCE_TORQUE_SENSOR_BROADCASTER_EXPORT __attribute__((visibility("default"))) -#define FORCE_TORQUE_SENSOR_BROADCASTER_IMPORT -#if __GNUC__ >= 4 -#define FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC __attribute__((visibility("default"))) -#define FORCE_TORQUE_SENSOR_BROADCASTER_LOCAL __attribute__((visibility("hidden"))) -#else -#define FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC -#define FORCE_TORQUE_SENSOR_BROADCASTER_LOCAL -#endif -#define FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC_TYPE -#endif - -#endif // FORCE_TORQUE_SENSOR_BROADCASTER__VISIBILITY_CONTROL_H_ diff --git a/forward_command_controller/CMakeLists.txt b/forward_command_controller/CMakeLists.txt index bf027866d6..f7610c5f85 100644 --- a/forward_command_controller/CMakeLists.txt +++ b/forward_command_controller/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.16) -project(forward_command_controller LANGUAGES CXX) +project(forward_command_controller) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable @@ -7,6 +7,10 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") -Werror=missing-braces) endif() +# using this instead of visibility macros +# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053 +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) + set(THIS_PACKAGE_INCLUDE_DEPENDS controller_interface generate_parameter_library @@ -48,12 +52,8 @@ target_link_libraries(forward_command_controller PUBLIC multi_interface_forward_command_controller_parameters ) ament_target_dependencies(forward_command_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(forward_command_controller PRIVATE "FORWARD_COMMAND_CONTROLLER_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface forward_command_plugin.xml) - if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) diff --git a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp index 12f2a28b22..5401263ec6 100644 --- a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp @@ -18,7 +18,6 @@ #include #include "forward_command_controller/forward_controllers_base.hpp" -#include "forward_command_controller/visibility_control.h" // auto-generated by generate_parameter_library #include "forward_command_controller_parameters.hpp" @@ -38,7 +37,6 @@ namespace forward_command_controller class ForwardCommandController : public ForwardControllersBase { public: - FORWARD_COMMAND_CONTROLLER_PUBLIC ForwardCommandController(); protected: diff --git a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp index fce7941d8a..c8b620d593 100644 --- a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp @@ -20,7 +20,6 @@ #include #include "controller_interface/controller_interface.hpp" -#include "forward_command_controller/visibility_control.h" #include "rclcpp/subscription.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.hpp" @@ -41,34 +40,25 @@ using CmdType = std_msgs::msg::Float64MultiArray; class ForwardControllersBase : public controller_interface::ControllerInterface { public: - FORWARD_COMMAND_CONTROLLER_PUBLIC ForwardControllersBase(); - FORWARD_COMMAND_CONTROLLER_PUBLIC ~ForwardControllersBase() = default; - FORWARD_COMMAND_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; - FORWARD_COMMAND_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; - FORWARD_COMMAND_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_init() override; - FORWARD_COMMAND_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; - FORWARD_COMMAND_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; - FORWARD_COMMAND_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; - FORWARD_COMMAND_CONTROLLER_PUBLIC controller_interface::return_type update( const rclcpp::Time & time, const rclcpp::Duration & period) override; diff --git a/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp index 50476c62e3..6ba33be018 100644 --- a/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp @@ -18,7 +18,6 @@ #include #include "forward_command_controller/forward_controllers_base.hpp" -#include "forward_command_controller/visibility_control.h" #include "multi_interface_forward_command_controller_parameters.hpp" namespace forward_command_controller @@ -38,7 +37,6 @@ class MultiInterfaceForwardCommandController : public forward_command_controller::ForwardControllersBase { public: - FORWARD_COMMAND_CONTROLLER_PUBLIC MultiInterfaceForwardCommandController(); protected: diff --git a/forward_command_controller/include/forward_command_controller/visibility_control.h b/forward_command_controller/include/forward_command_controller/visibility_control.h deleted file mode 100644 index 25a0697fdc..0000000000 --- a/forward_command_controller/include/forward_command_controller/visibility_control.h +++ /dev/null @@ -1,56 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// 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. - -/* This header must be included by all rclcpp headers which declare symbols - * which are defined in the rclcpp library. When not building the rclcpp - * library, i.e. when using the headers in other package's code, the contents - * of this header change the visibility of certain symbols which the rclcpp - * library cannot have, but the consuming code must have inorder to link. - */ - -#ifndef FORWARD_COMMAND_CONTROLLER__VISIBILITY_CONTROL_H_ -#define FORWARD_COMMAND_CONTROLLER__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define FORWARD_COMMAND_CONTROLLER_EXPORT __attribute__((dllexport)) -#define FORWARD_COMMAND_CONTROLLER_IMPORT __attribute__((dllimport)) -#else -#define FORWARD_COMMAND_CONTROLLER_EXPORT __declspec(dllexport) -#define FORWARD_COMMAND_CONTROLLER_IMPORT __declspec(dllimport) -#endif -#ifdef FORWARD_COMMAND_CONTROLLER_BUILDING_DLL -#define FORWARD_COMMAND_CONTROLLER_PUBLIC FORWARD_COMMAND_CONTROLLER_EXPORT -#else -#define FORWARD_COMMAND_CONTROLLER_PUBLIC FORWARD_COMMAND_CONTROLLER_IMPORT -#endif -#define FORWARD_COMMAND_CONTROLLER_PUBLIC_TYPE FORWARD_COMMAND_CONTROLLER_PUBLIC -#define FORWARD_COMMAND_CONTROLLER_LOCAL -#else -#define FORWARD_COMMAND_CONTROLLER_EXPORT __attribute__((visibility("default"))) -#define FORWARD_COMMAND_CONTROLLER_IMPORT -#if __GNUC__ >= 4 -#define FORWARD_COMMAND_CONTROLLER_PUBLIC __attribute__((visibility("default"))) -#define FORWARD_COMMAND_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) -#else -#define FORWARD_COMMAND_CONTROLLER_PUBLIC -#define FORWARD_COMMAND_CONTROLLER_LOCAL -#endif -#define FORWARD_COMMAND_CONTROLLER_PUBLIC_TYPE -#endif - -#endif // FORWARD_COMMAND_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/gpio_controllers/CMakeLists.txt b/gpio_controllers/CMakeLists.txt index 6ea97e04ca..62274b0cf9 100644 --- a/gpio_controllers/CMakeLists.txt +++ b/gpio_controllers/CMakeLists.txt @@ -1,17 +1,16 @@ cmake_minimum_required(VERSION 3.8) project(gpio_controllers) -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct -Werror=missing-braces) endif() +# using this instead of visibility macros +# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053 +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) + # find dependencies find_package(ament_cmake REQUIRED) find_package(controller_interface REQUIRED) @@ -23,7 +22,6 @@ find_package(realtime_tools REQUIRED) find_package(generate_parameter_library REQUIRED) find_package(control_msgs REQUIRED) - generate_parameter_library(gpio_command_controller_parameters src/gpio_command_controller_parameters.yaml ) @@ -44,9 +42,6 @@ ament_target_dependencies(gpio_controllers PUBLIC realtime_tools control_msgs ) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(gpio_controllers PRIVATE "GPIO_COMMAND_CONTROLLER_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface gpio_controllers_plugin.xml) install( diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp index febac1294e..0513d9e1f4 100644 --- a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -23,7 +23,6 @@ #include "control_msgs/msg/dynamic_interface_group_values.hpp" #include "controller_interface/controller_interface.hpp" #include "gpio_command_controller_parameters.hpp" -#include "gpio_controllers/visibility_control.h" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.hpp" @@ -45,28 +44,20 @@ using StateInterfaces = class GpioCommandController : public controller_interface::ControllerInterface { public: - GPIO_COMMAND_CONTROLLER_PUBLIC GpioCommandController(); - GPIO_COMMAND_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; - GPIO_COMMAND_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; - GPIO_COMMAND_CONTROLLER_PUBLIC CallbackReturn on_init() override; - GPIO_COMMAND_CONTROLLER_PUBLIC CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; - GPIO_COMMAND_CONTROLLER_PUBLIC CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; - GPIO_COMMAND_CONTROLLER_PUBLIC CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; - GPIO_COMMAND_CONTROLLER_PUBLIC controller_interface::return_type update( const rclcpp::Time & time, const rclcpp::Duration & period) override; diff --git a/gpio_controllers/include/gpio_controllers/visibility_control.h b/gpio_controllers/include/gpio_controllers/visibility_control.h deleted file mode 100644 index a735a1621c..0000000000 --- a/gpio_controllers/include/gpio_controllers/visibility_control.h +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright 2022 ICUBE Laboratory, University of Strasbourg -// -// 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 GPIO_CONTROLLERS__VISIBILITY_CONTROL_H_ -#define GPIO_CONTROLLERS__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define GPIO_COMMAND_CONTROLLER_EXPORT __attribute__((dllexport)) -#define GPIO_COMMAND_CONTROLLER_IMPORT __attribute__((dllimport)) -#else -#define GPIO_COMMAND_CONTROLLER_EXPORT __declspec(dllexport) -#define GPIO_COMMAND_CONTROLLER_IMPORT __declspec(dllimport) -#endif -#ifdef GPIO_COMMAND_CONTROLLER_BUILDING_LIBRARY -#define GPIO_COMMAND_CONTROLLER_PUBLIC GPIO_COMMAND_CONTROLLER_EXPORT -#else -#define GPIO_COMMAND_CONTROLLER_PUBLIC GPIO_COMMAND_CONTROLLER_IMPORT -#endif -#define GPIO_COMMAND_CONTROLLER_PUBLIC_TYPE GPIO_COMMAND_CONTROLLER_PUBLIC -#define GPIO_COMMAND_CONTROLLER_LOCAL -#else -#define GPIO_COMMAND_CONTROLLER_EXPORT __attribute__((visibility("default"))) -#define GPIO_COMMAND_CONTROLLER_IMPORT -#if __GNUC__ >= 4 -#define GPIO_COMMAND_CONTROLLER_PUBLIC __attribute__((visibility("default"))) -#define GPIO_COMMAND_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) -#else -#define GPIO_COMMAND_CONTROLLER_PUBLIC -#define GPIO_COMMAND_CONTROLLER_LOCAL -#endif -#define GPIO_COMMAND_CONTROLLER_PUBLIC_TYPE -#endif - -#endif // GPIO_CONTROLLERS__VISIBILITY_CONTROL_H_ diff --git a/gripper_controllers/CMakeLists.txt b/gripper_controllers/CMakeLists.txt index 4e9c72f79b..4ffdc76168 100644 --- a/gripper_controllers/CMakeLists.txt +++ b/gripper_controllers/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.16) -project(gripper_controllers LANGUAGES CXX) +project(gripper_controllers) if(APPLE OR WIN32) message(WARNING "gripper controllers are not available on OSX or Windows") @@ -47,10 +47,6 @@ target_link_libraries(gripper_action_controller PUBLIC ) ament_target_dependencies(gripper_action_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(gripper_action_controller PRIVATE "GRIPPER_ACTION_CONTROLLER_BUILDING_DLL") - pluginlib_export_plugin_description_file(controller_interface ros_control_plugins.xml) if(BUILD_TESTING) diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp index 478168391b..d166d12478 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp @@ -34,7 +34,6 @@ // ros_controls #include "controller_interface/controller_interface.hpp" -#include "gripper_controllers/visibility_control.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" #include "realtime_tools/realtime_buffer.hpp" @@ -69,39 +68,31 @@ class GripperActionController : public controller_interface::ControllerInterface double position_; // Last commanded position double max_effort_; // Max allowed effort }; - - GRIPPER_ACTION_CONTROLLER_PUBLIC GripperActionController(); + GripperActionController(); /** * @brief command_interface_configuration This controller requires the * position command interfaces for the controlled joints */ - GRIPPER_ACTION_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; /** * @brief command_interface_configuration This controller requires the * position and velocity state interfaces for the controlled joints */ - GRIPPER_ACTION_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; - GRIPPER_ACTION_CONTROLLER_PUBLIC controller_interface::return_type update( const rclcpp::Time & time, const rclcpp::Duration & period) override; - GRIPPER_ACTION_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_init() override; - GRIPPER_ACTION_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; - GRIPPER_ACTION_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; - GRIPPER_ACTION_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; diff --git a/gripper_controllers/include/gripper_controllers/visibility_control.hpp b/gripper_controllers/include/gripper_controllers/visibility_control.hpp deleted file mode 100644 index 76b8dafe54..0000000000 --- a/gripper_controllers/include/gripper_controllers/visibility_control.hpp +++ /dev/null @@ -1,56 +0,0 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. -// -// 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. - -/* This header must be included by all rclcpp headers which declare symbols - * which are defined in the rclcpp library. When not building the rclcpp - * library, i.e. when using the headers in other package's code, the contents - * of this header change the visibility of certain symbols which the rclcpp - * library cannot have, but the consuming code must have inorder to link. - */ - -#ifndef GRIPPER_CONTROLLERS__VISIBILITY_CONTROL_HPP_ -#define GRIPPER_CONTROLLERS__VISIBILITY_CONTROL_HPP_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define GRIPPER_ACTION_CONTROLLER_EXPORT __attribute__((dllexport)) -#define GRIPPER_ACTION_CONTROLLER_IMPORT __attribute__((dllimport)) -#else -#define GRIPPER_ACTION_CONTROLLER_EXPORT __declspec(dllexport) -#define GRIPPER_ACTION_CONTROLLER_IMPORT __declspec(dllimport) -#endif -#ifdef GRIPPER_ACTION_CONTROLLER_BUILDING_DLL -#define GRIPPER_ACTION_CONTROLLER_PUBLIC GRIPPER_ACTION_CONTROLLER_EXPORT -#else -#define GRIPPER_ACTION_CONTROLLER_PUBLIC GRIPPER_ACTION_CONTROLLER_IMPORT -#endif -#define GRIPPER_ACTION_CONTROLLER_PUBLIC_TYPE GRIPPER_ACTION_CONTROLLER_PUBLIC -#define GRIPPER_ACTION_CONTROLLER_LOCAL -#else -#define GRIPPER_ACTION_CONTROLLER_EXPORT __attribute__((visibility("default"))) -#define GRIPPER_ACTION_CONTROLLER_IMPORT -#if __GNUC__ >= 4 -#define GRIPPER_ACTION_CONTROLLER_PUBLIC __attribute__((visibility("default"))) -#define GRIPPER_ACTION_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) -#else -#define GRIPPER_ACTION_CONTROLLER_PUBLIC -#define GRIPPER_ACTION_CONTROLLER_LOCAL -#endif -#define GRIPPER_ACTION_CONTROLLER_PUBLIC_TYPE -#endif - -#endif // GRIPPER_CONTROLLERS__VISIBILITY_CONTROL_HPP_ diff --git a/imu_sensor_broadcaster/CMakeLists.txt b/imu_sensor_broadcaster/CMakeLists.txt index 38f0adbb54..7b12c9095e 100644 --- a/imu_sensor_broadcaster/CMakeLists.txt +++ b/imu_sensor_broadcaster/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.16) -project(imu_sensor_broadcaster LANGUAGES CXX) +project(imu_sensor_broadcaster) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable @@ -7,6 +7,10 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") -Werror=missing-braces) endif() +# using this instead of visibility macros +# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053 +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) + set(THIS_PACKAGE_INCLUDE_DEPENDS controller_interface generate_parameter_library @@ -41,10 +45,6 @@ target_link_libraries(imu_sensor_broadcaster PUBLIC ) ament_target_dependencies(imu_sensor_broadcaster PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(imu_sensor_broadcaster PRIVATE "IMU_SENSOR_BROADCASTER_BUILDING_DLL") - pluginlib_export_plugin_description_file( controller_interface imu_sensor_broadcaster.xml) diff --git a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp index 449020b6e2..fda18af83f 100644 --- a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp @@ -22,7 +22,6 @@ #include #include "controller_interface/controller_interface.hpp" -#include "imu_sensor_broadcaster/visibility_control.h" // auto-generated by generate_parameter_library #include "imu_sensor_broadcaster_parameters.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -35,27 +34,20 @@ namespace imu_sensor_broadcaster class IMUSensorBroadcaster : public controller_interface::ControllerInterface { public: - IMU_SENSOR_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; - IMU_SENSOR_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; + controller_interface::CallbackReturn on_init() override; - IMU_SENSOR_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_init() override; - - IMU_SENSOR_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; - IMU_SENSOR_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; - IMU_SENSOR_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; - IMU_SENSOR_BROADCASTER_PUBLIC controller_interface::return_type update( const rclcpp::Time & time, const rclcpp::Duration & period) override; diff --git a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/visibility_control.h b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/visibility_control.h deleted file mode 100644 index fbac130a07..0000000000 --- a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/visibility_control.h +++ /dev/null @@ -1,53 +0,0 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// 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. - -/* - * Author: Subhas Das, Denis Stogl - */ - -#ifndef IMU_SENSOR_BROADCASTER__VISIBILITY_CONTROL_H_ -#define IMU_SENSOR_BROADCASTER__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define IMU_SENSOR_BROADCASTER_EXPORT __attribute__((dllexport)) -#define IMU_SENSOR_BROADCASTER_IMPORT __attribute__((dllimport)) -#else -#define IMU_SENSOR_BROADCASTER_EXPORT __declspec(dllexport) -#define IMU_SENSOR_BROADCASTER_IMPORT __declspec(dllimport) -#endif -#ifdef IMU_SENSOR_BROADCASTER_BUILDING_DLL -#define IMU_SENSOR_BROADCASTER_PUBLIC IMU_SENSOR_BROADCASTER_EXPORT -#else -#define IMU_SENSOR_BROADCASTER_PUBLIC IMU_SENSOR_BROADCASTER_IMPORT -#endif -#define IMU_SENSOR_BROADCASTER_PUBLIC_TYPE IMU_SENSOR_BROADCASTER_PUBLIC -#define IMU_SENSOR_BROADCASTER_LOCAL -#else -#define IMU_SENSOR_BROADCASTER_EXPORT __attribute__((visibility("default"))) -#define IMU_SENSOR_BROADCASTER_IMPORT -#if __GNUC__ >= 4 -#define IMU_SENSOR_BROADCASTER_PUBLIC __attribute__((visibility("default"))) -#define IMU_SENSOR_BROADCASTER_LOCAL __attribute__((visibility("hidden"))) -#else -#define IMU_SENSOR_BROADCASTER_PUBLIC -#define IMU_SENSOR_BROADCASTER_LOCAL -#endif -#define IMU_SENSOR_BROADCASTER_PUBLIC_TYPE -#endif - -#endif // IMU_SENSOR_BROADCASTER__VISIBILITY_CONTROL_H_ diff --git a/joint_state_broadcaster/CMakeLists.txt b/joint_state_broadcaster/CMakeLists.txt index a75bbfa8f9..adbd5254e1 100644 --- a/joint_state_broadcaster/CMakeLists.txt +++ b/joint_state_broadcaster/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.16) -project(joint_state_broadcaster LANGUAGES CXX) +project(joint_state_broadcaster) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable @@ -7,6 +7,10 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") -Werror=missing-braces) endif() +# using this instead of visibility macros +# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053 +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) + set(THIS_PACKAGE_INCLUDE_DEPENDS builtin_interfaces control_msgs @@ -42,9 +46,6 @@ target_link_libraries(joint_state_broadcaster PUBLIC joint_state_broadcaster_parameters ) ament_target_dependencies(joint_state_broadcaster PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(joint_state_broadcaster PRIVATE "JOINT_STATE_BROADCASTER_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface joint_state_plugin.xml) if(BUILD_TESTING) diff --git a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp index 7ac98eccfb..da02c5feb8 100644 --- a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp @@ -22,7 +22,6 @@ #include "control_msgs/msg/dynamic_joint_state.hpp" #include "controller_interface/controller_interface.hpp" -#include "joint_state_broadcaster/visibility_control.h" // auto-generated by generate_parameter_library #include "joint_state_broadcaster_parameters.hpp" #include "realtime_tools/realtime_publisher.hpp" @@ -58,31 +57,23 @@ namespace joint_state_broadcaster class JointStateBroadcaster : public controller_interface::ControllerInterface { public: - JOINT_STATE_BROADCASTER_PUBLIC JointStateBroadcaster(); - JOINT_STATE_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; - JOINT_STATE_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; - JOINT_STATE_BROADCASTER_PUBLIC controller_interface::return_type update( const rclcpp::Time & time, const rclcpp::Duration & period) override; - JOINT_STATE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_init() override; - JOINT_STATE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; - JOINT_STATE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; - JOINT_STATE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; diff --git a/joint_state_broadcaster/include/joint_state_broadcaster/visibility_control.h b/joint_state_broadcaster/include/joint_state_broadcaster/visibility_control.h deleted file mode 100644 index 8bbe0fcd73..0000000000 --- a/joint_state_broadcaster/include/joint_state_broadcaster/visibility_control.h +++ /dev/null @@ -1,56 +0,0 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. -// -// 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. - -/* This header must be included by all rclcpp headers which declare symbols - * which are defined in the rclcpp library. When not building the rclcpp - * library, i.e. when using the headers in other package's code, the contents - * of this header change the visibility of certain symbols which the rclcpp - * library cannot have, but the consuming code must have inorder to link. - */ - -#ifndef JOINT_STATE_BROADCASTER__VISIBILITY_CONTROL_H_ -#define JOINT_STATE_BROADCASTER__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define JOINT_STATE_BROADCASTER_EXPORT __attribute__((dllexport)) -#define JOINT_STATE_BROADCASTER_IMPORT __attribute__((dllimport)) -#else -#define JOINT_STATE_BROADCASTER_EXPORT __declspec(dllexport) -#define JOINT_STATE_BROADCASTER_IMPORT __declspec(dllimport) -#endif -#ifdef JOINT_STATE_BROADCASTER_BUILDING_DLL -#define JOINT_STATE_BROADCASTER_PUBLIC JOINT_STATE_BROADCASTER_EXPORT -#else -#define JOINT_STATE_BROADCASTER_PUBLIC JOINT_STATE_BROADCASTER_IMPORT -#endif -#define JOINT_STATE_BROADCASTER_PUBLIC_TYPE JOINT_STATE_BROADCASTER_PUBLIC -#define JOINT_STATE_BROADCASTER_LOCAL -#else -#define JOINT_STATE_BROADCASTER_EXPORT __attribute__((visibility("default"))) -#define JOINT_STATE_BROADCASTER_IMPORT -#if __GNUC__ >= 4 -#define JOINT_STATE_BROADCASTER_PUBLIC __attribute__((visibility("default"))) -#define JOINT_STATE_BROADCASTER_LOCAL __attribute__((visibility("hidden"))) -#else -#define JOINT_STATE_BROADCASTER_PUBLIC -#define JOINT_STATE_BROADCASTER_LOCAL -#endif -#define JOINT_STATE_BROADCASTER_PUBLIC_TYPE -#endif - -#endif // JOINT_STATE_BROADCASTER__VISIBILITY_CONTROL_H_ diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index ec142c72f3..37e68b08c9 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.16) -project(joint_trajectory_controller LANGUAGES CXX) +project(joint_trajectory_controller) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable @@ -7,6 +7,10 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") -Werror=missing-braces) endif() +# using this instead of visibility macros +# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053 +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) + set(THIS_PACKAGE_INCLUDE_DEPENDS angles control_msgs @@ -49,9 +53,6 @@ target_link_libraries(joint_trajectory_controller PUBLIC ) ament_target_dependencies(joint_trajectory_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(joint_trajectory_controller PRIVATE "JOINT_TRAJECTORY_CONTROLLER_BUILDING_DLL" "_USE_MATH_DEFINES") pluginlib_export_plugin_description_file(controller_interface joint_trajectory_plugin.xml) if(BUILD_TESTING) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 324ccfe4f1..abefec9d79 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -29,7 +29,6 @@ #include "joint_trajectory_controller/interpolation_methods.hpp" #include "joint_trajectory_controller/tolerances.hpp" #include "joint_trajectory_controller/trajectory.hpp" -#include "joint_trajectory_controller/visibility_control.h" #include "rclcpp/duration.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/time.hpp" @@ -53,49 +52,38 @@ namespace joint_trajectory_controller class JointTrajectoryController : public controller_interface::ControllerInterface { public: - JOINT_TRAJECTORY_CONTROLLER_PUBLIC JointTrajectoryController(); /** * @brief command_interface_configuration */ - JOINT_TRAJECTORY_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; /** * @brief command_interface_configuration */ - JOINT_TRAJECTORY_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; - JOINT_TRAJECTORY_CONTROLLER_PUBLIC controller_interface::return_type update( const rclcpp::Time & time, const rclcpp::Duration & period) override; - JOINT_TRAJECTORY_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_init() override; - JOINT_TRAJECTORY_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; - JOINT_TRAJECTORY_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; - JOINT_TRAJECTORY_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; - JOINT_TRAJECTORY_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_cleanup( const rclcpp_lifecycle::State & previous_state) override; - JOINT_TRAJECTORY_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_error( const rclcpp_lifecycle::State & previous_state) override; - JOINT_TRAJECTORY_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_shutdown( const rclcpp_lifecycle::State & previous_state) override; @@ -198,17 +186,13 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms); // callback for topic interface - JOINT_TRAJECTORY_CONTROLLER_PUBLIC void topic_callback(const std::shared_ptr msg); // callbacks for action_server_ - JOINT_TRAJECTORY_CONTROLLER_PUBLIC rclcpp_action::GoalResponse goal_received_callback( const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); - JOINT_TRAJECTORY_CONTROLLER_PUBLIC rclcpp_action::CancelResponse goal_cancelled_callback( const std::shared_ptr> goal_handle); - JOINT_TRAJECTORY_CONTROLLER_PUBLIC void goal_accepted_callback( std::shared_ptr> goal_handle); @@ -222,25 +206,19 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa * @param[in] current The current state of the joints. * @param[in] desired The desired state of the joints. */ - JOINT_TRAJECTORY_CONTROLLER_PUBLIC void compute_error_for_joint( JointTrajectoryPoint & error, const size_t index, const JointTrajectoryPoint & current, const JointTrajectoryPoint & desired) const; // fill trajectory_msg so it matches joints controlled by this controller // positions set to current position, velocities, accelerations and efforts to 0.0 - JOINT_TRAJECTORY_CONTROLLER_PUBLIC void fill_partial_goal( std::shared_ptr trajectory_msg) const; // sorts the joints of the incoming message to our local order - JOINT_TRAJECTORY_CONTROLLER_PUBLIC void sort_to_local_joint_order( std::shared_ptr trajectory_msg) const; - JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool validate_trajectory_msg(const trajectory_msgs::msg::JointTrajectory & trajectory) const; - JOINT_TRAJECTORY_CONTROLLER_PUBLIC void add_new_trajectory_msg( const std::shared_ptr & traj_msg); - JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool validate_trajectory_point_field( size_t joint_names_size, const std::vector & vector_field, const std::string & string_for_vector_field, size_t i, bool allow_empty) const; @@ -250,28 +228,22 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // the tolerances used for the current goal realtime_tools::RealtimeBuffer active_tolerances_; - JOINT_TRAJECTORY_CONTROLLER_PUBLIC void preempt_active_goal(); /** @brief set the current position with zero velocity and acceleration as new command */ - JOINT_TRAJECTORY_CONTROLLER_PUBLIC std::shared_ptr set_hold_position(); /** @brief set last trajectory point to be repeated at success * * no matter if it has nonzero velocity or acceleration */ - JOINT_TRAJECTORY_CONTROLLER_PUBLIC std::shared_ptr set_success_trajectory_point(); - JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool reset(); - JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool has_active_trajectory() const; - JOINT_TRAJECTORY_CONTROLLER_PUBLIC void publish_state( const rclcpp::Time & time, const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error); diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index 74d4e28f3a..fb699ca7a4 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -19,7 +19,6 @@ #include #include "joint_trajectory_controller/interpolation_methods.hpp" -#include "joint_trajectory_controller/visibility_control.h" #include "rclcpp/time.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" @@ -32,13 +31,10 @@ using TrajectoryPointConstIter = class Trajectory { public: - JOINT_TRAJECTORY_CONTROLLER_PUBLIC Trajectory(); - JOINT_TRAJECTORY_CONTROLLER_PUBLIC explicit Trajectory(std::shared_ptr joint_trajectory); - JOINT_TRAJECTORY_CONTROLLER_PUBLIC explicit Trajectory( const rclcpp::Time & current_time, const trajectory_msgs::msg::JointTrajectoryPoint & current_point, @@ -52,13 +48,11 @@ class Trajectory * \param joints_angle_wraparound Vector of boolean where true value corresponds to a joint that * wrap around (ie. is continuous). */ - JOINT_TRAJECTORY_CONTROLLER_PUBLIC void set_point_before_trajectory_msg( const rclcpp::Time & current_time, const trajectory_msgs::msg::JointTrajectoryPoint & current_point, const std::vector & joints_angle_wraparound = std::vector()); - JOINT_TRAJECTORY_CONTROLLER_PUBLIC void update(std::shared_ptr joint_trajectory); /// Find the segment (made up of 2 points) and its expected state from the @@ -99,7 +93,6 @@ class Trajectory * \param[in] search_monotonically_increasing If set to true, the next sample call will start * searching in the trajectory at the index of this call's result. */ - JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool sample( const rclcpp::Time & sample_time, const interpolation_methods::InterpolationMethod interpolation_method, @@ -128,34 +121,26 @@ class Trajectory * \param[in] sample_time The time to sample, between time_a and time_b. * \param[out] output The state at \p sample_time. */ - JOINT_TRAJECTORY_CONTROLLER_PUBLIC void interpolate_between_points( const rclcpp::Time & time_a, const trajectory_msgs::msg::JointTrajectoryPoint & state_a, const rclcpp::Time & time_b, const trajectory_msgs::msg::JointTrajectoryPoint & state_b, const rclcpp::Time & sample_time, trajectory_msgs::msg::JointTrajectoryPoint & output); - JOINT_TRAJECTORY_CONTROLLER_PUBLIC TrajectoryPointConstIter begin() const; - JOINT_TRAJECTORY_CONTROLLER_PUBLIC TrajectoryPointConstIter end() const; - JOINT_TRAJECTORY_CONTROLLER_PUBLIC rclcpp::Time time_from_start() const; - JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool has_trajectory_msg() const; - JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool has_nontrivial_msg() const; - JOINT_TRAJECTORY_CONTROLLER_PUBLIC std::shared_ptr get_trajectory_msg() const { return trajectory_msg_; } - JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool is_sampled_already() const { return sampled_already_; } /// Get the index of the segment start returned by the last \p sample() operation. @@ -163,7 +148,6 @@ class Trajectory * As the trajectory is only accessed at monotonically increasing sampling times, this index is * used to speed up the selection of relevant trajectory points. */ - JOINT_TRAJECTORY_CONTROLLER_PUBLIC size_t last_sample_index() const { return last_sample_idx_; } private: diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/visibility_control.h b/joint_trajectory_controller/include/joint_trajectory_controller/visibility_control.h deleted file mode 100644 index ec44da7a2f..0000000000 --- a/joint_trajectory_controller/include/joint_trajectory_controller/visibility_control.h +++ /dev/null @@ -1,56 +0,0 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. -// -// 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. - -/* This header must be included by all rclcpp headers which declare symbols - * which are defined in the rclcpp library. When not building the rclcpp - * library, i.e. when using the headers in other package's code, the contents - * of this header change the visibility of certain symbols which the rclcpp - * library cannot have, but the consuming code must have inorder to link. - */ - -#ifndef JOINT_TRAJECTORY_CONTROLLER__VISIBILITY_CONTROL_H_ -#define JOINT_TRAJECTORY_CONTROLLER__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define JOINT_TRAJECTORY_CONTROLLER_EXPORT __attribute__((dllexport)) -#define JOINT_TRAJECTORY_CONTROLLER_IMPORT __attribute__((dllimport)) -#else -#define JOINT_TRAJECTORY_CONTROLLER_EXPORT __declspec(dllexport) -#define JOINT_TRAJECTORY_CONTROLLER_IMPORT __declspec(dllimport) -#endif -#ifdef JOINT_TRAJECTORY_CONTROLLER_BUILDING_DLL -#define JOINT_TRAJECTORY_CONTROLLER_PUBLIC JOINT_TRAJECTORY_CONTROLLER_EXPORT -#else -#define JOINT_TRAJECTORY_CONTROLLER_PUBLIC JOINT_TRAJECTORY_CONTROLLER_IMPORT -#endif -#define JOINT_TRAJECTORY_CONTROLLER_PUBLIC_TYPE JOINT_TRAJECTORY_CONTROLLER_PUBLIC -#define JOINT_TRAJECTORY_CONTROLLER_LOCAL -#else -#define JOINT_TRAJECTORY_CONTROLLER_EXPORT __attribute__((visibility("default"))) -#define JOINT_TRAJECTORY_CONTROLLER_IMPORT -#if __GNUC__ >= 4 -#define JOINT_TRAJECTORY_CONTROLLER_PUBLIC __attribute__((visibility("default"))) -#define JOINT_TRAJECTORY_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) -#else -#define JOINT_TRAJECTORY_CONTROLLER_PUBLIC -#define JOINT_TRAJECTORY_CONTROLLER_LOCAL -#endif -#define JOINT_TRAJECTORY_CONTROLLER_PUBLIC_TYPE -#endif - -#endif // JOINT_TRAJECTORY_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/mecanum_drive_controller/CMakeLists.txt b/mecanum_drive_controller/CMakeLists.txt index 48bac58fe7..095e910dee 100644 --- a/mecanum_drive_controller/CMakeLists.txt +++ b/mecanum_drive_controller/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.16) -project(mecanum_drive_controller LANGUAGES CXX) +project(mecanum_drive_controller) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable @@ -7,6 +7,10 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") -Werror=missing-braces) endif() +# using this instead of visibility macros +# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053 +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) + # find dependencies set(THIS_PACKAGE_INCLUDE_DEPENDS controller_interface @@ -47,10 +51,6 @@ target_link_libraries(mecanum_drive_controller PUBLIC mecanum_drive_controller_parameters) ament_target_dependencies(mecanum_drive_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(mecanum_drive_controller PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL") - pluginlib_export_plugin_description_file( controller_interface mecanum_drive_controller.xml) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp index f835752d43..00d28ab4c8 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -25,7 +25,6 @@ #include "controller_interface/chainable_controller_interface.hpp" #include "mecanum_drive_controller/odometry.hpp" -#include "mecanum_drive_controller/visibility_control.h" #include "mecanum_drive_controller_parameters.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -51,35 +50,26 @@ static constexpr size_t NR_REF_ITFS = 3; class MecanumDriveController : public controller_interface::ChainableControllerInterface { public: - MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC MecanumDriveController(); - MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_init() override; - MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; - MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; - MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; - MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; - MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; - MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC controller_interface::return_type update_reference_from_subscribers( const rclcpp::Time & time, const rclcpp::Duration & period) override; - MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC controller_interface::return_type update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) override; @@ -149,7 +139,6 @@ class MecanumDriveController : public controller_interface::ChainableControllerI private: // callback for topic interface - MECANUM_DRIVE_CONTROLLER__VISIBILITY_LOCAL void reference_callback(const std::shared_ptr msg); double velocity_in_center_frame_linear_x_; // [m/s] diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h b/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h deleted file mode 100644 index 3222b2fa52..0000000000 --- a/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// 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 MECANUM_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_ -#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) -#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) -#else -#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) -#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) -#endif -#ifdef MECANUM_DRIVE_CONTROLLER__VISIBILITY_BUILDING_DLL -#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC MECANUM_DRIVE_CONTROLLER__VISIBILITY_EXPORT -#else -#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC MECANUM_DRIVE_CONTROLLER__VISIBILITY_IMPORT -#endif -#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC_TYPE MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC -#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_LOCAL -#else -#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) -#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_IMPORT -#if __GNUC__ >= 4 -#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) -#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) -#else -#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC -#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_LOCAL -#endif -#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC_TYPE -#endif - -#endif // MECANUM_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/parallel_gripper_controller/CMakeLists.txt b/parallel_gripper_controller/CMakeLists.txt index 3d9be8dfac..8c1018895d 100644 --- a/parallel_gripper_controller/CMakeLists.txt +++ b/parallel_gripper_controller/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.16) -project(parallel_gripper_controller LANGUAGES CXX) +project(parallel_gripper_controller) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp index 739d1c570a..f5f5e97939 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp @@ -36,7 +36,6 @@ #include "controller_interface/controller_interface.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "parallel_gripper_controller/visibility_control.hpp" #include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_server_goal_handle.hpp" @@ -66,39 +65,31 @@ class GripperActionController : public controller_interface::ControllerInterface double max_velocity_; // Desired max gripper velocity double max_effort_; // Desired max allowed effort }; - - GRIPPER_ACTION_CONTROLLER_PUBLIC GripperActionController(); + GripperActionController(); /** * @brief command_interface_configuration This controller requires the * position command interfaces for the controlled joints */ - GRIPPER_ACTION_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; /** * @brief command_interface_configuration This controller requires the * position and velocity state interfaces for the controlled joints */ - GRIPPER_ACTION_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; - GRIPPER_ACTION_CONTROLLER_PUBLIC controller_interface::return_type update( const rclcpp::Time & time, const rclcpp::Duration & period) override; - GRIPPER_ACTION_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_init() override; - GRIPPER_ACTION_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; - GRIPPER_ACTION_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; - GRIPPER_ACTION_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/visibility_control.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/visibility_control.hpp deleted file mode 100644 index 63e5c8e1c7..0000000000 --- a/parallel_gripper_controller/include/parallel_gripper_controller/visibility_control.hpp +++ /dev/null @@ -1,56 +0,0 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. -// -// 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. - -/* This header must be included by all rclcpp headers which declare symbols - * which are defined in the rclcpp library. When not building the rclcpp - * library, i.e. when using the headers in other package's code, the contents - * of this header change the visibility of certain symbols which the rclcpp - * library cannot have, but the consuming code must have inorder to link. - */ - -#ifndef PARALLEL_GRIPPER_CONTROLLER__VISIBILITY_CONTROL_HPP_ -#define PARALLEL_GRIPPER_CONTROLLER__VISIBILITY_CONTROL_HPP_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define GRIPPER_ACTION_CONTROLLER_EXPORT __attribute__((dllexport)) -#define GRIPPER_ACTION_CONTROLLER_IMPORT __attribute__((dllimport)) -#else -#define GRIPPER_ACTION_CONTROLLER_EXPORT __declspec(dllexport) -#define GRIPPER_ACTION_CONTROLLER_IMPORT __declspec(dllimport) -#endif -#ifdef GRIPPER_ACTION_CONTROLLER_BUILDING_DLL -#define GRIPPER_ACTION_CONTROLLER_PUBLIC GRIPPER_ACTION_CONTROLLER_EXPORT -#else -#define GRIPPER_ACTION_CONTROLLER_PUBLIC GRIPPER_ACTION_CONTROLLER_IMPORT -#endif -#define GRIPPER_ACTION_CONTROLLER_PUBLIC_TYPE GRIPPER_ACTION_CONTROLLER_PUBLIC -#define GRIPPER_ACTION_CONTROLLER_LOCAL -#else -#define GRIPPER_ACTION_CONTROLLER_EXPORT __attribute__((visibility("default"))) -#define GRIPPER_ACTION_CONTROLLER_IMPORT -#if __GNUC__ >= 4 -#define GRIPPER_ACTION_CONTROLLER_PUBLIC __attribute__((visibility("default"))) -#define GRIPPER_ACTION_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) -#else -#define GRIPPER_ACTION_CONTROLLER_PUBLIC -#define GRIPPER_ACTION_CONTROLLER_LOCAL -#endif -#define GRIPPER_ACTION_CONTROLLER_PUBLIC_TYPE -#endif - -#endif // PARALLEL_GRIPPER_CONTROLLER__VISIBILITY_CONTROL_HPP_ diff --git a/pid_controller/CMakeLists.txt b/pid_controller/CMakeLists.txt index ed9bdcd8cf..d1374d54cb 100644 --- a/pid_controller/CMakeLists.txt +++ b/pid_controller/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.16) -project(pid_controller LANGUAGES CXX) +project(pid_controller) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable @@ -7,6 +7,10 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") -Werror=missing-braces) endif() +# using this instead of visibility macros +# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053 +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) + if(WIN32) add_compile_definitions( # For math constants @@ -55,10 +59,6 @@ target_link_libraries(pid_controller PUBLIC ) ament_target_dependencies(pid_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(pid_controller PRIVATE "PID_CONTROLLER__VISIBILITY_BUILDING_DLL") - pluginlib_export_plugin_description_file(controller_interface pid_controller.xml) if(BUILD_TESTING) diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index 0aba6cf849..afd5555c0c 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -26,7 +26,6 @@ #include "control_msgs/msg/multi_dof_state_stamped.hpp" #include "control_toolbox/pid_ros.hpp" #include "controller_interface/chainable_controller_interface.hpp" -#include "pid_controller/visibility_control.h" #include "pid_controller_parameters.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.hpp" @@ -45,39 +44,29 @@ enum class feedforward_mode_type : std::uint8_t class PidController : public controller_interface::ChainableControllerInterface { public: - PID_CONTROLLER__VISIBILITY_PUBLIC PidController(); - PID_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_init() override; - PID_CONTROLLER__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; - PID_CONTROLLER__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; - PID_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_cleanup( const rclcpp_lifecycle::State & previous_state) override; - PID_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; - PID_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; - PID_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; - PID_CONTROLLER__VISIBILITY_PUBLIC controller_interface::return_type update_reference_from_subscribers( const rclcpp::Time & time, const rclcpp::Duration & period) override; - PID_CONTROLLER__VISIBILITY_PUBLIC controller_interface::return_type update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) override; @@ -127,7 +116,6 @@ class PidController : public controller_interface::ChainableControllerInterface private: // callback for topic interface - PID_CONTROLLER__VISIBILITY_LOCAL void reference_callback(const std::shared_ptr msg); }; diff --git a/pid_controller/include/pid_controller/visibility_control.h b/pid_controller/include/pid_controller/visibility_control.h deleted file mode 100644 index 1509364b5a..0000000000 --- a/pid_controller/include/pid_controller/visibility_control.h +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// 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 PID_CONTROLLER__VISIBILITY_CONTROL_H_ -#define PID_CONTROLLER__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define PID_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) -#define PID_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) -#else -#define PID_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) -#define PID_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) -#endif -#ifdef PID_CONTROLLER__VISIBILITY_BUILDING_DLL -#define PID_CONTROLLER__VISIBILITY_PUBLIC PID_CONTROLLER__VISIBILITY_EXPORT -#else -#define PID_CONTROLLER__VISIBILITY_PUBLIC PID_CONTROLLER__VISIBILITY_IMPORT -#endif -#define PID_CONTROLLER__VISIBILITY_PUBLIC_TYPE PID_CONTROLLER__VISIBILITY_PUBLIC -#define PID_CONTROLLER__VISIBILITY_LOCAL -#else -#define PID_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) -#define PID_CONTROLLER__VISIBILITY_IMPORT -#if __GNUC__ >= 4 -#define PID_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) -#define PID_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) -#else -#define PID_CONTROLLER__VISIBILITY_PUBLIC -#define PID_CONTROLLER__VISIBILITY_LOCAL -#endif -#define PID_CONTROLLER__VISIBILITY_PUBLIC_TYPE -#endif - -#endif // PID_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/pose_broadcaster/CMakeLists.txt b/pose_broadcaster/CMakeLists.txt index 3cc434d2d0..2375779e33 100644 --- a/pose_broadcaster/CMakeLists.txt +++ b/pose_broadcaster/CMakeLists.txt @@ -1,8 +1,5 @@ cmake_minimum_required(VERSION 3.16) -project(pose_broadcaster - LANGUAGES - CXX -) +project(pose_broadcaster) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable @@ -10,6 +7,10 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") -Werror=missing-braces) endif() +# using this instead of visibility macros +# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053 +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) + set(THIS_PACKAGE_INCLUDE_DEPENDS controller_interface generate_parameter_library @@ -49,10 +50,6 @@ ament_target_dependencies(pose_broadcaster PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS} ) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(pose_broadcaster PRIVATE "POSE_BROADCASTER_BUILDING_DLL") - pluginlib_export_plugin_description_file( controller_interface pose_broadcaster.xml ) diff --git a/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp index 9317fce5e1..024aa41602 100644 --- a/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp +++ b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp @@ -21,7 +21,6 @@ #include "controller_interface/controller_interface.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "pose_broadcaster/visibility_control.h" #include "pose_broadcaster_parameters.hpp" #include "rclcpp/publisher.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -35,24 +34,22 @@ namespace pose_broadcaster class PoseBroadcaster : public controller_interface::ControllerInterface { public: - POSE_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration - command_interface_configuration() const override; + controller_interface::InterfaceConfiguration command_interface_configuration() const override; - POSE_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration - state_interface_configuration() const override; + controller_interface::InterfaceConfiguration state_interface_configuration() const override; - POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_init() override; + controller_interface::CallbackReturn on_init() override; - POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_configure( + controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; - POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_activate( + controller_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; - POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_deactivate( + controller_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; - POSE_BROADCASTER_PUBLIC controller_interface::return_type update( + controller_interface::return_type update( const rclcpp::Time & time, const rclcpp::Duration & period) override; private: diff --git a/pose_broadcaster/include/pose_broadcaster/visibility_control.h b/pose_broadcaster/include/pose_broadcaster/visibility_control.h deleted file mode 100644 index 5ce272658d..0000000000 --- a/pose_broadcaster/include/pose_broadcaster/visibility_control.h +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright 2024 FZI Forschungszentrum Informatik -// -// 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 POSE_BROADCASTER__VISIBILITY_CONTROL_H_ -#define POSE_BROADCASTER__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define POSE_BROADCASTER_EXPORT __attribute__((dllexport)) -#define POSE_BROADCASTER_IMPORT __attribute__((dllimport)) -#else -#define POSE_BROADCASTER_EXPORT __declspec(dllexport) -#define POSE_BROADCASTER_IMPORT __declspec(dllimport) -#endif -#ifdef POSE_BROADCASTER_BUILDING_DLL -#define POSE_BROADCASTER_PUBLIC POSE_BROADCASTER_EXPORT -#else -#define POSE_BROADCASTER_PUBLIC POSE_BROADCASTER_IMPORT -#endif -#define POSE_BROADCASTER_PUBLIC_TYPE POSE_BROADCASTER_PUBLIC -#define POSE_BROADCASTER_LOCAL -#else -#define POSE_BROADCASTER_EXPORT __attribute__((visibility("default"))) -#define POSE_BROADCASTER_IMPORT -#if __GNUC__ >= 4 -#define POSE_BROADCASTER_PUBLIC __attribute__((visibility("default"))) -#define POSE_BROADCASTER_LOCAL __attribute__((visibility("hidden"))) -#else -#define POSE_BROADCASTER_PUBLIC -#define POSE_BROADCASTER_LOCAL -#endif -#define POSE_BROADCASTER_PUBLIC_TYPE -#endif - -#endif // POSE_BROADCASTER__VISIBILITY_CONTROL_H_ diff --git a/position_controllers/CMakeLists.txt b/position_controllers/CMakeLists.txt index e76b76555e..71ffd0eeba 100644 --- a/position_controllers/CMakeLists.txt +++ b/position_controllers/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.16) -project(position_controllers LANGUAGES CXX) +project(position_controllers) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable @@ -7,6 +7,10 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") -Werror=missing-braces) endif() +# using this instead of visibility macros +# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053 +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) + set(THIS_PACKAGE_INCLUDE_DEPENDS forward_command_controller pluginlib @@ -29,9 +33,6 @@ target_include_directories(position_controllers PUBLIC ) ament_target_dependencies(position_controllers PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(position_controllers PRIVATE "POSITION_CONTROLLERS_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface position_controllers_plugins.xml) if(BUILD_TESTING) diff --git a/position_controllers/include/position_controllers/joint_group_position_controller.hpp b/position_controllers/include/position_controllers/joint_group_position_controller.hpp index 4eaf9086e4..ceaf7639ed 100644 --- a/position_controllers/include/position_controllers/joint_group_position_controller.hpp +++ b/position_controllers/include/position_controllers/joint_group_position_controller.hpp @@ -16,7 +16,6 @@ #define POSITION_CONTROLLERS__JOINT_GROUP_POSITION_CONTROLLER_HPP_ #include "forward_command_controller/forward_command_controller.hpp" -#include "position_controllers/visibility_control.h" namespace position_controllers { @@ -33,10 +32,9 @@ namespace position_controllers class JointGroupPositionController : public forward_command_controller::ForwardCommandController { public: - POSITION_CONTROLLERS_PUBLIC JointGroupPositionController(); - POSITION_CONTROLLERS_PUBLIC controller_interface::CallbackReturn on_init() override; + controller_interface::CallbackReturn on_init() override; }; } // namespace position_controllers diff --git a/position_controllers/include/position_controllers/visibility_control.h b/position_controllers/include/position_controllers/visibility_control.h deleted file mode 100644 index cac7aebc5a..0000000000 --- a/position_controllers/include/position_controllers/visibility_control.h +++ /dev/null @@ -1,56 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// 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. - -/* This header must be included by all rclcpp headers which declare symbols - * which are defined in the rclcpp library. When not building the rclcpp - * library, i.e. when using the headers in other package's code, the contents - * of this header change the visibility of certain symbols which the rclcpp - * library cannot have, but the consuming code must have inorder to link. - */ - -#ifndef POSITION_CONTROLLERS__VISIBILITY_CONTROL_H_ -#define POSITION_CONTROLLERS__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define POSITION_CONTROLLERS_EXPORT __attribute__((dllexport)) -#define POSITION_CONTROLLERS_IMPORT __attribute__((dllimport)) -#else -#define POSITION_CONTROLLERS_EXPORT __declspec(dllexport) -#define POSITION_CONTROLLERS_IMPORT __declspec(dllimport) -#endif -#ifdef POSITION_CONTROLLERS_BUILDING_DLL -#define POSITION_CONTROLLERS_PUBLIC POSITION_CONTROLLERS_EXPORT -#else -#define POSITION_CONTROLLERS_PUBLIC POSITION_CONTROLLERS_IMPORT -#endif -#define POSITION_CONTROLLERS_PUBLIC_TYPE POSITION_CONTROLLERS_PUBLIC -#define POSITION_CONTROLLERS_LOCAL -#else -#define POSITION_CONTROLLERS_EXPORT __attribute__((visibility("default"))) -#define POSITION_CONTROLLERS_IMPORT -#if __GNUC__ >= 4 -#define POSITION_CONTROLLERS_PUBLIC __attribute__((visibility("default"))) -#define POSITION_CONTROLLERS_LOCAL __attribute__((visibility("hidden"))) -#else -#define POSITION_CONTROLLERS_PUBLIC -#define POSITION_CONTROLLERS_LOCAL -#endif -#define POSITION_CONTROLLERS_PUBLIC_TYPE -#endif - -#endif // POSITION_CONTROLLERS__VISIBILITY_CONTROL_H_ diff --git a/range_sensor_broadcaster/CMakeLists.txt b/range_sensor_broadcaster/CMakeLists.txt index d70614ea53..744611bb65 100644 --- a/range_sensor_broadcaster/CMakeLists.txt +++ b/range_sensor_broadcaster/CMakeLists.txt @@ -13,6 +13,10 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") -Werror=missing-braces) endif() +# using this instead of visibility macros +# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053 +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) + set(THIS_PACKAGE_INCLUDE_DEPENDS controller_interface generate_parameter_library @@ -45,9 +49,6 @@ target_include_directories(range_sensor_broadcaster PRIVATE $ ) ament_target_dependencies(range_sensor_broadcaster ${THIS_PACKAGE_INCLUDE_DEPENDS}) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(range_sensor_broadcaster PRIVATE "RANGE_SENSOR_BROADCASTER_BUILDING_DLL") target_link_libraries(range_sensor_broadcaster range_sensor_broadcaster_parameters ) diff --git a/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp b/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp index 2e4e47b018..f1e2c8e72f 100644 --- a/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp +++ b/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp @@ -22,7 +22,6 @@ #include #include "controller_interface/controller_interface.hpp" -#include "range_sensor_broadcaster/visibility_control.h" #include "range_sensor_broadcaster_parameters.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_publisher.hpp" @@ -34,27 +33,20 @@ namespace range_sensor_broadcaster class RangeSensorBroadcaster : public controller_interface::ControllerInterface { public: - RANGE_SENSOR_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; - RANGE_SENSOR_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; + controller_interface::CallbackReturn on_init() override; - RANGE_SENSOR_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_init() override; - - RANGE_SENSOR_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; - RANGE_SENSOR_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; - RANGE_SENSOR_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; - RANGE_SENSOR_BROADCASTER_PUBLIC controller_interface::return_type update( const rclcpp::Time & time, const rclcpp::Duration & period) override; diff --git a/range_sensor_broadcaster/include/range_sensor_broadcaster/visibility_control.h b/range_sensor_broadcaster/include/range_sensor_broadcaster/visibility_control.h deleted file mode 100644 index 0a9a9f53a8..0000000000 --- a/range_sensor_broadcaster/include/range_sensor_broadcaster/visibility_control.h +++ /dev/null @@ -1,53 +0,0 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// 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. - -/* - * Author: Subhas Das, Denis Stogl - */ - -#ifndef RANGE_SENSOR_BROADCASTER__VISIBILITY_CONTROL_H_ -#define RANGE_SENSOR_BROADCASTER__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define RANGE_SENSOR_BROADCASTER_EXPORT __attribute__((dllexport)) -#define RANGE_SENSOR_BROADCASTER_IMPORT __attribute__((dllimport)) -#else -#define RANGE_SENSOR_BROADCASTER_EXPORT __declspec(dllexport) -#define RANGE_SENSOR_BROADCASTER_IMPORT __declspec(dllimport) -#endif -#ifdef RANGE_SENSOR_BROADCASTER_BUILDING_DLL -#define RANGE_SENSOR_BROADCASTER_PUBLIC RANGE_SENSOR_BROADCASTER_EXPORT -#else -#define RANGE_SENSOR_BROADCASTER_PUBLIC RANGE_SENSOR_BROADCASTER_IMPORT -#endif -#define RANGE_SENSOR_BROADCASTER_PUBLIC_TYPE RANGE_SENSOR_BROADCASTER_PUBLIC -#define RANGE_SENSOR_BROADCASTER_LOCAL -#else -#define RANGE_SENSOR_BROADCASTER_EXPORT __attribute__((visibility("default"))) -#define RANGE_SENSOR_BROADCASTER_IMPORT -#if __GNUC__ >= 4 -#define RANGE_SENSOR_BROADCASTER_PUBLIC __attribute__((visibility("default"))) -#define RANGE_SENSOR_BROADCASTER_LOCAL __attribute__((visibility("hidden"))) -#else -#define RANGE_SENSOR_BROADCASTER_PUBLIC -#define RANGE_SENSOR_BROADCASTER_LOCAL -#endif -#define RANGE_SENSOR_BROADCASTER_PUBLIC_TYPE -#endif - -#endif // RANGE_SENSOR_BROADCASTER__VISIBILITY_CONTROL_H_ diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt index fc79d54b7c..2e80ed198f 100644 --- a/steering_controllers_library/CMakeLists.txt +++ b/steering_controllers_library/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.16) -project(steering_controllers_library LANGUAGES CXX) +project(steering_controllers_library) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable @@ -7,6 +7,10 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") -Werror=missing-braces) endif() +# using this instead of visibility macros +# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053 +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) + # find dependencies set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs @@ -52,10 +56,6 @@ target_link_libraries(steering_controllers_library PUBLIC steering_controllers_library_parameters) ament_target_dependencies(steering_controllers_library PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(steering_controllers_library PRIVATE "STEERING_CONTROLLERS__VISIBILITY_BUILDING_DLL" "_USE_MATH_DEFINES") - if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index 49236986ee..e3a415c374 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -26,7 +26,6 @@ #include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" #include "steering_controllers_library/steering_odometry.hpp" -#include "steering_controllers_library/visibility_control.h" #include "steering_controllers_library_parameters.hpp" // TODO(anyone): Replace with controller specific messages @@ -41,40 +40,34 @@ namespace steering_controllers_library class SteeringControllersLibrary : public controller_interface::ChainableControllerInterface { public: - STEERING_CONTROLLERS__VISIBILITY_PUBLIC SteeringControllersLibrary(); + SteeringControllersLibrary(); - virtual STEERING_CONTROLLERS__VISIBILITY_PUBLIC void - initialize_implementation_parameter_listener() = 0; + virtual void initialize_implementation_parameter_listener() = 0; - STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_init() override; + controller_interface::CallbackReturn on_init() override; - STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration - command_interface_configuration() const override; + controller_interface::InterfaceConfiguration command_interface_configuration() const override; - STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration - state_interface_configuration() const override; + controller_interface::InterfaceConfiguration state_interface_configuration() const override; - virtual STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn - configure_odometry() = 0; + virtual controller_interface::CallbackReturn configure_odometry() = 0; - virtual STEERING_CONTROLLERS__VISIBILITY_PUBLIC bool update_odometry( - const rclcpp::Duration & period) = 0; + virtual bool update_odometry(const rclcpp::Duration & period) = 0; - STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_configure( + controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; - STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_activate( + controller_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; - STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_deactivate( + controller_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; - STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::return_type - update_reference_from_subscribers( + controller_interface::return_type update_reference_from_subscribers( const rclcpp::Time & time, const rclcpp::Duration & period) override; - STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::return_type - update_and_write_commands(const rclcpp::Time & time, const rclcpp::Duration & period) override; + controller_interface::return_type update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) override; using ControllerAckermannReferenceMsg = ackermann_msgs::msg::AckermannDriveStamped; using ControllerTwistReferenceMsg = geometry_msgs::msg::TwistStamped; @@ -134,8 +127,7 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl private: // callback for topic interface - STEERING_CONTROLLERS__VISIBILITY_LOCAL void reference_callback( - const std::shared_ptr msg); + void reference_callback(const std::shared_ptr msg); }; } // namespace steering_controllers_library diff --git a/steering_controllers_library/include/steering_controllers_library/visibility_control.h b/steering_controllers_library/include/steering_controllers_library/visibility_control.h deleted file mode 100644 index 123662031b..0000000000 --- a/steering_controllers_library/include/steering_controllers_library/visibility_control.h +++ /dev/null @@ -1,50 +0,0 @@ -// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// 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 STEERING_CONTROLLERS_LIBRARY__VISIBILITY_CONTROL_H_ -#define STEERING_CONTROLLERS_LIBRARY__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define STEERING_CONTROLLERS__VISIBILITY_EXPORT __attribute__((dllexport)) -#define STEERING_CONTROLLERS__VISIBILITY_IMPORT __attribute__((dllimport)) -#else -#define STEERING_CONTROLLERS__VISIBILITY_EXPORT __declspec(dllexport) -#define STEERING_CONTROLLERS__VISIBILITY_IMPORT __declspec(dllimport) -#endif -#ifdef STEERING_CONTROLLERS__VISIBILITY_BUILDING_DLL -#define STEERING_CONTROLLERS__VISIBILITY_PUBLIC STEERING_CONTROLLERS__VISIBILITY_EXPORT -#else -#define STEERING_CONTROLLERS__VISIBILITY_PUBLIC STEERING_CONTROLLERS__VISIBILITY_IMPORT -#endif -#define STEERING_CONTROLLERS__VISIBILITY_PUBLIC_TYPE STEERING_CONTROLLERS__VISIBILITY_PUBLIC -#define STEERING_CONTROLLERS__VISIBILITY_LOCAL -#else -#define STEERING_CONTROLLERS__VISIBILITY_EXPORT __attribute__((visibility("default"))) -#define STEERING_CONTROLLERS__VISIBILITY_IMPORT -#if __GNUC__ >= 4 -#define STEERING_CONTROLLERS__VISIBILITY_PUBLIC __attribute__((visibility("default"))) -#define STEERING_CONTROLLERS__VISIBILITY_PROTECTED __attribute__((visibility("protected"))) -#define STEERING_CONTROLLERS__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) -#else -#define STEERING_CONTROLLERS__VISIBILITY_PUBLIC -#define STEERING_CONTROLLERS__VISIBILITY_LOCAL -#endif -#define STEERING_CONTROLLERS__VISIBILITY_PUBLIC_TYPE -#endif - -#endif // STEERING_CONTROLLERS_LIBRARY__VISIBILITY_CONTROL_H_ diff --git a/tricycle_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt index ee877272ab..b5669530dc 100644 --- a/tricycle_controller/CMakeLists.txt +++ b/tricycle_controller/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.16) -project(tricycle_controller LANGUAGES CXX) +project(tricycle_controller) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable @@ -7,6 +7,10 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") -Werror=missing-braces) endif() +# using this instead of visibility macros +# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053 +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) + set(THIS_PACKAGE_INCLUDE_DEPENDS ackermann_msgs builtin_interfaces @@ -50,9 +54,6 @@ target_include_directories(tricycle_controller PUBLIC ) target_link_libraries(tricycle_controller PUBLIC tricycle_controller_parameters) ament_target_dependencies(tricycle_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(tricycle_controller PRIVATE "TRICYCLE_CONTROLLER_BUILDING_DLL" "_USE_MATH_DEFINES") pluginlib_export_plugin_description_file(controller_interface tricycle_controller.xml) diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index 4d64549613..86793fd435 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -40,7 +40,6 @@ #include "tricycle_controller/odometry.hpp" #include "tricycle_controller/steering_limiter.hpp" #include "tricycle_controller/traction_limiter.hpp" -#include "tricycle_controller/visibility_control.h" // auto-generated by generate_parameter_library #include "tricycle_controller_parameters.hpp" @@ -56,38 +55,27 @@ class TricycleController : public controller_interface::ControllerInterface using AckermannDrive = ackermann_msgs::msg::AckermannDrive; public: - TRICYCLE_CONTROLLER_PUBLIC TricycleController(); - TRICYCLE_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; - TRICYCLE_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; - TRICYCLE_CONTROLLER_PUBLIC controller_interface::return_type update( const rclcpp::Time & time, const rclcpp::Duration & period) override; - TRICYCLE_CONTROLLER_PUBLIC CallbackReturn on_init() override; - TRICYCLE_CONTROLLER_PUBLIC CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; - TRICYCLE_CONTROLLER_PUBLIC CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; - TRICYCLE_CONTROLLER_PUBLIC CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; - TRICYCLE_CONTROLLER_PUBLIC CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; - TRICYCLE_CONTROLLER_PUBLIC CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override; - TRICYCLE_CONTROLLER_PUBLIC CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override; protected: diff --git a/tricycle_controller/include/tricycle_controller/visibility_control.h b/tricycle_controller/include/tricycle_controller/visibility_control.h deleted file mode 100644 index bc9b34898b..0000000000 --- a/tricycle_controller/include/tricycle_controller/visibility_control.h +++ /dev/null @@ -1,53 +0,0 @@ -// Copyright 2022 Pixel Robotics. -// -// 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. - -/* - * Author: Tony Najjar - */ - -#ifndef TRICYCLE_CONTROLLER__VISIBILITY_CONTROL_H_ -#define TRICYCLE_CONTROLLER__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define TRICYCLE_CONTROLLER_EXPORT __attribute__((dllexport)) -#define TRICYCLE_CONTROLLER_IMPORT __attribute__((dllimport)) -#else -#define TRICYCLE_CONTROLLER_EXPORT __declspec(dllexport) -#define TRICYCLE_CONTROLLER_IMPORT __declspec(dllimport) -#endif -#ifdef TRICYCLE_CONTROLLER_BUILDING_DLL -#define TRICYCLE_CONTROLLER_PUBLIC TRICYCLE_CONTROLLER_EXPORT -#else -#define TRICYCLE_CONTROLLER_PUBLIC TRICYCLE_CONTROLLER_IMPORT -#endif -#define TRICYCLE_CONTROLLER_PUBLIC_TYPE TRICYCLE_CONTROLLER_PUBLIC -#define TRICYCLE_CONTROLLER_LOCAL -#else -#define TRICYCLE_CONTROLLER_EXPORT __attribute__((visibility("default"))) -#define TRICYCLE_CONTROLLER_IMPORT -#if __GNUC__ >= 4 -#define TRICYCLE_CONTROLLER_PUBLIC __attribute__((visibility("default"))) -#define TRICYCLE_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) -#else -#define TRICYCLE_CONTROLLER_PUBLIC -#define TRICYCLE_CONTROLLER_LOCAL -#endif -#define TRICYCLE_CONTROLLER_PUBLIC_TYPE -#endif - -#endif // TRICYCLE_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/tricycle_steering_controller/CMakeLists.txt b/tricycle_steering_controller/CMakeLists.txt index 9c82ee3574..9dbf5e3543 100644 --- a/tricycle_steering_controller/CMakeLists.txt +++ b/tricycle_steering_controller/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.16) -project(tricycle_steering_controller LANGUAGES CXX) +project(tricycle_steering_controller) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable @@ -7,6 +7,10 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") -Werror=missing-braces) endif() +# using this instead of visibility macros +# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053 +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) + # find dependencies set(THIS_PACKAGE_INCLUDE_DEPENDS controller_interface @@ -45,10 +49,6 @@ target_link_libraries(tricycle_steering_controller PUBLIC tricycle_steering_controller_parameters) ament_target_dependencies(tricycle_steering_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(tricycle_steering_controller PRIVATE "TRICYCLE_STEERING_CONTROLLER__VISIBILITY_BUILDING_DLL") - pluginlib_export_plugin_description_file( controller_interface tricycle_steering_controller.xml) diff --git a/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp b/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp index 559de6a223..28e477a578 100644 --- a/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp @@ -21,7 +21,6 @@ #include #include "steering_controllers_library/steering_controllers_library.hpp" -#include "tricycle_steering_controller/visibility_control.h" #include "tricycle_steering_controller_parameters.hpp" namespace tricycle_steering_controller @@ -45,14 +44,11 @@ class TricycleSteeringController : public steering_controllers_library::Steering public: TricycleSteeringController(); - TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn - configure_odometry() override; + controller_interface::CallbackReturn configure_odometry() override; - TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC bool update_odometry( - const rclcpp::Duration & period) override; + bool update_odometry(const rclcpp::Duration & period) override; - TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC void - initialize_implementation_parameter_listener() override; + void initialize_implementation_parameter_listener() override; protected: std::shared_ptr tricycle_param_listener_; diff --git a/tricycle_steering_controller/include/tricycle_steering_controller/visibility_control.h b/tricycle_steering_controller/include/tricycle_steering_controller/visibility_control.h deleted file mode 100644 index 606b067ad8..0000000000 --- a/tricycle_steering_controller/include/tricycle_steering_controller/visibility_control.h +++ /dev/null @@ -1,52 +0,0 @@ -// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// 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 TRICYCLE_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ -#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) -#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) -#else -#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) -#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) -#endif -#ifdef TRICYCLE_STEERING_CONTROLLER__VISIBILITY_BUILDING_DLL -#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ - TRICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT -#else -#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ - TRICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT -#endif -#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE \ - TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC -#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_LOCAL -#else -#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) -#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT -#if __GNUC__ >= 4 -#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) -#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) -#else -#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC -#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_LOCAL -#endif -#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE -#endif - -#endif // TRICYCLE_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/velocity_controllers/CMakeLists.txt b/velocity_controllers/CMakeLists.txt index feb4eae74f..e7df93d2ab 100644 --- a/velocity_controllers/CMakeLists.txt +++ b/velocity_controllers/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.16) -project(velocity_controllers LANGUAGES CXX) +project(velocity_controllers) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable @@ -7,6 +7,10 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") -Werror=missing-braces) endif() +# using this instead of visibility macros +# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053 +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) + set(THIS_PACKAGE_INCLUDE_DEPENDS forward_command_controller pluginlib @@ -29,10 +33,6 @@ target_include_directories(velocity_controllers PUBLIC ) ament_target_dependencies(velocity_controllers PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(velocity_controllers PRIVATE "VELOCITY_CONTROLLERS_BUILDING_DLL") - pluginlib_export_plugin_description_file(controller_interface velocity_controllers_plugins.xml) if(BUILD_TESTING) diff --git a/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp b/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp index e443ba76cc..4cb0f338ea 100644 --- a/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp +++ b/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp @@ -16,7 +16,6 @@ #define VELOCITY_CONTROLLERS__JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ #include "forward_command_controller/forward_command_controller.hpp" -#include "velocity_controllers/visibility_control.h" namespace velocity_controllers { @@ -33,12 +32,10 @@ namespace velocity_controllers class JointGroupVelocityController : public forward_command_controller::ForwardCommandController { public: - VELOCITY_CONTROLLERS_PUBLIC JointGroupVelocityController(); - VELOCITY_CONTROLLERS_PUBLIC controller_interface::CallbackReturn on_init() override; + controller_interface::CallbackReturn on_init() override; - VELOCITY_CONTROLLERS_PUBLIC controller_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; }; diff --git a/velocity_controllers/include/velocity_controllers/visibility_control.h b/velocity_controllers/include/velocity_controllers/visibility_control.h deleted file mode 100644 index 08b4ecc588..0000000000 --- a/velocity_controllers/include/velocity_controllers/visibility_control.h +++ /dev/null @@ -1,56 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// 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. - -/* This header must be included by all rclcpp headers which declare symbols - * which are defined in the rclcpp library. When not building the rclcpp - * library, i.e. when using the headers in other package's code, the contents - * of this header change the visibility of certain symbols which the rclcpp - * library cannot have, but the consuming code must have inorder to link. - */ - -#ifndef VELOCITY_CONTROLLERS__VISIBILITY_CONTROL_H_ -#define VELOCITY_CONTROLLERS__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define VELOCITY_CONTROLLERS_EXPORT __attribute__((dllexport)) -#define VELOCITY_CONTROLLERS_IMPORT __attribute__((dllimport)) -#else -#define VELOCITY_CONTROLLERS_EXPORT __declspec(dllexport) -#define VELOCITY_CONTROLLERS_IMPORT __declspec(dllimport) -#endif -#ifdef VELOCITY_CONTROLLERS_BUILDING_DLL -#define VELOCITY_CONTROLLERS_PUBLIC VELOCITY_CONTROLLERS_EXPORT -#else -#define VELOCITY_CONTROLLERS_PUBLIC VELOCITY_CONTROLLERS_IMPORT -#endif -#define VELOCITY_CONTROLLERS_PUBLIC_TYPE VELOCITY_CONTROLLERS_PUBLIC -#define VELOCITY_CONTROLLERS_LOCAL -#else -#define VELOCITY_CONTROLLERS_EXPORT __attribute__((visibility("default"))) -#define VELOCITY_CONTROLLERS_IMPORT -#if __GNUC__ >= 4 -#define VELOCITY_CONTROLLERS_PUBLIC __attribute__((visibility("default"))) -#define VELOCITY_CONTROLLERS_LOCAL __attribute__((visibility("hidden"))) -#else -#define VELOCITY_CONTROLLERS_PUBLIC -#define VELOCITY_CONTROLLERS_LOCAL -#endif -#define VELOCITY_CONTROLLERS_PUBLIC_TYPE -#endif - -#endif // VELOCITY_CONTROLLERS__VISIBILITY_CONTROL_H_ From 97d747513176bb1ce30e940f1ff6913623e55684 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Wed, 1 Jan 2025 10:47:02 +0100 Subject: [PATCH 46/84] Bump version of pre-commit hooks (#1458) Co-authored-by: christophfroehlich <3367244+christophfroehlich@users.noreply.github.com> --- .pre-commit-config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 75c5402ffe..feefcc317d 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -37,7 +37,7 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v3.19.0 + rev: v3.19.1 hooks: - id: pyupgrade args: [--py36-plus] @@ -63,7 +63,7 @@ repos: # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v19.1.4 + rev: v19.1.5 hooks: - id: clang-format args: ['-fallback-style=none', '-i'] From 2375aca137a00b8d162babf5f8dbec2c35be55ba Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 1 Jan 2025 20:09:55 +0100 Subject: [PATCH 47/84] Fix open-loop odometry in case of ref timeout (#1454) Co-authored-by: Dr. Denis --- .../src/steering_controllers_library.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 3d4164080c..2d47746fed 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -378,23 +378,23 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1])) { - // store (for open loop odometry) and set commands - last_linear_velocity_ = reference_interfaces_[0]; - last_angular_velocity_ = reference_interfaces_[1]; - const auto age_of_last_command = time - (*(input_ref_.readFromRT()))->header.stamp; const auto timeout = age_of_last_command > ref_timeout_ && ref_timeout_ != rclcpp::Duration::from_seconds(0); + // store (for open loop odometry) and set commands + last_linear_velocity_ = timeout ? 0.0 : reference_interfaces_[0]; + last_angular_velocity_ = timeout ? 0.0 : reference_interfaces_[1]; + auto [traction_commands, steering_commands] = odometry_.get_commands( - last_linear_velocity_, last_angular_velocity_, params_.open_loop, + reference_interfaces_[0], reference_interfaces_[1], params_.open_loop, params_.reduce_wheel_speed_until_steering_reached); if (params_.front_steering) { for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) { - command_interfaces_[i].set_value(timeout ? 0. : traction_commands[i]); + command_interfaces_[i].set_value(timeout ? 0.0 : traction_commands[i]); } for (size_t i = 0; i < params_.front_wheels_names.size(); i++) { @@ -406,7 +406,7 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c { for (size_t i = 0; i < params_.front_wheels_names.size(); i++) { - command_interfaces_[i].set_value(timeout ? 0. : traction_commands[i]); + command_interfaces_[i].set_value(timeout ? 0.0 : traction_commands[i]); } for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) { From 214e333e7c1e7c228c80650943094ed665ff923c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 2 Jan 2025 08:43:11 +0100 Subject: [PATCH 48/84] Remove visibility include from docs (#1462) --- doc/writing_new_controller.rst | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/doc/writing_new_controller.rst b/doc/writing_new_controller.rst index 5aa7068b98..da269d982a 100644 --- a/doc/writing_new_controller.rst +++ b/doc/writing_new_controller.rst @@ -22,14 +22,12 @@ The following is a step-by-step guide to create source files, basic tests, and c After creating the package, you should have at least ``CMakeLists.txt`` and ``package.xml`` files in it. Create also ``include//`` and ``src`` folders if they do not already exist. In ``include//`` folder add ``.hpp`` and ``.cpp`` in the ``src`` folder. - Optionally add ``visibility_control.h`` with the definition of export rules for Windows. - You can copy this file from an existing controller package and change the name prefix to the ````. 3. **Adding declarations into header file (.hpp)** 1. Take care that you use header guards. ROS 2-style is using ``#ifndef`` and ``#define`` preprocessor directives. (For more information on this, a search engine is your friend :) ). - 2. include ``"controller_interface/controller_interface.hpp"`` and ``visibility_control.h`` if you are using one. + 2. include ``"controller_interface/controller_interface.hpp"``. 3. Define a unique namespace for your controller. This is usually a package name written in ``snake_case``. From 0f2a683156e946cb3f1f3e1b1ae1092503a51ab9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 2 Jan 2025 12:41:22 +0100 Subject: [PATCH 49/84] Fix typos in steering_controllers_lib (#1464) --- .../src/ackermann_steering_controller.cpp | 2 +- .../test/test_ackermann_steering_controller.hpp | 2 +- .../test/test_bicycle_steering_controller.hpp | 2 +- .../steering_controllers_library.hpp | 8 ++++---- .../src/steering_controllers_library.cpp | 2 +- .../test/test_steering_controllers_library.hpp | 4 ++-- .../test/test_tricycle_steering_controller.hpp | 2 +- 7 files changed, 11 insertions(+), 11 deletions(-) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index d9d95bf8b5..77ba55812a 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -50,7 +50,7 @@ controller_interface::CallbackReturn AckermannSteeringController::configure_odom set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); - RCLCPP_INFO(get_node()->get_logger(), "ackermann odom configure successful"); + RCLCPP_INFO(get_node()->get_logger(), "Ackermann odom configure successful"); return controller_interface::CallbackReturn::SUCCESS; } diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index c45601d701..2e7d688471 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -32,7 +32,7 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using ControllerStateMsg = - steering_controllers_library::SteeringControllersLibrary::AckermanControllerState; + steering_controllers_library::SteeringControllersLibrary::AckermannControllerState; using ControllerReferenceMsg = steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 0253078bb9..25fec7fdf1 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -32,7 +32,7 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using ControllerStateMsg = - steering_controllers_library::SteeringControllersLibrary::AckermanControllerState; + steering_controllers_library::SteeringControllersLibrary::AckermannControllerState; using ControllerReferenceMsg = steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index e3a415c374..c671534353 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -73,7 +73,7 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl using ControllerTwistReferenceMsg = geometry_msgs::msg::TwistStamped; using ControllerStateMsgOdom = nav_msgs::msg::Odometry; using ControllerStateMsgTf = tf2_msgs::msg::TFMessage; - using AckermanControllerState = control_msgs::msg::SteeringControllerStatus; + using AckermannControllerState = control_msgs::msg::SteeringControllerStatus; protected: controller_interface::CallbackReturn set_interface_numbers( @@ -105,10 +105,10 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl /// Odometry: steering_odometry::SteeringOdometry odometry_; - AckermanControllerState published_state_; + AckermannControllerState published_state_; - using ControllerStatePublisher = realtime_tools::RealtimePublisher; - rclcpp::Publisher::SharedPtr controller_s_publisher_; + using ControllerStatePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr controller_s_publisher_; std::unique_ptr controller_state_publisher_; // name constants for state interfaces diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 2d47746fed..a91cb87105 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -179,7 +179,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( try { // State publisher - controller_s_publisher_ = get_node()->create_publisher( + controller_s_publisher_ = get_node()->create_publisher( "~/controller_state", rclcpp::SystemDefaultsQoS()); controller_state_publisher_ = std::make_unique(controller_s_publisher_); diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index ff0ab972d5..20d136ab32 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -32,11 +32,11 @@ #include "steering_controllers_library/steering_controllers_library.hpp" using ControllerStateMsg = - steering_controllers_library::SteeringControllersLibrary::AckermanControllerState; + steering_controllers_library::SteeringControllersLibrary::AckermannControllerState; using ControllerReferenceMsg = steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; -// NOTE: Testing steering_controllers_library for ackermann vehicle configuration only +// NOTE: Testing steering_controllers_library for Ackermann vehicle configuration only // name constants for state interfaces static constexpr size_t STATE_TRACTION_RIGHT_WHEEL = 0; diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index c5dd71099f..cfecf96cc8 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -32,7 +32,7 @@ #include "tricycle_steering_controller/tricycle_steering_controller.hpp" using ControllerStateMsg = - steering_controllers_library::SteeringControllersLibrary::AckermanControllerState; + steering_controllers_library::SteeringControllersLibrary::AckermannControllerState; using ControllerReferenceMsg = steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; From ad7739f4423ebaebe20a94cbb414e19e3b4dc500 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Thu, 2 Jan 2025 20:05:26 +0100 Subject: [PATCH 50/84] Update generate_parameter_library dependency in steering_controllers_library (#1465) --- steering_controllers_library/package.xml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index c48d22cb05..033e509acb 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -22,11 +22,10 @@ ament_cmake - generate_parameter_library - backward_ros control_msgs controller_interface + generate_parameter_library geometry_msgs hardware_interface nav_msgs From d86b9d7f1cc130b7b8b6d88f89f7e6da53fd08b8 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Sun, 5 Jan 2025 20:15:22 +0100 Subject: [PATCH 51/84] Remove custom logic to skip configuration of gripper_controllers on Windows or macOS (#1471) Co-authored-by: Sai Kishor Kothakota --- gripper_controllers/CMakeLists.txt | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/gripper_controllers/CMakeLists.txt b/gripper_controllers/CMakeLists.txt index 4ffdc76168..77c9d9183b 100644 --- a/gripper_controllers/CMakeLists.txt +++ b/gripper_controllers/CMakeLists.txt @@ -1,10 +1,9 @@ cmake_minimum_required(VERSION 3.16) project(gripper_controllers) -if(APPLE OR WIN32) - message(WARNING "gripper controllers are not available on OSX or Windows") - return() -endif() +# using this instead of visibility macros +# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053 +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable From 7ddaf83e023c1ed90d29d2cdda2f94cb719208e9 Mon Sep 17 00:00:00 2001 From: verma nakul Date: Tue, 7 Jan 2025 18:52:54 +0530 Subject: [PATCH 52/84] Use urdf/model.hpp for rolling (#1473) --- joint_state_broadcaster/src/joint_state_broadcaster.cpp | 6 ++++++ .../src/joint_trajectory_controller.cpp | 6 ++++++ 2 files changed, 12 insertions(+) diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 9bbb862925..068559d5a3 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -25,7 +25,13 @@ #include "rclcpp/qos.hpp" #include "rclcpp/time.hpp" #include "std_msgs/msg/header.hpp" + +#include "rclcpp/version.h" +#if RCLCPP_VERSION_GTE(29, 0, 0) +#include "urdf/model.hpp" +#else #include "urdf/model.h" +#endif namespace rclcpp_lifecycle { diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 763d2acd07..b1c97175cb 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -33,7 +33,13 @@ #include "rclcpp_action/create_server.hpp" #include "rclcpp_action/server_goal_handle.hpp" #include "rclcpp_lifecycle/state.hpp" + +#include "rclcpp/version.h" +#if RCLCPP_VERSION_GTE(29, 0, 0) +#include "urdf/model.hpp" +#else #include "urdf/model.h" +#endif namespace joint_trajectory_controller { From 6299094cd7fec7f99d590f81a14e2414a4d87ca9 Mon Sep 17 00:00:00 2001 From: verma nakul Date: Thu, 9 Jan 2025 15:45:46 +0530 Subject: [PATCH 53/84] Use urdf/model.hpp for rolling (#1476) --- .../joint_state_broadcaster/joint_state_broadcaster.hpp | 6 ++++++ joint_state_broadcaster/src/joint_state_broadcaster.cpp | 7 ------- 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp index da02c5feb8..f0f100d6d0 100644 --- a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp @@ -26,7 +26,13 @@ #include "joint_state_broadcaster_parameters.hpp" #include "realtime_tools/realtime_publisher.hpp" #include "sensor_msgs/msg/joint_state.hpp" + +#include "rclcpp/version.h" +#if RCLCPP_VERSION_GTE(29, 0, 0) +#include "urdf/model.hpp" +#else #include "urdf/model.h" +#endif namespace joint_state_broadcaster { diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 068559d5a3..5b72b9f35a 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -26,13 +26,6 @@ #include "rclcpp/time.hpp" #include "std_msgs/msg/header.hpp" -#include "rclcpp/version.h" -#if RCLCPP_VERSION_GTE(29, 0, 0) -#include "urdf/model.hpp" -#else -#include "urdf/model.h" -#endif - namespace rclcpp_lifecycle { class State; From abf3172690fb56c974d734779cca24afc75a62e6 Mon Sep 17 00:00:00 2001 From: Julia Jia Date: Thu, 9 Jan 2025 06:10:47 -0800 Subject: [PATCH 54/84] Remove empty on_shutdown() callbacks (#1477) --- .../diff_drive_controller/diff_drive_controller.hpp | 3 --- diff_drive_controller/src/diff_drive_controller.cpp | 6 ------ .../joint_trajectory_controller.hpp | 3 --- .../src/joint_trajectory_controller.cpp | 8 -------- .../include/tricycle_controller/tricycle_controller.hpp | 2 -- tricycle_controller/src/tricycle_controller.cpp | 5 ----- 6 files changed, 27 deletions(-) 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 36ba2d6874..0e5e555b8c 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 @@ -72,9 +72,6 @@ class DiffDriveController : public controller_interface::ControllerInterface controller_interface::CallbackReturn on_error( const rclcpp_lifecycle::State & previous_state) override; - controller_interface::CallbackReturn on_shutdown( - const rclcpp_lifecycle::State & previous_state) override; - protected: struct WheelHandle { diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 6957864321..fd9069237c 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -578,12 +578,6 @@ bool DiffDriveController::reset() return true; } -controller_interface::CallbackReturn DiffDriveController::on_shutdown( - const rclcpp_lifecycle::State &) -{ - return controller_interface::CallbackReturn::SUCCESS; -} - void DiffDriveController::halt() { const auto halt_wheels = [](auto & wheel_handles) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index abefec9d79..dd91377ee1 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -84,9 +84,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa controller_interface::CallbackReturn on_error( const rclcpp_lifecycle::State & previous_state) override; - controller_interface::CallbackReturn on_shutdown( - const rclcpp_lifecycle::State & previous_state) override; - protected: // To reduce number of variables and to make the code shorter the interfaces are ordered in types // as the following constants diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index b1c97175cb..9eea619d25 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1077,14 +1077,6 @@ bool JointTrajectoryController::reset() return true; } -controller_interface::CallbackReturn JointTrajectoryController::on_shutdown( - const rclcpp_lifecycle::State &) -{ - // TODO(karsten1987): what to do? - - return CallbackReturn::SUCCESS; -} - void JointTrajectoryController::publish_state( const rclcpp::Time & time, const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error) diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index 86793fd435..cd6251e803 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -76,8 +76,6 @@ class TricycleController : public controller_interface::ControllerInterface CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override; - CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override; - protected: struct TractionHandle { diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index bc04451ad6..9630d88b44 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -440,11 +440,6 @@ bool TricycleController::reset() return true; } -CallbackReturn TricycleController::on_shutdown(const rclcpp_lifecycle::State &) -{ - return CallbackReturn::SUCCESS; -} - void TricycleController::halt() { traction_joint_[0].velocity_command.get().set_value(0.0); From 67096bdb9b99cf0d010019728bf1ff46aad76dc9 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 10 Jan 2025 00:32:02 +0100 Subject: [PATCH 55/84] [pose_broadcaster] Check for valid pose before attempting to publish a tf for it (#1479) --- pose_broadcaster/src/pose_broadcaster.cpp | 26 ++++++++- .../test/test_pose_broadcaster.cpp | 56 +++++++++++++++++++ .../test/test_pose_broadcaster.hpp | 8 ++- 3 files changed, 86 insertions(+), 4 deletions(-) diff --git a/pose_broadcaster/src/pose_broadcaster.cpp b/pose_broadcaster/src/pose_broadcaster.cpp index 7e3aeaddf3..535a12f419 100644 --- a/pose_broadcaster/src/pose_broadcaster.cpp +++ b/pose_broadcaster/src/pose_broadcaster.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. #include "pose_broadcaster/pose_broadcaster.hpp" +#include +#include namespace { @@ -24,6 +26,19 @@ constexpr auto DEFAULT_TF_TOPIC = "/tf"; namespace pose_broadcaster { +bool is_pose_valid(const geometry_msgs::msg::Pose & pose) +{ + return std::isfinite(pose.position.x) && std::isfinite(pose.position.y) && + std::isfinite(pose.position.z) && std::isfinite(pose.orientation.x) && + std::isfinite(pose.orientation.y) && std::isfinite(pose.orientation.z) && + std::isfinite(pose.orientation.w) && + + std::abs( + pose.orientation.x * pose.orientation.x + pose.orientation.y * pose.orientation.y + + pose.orientation.z * pose.orientation.z + pose.orientation.w * pose.orientation.w - + 1.0) <= 10e-3; +} + controller_interface::InterfaceConfiguration PoseBroadcaster::command_interface_configuration() const { @@ -147,8 +162,15 @@ controller_interface::return_type PoseBroadcaster::update( realtime_publisher_->msg_.pose = pose; realtime_publisher_->unlockAndPublish(); } - - if (realtime_tf_publisher_ && realtime_tf_publisher_->trylock()) + if (!is_pose_valid(pose)) + { + RCLCPP_ERROR_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 1000, + "Invalid pose [%f, %f, %f], [%f, %f, %f, %f]", pose.position.x, pose.position.y, + pose.position.z, pose.orientation.x, pose.orientation.y, pose.orientation.z, + pose.orientation.w); + } + else if (realtime_tf_publisher_ && realtime_tf_publisher_->trylock()) { bool do_publish = false; // rlcpp::Time comparisons throw if clock types are not the same diff --git a/pose_broadcaster/test/test_pose_broadcaster.cpp b/pose_broadcaster/test/test_pose_broadcaster.cpp index 0ed2e84619..89668f3ea3 100644 --- a/pose_broadcaster/test/test_pose_broadcaster.cpp +++ b/pose_broadcaster/test/test_pose_broadcaster.cpp @@ -13,6 +13,8 @@ // limitations under the License. #include "test_pose_broadcaster.hpp" +#include +#include #include #include @@ -186,6 +188,60 @@ TEST_F(PoseBroadcasterTest, PublishSuccess) EXPECT_EQ(tf_msg.transforms[0].transform.rotation.w, pose_values_[6]); } +TEST_F(PoseBroadcasterTest, invalid_pose_no_tf_published) +{ + SetUpPoseBroadcaster(); + + // Set 'pose_name' and 'frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"pose_name", pose_name_}); + pose_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + + // Set 'tf.enable' and 'tf.child_frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"tf.enable", true}); + pose_broadcaster_->get_node()->set_parameter({"tf.child_frame_id", tf_child_frame_id_}); + + // Configure and activate controller + ASSERT_EQ( + pose_broadcaster_->on_configure(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + pose_broadcaster_->on_activate(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + + ASSERT_TRUE(pose_position_x_.set_value(std::numeric_limits::quiet_NaN())); + + // Subscribe to pose topic + geometry_msgs::msg::PoseStamped pose_msg; + subscribe_and_get_message("/test_pose_broadcaster/pose", pose_msg); + + // Verify content of pose message + EXPECT_EQ(pose_msg.header.frame_id, frame_id_); + EXPECT_TRUE(std::isnan(pose_msg.pose.position.x)); // We set that to NaN above + EXPECT_EQ(pose_msg.pose.position.y, pose_values_[1]); + EXPECT_EQ(pose_msg.pose.position.z, pose_values_[2]); + EXPECT_EQ(pose_msg.pose.orientation.x, pose_values_[3]); + EXPECT_EQ(pose_msg.pose.orientation.y, pose_values_[4]); + EXPECT_EQ(pose_msg.pose.orientation.z, pose_values_[5]); + EXPECT_EQ(pose_msg.pose.orientation.w, pose_values_[6]); + + // Subscribe to tf topic + tf2_msgs::msg::TFMessage tf_msg; + EXPECT_THROW(subscribe_and_get_message("/tf", tf_msg), std::runtime_error); + // Verify that no tf message was sent + ASSERT_EQ(tf_msg.transforms.size(), 0lu); + + // Set valid position but invalid quaternion + ASSERT_TRUE(pose_position_x_.set_value(0.0)); + ASSERT_TRUE(pose_orientation_x_.set_value(0.0)); + ASSERT_TRUE(pose_orientation_y_.set_value(0.0)); + ASSERT_TRUE(pose_orientation_z_.set_value(0.0)); + ASSERT_TRUE(pose_orientation_w_.set_value(0.0)); + + EXPECT_THROW(subscribe_and_get_message("/tf", tf_msg), std::runtime_error); + // Verify that no tf message was sent + ASSERT_EQ(tf_msg.transforms.size(), 0lu); +} + int main(int argc, char * argv[]) { ::testing::InitGoogleMock(&argc, argv); diff --git a/pose_broadcaster/test/test_pose_broadcaster.hpp b/pose_broadcaster/test/test_pose_broadcaster.hpp index 10b9c03d1c..e3b84c0307 100644 --- a/pose_broadcaster/test/test_pose_broadcaster.hpp +++ b/pose_broadcaster/test/test_pose_broadcaster.hpp @@ -39,7 +39,8 @@ class PoseBroadcasterTest : public ::testing::Test const std::string frame_id_ = "pose_base_frame"; const std::string tf_child_frame_id_ = "pose_frame"; - std::array pose_values_ = {{1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7}}; + std::array pose_values_ = { + {1.1, 2.2, 3.3, 0.39190382, 0.20056212, 0.53197575, 0.72331744}}; hardware_interface::StateInterface pose_position_x_{pose_name_, "position.x", &pose_values_[0]}; hardware_interface::StateInterface pose_position_y_{pose_name_, "position.x", &pose_values_[1]}; @@ -77,7 +78,10 @@ void PoseBroadcasterTest::subscribe_and_get_message(const std::string & topic, T constexpr size_t max_sub_check_loop_count = 5; for (size_t i = 0; !received_msg; ++i) { - ASSERT_LT(i, max_sub_check_loop_count); + if (i >= max_sub_check_loop_count) + { + throw std::runtime_error("Failed to receive message on topic: " + topic); + } pose_broadcaster_->update(rclcpp::Time{0}, rclcpp::Duration::from_seconds(0.01)); From 0736e6c068b1705b59acfd62a1e4e330f5df7830 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Fri, 10 Jan 2025 23:13:53 +0100 Subject: [PATCH 56/84] Check dt in updateFromVelocity (#1481) Signed-off-by: Tony Najjar --- diff_drive_controller/src/odometry.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/diff_drive_controller/src/odometry.cpp b/diff_drive_controller/src/odometry.cpp index a4a1dbd6ca..eb025ad23f 100644 --- a/diff_drive_controller/src/odometry.cpp +++ b/diff_drive_controller/src/odometry.cpp @@ -76,7 +76,10 @@ bool Odometry::update(double left_pos, double right_pos, const rclcpp::Time & ti bool Odometry::updateFromVelocity(double left_vel, double right_vel, const rclcpp::Time & time) { const double dt = time.seconds() - timestamp_.seconds(); - + if (dt < 0.0001) + { + return false; // Interval too small to integrate with + } // Compute linear and angular diff: const double linear = (left_vel + right_vel) * 0.5; // Now there is a bug about scout angular velocity From 2b1fe048228d467ac92836e17f4330cd0ddd963f Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 13 Jan 2025 11:27:55 +0000 Subject: [PATCH 57/84] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 6 ++++++ admittance_controller/CHANGELOG.rst | 5 +++++ bicycle_steering_controller/CHANGELOG.rst | 6 ++++++ diff_drive_controller/CHANGELOG.rst | 7 +++++++ effort_controllers/CHANGELOG.rst | 5 +++++ force_torque_sensor_broadcaster/CHANGELOG.rst | 5 +++++ forward_command_controller/CHANGELOG.rst | 5 +++++ gpio_controllers/CHANGELOG.rst | 5 +++++ gripper_controllers/CHANGELOG.rst | 6 ++++++ imu_sensor_broadcaster/CHANGELOG.rst | 5 +++++ joint_state_broadcaster/CHANGELOG.rst | 7 +++++++ joint_trajectory_controller/CHANGELOG.rst | 7 +++++++ mecanum_drive_controller/CHANGELOG.rst | 6 ++++++ parallel_gripper_controller/CHANGELOG.rst | 5 +++++ pid_controller/CHANGELOG.rst | 5 +++++ pose_broadcaster/CHANGELOG.rst | 6 ++++++ position_controllers/CHANGELOG.rst | 5 +++++ range_sensor_broadcaster/CHANGELOG.rst | 5 +++++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ steering_controllers_library/CHANGELOG.rst | 8 ++++++++ tricycle_controller/CHANGELOG.rst | 6 ++++++ tricycle_steering_controller/CHANGELOG.rst | 6 ++++++ velocity_controllers/CHANGELOG.rst | 5 +++++ 25 files changed, 135 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index b8f12995aa..fe586b1ae7 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix typos in steering_controllers_lib (`#1464 `_) +* Remove visibility macros (`#1451 `_) +* Contributors: Bence Magyar, Christoph Fröhlich + 4.18.0 (2024-12-19) ------------------- diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 7e0e2d00c3..2a62b06980 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove visibility macros (`#1451 `_) +* Contributors: Bence Magyar + 4.18.0 (2024-12-19) ------------------- * [CI] Add clang job and setup concurrency (`#1407 `_) diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 86833e9ca0..66d0e95b56 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix typos in steering_controllers_lib (`#1464 `_) +* Remove visibility macros (`#1451 `_) +* Contributors: Bence Magyar, Christoph Fröhlich + 4.18.0 (2024-12-19) ------------------- diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index eb32050dda..11895cac8e 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Check dt in updateFromVelocity (`#1481 `_) +* Remove empty on_shutdown() callbacks (`#1477 `_) +* Remove visibility macros (`#1451 `_) +* Contributors: Bence Magyar, Julia Jia, Tony Najjar + 4.18.0 (2024-12-19) ------------------- * Update command limiter of diff_drive_controller (`#1315 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 76125e7a08..7872f38ea9 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove visibility macros (`#1451 `_) +* Contributors: Bence Magyar + 4.18.0 (2024-12-19) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 33c741ad3f..700d7c339c 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove visibility macros (`#1451 `_) +* Contributors: Bence Magyar + 4.18.0 (2024-12-19) ------------------- diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index b72327a18f..3d38e48b01 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove visibility macros (`#1451 `_) +* Contributors: Bence Magyar + 4.18.0 (2024-12-19) ------------------- * [CI] Add clang job and setup concurrency (`#1407 `_) diff --git a/gpio_controllers/CHANGELOG.rst b/gpio_controllers/CHANGELOG.rst index 0bd9254a3c..c20059bfec 100644 --- a/gpio_controllers/CHANGELOG.rst +++ b/gpio_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gpio_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove visibility macros (`#1451 `_) +* Contributors: Bence Magyar + 4.18.0 (2024-12-19) ------------------- diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index f2d262c30d..375effd7a7 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove custom logic to skip configuration of gripper_controllers on Windows or macOS (`#1471 `_) +* Remove visibility macros (`#1451 `_) +* Contributors: Bence Magyar, Silvio Traversaro + 4.18.0 (2024-12-19) ------------------- diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 1d2379a9e5..66fff1d4f8 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove visibility macros (`#1451 `_) +* Contributors: Bence Magyar + 4.18.0 (2024-12-19) ------------------- diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index d1cfe32fe5..30bf74bd55 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use urdf/model.hpp for rolling (`#1476 `_) +* Use urdf/model.hpp for rolling (`#1473 `_) +* Remove visibility macros (`#1451 `_) +* Contributors: Bence Magyar, verma nakul + 4.18.0 (2024-12-19) ------------------- * [CI] Add clang job and setup concurrency (`#1407 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index e4f26fa2f0..726845f4bc 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove empty on_shutdown() callbacks (`#1477 `_) +* Use urdf/model.hpp for rolling (`#1473 `_) +* Remove visibility macros (`#1451 `_) +* Contributors: Bence Magyar, Julia Jia, verma nakul + 4.18.0 (2024-12-19) ------------------- * Add an error msg if empty message is received (`#1424 `_) diff --git a/mecanum_drive_controller/CHANGELOG.rst b/mecanum_drive_controller/CHANGELOG.rst index f45c653f27..8594e5ed8e 100644 --- a/mecanum_drive_controller/CHANGELOG.rst +++ b/mecanum_drive_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package mecanum_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove visibility macros (`#1451 `_) +* Clean up unused variable EPS in mecanum_drive_controller (`#1444 `_) +* Contributors: Bence Magyar, Shankar-Balajee + 4.18.0 (2024-12-19) ------------------- diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst index 8c889e82b2..ab0b45834b 100644 --- a/parallel_gripper_controller/CHANGELOG.rst +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package parallel_gripper_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove visibility macros (`#1451 `_) +* Contributors: Bence Magyar + 4.18.0 (2024-12-19) ------------------- diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index d2bdaf679d..77b709d8c0 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove visibility macros (`#1451 `_) +* Contributors: Bence Magyar + 4.18.0 (2024-12-19) ------------------- * [CI] Add clang job and setup concurrency (`#1407 `_) diff --git a/pose_broadcaster/CHANGELOG.rst b/pose_broadcaster/CHANGELOG.rst index 43fd04c3b3..9c20e62f4c 100644 --- a/pose_broadcaster/CHANGELOG.rst +++ b/pose_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package pose_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [pose_broadcaster] Check for valid pose before attempting to publish a tf for it (`#1479 `_) +* Remove visibility macros (`#1451 `_) +* Contributors: Bence Magyar, Felix Exner + 4.18.0 (2024-12-19) ------------------- diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 5d939991a0..f681295c42 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove visibility macros (`#1451 `_) +* Contributors: Bence Magyar + 4.18.0 (2024-12-19) ------------------- * Update position controller package.xml (`#1431 `_) diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index c6be8e44ac..faff9a164d 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove visibility macros (`#1451 `_) +* Contributors: Bence Magyar + 4.18.0 (2024-12-19) ------------------- diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 19d8e12c68..dd7b0d2401 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.18.0 (2024-12-19) ------------------- * Add missing plugins to ros2_controllers dependencies (`#1413 `_) diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index fa20e80af1..205055cac8 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.18.0 (2024-12-19) ------------------- diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 1210b917ef..d480b05939 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.18.0 (2024-12-19) ------------------- diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index cd91a8b8f9..44c07a9e60 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update generate_parameter_library dependency in steering_controllers_library (`#1465 `_) +* Fix typos in steering_controllers_lib (`#1464 `_) +* Fix open-loop odometry in case of ref timeout (`#1454 `_) +* Remove visibility macros (`#1451 `_) +* Contributors: Bence Magyar, Christoph Fröhlich, Silvio Traversaro + 4.18.0 (2024-12-19) ------------------- * steering_controllers_library: Add `reduce_wheel_speed_until_steering_reached` parameter (`#1314 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 45d8531c29..b64bd9ef45 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove empty on_shutdown() callbacks (`#1477 `_) +* Remove visibility macros (`#1451 `_) +* Contributors: Bence Magyar, Julia Jia + 4.18.0 (2024-12-19) ------------------- * [CI] Add clang job and setup concurrency (`#1407 `_) diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 18fef2ec79..18d9dbbb7e 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix typos in steering_controllers_lib (`#1464 `_) +* Remove visibility macros (`#1451 `_) +* Contributors: Bence Magyar, Christoph Fröhlich + 4.18.0 (2024-12-19) ------------------- diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 02440b920a..f36870005d 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove visibility macros (`#1451 `_) +* Contributors: Bence Magyar + 4.18.0 (2024-12-19) ------------------- From ba7c1dd6cb7d6e41858fc32f7d0160e7e190a859 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 13 Jan 2025 11:33:07 +0000 Subject: [PATCH 58/84] 4.19.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gpio_controllers/CHANGELOG.rst | 4 ++-- gpio_controllers/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- mecanum_drive_controller/CHANGELOG.rst | 4 ++-- mecanum_drive_controller/package.xml | 2 +- parallel_gripper_controller/CHANGELOG.rst | 4 ++-- parallel_gripper_controller/package.xml | 2 +- pid_controller/CHANGELOG.rst | 4 ++-- pid_controller/package.xml | 2 +- pose_broadcaster/CHANGELOG.rst | 4 ++-- pose_broadcaster/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 52 files changed, 77 insertions(+), 77 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index fe586b1ae7..795d981a4f 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2025-01-13) +------------------- * Fix typos in steering_controllers_lib (`#1464 `_) * Remove visibility macros (`#1451 `_) * Contributors: Bence Magyar, Christoph Fröhlich diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 21e9158a60..e31013d7ff 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.18.0 + 4.19.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Bence Magyar diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 2a62b06980..83a01ca3f3 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2025-01-13) +------------------- * Remove visibility macros (`#1451 `_) * Contributors: Bence Magyar diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index fedc56426f..b30e245e9f 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.18.0 + 4.19.0 Implementation of admittance controllers for different input and output interface. Bence Magyar diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 66d0e95b56..09655d3713 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2025-01-13) +------------------- * Fix typos in steering_controllers_lib (`#1464 `_) * Remove visibility macros (`#1451 `_) * Contributors: Bence Magyar, Christoph Fröhlich diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 42e0a0e137..95e1a469c8 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.18.0 + 4.19.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 11895cac8e..45f2499018 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2025-01-13) +------------------- * Check dt in updateFromVelocity (`#1481 `_) * Remove empty on_shutdown() callbacks (`#1477 `_) * Remove visibility macros (`#1451 `_) diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 1579e1b31f..1f616a271a 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.18.0 + 4.19.0 Controller for a differential-drive mobile base. Bence Magyar diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 7872f38ea9..44cd786d80 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2025-01-13) +------------------- * Remove visibility macros (`#1451 `_) * Contributors: Bence Magyar diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 51a8e869aa..fb851c4fd2 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.18.0 + 4.19.0 Generic controller for forwarding commands. Bence Magyar diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 700d7c339c..114f7e0065 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2025-01-13) +------------------- * Remove visibility macros (`#1451 `_) * Contributors: Bence Magyar diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 17a794b200..5d0f15d947 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.18.0 + 4.19.0 Controller to publish state of force-torque sensors. Bence Magyar diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 3d38e48b01..967360c948 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2025-01-13) +------------------- * Remove visibility macros (`#1451 `_) * Contributors: Bence Magyar diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index e6e666c5f4..14b316a961 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.18.0 + 4.19.0 Generic controller for forwarding commands. Bence Magyar diff --git a/gpio_controllers/CHANGELOG.rst b/gpio_controllers/CHANGELOG.rst index c20059bfec..a7147c670f 100644 --- a/gpio_controllers/CHANGELOG.rst +++ b/gpio_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gpio_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2025-01-13) +------------------- * Remove visibility macros (`#1451 `_) * Contributors: Bence Magyar diff --git a/gpio_controllers/package.xml b/gpio_controllers/package.xml index 108e951ffa..b28ad57c6c 100644 --- a/gpio_controllers/package.xml +++ b/gpio_controllers/package.xml @@ -2,7 +2,7 @@ gpio_controllers - 4.18.0 + 4.19.0 Controllers to interact with gpios. Bence Magyar diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 375effd7a7..3e51082926 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2025-01-13) +------------------- * Remove custom logic to skip configuration of gripper_controllers on Windows or macOS (`#1471 `_) * Remove visibility macros (`#1451 `_) * Contributors: Bence Magyar, Silvio Traversaro diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 3dc9ab83c0..17767846f6 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 4.18.0 + 4.19.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 66fff1d4f8..e1380f5bd0 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2025-01-13) +------------------- * Remove visibility macros (`#1451 `_) * Contributors: Bence Magyar diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 2bce9aa92d..681a63a95a 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.18.0 + 4.19.0 Controller to publish readings of IMU sensors. Bence Magyar diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 30bf74bd55..aa9ad6f312 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2025-01-13) +------------------- * Use urdf/model.hpp for rolling (`#1476 `_) * Use urdf/model.hpp for rolling (`#1473 `_) * Remove visibility macros (`#1451 `_) diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index fe40531c15..320dc449ab 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.18.0 + 4.19.0 Broadcaster to publish joint state Bence Magyar diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 726845f4bc..97f289703f 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2025-01-13) +------------------- * Remove empty on_shutdown() callbacks (`#1477 `_) * Use urdf/model.hpp for rolling (`#1473 `_) * Remove visibility macros (`#1451 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 9acd8bfdcd..e334b91400 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.18.0 + 4.19.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar diff --git a/mecanum_drive_controller/CHANGELOG.rst b/mecanum_drive_controller/CHANGELOG.rst index 8594e5ed8e..e2af0afde1 100644 --- a/mecanum_drive_controller/CHANGELOG.rst +++ b/mecanum_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package mecanum_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2025-01-13) +------------------- * Remove visibility macros (`#1451 `_) * Clean up unused variable EPS in mecanum_drive_controller (`#1444 `_) * Contributors: Bence Magyar, Shankar-Balajee diff --git a/mecanum_drive_controller/package.xml b/mecanum_drive_controller/package.xml index 076d215529..a5db87cb61 100644 --- a/mecanum_drive_controller/package.xml +++ b/mecanum_drive_controller/package.xml @@ -2,7 +2,7 @@ mecanum_drive_controller - 4.18.0 + 4.19.0 Implementation of mecanum drive controller for 4 wheel drive. diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst index ab0b45834b..7b71c6099a 100644 --- a/parallel_gripper_controller/CHANGELOG.rst +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package parallel_gripper_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2025-01-13) +------------------- * Remove visibility macros (`#1451 `_) * Contributors: Bence Magyar diff --git a/parallel_gripper_controller/package.xml b/parallel_gripper_controller/package.xml index 4db99f7537..591c69af04 100644 --- a/parallel_gripper_controller/package.xml +++ b/parallel_gripper_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> parallel_gripper_controller - 4.18.0 + 4.19.0 The parallel_gripper_controller package Bence Magyar diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 77b709d8c0..c9ef126ae8 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2025-01-13) +------------------- * Remove visibility macros (`#1451 `_) * Contributors: Bence Magyar diff --git a/pid_controller/package.xml b/pid_controller/package.xml index a422e2254d..c3e9602ad9 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.18.0 + 4.19.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar diff --git a/pose_broadcaster/CHANGELOG.rst b/pose_broadcaster/CHANGELOG.rst index 9c20e62f4c..c9fc763c29 100644 --- a/pose_broadcaster/CHANGELOG.rst +++ b/pose_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pose_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2025-01-13) +------------------- * [pose_broadcaster] Check for valid pose before attempting to publish a tf for it (`#1479 `_) * Remove visibility macros (`#1451 `_) * Contributors: Bence Magyar, Felix Exner diff --git a/pose_broadcaster/package.xml b/pose_broadcaster/package.xml index 02581f5aa2..15d9f9ac99 100644 --- a/pose_broadcaster/package.xml +++ b/pose_broadcaster/package.xml @@ -2,7 +2,7 @@ pose_broadcaster - 4.18.0 + 4.19.0 Broadcaster to publish cartesian states. Bence Magyar diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index f681295c42..2fa99f6e03 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2025-01-13) +------------------- * Remove visibility macros (`#1451 `_) * Contributors: Bence Magyar diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 12873809bf..66b379f7bc 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.18.0 + 4.19.0 Generic position controller for forwarding position commands. Bence Magyar diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index faff9a164d..f807b1063b 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2025-01-13) +------------------- * Remove visibility macros (`#1451 `_) * Contributors: Bence Magyar diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 3cf633e87a..7412249586 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.18.0 + 4.19.0 Controller to publish readings of range sensors. Bence Magyar diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index dd7b0d2401..fac6193901 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2025-01-13) +------------------- 4.18.0 (2024-12-19) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 491c88f395..3719733764 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.18.0 + 4.19.0 Metapackage for ros2_controllers related packages Bence Magyar diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 205055cac8..1fc89e68f5 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2025-01-13) +------------------- 4.18.0 (2024-12-19) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index ab5d69e2c3..199cd41672 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.18.0 + 4.19.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Bence Magyar diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index ca37c49797..814045a9e2 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.18.0", + version="4.19.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index d480b05939..b7a22789bb 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2025-01-13) +------------------- 4.18.0 (2024-12-19) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 34cc0f82ae..4a694b3d1b 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 4.18.0 + 4.19.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index a8c9ed1503..3531833c1e 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -21,7 +21,7 @@ setup( name=package_name, - version="4.18.0", + version="4.19.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 44c07a9e60..86b044856d 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2025-01-13) +------------------- * Update generate_parameter_library dependency in steering_controllers_library (`#1465 `_) * Fix typos in steering_controllers_lib (`#1464 `_) * Fix open-loop odometry in case of ref timeout (`#1454 `_) diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 033e509acb..2fe814faba 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.18.0 + 4.19.0 Package for steering robot configurations including odometry and interfaces. Bence Magyar diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index b64bd9ef45..6bf971a207 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2025-01-13) +------------------- * Remove empty on_shutdown() callbacks (`#1477 `_) * Remove visibility macros (`#1451 `_) * Contributors: Bence Magyar, Julia Jia diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index ce5fcfce6a..ce0394e8d9 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.18.0 + 4.19.0 Controller for a tricycle drive mobile base Bence Magyar diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 18d9dbbb7e..e24893f4f7 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2025-01-13) +------------------- * Fix typos in steering_controllers_lib (`#1464 `_) * Remove visibility macros (`#1451 `_) * Contributors: Bence Magyar, Christoph Fröhlich diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 1cb79f5af2..d0e5f19b87 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.18.0 + 4.19.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Bence Magyar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index f36870005d..ac007bf316 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.19.0 (2025-01-13) +------------------- * Remove visibility macros (`#1451 `_) * Contributors: Bence Magyar diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 0b27796fc4..e4b059a950 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.18.0 + 4.19.0 Generic controller for forwarding commands. Bence Magyar From e0b929b5efcd8d3f7ba98649d00b24e2d22b0c7e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 13 Jan 2025 13:32:42 +0100 Subject: [PATCH 59/84] Fix SpeedLimiter Constructor regression (#1478) --- .../diff_drive_controller.hpp | 4 ++-- .../diff_drive_controller/speed_limiter.hpp | 20 +++++++++---------- .../src/diff_drive_controller.cpp | 17 +++++----------- 3 files changed, 17 insertions(+), 24 deletions(-) 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 0e5e555b8c..0e37cc0b0b 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 @@ -120,8 +120,8 @@ class DiffDriveController : public controller_interface::ControllerInterface std::queue previous_commands_; // last two commands // speed limiters - SpeedLimiter limiter_linear_; - SpeedLimiter limiter_angular_; + std::unique_ptr limiter_linear_; + std::unique_ptr limiter_angular_; bool publish_limited_velocity_ = false; std::shared_ptr> limited_velocity_publisher_ = nullptr; diff --git a/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp b/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp index 4fa08fcdba..937e848460 100644 --- a/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp +++ b/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp @@ -40,7 +40,7 @@ class SpeedLimiter * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0 * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0 */ - [[deprecated]] SpeedLimiter( + [[deprecated]] explicit SpeedLimiter( bool has_velocity_limits = true, bool has_acceleration_limits = true, bool has_jerk_limits = true, double min_velocity = std::numeric_limits::quiet_NaN(), double max_velocity = std::numeric_limits::quiet_NaN(), @@ -79,16 +79,16 @@ class SpeedLimiter * >= 0 * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0 * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0 + * + * \note + * If max_* values are NAN, the respective limit is deactivated + * If min_* values are NAN (unspecified), defaults to -max + * If min_first_derivative_pos/max_first_derivative_neg values are NAN, symmetric limits are used */ - SpeedLimiter( - double min_velocity = std::numeric_limits::quiet_NaN(), - double max_velocity = std::numeric_limits::quiet_NaN(), - double max_acceleration_reverse = std::numeric_limits::quiet_NaN(), - double max_acceleration = std::numeric_limits::quiet_NaN(), - double max_deceleration = std::numeric_limits::quiet_NaN(), - double max_deceleration_reverse = std::numeric_limits::quiet_NaN(), - double min_jerk = std::numeric_limits::quiet_NaN(), - double max_jerk = std::numeric_limits::quiet_NaN()) + explicit SpeedLimiter( + double min_velocity, double max_velocity, double max_acceleration_reverse, + double max_acceleration, double max_deceleration, double max_deceleration_reverse, + double min_jerk, double max_jerk) { speed_limiter_ = control_toolbox::RateLimiter( min_velocity, max_velocity, max_acceleration_reverse, max_acceleration, max_deceleration, diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index fd9069237c..79bdd90419 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -45,14 +45,7 @@ using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; using lifecycle_msgs::msg::State; -DiffDriveController::DiffDriveController() -: controller_interface::ControllerInterface(), - // dummy limiter, will be created in on_configure - // could be done with shared_ptr instead -> but will break ABI - limiter_angular_(std::numeric_limits::quiet_NaN()), - limiter_linear_(std::numeric_limits::quiet_NaN()) -{ -} +DiffDriveController::DiffDriveController() : controller_interface::ControllerInterface() {} const char * DiffDriveController::feedback_type() const { @@ -241,9 +234,9 @@ controller_interface::return_type DiffDriveController::update( auto & last_command = previous_commands_.back().twist; auto & second_to_last_command = previous_commands_.front().twist; - limiter_linear_.limit( + limiter_linear_->limit( linear_command, last_command.linear.x, second_to_last_command.linear.x, period.seconds()); - limiter_angular_.limit( + limiter_angular_->limit( angular_command, last_command.angular.z, second_to_last_command.angular.z, period.seconds()); previous_commands_.pop(); @@ -365,13 +358,13 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( std::numeric_limits::quiet_NaN(); } // END DEPRECATED - limiter_linear_ = SpeedLimiter( + limiter_linear_ = std::make_unique( params_.linear.x.min_velocity, params_.linear.x.max_velocity, params_.linear.x.max_acceleration_reverse, params_.linear.x.max_acceleration, params_.linear.x.max_deceleration, params_.linear.x.max_deceleration_reverse, params_.linear.x.min_jerk, params_.linear.x.max_jerk); - limiter_angular_ = SpeedLimiter( + limiter_angular_ = std::make_unique( params_.angular.z.min_velocity, params_.angular.z.max_velocity, params_.angular.z.max_acceleration_reverse, params_.angular.z.max_acceleration, params_.angular.z.max_deceleration, params_.angular.z.max_deceleration_reverse, From a76f8208df04fb3a700337b08336e67fd47a46bc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 15 Jan 2025 22:46:42 +0100 Subject: [PATCH 60/84] Change branch of control_toolbox repo for humble (#1489) --- ros2_controllers.humble.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_controllers.humble.repos b/ros2_controllers.humble.repos index 548c95b44a..61687f667e 100644 --- a/ros2_controllers.humble.repos +++ b/ros2_controllers.humble.repos @@ -18,4 +18,4 @@ repositories: control_toolbox: type: git url: https://github.com/ros-controls/control_toolbox.git - version: ros2-master + version: humble From bfe622cd03fa4516d07c84ab12764282ad1bc693 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 15 Jan 2025 23:36:11 +0100 Subject: [PATCH 61/84] Update paths of GPL includes (#1487) --- .../ackermann_steering_controller.hpp | 2 +- .../admittance_controller/admittance_controller.hpp | 2 +- .../admittance_controller/admittance_rule.hpp | 2 +- .../bicycle_steering_controller.hpp | 2 +- .../diff_drive_controller/diff_drive_controller.hpp | 2 +- .../force_torque_sensor_broadcaster.hpp | 2 +- .../forward_command_controller.hpp | 2 +- .../multi_interface_forward_command_controller.hpp | 2 +- .../gpio_controllers/gpio_command_controller.hpp | 3 ++- .../gripper_action_controller.hpp | 2 +- .../imu_sensor_broadcaster.hpp | 7 ++++--- .../joint_state_broadcaster.hpp | 5 +++-- .../joint_trajectory_controller.hpp | 2 +- .../joint_trajectory_controller/tolerances.hpp | 2 +- .../mecanum_drive_controller.hpp | 12 ++++++------ .../parallel_gripper_action_controller.hpp | 2 +- .../include/pid_controller/pid_controller.hpp | 3 ++- .../include/pose_broadcaster/pose_broadcaster.hpp | 3 ++- .../range_sensor_broadcaster.hpp | 3 ++- ros2_controllers-not-released.jazzy.repos | 9 +++++---- ros2_controllers-not-released.rolling.repos | 9 +++++---- .../steering_controllers_library.hpp | 5 +++-- .../tricycle_controller/tricycle_controller.hpp | 4 ++-- .../tricycle_steering_controller.hpp | 2 +- 24 files changed, 49 insertions(+), 40 deletions(-) diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp index c1eacb40bb..3e4109b2fd 100644 --- a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp @@ -20,7 +20,7 @@ #include -#include "ackermann_steering_controller_parameters.hpp" +#include "ackermann_steering_controller/ackermann_steering_controller_parameters.hpp" #include "steering_controllers_library/steering_controllers_library.hpp" namespace ackermann_steering_controller diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 35b072f8d7..dc69431f1c 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -22,7 +22,7 @@ #include // auto-generated by generate_parameter_library -#include "admittance_controller_parameters.hpp" +#include "admittance_controller/admittance_controller_parameters.hpp" #include "admittance_controller/admittance_rule.hpp" #include "control_msgs/msg/admittance_controller_state.hpp" diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index bfb3a612c7..e2592f518e 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -23,7 +23,7 @@ #include #include -#include "admittance_controller_parameters.hpp" +#include "admittance_controller/admittance_controller_parameters.hpp" #include "control_msgs/msg/admittance_controller_state.hpp" #include "controller_interface/controller_interface_base.hpp" #include "kinematics_interface/kinematics_interface.hpp" diff --git a/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp b/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp index efb893f2bb..70c6cdeaae 100644 --- a/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp @@ -20,7 +20,7 @@ #include -#include "bicycle_steering_controller_parameters.hpp" +#include "bicycle_steering_controller/bicycle_steering_controller_parameters.hpp" #include "steering_controllers_library/steering_controllers_library.hpp" namespace bicycle_steering_controller 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 0e37cc0b0b..265e000b79 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 @@ -37,7 +37,7 @@ #include "tf2_msgs/msg/tf_message.hpp" // auto-generated by generate_parameter_library -#include "diff_drive_controller_parameters.hpp" +#include "diff_drive_controller/diff_drive_controller_parameters.hpp" namespace diff_drive_controller { diff --git a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp index 0861715678..34b7f242ee 100644 --- a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp @@ -24,7 +24,7 @@ #include "controller_interface/chainable_controller_interface.hpp" // auto-generated by generate_parameter_library -#include "force_torque_sensor_broadcaster_parameters.hpp" +#include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster_parameters.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_publisher.hpp" diff --git a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp index 5401263ec6..b4e92640a4 100644 --- a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp @@ -19,7 +19,7 @@ #include "forward_command_controller/forward_controllers_base.hpp" // auto-generated by generate_parameter_library -#include "forward_command_controller_parameters.hpp" +#include "forward_command_controller/forward_command_controller_parameters.hpp" namespace forward_command_controller { diff --git a/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp index 6ba33be018..7715a38c3a 100644 --- a/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp @@ -18,7 +18,7 @@ #include #include "forward_command_controller/forward_controllers_base.hpp" -#include "multi_interface_forward_command_controller_parameters.hpp" +#include "forward_command_controller/multi_interface_forward_command_controller_parameters.hpp" namespace forward_command_controller { diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp index 0513d9e1f4..35cf7c283d 100644 --- a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -22,12 +22,13 @@ #include "control_msgs/msg/dynamic_interface_group_values.hpp" #include "controller_interface/controller_interface.hpp" -#include "gpio_command_controller_parameters.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" +#include "gpio_controllers/gpio_command_controller_parameters.hpp" + namespace gpio_controllers { using CmdType = control_msgs::msg::DynamicInterfaceGroupValues; diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp index d166d12478..a23bd5d6e8 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp @@ -43,7 +43,7 @@ #include "gripper_controllers/hardware_interface_adapter.hpp" // auto-generated by generate_parameter_library -#include "gripper_action_controller_parameters.hpp" +#include "gripper_controllers/gripper_action_controller_parameters.hpp" namespace gripper_action_controller { diff --git a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp index fda18af83f..01045b7510 100644 --- a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp @@ -21,14 +21,15 @@ #include -#include "controller_interface/controller_interface.hpp" -// auto-generated by generate_parameter_library -#include "imu_sensor_broadcaster_parameters.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_publisher.hpp" #include "semantic_components/imu_sensor.hpp" #include "sensor_msgs/msg/imu.hpp" +#include "controller_interface/controller_interface.hpp" +// auto-generated by generate_parameter_library +#include "imu_sensor_broadcaster/imu_sensor_broadcaster_parameters.hpp" + namespace imu_sensor_broadcaster { class IMUSensorBroadcaster : public controller_interface::ControllerInterface diff --git a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp index f0f100d6d0..2ffd28e0f5 100644 --- a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp @@ -22,8 +22,6 @@ #include "control_msgs/msg/dynamic_joint_state.hpp" #include "controller_interface/controller_interface.hpp" -// auto-generated by generate_parameter_library -#include "joint_state_broadcaster_parameters.hpp" #include "realtime_tools/realtime_publisher.hpp" #include "sensor_msgs/msg/joint_state.hpp" @@ -34,6 +32,9 @@ #include "urdf/model.h" #endif +// auto-generated by generate_parameter_library +#include "joint_state_broadcaster/joint_state_broadcaster_parameters.hpp" + namespace joint_state_broadcaster { /** diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index dd91377ee1..6c265ecacb 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -42,7 +42,7 @@ #include "trajectory_msgs/msg/joint_trajectory_point.hpp" // auto-generated by generate_parameter_library -#include "joint_trajectory_controller_parameters.hpp" +#include "joint_trajectory_controller/joint_trajectory_controller_parameters.hpp" using namespace std::chrono_literals; // NOLINT diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index b33a2bf5c5..fbd11b9cdb 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -36,7 +36,7 @@ #include #include "control_msgs/action/follow_joint_trajectory.hpp" -#include "joint_trajectory_controller_parameters.hpp" +#include "joint_trajectory_controller/joint_trajectory_controller_parameters.hpp" namespace joint_trajectory_controller { diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp index 00d28ab4c8..189805c17c 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -23,19 +23,19 @@ #include #include +#include "control_msgs/msg/mecanum_drive_controller_state.hpp" #include "controller_interface/chainable_controller_interface.hpp" -#include "mecanum_drive_controller/odometry.hpp" -#include "mecanum_drive_controller_parameters.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" #include "std_srvs/srv/set_bool.hpp" - -#include "control_msgs/msg/mecanum_drive_controller_state.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "nav_msgs/msg/odometry.hpp" #include "tf2_msgs/msg/tf_message.hpp" + +#include "mecanum_drive_controller/mecanum_drive_controller_parameters.hpp" +#include "mecanum_drive_controller/odometry.hpp" namespace mecanum_drive_controller { // name constants for state interfaces diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp index f5f5e97939..903ba825e3 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp @@ -40,7 +40,7 @@ #include "realtime_tools/realtime_server_goal_handle.hpp" // Project -#include "parallel_gripper_action_controller_parameters.hpp" +#include "parallel_gripper_controller/parallel_gripper_action_controller_parameters.hpp" namespace parallel_gripper_action_controller { diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index afd5555c0c..e0a36f7b6b 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -26,12 +26,13 @@ #include "control_msgs/msg/multi_dof_state_stamped.hpp" #include "control_toolbox/pid_ros.hpp" #include "controller_interface/chainable_controller_interface.hpp" -#include "pid_controller_parameters.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" #include "std_srvs/srv/set_bool.hpp" +#include "pid_controller/pid_controller_parameters.hpp" + namespace pid_controller { diff --git a/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp index 024aa41602..4c1d2adc14 100644 --- a/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp +++ b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp @@ -21,13 +21,14 @@ #include "controller_interface/controller_interface.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "pose_broadcaster_parameters.hpp" #include "rclcpp/publisher.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_publisher.hpp" #include "semantic_components/pose_sensor.hpp" #include "tf2_msgs/msg/tf_message.hpp" +#include "pose_broadcaster/pose_broadcaster_parameters.hpp" + namespace pose_broadcaster { diff --git a/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp b/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp index f1e2c8e72f..65a1d3092f 100644 --- a/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp +++ b/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp @@ -22,12 +22,13 @@ #include #include "controller_interface/controller_interface.hpp" -#include "range_sensor_broadcaster_parameters.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_publisher.hpp" #include "semantic_components/range_sensor.hpp" #include "sensor_msgs/msg/range.hpp" +#include "range_sensor_broadcaster/range_sensor_broadcaster_parameters.hpp" + namespace range_sensor_broadcaster { class RangeSensorBroadcaster : public controller_interface::ControllerInterface diff --git a/ros2_controllers-not-released.jazzy.repos b/ros2_controllers-not-released.jazzy.repos index 66352f4960..1b3910e7e7 100644 --- a/ros2_controllers-not-released.jazzy.repos +++ b/ros2_controllers-not-released.jazzy.repos @@ -1,5 +1,6 @@ repositories: - generate_parameter_library: - type: git - url: https://github.com/picknikrobotics/generate_parameter_library.git - version: main + ## EXAMPLE DEPENDENCY +# : +# type: git +# url: git@github.com:/.git +# version: master diff --git a/ros2_controllers-not-released.rolling.repos b/ros2_controllers-not-released.rolling.repos index 66352f4960..1b3910e7e7 100644 --- a/ros2_controllers-not-released.rolling.repos +++ b/ros2_controllers-not-released.rolling.repos @@ -1,5 +1,6 @@ repositories: - generate_parameter_library: - type: git - url: https://github.com/picknikrobotics/generate_parameter_library.git - version: main + ## EXAMPLE DEPENDENCY +# : +# type: git +# url: git@github.com:/.git +# version: master diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index c671534353..786132d2b0 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -25,8 +25,6 @@ #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" -#include "steering_controllers_library/steering_odometry.hpp" -#include "steering_controllers_library_parameters.hpp" // TODO(anyone): Replace with controller specific messages #include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" @@ -35,6 +33,9 @@ #include "nav_msgs/msg/odometry.hpp" #include "tf2_msgs/msg/tf_message.hpp" +#include "steering_controllers_library/steering_controllers_library_parameters.hpp" +#include "steering_controllers_library/steering_odometry.hpp" + namespace steering_controllers_library { class SteeringControllersLibrary : public controller_interface::ChainableControllerInterface diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index cd6251e803..3a1c996b78 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -37,12 +37,12 @@ #include "realtime_tools/realtime_publisher.hpp" #include "std_srvs/srv/empty.hpp" #include "tf2_msgs/msg/tf_message.hpp" + #include "tricycle_controller/odometry.hpp" #include "tricycle_controller/steering_limiter.hpp" #include "tricycle_controller/traction_limiter.hpp" - // auto-generated by generate_parameter_library -#include "tricycle_controller_parameters.hpp" +#include "tricycle_controller/tricycle_controller_parameters.hpp" namespace tricycle_controller { diff --git a/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp b/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp index 28e477a578..acc23034d2 100644 --- a/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp @@ -21,7 +21,7 @@ #include #include "steering_controllers_library/steering_controllers_library.hpp" -#include "tricycle_steering_controller_parameters.hpp" +#include "tricycle_steering_controller/tricycle_steering_controller_parameters.hpp" namespace tricycle_steering_controller { From 828f759144c1bb3489c9daf776448e5636d4a472 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 17 Jan 2025 11:21:24 +0100 Subject: [PATCH 62/84] Also run CI workflows if any pluginlib xml is changed (#1492) --- .github/workflows/humble-abi-compatibility.yml | 1 + .github/workflows/humble-binary-build.yml | 2 ++ .github/workflows/humble-coverage-build.yml | 2 ++ .github/workflows/humble-debian-build.yml | 1 + .github/workflows/humble-rhel-semi-binary-build.yml | 1 + .github/workflows/humble-semi-binary-build.yml | 2 ++ .github/workflows/humble-semi-binary-downstream-build.yml | 1 + .github/workflows/humble-source-build.yml | 1 + .github/workflows/jazzy-abi-compatibility.yml | 1 + .github/workflows/jazzy-binary-build.yml | 2 ++ .github/workflows/jazzy-debian-build.yml | 1 + .github/workflows/jazzy-rhel-semi-binary-build.yml | 1 + .github/workflows/jazzy-semi-binary-build.yml | 2 ++ .github/workflows/jazzy-semi-binary-downstream-build.yml | 1 + .github/workflows/jazzy-source-build.yml | 1 + .github/workflows/rolling-abi-compatibility.yml | 1 + .github/workflows/rolling-binary-build.yml | 2 ++ .github/workflows/rolling-compatibility-humble-binary-build.yml | 2 ++ .github/workflows/rolling-compatibility-jazzy-binary-build.yml | 2 ++ .github/workflows/rolling-coverage-build.yml | 2 ++ .github/workflows/rolling-debian-build.yml | 1 + .github/workflows/rolling-rhel-semi-binary-build.yml | 1 + .github/workflows/rolling-semi-binary-build.yml | 2 ++ .github/workflows/rolling-semi-binary-downstream-build.yml | 1 + .github/workflows/rolling-source-build.yml | 1 + 25 files changed, 35 insertions(+) diff --git a/.github/workflows/humble-abi-compatibility.yml b/.github/workflows/humble-abi-compatibility.yml index 6cfd2c6068..3e5892711b 100644 --- a/.github/workflows/humble-abi-compatibility.yml +++ b/.github/workflows/humble-abi-compatibility.yml @@ -14,6 +14,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers-not-released.humble.repos' + - '**.xml' concurrency: # cancel previous runs of the same workflow, except for pushes on humble branch diff --git a/.github/workflows/humble-binary-build.yml b/.github/workflows/humble-binary-build.yml index f013f18880..069a5240f5 100644 --- a/.github/workflows/humble-binary-build.yml +++ b/.github/workflows/humble-binary-build.yml @@ -17,6 +17,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers-not-released.humble.repos' + - '**.xml' push: branches: - humble @@ -30,6 +31,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers-not-released.humble.repos' + - '**.xml' schedule: # Run every morning to detect flakiness and broken dependencies - cron: '03 1 * * *' diff --git a/.github/workflows/humble-coverage-build.yml b/.github/workflows/humble-coverage-build.yml index 40d1de7052..55d4aa0cfd 100644 --- a/.github/workflows/humble-coverage-build.yml +++ b/.github/workflows/humble-coverage-build.yml @@ -15,6 +15,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.humble.repos' + - '**.xml' pull_request: branches: - humble @@ -29,6 +30,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.humble.repos' + - '**.xml' concurrency: # cancel previous runs of the same workflow, except for pushes on humble branch diff --git a/.github/workflows/humble-debian-build.yml b/.github/workflows/humble-debian-build.yml index e52ffe6842..bc92501862 100644 --- a/.github/workflows/humble-debian-build.yml +++ b/.github/workflows/humble-debian-build.yml @@ -14,6 +14,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.humble.repos' + - '**.xml' schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' diff --git a/.github/workflows/humble-rhel-semi-binary-build.yml b/.github/workflows/humble-rhel-semi-binary-build.yml index 6019d08e46..46d3dd1989 100644 --- a/.github/workflows/humble-rhel-semi-binary-build.yml +++ b/.github/workflows/humble-rhel-semi-binary-build.yml @@ -14,6 +14,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.humble.repos' + - '**.xml' schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' diff --git a/.github/workflows/humble-semi-binary-build.yml b/.github/workflows/humble-semi-binary-build.yml index 2d0437782b..1fa14dfdfa 100644 --- a/.github/workflows/humble-semi-binary-build.yml +++ b/.github/workflows/humble-semi-binary-build.yml @@ -16,6 +16,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.humble.repos' + - '**.xml' push: branches: - humble @@ -29,6 +30,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.humble.repos' + - '**.xml' schedule: # Run every morning to detect flakiness and broken dependencies - cron: '33 1 * * *' diff --git a/.github/workflows/humble-semi-binary-downstream-build.yml b/.github/workflows/humble-semi-binary-downstream-build.yml index fc185032e1..a9ba472ff1 100644 --- a/.github/workflows/humble-semi-binary-downstream-build.yml +++ b/.github/workflows/humble-semi-binary-downstream-build.yml @@ -17,6 +17,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros_controls.humble.repos' + - '**.xml' concurrency: group: ${{ github.workflow }}-${{ github.ref }} diff --git a/.github/workflows/humble-source-build.yml b/.github/workflows/humble-source-build.yml index 94af6edd8c..841aa86bfe 100644 --- a/.github/workflows/humble-source-build.yml +++ b/.github/workflows/humble-source-build.yml @@ -14,6 +14,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.humble.repos' + - '**.xml' schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 3 * * *' diff --git a/.github/workflows/jazzy-abi-compatibility.yml b/.github/workflows/jazzy-abi-compatibility.yml index 5da5fb9d00..033321bc75 100644 --- a/.github/workflows/jazzy-abi-compatibility.yml +++ b/.github/workflows/jazzy-abi-compatibility.yml @@ -13,6 +13,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers-not-released.jazzy.repos' + - '**.xml' concurrency: # cancel previous runs of the same workflow, except for pushes on master branch diff --git a/.github/workflows/jazzy-binary-build.yml b/.github/workflows/jazzy-binary-build.yml index 7dd294a55b..fb68d930e4 100644 --- a/.github/workflows/jazzy-binary-build.yml +++ b/.github/workflows/jazzy-binary-build.yml @@ -19,6 +19,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers-not-released.jazzy.repos' + - '**.xml' push: branches: - master @@ -31,6 +32,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers-not-released.jazzy.repos' + - '**.xml' schedule: # Run every morning to detect flakiness and broken dependencies - cron: '03 1 * * *' diff --git a/.github/workflows/jazzy-debian-build.yml b/.github/workflows/jazzy-debian-build.yml index b1a60f1528..b6b9e31bc2 100644 --- a/.github/workflows/jazzy-debian-build.yml +++ b/.github/workflows/jazzy-debian-build.yml @@ -14,6 +14,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.jazzy.repos' + - '**.xml' schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' diff --git a/.github/workflows/jazzy-rhel-semi-binary-build.yml b/.github/workflows/jazzy-rhel-semi-binary-build.yml index f39c9cc570..ed09d0efb9 100644 --- a/.github/workflows/jazzy-rhel-semi-binary-build.yml +++ b/.github/workflows/jazzy-rhel-semi-binary-build.yml @@ -14,6 +14,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.jazzy.repos' + - '**.xml' schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' diff --git a/.github/workflows/jazzy-semi-binary-build.yml b/.github/workflows/jazzy-semi-binary-build.yml index f6585f6a02..5a39ea5393 100644 --- a/.github/workflows/jazzy-semi-binary-build.yml +++ b/.github/workflows/jazzy-semi-binary-build.yml @@ -18,6 +18,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.jazzy.repos' + - '**.xml' push: branches: - master @@ -31,6 +32,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.jazzy.repos' + - '**.xml' schedule: # Run every morning to detect flakiness and broken dependencies - cron: '33 1 * * *' diff --git a/.github/workflows/jazzy-semi-binary-downstream-build.yml b/.github/workflows/jazzy-semi-binary-downstream-build.yml index 91ec818e91..a2db0ef7b6 100644 --- a/.github/workflows/jazzy-semi-binary-downstream-build.yml +++ b/.github/workflows/jazzy-semi-binary-downstream-build.yml @@ -17,6 +17,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros_controls.jazzy.repos' + - '**.xml' concurrency: group: ${{ github.workflow }}-${{ github.ref }} diff --git a/.github/workflows/jazzy-source-build.yml b/.github/workflows/jazzy-source-build.yml index 45f6d1d25e..76c9bd2892 100644 --- a/.github/workflows/jazzy-source-build.yml +++ b/.github/workflows/jazzy-source-build.yml @@ -14,6 +14,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.jazzy.repos' + - '**.xml' schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 3 * * *' diff --git a/.github/workflows/rolling-abi-compatibility.yml b/.github/workflows/rolling-abi-compatibility.yml index 1f0ec61d40..353597003a 100644 --- a/.github/workflows/rolling-abi-compatibility.yml +++ b/.github/workflows/rolling-abi-compatibility.yml @@ -13,6 +13,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers-not-released.rolling.repos' + - '**.xml' concurrency: # cancel previous runs of the same workflow, except for pushes on master branch diff --git a/.github/workflows/rolling-binary-build.yml b/.github/workflows/rolling-binary-build.yml index a3686144f3..1fd126f43c 100644 --- a/.github/workflows/rolling-binary-build.yml +++ b/.github/workflows/rolling-binary-build.yml @@ -19,6 +19,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers-not-released.rolling.repos' + - '**.xml' push: branches: - master @@ -32,6 +33,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers-not-released.rolling.repos' + - '**.xml' schedule: # Run every morning to detect flakiness and broken dependencies - cron: '03 1 * * *' diff --git a/.github/workflows/rolling-compatibility-humble-binary-build.yml b/.github/workflows/rolling-compatibility-humble-binary-build.yml index 6370db30d1..54f85ecf4b 100644 --- a/.github/workflows/rolling-compatibility-humble-binary-build.yml +++ b/.github/workflows/rolling-compatibility-humble-binary-build.yml @@ -19,6 +19,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.rolling.repos' + - '**.xml' push: branches: - master @@ -32,6 +33,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.rolling.repos' + - '**.xml' concurrency: # cancel previous runs of the same workflow, except for pushes on master branch diff --git a/.github/workflows/rolling-compatibility-jazzy-binary-build.yml b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml index 4da98f2d09..a3f406fe87 100644 --- a/.github/workflows/rolling-compatibility-jazzy-binary-build.yml +++ b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml @@ -19,6 +19,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.rolling.repos' + - '**.xml' push: branches: - master @@ -32,6 +33,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.rolling.repos' + - '**.xml' concurrency: # cancel previous runs of the same workflow, except for pushes on master branch diff --git a/.github/workflows/rolling-coverage-build.yml b/.github/workflows/rolling-coverage-build.yml index abea8a90db..cd40b1954d 100644 --- a/.github/workflows/rolling-coverage-build.yml +++ b/.github/workflows/rolling-coverage-build.yml @@ -15,6 +15,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.rolling.repos' + - '**.xml' pull_request: branches: - master @@ -29,6 +30,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.rolling.repos' + - '**.xml' concurrency: # cancel previous runs of the same workflow, except for pushes on master branch diff --git a/.github/workflows/rolling-debian-build.yml b/.github/workflows/rolling-debian-build.yml index 8f9394762b..23c9aaf834 100644 --- a/.github/workflows/rolling-debian-build.yml +++ b/.github/workflows/rolling-debian-build.yml @@ -14,6 +14,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.rolling.repos' + - '**.xml' schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' diff --git a/.github/workflows/rolling-rhel-semi-binary-build.yml b/.github/workflows/rolling-rhel-semi-binary-build.yml index 0f03ad02c2..c1abdb3c5c 100644 --- a/.github/workflows/rolling-rhel-semi-binary-build.yml +++ b/.github/workflows/rolling-rhel-semi-binary-build.yml @@ -14,6 +14,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.rolling.repos' + - '**.xml' schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' diff --git a/.github/workflows/rolling-semi-binary-build.yml b/.github/workflows/rolling-semi-binary-build.yml index ecb462bad0..6521e90b5c 100644 --- a/.github/workflows/rolling-semi-binary-build.yml +++ b/.github/workflows/rolling-semi-binary-build.yml @@ -18,6 +18,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.rolling.repos' + - '**.xml' push: branches: - master @@ -31,6 +32,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.rolling.repos' + - '**.xml' schedule: # Run every morning to detect flakiness and broken dependencies - cron: '33 1 * * *' diff --git a/.github/workflows/rolling-semi-binary-downstream-build.yml b/.github/workflows/rolling-semi-binary-downstream-build.yml index 0cf978e528..02ed08578b 100644 --- a/.github/workflows/rolling-semi-binary-downstream-build.yml +++ b/.github/workflows/rolling-semi-binary-downstream-build.yml @@ -17,6 +17,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros_controls.rolling.repos' + - '**.xml' concurrency: group: ${{ github.workflow }}-${{ github.ref }} diff --git a/.github/workflows/rolling-source-build.yml b/.github/workflows/rolling-source-build.yml index e19a1e8cf7..e409150974 100644 --- a/.github/workflows/rolling-source-build.yml +++ b/.github/workflows/rolling-source-build.yml @@ -14,6 +14,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.rolling.repos' + - '**.xml' schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 3 * * *' From cd6d5f8445b33c49ee7b994e4106b2abafd5bc01 Mon Sep 17 00:00:00 2001 From: Julia Jia Date: Mon, 20 Jan 2025 07:08:27 -0800 Subject: [PATCH 63/84] Remove empty callbacks (#1488) --- .../include/admittance_controller/admittance_controller.hpp | 3 --- admittance_controller/src/admittance_controller.cpp | 6 ------ .../joint_trajectory_controller.hpp | 3 --- .../src/joint_trajectory_controller.cpp | 6 ------ pid_controller/include/pid_controller/pid_controller.hpp | 3 --- pid_controller/src/pid_controller.cpp | 6 ------ 6 files changed, 27 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index dc69431f1c..8f0bc25973 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -67,9 +67,6 @@ class AdmittanceController : public controller_interface::ChainableControllerInt controller_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; - controller_interface::CallbackReturn on_cleanup( - const rclcpp_lifecycle::State & previous_state) override; - controller_interface::CallbackReturn on_error( const rclcpp_lifecycle::State & previous_state) override; diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 468c02624b..7bcd770a52 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -479,12 +479,6 @@ controller_interface::CallbackReturn AdmittanceController::on_deactivate( return CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn AdmittanceController::on_cleanup( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - return controller_interface::CallbackReturn::SUCCESS; -} - controller_interface::CallbackReturn AdmittanceController::on_error( const rclcpp_lifecycle::State & /*previous_state*/) { diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 6c265ecacb..af0f35be18 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -78,9 +78,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa controller_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; - controller_interface::CallbackReturn on_cleanup( - const rclcpp_lifecycle::State & previous_state) override; - controller_interface::CallbackReturn on_error( const rclcpp_lifecycle::State & previous_state) override; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 9eea619d25..a4ad5210be 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1043,12 +1043,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( return CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn JointTrajectoryController::on_cleanup( - const rclcpp_lifecycle::State &) -{ - return CallbackReturn::SUCCESS; -} - controller_interface::CallbackReturn JointTrajectoryController::on_error( const rclcpp_lifecycle::State &) { diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index e0a36f7b6b..39ec58be6b 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -62,9 +62,6 @@ class PidController : public controller_interface::ChainableControllerInterface controller_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; - controller_interface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; - controller_interface::return_type update_reference_from_subscribers( const rclcpp::Time & time, const rclcpp::Duration & period) override; diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index f3b2ba33a0..521227b49c 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -443,12 +443,6 @@ controller_interface::CallbackReturn PidController::on_activate( return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn PidController::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - return controller_interface::CallbackReturn::SUCCESS; -} - controller_interface::return_type PidController::update_reference_from_subscribers( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { From 3d14b4260a1929fda117da844ea128716d13260d Mon Sep 17 00:00:00 2001 From: Arthur Lovekin <62527557+arthurlovekin@users.noreply.github.com> Date: Tue, 21 Jan 2025 02:26:32 -0800 Subject: [PATCH 64/84] Make diff_drive_controller a ChainableControllerInterface (#1485) --- diff_drive_controller/diff_drive_plugin.xml | 2 +- .../diff_drive_controller.hpp | 27 +- .../src/diff_drive_controller.cpp | 172 +++++-- .../test/test_diff_drive_controller.cpp | 419 ++++++++++++++++-- 4 files changed, 545 insertions(+), 75 deletions(-) diff --git a/diff_drive_controller/diff_drive_plugin.xml b/diff_drive_controller/diff_drive_plugin.xml index 08d41cf69c..88fdfd9ecb 100644 --- a/diff_drive_controller/diff_drive_plugin.xml +++ b/diff_drive_controller/diff_drive_plugin.xml @@ -1,5 +1,5 @@ - + The differential drive controller transforms linear and angular velocity messages into signals for each wheel(s) for a differential drive robot. 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 265e000b79..11ecd9337b 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 @@ -25,14 +25,14 @@ #include #include -#include "controller_interface/controller_interface.hpp" +#include "controller_interface/chainable_controller_interface.hpp" #include "diff_drive_controller/odometry.hpp" #include "diff_drive_controller/speed_limiter.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "odometry.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_box.hpp" +#include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" #include "tf2_msgs/msg/tf_message.hpp" @@ -41,7 +41,7 @@ namespace diff_drive_controller { -class DiffDriveController : public controller_interface::ControllerInterface +class DiffDriveController : public controller_interface::ChainableControllerInterface { using TwistStamped = geometry_msgs::msg::TwistStamped; @@ -52,7 +52,11 @@ class DiffDriveController : public controller_interface::ControllerInterface controller_interface::InterfaceConfiguration state_interface_configuration() const override; - controller_interface::return_type update( + // Chainable controller replaces update() with the following two functions + controller_interface::return_type update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + controller_interface::return_type update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) override; controller_interface::CallbackReturn on_init() override; @@ -73,6 +77,10 @@ class DiffDriveController : public controller_interface::ControllerInterface const rclcpp_lifecycle::State & previous_state) override; protected: + bool on_set_chained_mode(bool chained_mode) override; + + std::vector on_export_reference_interfaces() override; + struct WheelHandle { std::reference_wrapper feedback; @@ -100,7 +108,7 @@ class DiffDriveController : public controller_interface::ControllerInterface Odometry odometry_; // Timeout to consider cmd_vel commands old - std::chrono::milliseconds cmd_vel_timeout_{500}; + rclcpp::Duration cmd_vel_timeout_ = rclcpp::Duration::from_seconds(0.5); std::shared_ptr> odometry_publisher_ = nullptr; std::shared_ptr> @@ -114,11 +122,9 @@ class DiffDriveController : public controller_interface::ControllerInterface bool subscriber_is_active_ = false; 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 + realtime_tools::RealtimeBuffer> received_velocity_msg_ptr_{nullptr}; + std::queue> previous_two_commands_; // speed limiters std::unique_ptr limiter_linear_; std::unique_ptr limiter_angular_; @@ -139,6 +145,9 @@ class DiffDriveController : public controller_interface::ControllerInterface bool reset(); void halt(); + +private: + void reset_buffers(); }; } // namespace diff_drive_controller #endif // DIFF_DRIVE_CONTROLLER__DIFF_DRIVE_CONTROLLER_HPP_ diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 79bdd90419..7356d80219 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -45,7 +45,7 @@ using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; using lifecycle_msgs::msg::State; -DiffDriveController::DiffDriveController() : controller_interface::ControllerInterface() {} +DiffDriveController::DiffDriveController() : controller_interface::ChainableControllerInterface() {} const char * DiffDriveController::feedback_type() const { @@ -97,8 +97,8 @@ InterfaceConfiguration DiffDriveController::state_interface_configuration() cons return {interface_configuration_type::INDIVIDUAL, conf_names}; } -controller_interface::return_type DiffDriveController::update( - const rclcpp::Time & time, const rclcpp::Duration & period) +controller_interface::return_type DiffDriveController::update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { auto logger = get_node()->get_logger(); if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE) @@ -111,31 +111,64 @@ controller_interface::return_type DiffDriveController::update( return controller_interface::return_type::OK; } - // 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; }); + const std::shared_ptr command_msg_ptr = *(received_velocity_msg_ptr_.readFromRT()); - if (last_command_msg_ == nullptr) + if (command_msg_ptr == 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 - command_msg_ptr->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; + reference_interfaces_[0] = 0.0; + reference_interfaces_[1] = 0.0; + } + else if ( + std::isfinite(command_msg_ptr->twist.linear.x) && + std::isfinite(command_msg_ptr->twist.angular.z)) + { + reference_interfaces_[0] = command_msg_ptr->twist.linear.x; + reference_interfaces_[1] = command_msg_ptr->twist.angular.z; + } + else + { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + logger, *get_node()->get_clock(), cmd_vel_timeout_.seconds() * 1000, + "Command message contains NaNs. Not updating reference interfaces."); + } + + previous_update_timestamp_ = time; + + return controller_interface::return_type::OK; +} + +controller_interface::return_type DiffDriveController::update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + auto logger = get_node()->get_logger(); + if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE) + { + if (!is_halted) + { + halt(); + is_halted = true; + } + return controller_interface::return_type::OK; } // command may be limited further by SpeedLimit, // without affecting the stored twist command - TwistStamped command = *last_command_msg_; - double & linear_command = command.twist.linear.x; - double & angular_command = command.twist.angular.z; + double linear_command = reference_interfaces_[0]; + double angular_command = reference_interfaces_[1]; - previous_update_timestamp_ = time; + if (!std::isfinite(linear_command) || !std::isfinite(angular_command)) + { + // NaNs occur on initialization when the reference interfaces are not yet set + return controller_interface::return_type::OK; + } // Apply (possibly new) multipliers: const double wheel_separation = params_.wheel_separation_multiplier * params_.wheel_separation; @@ -232,22 +265,27 @@ controller_interface::return_type DiffDriveController::update( } } - auto & last_command = previous_commands_.back().twist; - auto & second_to_last_command = previous_commands_.front().twist; - limiter_linear_->limit( - linear_command, last_command.linear.x, second_to_last_command.linear.x, period.seconds()); - limiter_angular_->limit( - angular_command, last_command.angular.z, second_to_last_command.angular.z, period.seconds()); + double & last_linear = previous_two_commands_.back()[0]; + double & second_to_last_linear = previous_two_commands_.front()[0]; + double & last_angular = previous_two_commands_.back()[1]; + double & second_to_last_angular = previous_two_commands_.front()[1]; - previous_commands_.pop(); - previous_commands_.emplace(command); + limiter_linear_->limit(linear_command, last_linear, second_to_last_linear, period.seconds()); + limiter_angular_->limit(angular_command, last_angular, second_to_last_angular, period.seconds()); + previous_two_commands_.pop(); + previous_two_commands_.push({{linear_command, angular_command}}); // Publish limited velocity if (publish_limited_velocity_ && realtime_limited_velocity_publisher_->trylock()) { auto & limited_velocity_command = realtime_limited_velocity_publisher_->msg_; limited_velocity_command.header.stamp = time; - limited_velocity_command.twist = command.twist; + limited_velocity_command.twist.linear.x = linear_command; + limited_velocity_command.twist.linear.y = 0.0; + limited_velocity_command.twist.linear.z = 0.0; + limited_velocity_command.twist.angular.x = 0.0; + limited_velocity_command.twist.angular.y = 0.0; + limited_velocity_command.twist.angular.z = angular_command; realtime_limited_velocity_publisher_->unlockAndPublish(); } @@ -294,7 +332,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( odometry_.setWheelParams(wheel_separation, left_wheel_radius, right_wheel_radius); odometry_.setVelocityRollingWindowSize(static_cast(params_.velocity_rolling_window_size)); - cmd_vel_timeout_ = std::chrono::milliseconds{static_cast(params_.cmd_vel_timeout * 1000.0)}; + cmd_vel_timeout_ = rclcpp::Duration::from_seconds(params_.cmd_vel_timeout); publish_limited_velocity_ = params_.publish_limited_velocity; // TODO(christophfroehlich) remove deprecated parameters @@ -387,13 +425,6 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( limited_velocity_publisher_); } - 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(*last_command_msg_); - previous_commands_.emplace(*last_command_msg_); - // initialize command subscriber velocity_command_subscriber_ = get_node()->create_subscription( DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), @@ -410,10 +441,26 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( get_node()->get_logger(), "Received TwistStamped with zero timestamp, setting it to current " "time, this message will only be shown once"); - msg->header.stamp = get_node()->get_clock()->now(); + msg->header.stamp = get_node()->now(); + } + + const auto current_time_diff = get_node()->now() - msg->header.stamp; + + if ( + cmd_vel_timeout_ == rclcpp::Duration::from_seconds(0.0) || + current_time_diff < cmd_vel_timeout_) + { + received_velocity_msg_ptr_.writeFromNonRT(msg); + } + else + { + RCLCPP_WARN( + get_node()->get_logger(), + "Ignoring the received message (timestamp %.10f) because it is older than " + "the current time by %.10f seconds, which exceeds the allowed timeout (%.4f)", + rclcpp::Time(msg->header.stamp).seconds(), current_time_diff.seconds(), + cmd_vel_timeout_.seconds()); } - received_velocity_msg_ptr_.set([msg](std::shared_ptr & stored_value) - { stored_value = std::move(msg); }); }); // initialize odometry publisher and message @@ -527,6 +574,7 @@ controller_interface::CallbackReturn DiffDriveController::on_deactivate( halt(); is_halted = true; } + reset_buffers(); registered_left_wheel_handles_.clear(); registered_right_wheel_handles_.clear(); return controller_interface::CallbackReturn::SUCCESS; @@ -556,9 +604,7 @@ bool DiffDriveController::reset() { odometry_.resetOdometry(); - // release the old queue - std::queue empty; - std::swap(previous_commands_, empty); + reset_buffers(); registered_left_wheel_handles_.clear(); registered_right_wheel_handles_.clear(); @@ -566,11 +612,33 @@ bool DiffDriveController::reset() subscriber_is_active_ = false; velocity_command_subscriber_.reset(); - received_velocity_msg_ptr_.set(nullptr); is_halted = false; return true; } +void DiffDriveController::reset_buffers() +{ + reference_interfaces_ = std::vector(2, std::numeric_limits::quiet_NaN()); + // Empty out the old queue. Fill with zeros (not NaN) to catch early accelerations. + std::queue> empty; + std::swap(previous_two_commands_, empty); + previous_two_commands_.push({{0.0, 0.0}}); + previous_two_commands_.push({{0.0, 0.0}}); + + // Fill RealtimeBuffer with NaNs so it will contain a known value + // but still indicate that no command has yet been sent. + received_velocity_msg_ptr_.reset(); + std::shared_ptr empty_msg_ptr = std::make_shared(); + empty_msg_ptr->header.stamp = get_node()->now(); + empty_msg_ptr->twist.linear.x = std::numeric_limits::quiet_NaN(); + empty_msg_ptr->twist.linear.y = std::numeric_limits::quiet_NaN(); + empty_msg_ptr->twist.linear.z = std::numeric_limits::quiet_NaN(); + empty_msg_ptr->twist.angular.x = std::numeric_limits::quiet_NaN(); + empty_msg_ptr->twist.angular.y = std::numeric_limits::quiet_NaN(); + empty_msg_ptr->twist.angular.z = std::numeric_limits::quiet_NaN(); + received_velocity_msg_ptr_.writeFromNonRT(empty_msg_ptr); +} + void DiffDriveController::halt() { const auto halt_wheels = [](auto & wheel_handles) @@ -636,9 +704,35 @@ controller_interface::CallbackReturn DiffDriveController::configure_side( return controller_interface::CallbackReturn::SUCCESS; } + +bool DiffDriveController::on_set_chained_mode(bool chained_mode) +{ + // Always accept switch to/from chained mode (without linting type-cast error) + return true || chained_mode; +} + +std::vector +DiffDriveController::on_export_reference_interfaces() +{ + const int nr_ref_itfs = 2; + reference_interfaces_.resize(nr_ref_itfs, std::numeric_limits::quiet_NaN()); + std::vector reference_interfaces; + reference_interfaces.reserve(nr_ref_itfs); + + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name(), std::string("linear/") + hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[0])); + + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name(), std::string("angular/") + hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[1])); + + return reference_interfaces; +} + } // namespace diff_drive_controller #include "class_loader/register_macro.hpp" CLASS_LOADER_REGISTER_CLASS( - diff_drive_controller::DiffDriveController, controller_interface::ControllerInterface) + diff_drive_controller::DiffDriveController, controller_interface::ChainableControllerInterface) diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index f2fc671920..25cf2405f2 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -47,10 +47,7 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr 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; + return *(received_velocity_msg_ptr_.readFromNonRT()); } /** @@ -80,6 +77,11 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr { return realtime_odometry_publisher_; } + + // Declare these tests as friends so we can access controller_->reference_interfaces_ + FRIEND_TEST(TestDiffDriveController, chainable_controller_unchained_mode); + FRIEND_TEST(TestDiffDriveController, chainable_controller_chained_mode); + FRIEND_TEST(TestDiffDriveController, deactivate_then_activate); }; class TestDiffDriveController : public ::testing::Test @@ -105,6 +107,11 @@ class TestDiffDriveController : public ::testing::Test * angular - the magnitude of the angular command in geometry_msgs::twist message */ void publish(double linear, double angular) + { + publish_timestamped(linear, angular, pub_node->get_clock()->now()); + } + + void publish_timestamped(double linear, double angular, rclcpp::Time timestamp) { int wait_count = 0; auto topic = velocity_publisher->get_topic_name(); @@ -120,7 +127,7 @@ class TestDiffDriveController : public ::testing::Test } geometry_msgs::msg::TwistStamped velocity_message; - velocity_message.header.stamp = pub_node->get_clock()->now(); + velocity_message.header.stamp = timestamp; velocity_message.twist.linear.x = linear; velocity_message.twist.angular.z = angular; velocity_publisher->publish(velocity_message); @@ -239,7 +246,9 @@ TEST_F(TestDiffDriveController, configure_fails_with_mismatching_wheel_side_size ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } -TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) +TEST_F( + TestDiffDriveController, + command_and_state_interface_configuration_succeeds_when_wheels_are_specified) { ASSERT_EQ(InitController(), controller_interface::return_type::OK); @@ -451,14 +460,28 @@ TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned) TEST_F(TestDiffDriveController, test_speed_limiter) { + // If you send a linear velocity command without acceleration limits, + // then the wheel velocity command (rotations/s) will be: + // ideal_wheel_velocity_command (rotations/s) = linear_velocity_command (m/s) / wheel_radius (m). + // (The velocity command looks like a step function). + // However, if there are acceleration limits, then the actual wheel velocity command + // should always be less than the ideal velocity, and should only become + // equal at time = linear_velocity_command (m/s) / acceleration_limit (m/s^2) + + const double max_acceleration = 2.0; + const double max_deceleration = -4.0; + const double max_acceleration_reverse = -8.0; + const double max_deceleration_reverse = 10.0; ASSERT_EQ( InitController( left_wheel_names, right_wheel_names, { - rclcpp::Parameter("linear.x.max_acceleration", rclcpp::ParameterValue(2.0)), - rclcpp::Parameter("linear.x.max_deceleration", rclcpp::ParameterValue(-4.0)), - rclcpp::Parameter("linear.x.max_acceleration_reverse", rclcpp::ParameterValue(-8.0)), - rclcpp::Parameter("linear.x.max_deceleration_reverse", rclcpp::ParameterValue(10.0)), + rclcpp::Parameter("linear.x.max_acceleration", rclcpp::ParameterValue(max_acceleration)), + rclcpp::Parameter("linear.x.max_deceleration", rclcpp::ParameterValue(max_deceleration)), + rclcpp::Parameter( + "linear.x.max_acceleration_reverse", rclcpp::ParameterValue(max_acceleration_reverse)), + rclcpp::Parameter( + "linear.x.max_deceleration_reverse", rclcpp::ParameterValue(max_deceleration_reverse)), }), controller_interface::return_type::OK); @@ -499,17 +522,18 @@ TEST_F(TestDiffDriveController, test_speed_limiter) // wait for msg is be published to the system controller_->wait_for_twist(executor); - // should be in acceleration limit - const double time_acc = linear / 2.0; + const double time_acc = linear / max_acceleration; for (int i = 0; i < floor(time_acc / dt) - 1; ++i) { ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), controller_interface::return_type::OK); EXPECT_GT(linear / wheel_radius, left_wheel_vel_cmd_.get_value()) - << "at t: " << i * dt << "s, but should be t: " << time_acc; + << "at t: " << i * dt + << "s, but this angular velocity should only be achieved at t: " << time_acc; EXPECT_GT(linear / wheel_radius, right_wheel_vel_cmd_.get_value()) - << "at t: " << i * dt << "s, but should be t: " << time_acc; + << "at t: " << i * dt + << "s, but this angular velocity should only be achieved at t: " << time_acc; } ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), @@ -535,17 +559,18 @@ TEST_F(TestDiffDriveController, test_speed_limiter) // wait for msg is be published to the system controller_->wait_for_twist(executor); - // should be in acceleration limit - const double time_acc = -1.0 / (-4.0); + const double time_acc = -1.0 / max_deceleration; for (int i = 0; i < floor(time_acc / dt) - 1; ++i) { ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), controller_interface::return_type::OK); EXPECT_LT(linear / wheel_radius, left_wheel_vel_cmd_.get_value()) - << "at t: " << i * dt << "s, but should be t: " << time_acc; + << "at t: " << i * dt + << "s, but this angular velocity should only be achieved at t: " << time_acc; EXPECT_LT(linear / wheel_radius, right_wheel_vel_cmd_.get_value()) - << "at t: " << i * dt << "s, but should be t: " << time_acc; + << "at t: " << i * dt + << "s, but this angular velocity should only be achieved at t: " << time_acc; } ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), @@ -571,17 +596,18 @@ TEST_F(TestDiffDriveController, test_speed_limiter) // wait for msg is be published to the system controller_->wait_for_twist(executor); - // should be in acceleration limit - const double time_acc = -1.0 / (-8.0); + const double time_acc = -1.0 / max_acceleration_reverse; for (int i = 0; i < floor(time_acc / dt) - 1; ++i) { ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), controller_interface::return_type::OK); EXPECT_LT(linear / wheel_radius, left_wheel_vel_cmd_.get_value()) - << "at t: " << i * dt << "s, but should be t: " << time_acc; + << "at t: " << i * dt + << "s, but this angular velocity should only be achieved at t: " << time_acc; EXPECT_LT(linear / wheel_radius, right_wheel_vel_cmd_.get_value()) - << "at t: " << i * dt << "s, but should be t: " << time_acc; + << "at t: " << i * dt + << "s, but this angular velocity should only be achieved at t: " << time_acc; } ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), @@ -607,17 +633,18 @@ TEST_F(TestDiffDriveController, test_speed_limiter) // wait for msg is be published to the system controller_->wait_for_twist(executor); - // should be in acceleration limit - const double time_acc = 1.0 / (10.0); + const double time_acc = 1.0 / max_deceleration_reverse; for (int i = 0; i < floor(time_acc / dt) - 1; ++i) { ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), controller_interface::return_type::OK); EXPECT_GT(linear / wheel_radius, left_wheel_vel_cmd_.get_value()) - << "at t: " << i * dt << "s, but should be t: " << time_acc; + << "at t: " << i * dt + << "s, but this angular velocity should only be achieved at t: " << time_acc; EXPECT_GT(linear / wheel_radius, right_wheel_vel_cmd_.get_value()) - << "at t: " << i * dt << "s, but should be t: " << time_acc; + << "at t: " << i * dt + << "s, but this angular velocity should only be achieved at t: " << time_acc; } ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), @@ -765,6 +792,346 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) executor.cancel(); } +// When not in chained mode, we want to test that +// 1. The controller is configurable and all lifecycle functions work properly +// 2. command_interfaces are set to 0.0 when cmd_vel_timeout_ is exceeded and on deactivation +// 3. command_interfaces are set to correct command values the command messages are not timed-out. +// In particular, make sure that the command_interface is not set to NaN right when it starts up. +TEST_F(TestDiffDriveController, chainable_controller_unchained_mode) +{ + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("wheel_separation", 0.4), rclcpp::Parameter("wheel_radius", 1.0)}), + controller_interface::return_type::OK); + // choose radius = 1 so that the command values (rev/s) are the same as the linear velocity (m/s) + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_TRUE(controller_->is_chainable()); + ASSERT_TRUE(controller_->set_chained_mode(false)); + ASSERT_FALSE(controller_->is_in_chained_mode()); + + auto state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + assignResourcesPosFeedback(); + + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + waitForSetup(); + + // Reference interfaces should be NaN on initialization + // (Note: reference_interfaces_ is protected, but this is + // a FRIEND_TEST so we can use it) + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + // But NaNs should not propagate to command interfaces + // (these are set to 0.1 and 0.2 in InitController) + ASSERT_FALSE(std::isnan(left_wheel_vel_cmd_.get_value())); + ASSERT_FALSE(std::isnan(right_wheel_vel_cmd_.get_value())); + + // Check that a late command message causes the command interfaces to be set to 0.0 + const double linear = 1.0; + publish(linear, 0.0); + + // delay enough time to trigger the timeout (cmd_vel_timeout_ = 0.5s) + controller_->wait_for_twist(executor); + std::this_thread::sleep_for(std::chrono::milliseconds(501)); + + ASSERT_EQ( + controller_->update(pub_node->get_clock()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()) + << "Wheels should halt if command message is older than cmd_vel_timeout"; + EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()) + << "Wheels should halt if command message is older than cmd_vel_timeout"; + + // Now check that a timely published command message sets the command interfaces to the correct + // values + publish(linear, 0.0); + // wait for msg is be published to the system + controller_->wait_for_twist(executor); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_EQ(linear, left_wheel_vel_cmd_.get_value()); + EXPECT_EQ(linear, right_wheel_vel_cmd_.get_value()); + + // Now check that the command interfaces are set to 0.0 on deactivation + // (despite calls to update()) + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + state = controller_->get_node()->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()) << "Wheels should be halted on deactivate()"; + EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()) << "Wheels should be halted on deactivate()"; + + // cleanup + state = controller_->get_node()->cleanup(); + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()) << "Wheels should be halted on cleanup()"; + EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()) << "Wheels should be halted on cleanup()"; + + state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + executor.cancel(); +} + +// When in chained mode, we want to test that +// 1. The controller is configurable and all lifecycle functions work properly +// 2. command_interfaces are set to 0.0 on deactivation +// 3. command_interfaces are set to correct command values (not set to NaN right when it starts up) +TEST_F(TestDiffDriveController, chainable_controller_chained_mode) +{ + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("wheel_separation", 0.4), rclcpp::Parameter("wheel_radius", 1.0)}), + controller_interface::return_type::OK); + // choose radius = 1 so that the command values (rev/s) are the same as the linear velocity (m/s) + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_TRUE(controller_->is_chainable()); + ASSERT_TRUE(controller_->set_chained_mode(true)); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + auto state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + assignResourcesPosFeedback(); + + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + waitForSetup(); + + // Reference interfaces should be NaN on initialization + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + // But NaNs should not propagate to command interfaces + // (these are set to 0.1 and 0.2 in InitController) + ASSERT_FALSE(std::isnan(left_wheel_vel_cmd_.get_value())); + ASSERT_FALSE(std::isnan(right_wheel_vel_cmd_.get_value())); + + // Imitate preceding controllers by setting reference_interfaces_ + // (Note: reference_interfaces_ is protected, but this is + // a FRIEND_TEST so we can use it) + const double linear = 3.0; + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_EQ(linear, left_wheel_vel_cmd_.get_value()); + EXPECT_EQ(linear, right_wheel_vel_cmd_.get_value()); + + // Now check that the command interfaces are set to 0.0 on deactivation + // (despite calls to update()) + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + state = controller_->get_node()->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()) << "Wheels should be halted on deactivate()"; + EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()) << "Wheels should be halted on deactivate()"; + + // cleanup + state = controller_->get_node()->cleanup(); + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()) << "Wheels should be halted on cleanup()"; + EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()) << "Wheels should be halted on cleanup()"; + + state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + executor.cancel(); +} + +TEST_F(TestDiffDriveController, reference_interfaces_are_properly_exported) +{ + ASSERT_EQ( + InitController(left_wheel_names, right_wheel_names), controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), 2) + << "Expected exactly 2 reference interfaces: linear and angular"; + + const std::string expected_prefix_name = std::string(controller_->get_node()->get_name()); + const std::string expected_linear_interface_name = + std::string("linear/") + hardware_interface::HW_IF_VELOCITY; + const std::string expected_angular_interface_name = + std::string("angular/") + hardware_interface::HW_IF_VELOCITY; + const std::string expected_linear_name = + expected_prefix_name + std::string("/") + expected_linear_interface_name; + const std::string expected_angular_name = + expected_prefix_name + std::string("/") + expected_angular_interface_name; + + ASSERT_EQ(reference_interfaces[0]->get_name(), expected_linear_name); + ASSERT_EQ(reference_interfaces[1]->get_name(), expected_angular_name); + + EXPECT_EQ(reference_interfaces[0]->get_prefix_name(), expected_prefix_name); + EXPECT_EQ(reference_interfaces[0]->get_interface_name(), expected_linear_interface_name); + EXPECT_EQ(reference_interfaces[1]->get_prefix_name(), expected_prefix_name); + EXPECT_EQ(reference_interfaces[1]->get_interface_name(), expected_angular_interface_name); +} + +// Make sure that the controller is properly reset when deactivated +// and accepts new commands as expected when it is activated again. +TEST_F(TestDiffDriveController, deactivate_then_activate) +{ + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("wheel_separation", 0.4), rclcpp::Parameter("wheel_radius", 1.0)}), + controller_interface::return_type::OK); + // choose radius = 1 so that the command values (rev/s) are the same as the linear velocity (m/s) + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_TRUE(controller_->set_chained_mode(false)); + + auto state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + assignResourcesPosFeedback(); + + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + waitForSetup(); + + // Reference interfaces should be NaN on initialization + // (Note: reference_interfaces_ is protected, but this is + // a FRIEND_TEST so we can use it) + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + // But NaNs should not propagate to command interfaces + // (these are set to 0.1 and 0.2 in InitController) + ASSERT_FALSE(std::isnan(left_wheel_vel_cmd_.get_value())); + ASSERT_FALSE(std::isnan(right_wheel_vel_cmd_.get_value())); + + // published command message sets the command interfaces to the correct values + const double linear = 1.0; + publish(linear, 0.0); + // wait for msg is be published to the system + controller_->wait_for_twist(executor); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_EQ(linear, left_wheel_vel_cmd_.get_value()); + EXPECT_EQ(linear, right_wheel_vel_cmd_.get_value()); + + // Now check that the command interfaces are set to 0.0 on deactivation + // (despite calls to update()) + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + state = controller_->get_node()->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()) << "Wheels should be halted on deactivate()"; + EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()) << "Wheels should be halted on deactivate()"; + + // Activate again + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + waitForSetup(); + + // (Note: reference_interfaces_ is protected, but this is + // a FRIEND_TEST so we can use it) + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)) + << "Reference interfaces should initially be NaN on activation"; + } + EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()) + << "Wheels should still have the same command as when they were last set (on deactivation)"; + EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()) + << "Wheels should still have the same command as when they were last set (on deactivation)"; + + // A new command should work as expected + publish(linear, 0.0); + controller_->wait_for_twist(executor); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_EQ(linear, left_wheel_vel_cmd_.get_value()); + EXPECT_EQ(linear, right_wheel_vel_cmd_.get_value()); + + // Deactivate again and cleanup + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + state = controller_->get_node()->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + state = controller_->get_node()->cleanup(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); + executor.cancel(); +} + +TEST_F(TestDiffDriveController, command_with_zero_timestamp_is_accepted_with_warning) +{ + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("wheel_separation", 0.4), rclcpp::Parameter("wheel_radius", 1.0)}), + controller_interface::return_type::OK); + // choose radius = 1 so that the command values (rev/s) are the same as the linear velocity (m/s) + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_TRUE(controller_->set_chained_mode(false)); + + auto state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + assignResourcesPosFeedback(); + + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + waitForSetup(); + + // published command message with zero timestamp sets the command interfaces to the correct values + const double linear = 1.0; + publish_timestamped(linear, 0.0, rclcpp::Time(0, 0, RCL_ROS_TIME)); + // wait for msg is be published to the system + controller_->wait_for_twist(executor); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_EQ(linear, left_wheel_vel_cmd_.get_value()); + EXPECT_EQ(linear, right_wheel_vel_cmd_.get_value()); + + // Deactivate and cleanup + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + state = controller_->get_node()->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + state = controller_->get_node()->cleanup(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); + executor.cancel(); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); From af572aafdc5b3a2afb4d67c9eccfc631d3759eb5 Mon Sep 17 00:00:00 2001 From: Victor Coutinho Vieira Santos <69547580+ViktorCVS@users.noreply.github.com> Date: Wed, 22 Jan 2025 06:54:02 -0300 Subject: [PATCH 65/84] Improve antiwindup description (#1502) --- pid_controller/src/pid_controller.yaml | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml index 651cc1e7de..2d91b5a677 100644 --- a/pid_controller/src/pid_controller.yaml +++ b/pid_controller/src/pid_controller.yaml @@ -63,17 +63,20 @@ pid_controller: antiwindup: { type: bool, default_value: false, - description: "Antiwindup functionality." + description: "Antiwindup functionality. When set to true, limits + the integral error to prevent windup; otherwise, constrains the + integral contribution to the control output. i_clamp_max and + i_clamp_min are applied in both scenarios." } i_clamp_max: { type: double, default_value: 0.0, - description: "Upper integral clamp. Only used if antiwindup is activated." + description: "Upper integral clamp." } i_clamp_min: { type: double, default_value: 0.0, - description: "Lower integral clamp. Only used if antiwindup is activated." + description: "Lower integral clamp." } feedforward_gain: { type: double, From 04bdbe0615160faa1d2c59b968a9b68314c4eb15 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 29 Jan 2025 18:42:40 +0100 Subject: [PATCH 66/84] Use jazzy branch for realtime_tools (#1508) --- ros2_controllers.jazzy.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_controllers.jazzy.repos b/ros2_controllers.jazzy.repos index 8c20eccc96..6a52a6b52a 100644 --- a/ros2_controllers.jazzy.repos +++ b/ros2_controllers.jazzy.repos @@ -6,7 +6,7 @@ repositories: realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git - version: master + version: jazzy control_msgs: type: git url: https://github.com/ros-controls/control_msgs.git From 2e1052668c999b35c81569daf030a3837e8be551 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 29 Jan 2025 19:10:05 +0000 Subject: [PATCH 67/84] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 5 +++++ admittance_controller/CHANGELOG.rst | 6 ++++++ bicycle_steering_controller/CHANGELOG.rst | 5 +++++ diff_drive_controller/CHANGELOG.rst | 7 +++++++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 5 +++++ forward_command_controller/CHANGELOG.rst | 5 +++++ gpio_controllers/CHANGELOG.rst | 5 +++++ gripper_controllers/CHANGELOG.rst | 5 +++++ imu_sensor_broadcaster/CHANGELOG.rst | 5 +++++ joint_state_broadcaster/CHANGELOG.rst | 5 +++++ joint_trajectory_controller/CHANGELOG.rst | 6 ++++++ mecanum_drive_controller/CHANGELOG.rst | 5 +++++ parallel_gripper_controller/CHANGELOG.rst | 5 +++++ pid_controller/CHANGELOG.rst | 7 +++++++ pose_broadcaster/CHANGELOG.rst | 5 +++++ position_controllers/CHANGELOG.rst | 3 +++ range_sensor_broadcaster/CHANGELOG.rst | 5 +++++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ steering_controllers_library/CHANGELOG.rst | 5 +++++ tricycle_controller/CHANGELOG.rst | 5 +++++ tricycle_steering_controller/CHANGELOG.rst | 5 +++++ velocity_controllers/CHANGELOG.rst | 3 +++ 25 files changed, 119 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 795d981a4f..f85e0fe18b 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update paths of GPL includes (`#1487 `_) +* Contributors: Christoph Fröhlich + 4.19.0 (2025-01-13) ------------------- * Fix typos in steering_controllers_lib (`#1464 `_) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 83a01ca3f3..4880361db7 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove empty callbacks (`#1488 `_) +* Update paths of GPL includes (`#1487 `_) +* Contributors: Christoph Fröhlich, Julia Jia + 4.19.0 (2025-01-13) ------------------- * Remove visibility macros (`#1451 `_) diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 09655d3713..bdfded0249 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update paths of GPL includes (`#1487 `_) +* Contributors: Christoph Fröhlich + 4.19.0 (2025-01-13) ------------------- * Fix typos in steering_controllers_lib (`#1464 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 45f2499018..c033d50202 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Make diff_drive_controller a ChainableControllerInterface (`#1485 `_) +* Update paths of GPL includes (`#1487 `_) +* Fix SpeedLimiter Constructor regression (`#1478 `_) +* Contributors: Arthur Lovekin, Christoph Fröhlich, Sai Kishor Kothakota + 4.19.0 (2025-01-13) ------------------- * Check dt in updateFromVelocity (`#1481 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 44cd786d80..8416d18db6 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.19.0 (2025-01-13) ------------------- * Remove visibility macros (`#1451 `_) diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 114f7e0065..63b799ebd1 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update paths of GPL includes (`#1487 `_) +* Contributors: Christoph Fröhlich + 4.19.0 (2025-01-13) ------------------- * Remove visibility macros (`#1451 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 967360c948..77a38af5b1 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update paths of GPL includes (`#1487 `_) +* Contributors: Christoph Fröhlich + 4.19.0 (2025-01-13) ------------------- * Remove visibility macros (`#1451 `_) diff --git a/gpio_controllers/CHANGELOG.rst b/gpio_controllers/CHANGELOG.rst index a7147c670f..2a61e1f659 100644 --- a/gpio_controllers/CHANGELOG.rst +++ b/gpio_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gpio_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update paths of GPL includes (`#1487 `_) +* Contributors: Christoph Fröhlich + 4.19.0 (2025-01-13) ------------------- * Remove visibility macros (`#1451 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 3e51082926..c39ce3a8e7 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update paths of GPL includes (`#1487 `_) +* Contributors: Christoph Fröhlich + 4.19.0 (2025-01-13) ------------------- * Remove custom logic to skip configuration of gripper_controllers on Windows or macOS (`#1471 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index e1380f5bd0..4c9dba4880 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update paths of GPL includes (`#1487 `_) +* Contributors: Christoph Fröhlich + 4.19.0 (2025-01-13) ------------------- * Remove visibility macros (`#1451 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index aa9ad6f312..a788d80982 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update paths of GPL includes (`#1487 `_) +* Contributors: Christoph Fröhlich + 4.19.0 (2025-01-13) ------------------- * Use urdf/model.hpp for rolling (`#1476 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 97f289703f..d54f2f86af 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove empty callbacks (`#1488 `_) +* Update paths of GPL includes (`#1487 `_) +* Contributors: Christoph Fröhlich, Julia Jia + 4.19.0 (2025-01-13) ------------------- * Remove empty on_shutdown() callbacks (`#1477 `_) diff --git a/mecanum_drive_controller/CHANGELOG.rst b/mecanum_drive_controller/CHANGELOG.rst index e2af0afde1..debda9e60c 100644 --- a/mecanum_drive_controller/CHANGELOG.rst +++ b/mecanum_drive_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package mecanum_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update paths of GPL includes (`#1487 `_) +* Contributors: Christoph Fröhlich + 4.19.0 (2025-01-13) ------------------- * Remove visibility macros (`#1451 `_) diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst index 7b71c6099a..7a024a2d3c 100644 --- a/parallel_gripper_controller/CHANGELOG.rst +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package parallel_gripper_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update paths of GPL includes (`#1487 `_) +* Contributors: Christoph Fröhlich + 4.19.0 (2025-01-13) ------------------- * Remove visibility macros (`#1451 `_) diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index c9ef126ae8..844e84e045 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Improve antiwindup description (`#1502 `_) +* Remove empty callbacks (`#1488 `_) +* Update paths of GPL includes (`#1487 `_) +* Contributors: Christoph Fröhlich, Julia Jia, Victor Coutinho Vieira Santos + 4.19.0 (2025-01-13) ------------------- * Remove visibility macros (`#1451 `_) diff --git a/pose_broadcaster/CHANGELOG.rst b/pose_broadcaster/CHANGELOG.rst index c9fc763c29..53b50eae54 100644 --- a/pose_broadcaster/CHANGELOG.rst +++ b/pose_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package pose_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update paths of GPL includes (`#1487 `_) +* Contributors: Christoph Fröhlich + 4.19.0 (2025-01-13) ------------------- * [pose_broadcaster] Check for valid pose before attempting to publish a tf for it (`#1479 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 2fa99f6e03..466cd1312f 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.19.0 (2025-01-13) ------------------- * Remove visibility macros (`#1451 `_) diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index f807b1063b..9f0d2cd41c 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update paths of GPL includes (`#1487 `_) +* Contributors: Christoph Fröhlich + 4.19.0 (2025-01-13) ------------------- * Remove visibility macros (`#1451 `_) diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index fac6193901..353a44e761 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.19.0 (2025-01-13) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 1fc89e68f5..aa21046f1c 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.19.0 (2025-01-13) ------------------- diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index b7a22789bb..107363c2cd 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.19.0 (2025-01-13) ------------------- diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 86b044856d..a5a5881f88 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update paths of GPL includes (`#1487 `_) +* Contributors: Christoph Fröhlich + 4.19.0 (2025-01-13) ------------------- * Update generate_parameter_library dependency in steering_controllers_library (`#1465 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 6bf971a207..371213154e 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update paths of GPL includes (`#1487 `_) +* Contributors: Christoph Fröhlich + 4.19.0 (2025-01-13) ------------------- * Remove empty on_shutdown() callbacks (`#1477 `_) diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index e24893f4f7..068ead9739 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update paths of GPL includes (`#1487 `_) +* Contributors: Christoph Fröhlich + 4.19.0 (2025-01-13) ------------------- * Fix typos in steering_controllers_lib (`#1464 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index ac007bf316..c3b606c5a4 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.19.0 (2025-01-13) ------------------- * Remove visibility macros (`#1451 `_) From 50ff0267091d0eca05eea10fa07cbb21e23fea85 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 29 Jan 2025 19:10:43 +0000 Subject: [PATCH 68/84] 4.20.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gpio_controllers/CHANGELOG.rst | 4 ++-- gpio_controllers/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- mecanum_drive_controller/CHANGELOG.rst | 4 ++-- mecanum_drive_controller/package.xml | 2 +- parallel_gripper_controller/CHANGELOG.rst | 4 ++-- parallel_gripper_controller/package.xml | 2 +- pid_controller/CHANGELOG.rst | 4 ++-- pid_controller/package.xml | 2 +- pose_broadcaster/CHANGELOG.rst | 4 ++-- pose_broadcaster/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 52 files changed, 77 insertions(+), 77 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index f85e0fe18b..a67ac03cae 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2025-01-29) +------------------- * Update paths of GPL includes (`#1487 `_) * Contributors: Christoph Fröhlich diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index e31013d7ff..b7dcd5cc34 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.19.0 + 4.20.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Bence Magyar diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 4880361db7..a903b5c834 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2025-01-29) +------------------- * Remove empty callbacks (`#1488 `_) * Update paths of GPL includes (`#1487 `_) * Contributors: Christoph Fröhlich, Julia Jia diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index b30e245e9f..04307866a5 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.19.0 + 4.20.0 Implementation of admittance controllers for different input and output interface. Bence Magyar diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index bdfded0249..15481bc0e8 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2025-01-29) +------------------- * Update paths of GPL includes (`#1487 `_) * Contributors: Christoph Fröhlich diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 95e1a469c8..a280ab1bdc 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.19.0 + 4.20.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index c033d50202..d7ebc4ad8d 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2025-01-29) +------------------- * Make diff_drive_controller a ChainableControllerInterface (`#1485 `_) * Update paths of GPL includes (`#1487 `_) * Fix SpeedLimiter Constructor regression (`#1478 `_) diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 1f616a271a..deb134acd5 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.19.0 + 4.20.0 Controller for a differential-drive mobile base. Bence Magyar diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 8416d18db6..5389066996 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2025-01-29) +------------------- 4.19.0 (2025-01-13) ------------------- diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index fb851c4fd2..8040110370 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.19.0 + 4.20.0 Generic controller for forwarding commands. Bence Magyar diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 63b799ebd1..ca74bd1b32 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2025-01-29) +------------------- * Update paths of GPL includes (`#1487 `_) * Contributors: Christoph Fröhlich diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 5d0f15d947..e86ddde18d 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.19.0 + 4.20.0 Controller to publish state of force-torque sensors. Bence Magyar diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 77a38af5b1..385d0b390d 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2025-01-29) +------------------- * Update paths of GPL includes (`#1487 `_) * Contributors: Christoph Fröhlich diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 14b316a961..985095181f 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.19.0 + 4.20.0 Generic controller for forwarding commands. Bence Magyar diff --git a/gpio_controllers/CHANGELOG.rst b/gpio_controllers/CHANGELOG.rst index 2a61e1f659..cf30a98036 100644 --- a/gpio_controllers/CHANGELOG.rst +++ b/gpio_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gpio_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2025-01-29) +------------------- * Update paths of GPL includes (`#1487 `_) * Contributors: Christoph Fröhlich diff --git a/gpio_controllers/package.xml b/gpio_controllers/package.xml index b28ad57c6c..d8772e46ea 100644 --- a/gpio_controllers/package.xml +++ b/gpio_controllers/package.xml @@ -2,7 +2,7 @@ gpio_controllers - 4.19.0 + 4.20.0 Controllers to interact with gpios. Bence Magyar diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index c39ce3a8e7..0418be5753 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2025-01-29) +------------------- * Update paths of GPL includes (`#1487 `_) * Contributors: Christoph Fröhlich diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 17767846f6..d03239e317 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 4.19.0 + 4.20.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 4c9dba4880..b05b68d952 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2025-01-29) +------------------- * Update paths of GPL includes (`#1487 `_) * Contributors: Christoph Fröhlich diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 681a63a95a..e2f41bb325 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.19.0 + 4.20.0 Controller to publish readings of IMU sensors. Bence Magyar diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index a788d80982..7fbf86a0b9 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2025-01-29) +------------------- * Update paths of GPL includes (`#1487 `_) * Contributors: Christoph Fröhlich diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 320dc449ab..d8681fbefe 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.19.0 + 4.20.0 Broadcaster to publish joint state Bence Magyar diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index d54f2f86af..a4dd4ad988 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2025-01-29) +------------------- * Remove empty callbacks (`#1488 `_) * Update paths of GPL includes (`#1487 `_) * Contributors: Christoph Fröhlich, Julia Jia diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index e334b91400..695d382f83 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.19.0 + 4.20.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar diff --git a/mecanum_drive_controller/CHANGELOG.rst b/mecanum_drive_controller/CHANGELOG.rst index debda9e60c..380d0fcd03 100644 --- a/mecanum_drive_controller/CHANGELOG.rst +++ b/mecanum_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package mecanum_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2025-01-29) +------------------- * Update paths of GPL includes (`#1487 `_) * Contributors: Christoph Fröhlich diff --git a/mecanum_drive_controller/package.xml b/mecanum_drive_controller/package.xml index a5db87cb61..e1ba467909 100644 --- a/mecanum_drive_controller/package.xml +++ b/mecanum_drive_controller/package.xml @@ -2,7 +2,7 @@ mecanum_drive_controller - 4.19.0 + 4.20.0 Implementation of mecanum drive controller for 4 wheel drive. diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst index 7a024a2d3c..24ce2a7117 100644 --- a/parallel_gripper_controller/CHANGELOG.rst +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package parallel_gripper_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2025-01-29) +------------------- * Update paths of GPL includes (`#1487 `_) * Contributors: Christoph Fröhlich diff --git a/parallel_gripper_controller/package.xml b/parallel_gripper_controller/package.xml index 591c69af04..d78b2390ff 100644 --- a/parallel_gripper_controller/package.xml +++ b/parallel_gripper_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> parallel_gripper_controller - 4.19.0 + 4.20.0 The parallel_gripper_controller package Bence Magyar diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 844e84e045..2c6bf36cda 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2025-01-29) +------------------- * Improve antiwindup description (`#1502 `_) * Remove empty callbacks (`#1488 `_) * Update paths of GPL includes (`#1487 `_) diff --git a/pid_controller/package.xml b/pid_controller/package.xml index c3e9602ad9..7da50fe1e4 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.19.0 + 4.20.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar diff --git a/pose_broadcaster/CHANGELOG.rst b/pose_broadcaster/CHANGELOG.rst index 53b50eae54..243dcd66ef 100644 --- a/pose_broadcaster/CHANGELOG.rst +++ b/pose_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pose_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2025-01-29) +------------------- * Update paths of GPL includes (`#1487 `_) * Contributors: Christoph Fröhlich diff --git a/pose_broadcaster/package.xml b/pose_broadcaster/package.xml index 15d9f9ac99..8b73b85824 100644 --- a/pose_broadcaster/package.xml +++ b/pose_broadcaster/package.xml @@ -2,7 +2,7 @@ pose_broadcaster - 4.19.0 + 4.20.0 Broadcaster to publish cartesian states. Bence Magyar diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 466cd1312f..a1b4484dce 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2025-01-29) +------------------- 4.19.0 (2025-01-13) ------------------- diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 66b379f7bc..8bd9a8e1ca 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.19.0 + 4.20.0 Generic position controller for forwarding position commands. Bence Magyar diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 9f0d2cd41c..ddd2f0c924 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2025-01-29) +------------------- * Update paths of GPL includes (`#1487 `_) * Contributors: Christoph Fröhlich diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 7412249586..afb1dcaf7f 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.19.0 + 4.20.0 Controller to publish readings of range sensors. Bence Magyar diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 353a44e761..3d9f741bef 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2025-01-29) +------------------- 4.19.0 (2025-01-13) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 3719733764..4495828bff 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.19.0 + 4.20.0 Metapackage for ros2_controllers related packages Bence Magyar diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index aa21046f1c..fba9cc538b 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2025-01-29) +------------------- 4.19.0 (2025-01-13) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 199cd41672..020e12ecd5 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.19.0 + 4.20.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Bence Magyar diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index 814045a9e2..b6d5a19773 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.19.0", + version="4.20.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 107363c2cd..5a2af1ceec 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2025-01-29) +------------------- 4.19.0 (2025-01-13) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 4a694b3d1b..a623d06ecd 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 4.19.0 + 4.20.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index 3531833c1e..8b6d03db2a 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -21,7 +21,7 @@ setup( name=package_name, - version="4.19.0", + version="4.20.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index a5a5881f88..05201d81ac 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2025-01-29) +------------------- * Update paths of GPL includes (`#1487 `_) * Contributors: Christoph Fröhlich diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 2fe814faba..afe541563c 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.19.0 + 4.20.0 Package for steering robot configurations including odometry and interfaces. Bence Magyar diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 371213154e..3ef411440e 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2025-01-29) +------------------- * Update paths of GPL includes (`#1487 `_) * Contributors: Christoph Fröhlich diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index ce0394e8d9..dc46b63d39 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.19.0 + 4.20.0 Controller for a tricycle drive mobile base Bence Magyar diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 068ead9739..41740ea95a 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2025-01-29) +------------------- * Update paths of GPL includes (`#1487 `_) * Contributors: Christoph Fröhlich diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index d0e5f19b87..bf522f5604 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.19.0 + 4.20.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Bence Magyar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index c3b606c5a4..a815c0eade 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.20.0 (2025-01-29) +------------------- 4.19.0 (2025-01-13) ------------------- diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index e4b059a950..fb43afcc2d 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.19.0 + 4.20.0 Generic controller for forwarding commands. Bence Magyar From 5c16eb6b1865e928922fd56ae104d3260ce65d59 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 29 Jan 2025 21:47:25 +0100 Subject: [PATCH 69/84] Use ABI workflow from ros2_control_ci (#1510) --- .github/workflows/humble-abi-compatibility.yml | 12 +++--------- .github/workflows/jazzy-abi-compatibility.yml | 12 +++--------- .github/workflows/rolling-abi-compatibility.yml | 12 +++--------- 3 files changed, 9 insertions(+), 27 deletions(-) diff --git a/.github/workflows/humble-abi-compatibility.yml b/.github/workflows/humble-abi-compatibility.yml index 3e5892711b..4023b42a7b 100644 --- a/.github/workflows/humble-abi-compatibility.yml +++ b/.github/workflows/humble-abi-compatibility.yml @@ -23,12 +23,6 @@ concurrency: jobs: abi_check: - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v4 - - uses: ros-industrial/industrial_ci@master - env: - ROS_DISTRO: humble - ROS_REPO: testing - ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} - NOT_TEST_BUILD: true + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-abi-check.yml@master + with: + ros_distro: humble diff --git a/.github/workflows/jazzy-abi-compatibility.yml b/.github/workflows/jazzy-abi-compatibility.yml index 033321bc75..82fadb55a1 100644 --- a/.github/workflows/jazzy-abi-compatibility.yml +++ b/.github/workflows/jazzy-abi-compatibility.yml @@ -22,12 +22,6 @@ concurrency: jobs: abi_check: - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v4 - - uses: ros-industrial/industrial_ci@master - env: - ROS_DISTRO: jazzy - ROS_REPO: testing - ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} - NOT_TEST_BUILD: true + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-abi-check.yml@master + with: + ros_distro: jazzy diff --git a/.github/workflows/rolling-abi-compatibility.yml b/.github/workflows/rolling-abi-compatibility.yml index 353597003a..e3cd45c970 100644 --- a/.github/workflows/rolling-abi-compatibility.yml +++ b/.github/workflows/rolling-abi-compatibility.yml @@ -22,12 +22,6 @@ concurrency: jobs: abi_check: - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v4 - - uses: ros-industrial/industrial_ci@master - env: - ROS_DISTRO: rolling - ROS_REPO: testing - ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} - NOT_TEST_BUILD: true + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-abi-check.yml@master + with: + ros_distro: rolling From 694d6f184b91d7d4c6f52052e298aa106a219253 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sat, 1 Feb 2025 10:55:52 +0100 Subject: [PATCH 70/84] Bump version of pre-commit hooks (#1514) --- .pre-commit-config.yaml | 8 ++++---- .../gripper_controllers/gripper_action_controller.hpp | 2 +- .../parallel_gripper_action_controller.hpp | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index feefcc317d..f469640d98 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -50,7 +50,7 @@ repos: args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] - repo: https://github.com/psf/black - rev: 24.10.0 + rev: 25.1.0 hooks: - id: black args: ["--line-length=99"] @@ -63,7 +63,7 @@ repos: # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v19.1.5 + rev: v19.1.7 hooks: - id: clang-format args: ['-fallback-style=none', '-i'] @@ -126,14 +126,14 @@ repos: # Spellcheck in comments and docs # skipping of *.svg files is not working... - repo: https://github.com/codespell-project/codespell - rev: v2.3.0 + rev: v2.4.1 hooks: - id: codespell args: ['--write-changes', '--uri-ignore-words-list=ist', '-L manuel'] exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ - repo: https://github.com/python-jsonschema/check-jsonschema - rev: 0.30.0 + rev: 0.31.1 hooks: - id: check-github-workflows args: ["--verbose"] diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp index a23bd5d6e8..cf00b6c158 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp @@ -97,7 +97,7 @@ class GripperActionController : public controller_interface::ControllerInterface const rclcpp_lifecycle::State & previous_state) override; realtime_tools::RealtimeBuffer command_; - // pre-allocated memory that is re-used to set the realtime buffer + // pre-allocated memory that is reused to set the realtime buffer Commands command_struct_, command_struct_rt_; protected: diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp index 903ba825e3..0bd48b65f5 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp @@ -94,7 +94,7 @@ class GripperActionController : public controller_interface::ControllerInterface const rclcpp_lifecycle::State & previous_state) override; realtime_tools::RealtimeBuffer command_; - // pre-allocated memory that is re-used to set the realtime buffer + // pre-allocated memory that is reused to set the realtime buffer Commands command_struct_, command_struct_rt_; protected: From 28845e62cec9edf3c88787a4b1f8decf4bd634fa Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 4 Feb 2025 07:53:53 +0100 Subject: [PATCH 71/84] [JTC]: Abort goal on deactivate (#1517) --- .../src/joint_trajectory_controller.cpp | 2 +- .../test/test_trajectory_actions.cpp | 85 +++++++++++++++++++ 2 files changed, 86 insertions(+), 1 deletion(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index a4ad5210be..bbe9afde75 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1000,7 +1000,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( auto action_res = std::make_shared(); action_res->set__error_code(FollowJTrajAction::Result::INVALID_GOAL); action_res->set__error_string("Current goal cancelled during deactivate transition."); - active_goal->setCanceled(action_res); + active_goal->setAborted(action_res); rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); } diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index d4979deef8..6f846703f9 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -969,6 +969,91 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_tr EXPECT_TRUE(gh_future.get()); } +TEST_P(TestTrajectoryActionsTestParameterized, deactivate_controller_aborts_action) +{ + // deactivate velocity tolerance + std::vector params = { + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0)}; + SetUpExecutor(params, false, 1.0, 0.0); + + // We use our own hardware thread here, as we want to make sure the controller is deactivated + auto controller_thread = std::thread( + [&]() + { + // controller hardware cycle update loop + auto clock = rclcpp::Clock(RCL_STEADY_TIME); + auto now_time = clock.now(); + auto last_time = now_time; + rclcpp::Duration wait = rclcpp::Duration::from_seconds(0.5); + auto end_time = last_time + wait; + while (now_time < end_time) + { + now_time = now_time + rclcpp::Duration::from_seconds(0.01); + traj_controller_->update(now_time, now_time - last_time); + last_time = now_time; + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + RCLCPP_INFO(node_->get_logger(), "Controller hardware thread finished"); + traj_controller_->get_node()->deactivate(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + }); + + std::shared_future gh_future; + // send goal + std::vector point_positions{1.0, 2.0, 3.0}; + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(2.5); + point.positions = point_positions; + + points.push_back(point); + + gh_future = sendActionGoal(points, 1.0, goal_options_); + } + + controller_thread.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::ABORTED, common_resultcode_); + + auto state_ref = traj_controller_->get_state_reference(); + auto state = traj_controller_->get_state_feedback(); + + // run an update + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + + // There will be no active trajectory upon deactivation, so we can't use the expectCommandPoint + // method. + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(state_ref.positions.at(0), joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(state_ref.positions.at(1), joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(state_ref.positions.at(2), joint_pos_[2], COMMON_THRESHOLD); + } + + if (traj_controller_->has_velocity_command_interface()) + { + EXPECT_EQ(0.0, joint_vel_[0]); + EXPECT_EQ(0.0, joint_vel_[1]); + EXPECT_EQ(0.0, joint_vel_[2]); + } + + if (traj_controller_->has_acceleration_command_interface()) + { + EXPECT_EQ(0.0, joint_acc_[0]); + EXPECT_EQ(0.0, joint_acc_[1]); + EXPECT_EQ(0.0, joint_acc_[2]); + } + + if (traj_controller_->has_effort_command_interface()) + { + EXPECT_EQ(0.0, joint_eff_[0]); + EXPECT_EQ(0.0, joint_eff_[1]); + EXPECT_EQ(0.0, joint_eff_[2]); + } +} + // position controllers INSTANTIATE_TEST_SUITE_P( PositionTrajectoryControllersActions, TestTrajectoryActionsTestParameterized, From ce16e7f1905d21a3d4d7df1e525fb9ea855319cd Mon Sep 17 00:00:00 2001 From: Arthur Lovekin <62527557+arthurlovekin@users.noreply.github.com> Date: Mon, 3 Feb 2025 22:56:30 -0800 Subject: [PATCH 72/84] docs for chainable diff_drive_controller (#1518) --- diff_drive_controller/doc/userdoc.rst | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/diff_drive_controller/doc/userdoc.rst b/diff_drive_controller/doc/userdoc.rst index 29df4e55ba..2fce07a31b 100644 --- a/diff_drive_controller/doc/userdoc.rst +++ b/diff_drive_controller/doc/userdoc.rst @@ -20,6 +20,7 @@ Other features + Odometry publishing + Task-space velocity, acceleration and jerk limits + Automatic stop after command time-out + + Chainable Controller Description of controller's interfaces @@ -28,7 +29,13 @@ Description of controller's interfaces References ,,,,,,,,,,,,,,,,,, -(the controller is not yet implemented as chainable controller) +When controller is in chained mode, it exposes the following references which can be commanded by the preceding controller: + +- ``/linear/velocity`` double, in m/s +- ``/angular/velocity`` double, in rad/s + +Together, these represent the body twist (which in unchained-mode would be obtained from ~/cmd_vel). +The ```` is commonly set to ``diff_drive_controller``. Feedback ,,,,,,,,,,,,,, From 5486fcd2a651e0f6ca9a1827d5d2871917b99c2c Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 4 Feb 2025 14:43:08 +0100 Subject: [PATCH 73/84] [Doc] Fix broken link. (#1524) --- doc/writing_new_controller.rst | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/doc/writing_new_controller.rst b/doc/writing_new_controller.rst index da269d982a..0874448461 100644 --- a/doc/writing_new_controller.rst +++ b/doc/writing_new_controller.rst @@ -141,4 +141,7 @@ That's it! Enjoy writing great controllers! Useful External References --------------------------- -- `Templates and scripts for generating controllers shell `_ +- `Templates and scripts for generating controllers shell `_ + + + .. NOTE:: The script is currently only recommended to use with Humble, not compatible with the API from Jazzy and onwards. From ae6efebd2a87d90e522738399ccb3fd5556b47c7 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec <79221188+tpoignonec@users.noreply.github.com> Date: Mon, 10 Feb 2025 22:31:08 +0100 Subject: [PATCH 74/84] Fix reference in chained diff drive controller (#1529) --- diff_drive_controller/src/diff_drive_controller.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 7356d80219..3156d0d80f 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -335,6 +335,10 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( cmd_vel_timeout_ = rclcpp::Duration::from_seconds(params_.cmd_vel_timeout); publish_limited_velocity_ = params_.publish_limited_velocity; + // Allocate reference interfaces if needed + const int nr_ref_itfs = 2; + reference_interfaces_.resize(nr_ref_itfs, std::numeric_limits::quiet_NaN()); + // TODO(christophfroehlich) remove deprecated parameters // START DEPRECATED if (!params_.linear.x.has_velocity_limits) @@ -618,7 +622,9 @@ bool DiffDriveController::reset() void DiffDriveController::reset_buffers() { - reference_interfaces_ = std::vector(2, std::numeric_limits::quiet_NaN()); + std::fill( + reference_interfaces_.begin(), reference_interfaces_.end(), + std::numeric_limits::quiet_NaN()); // Empty out the old queue. Fill with zeros (not NaN) to catch early accelerations. std::queue> empty; std::swap(previous_two_commands_, empty); @@ -714,10 +720,8 @@ bool DiffDriveController::on_set_chained_mode(bool chained_mode) std::vector DiffDriveController::on_export_reference_interfaces() { - const int nr_ref_itfs = 2; - reference_interfaces_.resize(nr_ref_itfs, std::numeric_limits::quiet_NaN()); std::vector reference_interfaces; - reference_interfaces.reserve(nr_ref_itfs); + reference_interfaces.reserve(reference_interfaces_.size()); reference_interfaces.push_back(hardware_interface::CommandInterface( get_node()->get_name(), std::string("linear/") + hardware_interface::HW_IF_VELOCITY, From c2dd18394cbcd7871f22f91c577bce58a64a65d4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 11 Feb 2025 21:57:14 +0100 Subject: [PATCH 75/84] Fix jazzy debian build's job name (#1531) --- .github/workflows/jazzy-debian-build.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/jazzy-debian-build.yml b/.github/workflows/jazzy-debian-build.yml index b6b9e31bc2..ef1b42da8d 100644 --- a/.github/workflows/jazzy-debian-build.yml +++ b/.github/workflows/jazzy-debian-build.yml @@ -1,4 +1,4 @@ -name: Debian Rolling Source Build +name: Debian Jazzy Source Build on: workflow_dispatch: pull_request: From e5562fb371b47b5be81d6f4e2778aac397d8d840 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 12 Feb 2025 19:48:50 +0100 Subject: [PATCH 76/84] Remove dependency of admittance controller on JTC (#1532) Co-authored-by: Michael Ripperger --- admittance_controller/CMakeLists.txt | 1 - admittance_controller/package.xml | 1 - 2 files changed, 2 deletions(-) diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index a068239c17..bc9386274a 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -19,7 +19,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS generate_parameter_library geometry_msgs hardware_interface - joint_trajectory_controller kinematics_interface pluginlib rclcpp diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 04307866a5..df35f80255 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -31,7 +31,6 @@ generate_parameter_library geometry_msgs hardware_interface - joint_trajectory_controller pluginlib rclcpp rclcpp_lifecycle From c83cfb38b228c47fab4bf937b4cb5517c6fe4c81 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Wed, 12 Feb 2025 20:11:36 +0100 Subject: [PATCH 77/84] Fix use of M_PI in steering_controllers_library and tricycle_controller (#1536) --- steering_controllers_library/src/steering_odometry.cpp | 2 ++ steering_controllers_library/test/test_steering_odometry.cpp | 2 ++ tricycle_controller/src/tricycle_controller.cpp | 2 ++ 3 files changed, 6 insertions(+) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index b2bf100255..b8cf847033 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -17,6 +17,8 @@ * Author: Dr. Ing. Denis Stogl */ +#define _USE_MATH_DEFINES + #include "steering_controllers_library/steering_odometry.hpp" #include diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp index 3d6d97f15f..a211ac8bf4 100644 --- a/steering_controllers_library/test/test_steering_odometry.cpp +++ b/steering_controllers_library/test/test_steering_odometry.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#define _USE_MATH_DEFINES + #include #include "steering_controllers_library/steering_odometry.hpp" diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 9630d88b44..716583021f 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -16,6 +16,8 @@ * Author: Tony Najjar */ +#define _USE_MATH_DEFINES + #include #include #include From 6d681ef4dbe6027f1315c909b10e2ef8231df87d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 13 Feb 2025 12:16:10 +0100 Subject: [PATCH 78/84] Cleanup wrong lifecycle transitions in tests and unnecessary checks (#1534) --- .../diff_drive_controller.hpp | 2 -- .../src/diff_drive_controller.cpp | 26 +------------- .../test/test_diff_drive_controller.cpp | 11 ++++-- .../src/forward_command_controller.cpp | 5 --- ...i_interface_forward_command_controller.cpp | 5 --- .../src/gpio_command_controller.cpp | 5 --- .../gripper_action_controller_impl.hpp | 5 --- .../src/joint_state_broadcaster.cpp | 5 --- .../test/test_joint_state_broadcaster.cpp | 17 --------- .../src/joint_trajectory_controller.cpp | 10 ------ .../test/test_trajectory_actions.cpp | 3 -- .../test/test_trajectory_controller.cpp | 36 +------------------ ...arallel_gripper_action_controller_impl.hpp | 5 --- .../test/test_parallel_gripper_controller.cpp | 10 ------ .../tricycle_controller.hpp | 2 -- .../src/tricycle_controller.cpp | 13 +------ .../test/test_tricycle_controller.cpp | 11 +++--- 17 files changed, 17 insertions(+), 154 deletions(-) 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 11ecd9337b..665203c1c9 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 @@ -141,8 +141,6 @@ class DiffDriveController : public controller_interface::ChainableControllerInte rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0); rclcpp::Time previous_publish_timestamp_{0, 0, RCL_CLOCK_UNINITIALIZED}; - bool is_halted = false; - bool reset(); void halt(); diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 3156d0d80f..8eb5ba837e 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -101,15 +101,6 @@ controller_interface::return_type DiffDriveController::update_reference_from_sub const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { auto logger = get_node()->get_logger(); - if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE) - { - if (!is_halted) - { - halt(); - is_halted = true; - } - return controller_interface::return_type::OK; - } const std::shared_ptr command_msg_ptr = *(received_velocity_msg_ptr_.readFromRT()); @@ -149,15 +140,6 @@ controller_interface::return_type DiffDriveController::update_and_write_commands const rclcpp::Time & time, const rclcpp::Duration & period) { auto logger = get_node()->get_logger(); - if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE) - { - if (!is_halted) - { - halt(); - is_halted = true; - } - return controller_interface::return_type::OK; - } // command may be limited further by SpeedLimit, // without affecting the stored twist command @@ -562,7 +544,6 @@ controller_interface::CallbackReturn DiffDriveController::on_activate( return controller_interface::CallbackReturn::ERROR; } - is_halted = false; subscriber_is_active_ = true; RCLCPP_DEBUG(get_node()->get_logger(), "Subscriber and publisher are now active."); @@ -573,11 +554,7 @@ controller_interface::CallbackReturn DiffDriveController::on_deactivate( const rclcpp_lifecycle::State &) { subscriber_is_active_ = false; - if (!is_halted) - { - halt(); - is_halted = true; - } + halt(); reset_buffers(); registered_left_wheel_handles_.clear(); registered_right_wheel_handles_.clear(); @@ -616,7 +593,6 @@ bool DiffDriveController::reset() subscriber_is_active_ = false; velocity_command_subscriber_.reset(); - is_halted = false; return true; } diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 25cf2405f2..25c3add506 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -719,11 +719,16 @@ TEST_F(TestDiffDriveController, cleanup) controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + // should be moving + EXPECT_LT(0.0, left_wheel_vel_cmd_.get_value()); + EXPECT_LT(0.0, right_wheel_vel_cmd_.get_value()); + state = controller_->get_node()->deactivate(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - ASSERT_EQ( - controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + + // should be stopped + EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()) << "Wheels should be halted on deactivate()"; + EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()) << "Wheels should be halted on deactivate()"; state = controller_->get_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index 78fe8c9191..bc0a2589f5 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -31,11 +31,6 @@ void ForwardCommandController::declare_parameters() controller_interface::CallbackReturn ForwardCommandController::read_parameters() { - if (!param_listener_) - { - RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); - return controller_interface::CallbackReturn::ERROR; - } params_ = param_listener_->get_params(); if (params_.joints.empty()) diff --git a/forward_command_controller/src/multi_interface_forward_command_controller.cpp b/forward_command_controller/src/multi_interface_forward_command_controller.cpp index 06fe395cb4..692cc2ce0a 100644 --- a/forward_command_controller/src/multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/src/multi_interface_forward_command_controller.cpp @@ -32,11 +32,6 @@ void MultiInterfaceForwardCommandController::declare_parameters() controller_interface::CallbackReturn MultiInterfaceForwardCommandController::read_parameters() { - if (!param_listener_) - { - RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); - return controller_interface::CallbackReturn::ERROR; - } params_ = param_listener_->get_params(); if (params_.joint.empty()) diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index a509eae774..fdecf71c30 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -158,11 +158,6 @@ controller_interface::return_type GpioCommandController::update( bool GpioCommandController::update_dynamic_map_parameters() { auto logger = get_node()->get_logger(); - if (!param_listener_) - { - RCLCPP_ERROR(logger, "Error encountered during init"); - return false; - } // update the dynamic map parameters param_listener_->refresh_dynamic_parameters(); // get parameters from the listener in case they were updated diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index 2c115091af..034f7e43dd 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -206,11 +206,6 @@ controller_interface::CallbackReturn GripperActionController: const rclcpp_lifecycle::State &) { const auto logger = get_node()->get_logger(); - if (!param_listener_) - { - RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); - return controller_interface::CallbackReturn::ERROR; - } params_ = param_listener_->get_params(); // Action status checking update rate diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 5b72b9f35a..5a9d1caeef 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -90,11 +90,6 @@ controller_interface::InterfaceConfiguration JointStateBroadcaster::state_interf controller_interface::CallbackReturn JointStateBroadcaster::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - if (!param_listener_) - { - RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); - return controller_interface::CallbackReturn::ERROR; - } params_ = param_listener_->get_params(); if (use_all_available_interfaces()) diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 3ec7ff4ac2..370965e99e 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -149,23 +149,6 @@ void JointStateBroadcasterTest::assign_state_interfaces( state_broadcaster_->assign_interfaces({}, std::move(state_ifs)); } -TEST_F(JointStateBroadcasterTest, ConfigureErrorTest) -{ - // publishers not initialized yet - ASSERT_FALSE(state_broadcaster_->realtime_joint_state_publisher_); - ASSERT_FALSE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); - - // configure failed - ASSERT_THROW(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), std::exception); - - SetUpStateBroadcaster(); - // check state remains unchanged - - // publishers still not initialized - ASSERT_FALSE(state_broadcaster_->realtime_joint_state_publisher_); - ASSERT_FALSE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); -} - TEST_F(JointStateBroadcasterTest, ActivateEmptyTest) { // publishers not initialized yet diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index bbe9afde75..550ab9b476 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -144,10 +144,6 @@ JointTrajectoryController::state_interface_configuration() const controller_interface::return_type JointTrajectoryController::update( const rclcpp::Time & time, const rclcpp::Duration & period) { - if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) - { - return controller_interface::return_type::OK; - } auto logger = this->get_node()->get_logger(); // update dynamic parameters if (param_listener_->is_old(params_)) @@ -655,12 +651,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( { auto logger = get_node()->get_logger(); - if (!param_listener_) - { - RCLCPP_ERROR(logger, "Error encountered during init"); - return controller_interface::CallbackReturn::ERROR; - } - // update the dynamic map parameters param_listener_->refresh_dynamic_parameters(); diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 6f846703f9..894d887a22 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -1020,9 +1020,6 @@ TEST_P(TestTrajectoryActionsTestParameterized, deactivate_controller_aborts_acti auto state_ref = traj_controller_->get_state_reference(); auto state = traj_controller_->get_state_feedback(); - // run an update - updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); - // There will be no active trajectory upon deactivation, so we can't use the expectCommandPoint // method. if (traj_controller_->has_position_command_interface()) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 21d0277541..34f44e2d8c 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -46,39 +46,6 @@ TEST_P(TrajectoryControllerTestParameterized, invalid_robot_description) SetUpTrajectoryControllerLocal({}, "")); } -TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) -{ - rclcpp::executors::MultiThreadedExecutor executor; - SetUpTrajectoryController(executor); - traj_controller_->get_node()->set_parameter( - rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)); - - const auto state = traj_controller_->configure(); - ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); - - // send msg - constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); - builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; - // *INDENT-OFF* - std::vector> points{ - {{3.3, 4.4, 5.5}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; - std::vector> points_velocities{ - {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; - // *INDENT-ON* - publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); - traj_controller_->wait_for_trajectory(executor); - - traj_controller_->update( - rclcpp::Time(static_cast(0.5 * 1e9)), rclcpp::Duration::from_seconds(0.5)); - - // hw position == 0 because controller is not activated - EXPECT_EQ(0.0, joint_pos_[0]); - EXPECT_EQ(0.0, joint_pos_[1]); - EXPECT_EQ(0.0, joint_pos_[2]); - - executor.cancel(); -} - TEST_P(TrajectoryControllerTestParameterized, check_interface_names) { rclcpp::executors::MultiThreadedExecutor executor; @@ -148,7 +115,6 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup) auto state = traj_controller_->get_node()->deactivate(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); state = traj_controller_->get_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); @@ -223,8 +189,8 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param std::vector deactivated_positions{joint_pos_[0], joint_pos_[1], joint_pos_[2]}; state = traj_controller_->get_node()->deactivate(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + // it should be holding the current point - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1)); expectHoldingPointDeactivated(deactivated_positions); // reactivate diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp index d37cb87fc8..5b7e760ca2 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp @@ -226,11 +226,6 @@ controller_interface::CallbackReturn GripperActionController::on_configure( const rclcpp_lifecycle::State &) { const auto logger = get_node()->get_logger(); - if (!param_listener_) - { - RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); - return controller_interface::CallbackReturn::ERROR; - } params_ = param_listener_->get_params(); // Action status checking update rate diff --git a/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp b/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp index 417d87d40f..23c13b157a 100644 --- a/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp +++ b/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp @@ -67,11 +67,6 @@ TEST_F(GripperControllerTest, ParametersNotSet) this->SetUpController( "test_gripper_action_position_controller_no_parameters", controller_interface::return_type::ERROR); - - // configure failed, 'joints' parameter not set - ASSERT_EQ( - this->controller_->on_configure(rclcpp_lifecycle::State()), - controller_interface::CallbackReturn::ERROR); } TEST_F(GripperControllerTest, JointParameterIsEmpty) @@ -79,11 +74,6 @@ TEST_F(GripperControllerTest, JointParameterIsEmpty) this->SetUpController( "test_gripper_action_position_controller_empty_joint", controller_interface::return_type::ERROR); - - // configure failed, 'joints' is empty - ASSERT_EQ( - this->controller_->on_configure(rclcpp_lifecycle::State()), - controller_interface::CallbackReturn::ERROR); } TEST_F(GripperControllerTest, ConfigureParamsSuccess) diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index 3a1c996b78..60590012b9 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -135,8 +135,6 @@ class TricycleController : public controller_interface::ControllerInterface TractionLimiter limiter_traction_; SteeringLimiter limiter_steering_; - bool is_halted = false; - void reset_odometry( const std::shared_ptr request_header, const std::shared_ptr req, diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 716583021f..02f5449b4c 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -88,16 +88,6 @@ InterfaceConfiguration TricycleController::state_interface_configuration() const controller_interface::return_type TricycleController::update( const rclcpp::Time & time, const rclcpp::Duration & period) { - if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE) - { - if (!is_halted) - { - halt(); - is_halted = true; - } - return controller_interface::return_type::OK; - } - // 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; }); @@ -382,7 +372,6 @@ CallbackReturn TricycleController::on_activate(const rclcpp_lifecycle::State &) return CallbackReturn::ERROR; } - is_halted = false; subscriber_is_active_ = true; RCLCPP_DEBUG(get_node()->get_logger(), "Subscriber and publisher are now active."); @@ -392,6 +381,7 @@ CallbackReturn TricycleController::on_activate(const rclcpp_lifecycle::State &) CallbackReturn TricycleController::on_deactivate(const rclcpp_lifecycle::State &) { subscriber_is_active_ = false; + halt(); return CallbackReturn::SUCCESS; } @@ -438,7 +428,6 @@ bool TricycleController::reset() velocity_command_subscriber_.reset(); received_velocity_msg_ptr_.set(nullptr); - is_halted = false; return true; } diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index 5e868ebeea..d8d4f2cf63 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -282,9 +282,10 @@ TEST_F(TestTricycleController, cleanup) state = controller_->get_node()->deactivate(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - ASSERT_EQ( - controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + + // should be stopped + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); + EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()); state = controller_->get_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); @@ -333,11 +334,11 @@ TEST_F(TestTricycleController, correct_initialization_using_parameters) // deactivated // wait so controller process the second point when deactivated std::this_thread::sleep_for(std::chrono::milliseconds(500)); - state = controller_->get_node()->deactivate(); - ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + state = controller_->get_node()->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()) << "Wheels are halted on deactivate()"; EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()) << "Wheels are halted on deactivate()"; From 0d4b7e8f4867d0771aba36651e87facabac1dfde Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 13 Feb 2025 16:57:28 +0100 Subject: [PATCH 79/84] Fix the exported interface naming in the chainable controllers (#1528) --- .../test_ackermann_steering_controller.cpp | 15 ++++++----- .../test_ackermann_steering_controller.hpp | 3 +-- ...kermann_steering_controller_preceeding.cpp | 15 ++++++----- .../src/admittance_controller.cpp | 4 +-- .../test/test_admittance_controller.cpp | 21 ++++++++++++++++ .../test/test_bicycle_steering_controller.cpp | 15 ++++++----- .../test/test_bicycle_steering_controller.hpp | 3 +-- ...bicycle_steering_controller_preceeding.cpp | 15 ++++++----- .../src/diff_drive_controller.cpp | 4 +-- .../test/test_diff_drive_controller.cpp | 21 ++++++++-------- .../src/force_torque_sensor_broadcaster.cpp | 25 +++++++++++++------ .../test_force_torque_sensor_broadcaster.cpp | 19 ++++++++++++++ .../src/mecanum_drive_controller.cpp | 6 ++--- .../test/test_mecanum_drive_controller.cpp | 12 +++++---- .../test/test_mecanum_drive_controller.hpp | 3 +-- pid_controller/src/pid_controller.cpp | 6 +++-- pid_controller/test/test_pid_controller.cpp | 25 +++++++++++++++++-- .../test/test_pid_controller_preceding.cpp | 11 +++++--- .../src/steering_controllers_library.cpp | 4 +-- .../test_steering_controllers_library.cpp | 15 ++++++----- .../test_steering_controllers_library.hpp | 3 +-- .../test_tricycle_steering_controller.cpp | 19 ++++++++------ .../test_tricycle_steering_controller.hpp | 2 +- ...ricycle_steering_controller_preceeding.cpp | 18 +++++++------ 24 files changed, 189 insertions(+), 95 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index c434f73467..15eb39f1a4 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "test_ackermann_steering_controller.hpp" - #include #include #include +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "test_ackermann_steering_controller.hpp" + class AckermannSteeringControllerTest : public AckermannSteeringControllerFixture { @@ -87,11 +88,13 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) { - const std::string ref_itf_name = + const std::string ref_itf_prefix_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i]->get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ( + reference_interfaces[i]->get_name(), + ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_VELOCITY); } } diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 2e7d688471..4657ee2c95 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -295,8 +295,7 @@ class AckermannSteeringControllerFixture : public ::testing::Test std::array joint_state_values_ = {{0.5, 0.5, 0.0, 0.0}}; std::array joint_command_values_ = {{1.1, 3.3, 2.2, 4.4}}; - std::array joint_reference_interfaces_ = { - {"linear/velocity", "angular/velocity"}}; + std::array joint_reference_interfaces_ = {{"linear", "angular"}}; std::string steering_interface_name_ = "position"; // defined in setup std::string traction_interface_name_ = ""; diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp index 4f67beaa07..7a0e4d430e 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "test_ackermann_steering_controller.hpp" - #include #include #include +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "test_ackermann_steering_controller.hpp" + class AckermannSteeringControllerTest : public AckermannSteeringControllerFixture { @@ -89,11 +90,13 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) { - const std::string ref_itf_name = + const std::string ref_itf_prefix_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i]->get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ( + reference_interfaces[i]->get_name(), + ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_VELOCITY); } } diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 7bcd770a52..1ef76d1f61 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -147,9 +147,9 @@ AdmittanceController::on_export_reference_interfaces() { velocity_reference_.emplace_back(reference_interfaces_[index]); } - const auto full_name = joint + "/" + interface; + const auto exported_prefix = std::string(get_node()->get_name()) + "/" + joint; chainable_command_interfaces.emplace_back(hardware_interface::CommandInterface( - std::string(get_node()->get_name()), full_name, reference_interfaces_.data() + index)); + exported_prefix, interface, reference_interfaces_.data() + index)); index++; } diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index 5c4bbe9438..f6e28d599e 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -168,6 +168,27 @@ TEST_F(AdmittanceControllerTest, check_interfaces) ASSERT_EQ( controller_->state_interfaces_.size(), state_interface_types_.size() * joint_names_.size() + fts_state_values_.size()); + + const auto reference_interfaces = controller_->ordered_exported_reference_interfaces_; + ASSERT_EQ(reference_interfaces.size(), 2 * joint_names_.size()); + for (auto i = 0ul; i < joint_names_.size(); i++) + { + const std::string ref_itf_prefix_name = + std::string(controller_->get_node()->get_name()) + "/" + joint_names_[i]; + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name); + EXPECT_EQ( + reference_interfaces[i]->get_name(), + ref_itf_prefix_name + "/" + hardware_interface::HW_IF_POSITION); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_POSITION); + EXPECT_EQ( + reference_interfaces[i + joint_names_.size()]->get_prefix_name(), ref_itf_prefix_name); + EXPECT_EQ( + reference_interfaces[i + joint_names_.size()]->get_name(), + ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ( + reference_interfaces[i + joint_names_.size()]->get_interface_name(), + hardware_interface::HW_IF_VELOCITY); + } } TEST_F(AdmittanceControllerTest, activate_success) diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 7ec0983f93..9c41f5fd47 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "test_bicycle_steering_controller.hpp" - #include #include #include +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "test_bicycle_steering_controller.hpp" + class BicycleSteeringControllerTest : public BicycleSteeringControllerFixture { @@ -71,11 +72,13 @@ TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) { - const std::string ref_itf_name = + const std::string ref_itf_prefix_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i]->get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ( + reference_interfaces[i]->get_name(), + ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_VELOCITY); } } diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 25fec7fdf1..b6e5d616a7 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -263,8 +263,7 @@ class BicycleSteeringControllerFixture : public ::testing::Test std::array joint_state_values_ = {{3.3, 0.5}}; std::array joint_command_values_ = {{1.1, 2.2}}; - std::array joint_reference_interfaces_ = { - {"linear/velocity", "angular/velocity"}}; + std::array joint_reference_interfaces_ = {{"linear", "angular"}}; std::string steering_interface_name_ = "position"; // defined in setup diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp index 0a321009c2..500bb36c1c 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "test_bicycle_steering_controller.hpp" - #include #include #include +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "test_bicycle_steering_controller.hpp" + class BicycleSteeringControllerTest : public BicycleSteeringControllerFixture { @@ -75,11 +76,13 @@ TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) { - const std::string ref_itf_name = + const std::string ref_itf_prefix_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i]->get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ( + reference_interfaces[i]->get_name(), + ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_VELOCITY); } } diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 8eb5ba837e..537e32d3e1 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -700,11 +700,11 @@ DiffDriveController::on_export_reference_interfaces() reference_interfaces.reserve(reference_interfaces_.size()); reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), std::string("linear/") + hardware_interface::HW_IF_VELOCITY, + get_node()->get_name() + std::string("/linear"), hardware_interface::HW_IF_VELOCITY, &reference_interfaces_[0])); reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), std::string("angular/") + hardware_interface::HW_IF_VELOCITY, + get_node()->get_name() + std::string("/angular"), hardware_interface::HW_IF_VELOCITY, &reference_interfaces_[1])); return reference_interfaces; diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 25c3add506..80fc1bf3b8 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -976,23 +976,22 @@ TEST_F(TestDiffDriveController, reference_interfaces_are_properly_exported) ASSERT_EQ(reference_interfaces.size(), 2) << "Expected exactly 2 reference interfaces: linear and angular"; - const std::string expected_prefix_name = std::string(controller_->get_node()->get_name()); - const std::string expected_linear_interface_name = - std::string("linear/") + hardware_interface::HW_IF_VELOCITY; - const std::string expected_angular_interface_name = - std::string("angular/") + hardware_interface::HW_IF_VELOCITY; + const std::string expected_linear_prefix_name = + std::string(controller_->get_node()->get_name()) + std::string("/linear"); + const std::string expected_angular_prefix_name = + std::string(controller_->get_node()->get_name()) + std::string("/angular"); const std::string expected_linear_name = - expected_prefix_name + std::string("/") + expected_linear_interface_name; + expected_linear_prefix_name + std::string("/") + hardware_interface::HW_IF_VELOCITY; const std::string expected_angular_name = - expected_prefix_name + std::string("/") + expected_angular_interface_name; + expected_angular_prefix_name + std::string("/") + hardware_interface::HW_IF_VELOCITY; ASSERT_EQ(reference_interfaces[0]->get_name(), expected_linear_name); ASSERT_EQ(reference_interfaces[1]->get_name(), expected_angular_name); - EXPECT_EQ(reference_interfaces[0]->get_prefix_name(), expected_prefix_name); - EXPECT_EQ(reference_interfaces[0]->get_interface_name(), expected_linear_interface_name); - EXPECT_EQ(reference_interfaces[1]->get_prefix_name(), expected_prefix_name); - EXPECT_EQ(reference_interfaces[1]->get_interface_name(), expected_angular_interface_name); + EXPECT_EQ(reference_interfaces[0]->get_prefix_name(), expected_linear_prefix_name); + EXPECT_EQ(reference_interfaces[0]->get_interface_name(), hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(reference_interfaces[1]->get_prefix_name(), expected_angular_prefix_name); + EXPECT_EQ(reference_interfaces[1]->get_interface_name(), hardware_interface::HW_IF_VELOCITY); } // Make sure that the controller is properly reset when deactivated diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index ae105a511c..5b4ac3368c 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -176,6 +176,7 @@ ForceTorqueSensorBroadcaster::on_export_state_interfaces() std::vector torque_names( {params_.interface_names.torque.x, params_.interface_names.torque.y, params_.interface_names.torque.z}); + std::string export_prefix = get_node()->get_name(); if (!params_.sensor_name.empty()) { const auto semantic_comp_itf_names = force_torque_sensor_->get_state_interface_names(); @@ -183,37 +184,47 @@ ForceTorqueSensorBroadcaster::on_export_state_interfaces() semantic_comp_itf_names.begin(), semantic_comp_itf_names.begin() + 3, force_names.begin()); std::copy( semantic_comp_itf_names.begin() + 3, semantic_comp_itf_names.end(), torque_names.begin()); + + // Update the prefix and get the proper force and torque names + export_prefix = export_prefix + "/" + params_.sensor_name; + // strip "/" and get the second part of the information + // e.g. /ft_sensor/force.x -> force.x + std::for_each( + force_names.begin(), force_names.end(), + [](std::string & name) { name = name.substr(name.find_last_of("/") + 1); }); + std::for_each( + torque_names.begin(), torque_names.end(), + [](std::string & name) { name = name.substr(name.find_last_of("/") + 1); }); } - const std::string controller_name = get_node()->get_name(); if (!force_names[0].empty()) { exported_state_interfaces.emplace_back(hardware_interface::StateInterface( - controller_name, force_names[0], &realtime_publisher_->msg_.wrench.force.x)); + export_prefix, force_names[0], &realtime_publisher_->msg_.wrench.force.x)); } if (!force_names[1].empty()) { exported_state_interfaces.emplace_back(hardware_interface::StateInterface( - controller_name, force_names[1], &realtime_publisher_->msg_.wrench.force.y)); + export_prefix, force_names[1], &realtime_publisher_->msg_.wrench.force.y)); } if (!force_names[2].empty()) { exported_state_interfaces.emplace_back(hardware_interface::StateInterface( - controller_name, force_names[2], &realtime_publisher_->msg_.wrench.force.z)); + export_prefix, force_names[2], &realtime_publisher_->msg_.wrench.force.z)); } if (!torque_names[0].empty()) { exported_state_interfaces.emplace_back(hardware_interface::StateInterface( - controller_name, torque_names[0], &realtime_publisher_->msg_.wrench.torque.x)); + export_prefix, torque_names[0], &realtime_publisher_->msg_.wrench.torque.x)); } if (!torque_names[1].empty()) { exported_state_interfaces.emplace_back(hardware_interface::StateInterface( - controller_name, torque_names[1], &realtime_publisher_->msg_.wrench.torque.y)); + export_prefix, torque_names[1], &realtime_publisher_->msg_.wrench.torque.y)); } if (!torque_names[2].empty()) { exported_state_interfaces.emplace_back(hardware_interface::StateInterface( - controller_name, torque_names[2], &realtime_publisher_->msg_.wrench.torque.z)); + export_prefix, torque_names[2], &realtime_publisher_->msg_.wrench.torque.z)); } return exported_state_interfaces; } diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index 26f959e88f..46c396f7b1 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -324,8 +324,16 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Offsets exported_state_interfaces[4]->get_name(), controller_name + "/" + sensor_name_ + "/torque.y"); ASSERT_EQ( exported_state_interfaces[5]->get_name(), controller_name + "/" + sensor_name_ + "/torque.z"); + ASSERT_EQ(exported_state_interfaces[0]->get_interface_name(), "force.x"); + ASSERT_EQ(exported_state_interfaces[1]->get_interface_name(), "force.y"); + ASSERT_EQ(exported_state_interfaces[2]->get_interface_name(), "force.z"); + ASSERT_EQ(exported_state_interfaces[3]->get_interface_name(), "torque.x"); + ASSERT_EQ(exported_state_interfaces[4]->get_interface_name(), "torque.y"); + ASSERT_EQ(exported_state_interfaces[5]->get_interface_name(), "torque.z"); for (size_t i = 0; i < 6; ++i) { + ASSERT_EQ( + exported_state_interfaces[i]->get_prefix_name(), controller_name + "/" + sensor_name_); ASSERT_EQ( exported_state_interfaces[i]->get_value(), sensor_values_[i] + (i < 3 ? force_offsets[i] : torque_offsets[i - 3])); @@ -361,6 +369,10 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success) const std::string controller_name = fts_broadcaster_->get_node()->get_name(); ASSERT_EQ(exported_state_interfaces[0]->get_name(), controller_name + "/fts_sensor/force.x"); ASSERT_EQ(exported_state_interfaces[1]->get_name(), controller_name + "/fts_sensor/torque.z"); + ASSERT_EQ(exported_state_interfaces[0]->get_prefix_name(), controller_name); + ASSERT_EQ(exported_state_interfaces[1]->get_prefix_name(), controller_name); + ASSERT_EQ(exported_state_interfaces[0]->get_interface_name(), "fts_sensor/force.x"); + ASSERT_EQ(exported_state_interfaces[1]->get_interface_name(), "fts_sensor/torque.z"); ASSERT_EQ(exported_state_interfaces[0]->get_value(), sensor_values_[0]); ASSERT_EQ(exported_state_interfaces[1]->get_value(), sensor_values_[5]); } @@ -402,8 +414,15 @@ TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success) ASSERT_EQ(exported_state_interfaces[3]->get_name(), controller_name + "/fts_sensor/torque.x"); ASSERT_EQ(exported_state_interfaces[4]->get_name(), controller_name + "/fts_sensor/torque.y"); ASSERT_EQ(exported_state_interfaces[5]->get_name(), controller_name + "/fts_sensor/torque.z"); + ASSERT_EQ(exported_state_interfaces[0]->get_interface_name(), "fts_sensor/force.x"); + ASSERT_EQ(exported_state_interfaces[1]->get_interface_name(), "fts_sensor/force.y"); + ASSERT_EQ(exported_state_interfaces[2]->get_interface_name(), "fts_sensor/force.z"); + ASSERT_EQ(exported_state_interfaces[3]->get_interface_name(), "fts_sensor/torque.x"); + ASSERT_EQ(exported_state_interfaces[4]->get_interface_name(), "fts_sensor/torque.y"); + ASSERT_EQ(exported_state_interfaces[5]->get_interface_name(), "fts_sensor/torque.z"); for (size_t i = 0; i < 6; ++i) { + ASSERT_EQ(exported_state_interfaces[0]->get_prefix_name(), controller_name); ASSERT_EQ(exported_state_interfaces[i]->get_value(), sensor_values_[i]); } } diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 2fd5fdbdcb..93e607bd08 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -279,13 +279,13 @@ MecanumDriveController::on_export_reference_interfaces() reference_interfaces.reserve(reference_interfaces_.size()); - std::vector reference_interface_names = { - "linear/x/velocity", "linear/y/velocity", "angular/z/velocity"}; + std::vector reference_interface_names = {"/linear/x", "/linear/y", "/angular/z"}; for (size_t i = 0; i < reference_interfaces_.size(); ++i) { reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), reference_interface_names[i], &reference_interfaces_[i])); + get_node()->get_name() + reference_interface_names[i], hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[i])); } return reference_interfaces; diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index bbddcec06a..079a431e03 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -108,11 +108,13 @@ TEST_F(MecanumDriveControllerTest, when_controller_configured_expect_properly_ex for (size_t i = 0; i < reference_interface_names.size(); ++i) { - const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + - std::string("/") + reference_interface_names[i]; - EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i]->get_interface_name(), reference_interface_names[i]); + const std::string ref_itf_prefix_name = + std::string(controller_->get_node()->get_name()) + "/" + reference_interface_names[i]; + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name); + EXPECT_EQ( + reference_interfaces[i]->get_name(), + ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_VELOCITY); } } diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp index 5f7cf36dc7..766c3ec554 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -269,8 +269,7 @@ class MecanumDriveControllerFixture : public ::testing::Test } protected: - std::vector reference_interface_names = { - "linear/x/velocity", "linear/y/velocity", "angular/z/velocity"}; + std::vector reference_interface_names = {"linear/x", "linear/y", "angular/z"}; static constexpr char TEST_FRONT_LEFT_CMD_JOINT_NAME[] = "front_left_wheel_joint"; static constexpr char TEST_FRONT_RIGHT_CMD_JOINT_NAME[] = "front_right_wheel_joint"; diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 521227b49c..2816d5a5ec 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -392,7 +392,8 @@ std::vector PidController::on_export_refer for (const auto & dof_name : reference_and_state_dof_names_) { reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), dof_name + "/" + interface, &reference_interfaces_[index])); + std::string(get_node()->get_name()) + "/" + dof_name, interface, + &reference_interfaces_[index])); ++index; } } @@ -414,7 +415,8 @@ std::vector PidController::on_export_state_i for (const auto & dof_name : reference_and_state_dof_names_) { state_interfaces.push_back(hardware_interface::StateInterface( - get_node()->get_name(), dof_name + "/" + interface, &state_interfaces_values_[index])); + std::string(get_node()->get_name()) + "/" + dof_name, interface, + &state_interfaces_values_[index])); ++index; } } diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 60d484e463..a8a703e8a4 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -98,11 +98,32 @@ TEST_F(PidControllerTest, check_exported_interfaces) const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; EXPECT_EQ(ref_if_conf[ri_index]->get_name(), ref_itf_name); - EXPECT_EQ(ref_if_conf[ri_index]->get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(ref_if_conf[ri_index]->get_interface_name(), dof_name + "/" + interface); + EXPECT_EQ( + ref_if_conf[ri_index]->get_prefix_name(), + std::string(controller_->get_node()->get_name()) + "/" + dof_name); + EXPECT_EQ(ref_if_conf[ri_index]->get_interface_name(), interface); ++ri_index; } } + + // check exported state interfaces + auto state_ifs = controller_->export_state_interfaces(); + ASSERT_EQ(state_ifs.size(), dof_state_values_.size()); + size_t s_index = 0; + for (const auto & interface : state_interfaces_) + { + for (const auto & dof_name : dof_names_) + { + const std::string state_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; + EXPECT_EQ(state_ifs[s_index]->get_name(), state_itf_name); + EXPECT_EQ( + state_ifs[s_index]->get_prefix_name(), + std::string(controller_->get_node()->get_name()) + "/" + dof_name); + EXPECT_EQ(state_ifs[s_index]->get_interface_name(), interface); + ++s_index; + } + } } TEST_F(PidControllerTest, activate_success) diff --git a/pid_controller/test/test_pid_controller_preceding.cpp b/pid_controller/test/test_pid_controller_preceding.cpp index ee5fe46754..602303ef9e 100644 --- a/pid_controller/test/test_pid_controller_preceding.cpp +++ b/pid_controller/test/test_pid_controller_preceding.cpp @@ -85,8 +85,10 @@ TEST_F(PidControllerTest, check_exported_interfaces) const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; EXPECT_EQ(ref_if_conf[ri_index]->get_name(), ref_itf_name); - EXPECT_EQ(ref_if_conf[ri_index]->get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(ref_if_conf[ri_index]->get_interface_name(), dof_name + "/" + interface); + EXPECT_EQ( + ref_if_conf[ri_index]->get_prefix_name(), + std::string(controller_->get_node()->get_name()) + "/" + dof_name); + EXPECT_EQ(ref_if_conf[ri_index]->get_interface_name(), interface); ++ri_index; } } @@ -103,8 +105,9 @@ TEST_F(PidControllerTest, check_exported_interfaces) std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; EXPECT_EQ(exported_state_itfs[esi_index]->get_name(), state_itf_name); EXPECT_EQ( - exported_state_itfs[esi_index]->get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(exported_state_itfs[esi_index]->get_interface_name(), dof_name + "/" + interface); + exported_state_itfs[esi_index]->get_prefix_name(), + std::string(controller_->get_node()->get_name()) + "/" + dof_name); + EXPECT_EQ(exported_state_itfs[esi_index]->get_interface_name(), interface); ++esi_index; } } diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index a91cb87105..ed0346cbbc 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -317,11 +317,11 @@ SteeringControllersLibrary::on_export_reference_interfaces() reference_interfaces.reserve(nr_ref_itfs_); reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), std::string("linear/") + hardware_interface::HW_IF_VELOCITY, + get_node()->get_name() + std::string("/linear"), hardware_interface::HW_IF_VELOCITY, &reference_interfaces_[0])); reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), std::string("angular/") + hardware_interface::HW_IF_VELOCITY, + get_node()->get_name() + std::string("/angular"), hardware_interface::HW_IF_VELOCITY, &reference_interfaces_[1])); return reference_interfaces; diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index 3378efbef8..10a0f00376 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -12,13 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "test_steering_controllers_library.hpp" - #include #include #include #include +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "test_steering_controllers_library.hpp" + class SteeringControllersLibraryTest : public SteeringControllersLibraryFixture { @@ -68,11 +69,13 @@ TEST_F(SteeringControllersLibraryTest, check_exported_interfaces) ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) { - const std::string ref_itf_name = + const std::string ref_itf_prefix_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i]->get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name); + EXPECT_EQ( + reference_interfaces[i]->get_name(), + ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_VELOCITY); } } diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 20d136ab32..52bd69744f 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -317,8 +317,7 @@ class SteeringControllersLibraryFixture : public ::testing::Test std::array joint_state_values_ = {{0.5, 0.5, 0.0, 0.0}}; std::array joint_command_values_ = {{1.1, 3.3, 2.2, 4.4}}; - std::array joint_reference_interfaces_ = { - {"linear/velocity", "angular/velocity"}}; + std::array joint_reference_interfaces_ = {{"linear", "angular"}}; std::string steering_interface_name_ = "position"; // defined in setup std::string traction_interface_name_ = ""; diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index 8e29314f8e..93c92c865f 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "test_tricycle_steering_controller.hpp" - #include #include #include +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "test_tricycle_steering_controller.hpp" + class TricycleSteeringControllerTest : public TricycleSteeringControllerFixture { @@ -75,15 +76,17 @@ TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfs - auto ref_if_conf = controller_->export_reference_interfaces(); - ASSERT_EQ(ref_if_conf.size(), joint_reference_interfaces_.size()); + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) { - const std::string ref_itf_name = + const std::string ref_itf_prefix_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(ref_if_conf[i]->get_name(), ref_itf_name); - EXPECT_EQ(ref_if_conf[i]->get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(ref_if_conf[i]->get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name); + EXPECT_EQ( + reference_interfaces[i]->get_name(), + ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_VELOCITY); } } diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index cfecf96cc8..4a914120ef 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -280,7 +280,7 @@ class TricycleSteeringControllerFixture : public ::testing::Test std::array joint_state_values_{{0.5, 0.5, 0.0}}; std::array joint_command_values_{{1.1, 3.3, 2.2}}; - std::array joint_reference_interfaces_{{"linear/velocity", "angular/velocity"}}; + std::array joint_reference_interfaces_{{"linear", "angular"}}; std::string steering_interface_name_ = "position"; // defined in setup std::string traction_interface_name_ = ""; diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp index 566169e34e..f9b9afb67c 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "test_tricycle_steering_controller.hpp" - #include #include #include +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "test_tricycle_steering_controller.hpp" class TricycleSteeringControllerTest : public TricycleSteeringControllerFixture { @@ -78,15 +78,17 @@ TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfs - auto ref_if_conf = controller_->export_reference_interfaces(); - ASSERT_EQ(ref_if_conf.size(), joint_reference_interfaces_.size()); + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) { - const std::string ref_itf_name = + const std::string ref_itf_prefix_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(ref_if_conf[i]->get_name(), ref_itf_name); - EXPECT_EQ(ref_if_conf[i]->get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(ref_if_conf[i]->get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name); + EXPECT_EQ( + reference_interfaces[i]->get_name(), + ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_VELOCITY); } } From cf8b96ede01a88e78ef7e7a79eb1d9ff2b723456 Mon Sep 17 00:00:00 2001 From: Julia Jia Date: Thu, 13 Feb 2025 09:05:40 -0800 Subject: [PATCH 80/84] [pid_controller] Fix logic for feedforward_mode with single reference interface (#1520) --- pid_controller/src/pid_controller.cpp | 23 +-- .../test/pid_controller_params.yaml | 13 ++ pid_controller/test/test_pid_controller.cpp | 131 ++++++++++++++++++ pid_controller/test/test_pid_controller.hpp | 2 + 4 files changed, 161 insertions(+), 8 deletions(-) diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 2816d5a5ec..dd4d6df185 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -500,20 +500,27 @@ controller_interface::return_type PidController::update_and_write_commands( for (size_t i = 0; i < dof_; ++i) { - double tmp_command = std::numeric_limits::quiet_NaN(); + double tmp_command = 0.0; - // Using feedforward if (!std::isnan(reference_interfaces_[i]) && !std::isnan(measured_state_values_[i])) { // calculate feed-forward if (*(control_mode_.readFromRT()) == feedforward_mode_type::ON) { - tmp_command = reference_interfaces_[dof_ + i] * - params_.gains.dof_names_map[params_.dof_names[i]].feedforward_gain; - } - else - { - tmp_command = 0.0; + // two interfaces + if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_) + { + if (std::isfinite(reference_interfaces_[dof_ + i])) + { + tmp_command = reference_interfaces_[dof_ + i] * + params_.gains.dof_names_map[params_.dof_names[i]].feedforward_gain; + } + } + else // one interface + { + tmp_command = reference_interfaces_[i] * + params_.gains.dof_names_map[params_.dof_names[i]].feedforward_gain; + } } double error = reference_interfaces_[i] - measured_state_values_[i]; diff --git a/pid_controller/test/pid_controller_params.yaml b/pid_controller/test/pid_controller_params.yaml index 7555cfc156..cf0a3f4173 100644 --- a/pid_controller/test/pid_controller_params.yaml +++ b/pid_controller/test/pid_controller_params.yaml @@ -9,3 +9,16 @@ test_pid_controller: gains: joint1: {p: 1.0, i: 2.0, d: 10.0, i_clamp_max: 5.0, i_clamp_min: -5.0} + + +test_pid_controller_with_feedforward_gain: + ros__parameters: + dof_names: + - joint1 + + command_interface: position + + reference_and_state_interfaces: ["position"] + + gains: + joint1: {p: 0.5, i: 0.0, d: 0.0, i_clamp_max: 5.0, i_clamp_min: -5.0, feedforward_gain: 1.0} diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index a8a703e8a4..67ac680eae 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -542,6 +542,137 @@ TEST_F(PidControllerTest, receive_message_and_publish_updated_status) } } +/** + * @brief check chained pid controller with feedforward and gain as non-zero, single interface + */ +TEST_F(PidControllerTest, test_update_chained_feedforward_with_gain) +{ + // state interface value is 1.1 as defined in test fixture + // with p gain 0.5, the command value should be 0.5 * (5.0 - 1.1) = 1.95 + // with feedforward gain 1.0, the command value should be 1.95 + 1.0 * 5.0 = 6.95 + const double target_value = 5.0; + const double expected_command_value = 6.95; + + SetUpController("test_pid_controller_with_feedforward_gain"); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check on interfaces & pid gain parameters + for (const auto & dof_name : dof_names_) + { + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].p, 0.5); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].feedforward_gain, 1.0); + } + ASSERT_EQ(controller_->params_.command_interface, command_interface_); + EXPECT_THAT( + controller_->params_.reference_and_state_interfaces, + testing::ElementsAreArray(state_interfaces_)); + ASSERT_FALSE(controller_->params_.use_external_measured_states); + + // setup executor + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + controller_->set_chained_mode(true); + + // activate controller + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + // turn on feedforward + controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + + // send a message to update reference interface + std::shared_ptr msg = std::make_shared(); + msg->dof_names = controller_->params_.dof_names; + msg->values.resize(msg->dof_names.size(), 0.0); + for (size_t i = 0; i < msg->dof_names.size(); ++i) + { + msg->values[i] = target_value; + } + msg->values_dot.resize(msg->dof_names.size(), std::numeric_limits::quiet_NaN()); + controller_->input_ref_.writeFromNonRT(msg); + ASSERT_EQ( + controller_->update_reference_from_subscribers( + rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // run update + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // check on result from update + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value); +} + +/** + * @brief check chained pid controller with feedforward OFF and gain as non-zero, single interface + */ +TEST_F(PidControllerTest, test_update_chained_feedforward_off_with_gain) +{ + // state interface value is 1.1 as defined in test fixture + // given target value 5.0 + // with p gain 0.5, the command value should be 0.5 * (5.0 - 1.1) = 1.95 + // with feedforward off, the command value should be still 1.95 even though feedforward gain + // is 1.0 + const double target_value = 5.0; + const double expected_command_value = 1.95; + + SetUpController("test_pid_controller_with_feedforward_gain"); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check on interfaces & pid gain parameters + for (const auto & dof_name : dof_names_) + { + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].p, 0.5); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].feedforward_gain, 1.0); + } + ASSERT_EQ(controller_->params_.command_interface, command_interface_); + EXPECT_THAT( + controller_->params_.reference_and_state_interfaces, + testing::ElementsAreArray(state_interfaces_)); + ASSERT_FALSE(controller_->params_.use_external_measured_states); + + // setup executor + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + controller_->set_chained_mode(true); + + // activate controller + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + // feedforward by default is OFF + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + + // send a message to update reference interface + std::shared_ptr msg = std::make_shared(); + msg->dof_names = controller_->params_.dof_names; + msg->values.resize(msg->dof_names.size(), 0.0); + for (size_t i = 0; i < msg->dof_names.size(); ++i) + { + msg->values[i] = target_value; + } + msg->values_dot.resize(msg->dof_names.size(), std::numeric_limits::quiet_NaN()); + controller_->input_ref_.writeFromNonRT(msg); + ASSERT_EQ( + controller_->update_reference_from_subscribers( + rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // run update + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // check on result from update + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 4471f35a12..dd5ae029b9 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -59,6 +59,8 @@ class TestablePidController : public pid_controller::PidController FRIEND_TEST(PidControllerTest, test_update_logic_chainable_feedforward_on); FRIEND_TEST(PidControllerTest, subscribe_and_get_messages_success); FRIEND_TEST(PidControllerTest, receive_message_and_publish_updated_status); + FRIEND_TEST(PidControllerTest, test_update_chained_feedforward_with_gain); + FRIEND_TEST(PidControllerTest, test_update_chained_feedforward_off_with_gain); public: controller_interface::CallbackReturn on_configure( From c9ca6f728c61944af0edaa32742e7f7acd86ad75 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 13 Feb 2025 18:49:48 +0100 Subject: [PATCH 81/84] Update API of PID class (#1437) --- .../gripper_controllers/hardware_interface_adapter.hpp | 3 +-- .../src/joint_trajectory_controller.cpp | 7 +++---- .../test/test_trajectory_controller.cpp | 4 ++-- pid_controller/src/pid_controller.cpp | 8 ++++---- 4 files changed, 10 insertions(+), 12 deletions(-) diff --git a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp index 98fd5a56b1..66da3d869a 100644 --- a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp +++ b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp @@ -165,8 +165,7 @@ class HardwareInterfaceAdapter // Time since the last call to update const auto period = std::chrono::steady_clock::now() - last_update_time_; // Update PIDs - double command = - pid_->computeCommand(error_position, error_velocity, static_cast(period.count())); + double command = pid_->compute_command(error_position, error_velocity, period); command = std::min( fabs(max_allowed_effort), std::max(-fabs(max_allowed_effort), command)); joint_handle_->get().set_value(command); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 550ab9b476..12b39120d8 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -290,9 +290,8 @@ controller_interface::return_type JointTrajectoryController::update( for (auto i = 0ul; i < dof_; ++i) { tmp_command_[i] = (command_next_.velocities[i] * ff_velocity_scale_[i]) + - pids_[i]->computeCommand( - state_error_.positions[i], state_error_.velocities[i], - (uint64_t)period.nanoseconds()); + pids_[i]->compute_command( + state_error_.positions[i], state_error_.velocities[i], period); } } @@ -1582,7 +1581,7 @@ void JointTrajectoryController::update_pids() if (pids_[i]) { // update PIDs with gains from ROS parameters - pids_[i]->setGains(gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); + pids_[i]->set_gains(gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); } else { diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 34f44e2d8c..0969866422 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -325,7 +325,7 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters) if (traj_controller_->use_closed_loop_pid_adapter()) { EXPECT_EQ(pids.size(), 3); - auto gain_0 = pids.at(0)->getGains(); + auto gain_0 = pids.at(0)->get_gains(); EXPECT_EQ(gain_0.p_gain_, 0.0); double kp = 1.0; @@ -334,7 +334,7 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters) pids = traj_controller_->get_pids(); EXPECT_EQ(pids.size(), 3); - gain_0 = pids.at(0)->getGains(); + gain_0 = pids.at(0)->get_gains(); EXPECT_EQ(gain_0.p_gain_, kp); } else diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index dd4d6df185..709038046d 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -140,7 +140,7 @@ controller_interface::CallbackReturn PidController::configure_parameters() // prefix should be interpreted as parameters prefix pids_[i] = std::make_shared(get_node(), "gains." + params_.dof_names[i], true); - if (!pids_[i]->initPid()) + if (!pids_[i]->initialize_from_ros_parameters()) { return CallbackReturn::FAILURE; } @@ -539,19 +539,19 @@ controller_interface::return_type PidController::update_and_write_commands( !std::isnan(measured_state_values_[dof_ + i])) { // use calculation with 'error' and 'error_dot' - tmp_command += pids_[i]->computeCommand( + tmp_command += pids_[i]->compute_command( error, reference_interfaces_[dof_ + i] - measured_state_values_[dof_ + i], period); } else { // Fallback to calculation with 'error' only - tmp_command += pids_[i]->computeCommand(error, period); + tmp_command += pids_[i]->compute_command(error, period); } } else { // use calculation with 'error' only - tmp_command += pids_[i]->computeCommand(error, period); + tmp_command += pids_[i]->compute_command(error, period); } // write calculated values From 623dc5d36e1878a6c748c1f13cf3f74c5c7ac746 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 14 Feb 2025 09:10:02 +0100 Subject: [PATCH 82/84] Reset PID controllers on activation and add `save_i_term` parameter (#1507) --- doc/release_notes.rst | 1 + pid_controller/src/pid_controller.cpp | 5 + pid_controller/src/pid_controller.yaml | 5 + .../test/pid_controller_params.yaml | 24 ++++ pid_controller/test/test_pid_controller.cpp | 135 ++++++++++++++++++ pid_controller/test/test_pid_controller.hpp | 2 + 6 files changed, 172 insertions(+) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 74cd0402f1..9435b7141b 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -60,6 +60,7 @@ mecanum_drive_controller pid_controller ************************ * 🚀 The PID controller was added 🎉 (`#434 `_). +* Add ``save_i_term`` parameter to control retention of integral state after re-activation (`#1507 `_). steering_controllers_library ******************************** diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 709038046d..dfb4c8e706 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -442,6 +442,11 @@ controller_interface::CallbackReturn PidController::on_activate( measured_state_values_.assign( measured_state_values_.size(), std::numeric_limits::quiet_NaN()); + // prefixed save_i_term parameter is read from ROS parameters + for (auto & pid : pids_) + { + pid->reset(); + } return controller_interface::CallbackReturn::SUCCESS; } diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml index 2d91b5a677..02448f2a19 100644 --- a/pid_controller/src/pid_controller.yaml +++ b/pid_controller/src/pid_controller.yaml @@ -89,3 +89,8 @@ pid_controller: description: "For joints that wrap around (i.e., are continuous). Normalizes position-error to -pi to pi." } + save_i_term: { + type: bool, + default_value: true, + description: "Indicating if integral term is retained after re-activation" + } diff --git a/pid_controller/test/pid_controller_params.yaml b/pid_controller/test/pid_controller_params.yaml index cf0a3f4173..c8cc14753f 100644 --- a/pid_controller/test/pid_controller_params.yaml +++ b/pid_controller/test/pid_controller_params.yaml @@ -10,6 +10,30 @@ test_pid_controller: gains: joint1: {p: 1.0, i: 2.0, d: 10.0, i_clamp_max: 5.0, i_clamp_min: -5.0} +test_save_i_term_off: + ros__parameters: + dof_names: + - joint1 + + command_interface: position + + reference_and_state_interfaces: ["position"] + + gains: + joint1: {p: 1.0, i: 2.0, d: 3.0, i_clamp_max: 5.0, i_clamp_min: -5.0, save_i_term: false} + + +test_save_i_term_on: + ros__parameters: + dof_names: + - joint1 + + command_interface: position + + reference_and_state_interfaces: ["position"] + + gains: + joint1: {p: 1.0, i: 2.0, d: 3.0, i_clamp_max: 5.0, i_clamp_min: -5.0, save_i_term: true} test_pid_controller_with_feedforward_gain: ros__parameters: diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 67ac680eae..75c39fbc44 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -673,6 +673,141 @@ TEST_F(PidControllerTest, test_update_chained_feedforward_off_with_gain) ASSERT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value); } +/** + * @brief Test if retention of the integral state is deactivated + * + */ + +TEST_F(PidControllerTest, test_save_i_term_off) +{ + SetUpController("test_save_i_term_off"); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(false); + for (const auto & dof_name : dof_names_) + { + ASSERT_FALSE(controller_->params_.gains.dof_names_map[dof_name].save_i_term); + } + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + + std::shared_ptr msg = std::make_shared(); + msg->dof_names = dof_names_; + msg->values.resize(dof_names_.size(), 0.0); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + msg->values[i] = dof_command_values_[i]; + } + msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); + controller_->input_ref_.writeFromNonRT(msg); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // check the command value + // error = ref - state = 100.001, error_dot = error/ds = 10000.1, + // p_term = 100.001 * 1, i_term = 1.00001 * 2 = 2.00002, d_term = error/ds = 10000.1 * 3 + // feedforward OFF -> cmd = p_term + i_term + d_term = 30102.3 + auto expected_command_value = 30102.30102; + + double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; + EXPECT_NEAR(actual_value, expected_command_value, 1e-5); + + // deactivate the controller and set command=state + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + msg = std::make_shared(); + msg->dof_names = dof_names_; + msg->values.resize(dof_names_.size(), 0.0); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + msg->values[i] = dof_state_values_[i]; + } + msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); + controller_->input_ref_.writeFromNonRT(msg); + + // reactivate the controller, the integral term should NOT be saved + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; + EXPECT_NEAR(actual_value, 0.0, 1e-5); +} + +/** + * @brief Test if retention of the integral state is working + * + */ + +TEST_F(PidControllerTest, test_save_i_term_on) +{ + SetUpController("test_save_i_term_on"); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + for (const auto & dof_name : dof_names_) + { + ASSERT_TRUE(controller_->params_.gains.dof_names_map[dof_name].save_i_term); + } + controller_->set_chained_mode(false); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + + std::shared_ptr msg = std::make_shared(); + msg->dof_names = dof_names_; + msg->values.resize(dof_names_.size(), 0.0); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + msg->values[i] = dof_command_values_[i]; + } + msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); + controller_->input_ref_.writeFromNonRT(msg); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // check the command value + // error = ref - state = 100.001, error_dot = error/ds = 10000.1, + // p_term = 100.001 * 1, i_term = 1.00001 * 2 = 2.00002, d_term = error/ds = 10000.1 * 3 + // feedforward OFF -> cmd = p_term + i_term + d_term = 30102.3 + auto expected_command_value = 30102.30102; + + double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; + EXPECT_NEAR(actual_value, expected_command_value, 1e-5); + + // deactivate the controller and set command=state + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + msg = std::make_shared(); + msg->dof_names = dof_names_; + msg->values.resize(dof_names_.size(), 0.0); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + msg->values[i] = dof_state_values_[i]; + } + msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); + controller_->input_ref_.writeFromNonRT(msg); + + // reactivate the controller, the integral term should be saved + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + expected_command_value = 2.00002; // i_term from above + actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; + EXPECT_NEAR(actual_value, expected_command_value, 1e-5); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index dd5ae029b9..1ec762568c 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -61,6 +61,8 @@ class TestablePidController : public pid_controller::PidController FRIEND_TEST(PidControllerTest, receive_message_and_publish_updated_status); FRIEND_TEST(PidControllerTest, test_update_chained_feedforward_with_gain); FRIEND_TEST(PidControllerTest, test_update_chained_feedforward_off_with_gain); + FRIEND_TEST(PidControllerTest, test_save_i_term_on); + FRIEND_TEST(PidControllerTest, test_save_i_term_off); public: controller_interface::CallbackReturn on_configure( From b24b3100bcedc4a339f7eaface0b20bcdef02328 Mon Sep 17 00:00:00 2001 From: Julia Jia Date: Fri, 14 Feb 2025 12:03:30 -0800 Subject: [PATCH 83/84] [pid_controller] Update tests (#1538) --- pid_controller/CMakeLists.txt | 13 + pid_controller/src/pid_controller.cpp | 18 +- .../test/pid_controller_params.yaml | 39 ++- pid_controller/test/test_pid_controller.cpp | 244 +++++++----------- pid_controller/test/test_pid_controller.hpp | 22 +- .../test_pid_controller_dual_interface.cpp | 110 ++++++++ 6 files changed, 277 insertions(+), 169 deletions(-) create mode 100644 pid_controller/test/test_pid_controller_dual_interface.cpp diff --git a/pid_controller/CMakeLists.txt b/pid_controller/CMakeLists.txt index d1374d54cb..d07546bada 100644 --- a/pid_controller/CMakeLists.txt +++ b/pid_controller/CMakeLists.txt @@ -92,6 +92,19 @@ if(BUILD_TESTING) hardware_interface ) + add_rostest_with_parameters_gmock( + test_pid_controller_dual_interface + test/test_pid_controller_dual_interface.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/pid_controller_params.yaml + ) + target_include_directories(test_pid_controller_dual_interface PRIVATE include) + target_link_libraries(test_pid_controller_dual_interface pid_controller) + ament_target_dependencies( + test_pid_controller_dual_interface + controller_interface + hardware_interface + ) + ament_add_gmock(test_load_pid_controller test/test_load_pid_controller.cpp) target_include_directories(test_load_pid_controller PRIVATE include) ament_target_dependencies( diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index dfb4c8e706..782d39dfdc 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -477,6 +477,7 @@ controller_interface::return_type PidController::update_and_write_commands( // check for any parameter updates update_parameters(); + // Update feedback either from external measured state or from state interfaces if (params_.use_external_measured_states) { const auto measured_state = *(measured_state_.readFromRT()); @@ -503,17 +504,18 @@ controller_interface::return_type PidController::update_and_write_commands( state_interfaces_values_[i] = measured_state_values_[i]; } + // Iterate through all the dofs to calculate the output command for (size_t i = 0; i < dof_; ++i) { double tmp_command = 0.0; - if (!std::isnan(reference_interfaces_[i]) && !std::isnan(measured_state_values_[i])) + if (std::isfinite(reference_interfaces_[i]) && std::isfinite(measured_state_values_[i])) { // calculate feed-forward if (*(control_mode_.readFromRT()) == feedforward_mode_type::ON) { // two interfaces - if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_) + if (reference_interfaces_.size() == 2 * dof_) { if (std::isfinite(reference_interfaces_[dof_ + i])) { @@ -540,8 +542,8 @@ controller_interface::return_type PidController::update_and_write_commands( if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_) { if ( - !std::isnan(reference_interfaces_[dof_ + i]) && - !std::isnan(measured_state_values_[dof_ + i])) + std::isfinite(reference_interfaces_[dof_ + i]) && + std::isfinite(measured_state_values_[dof_ + i])) { // use calculation with 'error' and 'error_dot' tmp_command += pids_[i]->compute_command( @@ -560,7 +562,13 @@ controller_interface::return_type PidController::update_and_write_commands( } // write calculated values - command_interfaces_[i].set_value(tmp_command); + auto success = command_interfaces_[i].set_value(tmp_command); + if (!success) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to set command value for %s", + command_interfaces_[i].get_name().c_str()); + } } } diff --git a/pid_controller/test/pid_controller_params.yaml b/pid_controller/test/pid_controller_params.yaml index c8cc14753f..15c4eaf049 100644 --- a/pid_controller/test/pid_controller_params.yaml +++ b/pid_controller/test/pid_controller_params.yaml @@ -8,9 +8,9 @@ test_pid_controller: reference_and_state_interfaces: ["position"] gains: - joint1: {p: 1.0, i: 2.0, d: 10.0, i_clamp_max: 5.0, i_clamp_min: -5.0} + joint1: {p: 1.0, i: 2.0, d: 3.0, i_clamp_max: 5.0, i_clamp_min: -5.0} -test_save_i_term_off: +test_pid_controller_angle_wraparound_on: ros__parameters: dof_names: - joint1 @@ -20,10 +20,35 @@ test_save_i_term_off: reference_and_state_interfaces: ["position"] gains: - joint1: {p: 1.0, i: 2.0, d: 3.0, i_clamp_max: 5.0, i_clamp_min: -5.0, save_i_term: false} + joint1: {p: 1.0, i: 2.0, d: 3.0, i_clamp_max: 5.0, i_clamp_min: -5.0, angle_wraparound: true} +test_pid_controller_with_feedforward_gain: + ros__parameters: + dof_names: + - joint1 -test_save_i_term_on: + command_interface: position + + reference_and_state_interfaces: ["position"] + + gains: + joint1: {p: 0.5, i: 0.0, d: 0.0, i_clamp_max: 5.0, i_clamp_min: -5.0, feedforward_gain: 1.0} + +test_pid_controller_with_feedforward_gain_dual_interface: + ros__parameters: + dof_names: + - joint1 + - joint2 + + command_interface: velocity + + reference_and_state_interfaces: ["position", "velocity"] + + gains: + joint1: {p: 0.5, i: 0.3, d: 0.4, i_clamp_max: 5.0, i_clamp_min: -5.0, feedforward_gain: 1.0} + joint2: {p: 0.5, i: 0.3, d: 0.4, i_clamp_max: 5.0, i_clamp_min: -5.0, feedforward_gain: 1.0} + +test_save_i_term_off: ros__parameters: dof_names: - joint1 @@ -33,9 +58,9 @@ test_save_i_term_on: reference_and_state_interfaces: ["position"] gains: - joint1: {p: 1.0, i: 2.0, d: 3.0, i_clamp_max: 5.0, i_clamp_min: -5.0, save_i_term: true} + joint1: {p: 1.0, i: 2.0, d: 3.0, i_clamp_max: 5.0, i_clamp_min: -5.0, save_i_term: false} -test_pid_controller_with_feedforward_gain: +test_save_i_term_on: ros__parameters: dof_names: - joint1 @@ -45,4 +70,4 @@ test_pid_controller_with_feedforward_gain: reference_and_state_interfaces: ["position"] gains: - joint1: {p: 0.5, i: 0.0, d: 0.0, i_clamp_max: 5.0, i_clamp_min: -5.0, feedforward_gain: 1.0} + joint1: {p: 1.0, i: 2.0, d: 3.0, i_clamp_max: 5.0, i_clamp_min: -5.0, save_i_term: true} diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 75c39fbc44..1c0494a002 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -17,7 +17,6 @@ #include "test_pid_controller.hpp" -#include #include #include #include @@ -47,7 +46,7 @@ TEST_F(PidControllerTest, all_parameters_set_configure_success) { ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].p, 1.0); ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].i, 2.0); - ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].d, 10.0); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].d, 3.0); ASSERT_FALSE(controller_->params_.gains.dof_names_map[dof_name].antiwindup); ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].i_clamp_max, 5.0); ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].i_clamp_min, -5.0); @@ -223,6 +222,11 @@ TEST_F(PidControllerTest, test_feedforward_mode_service) ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); } +/** + * @brief Check the update logic in non chained mode with feedforward OFF + * + */ + TEST_F(PidControllerTest, test_update_logic_feedforward_off) { SetUpController(); @@ -241,15 +245,7 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_off) EXPECT_TRUE(std::isnan(interface)); } - std::shared_ptr msg = std::make_shared(); - msg->dof_names = dof_names_; - msg->values.resize(dof_names_.size(), 0.0); - for (size_t i = 0; i < dof_command_values_.size(); ++i) - { - msg->values[i] = dof_command_values_[i]; - } - msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); - controller_->input_ref_.writeFromNonRT(msg); + controller_->set_reference(dof_command_values_); for (size_t i = 0; i < dof_command_values_.size(); ++i) { @@ -270,9 +266,22 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_off) { EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); } + // check the command value + // error = ref - state = 100.001, error_dot = error/ds = 10000.1, + // p_term = 100.001 * 1, i_term = 1.00001 * 2 = 2.00002, d_term = error/ds = 10000.1 * 3 + // feedforward OFF -> cmd = p_term + i_term + d_term = 30102.3 + const double expected_command_value = 30102.30102; + + double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; + EXPECT_NEAR(actual_value, expected_command_value, 1e-5); } -TEST_F(PidControllerTest, test_update_logic_feedforward_on) +/** + * @brief Check the update logic in non chained mode with feedforward ON and feedforward gain is 0 + * + */ + +TEST_F(PidControllerTest, test_update_logic_feedforward_on_with_zero_feedforward_gain) { SetUpController(); rclcpp::executors::MultiThreadedExecutor executor; @@ -290,15 +299,7 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_on) EXPECT_TRUE(std::isnan(interface)); } - std::shared_ptr msg = std::make_shared(); - msg->dof_names = dof_names_; - msg->values.resize(dof_names_.size(), 0.0); - for (size_t i = 0; i < dof_command_values_.size(); ++i) - { - msg->values[i] = dof_command_values_[i]; - } - msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); - controller_->input_ref_.writeFromNonRT(msg); + controller_->set_reference(dof_command_values_); controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON); EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); @@ -321,101 +322,73 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_on) for (size_t i = 0; i < dof_command_values_.size(); ++i) { EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); - } -} -TEST_F(PidControllerTest, test_update_logic_chainable_feedforward_off) -{ - SetUpController(); - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(controller_->get_node()->get_node_base_interface()); - executor.add_node(service_caller_node_->get_node_base_interface()); + // check the command value: + // ref = 101.101, state = 1.1, ds = 0.01 + // error = ref - state = 100.001, error_dot = error/ds = 10000.1, + // p_term = 100.001 * 1, i_term = 1.00001 * 2 = 2.00002, d_term = error/ds = 10000.1 * 3 + // feedforward ON, feedforward_gain = 0 + // -> cmd = p_term + i_term + d_term + feedforward_gain * ref = 30102.3 + 0 * 101.101 = 30102.3 + const double expected_command_value = 30102.301020; - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - controller_->set_chained_mode(true); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(controller_->is_in_chained_mode()); - - std::shared_ptr msg = std::make_shared(); - msg->dof_names = dof_names_; - msg->values.resize(dof_names_.size(), 0.0); - for (size_t i = 0; i < dof_command_values_.size(); ++i) - { - msg->values[i] = dof_command_values_[i]; - } - msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); - controller_->input_ref_.writeFromNonRT(msg); - - for (size_t i = 0; i < dof_command_values_.size(); ++i) - { - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); - EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); - EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); - } - - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - ASSERT_TRUE(controller_->is_in_chained_mode()); - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); - EXPECT_EQ( - controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); - EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); - for (size_t i = 0; i < dof_command_values_.size(); ++i) - { - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); - EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; + EXPECT_NEAR(actual_value, expected_command_value, 1e-5); } } -TEST_F(PidControllerTest, test_update_logic_chainable_feedforward_on) +/** + * @brief Check the update logic when chain mode is on. + * in chain mode, update_reference_from_subscribers is not called from update method, and the + * reference value is used for calculation + */ + +TEST_F(PidControllerTest, test_update_logic_chainable_not_use_subscriber_update) { SetUpController(); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); executor.add_node(service_caller_node_->get_node_base_interface()); + // set chain mode to true ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); controller_->set_chained_mode(true); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_TRUE(controller_->is_in_chained_mode()); + // feedforward mode is off as default, use this for convenience EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); - std::shared_ptr msg = std::make_shared(); - msg->dof_names = dof_names_; - msg->values.resize(dof_names_.size(), 0.0); - for (size_t i = 0; i < dof_command_values_.size(); ++i) - { - msg->values[i] = dof_command_values_[i]; - } - msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); - controller_->input_ref_.writeFromNonRT(msg); + // update reference interface which will be used for calculation + const double ref_interface_value = 5.0; + controller_->reference_interfaces_[0] = ref_interface_value; - controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON); - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + // publish a command message which should be ignored as chain mode is on + publish_commands({10.0}, {0.0}); + controller_->wait_for_commands(executor); - for (size_t i = 0; i < dof_command_values_.size(); ++i) - { - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); - EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); - EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); - } + // check the reference interface is not updated as chain mode is on + EXPECT_EQ(controller_->reference_interfaces_[0], ref_interface_value); + // run update ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); ASSERT_TRUE(controller_->is_in_chained_mode()); - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + EXPECT_EQ( controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); - for (size_t i = 0; i < dof_command_values_.size(); ++i) - { - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); - EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); - } + + // check the command value + // ref = 5.0, state = 1.1, ds = 0.01, p_gain = 1.0, i_gain = 2.0, d_gain = 3.0 + // error = ref - state = 5.0 - 1.1 = 3.9, error_dot = error/ds = 3.9/0.01 = 390.0, + // p_term = error * p_gain = 3.9 * 1.0 = 3.9, + // i_term = error * ds * i_gain = 3.9 * 0.01 * 2.0 = 0.078, + // d_term = error_dot * d_gain = 390.0 * 3.0 = 1170.0 + // feedforward OFF -> cmd = p_term + i_term + d_term = 3.9 + 0.078 + 1170.0 = 1173.978 + const double expected_command_value = 1173.978; + + EXPECT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value); } /** @@ -429,15 +402,20 @@ TEST_F(PidControllerTest, test_update_logic_angle_wraparound_off) executor.add_node(service_caller_node_->get_node_base_interface()); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - controller_->set_chained_mode(true); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(controller_->is_in_chained_mode()); + ASSERT_FALSE(controller_->params_.gains.dof_names_map[dof_names_[0]].angle_wraparound); // write reference interface so that the values are would be wrapped + controller_->reference_interfaces_[0] = 10.0; // run update + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // check the result of the commands - the values are not wrapped + const double expected_command_value = 2679.078; + EXPECT_NEAR(controller_->command_interfaces_[0].get_value(), expected_command_value, 1e-5); } /** @@ -445,7 +423,7 @@ TEST_F(PidControllerTest, test_update_logic_angle_wraparound_off) */ TEST_F(PidControllerTest, test_update_logic_angle_wraparound_on) { - SetUpController(); + SetUpController("test_pid_controller_angle_wraparound_on"); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); executor.add_node(service_caller_node_->get_node_base_interface()); @@ -455,11 +433,20 @@ TEST_F(PidControllerTest, test_update_logic_angle_wraparound_on) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_TRUE(controller_->is_in_chained_mode()); - // write reference interface so that the values are would be wrapped + // Check on wraparound is on + ASSERT_TRUE(controller_->params_.gains.dof_names_map[dof_names_[0]].angle_wraparound); - // run update + // Write reference interface with values that would wrap, state is 1.1 + controller_->reference_interfaces_[0] = 10.0; + + // Run update + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); - // check the result of the commands - the values are wrapped + // Check the command value + const double expected_command_value = 787.713559; + EXPECT_NEAR(controller_->command_interfaces_[0].get_value(), expected_command_value, 1e-5); } TEST_F(PidControllerTest, subscribe_and_get_messages_success) @@ -584,15 +571,7 @@ TEST_F(PidControllerTest, test_update_chained_feedforward_with_gain) ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); // send a message to update reference interface - std::shared_ptr msg = std::make_shared(); - msg->dof_names = controller_->params_.dof_names; - msg->values.resize(msg->dof_names.size(), 0.0); - for (size_t i = 0; i < msg->dof_names.size(); ++i) - { - msg->values[i] = target_value; - } - msg->values_dot.resize(msg->dof_names.size(), std::numeric_limits::quiet_NaN()); - controller_->input_ref_.writeFromNonRT(msg); + controller_->set_reference({target_value}); ASSERT_EQ( controller_->update_reference_from_subscribers( rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), @@ -650,15 +629,7 @@ TEST_F(PidControllerTest, test_update_chained_feedforward_off_with_gain) ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); // send a message to update reference interface - std::shared_ptr msg = std::make_shared(); - msg->dof_names = controller_->params_.dof_names; - msg->values.resize(msg->dof_names.size(), 0.0); - for (size_t i = 0; i < msg->dof_names.size(); ++i) - { - msg->values[i] = target_value; - } - msg->values_dot.resize(msg->dof_names.size(), std::numeric_limits::quiet_NaN()); - controller_->input_ref_.writeFromNonRT(msg); + controller_->set_reference({target_value}); ASSERT_EQ( controller_->update_reference_from_subscribers( rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), @@ -694,15 +665,7 @@ TEST_F(PidControllerTest, test_save_i_term_off) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_FALSE(controller_->is_in_chained_mode()); - std::shared_ptr msg = std::make_shared(); - msg->dof_names = dof_names_; - msg->values.resize(dof_names_.size(), 0.0); - for (size_t i = 0; i < dof_command_values_.size(); ++i) - { - msg->values[i] = dof_command_values_[i]; - } - msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); - controller_->input_ref_.writeFromNonRT(msg); + controller_->set_reference(dof_command_values_); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), @@ -712,23 +675,14 @@ TEST_F(PidControllerTest, test_save_i_term_off) // error = ref - state = 100.001, error_dot = error/ds = 10000.1, // p_term = 100.001 * 1, i_term = 1.00001 * 2 = 2.00002, d_term = error/ds = 10000.1 * 3 // feedforward OFF -> cmd = p_term + i_term + d_term = 30102.3 - auto expected_command_value = 30102.30102; + const double expected_command_value = 30102.30102; double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; EXPECT_NEAR(actual_value, expected_command_value, 1e-5); // deactivate the controller and set command=state ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); - - msg = std::make_shared(); - msg->dof_names = dof_names_; - msg->values.resize(dof_names_.size(), 0.0); - for (size_t i = 0; i < dof_command_values_.size(); ++i) - { - msg->values[i] = dof_state_values_[i]; - } - msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); - controller_->input_ref_.writeFromNonRT(msg); + controller_->set_reference(dof_state_values_); // reactivate the controller, the integral term should NOT be saved ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -761,15 +715,7 @@ TEST_F(PidControllerTest, test_save_i_term_on) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_FALSE(controller_->is_in_chained_mode()); - std::shared_ptr msg = std::make_shared(); - msg->dof_names = dof_names_; - msg->values.resize(dof_names_.size(), 0.0); - for (size_t i = 0; i < dof_command_values_.size(); ++i) - { - msg->values[i] = dof_command_values_[i]; - } - msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); - controller_->input_ref_.writeFromNonRT(msg); + controller_->set_reference(dof_command_values_); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), @@ -779,23 +725,14 @@ TEST_F(PidControllerTest, test_save_i_term_on) // error = ref - state = 100.001, error_dot = error/ds = 10000.1, // p_term = 100.001 * 1, i_term = 1.00001 * 2 = 2.00002, d_term = error/ds = 10000.1 * 3 // feedforward OFF -> cmd = p_term + i_term + d_term = 30102.3 - auto expected_command_value = 30102.30102; + const double expected_command_value = 30102.30102; double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; EXPECT_NEAR(actual_value, expected_command_value, 1e-5); // deactivate the controller and set command=state ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); - - msg = std::make_shared(); - msg->dof_names = dof_names_; - msg->values.resize(dof_names_.size(), 0.0); - for (size_t i = 0; i < dof_command_values_.size(); ++i) - { - msg->values[i] = dof_state_values_[i]; - } - msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); - controller_->input_ref_.writeFromNonRT(msg); + controller_->set_reference(dof_state_values_); // reactivate the controller, the integral term should be saved ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -803,9 +740,8 @@ TEST_F(PidControllerTest, test_save_i_term_on) ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - expected_command_value = 2.00002; // i_term from above actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; - EXPECT_NEAR(actual_value, expected_command_value, 1e-5); + EXPECT_NEAR(actual_value, 2.00002, 1e-5); // i_term from above } int main(int argc, char ** argv) diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 1ec762568c..1451ae919b 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -54,13 +55,15 @@ class TestablePidController : public pid_controller::PidController FRIEND_TEST(PidControllerTest, reactivate_success); FRIEND_TEST(PidControllerTest, test_feedforward_mode_service); FRIEND_TEST(PidControllerTest, test_update_logic_feedforward_off); - FRIEND_TEST(PidControllerTest, test_update_logic_feedforward_on); - FRIEND_TEST(PidControllerTest, test_update_logic_chainable_feedforward_off); - FRIEND_TEST(PidControllerTest, test_update_logic_chainable_feedforward_on); + FRIEND_TEST(PidControllerTest, test_update_logic_feedforward_on_with_zero_feedforward_gain); + FRIEND_TEST(PidControllerTest, test_update_logic_chainable_not_use_subscriber_update); + FRIEND_TEST(PidControllerTest, test_update_logic_angle_wraparound_off); + FRIEND_TEST(PidControllerTest, test_update_logic_angle_wraparound_on); FRIEND_TEST(PidControllerTest, subscribe_and_get_messages_success); FRIEND_TEST(PidControllerTest, receive_message_and_publish_updated_status); FRIEND_TEST(PidControllerTest, test_update_chained_feedforward_with_gain); FRIEND_TEST(PidControllerTest, test_update_chained_feedforward_off_with_gain); + FRIEND_TEST(PidControllerDualInterfaceTest, test_chained_feedforward_with_gain_dual_interface); FRIEND_TEST(PidControllerTest, test_save_i_term_on); FRIEND_TEST(PidControllerTest, test_save_i_term_off); @@ -102,6 +105,19 @@ class TestablePidController : public pid_controller::PidController { wait_for_command(executor, timeout); } + + void set_reference(const std::vector & target_value) + { + std::shared_ptr msg = std::make_shared(); + msg->dof_names = params_.dof_names; + msg->values.resize(msg->dof_names.size(), 0.0); + for (size_t i = 0; i < msg->dof_names.size(); ++i) + { + msg->values[i] = target_value[i]; + } + msg->values_dot.resize(msg->dof_names.size(), std::numeric_limits::quiet_NaN()); + input_ref_.writeFromNonRT(msg); + } }; // We are using template class here for easier reuse of Fixture in specializations of controllers diff --git a/pid_controller/test/test_pid_controller_dual_interface.cpp b/pid_controller/test/test_pid_controller_dual_interface.cpp new file mode 100644 index 0000000000..e006986473 --- /dev/null +++ b/pid_controller/test/test_pid_controller_dual_interface.cpp @@ -0,0 +1,110 @@ +// Copyright 2025 ros2_control Development Team +// +// 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. +// + +#include "test_pid_controller.hpp" + +#include +#include +#include +#include + +using pid_controller::feedforward_mode_type; + +class PidControllerDualInterfaceTest : public PidControllerFixture +{ +public: + void SetUp() + { + PidControllerFixture::SetUp(); + + dof_names_ = {"joint1", "joint2"}; + + // set up interfaces + command_interface_ = "velocity"; + state_interfaces_ = {"position", "velocity"}; + dof_state_values_ = { + get_joint1_state_position(), get_joint2_state_position(), get_joint1_state_velocity(), + get_joint2_state_velocity()}; + } + + double get_joint1_state_position() const { return 10.0; } + double get_joint2_state_position() const { return 11.0; } + double get_joint1_state_velocity() const { return 5.0; } + double get_joint2_state_velocity() const { return 6.0; } + + double get_joint1_reference_position() const { return 15.0; } + double get_joint2_reference_position() const { return 16.0; } + double get_joint1_reference_velocity() const { return 6.0; } + double get_joint2_reference_velocity() const { return 7.0; } +}; + +/** + * @brief Test the feedforward gain with chained mode with two interfaces + * The second interface should be derivative of the first one + */ + +TEST_F(PidControllerDualInterfaceTest, test_chained_feedforward_with_gain_dual_interface) +{ + SetUpController("test_pid_controller_with_feedforward_gain_dual_interface"); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check on interfaces & pid gain parameters + for (const auto & dof_name : dof_names_) + { + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].p, 0.5); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].i, 0.3); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].d, 0.4); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].feedforward_gain, 1.0); + } + + ASSERT_EQ(controller_->params_.command_interface, command_interface_); + EXPECT_THAT( + controller_->params_.reference_and_state_interfaces, + testing::ElementsAreArray(state_interfaces_)); + controller_->set_chained_mode(true); + + // activate controller + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + // turn on feedforward + controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + + // set up the reference interface, + controller_->reference_interfaces_ = { + get_joint1_reference_position(), get_joint2_reference_position(), + get_joint1_reference_velocity(), get_joint2_reference_velocity()}; + + // run update + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // check the commands + const double joint1_expected_cmd = 8.915; + const double joint2_expected_cmd = 9.915; + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), joint1_expected_cmd); + ASSERT_EQ(controller_->command_interfaces_[1].get_value(), joint2_expected_cmd); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} From fc20a277a3c0b5a4e43284d9d49e06b8dbef40a6 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Fri, 14 Feb 2025 22:59:24 +0100 Subject: [PATCH 84/84] [JTC] Use time of the last command for set_point_before_trajectory_msg in open-loop mode (#780) Co-authored-by: bijoua Co-authored-by: Christoph Froehlich --- .../joint_trajectory_controller.hpp | 1 + .../src/joint_trajectory_controller.cpp | 8 +++++++- .../test/test_trajectory_controller.cpp | 14 +++++++------- 3 files changed, 15 insertions(+), 8 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index af0f35be18..2176993828 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -112,6 +112,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa rclcpp::Time traj_time_; trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; + rclcpp::Time last_commanded_time_; /// Specify interpolation method. Default to splines. interpolation_methods::InterpolationMethod interpolation_method_{ interpolation_methods::DEFAULT_INTERPOLATION}; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 12b39120d8..069434c303 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -190,8 +190,12 @@ controller_interface::return_type JointTrajectoryController::update( first_sample = true; if (params_.open_loop_control) { + if (last_commanded_time_.seconds() == 0.0) + { + last_commanded_time_ = time; + } traj_external_point_ptr_->set_point_before_trajectory_msg( - time, last_commanded_state_, joints_angle_wraparound_); + last_commanded_time_, last_commanded_state_, joints_angle_wraparound_); } else { @@ -322,6 +326,7 @@ controller_interface::return_type JointTrajectoryController::update( // store the previous command. Used in open-loop control mode last_commanded_state_ = command_next_; + last_commanded_time_ = time; } if (active_goal) @@ -950,6 +955,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( read_state_from_state_interfaces(state_current_); read_state_from_state_interfaces(last_commanded_state_); } + last_commanded_time_ = rclcpp::Time(); // The controller should start by holding position at the beginning of active state add_new_trajectory_msg(set_hold_position()); diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 0969866422..e110e244b7 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1679,9 +1679,9 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro publish( time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME), {}, first_goal_velocities); traj_controller_->wait_for_trajectory(executor); - updateControllerAsync(rclcpp::Duration::from_seconds(1.1)); + auto end_time = updateControllerAsync(rclcpp::Duration::from_seconds(1.1)); - // JTC is executing trajectory in open-loop therefore: + // JTC is NOT executing trajectory in open-loop therefore: // - internal state does not have to be updated (in this test-case it shouldn't) // - internal command is updated EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); @@ -1698,7 +1698,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro // One the first update(s) there should be a "jump" in opposite direction from command // (towards the state value) EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - auto end_time = updateControllerAsync(controller_period); + end_time = updateControllerAsync(controller_period, end_time); // Expect backward commands at first, consider advancement of the trajectory // exact value is not directly predictable, because of the spline interpolation -> increase // tolerance @@ -1715,7 +1715,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro EXPECT_LT(joint_pos_[0], first_goal[0]); // Finally the second goal will be commanded/reached - updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); // State interface should have offset from the command before starting a new trajectory @@ -1783,7 +1783,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e std::vector> points{{first_goal}}; publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); traj_controller_->wait_for_trajectory(executor); - updateControllerAsync(rclcpp::Duration::from_seconds(1.1)); + auto end_time = updateControllerAsync(rclcpp::Duration::from_seconds(1.1)); // JTC is executing trajectory in open-loop therefore: // - internal state does not have to be updated (in this test-case it shouldn't) @@ -1802,7 +1802,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e // One the first update(s) there **should not** be a "jump" in opposite direction from // command (towards the state value) EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - auto end_time = updateControllerAsync(controller_period); + end_time = updateControllerAsync(controller_period, end_time); // There should not be backward commands // exact value is not directly predictable, because of the spline interpolation -> increase // tolerance @@ -1818,7 +1818,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e EXPECT_LT(joint_pos_[0], second_goal[0]); // Finally the second goal will be commanded/reached - updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); + end_time = updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); // State interface should have offset from the command before starting a new trajectory