diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index 7d57720a26..7aa7d55575 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -6,6 +6,7 @@ set_compiler_options() export_windows_symbols() set(THIS_PACKAGE_INCLUDE_DEPENDS + control_msgs control_toolbox controller_interface generate_parameter_library @@ -17,6 +18,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp_lifecycle rcpputils realtime_tools + std_srvs tf2 tf2_msgs ) @@ -60,7 +62,8 @@ target_link_libraries(diff_drive_controller tf2::tf2 ${tf2_msgs_TARGETS} ${geometry_msgs_TARGETS} - ${nav_msgs_TARGETS}) + ${nav_msgs_TARGETS} + ${std_srvs_TARGETS}) pluginlib_export_plugin_description_file(controller_interface diff_drive_plugin.xml) if(BUILD_TESTING) diff --git a/diff_drive_controller/doc/userdoc.rst b/diff_drive_controller/doc/userdoc.rst index f2ec74f7d9..86e28c97e3 100644 --- a/diff_drive_controller/doc/userdoc.rst +++ b/diff_drive_controller/doc/userdoc.rst @@ -70,6 +70,13 @@ Publishers ~/cmd_vel_out [geometry_msgs/msg/TwistStamped] Velocity command for the controller, where limits were applied. Published only if ``publish_limited_velocity=true`` +Services +,,,,,,,,,,, +~/set_odometry [control_msgs::srv::SetOdometry] + This service can be used to set the current odometry of the robot to desired values. + +/reset_odometry [std_srvs::srv::Empty] + This service can be used to reset the odometry of the robot to zero. Parameters ,,,,,,,,,,,, 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 fca6e30039..78c975fdaa 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 @@ -19,12 +19,14 @@ #ifndef DIFF_DRIVE_CONTROLLER__DIFF_DRIVE_CONTROLLER_HPP_ #define DIFF_DRIVE_CONTROLLER__DIFF_DRIVE_CONTROLLER_HPP_ +#include #include #include #include #include #include +#include "control_msgs/srv/set_odometry.hpp" #include "controller_interface/chainable_controller_interface.hpp" #include "diff_drive_controller/odometry.hpp" #include "diff_drive_controller/speed_limiter.hpp" @@ -34,6 +36,7 @@ #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_publisher.hpp" #include "realtime_tools/realtime_thread_safe_box.hpp" +#include "std_srvs/srv/empty.hpp" #include "tf2_msgs/msg/tf_message.hpp" // auto-generated by generate_parameter_library @@ -76,6 +79,15 @@ class DiffDriveController : public controller_interface::ChainableControllerInte controller_interface::CallbackReturn on_error( const rclcpp_lifecycle::State & previous_state) override; + void set_odometry( + const std::shared_ptr request_header, + const std::shared_ptr req, + std::shared_ptr res); + void reset_odometry( + const std::shared_ptr request_header, + const std::shared_ptr req, + std::shared_ptr res); + protected: bool on_set_chained_mode(bool chained_mode) override; @@ -147,6 +159,14 @@ 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}; + rclcpp::Service::SharedPtr set_odom_service_; + rclcpp::Service::SharedPtr reset_odom_service_; + std::atomic set_odom_request_{false}, reset_odom_request_{false}; + struct + { + double x, y, yaw; + } requested_odom_params_; + bool reset(); void halt(); diff --git a/diff_drive_controller/include/diff_drive_controller/odometry.hpp b/diff_drive_controller/include/diff_drive_controller/odometry.hpp index 8111614132..7b9df2d97e 100644 --- a/diff_drive_controller/include/diff_drive_controller/odometry.hpp +++ b/diff_drive_controller/include/diff_drive_controller/odometry.hpp @@ -55,6 +55,7 @@ class Odometry bool update_from_pos(double left_pos, double right_pos, double dt); bool update_from_vel(double left_vel, double right_vel, double dt); bool try_update_open_loop(double linear_vel, double angular_vel, double dt); + void setOdometry(double x, double y, double heading); void resetOdometry(); double getX() const { return x_; } diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index e1f5ad331f..77ea8f561c 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -26,6 +26,7 @@ ros2_control_cmake backward_ros + control_msgs control_toolbox controller_interface geometry_msgs @@ -36,6 +37,7 @@ rclcpp_lifecycle rcpputils realtime_tools + std_srvs tf2 tf2_msgs diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 92fa27a7be..a938655449 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -28,6 +28,7 @@ #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/logging.hpp" #include "tf2/LinearMath/Quaternion.hpp" +#include "tf2/impl/utils.hpp" namespace { @@ -35,6 +36,8 @@ constexpr auto DEFAULT_COMMAND_TOPIC = "~/cmd_vel"; constexpr auto DEFAULT_COMMAND_OUT_TOPIC = "~/cmd_vel_out"; constexpr auto DEFAULT_ODOMETRY_TOPIC = "~/odom"; constexpr auto DEFAULT_TRANSFORM_TOPIC = "/tf"; +constexpr auto DEFAULT_SET_ODOM_SERVICE = "~/set_odometry"; +constexpr auto DEFAULT_RESET_ODOM_SERVICE = "~/reset_odometry"; } // namespace namespace diff_drive_controller @@ -161,56 +164,75 @@ controller_interface::return_type DiffDriveController::update_and_write_commands const double left_wheel_radius = params_.left_wheel_radius_multiplier * params_.wheel_radius; const double right_wheel_radius = params_.right_wheel_radius_multiplier * params_.wheel_radius; - // Update odometry bool odometry_updated = false; - if (params_.open_loop) + + // check if odometry set or reset was requested by non-RT thread + if (set_odom_request_.load()) { - odometry_updated = - odometry_.try_update_open_loop(linear_command, angular_command, period.seconds()); + odometry_.setOdometry( + requested_odom_params_.x, requested_odom_params_.y, requested_odom_params_.yaw); + set_odom_request_.store(false); + odometry_updated = true; + } + else if (reset_odom_request_.load()) + { + odometry_.resetOdometry(); + reset_odom_request_.store(false); + odometry_updated = true; } else { - double left_feedback_mean = 0.0; - double right_feedback_mean = 0.0; - for (size_t index = 0; index < static_cast(wheels_per_side_); ++index) + // Update odometry + if (params_.open_loop) { - const auto left_feedback_op = - registered_left_wheel_handles_[index].feedback.value().get().get_optional(); - const auto right_feedback_op = - registered_right_wheel_handles_[index].feedback.value().get().get_optional(); - - if (!left_feedback_op.has_value() || !right_feedback_op.has_value()) + odometry_updated = + odometry_.try_update_open_loop(linear_command, angular_command, period.seconds()); + } + else + { + double left_feedback_mean = 0.0; + double right_feedback_mean = 0.0; + for (size_t index = 0; index < static_cast(wheels_per_side_); ++index) { - RCLCPP_DEBUG(logger, "Unable to retrieve the data from the left or right wheels feedback!"); - return controller_interface::return_type::OK; - } + const auto left_feedback_op = + registered_left_wheel_handles_[index].feedback.value().get().get_optional(); + const auto right_feedback_op = + registered_right_wheel_handles_[index].feedback.value().get().get_optional(); - const double left_feedback = left_feedback_op.value(); - const double right_feedback = right_feedback_op.value(); + if (!left_feedback_op.has_value() || !right_feedback_op.has_value()) + { + RCLCPP_DEBUG( + logger, "Unable to retrieve the data from the left or right wheels feedback!"); + return controller_interface::return_type::OK; + } - if (std::isnan(left_feedback) || std::isnan(right_feedback)) - { - RCLCPP_ERROR( - logger, "Either the left or right wheel %s is invalid for index [%zu]", feedback_type(), - index); - return controller_interface::return_type::ERROR; - } + const double left_feedback = left_feedback_op.value(); + const double right_feedback = right_feedback_op.value(); - left_feedback_mean += left_feedback; - right_feedback_mean += right_feedback; - } - left_feedback_mean /= static_cast(wheels_per_side_); - right_feedback_mean /= static_cast(wheels_per_side_); + if (std::isnan(left_feedback) || std::isnan(right_feedback)) + { + RCLCPP_ERROR( + logger, "Either the left or right wheel %s is invalid for index [%zu]", feedback_type(), + index); + return controller_interface::return_type::ERROR; + } + + left_feedback_mean += left_feedback; + right_feedback_mean += right_feedback; + } + left_feedback_mean /= static_cast(wheels_per_side_); + right_feedback_mean /= static_cast(wheels_per_side_); - if (params_.position_feedback) - { - odometry_updated = - odometry_.update_from_pos(left_feedback_mean, right_feedback_mean, period.seconds()); - } - else - { - odometry_updated = - odometry_.update_from_vel(left_feedback_mean, right_feedback_mean, period.seconds()); + if (params_.position_feedback) + { + odometry_updated = + odometry_.update_from_pos(left_feedback_mean, right_feedback_mean, period.seconds()); + } + else + { + odometry_updated = + odometry_.update_from_vel(left_feedback_mean, right_feedback_mean, period.seconds()); + } } } @@ -492,6 +514,16 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( odometry_transform_message_.transforms.front().header.frame_id = odom_frame_id; odometry_transform_message_.transforms.front().child_frame_id = base_frame_id; + // Create odometry set & reset services + set_odom_service_ = get_node()->create_service( + DEFAULT_SET_ODOM_SERVICE, std::bind( + &DiffDriveController::set_odometry, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); + reset_odom_service_ = get_node()->create_service( + DEFAULT_RESET_ODOM_SERVICE, std::bind( + &DiffDriveController::reset_odometry, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); + previous_update_timestamp_ = get_node()->get_clock()->now(); return controller_interface::CallbackReturn::SUCCESS; } @@ -556,6 +588,29 @@ controller_interface::CallbackReturn DiffDriveController::on_error(const rclcpp_ return controller_interface::CallbackReturn::SUCCESS; } +void DiffDriveController::set_odometry( + const std::shared_ptr /*request_header*/, + const std::shared_ptr req, + std::shared_ptr res) +{ + // flip the flag for thread-safe odom set in the control loop + set_odom_request_.store(true); + requested_odom_params_.x = req->x; + requested_odom_params_.y = req->y; + requested_odom_params_.yaw = req->yaw; + res->success = true; + res->message = "Odometry set requested"; +} +void DiffDriveController::reset_odometry( + const std::shared_ptr /*request_header*/, + const std::shared_ptr /*req*/, + std::shared_ptr /*res*/) +{ + // flip the flag for thread-safe odom reset in the control loop + reset_odom_request_.store(true); + RCLCPP_INFO(get_node()->get_logger(), "Odometry reset requested"); +} + bool DiffDriveController::reset() { odometry_.resetOdometry(); diff --git a/diff_drive_controller/src/odometry.cpp b/diff_drive_controller/src/odometry.cpp index d1fcc3fc65..aa64da4210 100644 --- a/diff_drive_controller/src/odometry.cpp +++ b/diff_drive_controller/src/odometry.cpp @@ -167,11 +167,19 @@ bool Odometry::try_update_open_loop(double linear_vel, double angular_vel, doubl return true; } +void Odometry::setOdometry(double x, double y, double heading) +{ + x_ = x; + y_ = y; + heading_ = heading; + resetAccumulators(); +} void Odometry::resetOdometry() { x_ = 0.0; y_ = 0.0; heading_ = 0.0; + resetAccumulators(); } void Odometry::setWheelParams( diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 6d5d86487a..f745ca90cd 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -45,6 +45,7 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr { public: using DiffDriveController::DiffDriveController; + using DiffDriveController::odometry_; /** * @brief wait_for_twist block until a new twist is received. @@ -1166,6 +1167,104 @@ TEST_F(TestDiffDriveController, command_with_zero_timestamp_is_accepted_with_war executor.cancel(); } +TEST_F(TestDiffDriveController, odometry_set_reset_services) +{ + // 0. Initialize and activate the controller + 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); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + auto state = controller_->configure(); + assignResourcesPosFeedback(); + + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + EXPECT_EQ(0.01, left_wheel_vel_cmd_->get_optional().value()); + EXPECT_EQ(0.02, right_wheel_vel_cmd_->get_optional().value()); + + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + rclcpp::Time test_time(0, 0, RCL_ROS_TIME); + rclcpp::Duration period = rclcpp::Duration::from_seconds(0.1); + + // 1. Move the robot first + publish(1.0, 0.0); + controller_->wait_for_twist(executor); + controller_->update(test_time, period); + test_time += period; + + // verify initial movement + ASSERT_GT(controller_->odometry_.getX(), 0.0); + + // 2. Stop and call odom reset + publish(0.0, 0.0); + controller_->wait_for_twist(executor); + + auto reset_request = std::make_shared(); + auto reset_response = std::make_shared(); + controller_->reset_odometry(nullptr, reset_request, reset_response); + + // run update to process the reset and verify odometry values are zeroed + controller_->update(test_time, period); + test_time += period; + EXPECT_EQ(controller_->odometry_.getX(), 0.0); + EXPECT_EQ(controller_->odometry_.getY(), 0.0); + EXPECT_EQ(controller_->odometry_.getHeading(), 0.0); + + // 3. Move again to ensure it still works after reset + publish(1.0, 0.0); + controller_->wait_for_twist(executor); + + // simulate the movement by updating the position feedback + position_values_[0] += 0.1; // left wheel moved + position_values_[1] += 0.1; // right wheel moved + controller_->update(test_time, period); + test_time += period; + ASSERT_GT(controller_->odometry_.getX(), 0.0); + + // 4. Stop and call odom set + publish(0.0, 0.0); + controller_->wait_for_twist(executor); + auto set_request = std::make_shared(); + auto set_response = std::make_shared(); + set_request->x = 5.0; + set_request->y = -2.0; + set_request->yaw = 1.57079632679; // 90 degrees + controller_->set_odometry(nullptr, set_request, set_response); + EXPECT_TRUE(set_response->success); + + // run update to process the set and verify odom values + controller_->update(test_time, period); + test_time += period; + EXPECT_NEAR(controller_->odometry_.getX(), 5.0, 1e-6); + EXPECT_NEAR(controller_->odometry_.getY(), -2.0, 1e-6); + EXPECT_NEAR(controller_->odometry_.getHeading(), 1.57079632679, 1e-5); // 90 deg + + // 5. Move again to ensure it still works + publish(1.0, 0.0); // we move in Y now + controller_->wait_for_twist(executor); + + // simulate the movement by updating the position feedback + position_values_[0] += 0.1; // left wheel moved + position_values_[1] += 0.1; // right wheel moved + controller_->update(test_time, period); + test_time += period; + EXPECT_GT(controller_->odometry_.getY(), -2.0); + + // 6. 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); diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 341a10318e..05b7907d93 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -17,6 +17,7 @@ diff_drive_controller ***************************** * Parameter ``tf_frame_prefix_enable`` got deprecated and will be removed in a future release (`#1997 `_). * Now any tilde ("~") character in ``tf_frame_prefix`` is substituted with node namespace. (`#1997 `_). +* Set and reset odometry services added to be used at runtime. (`#2096 `_). mecanum_drive_controller *****************************