Fix RayTracing (#86)

Author: MatthijsBurgh

ed_sensor_integration/src/kinect/kinect_plugin.cpp

@@ -4,10 +4,11 @@
 
 #include <ros/node_handle.h>
 
-#include <ed/uuid.h>
-#include <ed/world_model.h>
 #include <ed/entity.h>
+#include <ed/error_context.h>
 #include <ed/update_request.h>
+#include <ed/uuid.h>
+#include <ed/world_model.h>
 
 #include <tue/config/reader.h>
 
@@ -31,18 +32,21 @@
 
 KinectPlugin::KinectPlugin()
 {
+    ed::ErrorContext errc("RGBDPlugin", "constructor");
 }
 
 // ----------------------------------------------------------------------------------------------------
 
 KinectPlugin::~KinectPlugin()
 {
+    ed::ErrorContext errc("RGBDPlugin", "destructor");
 }
 
 // ----------------------------------------------------------------------------------------------------
 
 void KinectPlugin::initialize(ed::InitData& init)
 {
+    ed::ErrorContext errc("RGBDPlugin", "initialize");
     tue::Configuration& config = init.config;
 
     std::string topic;
@@ -69,6 +73,7 @@ void KinectPlugin::initialize(ed::InitData& init)
 
 void KinectPlugin::process(const ed::PluginInput& data, ed::UpdateRequest& req)
 {
+    ed::ErrorContext errc("RGBDPlugin", "process");
     // - - - - - - - - - - - - - - - - - -
     // Fetch kinect image and pose
 
@@ -94,6 +99,7 @@ void KinectPlugin::process(const ed::PluginInput& data, ed::UpdateRequest& req)
 
 bool KinectPlugin::srvGetImage(ed_sensor_integration_msgs::GetImage::Request& req, ed_sensor_integration_msgs::GetImage::Response& res)
 {
+    ed::ErrorContext errc("RGBDPlugin", "srvGetImage");
     // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
     // Get new image
 
@@ -147,6 +153,7 @@ bool KinectPlugin::srvGetImage(ed_sensor_integration_msgs::GetImage::Request& re
 
 bool KinectPlugin::srvUpdate(ed_sensor_integration_msgs::Update::Request& req, ed_sensor_integration_msgs::Update::Response& res)
 {
+    ed::ErrorContext errc("RGBDPlugin", "srvUpdate");
     // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
     // Get new image
 
@@ -203,6 +210,7 @@ bool KinectPlugin::srvUpdate(ed_sensor_integration_msgs::Update::Request& req, e
 
 bool KinectPlugin::srvRayTrace(ed_sensor_integration_msgs::RayTrace::Request& req, ed_sensor_integration_msgs::RayTrace::Response& res)
 {
+    ed::ErrorContext errc("RGBDPlugin", "srvRayTrace");
     if (req.raytrace_pose.header.frame_id != "/map" && req.raytrace_pose.header.frame_id != "map")
     {
         ROS_ERROR("KinectPlugin::srvRayTrace only works with poses expressed in map frame");

ed_sensor_integration/src/kinect/ray_tracer.cpp

@@ -1,7 +1,8 @@
 #include "ray_tracer.h"
 
-#include <ed/world_model.h>
 #include <ed/entity.h>
+#include <ed/error_context.h>
+#include <ed/world_model.h>
 
 #include <geolib/Shape.h>
 
@@ -47,6 +48,8 @@ class PointRenderResult : public geo::LaserRangeFinder::RenderResult
 
 RayTraceResult ray_trace(const ed::WorldModel& world, const geo::Pose3D& raytrace_pose)
 {
+    ed::ErrorContext errc("ray_trace");
+    const geo::Pose3D raytrace_pose_inv = raytrace_pose.inverse();
     PointRenderResult res;
 
     geo::LaserRangeFinder lrf;
@@ -61,7 +64,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();
@@ -73,15 +76,16 @@ RayTraceResult ray_trace(const ed::WorldModel& world, const geo::Pose3D& raytrac
             {
                 ROS_DEBUG("Raytrace on_top_of array of %s included", e->id().c_str());
                 geo::LaserRangeFinder::RenderOptions opt;
-                opt.setMesh(shape->getMesh(), raytrace_pose.inverse() * e->pose());
+                opt.setMesh(shape->getMesh(), raytrace_pose_inv * e->pose());
 
                 lrf.render(opt, res);
             }
         }
 
-        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_inv * e->pose()); // Use mesh
 
         lrf.render(opt, res);
     }