Apply suggestions from code review

ros-controls · Jan 27, 2023 · 8935e78 · 8935e78
1 parent eadabde
commit 8935e78
Show file tree

Hide file tree

Showing 13 changed files with 18 additions and 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
@@ -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<ControllerReferenceMsg> 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
@@ -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
@@ -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
@@ -36,7 +36,6 @@
 
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>controller_manager</test_depend>
-  <test_depend>hardware_interface</test_depend>
   <test_depend>ros2_control_test_assets</test_depend>
 
   <export>

diff --git 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<double>::quiet_NaN();
-    reference_interfaces_[1] = std::numeric_limits<double>::quiet_NaN();
-    reference_interfaces_[2] = std::numeric_limits<double>::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<double>::quiet_NaN();
-    current_ref->twist.linear.y = std::numeric_limits<double>::quiet_NaN();
-    current_ref->twist.angular.z = std::numeric_limits<double>::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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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.