Refactor to remove compiler warnings jazzy

.gitignore

@@ -10,3 +10,6 @@ docs/_templates/*
 */.history
 **/.idea/
 **/cmake-build-debug/
+build
+install
+log

flatland_plugins/src/bumper.cpp

@@ -114,6 +114,9 @@ void Bumper::BeforePhysicsStep(const Timekeeper & timekeeper)
   for (it = contact_states_.begin(); it != contact_states_.end(); it++) {
     it->second.Reset();
   }
+
+  // Avoid -Wunused-parameter warnings - remove if parameter is used!
+  (void)timekeeper;
 }
 
 void Bumper::AfterPhysicsStep(const Timekeeper & timekeeper)

flatland_plugins/src/diff_drive.cpp

@@ -162,7 +162,7 @@ void DiffDrive::OnInitialize(const YAML::Node & config)
     "twist_sub(%s) odom_pub(%s) ground_truth_pub(%s) "
     "odom_pose_noise({%f,%f,%f}) odom_twist_noise({%f,%f,%f}) "
     "pub_rate(%f)\n",
-    body_, body_->name_.c_str(), odom_frame_id.c_str(), twist_topic.c_str(), odom_topic.c_str(),
+    (void*)body_, body_->name_.c_str(), odom_frame_id.c_str(), twist_topic.c_str(), odom_topic.c_str(),
     ground_truth_topic.c_str(), odom_pose_noise[0], odom_pose_noise[1], odom_pose_noise[2],
     odom_twist_noise[0], odom_twist_noise[1], odom_twist_noise[2], pub_rate);
 }

flatland_plugins/src/laser.cpp

@@ -218,6 +218,10 @@ float LaserCallback::ReportFixture(
   did_hit_ = true;
   fraction_ = fraction;
 
+  // Avoid -Wunused-parameter warnings - remove if parameter is used!
+  (void)point;
+  (void)normal;
+
   return fraction;
 }
 
@@ -274,7 +278,7 @@ void Laser::ParseParameters(const YAML::Node & config)
     "frame_id(%s) broadcast_tf(%d) update_rate(%f) range(%f)  "
     "noise_std_dev(%f) angle_min(%f) angle_max(%f) "
     "angle_increment(%f) layers(0x%u {%s})",
-    GetName().c_str(), topic_.c_str(), body_name.c_str(), body_, origin_.x, origin_.y,
+    GetName().c_str(), topic_.c_str(), body_name.c_str(), (void*)body_, origin_.x, origin_.y,
     origin_.theta, frame_id_.c_str(), broadcast_tf_, update_rate_, range_, noise_std_dev_,
     min_angle_, max_angle_, increment_, layers_bits_, boost::algorithm::join(layers, ",").c_str());
 }

flatland_plugins/src/model_tf_publisher.cpp

@@ -102,7 +102,7 @@ void ModelTfPublisher::OnInitialize(const YAML::Node & config)
     rclcpp::get_logger("ModelTFPublisher"),
     "Initialized with params: reference(%s, %p) "
     "publish_tf_world(%d) world_frame_id(%s) update_rate(%f), exclude({%s})",
-    reference_body_->name_.c_str(), reference_body_, publish_tf_world_, world_frame_id_.c_str(),
+    reference_body_->name_.c_str(), (void*)reference_body_, publish_tf_world_, world_frame_id_.c_str(),
     update_rate_, boost::algorithm::join(excluded_body_names, ",").c_str());
 }
 

flatland_plugins/src/tricycle_drive.cpp

@@ -174,8 +174,8 @@ void TricycleDrive::OnInitialize(const YAML::Node & config)
     "odom_frame_id(%s) twist_sub(%s) odom_pub(%s) "
     "ground_truth_pub(%s) odom_pose_noise({%f,%f,%f}) "
     "odom_twist_noise({%f,%f,%f}) pub_rate(%f)\n",
-    body_, body_->GetName().c_str(), front_wj_, front_wj_->GetName().c_str(), rear_left_wj_,
-    rear_left_wj_->GetName().c_str(), rear_right_wj_, rear_right_wj_->GetName().c_str(),
+    (void*)body_, body_->GetName().c_str(), (void*)front_wj_, front_wj_->GetName().c_str(), (void*)rear_left_wj_,
+    rear_left_wj_->GetName().c_str(), (void*)rear_right_wj_, rear_right_wj_->GetName().c_str(),
     odom_frame_id.c_str(), twist_topic.c_str(), odom_topic.c_str(), ground_truth_topic.c_str(),
     odom_pose_noise[0], odom_pose_noise[1], odom_pose_noise[2], odom_twist_noise[0],
     odom_twist_noise[1], odom_twist_noise[2], pub_rate);

