Another review address round

irobot_create_description/urdf/sensors/ir_opcode_receivers.urdf.xacro

@@ -80,7 +80,7 @@
     </xacro:if>
 
     <gazebo>
-      <plugin name="${name}_plugin" filename="libgazebo_ros_create_ir_opcodes.so">
+      <plugin name="${name}_plugin" filename="libgazebo_ros_create_ir_opcode.so">
         <ros>
           <namespace>/</namespace>
           <remapping>~/out:=ir_opcode</remapping>

irobot_create_gazebo_plugins/CMakeLists.txt

@@ -45,7 +45,7 @@ add_library(gazebo_ros_create_optical_mouse SHARED
   src/gazebo_ros_optical_mouse.cpp
 )
 
-add_library(gazebo_ros_create_ir_opcodes SHARED
+add_library(gazebo_ros_create_ir_opcode SHARED
   src/gazebo_ros_ir_opcode.cpp
 )
 
@@ -122,16 +122,16 @@ ament_target_dependencies(gazebo_ros_create_optical_mouse
 ament_export_libraries(gazebo_ros_create_optical_mouse)
 target_link_libraries(gazebo_ros_create_optical_mouse gazebo_ros_create_helpers)
 
-## gazebo_ros_create_ir_opcodes
-target_include_directories(gazebo_ros_create_ir_opcodes PUBLIC include)
-ament_target_dependencies(gazebo_ros_create_ir_opcodes
+## gazebo_ros_create_ir_opcode
+target_include_directories(gazebo_ros_create_ir_opcode PUBLIC include)
+ament_target_dependencies(gazebo_ros_create_ir_opcode
   "gazebo_dev"
   "gazebo_ros"
   "irobot_create_msgs"
   "rclcpp"
 )
-ament_export_libraries(gazebo_ros_create_ir_opcodes)
-target_link_libraries(gazebo_ros_create_ir_opcodes gazebo_ros_create_helpers docking_manager)
+ament_export_libraries(gazebo_ros_create_ir_opcode)
+target_link_libraries(gazebo_ros_create_ir_opcode gazebo_ros_create_helpers docking_manager)
 
 ## docking_manager
 target_include_directories(docking_manager PUBLIC include)
@@ -170,7 +170,7 @@ install(TARGETS
     gazebo_ros_create_imu
     gazebo_ros_create_ir_intensity_sensor
     gazebo_ros_create_optical_mouse
-    gazebo_ros_create_ir_opcodes
+    gazebo_ros_create_ir_opcode
     docking_manager
     gazebo_ros_create_wheel_drop
   ARCHIVE DESTINATION lib

irobot_create_gazebo_plugins/include/irobot_create_gazebo_plugins/docking_manager.hpp

@@ -21,15 +21,15 @@
 #include <gazebo/physics/Link.hh>
 #include <gazebo/physics/Model.hh>
 #include <gazebo/physics/World.hh>
-#include "irobot_create_gazebo_plugins/gazebo_ros_helpers.hpp"
 
 #include <cmath>
 #include <memory>
 #include <string>
 
+#include "irobot_create_gazebo_plugins/gazebo_ros_helpers.hpp"
+
 namespace irobot_create_gazebo_plugins
 {
-
 class DockingManager
 {
 private:
@@ -55,15 +55,17 @@ class DockingManager
 
   /// Use this method to check that the models are ready in gazebo before dereferencing their
   /// pointers
-  bool checkIfModelsReady();
+  bool AreModelsReady();
 
   /// Change reference frame of a cartesian point WRT emitter and return in the new frame as
   /// polar point
-  utils::PolarCoordinate emitterWRTReceiverPolarPoint(const ignition::math::Vector2d & emitter_point);
+  utils::PolarCoordinate emitterWRTReceiverPolarPoint(
+    const ignition::math::Vector2d & emitter_point);
 
   /// Change reference frame of a cartesian point WRT receiver and return in the new frame as
   /// polar point
-  utils::PolarCoordinate receiverWRTEmitterPolarPoint(const ignition::math::Vector2d & receiver_point);
+  utils::PolarCoordinate receiverWRTEmitterPolarPoint(
+    const ignition::math::Vector2d & receiver_point);
 };
 }  // namespace irobot_create_gazebo_plugins
 

irobot_create_gazebo_plugins/include/irobot_create_gazebo_plugins/gazebo_ros_helpers.hpp

@@ -120,8 +120,8 @@ PolarCoordinate toPolar(const ignition::math::Vector2d & cartesian)
 
 ignition::math::Vector2d fromPolar(const PolarCoordinate & polar)
 {
-  ignition::math::Vector2d cartesian{polar.radius * cos(polar.azimuth), polar.radius * sin(
-      polar.azimuth)};
+  ignition::math::Vector2d cartesian{
+    polar.radius * cos(polar.azimuth), polar.radius * sin(polar.azimuth)};
   return cartesian;
 }
 

irobot_create_gazebo_plugins/include/irobot_create_gazebo_plugins/gazebo_ros_ir_opcode.hpp

@@ -31,14 +31,6 @@
 
 namespace irobot_create_gazebo_plugins
 {
-
-struct SensorParams
-{
-  int sensor_id;
-  double fov;
-  double range;
-};
-
 class GazeboRosIrOpcode : public gazebo::ModelPlugin
 {
 public:
@@ -49,20 +41,27 @@ class GazeboRosIrOpcode : public gazebo::ModelPlugin
   virtual ~GazeboRosIrOpcode();
 
   /// Gazebo calls this when the plugin is loaded.
-  /// \param[in] model Pointer to parent model. Other plugin types will expose different entities,
+  /// @param[in] model Pointer to parent model. Other plugin types will expose different entities,
   /// such as `gazebo::sensors::SensorPtr`, `gazebo::physics::WorldPtr`,
   /// `gazebo::rendering::VisualPtr`, etc.
-  /// \param[in] sdf SDF element containing user-defined parameters.
+  /// @param[in] sdf SDF element containing user-defined parameters.
   void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) override;
 
   /// Callback to be called at every simulation iteration.
+  /// @param[in] info Object containing the world name, sim time and the real time of simulation.
   void OnUpdate(const gazebo::common::UpdateInfo & info);
 
 private:
   // Robot receiver parameters
   // Index 0: Sensor 0, the omnidirectional receiver
   // Index 1: Sensor 1, is the forward facing receiver
-  SensorParams sensors_[2];
+  struct SensorParams
+  {
+    int sensor_id;
+    double fov;
+    double range;
+  };
+  std::array<SensorParams, 2> sensors_;
 
   // Dock emitter parameters
   const double DOCK_BUOYS_FOV_ = 50 * M_PI / 180;  // Convert to radians
@@ -92,12 +91,12 @@ class GazeboRosIrOpcode : public gazebo::ModelPlugin
   utils::UpdateRateEnforcer update_rate_enforcer_;
 
   // DockingManager
-  std::shared_ptr<DockingManager> dock_manager_ptr_{nullptr};
+  std::shared_ptr<DockingManager> dock_manager_{nullptr};
 
   // Check dock visibility and return the associated opcode
   int CheckBuoysDetection(const double fov, const double range);
   int CheckForceFieldDetection(const double fov, const double range);
-  void PublishSensors(const int detected_opcodes[2]);
+  void PublishSensors(const std::array<int, 2> detected_opcodes);
 };
 }  // namespace irobot_create_gazebo_plugins
 

irobot_create_gazebo_plugins/src/docking_manager.cpp

@@ -15,7 +15,6 @@
 // @author Rodrigo Jose Causarano Nunez (rcausaran@irobot.com)
 
 #include <irobot_create_gazebo_plugins/docking_manager.hpp>
-
 #include <string>
 
 namespace irobot_create_gazebo_plugins
@@ -32,18 +31,18 @@ void DockingManager::initLinks(
 {
   // Retrieve receiver link and assert it.
   receiver_link_ = robot_model->GetLink("ir_omni");
-  GZ_ASSERT(receiver_link_, "World pointer is invalid!");
+  GZ_ASSERT(receiver_link_, "Receiver link pointer is invalid!");
 
   // Retrieve emitter link and assert it.
   emitter_link_ = dock_model->GetLink("halo_link");
-  GZ_ASSERT(emitter_link_, "World pointer is invalid!");
+  GZ_ASSERT(emitter_link_, "Emitter link pointer is invalid!");
 }
 
-bool DockingManager::checkIfModelsReady()
+bool DockingManager::AreModelsReady()
 {
   const gazebo::physics::ModelPtr dock_model = world_->ModelByName(dock_model_name_);
   const gazebo::physics::ModelPtr robot_model = world_->ModelByName(robot_model_name_);
-  bool models_ready = dock_model != nullptr && robot_model != nullptr;
+  const bool models_ready = dock_model != nullptr && robot_model != nullptr;
   if (models_ready && (receiver_link_ == nullptr || emitter_link_ == nullptr)) {
     initLinks(dock_model, robot_model);
   }

irobot_create_gazebo_plugins/src/gazebo_ros_ir_opcode.cpp

@@ -15,9 +15,8 @@
 // @author Rodrigo Jose Causarano Nunez (rcausaran@irobot.com)
 
 #include <irobot_create_gazebo_plugins/gazebo_ros_ir_opcode.hpp>
-
-#include <string>
 #include <memory>
+#include <string>
 
 namespace irobot_create_gazebo_plugins
 {
@@ -52,10 +51,10 @@ void GazeboRosIrOpcode::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sd
 
   // Fill the SensorParams array for Sensor 0 and Sensor 1
   sensors_[0] = {irobot_create_msgs::msg::IrOpcode::SENSOR_OMNI, sensor_0_fov, sensor_0_range};
-  sensors_[1] = {irobot_create_msgs::msg::IrOpcode::SENSOR_DIRECTIONAL_FRONT, sensor_1_fov,
-    sensor_1_range};
+  sensors_[1] = {
+    irobot_create_msgs::msg::IrOpcode::SENSOR_DIRECTIONAL_FRONT, sensor_1_fov, sensor_1_range};
 
-  dock_manager_ptr_ = std::make_shared<DockingManager>(world_, "create3", "standard_dock");
+  dock_manager_ = std::make_shared<DockingManager>(world_, "create3", "standard_dock");
 
   // Initialize ROS publisher
   pub_ = ros_node_->create_publisher<irobot_create_msgs::msg::IrOpcode>(
@@ -87,25 +86,27 @@ void GazeboRosIrOpcode::OnUpdate(const gazebo::common::UpdateInfo & info)
   const double time_elapsed = (current_time - last_time_).Double();
 
   // Check if on this iteration corresponds to send the message
-  if (!update_rate_enforcer_.shouldUpdate(time_elapsed)) {return;}
+  if (!update_rate_enforcer_.shouldUpdate(time_elapsed)) {
+    return;
+  }
 
   // Update time
   last_time_ = current_time;
 
-  if (!dock_manager_ptr_->checkIfModelsReady()) {
+  if (!dock_manager_->AreModelsReady()) {
     RCLCPP_DEBUG(ros_node_->get_logger(), "standard_dock model is not ready yet");
     return;
   }
 
   // Array to hold detected opcodes from force field
-  const int detected_forcefield_opcodes[] = {CheckForceFieldDetection(
-      sensors_[0].fov,
-      sensors_[0].range), CheckForceFieldDetection(sensors_[1].fov, sensors_[1].range)};
+  const std::array<int, 2> detected_forcefield_opcodes = {
+    CheckForceFieldDetection(sensors_[0].fov, sensors_[0].range),
+    CheckForceFieldDetection(sensors_[1].fov, sensors_[1].range)};
 
   // Array to hold detected opcodes from buoys
-  const int detected_buoys_opcodes[] = {CheckBuoysDetection(
-      sensors_[0].fov,
-      sensors_[0].range), CheckBuoysDetection(sensors_[1].fov, sensors_[1].range)};
+  const std::array<int, 2> detected_buoys_opcodes = {
+    CheckBuoysDetection(sensors_[0].fov, sensors_[0].range),
+    CheckBuoysDetection(sensors_[1].fov, sensors_[1].range)};
 
   // Check and publish Sensors
   PublishSensors(detected_forcefield_opcodes);
@@ -116,11 +117,11 @@ int GazeboRosIrOpcode::CheckBuoysDetection(const double fov, const double range)
 {
   // Get the origin of the receiver in a polar point WRT the emitter
   const utils::PolarCoordinate receiver_wrt_emitter_polar =
-    dock_manager_ptr_->receiverWRTEmitterPolarPoint({0.0, 0.0});
+    dock_manager_->receiverWRTEmitterPolarPoint({0.0, 0.0});
 
   // Get the origin of the emitter in a polar point WRT the receiver
   const utils::PolarCoordinate emitter_wrt_receiver_polar =
-    dock_manager_ptr_->emitterWRTReceiverPolarPoint({0.0, 0.0});
+    dock_manager_->emitterWRTReceiverPolarPoint({0.0, 0.0});
 
   bool receiver_sees_emitter = false;
   bool in_front_of_buoys = false;
@@ -130,14 +131,15 @@ int GazeboRosIrOpcode::CheckBuoysDetection(const double fov, const double range)
   int detected_opcode = 0;
 
   // Check emitter fov
-  if (emitter_wrt_receiver_polar.azimuth > -fov / 2 &&
-    emitter_wrt_receiver_polar.azimuth < fov / 2)
+  if (
+    emitter_wrt_receiver_polar.azimuth > -fov / 2 && emitter_wrt_receiver_polar.azimuth < fov / 2)
   {
     receiver_sees_emitter = true;
   }
 
   // Check that receiver is in front of emitter
-  if (receiver_wrt_emitter_polar.azimuth > -M_PI / 2 &&
+  if (
+    receiver_wrt_emitter_polar.azimuth > -M_PI / 2 &&
     receiver_wrt_emitter_polar.azimuth < M_PI / 2)
   {
     in_front_of_buoys = true;
@@ -151,7 +153,8 @@ int GazeboRosIrOpcode::CheckBuoysDetection(const double fov, const double range)
   // Check if receiver within red fov
   // Red fov angle is DOCK_BUOY_FOV_RATIO_*DOCK_BUOYS_FOV_ but is offset from DOCK_BUOYS_FOV_/2
   // pointing towards the left of the dock
-  if (receiver_wrt_emitter_polar.azimuth < DOCK_BUOYS_FOV_ / 2 &&
+  if (
+    receiver_wrt_emitter_polar.azimuth < DOCK_BUOYS_FOV_ / 2 &&
     receiver_wrt_emitter_polar.azimuth >
     DOCK_BUOYS_FOV_ / 2 - DOCK_BUOY_FOV_RATIO_ * DOCK_BUOYS_FOV_)
   {
@@ -161,7 +164,8 @@ int GazeboRosIrOpcode::CheckBuoysDetection(const double fov, const double range)
   // Check if receiver within green fov
   // Green fov angle is DOCK_BUOY_FOV_RATIO_*DOCK_BUOYS_FOV_ but is offset from -DOCK_BUOYS_FOV_/2
   // pointing towards the right of the dock
-  if (receiver_wrt_emitter_polar.azimuth > -DOCK_BUOYS_FOV_ / 2 &&
+  if (
+    receiver_wrt_emitter_polar.azimuth > -DOCK_BUOYS_FOV_ / 2 &&
     receiver_wrt_emitter_polar.azimuth <
     DOCK_BUOY_FOV_RATIO_ * DOCK_BUOYS_FOV_ - DOCK_BUOYS_FOV_ / 2)
   {
@@ -184,7 +188,7 @@ int GazeboRosIrOpcode::CheckForceFieldDetection(const double fov, const double r
 {
   // Get the origin of the emitter in a polar point WRT the receiver
   const utils::PolarCoordinate emitter_wrt_receiver_polar =
-    dock_manager_ptr_->emitterWRTReceiverPolarPoint({0.0, 0.0});
+    dock_manager_->emitterWRTReceiverPolarPoint({0.0, 0.0});
 
   bool force_field_in_range = false;
   bool receiver_sees_emitter = false;
@@ -209,11 +213,10 @@ int GazeboRosIrOpcode::CheckForceFieldDetection(const double fov, const double r
   return detected_opcode;
 }
 
-
-void GazeboRosIrOpcode::PublishSensors(const int detected_opcodes[2])
+void GazeboRosIrOpcode::PublishSensors(const std::array<int, 2> detected_opcodes)
 {
   // First for sensor 0 then sensor 1
-  for (int k = 0; k < 2; k++) {
+  for (size_t k = 0; k < detected_opcodes.size(); k++) {
     const int detected_opcode = detected_opcodes[k];
     if (detected_opcode > 0) {
       // Fill msg for this iteration

irobot_create_toolbox/include/irobot_create_toolbox/motion_control_node.hpp

@@ -39,15 +39,15 @@ class MotionControlNode : public rclcpp::Node
     const std::vector<rclcpp::Parameter> & parameters);
 
   /// \brief Name of parameter for enabling/disabling all reflexes
-  const std::string reflex_enabled_param_name_ {"reflexes_enabled"};
+  const std::string reflex_enabled_param_name_{"reflexes_enabled"};
   /// \brief Vector of reflex names (these corresponds to exposed parameters)
-  const std::vector<std::string> reflex_names_ {
+  const std::vector<std::string> reflex_names_{
     "REFLEX_BUMP", "REFLEX_CLIFF", "REFLEX_DOCK_AVOID",
     "REFLEX_GYRO_CAL", "REFLEX_PANIC", "REFLEX_PROXIMITY_SLOWDOWN",
     "REFLEX_STUCK", "REFLEX_VIRTUAL_WALL", "REFLEX_WHEEL_DROP"};
   /// \brief Storage for custom parameter validation callbacks
-  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr
-    m_params_callback_handle {nullptr};
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr m_params_callback_handle{
+    nullptr};
 };
 
 }  // namespace irobot_create_toolbox

irobot_create_toolbox/src/motion_control_node.cpp

@@ -14,12 +14,12 @@
 //
 // @author Alberto Soragna (asoragna@irobot.com)
 
+#include "irobot_create_toolbox/motion_control_node.hpp"
+
 #include <memory>
 #include <string>
 #include <vector>
 
-#include "irobot_create_toolbox/motion_control_node.hpp"
-
 namespace irobot_create_toolbox
 {
 using namespace std::placeholders;