From 247da672c3713e4cbb5ee66d9500fba53b76ab1a Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Sat, 16 Apr 2022 20:42:27 +0200 Subject: [PATCH 1/5] fix odometry issue in diff_drive_controller --- diff_drive_controller/src/diff_drive_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 382740d8f4..a752903274 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -218,7 +218,7 @@ controller_interface::return_type DiffDriveController::update( } else { - odometry_.updateFromVelocity(left_feedback_mean, right_feedback_mean, current_time); + odometry_.updateFromVelocity(left_feedback_mean*update_dt.seconds(), right_feedback_mean*update_dt.seconds(), current_time); } } From d7886835a99661ec62f78209a53868526299be43 Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Sat, 16 Apr 2022 20:50:03 +0200 Subject: [PATCH 2/5] Make update_dt variable appear earlier in code --- diff_drive_controller/src/diff_drive_controller.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index a752903274..0a1e196f7e 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -178,6 +178,9 @@ controller_interface::return_type DiffDriveController::update( double & linear_command = command.twist.linear.x; double & angular_command = command.twist.angular.z; + const auto update_dt = current_time - previous_update_timestamp_; + previous_update_timestamp_ = current_time; + // Apply (possibly new) multipliers: const auto wheels = wheel_params_; const double wheel_separation = wheels.separation_multiplier * wheels.separation; @@ -258,9 +261,6 @@ controller_interface::return_type DiffDriveController::update( } } - const auto update_dt = current_time - previous_update_timestamp_; - previous_update_timestamp_ = current_time; - auto & last_command = previous_commands_.back().twist; auto & second_to_last_command = previous_commands_.front().twist; limiter_linear_.limit( From b1aefc5f7dc9ab251490c085fe4c29a7bbf115d4 Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Sat, 16 Apr 2022 20:53:33 +0200 Subject: [PATCH 3/5] Reformat --- diff_drive_controller/src/diff_drive_controller.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 0a1e196f7e..5000993e10 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -221,7 +221,9 @@ controller_interface::return_type DiffDriveController::update( } else { - odometry_.updateFromVelocity(left_feedback_mean*update_dt.seconds(), right_feedback_mean*update_dt.seconds(), current_time); + odometry_.updateFromVelocity( + left_feedback_mean*update_dt.seconds(), right_feedback_mean*update_dt.seconds(), + current_time); } } From 81f956b592b405f9110eb85d4abb2de089e10009 Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Sat, 16 Apr 2022 20:55:42 +0200 Subject: [PATCH 4/5] Reformat --- diff_drive_controller/src/diff_drive_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 5000993e10..e1ab4d9e17 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -222,7 +222,7 @@ controller_interface::return_type DiffDriveController::update( else { odometry_.updateFromVelocity( - left_feedback_mean*update_dt.seconds(), right_feedback_mean*update_dt.seconds(), + left_feedback_mean*update_dt.seconds(), right_feedback_mean*update_dt.seconds(), current_time); } } From 304c09cd9f19015e907fe2243d43994cdbf4bf77 Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Sun, 1 May 2022 14:47:58 +0200 Subject: [PATCH 5/5] Simplify time/period occurrences --- .../src/diff_drive_controller.cpp | 29 +++++++++---------- 1 file changed, 13 insertions(+), 16 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index e1ab4d9e17..825c3be3f5 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -140,7 +140,7 @@ InterfaceConfiguration DiffDriveController::state_interface_configuration() cons } controller_interface::return_type DiffDriveController::update( - const rclcpp::Time & time, const rclcpp::Duration & /*period*/) + const rclcpp::Time & time, const rclcpp::Duration & period) { auto logger = node_->get_logger(); if (get_state().id() == State::PRIMARY_STATE_INACTIVE) @@ -153,8 +153,6 @@ controller_interface::return_type DiffDriveController::update( return controller_interface::return_type::OK; } - const auto current_time = time; - std::shared_ptr last_command_msg; received_velocity_msg_ptr_.get(last_command_msg); @@ -164,7 +162,7 @@ controller_interface::return_type DiffDriveController::update( return controller_interface::return_type::ERROR; } - const auto age_of_last_command = current_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_) { @@ -178,8 +176,7 @@ controller_interface::return_type DiffDriveController::update( double & linear_command = command.twist.linear.x; double & angular_command = command.twist.angular.z; - const auto update_dt = current_time - previous_update_timestamp_; - previous_update_timestamp_ = current_time; + previous_update_timestamp_ = time; // Apply (possibly new) multipliers: const auto wheels = wheel_params_; @@ -189,7 +186,7 @@ controller_interface::return_type DiffDriveController::update( if (odom_params_.open_loop) { - odometry_.updateOpenLoop(linear_command, angular_command, current_time); + odometry_.updateOpenLoop(linear_command, angular_command, time); } else { @@ -217,27 +214,27 @@ controller_interface::return_type DiffDriveController::update( if (odom_params_.position_feedback) { - odometry_.update(left_feedback_mean, right_feedback_mean, current_time); + odometry_.update(left_feedback_mean, right_feedback_mean, time); } else { odometry_.updateFromVelocity( - left_feedback_mean*update_dt.seconds(), right_feedback_mean*update_dt.seconds(), - current_time); + left_feedback_mean*period.seconds(), right_feedback_mean*period.seconds(), + time); } } tf2::Quaternion orientation; orientation.setRPY(0.0, 0.0, odometry_.getHeading()); - if (previous_publish_timestamp_ + publish_period_ < current_time) + if (previous_publish_timestamp_ + publish_period_ < time) { previous_publish_timestamp_ += publish_period_; if (realtime_odometry_publisher_->trylock()) { auto & odometry_message = realtime_odometry_publisher_->msg_; - odometry_message.header.stamp = current_time; + odometry_message.header.stamp = time; odometry_message.pose.pose.position.x = odometry_.getX(); odometry_message.pose.pose.position.y = odometry_.getY(); odometry_message.pose.pose.orientation.x = orientation.x(); @@ -252,7 +249,7 @@ controller_interface::return_type DiffDriveController::update( if (odom_params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) { auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); - transform.header.stamp = current_time; + transform.header.stamp = time; transform.transform.translation.x = odometry_.getX(); transform.transform.translation.y = odometry_.getY(); transform.transform.rotation.x = orientation.x(); @@ -266,9 +263,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( - linear_command, last_command.linear.x, second_to_last_command.linear.x, update_dt.seconds()); + 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, update_dt.seconds()); + angular_command, last_command.angular.z, second_to_last_command.angular.z, period.seconds()); previous_commands_.pop(); previous_commands_.emplace(command); @@ -277,7 +274,7 @@ controller_interface::return_type DiffDriveController::update( if (publish_limited_velocity_ && realtime_limited_velocity_publisher_->trylock()) { auto & limited_velocity_command = realtime_limited_velocity_publisher_->msg_; - limited_velocity_command.header.stamp = current_time; + limited_velocity_command.header.stamp = time; limited_velocity_command.twist = command.twist; realtime_limited_velocity_publisher_->unlockAndPublish(); }