flatland_plugins/src/tween.cpp

@@ -242,7 +242,7 @@ void Tween::OnInitialize(const YAML::Node & config)
     "duration %f "
     "mode: %s [%d] "
     "easing: %s\n",
-    body_, body_->name_.c_str(), start_.x, start_.y, start_.theta, delta_.x, delta_.y, delta_.theta,
+    (void*)body_, body_->name_.c_str(), start_.x, start_.y, start_.theta, delta_.x, delta_.y, delta_.theta,
     duration_, mode.c_str(), (int)mode_, easing.c_str());
 }
 

flatland_plugins/src/world_modifier.cpp

@@ -78,6 +78,11 @@ float RayTrace::ReportFixture(
   }
   is_hit_ = true;
   fraction_ = fraction;
+
+  // Avoid -Wunused-parameter warnings - remove if parameter is used!
+  (void)point;
+  (void)normal;
+
   return fraction;
 }
 

flatland_plugins/src/world_random_wall.cpp

@@ -54,6 +54,7 @@
 #include <algorithm>
 #include <iostream>
 #include <pluginlib/class_list_macros.hpp>
+#include <random>
 #include <rclcpp/rclcpp.hpp>
 #include <string>
 
@@ -113,8 +114,9 @@ void RandomWall::OnInitialize(const YAML::Node & config, YamlReader & world_conf
   for (b2Fixture * f = layer->body_->physics_body_->GetFixtureList(); f; f = f->GetNext()) {
     Wall_List.push_back(static_cast<b2EdgeShape *>(f->GetShape()));
   }
-  std::srand(std::time(0));
-  std::random_shuffle(Wall_List.begin(), Wall_List.end());
+  auto rd = std::random_device {};
+  auto rng = std::default_random_engine { rd() };
+  std::shuffle(Wall_List.begin(), Wall_List.end(), rng);
   try {
     for (unsigned int i = 0; i < num_of_walls; i++) {
       modifier.AddFullWall(Wall_List[i]);

flatland_plugins/test/laser_test.cpp

@@ -64,6 +64,7 @@ class LaserPluginTest : public ::testing::Test
   boost::filesystem::path this_file_dir;
   boost::filesystem::path world_yaml;
   sensor_msgs::msg::LaserScan scan_front, scan_center, scan_back;
+  bool scan_front_received, scan_center_received, scan_back_received;
   World * w;
   std::shared_ptr<rclcpp::Node> node;
 
@@ -73,6 +74,9 @@ class LaserPluginTest : public ::testing::Test
   {
     this_file_dir = boost::filesystem::path(__FILE__).parent_path();
     w = nullptr;
+    scan_front_received = false;
+    scan_center_received = false;
+    scan_back_received = false;
   }
 
   void TearDown() override { delete w; }
@@ -117,18 +121,47 @@ class LaserPluginTest : public ::testing::Test
     float angle_max, float angle_increment, float time_increment, float scan_time, float range_min,
     float range_max, std::vector<float> ranges, std::vector<float> intensities)
   {
+    bool return_value = true;
     if (scan.header.frame_id != frame_id) {
       printf("frame_id Actual:%s != Expected:%s\n", scan.header.frame_id.c_str(), frame_id.c_str());
-      return false;
+      return_value = false;
+    }
+
+    if (!FloatEq("angle_min", scan.angle_min, angle_min)) {
+      printf("angle_min Actual: %f != Expected: %f\n", scan.angle_min, angle_min);
+      return_value = false;
+    }
+
+    if (!FloatEq("angle_max", scan.angle_max, angle_max)) {
+      printf("angle_max Actual: %f != Expected: %f\n", scan.angle_max, angle_max);
+      return_value = false;
+    }
+
+    if (!FloatEq("angle_increment", scan.angle_increment, angle_increment)) {
+      printf("angle_increment Actual: %f != Expected: %f\n", scan.angle_increment, angle_increment);
+      return_value = false;
+    }
+
+    if (!FloatEq("time_increment", scan.time_increment, time_increment)) {
+      printf("time_increment Actual: %f != Expected: %f\n", scan.time_increment, time_increment);
+      return_value = false;
+    }
+
+    if (!FloatEq("scan_time", scan.scan_time, scan_time)) {
+      printf("scan_time Actual: %f != Expected: %f\n", scan.scan_time, scan_time);
+      return_value = false;
+    }
+
+    if (!FloatEq("range_min", scan.range_min, range_min)) {
+      printf("range_min Actual: %f != Expected: %f\n", scan.range_min, range_min);
+      return_value = false;
+    }
+
+    if (!FloatEq("range_max", scan.range_max, range_max)) {
+      printf("range_max Actual: %f != Expected: %f\n", scan.range_max, range_max);
+      return_value = false;
     }
 
-    if (!FloatEq("angle_min", scan.angle_min, angle_min)) return false;
-    if (!FloatEq("angle_max", scan.angle_max, angle_max)) return false;
-    if (!FloatEq("angle_increment", scan.angle_increment, angle_increment)) return false;
-    if (!FloatEq("time_increment", scan.time_increment, time_increment)) return false;
-    if (!FloatEq("scan_time", scan.scan_time, scan_time)) return false;
-    if (!FloatEq("range_min", scan.range_min, range_min)) return false;
-    if (!FloatEq("range_max", scan.range_max, range_max)) return false;
 
     if (
       ranges.size() != scan.ranges.size() ||
@@ -140,7 +173,7 @@ class LaserPluginTest : public ::testing::Test
       printf("Expected: ");
       print_flt_vec(ranges);
       printf("\n");
-      return false;
+      return_value = false;
     }
 
     if (
@@ -153,15 +186,27 @@ class LaserPluginTest : public ::testing::Test
       printf("Expected: ");
       print_flt_vec(intensities);
       printf("\n");
-      return false;
+      return_value = false;
     }
 
-    return true;
+    return return_value;
   }
 
-  void ScanFrontCb(const sensor_msgs::msg::LaserScan & msg) { scan_front = msg; };
-  void ScanCenterCb(const sensor_msgs::msg::LaserScan & msg) { scan_center = msg; };
-  void ScanBackCb(const sensor_msgs::msg::LaserScan & msg) { scan_back = msg; };
+  void ScanFrontCb(const sensor_msgs::msg::LaserScan & msg) {
+    scan_front = msg;
+    scan_front_received = true;
+  };
+
+  void ScanCenterCb(const sensor_msgs::msg::LaserScan & msg) {
+    scan_center = msg;
+    scan_center_received = true;
+  };
+
+  void ScanBackCb(const sensor_msgs::msg::LaserScan & msg) {
+    scan_back = msg;
+    scan_back_received = true;
+  };
+
 };
 
 /**
@@ -178,38 +223,43 @@ TEST_F(LaserPluginTest, range_test)
 
   auto * obj = dynamic_cast<LaserPluginTest *>(this);
   auto sub_1 = node->create_subscription<sensor_msgs::msg::LaserScan>(
-    "scan", 1, std::bind(&LaserPluginTest::ScanFrontCb, obj, _1));
+    "robot1/scan", 1, std::bind(&LaserPluginTest::ScanFrontCb, obj, _1));
   auto sub_2 = node->create_subscription<sensor_msgs::msg::LaserScan>(
-    "scan_center", 1, std::bind(&LaserPluginTest::ScanCenterCb, obj, _1));
+    "robot1/scan_center", 1, std::bind(&LaserPluginTest::ScanCenterCb, obj, _1));
   auto sub_3 = node->create_subscription<sensor_msgs::msg::LaserScan>(
-    "scan_back", 1, std::bind(&LaserPluginTest::ScanBackCb, obj, _1));
+    "robot1/scan_back", 1, std::bind(&LaserPluginTest::ScanBackCb, obj, _1));
 
   auto * p1 = dynamic_cast<Laser *>(w->plugin_manager_.model_plugins_[0].get());
   auto * p2 = dynamic_cast<Laser *>(w->plugin_manager_.model_plugins_[1].get());
   auto * p3 = dynamic_cast<Laser *>(w->plugin_manager_.model_plugins_[2].get());
 
   // let it spin for 10 times to make sure the message gets through
   rclcpp::WallRate rate(500);
-  for (unsigned int i = 0; i < 10; i++) {
+  for (unsigned int i = 0; i < 100 && (!scan_front_received ||
+                                       !scan_center_received ||
+                                       !scan_back_received); i++) {
     w->Update(timekeeper);
     rclcpp::spin_some(node);
     rate.sleep();
   }
 
   // check scan returns
+  EXPECT_TRUE(scan_front_received);
   EXPECT_TRUE(ScanEq(
     scan_front, "r_laser_front", -M_PI / 2, M_PI / 2, M_PI / 2, 0.0, 0.0, 0.0, 5.0, {4.5, 4.4, 4.3},
     {}));
   EXPECT_TRUE(fltcmp(p1->update_rate_, std::numeric_limits<float>::infinity()))
     << "Actual: " << p1->update_rate_;
   EXPECT_EQ(p1->body_, w->models_[0]->bodies_[0]);
 
+  EXPECT_TRUE(scan_center_received);
   EXPECT_TRUE(ScanEq(
     scan_center, "r_center_laser", 0, 2 * M_PI, M_PI / 2, 0.0, 0.0, 0.0, 5.0,
     {4.8, 4.7, 4.6, 4.9, 4.8}, {}));
   EXPECT_TRUE(fltcmp(p2->update_rate_, 5000)) << "Actual: " << p2->update_rate_;
   EXPECT_EQ(p2->body_, w->models_[0]->bodies_[0]);
 
+  EXPECT_TRUE(scan_back_received);
   EXPECT_TRUE(ScanEq(
     scan_back, "r_laser_back", 0, 2 * M_PI, M_PI / 2, 0.0, 0.0, 0.0, 4, {NAN, 3.2, 3.5, NAN, NAN},
     {}));
@@ -230,11 +280,11 @@ TEST_F(LaserPluginTest, intensity_test)
 
   auto * obj = dynamic_cast<LaserPluginTest *>(this);
   auto sub_1 = node->create_subscription<sensor_msgs::msg::LaserScan>(
-    "scan", 1, std::bind(&LaserPluginTest::ScanFrontCb, obj, _1));
+    "robot1/scan", 1, std::bind(&LaserPluginTest::ScanFrontCb, obj, _1));
   auto sub_2 = node->create_subscription<sensor_msgs::msg::LaserScan>(
-    "scan_center", 1, std::bind(&LaserPluginTest::ScanCenterCb, obj, _1));
+    "robot1/scan_center", 1, std::bind(&LaserPluginTest::ScanCenterCb, obj, _1));
   auto sub_3 = node->create_subscription<sensor_msgs::msg::LaserScan>(
-    "scan_back", 1, std::bind(&LaserPluginTest::ScanBackCb, obj, _1));
+    "robot1/scan_back", 1, std::bind(&LaserPluginTest::ScanBackCb, obj, _1));
 
   auto * p1 = dynamic_cast<Laser *>(w->plugin_manager_.model_plugins_[0].get());
   auto * p2 = dynamic_cast<Laser *>(w->plugin_manager_.model_plugins_[1].get());

flatland_plugins/test/laser_tests/intensity_test/robot.model.yaml

@@ -20,7 +20,7 @@ plugins:
 
   - type: Laser
     name: laser_center
-    topic: /scan_center
+    topic: scan_center
     frame: center_laser
     body: base_link
     origin: [0, 0, 0]

flatland_plugins/test/laser_tests/range_test/robot.model.yaml

@@ -20,7 +20,7 @@ plugins:
 
   - type: Laser
     name: laser_center
-    topic: /scan_center
+    topic: scan_center
     frame: center_laser
     body: base_link
     origin: [0, 0, 0]

flatland_plugins/test/update_timer_test.cpp

@@ -67,6 +67,9 @@ class TestPlugin : public ModelPlugin
   {
     update_timer_.SetRate(0);
     update_counter_ = 0;
+
+    // Avoid -Wunused-parameter warnings - remove if parameter is used!
+    (void)config;
   }
 
   void BeforePhysicsStep(const Timekeeper & timekeeper) override

flatland_server/include/flatland_server/model.h

@@ -72,8 +72,8 @@ class Model : public Entity
   std::string namespace_;            ///< namespace of the model
   std::vector<ModelBody *> bodies_;  ///< list of bodies in the model
   std::vector<Joint *> joints_;      ///< list of joints in the model
-  YamlReader plugins_reader_;        ///< for storing plugins when paring YAML
   CollisionFilterRegistry * cfr_;    ///< Collision filter registry
+  YamlReader plugins_reader_;        ///< for storing plugins when paring YAML
   std::string viz_name_;             ///< used for visualization
 
   /**

flatland_server/include/flatland_server/world.h

@@ -80,9 +80,9 @@ class World : public b2ContactListener
   std::vector<Layer *> layers_;                  ///< list of layers
   std::vector<Model *> models_;                  ///< list of models
   CollisionFilterRegistry cfr_;                  ///< collision registry for layers and models
-  PluginManager plugin_manager_;                 ///< for loading and updating plugins
   bool service_paused_;                          ///< indicates if simulation is paused by a service
                                                  /// call or not
+  PluginManager plugin_manager_;                 ///< for loading and updating plugins
   InteractiveMarkerManager int_marker_manager_;  ///< for dynamically moving models from Rviz
   int physics_position_iterations_;              ///< Box2D solver param
   int physics_velocity_iterations_;              ///< Box2D solver param

flatland_server/src/body.cpp

@@ -103,8 +103,8 @@ void Body::DebugOutput() const
     "Body %p: entity(%p, %s) name(%s) color(%f,%f,%f,%f) "
     "physics_body(%p) num_fixtures(%d) type(%d) pose(%f, %f, %f) "
     "angular_damping(%f) linear_damping(%f)",
-    this, entity_, entity_->name_.c_str(), name_.c_str(), color_.r, color_.g, color_.b, color_.a,
-    physics_body_, GetFixturesCount(), physics_body_->GetType(), physics_body_->GetPosition().x,
+    (void*)this, (void*)entity_, entity_->name_.c_str(), name_.c_str(), color_.r, color_.g, color_.b, color_.a,
+    (void*)physics_body_, GetFixturesCount(), physics_body_->GetType(), physics_body_->GetPosition().x,
     physics_body_->GetPosition().y, physics_body_->GetAngle(), physics_body_->GetAngularDamping(),
     physics_body_->GetLinearDamping());
 }

flatland_server/src/dummy_world_plugin.cpp

@@ -73,6 +73,10 @@ void DummyWorldPlugin::OnInitialize(const YAML::Node & plugin_reader, YamlReader
       "\"DummyWorldPluginType\", the type is " +
       type_);
   }
+
+  // Avoid -Wunused-parameter warnings - remove if parameter is used!
+  (void)plugin_reader;
+  (void)world_config;
 }
 }  // namespace flatland_plugins
 

flatland_server/src/interactive_marker_manager.cpp

@@ -193,12 +193,18 @@ void InteractiveMarkerManager::processMouseDownFeedback(
   const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr & feedback)
 {
   manipulating_model_ = true;
+
+  // Avoid -Wunused-parameter warnings - remove if parameter is used!
+  (void)feedback;
 }
 
 void InteractiveMarkerManager::processPoseUpdateFeedback(
   const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr & feedback)
 {
   pose_update_stamp_ = rclcpp::Clock(RCL_SYSTEM_TIME).now();
+
+  // Avoid -Wunused-parameter warnings - remove if parameter is used!
+  (void)feedback;
 }
 
 void InteractiveMarkerManager::update()

flatland_server/src/joint.cpp

@@ -202,8 +202,8 @@ void Joint::DebugOutput() const
     "Joint %p: model(%p, %s) name(%s) color(%f,%f,%f,%f) "
     "physics_joint(%p) body_A(%p, %s) anchor_A_world(%f, %f) "
     "body_B(%p, %s) anchor_B_world(%f, %f)",
-    this, model_, model_->name_.c_str(), name_.c_str(), color_.r, color_.g, color_.b, color_.a,
-    physics_joint_, body_A, body_A->name_.c_str(), j->GetAnchorA().x, j->GetAnchorA().y, body_B,
+    (void*)this, (void*)model_, model_->name_.c_str(), name_.c_str(), color_.r, color_.g, color_.b, color_.a,
+    (void*)physics_joint_, (void*)body_A, body_A->name_.c_str(), j->GetAnchorA().x, j->GetAnchorA().y, (void*)body_B,
     body_B->name_.c_str(), j->GetAnchorB().x, j->GetAnchorB().y);
 }