add new output port wall to single sonar srevoing

auv-avalon · Sep 2, 2014 · 8ec8d53 · 8ec8d53
1 parent 327c346
commit 8ec8d53
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 0 deletions.
diff --git a/tasks/SingleSonarServoing.cpp b/tasks/SingleSonarServoing.cpp
@@ -559,6 +559,16 @@ void SingleSonarServoing::updateHook()
 
         _wall_servoing_debug.write(debugData);
     }
+
+    //write detected wall data
+    sonar_detectors::Wall wall_out;
+    wall_out.time = base::Time::now();
+    wall_out.wall_distance = distance_to_wall;
+    wall_out.wall_angle = current_wall_angle.getRad();
+    wall_out.last_detection = base::unset<double>();
+
+    _wall.write(wall_out);
+
 }
 
 void SingleSonarServoing::errorHook()

diff --git a/wall_servoing.orogen b/wall_servoing.orogen
@@ -149,6 +149,9 @@ task_context "SingleSonarServoing" do
     output_port('wall_servoing_debug', 'sonar_detectors/WallServoingDebugData').
         doc("estimated wall data, the pointcloud, the wall, the distance, the time")
 
+    output_port("wall", "sonar_detectors::Wall").
+        doc("the detected wall")
+
     runtime_states :WALL_SERVOING, :SEARCHING_WALL, :CHECKING_WALL, :DETECTED_CORNER, :LOST_WALL, :ORIGIN_ALIGNMENT, :ALIGNMENT_COMPLETE
     error_states :MISCONFIGURATION