rwc2022: supporting entity (#69)

Author: MatthijsBurgh

ed_sensor_integration/include/ed/kinect/updater.h

@@ -24,6 +24,9 @@ struct UpdateRequest
     // When refitting an entity, this states the maximum change in yaw (in radians), i.e., the fitted
     // yaw will deviate at most 'max_yaw_change' from the estimated yaw
     double max_yaw_change;
+
+    // Should the supporting entity be fitted
+    bool fit_supporting_entity;
 };
 
 // ----------------------------------------------------------------------------------------------------

ed_sensor_integration/src/kinect/kinect_plugin.cpp

@@ -167,6 +167,7 @@ bool KinectPlugin::srvUpdate(ed_sensor_integration_msgs::Update::Request& req, e
     UpdateRequest kinect_update_req;
     kinect_update_req.area_description = req.area_description;
     kinect_update_req.background_padding = req.background_padding;
+    kinect_update_req.fit_supporting_entity = req.fit_supporting_entity;
 
     // We expect the orientation of the supporting entity to be approximately correct.
     // Therefore, only allow rotation updates up to 45 degrees (both clock-wise and anti-clock-wise)

ed_sensor_integration/src/kinect/ray_tracer.cpp

@@ -67,8 +67,8 @@ RayTraceResult ray_trace(const ed::WorldModel& world, const geo::Pose3D& raytrac
         res.active_entity_ = e->id().str();
         for (const auto& volume : e->volumes())
         {
-            std::string name = volume.first;
-            geo::ShapeConstPtr shape = volume.second;
+            const std::string& name = volume.first;
+            const geo::ShapeConstPtr& shape = volume.second;
             if (name == "on_top_of")
             {
                 ROS_DEBUG("Raytrace on_top_of array of %s included", e->id().c_str());

ed_sensor_integration/src/kinect/updater.cpp

@@ -225,7 +225,7 @@ bool Updater::update(const ed::WorldModel& world, const rgbd::ImageConstPtr& ima
         // Check if the update_command is a segmented entity.
         // If so, lookup the corresponding area_description
 
-        bool fit_supporting_entity = true;
+        bool fit_supporting_entity = req.fit_supporting_entity;
 
         std::map<ed::UUID, std::string>::const_iterator it_area_descr = id_to_area_description_.find(req.area_description);
         if (it_area_descr != id_to_area_description_.end())

ed_sensor_integration_msgs/srv/Update.srv

@@ -12,6 +12,9 @@ string area_description
 # Padding of the world model when subtracting background for segmentation (in meters)
 float32 background_padding
 
+# Should the supporting entity be fitted
+bool fit_supporting_entity
+
 ---
 string[] new_ids      # ids of new entities
 string[] updated_ids  # ids of updated entities