added some checks to determine if entities are NULL before they are a…

…cessed. Didnt solve all the crashes, and it might cause some unexpected behaviour.
tue-robotics · Feb 19, 2015 · c38e42c · c38e42c
1 parent 3e13b9c
commit c38e42c
Show file tree

Hide file tree

Showing 6 changed files with 56 additions and 42 deletions.
diff --git a/plugins/occupancy_grid_publisher_plugin.cpp b/plugins/occupancy_grid_publisher_plugin.cpp
@@ -75,44 +75,48 @@ bool OccupancyGridPublisherPlugin::getMapData(const ed::WorldModel& world, std::
     {
         ed::EntityConstPtr e = *it;
 
-		//! Only entities with measurementSeq() >= 10 or 0 (known)
-		if (e->measurementSeq() > 0 && e->measurementSeq() < 10)
-			continue;
-
-        //! Check the specifier, if set
-        if (specifier_ != "")
-        {
-            double val;
-            if (!tue::config::Reader(e->data()).value(specifier_, val, tue::config::OPTIONAL) || !val)
+        // check if its NULL, theres a bug somewhere
+        if (e){
+            //! Only entities with measurementSeq() >= 10 or 0 (known)
+            if (e->measurementSeq() > 0 && e->measurementSeq() < 10)
                 continue;
-        }
 
-        //! Push back the entity
-        entities_to_be_projected.push_back(e);
+            //! Check the specifier, if set
+            if (specifier_ != "")
+            {
+                double val;
+                if (!tue::config::Reader(e->data()).value(specifier_, val, tue::config::OPTIONAL) || !val)
+                    continue;
+            }
+
+            //! Push back the entity
+            entities_to_be_projected.push_back(e);
 
-        //! Update the map bounds
-        geo::ShapeConstPtr shape = e->shape();
-        if (shape)  // Do shape
-        {
-            const std::vector<geo::Triangle>& triangles = shape->getMesh().getTriangles();
+            //! Update the map bounds
+            geo::ShapeConstPtr shape = e->shape();
+            if (shape)  // Do shape
+            {
+                const std::vector<geo::Triangle>& triangles = shape->getMesh().getTriangles();
 
-            for(std::vector<geo::Triangle>::const_iterator it = triangles.begin(); it != triangles.end(); ++it) {
+                for(std::vector<geo::Triangle>::const_iterator it = triangles.begin(); it != triangles.end(); ++it) {
 
-                geo::Vector3 p1w = e->pose() * it->p1_;
-                geo::Vector3 p2w = e->pose() * it->p2_;
-                geo::Vector3 p3w = e->pose() * it->p3_;
+                    geo::Vector3 p1w = e->pose() * it->p1_;
+                    geo::Vector3 p2w = e->pose() * it->p2_;
+                    geo::Vector3 p3w = e->pose() * it->p3_;
 
-                // Filter the ground
-                if (p1w.getZ() > 0.05001 && p2w.getZ() > 0.050001 && p3w.getZ() > 0.05001)
-                {
-                    min.x = std::min(std::min(std::min(p1w.x, min.x), p2w.x), p3w.x);
-                    max.x = std::max(std::max(std::max(p1w.x, max.x), p2w.x), p3w.x);
+                    // Filter the ground
+                    if (p1w.getZ() > 0.05001 && p2w.getZ() > 0.050001 && p3w.getZ() > 0.05001)
+                    {
+                        min.x = std::min(std::min(std::min(p1w.x, min.x), p2w.x), p3w.x);
+                        max.x = std::max(std::max(std::max(p1w.x, max.x), p2w.x), p3w.x);
 
-                    min.y = std::min(std::min(std::min(p1w.y, min.y), p2w.y), p3w.y);
-                    max.y = std::max(std::max(std::max(p1w.y, max.y), p2w.y), p3w.y);
+                        min.y = std::min(std::min(std::min(p1w.y, min.y), p2w.y), p3w.y);
+                        max.y = std::max(std::max(std::max(p1w.y, max.y), p2w.y), p3w.y);
+                    }
                 }
             }
         }
+
     }
 
     // Bounds fix

diff --git a/plugins/tf_publisher_plugin.cpp b/plugins/tf_publisher_plugin.cpp
@@ -40,22 +40,25 @@ void TFPublisherPlugin::process(const ed::WorldModel& world, ed::UpdateRequest&
     {
         const ed::EntityConstPtr& e = *it;
 
-        geo::Pose3D pose_MAP;
+        // check if its NULL, theres a bug somewhere
+        if (e){
+            geo::Pose3D pose_MAP;
 
-        pose_MAP = e->pose();
+            pose_MAP = e->pose();
 
-		if (e->bestMeasurement())
-		{
-			pose_MAP.t = e->convexHull().center_point;
-		}
+            if (e->bestMeasurement())
+            {
+                pose_MAP.t = e->convexHull().center_point;
+            }
 
-        tf::StampedTransform t;
-        geo::convert(pose_MAP, t);
-        t.frame_id_ = root_frame_id_;
-        t.child_frame_id_ = e->id().str();
-        t.stamp_ = ros::Time::now();
+            tf::StampedTransform t;
+            geo::convert(pose_MAP, t);
+            t.frame_id_ = root_frame_id_;
+            t.child_frame_id_ = e->id().str();
+            t.stamp_ = ros::Time::now();
 
-        tf_broadcaster_->sendTransform(t);
+            tf_broadcaster_->sendTransform(t);
+        }
     }
 }
 

diff --git a/src/helpers/visualization.cpp b/src/helpers/visualization.cpp
@@ -568,7 +568,7 @@ void showMeasurements(const WorldModel& world_model, rgbd::ImageConstPtr rgbd_im
     {
         const EntityConstPtr& e = *it;
 
-        if (!e->shape()) //! if it has no shape
+        if (e && !e->shape()) //! if it has no shape
         {
             if (e->lastMeasurement())
             {

diff --git a/src/perception.cpp b/src/perception.cpp
@@ -125,6 +125,10 @@ void Perception::update(const WorldModelConstPtr& world_model, UpdateRequest& re
     for(WorldModel::const_iterator it = world_model->begin(); it != world_model->end(); ++it)
     {        
         const EntityConstPtr& e = *it;
+
+        // skip if e is null, theres some bug somewhere
+        if (e == NULL) continue;
+
         const UUID& id = e->id();
 
 //        if (e->type() != "")

diff --git a/src/sensor_modules/kinect.cpp b/src/sensor_modules/kinect.cpp
@@ -53,7 +53,7 @@ void filterPointsBehindWorldModel(const WorldModelConstPtr& world_model, const g
     {
         const EntityConstPtr& e = *it;
 
-        if (e->shape())
+        if (e && e->shape())
         {
             geo::Pose3D pose = sensor_pose.inverse() * e->pose();
             geo::RenderOptions opt;

diff --git a/src/server.cpp b/src/server.cpp
@@ -439,6 +439,9 @@ void Server::mergeEntities(const WorldModelPtr& world_model, double not_updated_
     {
         const EntityConstPtr& e = *it;
 
+        // skip if e is null, theres some bug somewhere
+        if (e == NULL) continue;
+
         if (!e->lastMeasurement())
             continue;