* Plugin load function. Called by Gazebo each time the plugin is instantiated. * * @param parent Gazebo sensor that this plugin connects to. * @param sdf SDF element containing this plugin. */
| 135 | * @param sdf SDF element containing this plugin. |
| 136 | */ |
| 137 | void Load(gazebo::sensors::SensorPtr parent, sdf::ElementPtr sdf) override |
| 138 | { |
| 139 | if (!ros::isInitialized()) |
| 140 | { |
| 141 | ROS_FATAL_NAMED("throttling_camera", "ROS node for Gazebo has not been initialized, unable to load plugin"); |
| 142 | return; |
| 143 | } |
| 144 | ROS_DEBUG_NAMED("throttling_camera", "Initializing ROS throttling (stable-rate) camera"); |
| 145 | |
| 146 | CameraPlugin::Load(parent, sdf); |
| 147 | |
| 148 | world = gazebo::physics::get_world(parent->WorldName()); |
| 149 | #if GAZEBO_MAJOR_VERSION >= 8 |
| 150 | presetManager = world->PresetMgr(); |
| 151 | #else |
| 152 | presetManager = world->GetPresetManager(); |
| 153 | #endif /* GAZEBO_MAJOR_VERSION */ |
| 154 | |
| 155 | // Same as in PX4 |
| 156 | if (presetManager->CurrentProfile() != "default_physics") |
| 157 | { |
| 158 | gzwarn << "Current physics profile is not default_physics, but actually is " << presetManager->CurrentProfile() << "\n"; |
| 159 | if (!presetManager->CurrentProfile("default_physics")) |
| 160 | { |
| 161 | gzerr << "Could not set current profile to default_physics!\n"; |
| 162 | } |
| 163 | } |
| 164 | |
| 165 | double minUpdateRate = parent->UpdateRate(); |
| 166 | if (sdf->HasElement("minUpdateRate")) |
| 167 | { |
| 168 | minUpdateRate = sdf->Get<double>("minUpdateRate"); |
| 169 | } |
| 170 | maxUpdateInterval = 1.0 / minUpdateRate; |
| 171 | |
| 172 | size_t windowSize = 10; |
| 173 | if (sdf->HasElement("windowSize")) |
| 174 | { |
| 175 | windowSize = sdf->Get<size_t>("windowSize"); |
| 176 | } |
| 177 | |
| 178 | double maxStDev = 0.02; |
| 179 | if (sdf->HasElement("maxStDev")) |
| 180 | { |
| 181 | maxStDev = sdf->Get<double>("maxStDev"); |
| 182 | } |
| 183 | |
| 184 | timeSamples.reset(new AverageStat(windowSize, maxStDev)); |
| 185 | |
| 186 | camPtr = parent; |
| 187 | |
| 188 | parentSensor_ = camPtr; |
| 189 | width_ = width; |
| 190 | height_ = height; |
| 191 | depth_ = depth; |
| 192 | format_ = format; |
| 193 | camera_ = camera; |
| 194 |