-

modules/catkin_ws/src/car/src/ultrasonic/Ultrasonic.cpp

@@ -22,16 +22,14 @@ void Ultrasonic::onInit()
     messageOStream.write("onInit", "START");
 
     ussData = nh.advertise<ussDataMsg>("ussData", 1);
-    //sensor.init();
+    sensor.init();
     timer = networking::time::Timer::create(net);
     timer->startPeriodicTimeout(
         std::chrono::milliseconds(UltrasonicSensor::DELAY),
         [&]
         {
             ussDataMsg msg;
-            //auto distance = streamMedianFilter.moveWindow(sensor.getDistance());
-            static int distance = 0;
-            distance++;
+            auto distance = streamMedianFilter.moveWindow(sensor.getDistance());
             msg.distance = distance;
             msg.timestamp = ros::Time::now();
             ussData.publish(msg);