From 8935e78b0669cf9da4f41d0a3621d3abc88f8f44 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Fri, 27 Jan 2023 23:52:04 +0100 Subject: [PATCH] Apply suggestions from code review --- .../mecanum_drive_controller.hpp | 8 +++---- .../mecanum_drive_controller/odometry.hpp | 2 +- .../visibility_control.h | 2 +- mecanum_drive_controller/package.xml | 1 - .../src/mecanum_drive_controller.cpp | 23 ++++--------------- .../src/mecanum_drive_controller.yaml | 2 +- mecanum_drive_controller/src/odometry.cpp | 5 +--- .../test/mecanum_drive_controller_params.yaml | 5 +--- ...um_drive_controller_preceeding_params.yaml | 5 +--- .../test_load_mecanum_drive_controller.cpp | 2 +- .../test/test_mecanum_drive_controller.cpp | 2 +- .../test/test_mecanum_drive_controller.hpp | 2 +- ...st_mecanum_drive_controller_preceeding.cpp | 2 +- 13 files changed, 18 insertions(+), 43 deletions(-) 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 47dd1afe97..9bd260ee44 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 @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// 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. @@ -127,9 +127,9 @@ class MecanumDriveController : public controller_interface::ChainableControllerI 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] + 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 diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp index 56157f9f69..fc04bef50e 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// 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. diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h b/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h index b9a5062612..3222b2fa52 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h +++ b/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// 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. diff --git a/mecanum_drive_controller/package.xml b/mecanum_drive_controller/package.xml index bde04b5610..c989b9266f 100644 --- a/mecanum_drive_controller/package.xml +++ b/mecanum_drive_controller/package.xml @@ -36,7 +36,6 @@ ament_cmake_gmock controller_manager - hardware_interface ros2_control_test_assets diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 5c7f8f064c..5db3f794c7 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// 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. @@ -374,9 +374,9 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma // INVERSE KINEMATICS (move robot). // Compute wheels velocities (this is the actual ik): // NOTE: the input desired twist (from topic `~/reference`) is a body twist. - auto current_ref = *(input_ref_.readFromRT()); - const auto age_of_last_command = time - current_ref->header.stamp; - if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) + 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); @@ -417,32 +417,17 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma params_.kinematics.wheels_k * velocity_in_center_frame_angular_z); // Set wheels velocities: - command_interfaces_[0].set_value(w_front_left_vel); - fprintf(stderr, " command_interfaces_[0] = %f \n", w_front_left_vel); command_interfaces_[1].set_value(w_back_left_vel); - fprintf(stderr, " command_interfaces_[1] = %f \n", w_back_left_vel); command_interfaces_[2].set_value(w_back_right_vel); - fprintf(stderr, " command_interfaces_[2] = %f \n", w_back_right_vel); command_interfaces_[3].set_value(w_front_right_vel); - fprintf(stderr, " command_interfaces_[3] = %f \n", w_front_right_vel); } else { - reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); - reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); - reference_interfaces_[2] = std::numeric_limits::quiet_NaN(); - command_interfaces_[0].set_value(0.0); - // left intentionally for debug - // fprintf(stderr, " command_interfaces_[0] = %f \n", command_interfaces_[0].get_value()); command_interfaces_[1].set_value(0.0); command_interfaces_[2].set_value(0.0); command_interfaces_[3].set_value(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(); } // Publish odometry message diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.yaml b/mecanum_drive_controller/src/mecanum_drive_controller.yaml index 9dd6f55f2f..ad4e7860e6 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.yaml +++ b/mecanum_drive_controller/src/mecanum_drive_controller.yaml @@ -32,7 +32,7 @@ mecanum_drive_controller: interface_name: { type: string, default_value: "", - description: "Name of the interface used by the controller on joints and command_joints.", + description: "Name of the interface used by the controller for sending commands, reading states and getting references.", read_only: true, } diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp index cfd80668ae..e1b0c4778c 100644 --- a/mecanum_drive_controller/src/odometry.cpp +++ b/mecanum_drive_controller/src/odometry.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// 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. @@ -107,9 +107,6 @@ bool Odometry::update( 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; - // leaving the below comments intentionally - // fprintf(stderr, " position_x_in_base_frame_ = %f \n", position_x_in_base_frame_); - // fprintf(stderr, " position_y_in_base_frame_ = %f \n", position_y_in_base_frame_); return true; } diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml index ae99da685f..b8a1307234 100644 --- a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml +++ b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml @@ -10,10 +10,7 @@ test_mecanum_drive_controller: interface_name: velocity kinematics: - base_frame_offset: - x: 0.0 - y: 0.0 - theta: 0.0 + base_frame_offset: { x: 0.0, y: 0.0, theta: 0.0 } wheels_radius: 0.5 diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml index 7517c29441..ee1c7ffca0 100644 --- a/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml +++ b/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml @@ -10,10 +10,7 @@ test_mecanum_drive_controller: interface_name: velocity kinematics: - base_frame_offset: - x: 0.0 - y: 0.0 - theta: 0.0 + base_frame_offset: { x: 0.0, y: 0.0, theta: 0.0 } wheels_radius: 0.5 diff --git a/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp index ab454ba236..a6de9504a8 100644 --- a/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// 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. diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index a4e45989be..76078fbc17 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// 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. diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp index f0ebea668b..f13a3bd72c 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// 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. diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp index 5fca426813..a16ae4db16 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// 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.