Merge pull request #215 from evankapi/region_marker

RegionFilter with markers

(all test are working and region_markers are working too)

Author: sunava

robosherlock/descriptors/annotators/filter/RegionFilter.yaml

@@ -19,4 +19,6 @@ parameters:
   global_threshold: 0.12
   pixel_threshold: 0.15
   semantic_map_definition: "semantic_map_iai_kitchen.yaml"
-
+  # Markers:
+  publish_marker: true
+  marker_topic: "robosherlock/region_markers"

robosherlock/src/filter/src/RegionFilter.cpp

@@ -44,6 +44,8 @@
 
 #include <yaml-cpp/yaml.h>
 
+#include <visualization_msgs/Marker.h>
+
 using namespace uima;
 
 /**
@@ -56,15 +58,15 @@ class RegionFilter : public DrawingAnnotator
   /**
    * @brief The SemanticMapItem struct
    * transform describing transformation from reference frame to center of region you are definging
-   * Dimensions are defined on each axis. If defining a regins that should have a supporting plane you
-   * can optionally add the equation of the plane to the map entry
+   * Dimensions are defined on each axis. If defining a regins that should have a supporting plane
+   * you can optionally add the equation of the plane to the map entry
    */
   struct SemanticMapItem
   {
     tf::Transform transform;
     std::string reference_frame;
     std::string name, type;
-    double y_dimention, z_dimension, x_dimention;
+    double y_dimension, z_dimension, x_dimension;
     cv::Vec4f plane_eq;
     bool has_plane_equations = false;
   };
@@ -99,9 +101,56 @@ class RegionFilter : public DrawingAnnotator
   // for frustum culling
   sensor_msgs::CameraInfo camera_info_;
   pcl::visualization::Camera camera;
-
   rs::TFListenerProxy listener_;
   double frustum[24];
+  bool enabled = true;
+
+  // Markers for regions:
+  std::string marker_topic_;
+  bool publish_marker_;
+  ros::Publisher marker_publisher_;
+
+  void publishSemanticMapMarker(const SemanticMapItem& item)
+  {
+    visualization_msgs::Marker m, m_text;
+    m.header.frame_id = m_text.header.frame_id = item.reference_frame;
+    m.header.stamp = m_text.header.stamp = ros::Time::now();
+    m_text.text = item.name;
+
+    m.ns = m_text.ns = item.name;
+    m.id = 0;
+    m_text.id = 1;
+    m.lifetime = m_text.lifetime = ros::Duration();
+
+    m.type = visualization_msgs::Marker::CUBE;
+    m_text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
+    m.color.r = 0.0f;
+    m.color.g = 0.0f;
+    m.color.b = 1.0f;
+    m.color.a = 0.75f;
+    m_text.color.a = 1;
+
+    auto transform = item.transform;
+    auto origin = transform.getOrigin();
+    auto rotation = transform.getRotation();
+
+    m.pose.position.x = m_text.pose.position.x = origin.x();
+    m.pose.position.y = m_text.pose.position.y = origin.y();
+    m.pose.position.z = m_text.pose.position.z = origin.z();
+    m.pose.orientation.x = rotation.getX();
+    m.pose.orientation.y = rotation.getY();
+    m.pose.orientation.z = rotation.getZ();
+    m.pose.orientation.w = rotation.getW();
+    m_text.pose.orientation.w = 1;
+
+    m.scale.x = item.x_dimension;
+    m.scale.y = item.y_dimension;
+    m.scale.z = item.z_dimension;
+    m_text.scale.z = 0.1f;
+
+    marker_publisher_.publish(m);
+    marker_publisher_.publish(m_text);
+  }
 
 public:
   RegionFilter()
@@ -119,7 +168,22 @@ class RegionFilter : public DrawingAnnotator
     , filtered(0)
     , last_time_(ros::Time::now())
     , timeout(120)
+    , marker_topic_("perception_marker/regions")
+    , publish_marker_(false)
+  {
+  }
+
+  TyErrorId reconfigure()
   {
+    outInfo("Reconfigure..");
+    AnnotatorContext& ctx = getAnnotatorContext();
+
+    if (ctx.isParameterDefined("enabled"))
+    {
+      ctx.extractValue("enabled", enabled);
+    }
+
+    return UIMA_ERR_NONE;
   }
 
   TyErrorId initialize(AnnotatorContext& ctx)
@@ -162,6 +226,27 @@ class RegionFilter : public DrawingAnnotator
       ctx.extractValue("semantic_map_definition", file);
       readSemanticMap(file);
     }
+    if (ctx.isParameterDefined("enabled"))
+    {
+      ctx.extractValue("enabled", enabled);
+    }
+
+    // Marker for regions:
+    if (ctx.isParameterDefined("publish_marker"))
+    {
+      ctx.extractValue("publish_marker", publish_marker_);
+    }
+    if (ctx.isParameterDefined("marker_topic"))
+    {
+      ctx.extractValue("marker_topic", marker_topic_);
+    }
+
+    if (publish_marker_)
+    {
+      ros::NodeHandle n;
+      marker_publisher_ = n.advertise<visualization_msgs::Marker>(marker_topic_, 100);
+    }
+
     return UIMA_ERR_NONE;
   }
 
