Fix(RGBD) ray tracing collision/visual issues

MatthijsBurgh · MatthijsBurgh · commit 92b0a3f1a159 · 2023-05-07T13:10:29.000+02:00
diff --git a/ed_sensor_integration/src/kinect/ray_tracer.cpp b/ed_sensor_integration/src/kinect/ray_tracer.cpp
@@ -63,7 +63,7 @@ RayTraceResult ray_trace(const ed::WorldModel& world, const geo::Pose3D& raytrac
     {
         const ed::EntityConstPtr& e = *it;
 
-        if (!e->visual() || !e->has_pose())
+        if ((!e->collision() && !e->visual()) || !e->has_pose())
             continue;
 
         res.active_entity_ = e->id().str();
@@ -81,9 +81,10 @@ RayTraceResult ray_trace(const ed::WorldModel& world, const geo::Pose3D& raytrac
             }
         }
 
-        ROS_DEBUG_STREAM("Raytracing to " << e->id() << " mesh");
+        ROS_DEBUG_STREAM("Raytracing to " << e->id() << " " << (e->collision() ? "collision" : "visual") << " mesh");
         geo::LaserRangeFinder::RenderOptions opt;
-        opt.setMesh(e->collision()->getMesh(), raytrace_pose.inverse() * e->pose()); // Use mesh
+        geo::ShapeConstPtr shape = e->collision() ? e->collision() : e->visual();
+        opt.setMesh(shape->getMesh(), raytrace_pose.inverse() * e->pose()); // Use mesh
 
         lrf.render(opt, res);
     }

Original file line number	Diff line number	Diff line change
`@@ -63,7 +63,7 @@ RayTraceResult ray_trace(const ed::WorldModel& world, const geo::Pose3D& raytrac`
`63`	`63`	`{`
`64`	`64`	`const ed::EntityConstPtr& e = *it;`
`65`	`65`
`66`		`- if (!e->visual() \|\| !e->has_pose())`
	`66`	`+ if ((!e->collision() && !e->visual()) \|\| !e->has_pose())`
`67`	`67`	`continue;`
`68`	`68`
`69`	`69`	`res.active_entity_ = e->id().str();`
`@@ -81,9 +81,10 @@ RayTraceResult ray_trace(const ed::WorldModel& world, const geo::Pose3D& raytrac`
`81`	`81`	`}`
`82`	`82`	`}`
`83`	`83`
`84`		`- ROS_DEBUG_STREAM("Raytracing to " << e->id() << " mesh");`
	`84`	`+ ROS_DEBUG_STREAM("Raytracing to " << e->id() << " " << (e->collision() ? "collision" : "visual") << " mesh");`
`85`	`85`	`geo::LaserRangeFinder::RenderOptions opt;`
`86`		`- opt.setMesh(e->collision()->getMesh(), raytrace_pose.inverse() * e->pose()); // Use mesh`
	`86`	`+ geo::ShapeConstPtr shape = e->collision() ? e->collision() : e->visual();`
	`87`	`+ opt.setMesh(shape->getMesh(), raytrace_pose.inverse() * e->pose()); // Use mesh`
`87`	`88`
`88`	`89`	`lrf.render(opt, res);`
`89`	`90`	`}`