throttling_camera.cpp

Go to the documentation of this file.

#include <gazebo/plugins/CameraPlugin.hh>
// physics definitions and functions: Required to perform slowdown
#include <gazebo/physics/physics.hh>
// ROS simulated camera class with most of the boilerplate already in place
#include <gazebo_plugins/gazebo_ros_camera_utils.h>
#include <ros/ros.h>
#include <memory>

#include <deque>
#include <algorithm>

namespace
{
class AverageStat
{
private:
        std::deque<double> samples;
        size_t maxSamples;
        double average = 0;
        double stdev = 0;
        double maxStDev;

public:
        AverageStat(size_t numSamples, double validStDev) :
                samples(),
                maxSamples(numSamples),
                maxStDev(validStDev)
        {}

        double update(double sample)
        {
                samples.push_back(sample);
                if (samples.size() > maxSamples)
                {
                        samples.pop_front();
                }
                average = std::accumulate(samples.begin(), samples.end(), 0.0) / samples.size();
                double stdevSquared = std::accumulate(samples.begin(), samples.end(), 0.0,
                        [&](double sum, double xi) {
                                return sum + (xi - average) * (xi - average);
                        }) / samples.size();
                stdev = std::sqrt(stdevSquared);
                return average;
        }

        double getAverage() const
        {
                return average;
        }

        double getStDev() const
        {
                return stdev;
        }

        bool isAdequate() const
        {
                return (samples.size() >= maxSamples) && (stdev < maxStDev);
        }

        void reset()
        {
                samples.clear();
        }
};

}

namespace throttling_camera
{

class ThrottlingCamera : public gazebo::CameraPlugin, gazebo::GazeboRosCameraUtils
{
private:
        gazebo::sensors::SensorPtr camPtr;
        gazebo::physics::WorldPtr world;
        gazebo::physics::PresetManagerPtr presetManager;

        double maxUpdateInterval;

        std::unique_ptr<AverageStat> timeSamples;
public:
        ThrottlingCamera() = default;
        ~ThrottlingCamera() override = default;

        void Load(gazebo::sensors::SensorPtr parent, sdf::ElementPtr sdf) override
        {
                if (!ros::isInitialized())
                {
                        ROS_FATAL_NAMED("throttling_camera", "ROS node for Gazebo has not been initialized, unable to load plugin");
                        return;
                }
                ROS_DEBUG_NAMED("throttling_camera", "Initializing ROS throttling (stable-rate) camera");

                CameraPlugin::Load(parent, sdf);

                world = gazebo::physics::get_world(parent->WorldName());
#if GAZEBO_MAJOR_VERSION >= 8
                presetManager = world->PresetMgr();
#else
                presetManager = world->GetPresetManager();
#endif /* GAZEBO_MAJOR_VERSION */

                // Same as in PX4
                if (presetManager->CurrentProfile() != "default_physics")
                {
                        gzwarn << "Current physics profile is not default_physics, but actually is " << presetManager->CurrentProfile() << "\n";
                        if (!presetManager->CurrentProfile("default_physics"))
                        {
                                gzerr << "Could not set current profile to default_physics!\n";
                        }
                }

                double minUpdateRate = parent->UpdateRate();
                if (sdf->HasElement("minUpdateRate"))
                {
                        minUpdateRate = sdf->Get<double>("minUpdateRate");
                }
                maxUpdateInterval = 1.0 / minUpdateRate;

                size_t windowSize = 10;
                if (sdf->HasElement("windowSize"))
                {
                        windowSize = sdf->Get<size_t>("windowSize");
                }

                double maxStDev = 0.02;
                if (sdf->HasElement("maxStDev"))
                {
                        maxStDev = sdf->Get<double>("maxStDev");
                }

                timeSamples.reset(new AverageStat(windowSize, maxStDev));

                camPtr = parent;

                parentSensor_ = camPtr;
                width_ = width;
                height_ = height;
                depth_ = depth;
                format_ = format;
                camera_ = camera;

                gazebo::GazeboRosCameraUtils::Load(parent, sdf);
        }

        void OnNewFrame(const unsigned char *image, unsigned int width, unsigned int height, unsigned int depth,
                        const std::string &format) override {

                // Note: sensorUpdateTime uses simulated time
                auto sensorUpdateTime = camPtr->LastMeasurementTime();
                // If sensor was not active for some reason, we allow it to get new data on next frame
                if (!camPtr->IsActive())
                {
                        camPtr->SetActive(true);
                        last_update_time_ = sensorUpdateTime;
                        timeSamples->reset();
                        return;
                }

                boost::mutex::scoped_lock lock(*image_connect_count_lock_);
                if (*image_connect_count_ > 0)
                {
                        if (sensorUpdateTime < last_update_time_)
                        {
                                ROS_WARN_NAMED("throttling_camera", "Negative sensor update time difference (world reset?)");
                                last_update_time_ = sensorUpdateTime;
                                timeSamples->reset();
                        }

                        auto timeDelta = sensorUpdateTime - last_update_time_;
                        timeSamples->update(timeDelta.Double());

                        // We want to throttle the simulation down if we have measurements too far apart
                        if (timeSamples->isAdequate() && timeSamples->getAverage() > maxUpdateInterval)
                        {
                                ROS_INFO_STREAM_NAMED("throttling_camera", "Had average update period of "
                                                                                << timeSamples->getAverage() << " (stdev: " << timeSamples->getStDev() <<  ")"
                                                                                << ", but desired update period is " << update_period_
                                                                                << ", throttling simulation down");
                                boost::any currentRealTimeUpadteRateParam;
                                if (!presetManager->GetCurrentProfileParam("real_time_update_rate", currentRealTimeUpadteRateParam))
                                {
                                        gzerr << "Failed to get real time update rate parameter!\n";
                                }
                                auto currentRate = boost::any_cast<double>(currentRealTimeUpadteRateParam);
                                // We are being somewhat aggressive here, maybe we could throttle the world
                                // down in steps?
                                double slowdownFactor = update_period_ / timeSamples->getAverage();
                                auto nextRate = currentRate * slowdownFactor;
                                if (!presetManager->SetCurrentProfileParam("real_time_update_rate", nextRate))
                                {
                                        gzerr << "Failed to set real time update rate parameter!\n";
                                }
                                if (slowdownFactor < 0.5)
                                {
                                        ROS_WARN_STREAM_NAMED("throttling_camera", "Simulation slowed down significantly; consider running"
                                                              "the simulation with a lower PX4_SIM_SPEED_FACTOR value (slowed down from "
                                                              << currentRate << " to " << nextRate << " updates per second)");
                                }
                                // We're discarding old samples to avoid extensive slowdown
                                timeSamples->reset();
                        }
                        PutCameraData(image, sensorUpdateTime);
                        PublishCameraInfo(sensorUpdateTime);
                }
                last_update_time_ = sensorUpdateTime;
        }
};
}

GZ_REGISTER_SENSOR_PLUGIN(throttling_camera::ThrottlingCamera);