refactored error messages

RGLServerPlugin/src/Lidar.cc

@@ -22,8 +22,6 @@ using namespace rgl;
 
 void RGLServerPluginInstance::CreateLidar(ignition::gazebo::Entity entity) {
     updates_between_raytraces = std::chrono::duration_cast<std::chrono::milliseconds>(time_between_raytraces).count();
-
-    ignmsg << "attached to: " << entity << std::endl;
     gazebo_lidar = entity;
     static int next_free_id = 0;
     lidar_id = next_free_id;
@@ -78,24 +76,14 @@ void RGLServerPluginInstance::UpdateLidarPose(const ignition::gazebo::EntityComp
     }
     auto rgl_pose_matrix = RGLServerPluginManager::GetRglMatrix(gazebo_lidar, ecm);
     RGL_CHECK(rgl_node_rays_transform(&node_lidar_pose, &rgl_pose_matrix));
-
-    /// Debug printf
-//    ignmsg << "lidar: " << gazebo_lidar << " rgl_pose_matrix: " << std::endl;
-//    for (int i = 0; i < 3; ++i) {
-//        for (int j = 0; j < 4; ++j) {
-//            ignmsg << rgl_pose_matrix.value[i][j] << " ";
-//        }
-//        ignmsg << std::endl;
-//    }
-
 }
 
 void RGLServerPluginInstance::RayTrace(ignition::gazebo::EntityComponentManager& ecm,
-                                       std::chrono::steady_clock::duration sim_time, bool paused) {
+                                       std::chrono::steady_clock::duration sim_time,
+                                       bool paused) {
     if (!lidar_exists) {
         return;
     }
-
     current_update++;
 
     if (!paused && sim_time < last_raytrace_time + time_between_raytraces) {
@@ -116,21 +104,19 @@ void RGLServerPluginInstance::RayTrace(ignition::gazebo::EntityComponentManager&
     int hitpoint_count = 0;
     int size;
     RGL_CHECK(rgl_graph_get_result_size(node_compact, RGL_FIELD_XYZ_F32, &hitpoint_count, &size));
+    if (size != sizeof(rgl_vec3f)) {
+        ignerr << "invalid raytrace size of element: " << size << "\n";
+        return;
+    }
     if (hitpoint_count == 0) {
         return;
     }
     std::vector<rgl_vec3f> results(hitpoint_count, rgl_vec3f());
     RGL_CHECK(rgl_graph_get_result_data(node_compact, RGL_FIELD_XYZ_F32, results.data()));
 
-    ignmsg << "Lidar id: " << lidar_id << " Got " << hitpoint_count << " hitpoint(s)\n";
-//    for (int i = 0; i < hitpoint_count; ++i) {
-//        ignmsg << " hit: " << i << " coordinates: " << results[i].value[0] << "," << results[i].value[1] << ","
-//               << results[i].value[2] << std::endl;
-//    }
-
     // Create message
     ignition::msgs::PointCloudPacked point_cloud_msg;
-    ignition::msgs::InitPointCloudPacked(point_cloud_msg, "some_frame", true,
+    ignition::msgs::InitPointCloudPacked(point_cloud_msg, "RGL", true,
                                          {{"xyz", ignition::msgs::PointCloudPacked::Field::FLOAT32}});
     point_cloud_msg.mutable_data()->resize(hitpoint_count * point_cloud_msg.point_step());
     point_cloud_msg.set_height(1);

RGLServerPlugin/src/Mesh.cc

@@ -191,7 +191,7 @@ const ignition::common::Mesh* RGLServerPluginManager::GetMeshPointer(
             return nullptr;
     }
     if (nullptr == mesh_pointer) {
-        ignerr << "Error in importing mesh - it will not be loaded to rgl\n";
+        ignerr << "Error in importing mesh - it will not be loaded to RGL\n";
     }
     return mesh_pointer;
 }

RGLServerPlugin/src/Scene.cc

@@ -27,7 +27,6 @@ using namespace rgl;
 bool RGLServerPluginManager::CheckNewLidarsCb(
         ignition::gazebo::Entity entity,
         const ignition::gazebo::EntityComponentManager& ecm) {
-
     auto data = ecm.ComponentData<ignition::gazebo::components::SystemPluginInfo>(entity);
     if (data == std::nullopt) {
         return true;
@@ -76,7 +75,6 @@ bool RGLServerPluginManager::LoadEntityToRGLCb(
     rgl_entity_t new_rgl_entity;
     RGL_CHECK(rgl_entity_create(&new_rgl_entity, nullptr, new_mesh));
     entities_in_rgl.insert(std::make_pair(entity, std::make_pair(new_rgl_entity, new_mesh)));
-    ignmsg << "Added entity: " << entity << std::endl;
     return true;
 }
 
@@ -95,7 +93,6 @@ bool RGLServerPluginManager::RemoveEntityFromRGLCb(
     RGL_CHECK(rgl_entity_destroy(entities_in_rgl.at(entity).first));
     RGL_CHECK(rgl_mesh_destroy(entities_in_rgl.at(entity).second));
     entities_in_rgl.erase(entity);
-    ignmsg << "Removed entity: " << entity << std::endl;
     return true;
 }
 #pragma clang diagnostic pop
@@ -104,15 +101,5 @@ void RGLServerPluginManager::UpdateRGLEntityPose(const ignition::gazebo::EntityC
     for (auto entity: entities_in_rgl) {
         auto rgl_matrix = GetRglMatrix(entity.first, ecm);
         RGL_CHECK(rgl_entity_set_pose(entity.second.first, &rgl_matrix));
-
-        /// Debug printf
-//        ignmsg << "entity: " << entity.first << " rgl_matrix: " << std::endl;
-//        for (int i = 0; i < 3; ++i) {
-//            for (int j = 0; j < 4; ++j) {
-//                ignmsg << rgl_matrix.value[i][j] << " ";
-//            }
-//            ignmsg << std::endl;
-//        }
-
     }
 }

RGLServerPlugin/src/Utils.cc

@@ -26,7 +26,7 @@ ignition::math::Pose3<double> RGLServerPluginManager::FindWorldPose(
 
     auto local_pose = ecm.Component<ignition::gazebo::components::Pose>(entity);
     if (nullptr == local_pose) {
-        std::cout << "pose data missing - using default pose (0, 0, 0, 0, 0, 0)\n";
+        ignmsg << "pose data missing - using default pose (0, 0, 0, 0, 0, 0)\n";
         return ignition::math::Pose3d::Zero;
     }
     auto world_pose = local_pose->Data();
@@ -37,7 +37,7 @@ ignition::math::Pose3<double> RGLServerPluginManager::FindWorldPose(
     while ((parent = ecm.ParentEntity(this_entity)) != WORLD_ENTITY_ID) {
         auto parent_pose = ecm.Component<ignition::gazebo::components::Pose>(parent);
         if (nullptr == parent_pose) {
-            std::cout << "pose data missing - using default pose (0, 0, 0, 0, 0, 0)\n";
+            ignmsg << "pose data missing - using default pose (0, 0, 0, 0, 0, 0)\n";
             return ignition::math::Pose3d::Zero;
         }
         world_pose += parent_pose->Data();
@@ -53,11 +53,7 @@ rgl_mat3x4f RGLServerPluginManager::GetRglMatrix(
 
     auto gazebo_matrix = ignition::math::Matrix4<double>(FindWorldPose(entity, ecm));
 
-    /// Debug printf
-//    ignmsg << "entity: " << entity << " gazebo_matrix: " << gazebo_matrix << std::endl;
-
     rgl_mat3x4f rgl_matrix;
-
     for (int i = 0; i < 3; ++i) {
         for (int j = 0; j < 4; ++j) {
             rgl_matrix.value[i][j] = static_cast<float>(gazebo_matrix(i, j));