@@ -200,8 +285,8 @@ class RegionFilter : public DrawingAnnotator
 
         item.name = sem_map_entries[i];
         item.type = entry["type"].as<std::string>();
-        item.x_dimention = entry["depth"].as<double>();   // X-dim
-        item.y_dimention = entry["width"].as<double>();   // Y-dim
+        item.x_dimension = entry["depth"].as<double>();   // X-dim
+        item.y_dimension = entry["width"].as<double>();   // Y-dim
         item.z_dimension = entry["height"].as<double>();  // Z-dim
 
         if (entry["reference_frame"])
@@ -242,6 +327,12 @@ class RegionFilter : public DrawingAnnotator
 private:
   TyErrorId processWithLock(CAS& tcas, ResultSpecification const& res_spec)
   {
+    if (!enabled)
+    {
+      outInfo("process aborted -> Filter is disabled");
+      return UIMA_ERR_NONE;
+    }
+
     MEASURE_TIME;
     outInfo("process begins");
     rs::SceneCas cas(tcas);
@@ -255,11 +346,16 @@ class RegionFilter : public DrawingAnnotator
       rs::SemanticMapObject obj = rs::create<rs::SemanticMapObject>(tcas);
       obj.name(item.name);
       obj.typeName(item.type);
-      obj.width(static_cast<double>(item.y_dimention));
+      obj.width(static_cast<double>(item.y_dimension));
       obj.height(static_cast<double>(item.z_dimension));
-      obj.depth(static_cast<double>(item.x_dimention));
+      obj.depth(static_cast<double>(item.x_dimension));
       obj.transform(rs::conversion::to(tcas, item.transform));
       semantic_map.push_back(obj);
+
+      if (publish_marker_)
+      {
+        publishSemanticMapMarker(item);
+      }
     }
     cas.set(VIEW_SEMANTIC_MAP, semantic_map);
 
@@ -294,7 +390,7 @@ class RegionFilter : public DrawingAnnotator
     {
       rapidjson::Document json_doc;
       std::string json_string = qs.query();
-      json_doc.Parse(json_string);
+      json_doc.Parse(json_string.c_str());
       outWarn("query in CAS : " << json_string);
 
       rapidjson::Pointer frame_pointer_in("/detect/location");
@@ -376,7 +472,7 @@ class RegionFilter : public DrawingAnnotator
             plane_model_as_std_vect[3] = new_plane_eq[3];
             supp_plane.model.set(plane_model_as_std_vect);
             supp_plane.source("RegionFilter");
-            outDebug("Setting plane eq to CAS: "<<new_plane_eq);
+            outDebug("Setting plane eq to CAS: " << new_plane_eq);
             scene.annotations.append(supp_plane);
           }
         }
@@ -480,8 +576,8 @@ class RegionFilter : public DrawingAnnotator
     }
 
     // check region bounding box
-    Eigen::Vector3d bbMin(-(region.y_dimention / 2), -(region.z_dimension / 2), -(region.x_dimention / 2));
-    Eigen::Vector3d bbMax((region.y_dimention / 2), (region.z_dimension / 2), 0.5);
+    Eigen::Vector3d bbMin(-(region.y_dimension / 2), -(region.z_dimension / 2), -(region.x_dimension / 2));
+    Eigen::Vector3d bbMax((region.y_dimension / 2), (region.z_dimension / 2), 0.5);
     pcl::visualization::FrustumCull res =
         (pcl::visualization::FrustumCull)pcl::visualization::cullFrustum(tFrustum, bbMin, bbMax);
     return res != pcl::visualization::PCL_OUTSIDE_FRUSTUM;
@@ -557,10 +653,10 @@ class RegionFilter : public DrawingAnnotator
 
   void filterRegion(const SemanticMapItem& region)
   {
-    const float minX = -(region.x_dimention / 2) + border_;
-    const float maxX = (region.x_dimention / 2) - border_;
-    float minY = -(region.y_dimention / 2) + border_;
-    const float maxY = (region.y_dimention / 2) - border_;
+    const float minX = -(region.x_dimension / 2) + border_;
+    const float maxX = (region.x_dimension / 2) - border_;
+    float minY = -(region.y_dimension / 2) + border_;
+    const float maxY = (region.y_dimension / 2) - border_;
     const float minZ = -(region.z_dimension / 2);
     float maxZ = +(region.z_dimension / 2);
 
@@ -692,7 +788,7 @@ class RegionFilter : public DrawingAnnotator
       tf::vectorTFToEigen(transform.getOrigin(), translation);
       tf::quaternionTFToEigen(transform.getRotation(), rotation);
 
-      visualizer.addCube(translation.cast<float>(), rotation.cast<float>(), region.x_dimention, region.y_dimention,
+      visualizer.addCube(translation.cast<float>(), rotation.cast<float>(), region.x_dimension, region.y_dimension,
                          region.z_dimension, oss.str());
       visualizer.setRepresentationToWireframeForAllActors();
     }