зеркало из https://github.com/microsoft/AirSim.git
do the clang-formatting.
This commit is contained in:
Chris Lovett
Родитель
7fe46feca1
Коммит
0edf734254
|
|
@ -10,72 +10,74 @@
|
|||
#include <map>
|
||||
#include "common/common_utils/UniqueValueMap.hpp"
|
||||
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
class ApiProvider {
|
||||
public:
|
||||
ApiProvider(WorldSimApiBase * world_sim_api)
|
||||
: world_sim_api_(world_sim_api)
|
||||
class ApiProvider
|
||||
{
|
||||
}
|
||||
virtual ~ApiProvider() = default;
|
||||
public:
|
||||
ApiProvider(WorldSimApiBase* world_sim_api)
|
||||
: world_sim_api_(world_sim_api)
|
||||
{
|
||||
}
|
||||
virtual ~ApiProvider() = default;
|
||||
|
||||
//vehicle API
|
||||
virtual VehicleApiBase* getVehicleApi(const std::string& vehicle_name)
|
||||
{
|
||||
return vehicle_apis_.findOrDefault(vehicle_name, nullptr);
|
||||
}
|
||||
|
||||
//vehicle API
|
||||
virtual VehicleApiBase* getVehicleApi(const std::string& vehicle_name)
|
||||
{
|
||||
return vehicle_apis_.findOrDefault(vehicle_name, nullptr);
|
||||
}
|
||||
//world simulation API
|
||||
virtual WorldSimApiBase* getWorldSimApi()
|
||||
{
|
||||
return world_sim_api_;
|
||||
}
|
||||
|
||||
//world simulation API
|
||||
virtual WorldSimApiBase* getWorldSimApi()
|
||||
{
|
||||
return world_sim_api_;
|
||||
}
|
||||
//vehicle simulation API
|
||||
virtual VehicleSimApiBase* getVehicleSimApi(const std::string& vehicle_name) const
|
||||
{
|
||||
return vehicle_sim_apis_.findOrDefault(vehicle_name, nullptr);
|
||||
}
|
||||
|
||||
//vehicle simulation API
|
||||
virtual VehicleSimApiBase* getVehicleSimApi(const std::string& vehicle_name) const
|
||||
{
|
||||
return vehicle_sim_apis_.findOrDefault(vehicle_name, nullptr);
|
||||
}
|
||||
size_t getVehicleCount() const
|
||||
{
|
||||
return vehicle_apis_.valsSize();
|
||||
}
|
||||
void insert_or_assign(const std::string& vehicle_name, VehicleApiBase* vehicle_api,
|
||||
VehicleSimApiBase* vehicle_sim_api)
|
||||
{
|
||||
vehicle_apis_.insert_or_assign(vehicle_name, vehicle_api);
|
||||
vehicle_sim_apis_.insert_or_assign(vehicle_name, vehicle_sim_api);
|
||||
}
|
||||
const common_utils::UniqueValueMap<std::string, VehicleApiBase*>& getVehicleApis()
|
||||
{
|
||||
return vehicle_apis_;
|
||||
}
|
||||
const common_utils::UniqueValueMap<std::string, VehicleSimApiBase*>& getVehicleSimApis()
|
||||
{
|
||||
return vehicle_sim_apis_;
|
||||
}
|
||||
bool hasDefaultVehicle() const
|
||||
{
|
||||
return !(vehicle_apis_.findOrDefault("", nullptr) == nullptr &&
|
||||
vehicle_sim_apis_.findOrDefault("", nullptr) == nullptr);
|
||||
}
|
||||
|
||||
size_t getVehicleCount() const
|
||||
{
|
||||
return vehicle_apis_.valsSize();
|
||||
}
|
||||
void insert_or_assign(const std::string& vehicle_name, VehicleApiBase* vehicle_api,
|
||||
VehicleSimApiBase* vehicle_sim_api)
|
||||
{
|
||||
vehicle_apis_.insert_or_assign(vehicle_name, vehicle_api);
|
||||
vehicle_sim_apis_.insert_or_assign(vehicle_name, vehicle_sim_api);
|
||||
}
|
||||
const common_utils::UniqueValueMap<std::string, VehicleApiBase*>& getVehicleApis()
|
||||
{
|
||||
return vehicle_apis_;
|
||||
}
|
||||
const common_utils::UniqueValueMap<std::string, VehicleSimApiBase*>& getVehicleSimApis()
|
||||
{
|
||||
return vehicle_sim_apis_;
|
||||
}
|
||||
bool hasDefaultVehicle() const
|
||||
{
|
||||
return !(vehicle_apis_.findOrDefault("", nullptr) == nullptr &&
|
||||
vehicle_sim_apis_.findOrDefault("", nullptr) == nullptr);
|
||||
}
|
||||
void makeDefaultVehicle(const std::string& vehicle_name)
|
||||
{
|
||||
vehicle_apis_.insert_or_assign("", vehicle_apis_.at(vehicle_name));
|
||||
vehicle_sim_apis_.insert_or_assign("", vehicle_sim_apis_.at(vehicle_name));
|
||||
}
|
||||
|
||||
void makeDefaultVehicle(const std::string& vehicle_name)
|
||||
{
|
||||
vehicle_apis_.insert_or_assign("", vehicle_apis_.at(vehicle_name));
|
||||
vehicle_sim_apis_.insert_or_assign("", vehicle_sim_apis_.at(vehicle_name));
|
||||
}
|
||||
private:
|
||||
WorldSimApiBase* world_sim_api_;
|
||||
|
||||
private:
|
||||
WorldSimApiBase* world_sim_api_;
|
||||
|
||||
common_utils::UniqueValueMap<std::string, VehicleApiBase*> vehicle_apis_;
|
||||
common_utils::UniqueValueMap<std::string, VehicleSimApiBase*> vehicle_sim_apis_;
|
||||
};
|
||||
|
||||
}} //namespace
|
||||
common_utils::UniqueValueMap<std::string, VehicleApiBase*> vehicle_apis_;
|
||||
common_utils::UniqueValueMap<std::string, VehicleSimApiBase*> vehicle_sim_apis_;
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
@ -7,15 +7,19 @@
|
|||
#include <functional>
|
||||
#include "common/Common.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
class ApiServerBase {
|
||||
public:
|
||||
virtual void start(bool block, std::size_t thread_count) = 0;
|
||||
virtual void stop() = 0;
|
||||
class ApiServerBase
|
||||
{
|
||||
public:
|
||||
virtual void start(bool block, std::size_t thread_count) = 0;
|
||||
virtual void stop() = 0;
|
||||
|
||||
virtual ~ApiServerBase() = default;
|
||||
};
|
||||
|
||||
}} //namespace
|
||||
virtual ~ApiServerBase() = default;
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
Разница между файлами не показана из-за своего большого размера
Загрузить разницу
|
|
@ -16,120 +16,130 @@
|
|||
#include "physics/Environment.hpp"
|
||||
#include "api/WorldSimApiBase.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
//common methods for RCP clients of different vehicles
|
||||
class RpcLibClientBase {
|
||||
public:
|
||||
enum class ConnectionState : uint {
|
||||
Initial = 0, Connected, Disconnected, Reset, Unknown
|
||||
//common methods for RCP clients of different vehicles
|
||||
class RpcLibClientBase
|
||||
{
|
||||
public:
|
||||
enum class ConnectionState : uint
|
||||
{
|
||||
Initial = 0,
|
||||
Connected,
|
||||
Disconnected,
|
||||
Reset,
|
||||
Unknown
|
||||
};
|
||||
|
||||
public:
|
||||
RpcLibClientBase(const string& ip_address = "localhost", uint16_t port = RpcLibPort, float timeout_sec = 60);
|
||||
virtual ~RpcLibClientBase(); //required for pimpl
|
||||
|
||||
void confirmConnection();
|
||||
void reset();
|
||||
|
||||
ConnectionState getConnectionState();
|
||||
bool ping();
|
||||
int getClientVersion() const;
|
||||
int getServerVersion() const;
|
||||
int getMinRequiredServerVersion() const;
|
||||
int getMinRequiredClientVersion() const;
|
||||
|
||||
bool simIsPaused() const;
|
||||
void simPause(bool is_paused);
|
||||
void simContinueForTime(double seconds);
|
||||
void simContinueForFrames(uint32_t frames);
|
||||
|
||||
void simSetTimeOfDay(bool is_enabled, const string& start_datetime = "", bool is_start_datetime_dst = false,
|
||||
float celestial_clock_speed = 1, float update_interval_secs = 60, bool move_sun = true);
|
||||
|
||||
void simEnableWeather(bool enable);
|
||||
void simSetWeatherParameter(WorldSimApiBase::WeatherParameter param, float val);
|
||||
|
||||
vector<string> simListSceneObjects(const string& name_regex = string(".*")) const;
|
||||
Pose simGetObjectPose(const std::string& object_name) const;
|
||||
bool simLoadLevel(const string& level_name);
|
||||
Vector3r simGetObjectScale(const std::string& object_name) const;
|
||||
bool simSetObjectPose(const std::string& object_name, const Pose& pose, bool teleport = true);
|
||||
bool simSetObjectScale(const std::string& object_name, const Vector3r& scale);
|
||||
|
||||
//task management APIs
|
||||
void cancelLastTask(const std::string& vehicle_name = "");
|
||||
virtual RpcLibClientBase* waitOnLastTask(bool* task_result = nullptr, float timeout_sec = Utils::nan<float>());
|
||||
|
||||
bool simSetSegmentationObjectID(const std::string& mesh_name, int object_id, bool is_name_regex = false);
|
||||
int simGetSegmentationObjectID(const std::string& mesh_name) const;
|
||||
void simPrintLogMessage(const std::string& message, std::string message_param = "", unsigned char severity = 0);
|
||||
|
||||
void simFlushPersistentMarkers();
|
||||
void simPlotPoints(const vector<Vector3r>& points, const vector<float>& color_rgba, float size, float duration, bool is_persistent);
|
||||
void simPlotLineStrip(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent);
|
||||
void simPlotLineList(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent);
|
||||
void simPlotArrows(const vector<Vector3r>& points_start, const vector<Vector3r>& points_end, const vector<float>& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent);
|
||||
void simPlotStrings(const vector<std::string>& strings, const vector<Vector3r>& positions, float scale, const vector<float>& color_rgba, float duration);
|
||||
void simPlotTransforms(const vector<Pose>& poses, float scale, float thickness, float duration, bool is_persistent);
|
||||
void simPlotTransformsWithNames(const vector<Pose>& poses, const vector<std::string>& names, float tf_scale, float tf_thickness, float text_scale, const vector<float>& text_color_rgba, float duration);
|
||||
|
||||
bool armDisarm(bool arm, const std::string& vehicle_name = "");
|
||||
bool isApiControlEnabled(const std::string& vehicle_name = "") const;
|
||||
void enableApiControl(bool is_enabled, const std::string& vehicle_name = "");
|
||||
|
||||
msr::airlib::GeoPoint getHomeGeoPoint(const std::string& vehicle_name = "") const;
|
||||
|
||||
bool simRunConsoleCommand(const std::string& command);
|
||||
|
||||
// sensor APIs
|
||||
msr::airlib::LidarData getLidarData(const std::string& lidar_name = "", const std::string& vehicle_name = "") const;
|
||||
msr::airlib::ImuBase::Output getImuData(const std::string& imu_name = "", const std::string& vehicle_name = "") const;
|
||||
msr::airlib::BarometerBase::Output getBarometerData(const std::string& barometer_name = "", const std::string& vehicle_name = "") const;
|
||||
msr::airlib::MagnetometerBase::Output getMagnetometerData(const std::string& magnetometer_name = "", const std::string& vehicle_name = "") const;
|
||||
msr::airlib::GpsBase::Output getGpsData(const std::string& gps_name = "", const std::string& vehicle_name = "") const;
|
||||
msr::airlib::DistanceSensorData getDistanceSensorData(const std::string& distance_sensor_name = "", const std::string& vehicle_name = "") const;
|
||||
|
||||
Pose simGetVehiclePose(const std::string& vehicle_name = "") const;
|
||||
void simSetVehiclePose(const Pose& pose, bool ignore_collision, const std::string& vehicle_name = "");
|
||||
void simSetTraceLine(const std::vector<float>& color_rgba, float thickness = 3.0f, const std::string& vehicle_name = "");
|
||||
|
||||
vector<ImageCaptureBase::ImageResponse> simGetImages(vector<ImageCaptureBase::ImageRequest> request, const std::string& vehicle_name = "");
|
||||
vector<uint8_t> simGetImage(const std::string& camera_name, ImageCaptureBase::ImageType type, const std::string& vehicle_name = "");
|
||||
|
||||
vector<MeshPositionVertexBuffersResponse> simGetMeshPositionVertexBuffers();
|
||||
bool simAddVehicle(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path = "");
|
||||
|
||||
CollisionInfo simGetCollisionInfo(const std::string& vehicle_name = "") const;
|
||||
|
||||
CameraInfo simGetCameraInfo(const std::string& camera_name, const std::string& vehicle_name = "") const;
|
||||
void simSetDistortionParam(const std::string& camera_name, const std::string& param_name, float value, const std::string& vehicle_name = "");
|
||||
std::vector<float> simGetDistortionParams(const std::string& camera_name, const std::string& vehicle_name = "");
|
||||
void simSetCameraPose(const std::string& camera_name, const Pose& pose, const std::string& vehicle_name = "");
|
||||
void simSetCameraFov(const std::string& camera_name, float fov_degrees, const std::string& vehicle_name = "");
|
||||
// This is a backwards-compatibility wrapper over simSetCameraPose, and can be removed in future major releases
|
||||
void simSetCameraOrientation(const std::string& camera_name, const Quaternionr& orientation, const std::string& vehicle_name = "");
|
||||
bool simCreateVoxelGrid(const Vector3r& position, const int& x_size, const int& y_size, const int& z_size, const float& res, const std::string& output_file);
|
||||
msr::airlib::Kinematics::State simGetGroundTruthKinematics(const std::string& vehicle_name = "") const;
|
||||
msr::airlib::Environment::State simGetGroundTruthEnvironment(const std::string& vehicle_name = "") const;
|
||||
std::vector<std::string> simSwapTextures(const std::string& tags, int tex_id = 0, int component_id = 0, int material_id = 0);
|
||||
|
||||
// Recording APIs
|
||||
void startRecording();
|
||||
void stopRecording();
|
||||
bool isRecording();
|
||||
|
||||
void simSetWind(const Vector3r& wind) const;
|
||||
|
||||
std::string getSettingsString() const;
|
||||
|
||||
protected:
|
||||
void* getClient();
|
||||
const void* getClient() const;
|
||||
|
||||
private:
|
||||
struct impl;
|
||||
std::unique_ptr<impl> pimpl_;
|
||||
};
|
||||
public:
|
||||
RpcLibClientBase(const string& ip_address = "localhost", uint16_t port = RpcLibPort, float timeout_sec = 60);
|
||||
virtual ~RpcLibClientBase(); //required for pimpl
|
||||
|
||||
void confirmConnection();
|
||||
void reset();
|
||||
|
||||
ConnectionState getConnectionState();
|
||||
bool ping();
|
||||
int getClientVersion() const;
|
||||
int getServerVersion() const;
|
||||
int getMinRequiredServerVersion() const;
|
||||
int getMinRequiredClientVersion() const;
|
||||
|
||||
bool simIsPaused() const;
|
||||
void simPause(bool is_paused);
|
||||
void simContinueForTime(double seconds);
|
||||
void simContinueForFrames(uint32_t frames);
|
||||
|
||||
void simSetTimeOfDay(bool is_enabled, const string& start_datetime = "", bool is_start_datetime_dst = false,
|
||||
float celestial_clock_speed = 1, float update_interval_secs = 60, bool move_sun = true);
|
||||
|
||||
void simEnableWeather(bool enable);
|
||||
void simSetWeatherParameter(WorldSimApiBase::WeatherParameter param, float val);
|
||||
|
||||
vector<string> simListSceneObjects(const string& name_regex = string(".*")) const;
|
||||
Pose simGetObjectPose(const std::string& object_name) const;
|
||||
bool simLoadLevel(const string& level_name);
|
||||
Vector3r simGetObjectScale(const std::string& object_name) const;
|
||||
bool simSetObjectPose(const std::string& object_name, const Pose& pose, bool teleport = true);
|
||||
bool simSetObjectScale(const std::string& object_name, const Vector3r& scale);
|
||||
|
||||
//task management APIs
|
||||
void cancelLastTask(const std::string& vehicle_name = "");
|
||||
virtual RpcLibClientBase* waitOnLastTask(bool* task_result = nullptr, float timeout_sec = Utils::nan<float>());
|
||||
|
||||
bool simSetSegmentationObjectID(const std::string& mesh_name, int object_id, bool is_name_regex = false);
|
||||
int simGetSegmentationObjectID(const std::string& mesh_name) const;
|
||||
void simPrintLogMessage(const std::string& message, std::string message_param = "", unsigned char severity = 0);
|
||||
|
||||
void simFlushPersistentMarkers();
|
||||
void simPlotPoints(const vector<Vector3r>& points, const vector<float>& color_rgba, float size, float duration, bool is_persistent);
|
||||
void simPlotLineStrip(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent);
|
||||
void simPlotLineList(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent);
|
||||
void simPlotArrows(const vector<Vector3r>& points_start, const vector<Vector3r>& points_end, const vector<float>& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent);
|
||||
void simPlotStrings(const vector<std::string>& strings, const vector<Vector3r>& positions, float scale, const vector<float>& color_rgba, float duration);
|
||||
void simPlotTransforms(const vector<Pose>& poses, float scale, float thickness, float duration, bool is_persistent);
|
||||
void simPlotTransformsWithNames(const vector<Pose>& poses, const vector<std::string>& names, float tf_scale, float tf_thickness, float text_scale, const vector<float>& text_color_rgba, float duration);
|
||||
|
||||
bool armDisarm(bool arm, const std::string& vehicle_name = "");
|
||||
bool isApiControlEnabled(const std::string& vehicle_name = "") const;
|
||||
void enableApiControl(bool is_enabled, const std::string& vehicle_name = "");
|
||||
|
||||
msr::airlib::GeoPoint getHomeGeoPoint(const std::string& vehicle_name = "") const;
|
||||
|
||||
bool simRunConsoleCommand(const std::string& command);
|
||||
|
||||
// sensor APIs
|
||||
msr::airlib::LidarData getLidarData(const std::string& lidar_name = "", const std::string& vehicle_name = "") const;
|
||||
msr::airlib::ImuBase::Output getImuData(const std::string& imu_name = "", const std::string& vehicle_name = "") const;
|
||||
msr::airlib::BarometerBase::Output getBarometerData(const std::string& barometer_name = "", const std::string& vehicle_name = "") const;
|
||||
msr::airlib::MagnetometerBase::Output getMagnetometerData(const std::string& magnetometer_name = "", const std::string& vehicle_name = "") const;
|
||||
msr::airlib::GpsBase::Output getGpsData(const std::string& gps_name = "", const std::string& vehicle_name = "") const;
|
||||
msr::airlib::DistanceSensorData getDistanceSensorData(const std::string& distance_sensor_name = "", const std::string& vehicle_name = "") const;
|
||||
|
||||
Pose simGetVehiclePose(const std::string& vehicle_name = "") const;
|
||||
void simSetVehiclePose(const Pose& pose, bool ignore_collision, const std::string& vehicle_name = "");
|
||||
void simSetTraceLine(const std::vector<float>& color_rgba, float thickness=3.0f, const std::string& vehicle_name = "");
|
||||
|
||||
vector<ImageCaptureBase::ImageResponse> simGetImages(vector<ImageCaptureBase::ImageRequest> request, const std::string& vehicle_name = "");
|
||||
vector<uint8_t> simGetImage(const std::string& camera_name, ImageCaptureBase::ImageType type, const std::string& vehicle_name = "");
|
||||
|
||||
vector<MeshPositionVertexBuffersResponse> simGetMeshPositionVertexBuffers();
|
||||
bool simAddVehicle(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path = "");
|
||||
|
||||
CollisionInfo simGetCollisionInfo(const std::string& vehicle_name = "") const;
|
||||
|
||||
CameraInfo simGetCameraInfo(const std::string& camera_name, const std::string& vehicle_name = "") const;
|
||||
void simSetDistortionParam(const std::string& camera_name, const std::string& param_name, float value, const std::string& vehicle_name = "");
|
||||
std::vector<float> simGetDistortionParams(const std::string& camera_name, const std::string& vehicle_name = "");
|
||||
void simSetCameraPose(const std::string& camera_name, const Pose& pose, const std::string& vehicle_name = "");
|
||||
void simSetCameraFov(const std::string& camera_name, float fov_degrees, const std::string& vehicle_name = "");
|
||||
// This is a backwards-compatibility wrapper over simSetCameraPose, and can be removed in future major releases
|
||||
void simSetCameraOrientation(const std::string& camera_name, const Quaternionr& orientation, const std::string& vehicle_name = "");
|
||||
bool simCreateVoxelGrid(const Vector3r& position, const int& x_size, const int& y_size, const int& z_size, const float& res, const std::string& output_file);
|
||||
msr::airlib::Kinematics::State simGetGroundTruthKinematics(const std::string& vehicle_name = "") const;
|
||||
msr::airlib::Environment::State simGetGroundTruthEnvironment(const std::string& vehicle_name = "") const;
|
||||
std::vector<std::string> simSwapTextures(const std::string& tags, int tex_id = 0, int component_id = 0, int material_id = 0);
|
||||
|
||||
// Recording APIs
|
||||
void startRecording();
|
||||
void stopRecording();
|
||||
bool isRecording();
|
||||
|
||||
void simSetWind(const Vector3r& wind) const;
|
||||
|
||||
std::string getSettingsString() const;
|
||||
|
||||
protected:
|
||||
void* getClient();
|
||||
const void* getClient() const;
|
||||
|
||||
private:
|
||||
struct impl;
|
||||
std::unique_ptr<impl> pimpl_;
|
||||
};
|
||||
|
||||
}} //namespace
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -8,65 +8,66 @@
|
|||
#include "api/ApiServerBase.hpp"
|
||||
#include "api/ApiProvider.hpp"
|
||||
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
|
||||
class RpcLibServerBase : public ApiServerBase {
|
||||
public:
|
||||
RpcLibServerBase(ApiProvider* api_provider, const std::string& server_address, uint16_t port = RpcLibPort);
|
||||
virtual ~RpcLibServerBase() override;
|
||||
|
||||
virtual void start(bool block, std::size_t thread_count) override;
|
||||
virtual void stop() override;
|
||||
|
||||
class ApiNotSupported : public std::runtime_error {
|
||||
class RpcLibServerBase : public ApiServerBase
|
||||
{
|
||||
public:
|
||||
ApiNotSupported(const std::string& message)
|
||||
: std::runtime_error(message) {
|
||||
RpcLibServerBase(ApiProvider* api_provider, const std::string& server_address, uint16_t port = RpcLibPort);
|
||||
virtual ~RpcLibServerBase() override;
|
||||
|
||||
virtual void start(bool block, std::size_t thread_count) override;
|
||||
virtual void stop() override;
|
||||
|
||||
class ApiNotSupported : public std::runtime_error
|
||||
{
|
||||
public:
|
||||
ApiNotSupported(const std::string& message)
|
||||
: std::runtime_error(message)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
protected:
|
||||
void* getServer() const;
|
||||
|
||||
virtual VehicleApiBase* getVehicleApi(const std::string& vehicle_name)
|
||||
{
|
||||
auto* api = api_provider_->getVehicleApi(vehicle_name);
|
||||
if (api)
|
||||
return api;
|
||||
else
|
||||
throw ApiNotSupported("Vehicle API for '" + vehicle_name +
|
||||
"' is not available. This could either because this is simulation-only API or this vehicle does not exist");
|
||||
}
|
||||
virtual VehicleSimApiBase* getVehicleSimApi(const std::string& vehicle_name)
|
||||
{
|
||||
auto* api = api_provider_->getVehicleSimApi(vehicle_name);
|
||||
if (api)
|
||||
return api;
|
||||
else
|
||||
throw ApiNotSupported("Vehicle Sim-API for '" + vehicle_name +
|
||||
"' is not available. This could either because this is not a simulation or this vehicle does not exist");
|
||||
}
|
||||
virtual WorldSimApiBase* getWorldSimApi()
|
||||
{
|
||||
auto* api = api_provider_->getWorldSimApi();
|
||||
if (api)
|
||||
return api;
|
||||
else
|
||||
throw ApiNotSupported("World-Sim API "
|
||||
"' is not available. This could be because this is not a simulation");
|
||||
}
|
||||
|
||||
private:
|
||||
ApiProvider* api_provider_;
|
||||
|
||||
struct impl;
|
||||
std::unique_ptr<impl> pimpl_;
|
||||
};
|
||||
|
||||
protected:
|
||||
void* getServer() const;
|
||||
|
||||
|
||||
virtual VehicleApiBase* getVehicleApi(const std::string& vehicle_name)
|
||||
{
|
||||
auto* api = api_provider_->getVehicleApi(vehicle_name);
|
||||
if (api)
|
||||
return api;
|
||||
else
|
||||
throw ApiNotSupported("Vehicle API for '" + vehicle_name +
|
||||
"' is not available. This could either because this is simulation-only API or this vehicle does not exist");
|
||||
}
|
||||
virtual VehicleSimApiBase* getVehicleSimApi(const std::string& vehicle_name)
|
||||
{
|
||||
auto* api = api_provider_->getVehicleSimApi(vehicle_name);
|
||||
if (api)
|
||||
return api;
|
||||
else
|
||||
throw ApiNotSupported("Vehicle Sim-API for '" + vehicle_name +
|
||||
"' is not available. This could either because this is not a simulation or this vehicle does not exist");
|
||||
}
|
||||
virtual WorldSimApiBase* getWorldSimApi()
|
||||
{
|
||||
auto* api = api_provider_->getWorldSimApi();
|
||||
if (api)
|
||||
return api;
|
||||
else
|
||||
throw ApiNotSupported("World-Sim API "
|
||||
"' is not available. This could be because this is not a simulation");
|
||||
}
|
||||
|
||||
|
||||
private:
|
||||
ApiProvider* api_provider_;
|
||||
|
||||
struct impl;
|
||||
std::unique_ptr<impl> pimpl_;
|
||||
};
|
||||
|
||||
|
||||
}} //namespace
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
@ -21,200 +21,207 @@
|
|||
#include <exception>
|
||||
#include <string>
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
/*
|
||||
/*
|
||||
Vehicle controller allows to obtain state from vehicle and send control commands to the vehicle.
|
||||
State can include many things including sensor data, logs, estimated state from onboard computer etc.
|
||||
Control commands can be low level actuation commands or high level movement commands.
|
||||
The base class defines usually available methods that all vehicle controllers may implement.
|
||||
Some methods may not be applicable to specific vehicle in which case an exception may be raised or call may be ignored.
|
||||
*/
|
||||
class VehicleApiBase : public UpdatableObject {
|
||||
public:
|
||||
virtual void enableApiControl(bool is_enabled) = 0;
|
||||
virtual bool isApiControlEnabled() const = 0;
|
||||
virtual bool armDisarm(bool arm) = 0;
|
||||
virtual GeoPoint getHomeGeoPoint() const = 0;
|
||||
|
||||
virtual void update() override
|
||||
class VehicleApiBase : public UpdatableObject
|
||||
{
|
||||
UpdatableObject::update();
|
||||
}
|
||||
public:
|
||||
virtual void enableApiControl(bool is_enabled) = 0;
|
||||
virtual bool isApiControlEnabled() const = 0;
|
||||
virtual bool armDisarm(bool arm) = 0;
|
||||
virtual GeoPoint getHomeGeoPoint() const = 0;
|
||||
|
||||
virtual void cancelLastTask()
|
||||
{
|
||||
//if derived class supports async task then override this method
|
||||
}
|
||||
virtual void update() override
|
||||
{
|
||||
UpdatableObject::update();
|
||||
}
|
||||
|
||||
virtual bool isReady(std::string& message) const
|
||||
{
|
||||
unused(message);
|
||||
return true;
|
||||
}
|
||||
virtual void cancelLastTask()
|
||||
{
|
||||
//if derived class supports async task then override this method
|
||||
}
|
||||
|
||||
virtual bool canArm() const
|
||||
{
|
||||
return true;
|
||||
}
|
||||
virtual bool isReady(std::string& message) const
|
||||
{
|
||||
unused(message);
|
||||
return true;
|
||||
}
|
||||
|
||||
//if vehicle supports it, call this method to send
|
||||
//kinematics and other info to somewhere (ex. log viewer, file, cloud etc)
|
||||
virtual void sendTelemetry(float last_interval = -1)
|
||||
{
|
||||
//no default action
|
||||
unused(last_interval);
|
||||
}
|
||||
virtual bool canArm() const
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
//below APIs are used by FastPhysicsEngine
|
||||
virtual real_T getActuation(unsigned int actuator_index) const
|
||||
{
|
||||
unused(actuator_index);
|
||||
throw VehicleCommandNotImplementedException("getActuation API is not supported for this vehicle");
|
||||
}
|
||||
virtual size_t getActuatorCount() const
|
||||
{
|
||||
throw VehicleCommandNotImplementedException("getActuatorCount API is not supported for this vehicle");
|
||||
}
|
||||
//if vehicle supports it, call this method to send
|
||||
//kinematics and other info to somewhere (ex. log viewer, file, cloud etc)
|
||||
virtual void sendTelemetry(float last_interval = -1)
|
||||
{
|
||||
//no default action
|
||||
unused(last_interval);
|
||||
}
|
||||
|
||||
virtual void getStatusMessages(std::vector<std::string>& messages)
|
||||
{
|
||||
unused(messages);
|
||||
//default implementation
|
||||
}
|
||||
//below APIs are used by FastPhysicsEngine
|
||||
virtual real_T getActuation(unsigned int actuator_index) const
|
||||
{
|
||||
unused(actuator_index);
|
||||
throw VehicleCommandNotImplementedException("getActuation API is not supported for this vehicle");
|
||||
}
|
||||
virtual size_t getActuatorCount() const
|
||||
{
|
||||
throw VehicleCommandNotImplementedException("getActuatorCount API is not supported for this vehicle");
|
||||
}
|
||||
|
||||
/*
|
||||
virtual void getStatusMessages(std::vector<std::string>& messages)
|
||||
{
|
||||
unused(messages);
|
||||
//default implementation
|
||||
}
|
||||
|
||||
/*
|
||||
For RCs, there are two cases: (1) vehicle may be configured to use
|
||||
RC bound to its hardware (2) vehicle may be configured to get RC data
|
||||
supplied via API calls. Below two APIs are not symmetrical, i.e.,
|
||||
getRCData() may or may not return same thing as setRCData().
|
||||
*/
|
||||
//get reading from RC bound to vehicle (if unsupported then RCData::is_valid = false)
|
||||
virtual RCData getRCData() const
|
||||
{
|
||||
static const RCData invalid_rc_data {};
|
||||
return invalid_rc_data;
|
||||
}
|
||||
|
||||
//set external RC data to vehicle (if unsupported then returns false)
|
||||
virtual bool setRCData(const RCData& rc_data)
|
||||
{
|
||||
unused(rc_data);
|
||||
return false;
|
||||
}
|
||||
|
||||
// Sensors APIs
|
||||
virtual const SensorCollection& getSensors() const
|
||||
{
|
||||
throw VehicleCommandNotImplementedException("getSensors API is not supported for this vehicle");
|
||||
}
|
||||
|
||||
// Lidar APIs
|
||||
virtual LidarData getLidarData(const std::string& lidar_name) const
|
||||
{
|
||||
auto *lidar = static_cast<const LidarBase*>(findSensorByName(lidar_name, SensorBase::SensorType::Lidar));
|
||||
if (lidar == nullptr)
|
||||
throw VehicleControllerException(Utils::stringf("No lidar with name %s exist on vehicle", lidar_name.c_str()));
|
||||
|
||||
return lidar->getOutput();
|
||||
}
|
||||
|
||||
// IMU API
|
||||
virtual ImuBase::Output getImuData(const std::string& imu_name) const
|
||||
{
|
||||
auto *imu = static_cast<const ImuBase*>(findSensorByName(imu_name, SensorBase::SensorType::Imu));
|
||||
if (imu == nullptr)
|
||||
throw VehicleControllerException(Utils::stringf("No IMU with name %s exist on vehicle", imu_name.c_str()));
|
||||
|
||||
return imu->getOutput();
|
||||
}
|
||||
|
||||
// Barometer API
|
||||
virtual BarometerBase::Output getBarometerData(const std::string& barometer_name) const
|
||||
{
|
||||
auto *barometer = static_cast<const BarometerBase*>(findSensorByName(barometer_name, SensorBase::SensorType::Barometer));
|
||||
if (barometer == nullptr)
|
||||
throw VehicleControllerException(Utils::stringf("No barometer with name %s exist on vehicle", barometer_name.c_str()));
|
||||
|
||||
return barometer->getOutput();
|
||||
}
|
||||
|
||||
// Magnetometer API
|
||||
virtual MagnetometerBase::Output getMagnetometerData(const std::string& magnetometer_name) const
|
||||
{
|
||||
auto *magnetometer = static_cast<const MagnetometerBase*>(findSensorByName(magnetometer_name, SensorBase::SensorType::Magnetometer));
|
||||
if (magnetometer == nullptr)
|
||||
throw VehicleControllerException(Utils::stringf("No magnetometer with name %s exist on vehicle", magnetometer_name.c_str()));
|
||||
|
||||
return magnetometer->getOutput();
|
||||
}
|
||||
|
||||
// Gps API
|
||||
virtual GpsBase::Output getGpsData(const std::string& gps_name) const
|
||||
{
|
||||
auto *gps = static_cast<const GpsBase*>(findSensorByName(gps_name, SensorBase::SensorType::Gps));
|
||||
if (gps == nullptr)
|
||||
throw VehicleControllerException(Utils::stringf("No gps with name %s exist on vehicle", gps_name.c_str()));
|
||||
|
||||
return gps->getOutput();
|
||||
}
|
||||
|
||||
// Distance Sensor API
|
||||
virtual DistanceSensorData getDistanceSensorData(const std::string& distance_sensor_name) const
|
||||
{
|
||||
auto *distance_sensor = static_cast<const DistanceBase*>(findSensorByName(distance_sensor_name, SensorBase::SensorType::Distance));
|
||||
if (distance_sensor == nullptr)
|
||||
throw VehicleControllerException(Utils::stringf("No distance sensor with name %s exist on vehicle", distance_sensor_name.c_str()));
|
||||
|
||||
return distance_sensor->getOutput();
|
||||
}
|
||||
|
||||
virtual ~VehicleApiBase() = default;
|
||||
|
||||
//exceptions
|
||||
class VehicleControllerException : public std::runtime_error {
|
||||
public:
|
||||
VehicleControllerException(const std::string& message)
|
||||
: runtime_error(message) {
|
||||
//get reading from RC bound to vehicle (if unsupported then RCData::is_valid = false)
|
||||
virtual RCData getRCData() const
|
||||
{
|
||||
static const RCData invalid_rc_data{};
|
||||
return invalid_rc_data;
|
||||
}
|
||||
};
|
||||
|
||||
class VehicleCommandNotImplementedException : public VehicleControllerException {
|
||||
public:
|
||||
VehicleCommandNotImplementedException(const std::string& message)
|
||||
: VehicleControllerException(message) {
|
||||
//set external RC data to vehicle (if unsupported then returns false)
|
||||
virtual bool setRCData(const RCData& rc_data)
|
||||
{
|
||||
unused(rc_data);
|
||||
return false;
|
||||
}
|
||||
};
|
||||
|
||||
class VehicleMoveException : public VehicleControllerException {
|
||||
public:
|
||||
VehicleMoveException(const std::string& message)
|
||||
: VehicleControllerException(message) {
|
||||
// Sensors APIs
|
||||
virtual const SensorCollection& getSensors() const
|
||||
{
|
||||
throw VehicleCommandNotImplementedException("getSensors API is not supported for this vehicle");
|
||||
}
|
||||
};
|
||||
|
||||
// Lidar APIs
|
||||
virtual LidarData getLidarData(const std::string& lidar_name) const
|
||||
{
|
||||
auto* lidar = static_cast<const LidarBase*>(findSensorByName(lidar_name, SensorBase::SensorType::Lidar));
|
||||
if (lidar == nullptr)
|
||||
throw VehicleControllerException(Utils::stringf("No lidar with name %s exist on vehicle", lidar_name.c_str()));
|
||||
|
||||
return lidar->getOutput();
|
||||
}
|
||||
|
||||
// IMU API
|
||||
virtual ImuBase::Output getImuData(const std::string& imu_name) const
|
||||
{
|
||||
auto* imu = static_cast<const ImuBase*>(findSensorByName(imu_name, SensorBase::SensorType::Imu));
|
||||
if (imu == nullptr)
|
||||
throw VehicleControllerException(Utils::stringf("No IMU with name %s exist on vehicle", imu_name.c_str()));
|
||||
|
||||
return imu->getOutput();
|
||||
}
|
||||
|
||||
// Barometer API
|
||||
virtual BarometerBase::Output getBarometerData(const std::string& barometer_name) const
|
||||
{
|
||||
auto* barometer = static_cast<const BarometerBase*>(findSensorByName(barometer_name, SensorBase::SensorType::Barometer));
|
||||
if (barometer == nullptr)
|
||||
throw VehicleControllerException(Utils::stringf("No barometer with name %s exist on vehicle", barometer_name.c_str()));
|
||||
|
||||
return barometer->getOutput();
|
||||
}
|
||||
|
||||
// Magnetometer API
|
||||
virtual MagnetometerBase::Output getMagnetometerData(const std::string& magnetometer_name) const
|
||||
{
|
||||
auto* magnetometer = static_cast<const MagnetometerBase*>(findSensorByName(magnetometer_name, SensorBase::SensorType::Magnetometer));
|
||||
if (magnetometer == nullptr)
|
||||
throw VehicleControllerException(Utils::stringf("No magnetometer with name %s exist on vehicle", magnetometer_name.c_str()));
|
||||
|
||||
return magnetometer->getOutput();
|
||||
}
|
||||
|
||||
// Gps API
|
||||
virtual GpsBase::Output getGpsData(const std::string& gps_name) const
|
||||
{
|
||||
auto* gps = static_cast<const GpsBase*>(findSensorByName(gps_name, SensorBase::SensorType::Gps));
|
||||
if (gps == nullptr)
|
||||
throw VehicleControllerException(Utils::stringf("No gps with name %s exist on vehicle", gps_name.c_str()));
|
||||
|
||||
return gps->getOutput();
|
||||
}
|
||||
|
||||
// Distance Sensor API
|
||||
virtual DistanceSensorData getDistanceSensorData(const std::string& distance_sensor_name) const
|
||||
{
|
||||
auto* distance_sensor = static_cast<const DistanceBase*>(findSensorByName(distance_sensor_name, SensorBase::SensorType::Distance));
|
||||
if (distance_sensor == nullptr)
|
||||
throw VehicleControllerException(Utils::stringf("No distance sensor with name %s exist on vehicle", distance_sensor_name.c_str()));
|
||||
|
||||
return distance_sensor->getOutput();
|
||||
}
|
||||
|
||||
virtual ~VehicleApiBase() = default;
|
||||
|
||||
//exceptions
|
||||
class VehicleControllerException : public std::runtime_error
|
||||
{
|
||||
public:
|
||||
VehicleControllerException(const std::string& message)
|
||||
: runtime_error(message)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
class VehicleCommandNotImplementedException : public VehicleControllerException
|
||||
{
|
||||
public:
|
||||
VehicleCommandNotImplementedException(const std::string& message)
|
||||
: VehicleControllerException(message)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
class VehicleMoveException : public VehicleControllerException
|
||||
{
|
||||
public:
|
||||
VehicleMoveException(const std::string& message)
|
||||
: VehicleControllerException(message)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
private:
|
||||
const SensorBase* findSensorByName(const std::string& sensor_name, const SensorBase::SensorType type) const
|
||||
{
|
||||
const SensorBase* sensor = nullptr;
|
||||
|
||||
// Find sensor with the given name (for empty input name, return the first one found)
|
||||
// Not efficient but should suffice given small number of sensors
|
||||
uint count_sensors = getSensors().size(type);
|
||||
for (uint i = 0; i < count_sensors; i++)
|
||||
const SensorBase* findSensorByName(const std::string& sensor_name, const SensorBase::SensorType type) const
|
||||
{
|
||||
const SensorBase* current_sensor = getSensors().getByType(type, i);
|
||||
if (current_sensor != nullptr && (current_sensor->getName() == sensor_name || sensor_name == ""))
|
||||
{
|
||||
sensor = current_sensor;
|
||||
break;
|
||||
const SensorBase* sensor = nullptr;
|
||||
|
||||
// Find sensor with the given name (for empty input name, return the first one found)
|
||||
// Not efficient but should suffice given small number of sensors
|
||||
uint count_sensors = getSensors().size(type);
|
||||
for (uint i = 0; i < count_sensors; i++) {
|
||||
const SensorBase* current_sensor = getSensors().getByType(type, i);
|
||||
if (current_sensor != nullptr && (current_sensor->getName() == sensor_name || sensor_name == "")) {
|
||||
sensor = current_sensor;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return sensor;
|
||||
}
|
||||
|
||||
return sensor;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
}} //namespace
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -11,68 +11,72 @@
|
|||
#include "physics/Environment.hpp"
|
||||
#include "common/AirSimSettings.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
class VehicleSimApiBase : public msr::airlib::UpdatableObject {
|
||||
public:
|
||||
virtual ~VehicleSimApiBase() = default;
|
||||
|
||||
virtual void update() override
|
||||
class VehicleSimApiBase : public msr::airlib::UpdatableObject
|
||||
{
|
||||
UpdatableObject::update();
|
||||
}
|
||||
public:
|
||||
virtual ~VehicleSimApiBase() = default;
|
||||
|
||||
//this method is called at every render tick when we want to transfer state from
|
||||
//physics engine to render engine. As physics engine is halted while
|
||||
//this happens, this method should do minimal processing
|
||||
virtual void updateRenderedState(float dt)
|
||||
{
|
||||
unused(dt);
|
||||
//derived class should override if needed
|
||||
}
|
||||
//called when render changes are required at every render tick
|
||||
virtual void updateRendering(float dt)
|
||||
{
|
||||
unused(dt);
|
||||
//derived class should override if needed
|
||||
}
|
||||
virtual void update() override
|
||||
{
|
||||
UpdatableObject::update();
|
||||
}
|
||||
|
||||
virtual const ImageCaptureBase* getImageCapture() const = 0;
|
||||
virtual ImageCaptureBase* getImageCapture()
|
||||
{
|
||||
return const_cast<ImageCaptureBase*>(static_cast<const VehicleSimApiBase*>(this)->getImageCapture());
|
||||
}
|
||||
//this method is called at every render tick when we want to transfer state from
|
||||
//physics engine to render engine. As physics engine is halted while
|
||||
//this happens, this method should do minimal processing
|
||||
virtual void updateRenderedState(float dt)
|
||||
{
|
||||
unused(dt);
|
||||
//derived class should override if needed
|
||||
}
|
||||
//called when render changes are required at every render tick
|
||||
virtual void updateRendering(float dt)
|
||||
{
|
||||
unused(dt);
|
||||
//derived class should override if needed
|
||||
}
|
||||
|
||||
virtual void initialize() = 0;
|
||||
virtual const ImageCaptureBase* getImageCapture() const = 0;
|
||||
virtual ImageCaptureBase* getImageCapture()
|
||||
{
|
||||
return const_cast<ImageCaptureBase*>(static_cast<const VehicleSimApiBase*>(this)->getImageCapture());
|
||||
}
|
||||
|
||||
virtual std::vector<ImageCaptureBase::ImageResponse> getImages(const std::vector<ImageCaptureBase::ImageRequest>& request) const = 0;
|
||||
virtual std::vector<uint8_t> getImage(const std::string& camera_name, ImageCaptureBase::ImageType image_type) const = 0;
|
||||
virtual void initialize() = 0;
|
||||
|
||||
virtual Pose getPose() const = 0;
|
||||
virtual void setPose(const Pose& pose, bool ignore_collision) = 0;
|
||||
virtual const Kinematics::State* getGroundTruthKinematics() const = 0;
|
||||
virtual const msr::airlib::Environment* getGroundTruthEnvironment() const = 0;
|
||||
virtual std::vector<ImageCaptureBase::ImageResponse> getImages(const std::vector<ImageCaptureBase::ImageRequest>& request) const = 0;
|
||||
virtual std::vector<uint8_t> getImage(const std::string& camera_name, ImageCaptureBase::ImageType image_type) const = 0;
|
||||
|
||||
virtual CameraInfo getCameraInfo(const std::string& camera_name) const = 0;
|
||||
virtual void setCameraPose(const std::string& camera_name, const Pose& pose) = 0;
|
||||
virtual void setCameraFoV(const std::string& camera_name, float fov_degrees) = 0;
|
||||
virtual void setDistortionParam(const std::string& camera_name, const std::string& param_name, float value) = 0;
|
||||
virtual std::vector<float> getDistortionParams(const std::string& camera_name) = 0;
|
||||
virtual Pose getPose() const = 0;
|
||||
virtual void setPose(const Pose& pose, bool ignore_collision) = 0;
|
||||
virtual const Kinematics::State* getGroundTruthKinematics() const = 0;
|
||||
virtual const msr::airlib::Environment* getGroundTruthEnvironment() const = 0;
|
||||
|
||||
virtual CollisionInfo getCollisionInfo() const = 0;
|
||||
virtual int getRemoteControlID() const = 0; //which RC to use, 0 is first one, -1 means disable RC (use keyborad)
|
||||
virtual RCData getRCData() const = 0; //get reading from RC from simulator's host OS
|
||||
virtual std::string getVehicleName() const = 0;
|
||||
virtual std::string getRecordFileLine(bool is_header_line) const = 0;
|
||||
virtual void toggleTrace() = 0;
|
||||
virtual void setTraceLine(const std::vector<float>& color_rgba, float thickness) = 0;
|
||||
virtual CameraInfo getCameraInfo(const std::string& camera_name) const = 0;
|
||||
virtual void setCameraPose(const std::string& camera_name, const Pose& pose) = 0;
|
||||
virtual void setCameraFoV(const std::string& camera_name, float fov_degrees) = 0;
|
||||
virtual void setDistortionParam(const std::string& camera_name, const std::string& param_name, float value) = 0;
|
||||
virtual std::vector<float> getDistortionParams(const std::string& camera_name) = 0;
|
||||
|
||||
//use pointer here because of derived classes for VehicleSetting
|
||||
const AirSimSettings::VehicleSetting* getVehicleSetting() const
|
||||
{
|
||||
return AirSimSettings::singleton().getVehicleSetting(getVehicleName());
|
||||
}
|
||||
};
|
||||
virtual CollisionInfo getCollisionInfo() const = 0;
|
||||
virtual int getRemoteControlID() const = 0; //which RC to use, 0 is first one, -1 means disable RC (use keyborad)
|
||||
virtual RCData getRCData() const = 0; //get reading from RC from simulator's host OS
|
||||
virtual std::string getVehicleName() const = 0;
|
||||
virtual std::string getRecordFileLine(bool is_header_line) const = 0;
|
||||
virtual void toggleTrace() = 0;
|
||||
virtual void setTraceLine(const std::vector<float>& color_rgba, float thickness) = 0;
|
||||
|
||||
} } //namespace
|
||||
//use pointer here because of derived classes for VehicleSetting
|
||||
const AirSimSettings::VehicleSetting* getVehicleSetting() const
|
||||
{
|
||||
return AirSimSettings::singleton().getVehicleSetting(getVehicleName());
|
||||
}
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -7,82 +7,84 @@
|
|||
#include "common/CommonStructs.hpp"
|
||||
#include "common/AirSimSettings.hpp"
|
||||
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
class WorldSimApiBase
|
||||
{
|
||||
public:
|
||||
enum class WeatherParameter
|
||||
{
|
||||
Rain = 0,
|
||||
Roadwetness = 1,
|
||||
Snow = 2,
|
||||
RoadSnow = 3,
|
||||
MapleLeaf = 4,
|
||||
RoadLeaf = 5,
|
||||
Dust = 6,
|
||||
Fog = 7,
|
||||
Enabled = 8
|
||||
};
|
||||
|
||||
virtual ~WorldSimApiBase() = default;
|
||||
|
||||
class WorldSimApiBase {
|
||||
public:
|
||||
enum class WeatherParameter {
|
||||
Rain = 0,
|
||||
Roadwetness = 1,
|
||||
Snow = 2,
|
||||
RoadSnow = 3,
|
||||
MapleLeaf = 4,
|
||||
RoadLeaf = 5,
|
||||
Dust = 6,
|
||||
Fog = 7,
|
||||
Enabled = 8
|
||||
// ------ Level setting apis ----- //
|
||||
virtual bool loadLevel(const std::string& level_name) = 0;
|
||||
virtual string spawnObject(string& object_name, const string& load_component, const Pose& pose, const Vector3r& scale, bool physics_enabled) = 0;
|
||||
virtual bool destroyObject(const string& object_name) = 0;
|
||||
|
||||
virtual bool isPaused() const = 0;
|
||||
virtual void reset() = 0;
|
||||
virtual void pause(bool is_paused) = 0;
|
||||
virtual void continueForTime(double seconds) = 0;
|
||||
virtual void continueForFrames(uint32_t frames) = 0;
|
||||
|
||||
virtual void setTimeOfDay(bool is_enabled, const std::string& start_datetime, bool is_start_datetime_dst,
|
||||
float celestial_clock_speed, float update_interval_secs, bool move_sun) = 0;
|
||||
|
||||
virtual void enableWeather(bool enable) = 0;
|
||||
virtual void setWeatherParameter(WeatherParameter param, float val) = 0;
|
||||
|
||||
virtual bool setSegmentationObjectID(const std::string& mesh_name, int object_id, bool is_name_regex = false) = 0;
|
||||
virtual int getSegmentationObjectID(const std::string& mesh_name) const = 0;
|
||||
|
||||
virtual bool addVehicle(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path = "") = 0;
|
||||
|
||||
virtual void printLogMessage(const std::string& message,
|
||||
const std::string& message_param = "", unsigned char severity = 0) = 0;
|
||||
|
||||
//----------- Plotting APIs ----------/
|
||||
virtual void simFlushPersistentMarkers() = 0;
|
||||
virtual void simPlotPoints(const vector<Vector3r>& points, const vector<float>& color_rgba, float size, float duration, bool is_persistent) = 0;
|
||||
virtual void simPlotLineStrip(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent) = 0;
|
||||
virtual void simPlotLineList(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent) = 0;
|
||||
virtual void simPlotArrows(const vector<Vector3r>& points_start, const vector<Vector3r>& points_end, const vector<float>& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent) = 0;
|
||||
virtual void simPlotStrings(const vector<std::string>& strings, const vector<Vector3r>& positions, float scale, const vector<float>& color_rgba, float duration) = 0;
|
||||
virtual void simPlotTransforms(const vector<Pose>& poses, float scale, float thickness, float duration, bool is_persistent) = 0;
|
||||
virtual void simPlotTransformsWithNames(const vector<Pose>& poses, const vector<std::string>& names, float tf_scale, float tf_thickness, float text_scale, const vector<float>& text_color_rgba, float duration) = 0;
|
||||
|
||||
virtual std::vector<std::string> listSceneObjects(const std::string& name_regex) const = 0;
|
||||
virtual Pose getObjectPose(const std::string& object_name) const = 0;
|
||||
virtual Vector3r getObjectScale(const std::string& object_name) const = 0;
|
||||
virtual bool setObjectPose(const std::string& object_name, const Pose& pose, bool teleport) = 0;
|
||||
virtual bool runConsoleCommand(const std::string& command) = 0;
|
||||
virtual bool setObjectScale(const std::string& object_name, const Vector3r& scale) = 0;
|
||||
virtual std::unique_ptr<std::vector<std::string>> swapTextures(const std::string& tag, int tex_id = 0, int component_id = 0, int material_id = 0) = 0;
|
||||
virtual vector<MeshPositionVertexBuffersResponse> getMeshPositionVertexBuffers() const = 0;
|
||||
|
||||
virtual bool createVoxelGrid(const Vector3r& position, const int& x_size, const int& y_size, const int& z_size, const float& res, const std::string& output_file) = 0;
|
||||
|
||||
// Recording APIs
|
||||
virtual void startRecording() = 0;
|
||||
virtual void stopRecording() = 0;
|
||||
virtual bool isRecording() const = 0;
|
||||
|
||||
virtual void setWind(const Vector3r& wind) const = 0;
|
||||
|
||||
virtual std::string getSettingsString() const = 0;
|
||||
};
|
||||
|
||||
virtual ~WorldSimApiBase() = default;
|
||||
|
||||
// ------ Level setting apis ----- //
|
||||
virtual bool loadLevel(const std::string& level_name) = 0;
|
||||
virtual string spawnObject(string& object_name, const string& load_component, const Pose& pose, const Vector3r& scale, bool physics_enabled) = 0;
|
||||
virtual bool destroyObject(const string& object_name) = 0;
|
||||
|
||||
virtual bool isPaused() const = 0;
|
||||
virtual void reset() = 0;
|
||||
virtual void pause(bool is_paused) = 0;
|
||||
virtual void continueForTime(double seconds) = 0;
|
||||
virtual void continueForFrames(uint32_t frames) = 0;
|
||||
|
||||
virtual void setTimeOfDay(bool is_enabled, const std::string& start_datetime, bool is_start_datetime_dst,
|
||||
float celestial_clock_speed, float update_interval_secs, bool move_sun) = 0;
|
||||
|
||||
virtual void enableWeather(bool enable) = 0;
|
||||
virtual void setWeatherParameter(WeatherParameter param, float val) = 0;
|
||||
|
||||
virtual bool setSegmentationObjectID(const std::string& mesh_name, int object_id, bool is_name_regex = false) = 0;
|
||||
virtual int getSegmentationObjectID(const std::string& mesh_name) const = 0;
|
||||
|
||||
virtual bool addVehicle(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path = "") = 0;
|
||||
|
||||
virtual void printLogMessage(const std::string& message,
|
||||
const std::string& message_param = "", unsigned char severity = 0) = 0;
|
||||
|
||||
//----------- Plotting APIs ----------/
|
||||
virtual void simFlushPersistentMarkers() = 0;
|
||||
virtual void simPlotPoints(const vector<Vector3r>& points, const vector<float>& color_rgba, float size, float duration, bool is_persistent) = 0;
|
||||
virtual void simPlotLineStrip(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent) = 0;
|
||||
virtual void simPlotLineList(const vector<Vector3r>& points, const vector<float>& color_rgba, float thickness, float duration, bool is_persistent) = 0;
|
||||
virtual void simPlotArrows(const vector<Vector3r>& points_start, const vector<Vector3r>& points_end, const vector<float>& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent) = 0;
|
||||
virtual void simPlotStrings(const vector<std::string>& strings, const vector<Vector3r>& positions, float scale, const vector<float>& color_rgba, float duration) = 0;
|
||||
virtual void simPlotTransforms(const vector<Pose>& poses, float scale, float thickness, float duration, bool is_persistent) = 0;
|
||||
virtual void simPlotTransformsWithNames(const vector<Pose>& poses, const vector<std::string>& names, float tf_scale, float tf_thickness, float text_scale, const vector<float>& text_color_rgba, float duration) = 0;
|
||||
|
||||
virtual std::vector<std::string> listSceneObjects(const std::string& name_regex) const = 0;
|
||||
virtual Pose getObjectPose(const std::string& object_name) const = 0;
|
||||
virtual Vector3r getObjectScale(const std::string& object_name) const = 0;
|
||||
virtual bool setObjectPose(const std::string& object_name, const Pose& pose, bool teleport) = 0;
|
||||
virtual bool runConsoleCommand(const std::string& command) = 0;
|
||||
virtual bool setObjectScale(const std::string& object_name, const Vector3r& scale) = 0;
|
||||
virtual std::unique_ptr<std::vector<std::string>> swapTextures(const std::string& tag, int tex_id = 0, int component_id = 0, int material_id = 0) = 0;
|
||||
virtual vector<MeshPositionVertexBuffersResponse> getMeshPositionVertexBuffers() const = 0;
|
||||
|
||||
virtual bool createVoxelGrid(const Vector3r& position, const int& x_size, const int& y_size, const int& z_size, const float& res, const std::string& output_file) = 0;
|
||||
|
||||
// Recording APIs
|
||||
virtual void startRecording() = 0;
|
||||
virtual void stopRecording() = 0;
|
||||
virtual bool isRecording() const = 0;
|
||||
|
||||
virtual void setWind(const Vector3r& wind) const = 0;
|
||||
|
||||
virtual std::string getSettingsString() const = 0;
|
||||
};
|
||||
|
||||
|
||||
}} //namespace
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -1310,7 +1310,6 @@ namespace airlib
|
|||
createDefaultSensorSettings(simmode_name, sensors);
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -9,96 +9,100 @@
|
|||
#include "common/Common.hpp"
|
||||
#include "common/common_utils/Utils.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
class CancelToken {
|
||||
public:
|
||||
CancelToken()
|
||||
: is_cancelled_(false), is_complete_(false), recursion_count_(0)
|
||||
class CancelToken
|
||||
{
|
||||
}
|
||||
|
||||
void reset()
|
||||
{
|
||||
is_cancelled_ = false;
|
||||
is_complete_ = false;
|
||||
recursion_count_ = 0;
|
||||
}
|
||||
|
||||
bool isCancelled() const
|
||||
{
|
||||
return is_cancelled_;
|
||||
}
|
||||
|
||||
void cancel()
|
||||
{
|
||||
is_cancelled_ = true;
|
||||
}
|
||||
|
||||
bool sleep(double secs)
|
||||
{
|
||||
//We can pass duration directly to sleep_for however it is known that on
|
||||
//some systems, sleep_for makes system call anyway even if passed duration
|
||||
//is <= 0. This can cause 50us of delay due to context switch.
|
||||
if (isCancelled()) {
|
||||
return false;
|
||||
public:
|
||||
CancelToken()
|
||||
: is_cancelled_(false), is_complete_(false), recursion_count_(0)
|
||||
{
|
||||
}
|
||||
|
||||
TTimePoint start = ClockFactory::get()->nowNanos();
|
||||
static constexpr std::chrono::duration<double> MinSleepDuration(0);
|
||||
|
||||
while (secs > 0 && !isCancelled() &&
|
||||
ClockFactory::get()->elapsedSince(start) < secs) {
|
||||
|
||||
std::this_thread::sleep_for(MinSleepDuration);
|
||||
void reset()
|
||||
{
|
||||
is_cancelled_ = false;
|
||||
is_complete_ = false;
|
||||
recursion_count_ = 0;
|
||||
}
|
||||
|
||||
return !isCancelled();
|
||||
}
|
||||
bool isCancelled() const
|
||||
{
|
||||
return is_cancelled_;
|
||||
}
|
||||
|
||||
void complete(bool is_complete = true)
|
||||
{
|
||||
is_complete_ = is_complete;
|
||||
}
|
||||
void cancel()
|
||||
{
|
||||
is_cancelled_ = true;
|
||||
}
|
||||
|
||||
bool isComplete() const
|
||||
{
|
||||
return is_complete_;
|
||||
}
|
||||
bool sleep(double secs)
|
||||
{
|
||||
//We can pass duration directly to sleep_for however it is known that on
|
||||
//some systems, sleep_for makes system call anyway even if passed duration
|
||||
//is <= 0. This can cause 50us of delay due to context switch.
|
||||
if (isCancelled()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
int getRecursionCount()
|
||||
{
|
||||
return recursion_count_;
|
||||
}
|
||||
TTimePoint start = ClockFactory::get()->nowNanos();
|
||||
static constexpr std::chrono::duration<double> MinSleepDuration(0);
|
||||
|
||||
bool try_lock()
|
||||
{
|
||||
bool result = wait_mutex_.try_lock();
|
||||
if (result)
|
||||
while (secs > 0 && !isCancelled() &&
|
||||
ClockFactory::get()->elapsedSince(start) < secs) {
|
||||
|
||||
std::this_thread::sleep_for(MinSleepDuration);
|
||||
}
|
||||
|
||||
return !isCancelled();
|
||||
}
|
||||
|
||||
void complete(bool is_complete = true)
|
||||
{
|
||||
is_complete_ = is_complete;
|
||||
}
|
||||
|
||||
bool isComplete() const
|
||||
{
|
||||
return is_complete_;
|
||||
}
|
||||
|
||||
int getRecursionCount()
|
||||
{
|
||||
return recursion_count_;
|
||||
}
|
||||
|
||||
bool try_lock()
|
||||
{
|
||||
bool result = wait_mutex_.try_lock();
|
||||
if (result)
|
||||
++recursion_count_;
|
||||
return result;
|
||||
}
|
||||
|
||||
void unlock()
|
||||
{
|
||||
wait_mutex_.unlock();
|
||||
if (recursion_count_ > 0)
|
||||
--recursion_count_;
|
||||
}
|
||||
|
||||
void lock()
|
||||
{
|
||||
wait_mutex_.lock();
|
||||
++recursion_count_;
|
||||
return result;
|
||||
}
|
||||
}
|
||||
|
||||
void unlock()
|
||||
{
|
||||
wait_mutex_.unlock();
|
||||
if (recursion_count_ > 0)
|
||||
--recursion_count_;
|
||||
}
|
||||
private:
|
||||
std::atomic<bool> is_cancelled_;
|
||||
std::atomic<bool> is_complete_;
|
||||
std::atomic<int> recursion_count_;
|
||||
|
||||
void lock()
|
||||
{
|
||||
wait_mutex_.lock();
|
||||
++recursion_count_;
|
||||
}
|
||||
|
||||
private:
|
||||
std::atomic<bool> is_cancelled_;
|
||||
std::atomic<bool> is_complete_;
|
||||
std::atomic<int> recursion_count_;
|
||||
|
||||
std::recursive_mutex wait_mutex_;
|
||||
};
|
||||
|
||||
}} //namespace
|
||||
std::recursive_mutex wait_mutex_;
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -8,86 +8,89 @@
|
|||
#include <chrono>
|
||||
#include "Common.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
class ClockBase {
|
||||
public:
|
||||
//returns value indicating nanoseconds elapsed since some reference timepoint in history
|
||||
//typically nanoseconds from Unix epoch
|
||||
virtual TTimePoint nowNanos() const = 0;
|
||||
virtual TTimePoint getStart() const = 0;
|
||||
|
||||
|
||||
ClockBase()
|
||||
class ClockBase
|
||||
{
|
||||
wall_clock_start_ = Utils::getTimeSinceEpochNanos();
|
||||
}
|
||||
public:
|
||||
//returns value indicating nanoseconds elapsed since some reference timepoint in history
|
||||
//typically nanoseconds from Unix epoch
|
||||
virtual TTimePoint nowNanos() const = 0;
|
||||
virtual TTimePoint getStart() const = 0;
|
||||
|
||||
TTimeDelta elapsedSince(TTimePoint since) const
|
||||
{
|
||||
return elapsedBetween(nowNanos(), since);
|
||||
}
|
||||
static TTimeDelta elapsedBetween(TTimePoint second, TTimePoint first)
|
||||
{
|
||||
return (second - first) / 1.0E9;
|
||||
}
|
||||
TTimePoint addTo(TTimePoint t, TTimeDelta dt)
|
||||
{
|
||||
return static_cast<TTimePoint>(t + dt * 1.0E9);
|
||||
}
|
||||
TTimeDelta updateSince(TTimePoint& since) const
|
||||
{
|
||||
TTimePoint cur = nowNanos();
|
||||
TTimeDelta elapsed = elapsedBetween(cur, since);
|
||||
since = cur;
|
||||
return elapsed;
|
||||
}
|
||||
ClockBase()
|
||||
{
|
||||
wall_clock_start_ = Utils::getTimeSinceEpochNanos();
|
||||
}
|
||||
|
||||
virtual TTimePoint step()
|
||||
{
|
||||
//by default step doesn't do anything
|
||||
//for steppeble clock, this would advance to next tick
|
||||
//for wall clocks, step() is no-op
|
||||
++step_count_;
|
||||
TTimeDelta elapsedSince(TTimePoint since) const
|
||||
{
|
||||
return elapsedBetween(nowNanos(), since);
|
||||
}
|
||||
static TTimeDelta elapsedBetween(TTimePoint second, TTimePoint first)
|
||||
{
|
||||
return (second - first) / 1.0E9;
|
||||
}
|
||||
TTimePoint addTo(TTimePoint t, TTimeDelta dt)
|
||||
{
|
||||
return static_cast<TTimePoint>(t + dt * 1.0E9);
|
||||
}
|
||||
TTimeDelta updateSince(TTimePoint& since) const
|
||||
{
|
||||
TTimePoint cur = nowNanos();
|
||||
TTimeDelta elapsed = elapsedBetween(cur, since);
|
||||
since = cur;
|
||||
return elapsed;
|
||||
}
|
||||
|
||||
return nowNanos();
|
||||
}
|
||||
virtual TTimePoint step()
|
||||
{
|
||||
//by default step doesn't do anything
|
||||
//for steppeble clock, this would advance to next tick
|
||||
//for wall clocks, step() is no-op
|
||||
++step_count_;
|
||||
|
||||
uint64_t getStepCount() const
|
||||
{
|
||||
return step_count_;
|
||||
}
|
||||
return nowNanos();
|
||||
}
|
||||
|
||||
virtual void sleep_for(TTimeDelta dt)
|
||||
{
|
||||
if (dt <= 0)
|
||||
return;
|
||||
uint64_t getStepCount() const
|
||||
{
|
||||
return step_count_;
|
||||
}
|
||||
|
||||
static constexpr std::chrono::duration<double> MinSleepDuration(0);
|
||||
TTimePoint start = nowNanos();
|
||||
//spin wait
|
||||
while (elapsedSince(start) < dt)
|
||||
std::this_thread::sleep_for(MinSleepDuration);
|
||||
}
|
||||
virtual void sleep_for(TTimeDelta dt)
|
||||
{
|
||||
if (dt <= 0)
|
||||
return;
|
||||
|
||||
double getTrueScaleWrtWallClock()
|
||||
{
|
||||
TTimePoint wall_clock_now = Utils::getTimeSinceEpochNanos();
|
||||
TTimeDelta wall_clock_elapsed = elapsedBetween(wall_clock_now, wall_clock_start_);
|
||||
static constexpr std::chrono::duration<double> MinSleepDuration(0);
|
||||
TTimePoint start = nowNanos();
|
||||
//spin wait
|
||||
while (elapsedSince(start) < dt)
|
||||
std::this_thread::sleep_for(MinSleepDuration);
|
||||
}
|
||||
|
||||
TTimePoint clock_now = nowNanos();
|
||||
TTimeDelta clock_elapsed = elapsedBetween(clock_now, getStart());
|
||||
double getTrueScaleWrtWallClock()
|
||||
{
|
||||
TTimePoint wall_clock_now = Utils::getTimeSinceEpochNanos();
|
||||
TTimeDelta wall_clock_elapsed = elapsedBetween(wall_clock_now, wall_clock_start_);
|
||||
|
||||
return static_cast<double>(clock_elapsed) / wall_clock_elapsed;
|
||||
}
|
||||
TTimePoint clock_now = nowNanos();
|
||||
TTimeDelta clock_elapsed = elapsedBetween(clock_now, getStart());
|
||||
|
||||
private:
|
||||
template <typename T>
|
||||
using duration = std::chrono::duration<T>;
|
||||
return static_cast<double>(clock_elapsed) / wall_clock_elapsed;
|
||||
}
|
||||
|
||||
uint64_t step_count_ = 0;
|
||||
TTimePoint wall_clock_start_;
|
||||
};
|
||||
private:
|
||||
template <typename T>
|
||||
using duration = std::chrono::duration<T>;
|
||||
|
||||
}} //namespace
|
||||
uint64_t step_count_ = 0;
|
||||
TTimePoint wall_clock_start_;
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -7,32 +7,36 @@
|
|||
#include "ScalableClock.hpp"
|
||||
#include <memory>
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
class ClockFactory {
|
||||
public:
|
||||
//output of this function should not be stored as pointer might change
|
||||
static ClockBase* get(std::shared_ptr<ClockBase> val = nullptr)
|
||||
class ClockFactory
|
||||
{
|
||||
static std::shared_ptr<ClockBase> clock;
|
||||
public:
|
||||
//output of this function should not be stored as pointer might change
|
||||
static ClockBase* get(std::shared_ptr<ClockBase> val = nullptr)
|
||||
{
|
||||
static std::shared_ptr<ClockBase> clock;
|
||||
|
||||
if (val != nullptr)
|
||||
clock = val;
|
||||
if (val != nullptr)
|
||||
clock = val;
|
||||
|
||||
if (clock == nullptr)
|
||||
clock = std::make_shared<ScalableClock>();
|
||||
if (clock == nullptr)
|
||||
clock = std::make_shared<ScalableClock>();
|
||||
|
||||
return clock.get();
|
||||
}
|
||||
return clock.get();
|
||||
}
|
||||
|
||||
//don't allow multiple instances of this class
|
||||
ClockFactory(ClockFactory const&) = delete;
|
||||
void operator=(ClockFactory const&) = delete;
|
||||
//don't allow multiple instances of this class
|
||||
ClockFactory(ClockFactory const&) = delete;
|
||||
void operator=(ClockFactory const&) = delete;
|
||||
|
||||
private:
|
||||
//disallow instance creation
|
||||
ClockFactory(){}
|
||||
};
|
||||
|
||||
}} //namespace
|
||||
private:
|
||||
//disallow instance creation
|
||||
ClockFactory() {}
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -21,55 +21,59 @@
|
|||
|
||||
#define RpcLibPort 41451
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
//numericals
|
||||
typedef float real_T;
|
||||
//this is not required for most compilers
|
||||
typedef unsigned int uint;
|
||||
//numericals
|
||||
typedef float real_T;
|
||||
//this is not required for most compilers
|
||||
typedef unsigned int uint;
|
||||
|
||||
//well known types
|
||||
typedef msr::airlib::VectorMathf VectorMath;
|
||||
typedef VectorMath::Vector3f Vector3r;
|
||||
typedef VectorMath::Vector2f Vector2r;
|
||||
typedef VectorMath::Vector1f Vector1r;
|
||||
typedef VectorMath::Array3f Array3r;
|
||||
typedef VectorMath::Pose Pose;
|
||||
typedef VectorMath::Quaternionf Quaternionr;
|
||||
typedef VectorMath::Matrix3x3f Matrix3x3r;
|
||||
typedef VectorMath::AngleAxisf AngleAxisr;
|
||||
typedef common_utils::RandomGeneratorF RandomGeneratorR;
|
||||
typedef common_utils::RandomGeneratorGaussianF RandomGeneratorGausianR;
|
||||
typedef std::string string;
|
||||
typedef common_utils::Utils Utils;
|
||||
typedef VectorMath::RandomVectorGaussianT RandomVectorGaussianR;
|
||||
typedef VectorMath::RandomVectorT RandomVectorR;
|
||||
typedef uint64_t TTimePoint;
|
||||
typedef double TTimeDelta;
|
||||
//well known types
|
||||
typedef msr::airlib::VectorMathf VectorMath;
|
||||
typedef VectorMath::Vector3f Vector3r;
|
||||
typedef VectorMath::Vector2f Vector2r;
|
||||
typedef VectorMath::Vector1f Vector1r;
|
||||
typedef VectorMath::Array3f Array3r;
|
||||
typedef VectorMath::Pose Pose;
|
||||
typedef VectorMath::Quaternionf Quaternionr;
|
||||
typedef VectorMath::Matrix3x3f Matrix3x3r;
|
||||
typedef VectorMath::AngleAxisf AngleAxisr;
|
||||
typedef common_utils::RandomGeneratorF RandomGeneratorR;
|
||||
typedef common_utils::RandomGeneratorGaussianF RandomGeneratorGausianR;
|
||||
typedef std::string string;
|
||||
typedef common_utils::Utils Utils;
|
||||
typedef VectorMath::RandomVectorGaussianT RandomVectorGaussianR;
|
||||
typedef VectorMath::RandomVectorT RandomVectorR;
|
||||
typedef uint64_t TTimePoint;
|
||||
typedef double TTimeDelta;
|
||||
|
||||
template <typename T>
|
||||
using vector = std::vector<T>;
|
||||
template <typename TKey, typename TValue>
|
||||
using unordered_map = std::unordered_map<TKey, TValue>;
|
||||
template <typename TKey>
|
||||
using unordered_set = std::unordered_set<TKey>;
|
||||
template <typename T>
|
||||
using unique_ptr = std::unique_ptr<T>;
|
||||
template <typename T>
|
||||
using shared_ptr = std::shared_ptr<T>;
|
||||
template <typename T>
|
||||
using vector_size_type = typename std::vector<T>::size_type;
|
||||
|
||||
template <typename T>
|
||||
using vector = std::vector<T>;
|
||||
template <typename TKey, typename TValue>
|
||||
using unordered_map = std::unordered_map<TKey, TValue>;
|
||||
template <typename TKey>
|
||||
using unordered_set = std::unordered_set<TKey>;
|
||||
template <typename T>
|
||||
using unique_ptr = std::unique_ptr<T>;
|
||||
template <typename T>
|
||||
using shared_ptr = std::shared_ptr<T>;
|
||||
template <typename T>
|
||||
using vector_size_type = typename std::vector<T>::size_type;
|
||||
inline std::ostream& operator<<(std::ostream& os, Quaternionr const& q)
|
||||
{
|
||||
float p, r, y;
|
||||
VectorMath::toEulerianAngle(q, p, r, y);
|
||||
return os << "(" << r << "\t" << p << "\t" << y << ")" << q.w() << q.x() << "\t" << q.y() << "\t" << q.z() << "\t";
|
||||
}
|
||||
|
||||
inline std::ostream& operator<<(std::ostream &os, Quaternionr const &q) {
|
||||
float p, r, y;
|
||||
VectorMath::toEulerianAngle(q, p, r, y);
|
||||
return os << "(" << r << "\t" << p << "\t" << y << ")" << q.w() << q.x() << "\t" << q.y() << "\t" << q.z() << "\t";
|
||||
inline std::ostream& operator<<(std::ostream& os, Vector3r const& vec)
|
||||
{
|
||||
return os << vec.x() << "\t" << vec.y() << "\t" << vec.z() << "\t";
|
||||
}
|
||||
}
|
||||
|
||||
inline std::ostream& operator<<(std::ostream &os, Vector3r const &vec) {
|
||||
return os << vec.x() << "\t" << vec.y() << "\t" << vec.z() << "\t";
|
||||
}
|
||||
|
||||
}} //namespace
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -7,329 +7,372 @@
|
|||
#include "common/Common.hpp"
|
||||
#include <ostream>
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
//velocity
|
||||
struct Twist {
|
||||
Vector3r linear, angular;
|
||||
|
||||
Twist()
|
||||
{}
|
||||
|
||||
Twist(const Vector3r& linear_val, const Vector3r& angular_val)
|
||||
: linear(linear_val), angular(angular_val)
|
||||
//velocity
|
||||
struct Twist
|
||||
{
|
||||
}
|
||||
Vector3r linear, angular;
|
||||
|
||||
static const Twist zero()
|
||||
Twist()
|
||||
{
|
||||
}
|
||||
|
||||
Twist(const Vector3r& linear_val, const Vector3r& angular_val)
|
||||
: linear(linear_val), angular(angular_val)
|
||||
{
|
||||
}
|
||||
|
||||
static const Twist zero()
|
||||
{
|
||||
static const Twist zero_twist(Vector3r::Zero(), Vector3r::Zero());
|
||||
return zero_twist;
|
||||
}
|
||||
};
|
||||
|
||||
//force & torque
|
||||
struct Wrench
|
||||
{
|
||||
static const Twist zero_twist(Vector3r::Zero(), Vector3r::Zero());
|
||||
return zero_twist;
|
||||
}
|
||||
};
|
||||
Vector3r force, torque;
|
||||
|
||||
//force & torque
|
||||
struct Wrench {
|
||||
Vector3r force, torque;
|
||||
Wrench()
|
||||
{
|
||||
}
|
||||
|
||||
Wrench()
|
||||
{}
|
||||
Wrench(const Vector3r& force_val, const Vector3r& torque_val)
|
||||
: force(force_val), torque(torque_val)
|
||||
{
|
||||
}
|
||||
|
||||
Wrench(const Vector3r& force_val, const Vector3r& torque_val)
|
||||
: force(force_val), torque(torque_val)
|
||||
//support basic arithmatic
|
||||
Wrench operator+(const Wrench& other) const
|
||||
{
|
||||
Wrench result;
|
||||
result.force = this->force + other.force;
|
||||
result.torque = this->torque + other.torque;
|
||||
return result;
|
||||
}
|
||||
Wrench operator+=(const Wrench& other)
|
||||
{
|
||||
force += other.force;
|
||||
torque += other.torque;
|
||||
return *this;
|
||||
}
|
||||
Wrench operator-(const Wrench& other) const
|
||||
{
|
||||
Wrench result;
|
||||
result.force = this->force - other.force;
|
||||
result.torque = this->torque - other.torque;
|
||||
return result;
|
||||
}
|
||||
Wrench operator-=(const Wrench& other)
|
||||
{
|
||||
force -= other.force;
|
||||
torque -= other.torque;
|
||||
return *this;
|
||||
}
|
||||
|
||||
static const Wrench zero()
|
||||
{
|
||||
static const Wrench zero_wrench(Vector3r::Zero(), Vector3r::Zero());
|
||||
return zero_wrench;
|
||||
}
|
||||
};
|
||||
|
||||
struct Momentums
|
||||
{
|
||||
}
|
||||
Vector3r linear;
|
||||
Vector3r angular;
|
||||
|
||||
//support basic arithmatic
|
||||
Wrench operator+(const Wrench& other) const
|
||||
Momentums()
|
||||
{
|
||||
}
|
||||
|
||||
Momentums(const Vector3r& linear_val, const Vector3r& angular_val)
|
||||
: linear(linear_val), angular(angular_val)
|
||||
{
|
||||
}
|
||||
|
||||
static const Momentums zero()
|
||||
{
|
||||
static const Momentums zero_val(Vector3r::Zero(), Vector3r::Zero());
|
||||
return zero_val;
|
||||
}
|
||||
};
|
||||
|
||||
struct Accelerations
|
||||
{
|
||||
Wrench result;
|
||||
result.force = this->force + other.force;
|
||||
result.torque = this->torque + other.torque;
|
||||
return result;
|
||||
}
|
||||
Wrench operator+=(const Wrench& other)
|
||||
Vector3r linear;
|
||||
Vector3r angular;
|
||||
|
||||
Accelerations()
|
||||
{
|
||||
}
|
||||
|
||||
Accelerations(const Vector3r& linear_val, const Vector3r& angular_val)
|
||||
: linear(linear_val), angular(angular_val)
|
||||
{
|
||||
}
|
||||
|
||||
static const Accelerations zero()
|
||||
{
|
||||
static const Accelerations zero_val(Vector3r::Zero(), Vector3r::Zero());
|
||||
return zero_val;
|
||||
}
|
||||
};
|
||||
|
||||
struct PoseWithCovariance
|
||||
{
|
||||
force += other.force;
|
||||
torque += other.torque;
|
||||
return *this;
|
||||
}
|
||||
Wrench operator-(const Wrench& other) const
|
||||
VectorMath::Pose pose;
|
||||
vector<real_T> covariance; //36 elements, 6x6 matrix
|
||||
|
||||
PoseWithCovariance()
|
||||
: covariance(36, 0)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
struct PowerSupply
|
||||
{
|
||||
Wrench result;
|
||||
result.force = this->force - other.force;
|
||||
result.torque = this->torque - other.torque;
|
||||
return result;
|
||||
}
|
||||
Wrench operator-=(const Wrench& other)
|
||||
vector<real_T> voltage, current;
|
||||
};
|
||||
|
||||
struct TwistWithCovariance
|
||||
{
|
||||
force -= other.force;
|
||||
torque -= other.torque;
|
||||
return *this;
|
||||
}
|
||||
Twist twist;
|
||||
vector<real_T> covariance; //36 elements, 6x6 matrix
|
||||
|
||||
static const Wrench zero()
|
||||
TwistWithCovariance()
|
||||
: covariance(36, 0)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
struct Joystick
|
||||
{
|
||||
static const Wrench zero_wrench(Vector3r::Zero(), Vector3r::Zero());
|
||||
return zero_wrench;
|
||||
}
|
||||
};
|
||||
vector<float> axes;
|
||||
vector<int> buttons;
|
||||
};
|
||||
|
||||
struct Momentums {
|
||||
Vector3r linear;
|
||||
Vector3r angular;
|
||||
|
||||
Momentums()
|
||||
{}
|
||||
|
||||
Momentums(const Vector3r& linear_val, const Vector3r& angular_val)
|
||||
: linear(linear_val), angular(angular_val)
|
||||
struct Odometry
|
||||
{
|
||||
}
|
||||
PoseWithCovariance pose;
|
||||
TwistWithCovariance twist;
|
||||
};
|
||||
|
||||
static const Momentums zero()
|
||||
struct GeoPoint
|
||||
{
|
||||
static const Momentums zero_val(Vector3r::Zero(), Vector3r::Zero());
|
||||
return zero_val;
|
||||
}
|
||||
};
|
||||
double latitude = 0, longitude = 0;
|
||||
float altitude = 0;
|
||||
|
||||
struct Accelerations {
|
||||
Vector3r linear;
|
||||
Vector3r angular;
|
||||
GeoPoint()
|
||||
{
|
||||
}
|
||||
|
||||
Accelerations()
|
||||
{}
|
||||
GeoPoint(double latitude_val, double longitude_val, float altitude_val)
|
||||
{
|
||||
set(latitude_val, longitude_val, altitude_val);
|
||||
}
|
||||
|
||||
Accelerations(const Vector3r& linear_val, const Vector3r& angular_val)
|
||||
: linear(linear_val), angular(angular_val)
|
||||
void set(double latitude_val, double longitude_val, float altitude_val)
|
||||
{
|
||||
latitude = latitude_val, longitude = longitude_val;
|
||||
altitude = altitude_val;
|
||||
}
|
||||
|
||||
friend std::ostream& operator<<(std::ostream& os, GeoPoint const& g)
|
||||
{
|
||||
return os << "[" << g.latitude << ", " << g.longitude << ", " << g.altitude << "]";
|
||||
}
|
||||
|
||||
std::string to_string() const
|
||||
{
|
||||
return std::to_string(latitude) + string(", ") + std::to_string(longitude) + string(", ") + std::to_string(altitude);
|
||||
}
|
||||
};
|
||||
|
||||
struct HomeGeoPoint
|
||||
{
|
||||
}
|
||||
GeoPoint home_geo_point;
|
||||
double lat_rad, lon_rad;
|
||||
double cos_lat, sin_lat;
|
||||
|
||||
static const Accelerations zero()
|
||||
HomeGeoPoint()
|
||||
{
|
||||
}
|
||||
HomeGeoPoint(const GeoPoint& home_geo_point_val)
|
||||
{
|
||||
initialize(home_geo_point_val);
|
||||
}
|
||||
void initialize(const GeoPoint& home_geo_point_val)
|
||||
{
|
||||
home_geo_point = home_geo_point_val;
|
||||
lat_rad = Utils::degreesToRadians(home_geo_point.latitude);
|
||||
lon_rad = Utils::degreesToRadians(home_geo_point.longitude);
|
||||
cos_lat = cos(lat_rad);
|
||||
sin_lat = sin(lat_rad);
|
||||
}
|
||||
};
|
||||
|
||||
struct ProjectionMatrix
|
||||
{
|
||||
static const Accelerations zero_val(Vector3r::Zero(), Vector3r::Zero());
|
||||
return zero_val;
|
||||
}
|
||||
};
|
||||
float matrix[4][4];
|
||||
|
||||
struct PoseWithCovariance {
|
||||
VectorMath::Pose pose;
|
||||
vector<real_T> covariance; //36 elements, 6x6 matrix
|
||||
void setTo(float val)
|
||||
{
|
||||
for (auto i = 0; i < 4; ++i)
|
||||
for (auto j = 0; j < 4; ++j)
|
||||
matrix[i][j] = val;
|
||||
}
|
||||
};
|
||||
|
||||
PoseWithCovariance()
|
||||
: covariance(36, 0)
|
||||
{}
|
||||
};
|
||||
|
||||
struct PowerSupply {
|
||||
vector<real_T> voltage, current;
|
||||
};
|
||||
|
||||
struct TwistWithCovariance {
|
||||
Twist twist;
|
||||
vector<real_T> covariance; //36 elements, 6x6 matrix
|
||||
|
||||
TwistWithCovariance()
|
||||
: covariance(36, 0)
|
||||
{}
|
||||
};
|
||||
|
||||
struct Joystick {
|
||||
vector<float> axes;
|
||||
vector<int> buttons;
|
||||
};
|
||||
|
||||
struct Odometry {
|
||||
PoseWithCovariance pose;
|
||||
TwistWithCovariance twist;
|
||||
};
|
||||
|
||||
struct GeoPoint {
|
||||
double latitude = 0, longitude = 0;
|
||||
float altitude = 0;
|
||||
|
||||
GeoPoint()
|
||||
{}
|
||||
|
||||
GeoPoint(double latitude_val, double longitude_val, float altitude_val)
|
||||
struct CollisionInfo
|
||||
{
|
||||
set(latitude_val, longitude_val, altitude_val);
|
||||
}
|
||||
bool has_collided = false;
|
||||
Vector3r normal = Vector3r::Zero();
|
||||
Vector3r impact_point = Vector3r::Zero();
|
||||
Vector3r position = Vector3r::Zero();
|
||||
real_T penetration_depth = 0;
|
||||
TTimePoint time_stamp = 0;
|
||||
unsigned int collision_count = 0;
|
||||
std::string object_name;
|
||||
int object_id = -1;
|
||||
|
||||
void set(double latitude_val, double longitude_val, float altitude_val)
|
||||
CollisionInfo()
|
||||
{
|
||||
}
|
||||
|
||||
CollisionInfo(bool has_collided_val, const Vector3r& normal_val,
|
||||
const Vector3r& impact_point_val, const Vector3r& position_val,
|
||||
real_T penetration_depth_val, TTimePoint time_stamp_val,
|
||||
const std::string& object_name_val, int object_id_val)
|
||||
: has_collided(has_collided_val), normal(normal_val), impact_point(impact_point_val), position(position_val), penetration_depth(penetration_depth_val), time_stamp(time_stamp_val), object_name(object_name_val), object_id(object_id_val)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
struct CameraInfo
|
||||
{
|
||||
latitude = latitude_val, longitude = longitude_val; altitude = altitude_val;
|
||||
}
|
||||
Pose pose;
|
||||
float fov;
|
||||
ProjectionMatrix proj_mat;
|
||||
|
||||
friend std::ostream& operator<<(std::ostream &os, GeoPoint const &g) {
|
||||
return os << "[" << g.latitude << ", " << g.longitude << ", " << g.altitude << "]";
|
||||
}
|
||||
CameraInfo()
|
||||
{
|
||||
}
|
||||
|
||||
std::string to_string() const
|
||||
CameraInfo(const Pose& pose_val, float fov_val, const ProjectionMatrix& proj_mat_val)
|
||||
: pose(pose_val), fov(fov_val), proj_mat(proj_mat_val)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
struct CollisionResponse
|
||||
{
|
||||
return std::to_string(latitude) + string(", ") + std::to_string(longitude) + string(", ") + std::to_string(altitude);
|
||||
}
|
||||
};
|
||||
unsigned int collision_count_raw = 0;
|
||||
unsigned int collision_count_non_resting = 0;
|
||||
TTimePoint collision_time_stamp = 0;
|
||||
};
|
||||
|
||||
struct HomeGeoPoint {
|
||||
GeoPoint home_geo_point;
|
||||
double lat_rad, lon_rad;
|
||||
double cos_lat, sin_lat;
|
||||
|
||||
HomeGeoPoint()
|
||||
{}
|
||||
HomeGeoPoint(const GeoPoint& home_geo_point_val)
|
||||
struct GeoPose
|
||||
{
|
||||
initialize(home_geo_point_val);
|
||||
}
|
||||
void initialize(const GeoPoint& home_geo_point_val)
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
Quaternionr orientation;
|
||||
GeoPoint position;
|
||||
};
|
||||
|
||||
struct RCData
|
||||
{
|
||||
home_geo_point = home_geo_point_val;
|
||||
lat_rad = Utils::degreesToRadians(home_geo_point.latitude);
|
||||
lon_rad = Utils::degreesToRadians(home_geo_point.longitude);
|
||||
cos_lat = cos(lat_rad);
|
||||
sin_lat = sin(lat_rad);
|
||||
}
|
||||
};
|
||||
TTimePoint timestamp = 0;
|
||||
//pitch, roll, yaw should be in range -1 to 1
|
||||
//switches should be integer value indicating its state, 0=on, 1=off for example.
|
||||
float pitch = 0, roll = 0, throttle = 0, yaw = 0;
|
||||
float left_z = 0, right_z = 0;
|
||||
uint16_t switches = 0;
|
||||
std::string vendor_id = "";
|
||||
bool is_initialized = false; //is RC connected?
|
||||
bool is_valid = false; //must be true for data to be valid
|
||||
|
||||
struct ProjectionMatrix {
|
||||
float matrix[4][4];
|
||||
unsigned int getSwitch(uint16_t index) const
|
||||
{
|
||||
return switches & (1 << index) ? 1 : 0;
|
||||
}
|
||||
|
||||
void setTo(float val)
|
||||
void add(const RCData& other)
|
||||
{
|
||||
pitch += other.pitch;
|
||||
roll += other.roll;
|
||||
throttle += other.throttle;
|
||||
yaw += other.yaw;
|
||||
}
|
||||
void subtract(const RCData& other)
|
||||
{
|
||||
pitch -= other.pitch;
|
||||
roll -= other.roll;
|
||||
throttle -= other.throttle;
|
||||
yaw -= other.yaw;
|
||||
}
|
||||
void divideBy(float k)
|
||||
{
|
||||
pitch /= k;
|
||||
roll /= k;
|
||||
throttle /= k;
|
||||
yaw /= k;
|
||||
}
|
||||
bool isAnyMoreThan(float k)
|
||||
{
|
||||
using std::abs;
|
||||
return abs(pitch) > k || abs(roll) > k || abs(throttle) > k || abs(yaw) > k;
|
||||
}
|
||||
string toString()
|
||||
{
|
||||
return Utils::stringf("RCData[pitch=%f, roll=%f, throttle=%f, yaw=%f]", pitch, roll, throttle, yaw);
|
||||
}
|
||||
};
|
||||
|
||||
struct LidarData
|
||||
{
|
||||
for (auto i = 0; i < 4; ++i)
|
||||
for (auto j = 0; j < 4; ++j)
|
||||
matrix[i][j] = val;
|
||||
}
|
||||
};
|
||||
TTimePoint time_stamp = 0;
|
||||
// data
|
||||
// - array of floats that represent [x,y,z] coordinate for each point hit within the range
|
||||
// x0, y0, z0, x1, y1, z1, ..., xn, yn, zn
|
||||
// TODO: Do we need an intensity place-holder [x,y,z, intensity]?
|
||||
// - in lidar local NED coordinates
|
||||
// - in meters
|
||||
vector<real_T> point_cloud;
|
||||
Pose pose;
|
||||
vector<int> segmentation;
|
||||
|
||||
struct CollisionInfo {
|
||||
bool has_collided = false;
|
||||
Vector3r normal = Vector3r::Zero();
|
||||
Vector3r impact_point = Vector3r::Zero();
|
||||
Vector3r position = Vector3r::Zero();
|
||||
real_T penetration_depth = 0;
|
||||
TTimePoint time_stamp = 0;
|
||||
unsigned int collision_count = 0;
|
||||
std::string object_name;
|
||||
int object_id = -1;
|
||||
LidarData()
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
CollisionInfo()
|
||||
{}
|
||||
|
||||
CollisionInfo(bool has_collided_val, const Vector3r& normal_val,
|
||||
const Vector3r& impact_point_val, const Vector3r& position_val,
|
||||
real_T penetration_depth_val, TTimePoint time_stamp_val,
|
||||
const std::string& object_name_val, int object_id_val)
|
||||
: has_collided(has_collided_val), normal(normal_val),
|
||||
impact_point(impact_point_val), position(position_val),
|
||||
penetration_depth(penetration_depth_val), time_stamp(time_stamp_val),
|
||||
object_name(object_name_val), object_id(object_id_val)
|
||||
struct DistanceSensorData
|
||||
{
|
||||
}
|
||||
};
|
||||
TTimePoint time_stamp;
|
||||
real_T distance; //meters
|
||||
real_T min_distance; //m
|
||||
real_T max_distance; //m
|
||||
Pose relative_pose;
|
||||
|
||||
struct CameraInfo {
|
||||
Pose pose;
|
||||
float fov;
|
||||
ProjectionMatrix proj_mat;
|
||||
DistanceSensorData()
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
CameraInfo()
|
||||
{}
|
||||
|
||||
CameraInfo(const Pose& pose_val, float fov_val, const ProjectionMatrix& proj_mat_val)
|
||||
: pose(pose_val), fov(fov_val), proj_mat(proj_mat_val)
|
||||
struct MeshPositionVertexBuffersResponse
|
||||
{
|
||||
}
|
||||
};
|
||||
Vector3r position;
|
||||
Quaternionr orientation;
|
||||
|
||||
struct CollisionResponse {
|
||||
unsigned int collision_count_raw = 0;
|
||||
unsigned int collision_count_non_resting = 0;
|
||||
TTimePoint collision_time_stamp = 0;
|
||||
};
|
||||
|
||||
struct GeoPose {
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
Quaternionr orientation;
|
||||
GeoPoint position;
|
||||
};
|
||||
|
||||
struct RCData {
|
||||
TTimePoint timestamp = 0;
|
||||
//pitch, roll, yaw should be in range -1 to 1
|
||||
//switches should be integer value indicating its state, 0=on, 1=off for example.
|
||||
float pitch = 0, roll = 0, throttle = 0, yaw = 0;
|
||||
float left_z = 0, right_z = 0;
|
||||
uint16_t switches = 0;
|
||||
std::string vendor_id = "";
|
||||
bool is_initialized = false; //is RC connected?
|
||||
bool is_valid = false; //must be true for data to be valid
|
||||
|
||||
unsigned int getSwitch(uint16_t index) const
|
||||
{
|
||||
return switches & (1 << index) ? 1 : 0;
|
||||
}
|
||||
|
||||
void add(const RCData& other)
|
||||
{
|
||||
pitch += other.pitch; roll += other.roll; throttle += other.throttle; yaw += other.yaw;
|
||||
}
|
||||
void subtract(const RCData& other)
|
||||
{
|
||||
pitch -= other.pitch; roll -= other.roll; throttle -= other.throttle; yaw -= other.yaw;
|
||||
}
|
||||
void divideBy(float k)
|
||||
{
|
||||
pitch /= k; roll /= k; throttle /= k; yaw /= k;
|
||||
}
|
||||
bool isAnyMoreThan(float k)
|
||||
{
|
||||
using std::abs;
|
||||
return abs(pitch) > k || abs(roll) > k || abs(throttle) > k || abs(yaw) > k;
|
||||
}
|
||||
string toString()
|
||||
{
|
||||
return Utils::stringf("RCData[pitch=%f, roll=%f, throttle=%f, yaw=%f]", pitch, roll, throttle, yaw);
|
||||
}
|
||||
};
|
||||
|
||||
struct LidarData {
|
||||
TTimePoint time_stamp = 0;
|
||||
// data
|
||||
// - array of floats that represent [x,y,z] coordinate for each point hit within the range
|
||||
// x0, y0, z0, x1, y1, z1, ..., xn, yn, zn
|
||||
// TODO: Do we need an intensity place-holder [x,y,z, intensity]?
|
||||
// - in lidar local NED coordinates
|
||||
// - in meters
|
||||
vector<real_T> point_cloud;
|
||||
Pose pose;
|
||||
vector<int> segmentation;
|
||||
|
||||
LidarData()
|
||||
{}
|
||||
};
|
||||
|
||||
struct DistanceSensorData {
|
||||
TTimePoint time_stamp;
|
||||
real_T distance; //meters
|
||||
real_T min_distance; //m
|
||||
real_T max_distance; //m
|
||||
Pose relative_pose;
|
||||
|
||||
DistanceSensorData()
|
||||
{}
|
||||
};
|
||||
|
||||
struct MeshPositionVertexBuffersResponse {
|
||||
Vector3r position;
|
||||
Quaternionr orientation;
|
||||
|
||||
std::vector<float> vertices;
|
||||
std::vector<uint32_t> indices;
|
||||
std::string name;
|
||||
};
|
||||
|
||||
}} //namespace
|
||||
std::vector<float> vertices;
|
||||
std::vector<uint32_t> indices;
|
||||
std::string name;
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -8,82 +8,86 @@
|
|||
#include "UpdatableObject.hpp"
|
||||
#include <list>
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
template<typename T>
|
||||
class DelayLine : public UpdatableObject {
|
||||
public:
|
||||
DelayLine()
|
||||
{}
|
||||
DelayLine(TTimeDelta delay) //in seconds
|
||||
template <typename T>
|
||||
class DelayLine : public UpdatableObject
|
||||
{
|
||||
initialize(delay);
|
||||
}
|
||||
void initialize(TTimeDelta delay) //in seconds
|
||||
{
|
||||
setDelay(delay);
|
||||
}
|
||||
void setDelay(TTimeDelta delay)
|
||||
{
|
||||
delay_ = delay;
|
||||
}
|
||||
double getDelay() const
|
||||
{
|
||||
return delay_;
|
||||
}
|
||||
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
values_.clear();
|
||||
times_.clear();
|
||||
last_time_ = 0;
|
||||
last_value_ = T();
|
||||
}
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
UpdatableObject::update();
|
||||
|
||||
if (!times_.empty() &&
|
||||
ClockBase::elapsedBetween(clock()->nowNanos(), times_.front()) >= delay_) {
|
||||
|
||||
last_value_ = values_.front();
|
||||
last_time_ = times_.front();
|
||||
|
||||
times_.pop_front();
|
||||
values_.pop_front();
|
||||
public:
|
||||
DelayLine()
|
||||
{
|
||||
}
|
||||
DelayLine(TTimeDelta delay) //in seconds
|
||||
{
|
||||
initialize(delay);
|
||||
}
|
||||
void initialize(TTimeDelta delay) //in seconds
|
||||
{
|
||||
setDelay(delay);
|
||||
}
|
||||
void setDelay(TTimeDelta delay)
|
||||
{
|
||||
delay_ = delay;
|
||||
}
|
||||
double getDelay() const
|
||||
{
|
||||
return delay_;
|
||||
}
|
||||
}
|
||||
//*** End: UpdatableState implementation ***//
|
||||
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
values_.clear();
|
||||
times_.clear();
|
||||
last_time_ = 0;
|
||||
last_value_ = T();
|
||||
}
|
||||
|
||||
T getOutput() const
|
||||
{
|
||||
return last_value_;
|
||||
}
|
||||
double getOutputTime() const
|
||||
{
|
||||
return last_time_;
|
||||
}
|
||||
virtual void update() override
|
||||
{
|
||||
UpdatableObject::update();
|
||||
|
||||
void push_back(const T& val, TTimePoint time_offset = 0)
|
||||
{
|
||||
values_.push_back(val);
|
||||
times_.push_back(clock()->nowNanos() + time_offset);
|
||||
}
|
||||
if (!times_.empty() &&
|
||||
ClockBase::elapsedBetween(clock()->nowNanos(), times_.front()) >= delay_) {
|
||||
|
||||
private:
|
||||
template<typename TItem>
|
||||
using list = std::list<TItem>;
|
||||
last_value_ = values_.front();
|
||||
last_time_ = times_.front();
|
||||
|
||||
list<T> values_;
|
||||
list<TTimePoint> times_;
|
||||
TTimeDelta delay_;
|
||||
times_.pop_front();
|
||||
values_.pop_front();
|
||||
}
|
||||
}
|
||||
//*** End: UpdatableState implementation ***//
|
||||
|
||||
T last_value_;
|
||||
TTimePoint last_time_;
|
||||
};
|
||||
T getOutput() const
|
||||
{
|
||||
return last_value_;
|
||||
}
|
||||
double getOutputTime() const
|
||||
{
|
||||
return last_time_;
|
||||
}
|
||||
|
||||
}} //namespace
|
||||
void push_back(const T& val, TTimePoint time_offset = 0)
|
||||
{
|
||||
values_.push_back(val);
|
||||
times_.push_back(clock()->nowNanos() + time_offset);
|
||||
}
|
||||
|
||||
private:
|
||||
template <typename TItem>
|
||||
using list = std::list<TItem>;
|
||||
|
||||
list<T> values_;
|
||||
list<TTimePoint> times_;
|
||||
TTimeDelta delay_;
|
||||
|
||||
T last_value_;
|
||||
TTimePoint last_time_;
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -6,204 +6,197 @@ https://github.com/mourner/suncalc
|
|||
#ifndef airsim_core_EarthCelestial_hpp
|
||||
#define airsim_core_EarthCelestial_hpp
|
||||
|
||||
|
||||
#include "common/Common.hpp"
|
||||
#include "EarthUtils.hpp"
|
||||
#include <chrono>
|
||||
#include <ctime>
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
|
||||
class EarthCelestial {
|
||||
public:
|
||||
|
||||
struct CelestialGlobalCoord
|
||||
class EarthCelestial
|
||||
{
|
||||
double declination;
|
||||
double rightAscension;
|
||||
double distance = Utils::nan<double>();
|
||||
double parallacticAngle = Utils::nan<double>();
|
||||
public:
|
||||
struct CelestialGlobalCoord
|
||||
{
|
||||
double declination;
|
||||
double rightAscension;
|
||||
double distance = Utils::nan<double>();
|
||||
double parallacticAngle = Utils::nan<double>();
|
||||
};
|
||||
|
||||
struct CelestialLocalCoord
|
||||
{
|
||||
double azimuth;
|
||||
double altitude;
|
||||
double distance = Utils::nan<double>();
|
||||
double parallacticAngle = Utils::nan<double>();
|
||||
};
|
||||
|
||||
struct CelestialPhase
|
||||
{
|
||||
double fraction;
|
||||
double phase;
|
||||
double angle;
|
||||
};
|
||||
|
||||
public:
|
||||
static CelestialLocalCoord getSunCoordinates(uint64_t date, double lat, double lng)
|
||||
{
|
||||
double lw = Utils::degreesToRadians(-lng);
|
||||
double phi = Utils::degreesToRadians(lat);
|
||||
double d = toDays(date);
|
||||
|
||||
CelestialGlobalCoord c = getGlobalSunCoords(d);
|
||||
double H = siderealTime(d, lw) - c.rightAscension;
|
||||
|
||||
CelestialLocalCoord coord;
|
||||
coord.azimuth = Utils::radiansToDegrees(azimuth(H, phi, c.declination)) + 180.0;
|
||||
coord.altitude = Utils::radiansToDegrees(altitude(H, phi, c.declination));
|
||||
|
||||
return coord;
|
||||
}
|
||||
|
||||
static CelestialLocalCoord getMoonCoordinates(uint64_t date, double lat, double lng)
|
||||
{
|
||||
|
||||
double lw = Utils::degreesToRadians(-lng);
|
||||
double phi = Utils::degreesToRadians(lat);
|
||||
double d = toDays(date);
|
||||
|
||||
CelestialGlobalCoord c = getGlobalMoonCoords(d);
|
||||
double H = siderealTime(d, lw) - c.rightAscension;
|
||||
|
||||
// formula 14.1 of "Astronomical Algorithms" 2nd edition by Jean Meeus (Willmann-Bell, Richmond) 1998.
|
||||
double pa = std::atan2(std::sin(H), std::tan(phi) * std::cos(c.declination) - std::sin(c.declination) * std::cos(H));
|
||||
|
||||
double h = altitude(H, phi, c.declination);
|
||||
h = h + astroRefraction(h); // altitude correction for refraction
|
||||
|
||||
CelestialLocalCoord coord;
|
||||
coord.azimuth = Utils::radiansToDegrees(azimuth(H, phi, c.declination));
|
||||
coord.altitude = Utils::radiansToDegrees(h);
|
||||
coord.distance = c.distance;
|
||||
coord.parallacticAngle = Utils::radiansToDegrees(pa);
|
||||
return coord;
|
||||
};
|
||||
|
||||
// calculations for illumination parameters of the moon,
|
||||
// based on http://idlastro.gsfc.nasa.gov/ftp/pro/astro/mphase.pro formulas and
|
||||
// Chapter 48 of "Astronomical Algorithms" 2nd edition by Jean Meeus (Willmann-Bell, Richmond) 1998.
|
||||
static CelestialPhase getMoonPhase(uint64_t date)
|
||||
{
|
||||
double d = toDays(date);
|
||||
CelestialGlobalCoord s = getGlobalSunCoords(d);
|
||||
CelestialGlobalCoord m = getGlobalMoonCoords(d);
|
||||
|
||||
double sdist = EarthUtils::DistanceFromSun / 1000; // distance from Earth to Sun in km
|
||||
|
||||
double phi = std::acos(std::sin(s.declination) * std::sin(m.declination) + std::cos(s.declination) * std::cos(m.declination) * std::cos(s.rightAscension - m.rightAscension));
|
||||
double inc = std::atan2(sdist * std::sin(phi), m.distance - sdist * std::cos(phi));
|
||||
double angle = std::atan2(std::cos(s.declination) * std::sin(s.rightAscension - m.rightAscension), std::sin(s.declination) * std::cos(m.declination) - std::cos(s.declination) * std::sin(m.declination) * std::cos(s.rightAscension - m.rightAscension));
|
||||
|
||||
CelestialPhase moonPhase;
|
||||
moonPhase.fraction = (1 + cos(inc)) / 2;
|
||||
moonPhase.phase = 0.5 + 0.5 * inc * (angle < 0 ? -1 : 1) / M_PI;
|
||||
moonPhase.angle = angle;
|
||||
return moonPhase;
|
||||
};
|
||||
|
||||
private:
|
||||
static double toDays(uint64_t date)
|
||||
{
|
||||
static constexpr double kJulianDaysOnY2000 = 2451545;
|
||||
static constexpr double kDaysToHours = 60 * 60 * 24;
|
||||
static constexpr double kJulianDaysOnEpoch = 2440588;
|
||||
|
||||
double julian_days = date / kDaysToHours - 0.5 + kJulianDaysOnEpoch;
|
||||
;
|
||||
return julian_days - kJulianDaysOnY2000;
|
||||
}
|
||||
|
||||
static double rightAscension(double l, double b)
|
||||
{
|
||||
return std::atan2(std::sin(l) * std::cos(EarthUtils::Obliquity) - std::tan(b) * std::sin(EarthUtils::Obliquity), std::cos(l));
|
||||
}
|
||||
|
||||
static double declination(double l, double b)
|
||||
{
|
||||
return std::asin(std::sin(b) * std::cos(EarthUtils::Obliquity) + std::cos(b) * std::sin(EarthUtils::Obliquity) * std::sin(l));
|
||||
}
|
||||
|
||||
static double azimuth(double H, double phi, double declination)
|
||||
{
|
||||
return std::atan2(std::sin(H), std::cos(H) * std::sin(phi) - std::tan(declination) * std::cos(phi));
|
||||
}
|
||||
|
||||
static double altitude(double H, double phi, double declination)
|
||||
{
|
||||
return std::asin(std::sin(phi) * std::sin(declination) + std::cos(phi) * std::cos(declination) * std::cos(H));
|
||||
}
|
||||
|
||||
static double siderealTime(double d, double lw)
|
||||
{
|
||||
return Utils::degreesToRadians((280.16 + 360.9856235 * d)) - lw;
|
||||
}
|
||||
|
||||
static double astroRefraction(double h)
|
||||
{
|
||||
if (h < 0) // the following formula works for positive altitudes only.
|
||||
h = 0; // if h = -0.08901179 a div/0 would occur.
|
||||
|
||||
// formula 16.4 of "Astronomical Algorithms" 2nd edition by Jean Meeus (Willmann-Bell, Richmond) 1998.
|
||||
// 1.02 / tan(h + 10.26 / (h + 5.10)) h in degrees, result in arc minutes -> converted to rad:
|
||||
return 0.0002967 / std::tan(h + 0.00312536 / (h + 0.08901179));
|
||||
}
|
||||
|
||||
static double solarMeanAnomaly(double d)
|
||||
{
|
||||
return Utils::degreesToRadians((357.5291 + 0.98560028 * d));
|
||||
}
|
||||
|
||||
static double eclipticLongitude(double M)
|
||||
{
|
||||
double C = Utils::degreesToRadians((1.9148 * std::sin(M) + 0.02 * std::sin(2 * M) + 0.0003 * std::sin(3 * M))); // equation of center
|
||||
|
||||
return M + C + EarthUtils::Perihelion + M_PI;
|
||||
}
|
||||
|
||||
static CelestialGlobalCoord getGlobalSunCoords(double d)
|
||||
{
|
||||
double M = solarMeanAnomaly(d);
|
||||
double L = eclipticLongitude(M);
|
||||
|
||||
CelestialGlobalCoord sunCoords;
|
||||
sunCoords.declination = declination(L, 0);
|
||||
sunCoords.rightAscension = rightAscension(L, 0);
|
||||
|
||||
return sunCoords;
|
||||
}
|
||||
|
||||
// moon calculations, based on http://aa.quae.nl/en/reken/hemelpositie.html formulas
|
||||
static CelestialGlobalCoord getGlobalMoonCoords(double d)
|
||||
{
|
||||
// geocentric ecliptic coordinates of the moon
|
||||
|
||||
double L = Utils::degreesToRadians((218.316 + 13.176396 * d)); // ecliptic longitude
|
||||
double M = Utils::degreesToRadians((134.963 + 13.064993 * d)); // mean anomaly
|
||||
double F = Utils::degreesToRadians((93.272 + 13.229350 * d)); // mean distance
|
||||
|
||||
double l = L + Utils::degreesToRadians(6.289 * std::sin(M)); // longitude
|
||||
double b = Utils::degreesToRadians(5.128 * std::sin(F)); // latitude
|
||||
double dt = 385001 - 20905 * std::cos(M); // distance to the moon in km
|
||||
|
||||
CelestialGlobalCoord moonCoords;
|
||||
moonCoords.rightAscension = rightAscension(l, b);
|
||||
moonCoords.declination = declination(l, b);
|
||||
moonCoords.distance = dt;
|
||||
|
||||
return moonCoords;
|
||||
}
|
||||
};
|
||||
|
||||
struct CelestialLocalCoord
|
||||
{
|
||||
double azimuth;
|
||||
double altitude;
|
||||
double distance = Utils::nan<double>();
|
||||
double parallacticAngle = Utils::nan<double>();
|
||||
};
|
||||
|
||||
struct CelestialPhase
|
||||
{
|
||||
double fraction;
|
||||
double phase;
|
||||
double angle;
|
||||
};
|
||||
|
||||
|
||||
public:
|
||||
static CelestialLocalCoord getSunCoordinates(uint64_t date, double lat, double lng)
|
||||
{
|
||||
double lw = Utils::degreesToRadians(-lng);
|
||||
double phi = Utils::degreesToRadians(lat);
|
||||
double d = toDays(date);
|
||||
|
||||
CelestialGlobalCoord c = getGlobalSunCoords(d);
|
||||
double H = siderealTime(d, lw) - c.rightAscension;
|
||||
|
||||
CelestialLocalCoord coord;
|
||||
coord.azimuth = Utils::radiansToDegrees( azimuth(H, phi, c.declination) ) + 180.0;
|
||||
coord.altitude = Utils::radiansToDegrees( altitude(H, phi, c.declination) );
|
||||
|
||||
return coord;
|
||||
}
|
||||
|
||||
|
||||
static CelestialLocalCoord getMoonCoordinates(uint64_t date, double lat, double lng)
|
||||
{
|
||||
|
||||
double lw = Utils::degreesToRadians(-lng);
|
||||
double phi = Utils::degreesToRadians(lat);
|
||||
double d = toDays(date);
|
||||
|
||||
CelestialGlobalCoord c = getGlobalMoonCoords(d);
|
||||
double H = siderealTime(d, lw) - c.rightAscension;
|
||||
|
||||
// formula 14.1 of "Astronomical Algorithms" 2nd edition by Jean Meeus (Willmann-Bell, Richmond) 1998.
|
||||
double pa = std::atan2(std::sin(H), std::tan(phi) * std::cos(c.declination) - std::sin(c.declination) * std::cos(H));
|
||||
|
||||
double h = altitude(H, phi, c.declination);
|
||||
h = h + astroRefraction(h); // altitude correction for refraction
|
||||
|
||||
CelestialLocalCoord coord;
|
||||
coord.azimuth = Utils::radiansToDegrees( azimuth(H, phi, c.declination) );
|
||||
coord.altitude = Utils::radiansToDegrees(h);
|
||||
coord.distance = c.distance;
|
||||
coord.parallacticAngle = Utils::radiansToDegrees(pa);
|
||||
return coord;
|
||||
};
|
||||
|
||||
|
||||
// calculations for illumination parameters of the moon,
|
||||
// based on http://idlastro.gsfc.nasa.gov/ftp/pro/astro/mphase.pro formulas and
|
||||
// Chapter 48 of "Astronomical Algorithms" 2nd edition by Jean Meeus (Willmann-Bell, Richmond) 1998.
|
||||
static CelestialPhase getMoonPhase(uint64_t date)
|
||||
{
|
||||
double d = toDays(date);
|
||||
CelestialGlobalCoord s = getGlobalSunCoords(d);
|
||||
CelestialGlobalCoord m = getGlobalMoonCoords(d);
|
||||
|
||||
double sdist = EarthUtils::DistanceFromSun / 1000; // distance from Earth to Sun in km
|
||||
|
||||
double phi = std::acos(std::sin(s.declination) * std::sin(m.declination) + std::cos(s.declination) * std::cos(m.declination) * std::cos(s.rightAscension - m.rightAscension));
|
||||
double inc = std::atan2(sdist * std::sin(phi), m.distance - sdist * std::cos(phi));
|
||||
double angle = std::atan2(std::cos(s.declination) * std::sin(s.rightAscension - m.rightAscension), std::sin(s.declination) * std::cos(m.declination) - std::cos(s.declination) * std::sin(m.declination) * std::cos(s.rightAscension - m.rightAscension));
|
||||
|
||||
CelestialPhase moonPhase;
|
||||
moonPhase.fraction = (1 + cos(inc)) / 2;
|
||||
moonPhase.phase = 0.5 + 0.5 * inc * (angle < 0 ? -1 : 1) / M_PI;
|
||||
moonPhase.angle = angle;
|
||||
return moonPhase;
|
||||
};
|
||||
|
||||
|
||||
private:
|
||||
|
||||
static double toDays(uint64_t date)
|
||||
{
|
||||
static constexpr double kJulianDaysOnY2000 = 2451545;
|
||||
static constexpr double kDaysToHours = 60 * 60 * 24;
|
||||
static constexpr double kJulianDaysOnEpoch = 2440588;
|
||||
|
||||
double julian_days = date / kDaysToHours - 0.5 + kJulianDaysOnEpoch;;
|
||||
return julian_days - kJulianDaysOnY2000;
|
||||
}
|
||||
|
||||
|
||||
static double rightAscension(double l, double b)
|
||||
{
|
||||
return std::atan2(std::sin(l) * std::cos(EarthUtils::Obliquity) - std::tan(b) * std::sin(EarthUtils::Obliquity), std::cos(l));
|
||||
}
|
||||
|
||||
static double declination(double l, double b)
|
||||
{
|
||||
return std::asin(std::sin(b) * std::cos(EarthUtils::Obliquity) + std::cos(b) * std::sin(EarthUtils::Obliquity) * std::sin(l));
|
||||
}
|
||||
|
||||
static double azimuth(double H, double phi, double declination)
|
||||
{
|
||||
return std::atan2(std::sin(H), std::cos(H) * std::sin(phi) - std::tan(declination) * std::cos(phi));
|
||||
}
|
||||
|
||||
static double altitude(double H, double phi, double declination)
|
||||
{
|
||||
return std::asin(std::sin(phi) * std::sin(declination) + std::cos(phi) * std::cos(declination) * std::cos(H));
|
||||
}
|
||||
|
||||
static double siderealTime(double d, double lw)
|
||||
{
|
||||
return Utils::degreesToRadians((280.16 + 360.9856235 * d)) - lw;
|
||||
}
|
||||
|
||||
static double astroRefraction(double h)
|
||||
{
|
||||
if (h < 0) // the following formula works for positive altitudes only.
|
||||
h = 0; // if h = -0.08901179 a div/0 would occur.
|
||||
|
||||
// formula 16.4 of "Astronomical Algorithms" 2nd edition by Jean Meeus (Willmann-Bell, Richmond) 1998.
|
||||
// 1.02 / tan(h + 10.26 / (h + 5.10)) h in degrees, result in arc minutes -> converted to rad:
|
||||
return 0.0002967 / std::tan(h + 0.00312536 / (h + 0.08901179));
|
||||
}
|
||||
|
||||
|
||||
static double solarMeanAnomaly(double d)
|
||||
{
|
||||
return Utils::degreesToRadians((357.5291 + 0.98560028 * d));
|
||||
}
|
||||
|
||||
static double eclipticLongitude(double M)
|
||||
{
|
||||
double C = Utils::degreesToRadians((1.9148 * std::sin(M) + 0.02 * std::sin(2 * M) + 0.0003 * std::sin(3 * M))); // equation of center
|
||||
|
||||
return M + C + EarthUtils::Perihelion + M_PI;
|
||||
}
|
||||
|
||||
static CelestialGlobalCoord getGlobalSunCoords(double d)
|
||||
{
|
||||
double M = solarMeanAnomaly(d);
|
||||
double L = eclipticLongitude(M);
|
||||
|
||||
CelestialGlobalCoord sunCoords;
|
||||
sunCoords.declination = declination(L, 0);
|
||||
sunCoords.rightAscension = rightAscension(L, 0);
|
||||
|
||||
return sunCoords;
|
||||
}
|
||||
|
||||
|
||||
// moon calculations, based on http://aa.quae.nl/en/reken/hemelpositie.html formulas
|
||||
static CelestialGlobalCoord getGlobalMoonCoords(double d)
|
||||
{
|
||||
// geocentric ecliptic coordinates of the moon
|
||||
|
||||
double L = Utils::degreesToRadians((218.316 + 13.176396 * d)); // ecliptic longitude
|
||||
double M = Utils::degreesToRadians((134.963 + 13.064993 * d)); // mean anomaly
|
||||
double F = Utils::degreesToRadians((93.272 + 13.229350 * d)); // mean distance
|
||||
|
||||
double l = L + Utils::degreesToRadians(6.289 * std::sin(M)); // longitude
|
||||
double b = Utils::degreesToRadians(5.128 * std::sin(F)); // latitude
|
||||
double dt = 385001 - 20905 * std::cos(M); // distance to the moon in km
|
||||
|
||||
CelestialGlobalCoord moonCoords;
|
||||
moonCoords.rightAscension = rightAscension(l, b);
|
||||
moonCoords.declination = declination(l, b);
|
||||
moonCoords.distance = dt;
|
||||
|
||||
return moonCoords;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
}} //namespace
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -1,7 +1,6 @@
|
|||
// Copyright (c) Microsoft Corporation. All rights reserved.
|
||||
// Licensed under the MIT License.
|
||||
|
||||
|
||||
#ifndef air_Earth_hpp
|
||||
#define air_Earth_hpp
|
||||
|
||||
|
|
@ -10,202 +9,208 @@
|
|||
#include "common/CommonStructs.hpp"
|
||||
#include "common/Common.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
class EarthUtils {
|
||||
private:
|
||||
|
||||
/** set this always to the sampling in degrees for the table below */
|
||||
static constexpr int MAG_SAMPLING_RES = 10;
|
||||
static constexpr int MAG_SAMPLING_MIN_LAT = -60;
|
||||
static constexpr int MAG_SAMPLING_MAX_LAT = 60;
|
||||
static constexpr int MAG_SAMPLING_MIN_LON = -180;
|
||||
static constexpr int MAG_SAMPLING_MAX_LON = 180;
|
||||
|
||||
static constexpr int DECLINATION_TABLE[13][37] =
|
||||
class EarthUtils
|
||||
{
|
||||
{ 46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, -66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46 },
|
||||
{ 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, -3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, 29, 30 },
|
||||
{ 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, -40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21 },
|
||||
{ 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, 8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, 10, 13, 15, 16 },
|
||||
{ 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, -10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12 },
|
||||
{ 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, -14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10 },
|
||||
{ 9, 9, 9, 9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, 7, 8, 9 },
|
||||
{ 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, 0, 0, 0, 1, 3, 5, 7, 8 },
|
||||
{ 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, 0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8 },
|
||||
{ 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, 0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6 },
|
||||
{ 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, -16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5 },
|
||||
{ 4, 8, 12, 15, 17, 18, 16, 12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, 0, 4 },
|
||||
{ 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3 },
|
||||
};
|
||||
public:
|
||||
//return declination in degrees
|
||||
//Ref: https://github.com/PX4/ecl/blob/master/EKF/geo.cpp
|
||||
static real_T getMagDeclination(real_T latitude, real_T longitude)
|
||||
{
|
||||
/*
|
||||
private:
|
||||
/** set this always to the sampling in degrees for the table below */
|
||||
static constexpr int MAG_SAMPLING_RES = 10;
|
||||
static constexpr int MAG_SAMPLING_MIN_LAT = -60;
|
||||
static constexpr int MAG_SAMPLING_MAX_LAT = 60;
|
||||
static constexpr int MAG_SAMPLING_MIN_LON = -180;
|
||||
static constexpr int MAG_SAMPLING_MAX_LON = 180;
|
||||
|
||||
static constexpr int DECLINATION_TABLE[13][37] =
|
||||
{
|
||||
{ 46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, -66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46 },
|
||||
{ 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, -3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, 29, 30 },
|
||||
{ 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, -40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21 },
|
||||
{ 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, 8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, 10, 13, 15, 16 },
|
||||
{ 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, -10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12 },
|
||||
{ 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, -14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10 },
|
||||
{ 9, 9, 9, 9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, 7, 8, 9 },
|
||||
{ 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, 0, 0, 0, 1, 3, 5, 7, 8 },
|
||||
{ 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, 0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8 },
|
||||
{ 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, 0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6 },
|
||||
{ 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, -16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5 },
|
||||
{ 4, 8, 12, 15, 17, 18, 16, 12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, 0, 4 },
|
||||
{ 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3 },
|
||||
};
|
||||
|
||||
public:
|
||||
//return declination in degrees
|
||||
//Ref: https://github.com/PX4/ecl/blob/master/EKF/geo.cpp
|
||||
static real_T getMagDeclination(real_T latitude, real_T longitude)
|
||||
{
|
||||
/*
|
||||
* If the values exceed valid ranges, return zero as default
|
||||
* as we have no way of knowing what the closest real value
|
||||
* would be.
|
||||
*/
|
||||
if (latitude < -90.0f || latitude > 90.0f ||
|
||||
longitude < -180.0f || longitude > 180.0f) {
|
||||
throw std::out_of_range(Utils::stringf("invalid latitude (%f) or longitude (%f) value", latitude, longitude));
|
||||
}
|
||||
if (latitude < -90.0f || latitude > 90.0f ||
|
||||
longitude < -180.0f || longitude > 180.0f) {
|
||||
throw std::out_of_range(Utils::stringf("invalid latitude (%f) or longitude (%f) value", latitude, longitude));
|
||||
}
|
||||
|
||||
/* round down to nearest sampling resolution */
|
||||
int min_lat = static_cast<int>(latitude / MAG_SAMPLING_RES) * MAG_SAMPLING_RES;
|
||||
int min_lon = static_cast<int>(longitude / MAG_SAMPLING_RES) * MAG_SAMPLING_RES;
|
||||
/* round down to nearest sampling resolution */
|
||||
int min_lat = static_cast<int>(latitude / MAG_SAMPLING_RES) * MAG_SAMPLING_RES;
|
||||
int min_lon = static_cast<int>(longitude / MAG_SAMPLING_RES) * MAG_SAMPLING_RES;
|
||||
|
||||
/* for the rare case of hitting the bounds exactly
|
||||
/* for the rare case of hitting the bounds exactly
|
||||
* the rounding logic wouldn't fit, so enforce it.
|
||||
*/
|
||||
|
||||
/* limit to table bounds - required for maxima even when table spans full globe range */
|
||||
if (latitude <= MAG_SAMPLING_MIN_LAT) {
|
||||
min_lat = MAG_SAMPLING_MIN_LAT;
|
||||
/* limit to table bounds - required for maxima even when table spans full globe range */
|
||||
if (latitude <= MAG_SAMPLING_MIN_LAT) {
|
||||
min_lat = MAG_SAMPLING_MIN_LAT;
|
||||
}
|
||||
|
||||
if (latitude >= MAG_SAMPLING_MAX_LAT) {
|
||||
min_lat = static_cast<int>(latitude / MAG_SAMPLING_RES) * MAG_SAMPLING_RES - MAG_SAMPLING_RES;
|
||||
}
|
||||
|
||||
if (longitude <= MAG_SAMPLING_MIN_LON) {
|
||||
min_lon = MAG_SAMPLING_MIN_LON;
|
||||
}
|
||||
|
||||
if (longitude >= MAG_SAMPLING_MAX_LON) {
|
||||
min_lon = static_cast<int>(longitude / MAG_SAMPLING_RES) * MAG_SAMPLING_RES - MAG_SAMPLING_RES;
|
||||
}
|
||||
|
||||
/* find index of nearest low sampling point */
|
||||
int min_lat_index = (-(MAG_SAMPLING_MIN_LAT) + min_lat) / MAG_SAMPLING_RES;
|
||||
int min_lon_index = (-(MAG_SAMPLING_MIN_LON) + min_lon) / MAG_SAMPLING_RES;
|
||||
|
||||
real_T declination_sw = get_mag_lookup_table_val(min_lat_index, min_lon_index);
|
||||
real_T declination_se = get_mag_lookup_table_val(min_lat_index, min_lon_index + 1);
|
||||
real_T declination_ne = get_mag_lookup_table_val(min_lat_index + 1, min_lon_index + 1);
|
||||
real_T declination_nw = get_mag_lookup_table_val(min_lat_index + 1, min_lon_index);
|
||||
|
||||
/* perform bilinear interpolation on the four grid corners */
|
||||
|
||||
real_T declination_min = ((longitude - min_lon) / MAG_SAMPLING_RES) * (declination_se - declination_sw) + declination_sw;
|
||||
real_T declination_max = ((longitude - min_lon) / MAG_SAMPLING_RES) * (declination_ne - declination_nw) + declination_nw;
|
||||
|
||||
return ((latitude - min_lat) / MAG_SAMPLING_RES) * (declination_max - declination_min) + declination_min;
|
||||
}
|
||||
|
||||
if (latitude >= MAG_SAMPLING_MAX_LAT) {
|
||||
min_lat = static_cast<int>(latitude / MAG_SAMPLING_RES) * MAG_SAMPLING_RES - MAG_SAMPLING_RES;
|
||||
//geopot_height = Earth_radius * altitude / (Earth_radius + altitude) /// all in kilometers
|
||||
//temperature is in Kelvin = 273.15 + celcius
|
||||
static real_T getStandardTemperature(real_T geopot_height)
|
||||
{
|
||||
//standard atmospheric pressure
|
||||
//Below 51km: Practical Meteorology by Roland Stull, pg 12
|
||||
//Above 51km: http://www.braeunig.us/space/atmmodel.htm
|
||||
if (geopot_height <= 11) //troposphere
|
||||
return 288.15f - (6.5f * geopot_height);
|
||||
else if (geopot_height <= 20) //Staroshere starts
|
||||
return 216.65f;
|
||||
else if (geopot_height <= 32)
|
||||
return 196.65f + geopot_height;
|
||||
else if (geopot_height <= 47)
|
||||
return 228.65f + 2.8f * (geopot_height - 32);
|
||||
else if (geopot_height <= 51) //Mesosphere starts
|
||||
return 270.65f;
|
||||
else if (geopot_height <= 71)
|
||||
return 270.65f - 2.8f * (geopot_height - 51);
|
||||
else if (geopot_height <= 84.85f)
|
||||
return 214.65f - 2 * (geopot_height - 71);
|
||||
else
|
||||
return 3;
|
||||
//Thermospehere has high kinetic temperature (500c to 2000c) but temperature
|
||||
//as measured by thermometer would be very low because of almost vacuum
|
||||
//throw std::out_of_range("geopot_height must be less than 85km. Space domain is not supported yet!");
|
||||
}
|
||||
|
||||
if (longitude <= MAG_SAMPLING_MIN_LON) {
|
||||
min_lon = MAG_SAMPLING_MIN_LON;
|
||||
static real_T getGeopotential(real_T altitude_km)
|
||||
{
|
||||
static constexpr real_T radius_km = EARTH_RADIUS / 1000.0f;
|
||||
return radius_km * altitude_km / (radius_km + altitude_km);
|
||||
}
|
||||
|
||||
if (longitude >= MAG_SAMPLING_MAX_LON) {
|
||||
min_lon = static_cast<int>(longitude / MAG_SAMPLING_RES) * MAG_SAMPLING_RES - MAG_SAMPLING_RES;
|
||||
static real_T getStandardPressure(real_T altitude /* meters */) //return Pa
|
||||
{
|
||||
real_T geopot_height = getGeopotential(altitude / 1000.0f);
|
||||
|
||||
real_T t = getStandardTemperature(geopot_height);
|
||||
|
||||
return getStandardPressure(geopot_height, t);
|
||||
}
|
||||
|
||||
/* find index of nearest low sampling point */
|
||||
int min_lat_index = (-(MAG_SAMPLING_MIN_LAT) + min_lat) / MAG_SAMPLING_RES;
|
||||
int min_lon_index = (-(MAG_SAMPLING_MIN_LON) + min_lon) / MAG_SAMPLING_RES;
|
||||
static real_T getStandardPressure(real_T geopot_height, real_T std_temperature) //return Pa
|
||||
{
|
||||
//Below 51km: Practical Meteorology by Roland Stull, pg 12
|
||||
//Above 51km: http://www.braeunig.us/space/atmmodel.htm
|
||||
//Validation data: https://www.avs.org/AVS/files/c7/c7edaedb-95b2-438f-adfb-36de54f87b9e.pdf
|
||||
|
||||
real_T declination_sw = get_mag_lookup_table_val(min_lat_index, min_lon_index);
|
||||
real_T declination_se = get_mag_lookup_table_val(min_lat_index, min_lon_index + 1);
|
||||
real_T declination_ne = get_mag_lookup_table_val(min_lat_index + 1, min_lon_index + 1);
|
||||
real_T declination_nw = get_mag_lookup_table_val(min_lat_index + 1, min_lon_index);
|
||||
//TODO: handle -ve altitude better (shouldn't grow indefinitely!)
|
||||
|
||||
/* perform bilinear interpolation on the four grid corners */
|
||||
|
||||
real_T declination_min = ((longitude - min_lon) / MAG_SAMPLING_RES) * (declination_se - declination_sw) + declination_sw;
|
||||
real_T declination_max = ((longitude - min_lon) / MAG_SAMPLING_RES) * (declination_ne - declination_nw) + declination_nw;
|
||||
|
||||
return ((latitude - min_lat) / MAG_SAMPLING_RES) * (declination_max - declination_min) + declination_min;
|
||||
}
|
||||
|
||||
//geopot_height = Earth_radius * altitude / (Earth_radius + altitude) /// all in kilometers
|
||||
//temperature is in Kelvin = 273.15 + celcius
|
||||
static real_T getStandardTemperature(real_T geopot_height)
|
||||
{
|
||||
//standard atmospheric pressure
|
||||
//Below 51km: Practical Meteorology by Roland Stull, pg 12
|
||||
//Above 51km: http://www.braeunig.us/space/atmmodel.htm
|
||||
if (geopot_height <= 11) //troposphere
|
||||
return 288.15f - (6.5f * geopot_height);
|
||||
else if (geopot_height <= 20) //Staroshere starts
|
||||
return 216.65f;
|
||||
else if (geopot_height <= 32)
|
||||
return 196.65f + geopot_height;
|
||||
else if (geopot_height <= 47)
|
||||
return 228.65f + 2.8f * (geopot_height - 32);
|
||||
else if (geopot_height <= 51) //Mesosphere starts
|
||||
return 270.65f;
|
||||
else if (geopot_height <= 71)
|
||||
return 270.65f - 2.8f * (geopot_height - 51);
|
||||
else if (geopot_height <= 84.85f)
|
||||
return 214.65f - 2 * (geopot_height - 71);
|
||||
else return 3;
|
||||
//Thermospehere has high kinetic temperature (500c to 2000c) but temperature
|
||||
//as measured by thermometer would be very low because of almost vacuum
|
||||
//throw std::out_of_range("geopot_height must be less than 85km. Space domain is not supported yet!");
|
||||
}
|
||||
|
||||
static real_T getGeopotential(real_T altitude_km)
|
||||
{
|
||||
static constexpr real_T radius_km = EARTH_RADIUS / 1000.0f;
|
||||
return radius_km * altitude_km / (radius_km + altitude_km);
|
||||
}
|
||||
|
||||
static real_T getStandardPressure(real_T altitude /* meters */) //return Pa
|
||||
{
|
||||
real_T geopot_height = getGeopotential(altitude / 1000.0f);
|
||||
|
||||
real_T t = getStandardTemperature(geopot_height);
|
||||
|
||||
return getStandardPressure(geopot_height, t);
|
||||
}
|
||||
|
||||
static real_T getStandardPressure(real_T geopot_height, real_T std_temperature) //return Pa
|
||||
{
|
||||
//Below 51km: Practical Meteorology by Roland Stull, pg 12
|
||||
//Above 51km: http://www.braeunig.us/space/atmmodel.htm
|
||||
//Validation data: https://www.avs.org/AVS/files/c7/c7edaedb-95b2-438f-adfb-36de54f87b9e.pdf
|
||||
|
||||
//TODO: handle -ve altitude better (shouldn't grow indefinitely!)
|
||||
|
||||
if (geopot_height <= 11)
|
||||
//at alt 0, return sea level pressure
|
||||
return SeaLevelPressure * powf(288.15f / std_temperature, -5.255877f);
|
||||
else if (geopot_height <= 20)
|
||||
return 22632.06f * expf(-0.1577f * (geopot_height - 11));
|
||||
else if (geopot_height <= 32)
|
||||
return 5474.889f * powf(216.65f / std_temperature, 34.16319f);
|
||||
else if (geopot_height <= 47)
|
||||
return 868.0187f * powf(228.65f / std_temperature, 12.2011f);
|
||||
else if (geopot_height <= 51)
|
||||
return 110.9063f * exp(-0.1262f * (geopot_height - 47));
|
||||
else if (geopot_height <= 71)
|
||||
return 66.93887f * powf(270.65f / std_temperature, -12.2011f);
|
||||
else if (geopot_height <= 84.85f)
|
||||
return 3.956420f * powf(214.65f / std_temperature, -17.0816f);
|
||||
else return 1E-3f;
|
||||
//throw std::out_of_range("altitude must be less than 86km. Space domain is not supported yet!");
|
||||
}
|
||||
|
||||
static real_T getAirDensity(real_T std_pressure, real_T std_temperature) //kg / m^3
|
||||
{
|
||||
//http://www.braeunig.us/space/atmmodel.htm
|
||||
return std_pressure / 287.053f / std_temperature;
|
||||
}
|
||||
|
||||
static real_T getAirDensity(real_T altitude) //kg / m^3
|
||||
{
|
||||
real_T geo_pot = getGeopotential(altitude / 1000.0f);
|
||||
real_T std_temperature = getStandardTemperature(geo_pot);
|
||||
real_T std_pressure = getStandardPressure(geo_pot, std_temperature);
|
||||
return getAirDensity(std_pressure, std_temperature);
|
||||
}
|
||||
|
||||
static real_T getSpeedofSound(real_T altitude) // m/s
|
||||
{
|
||||
//http://www.braeunig.us/space/atmmodel.htm
|
||||
return sqrt(1.400f * 287.053f * getStandardTemperature(getGeopotential(altitude)));
|
||||
}
|
||||
|
||||
static real_T getGravity(real_T altitude)
|
||||
{
|
||||
//derivation: http://www.citycollegiate.com/gravitation_XId.htm
|
||||
if (altitude < 10000 && altitude > -10000) //up to 10 km, difference is too small
|
||||
return EarthUtils::Gravity;
|
||||
if (altitude < 100000 && altitude > -100000) //use first exproximation using binomial expansion
|
||||
return EarthUtils::Gravity * (1 - 2 * altitude / EARTH_RADIUS);
|
||||
else {
|
||||
real_T factor = 1 + altitude / EARTH_RADIUS;
|
||||
return EarthUtils::Gravity / factor / factor;
|
||||
if (geopot_height <= 11)
|
||||
//at alt 0, return sea level pressure
|
||||
return SeaLevelPressure * powf(288.15f / std_temperature, -5.255877f);
|
||||
else if (geopot_height <= 20)
|
||||
return 22632.06f * expf(-0.1577f * (geopot_height - 11));
|
||||
else if (geopot_height <= 32)
|
||||
return 5474.889f * powf(216.65f / std_temperature, 34.16319f);
|
||||
else if (geopot_height <= 47)
|
||||
return 868.0187f * powf(228.65f / std_temperature, 12.2011f);
|
||||
else if (geopot_height <= 51)
|
||||
return 110.9063f * exp(-0.1262f * (geopot_height - 47));
|
||||
else if (geopot_height <= 71)
|
||||
return 66.93887f * powf(270.65f / std_temperature, -12.2011f);
|
||||
else if (geopot_height <= 84.85f)
|
||||
return 3.956420f * powf(214.65f / std_temperature, -17.0816f);
|
||||
else
|
||||
return 1E-3f;
|
||||
//throw std::out_of_range("altitude must be less than 86km. Space domain is not supported yet!");
|
||||
}
|
||||
}
|
||||
|
||||
static Vector3r getMagField(const GeoPoint& geo_point) //return Tesla
|
||||
{
|
||||
double declination, inclination;
|
||||
return getMagField(geo_point, declination, inclination);
|
||||
}
|
||||
static real_T getAirDensity(real_T std_pressure, real_T std_temperature) //kg / m^3
|
||||
{
|
||||
//http://www.braeunig.us/space/atmmodel.htm
|
||||
return std_pressure / 287.053f / std_temperature;
|
||||
}
|
||||
|
||||
static Vector3r getMagField(const GeoPoint& geo_point, double& declination, double& inclination) //return Tesla
|
||||
{
|
||||
/*
|
||||
static real_T getAirDensity(real_T altitude) //kg / m^3
|
||||
{
|
||||
real_T geo_pot = getGeopotential(altitude / 1000.0f);
|
||||
real_T std_temperature = getStandardTemperature(geo_pot);
|
||||
real_T std_pressure = getStandardPressure(geo_pot, std_temperature);
|
||||
return getAirDensity(std_pressure, std_temperature);
|
||||
}
|
||||
|
||||
static real_T getSpeedofSound(real_T altitude) // m/s
|
||||
{
|
||||
//http://www.braeunig.us/space/atmmodel.htm
|
||||
return sqrt(1.400f * 287.053f * getStandardTemperature(getGeopotential(altitude)));
|
||||
}
|
||||
|
||||
static real_T getGravity(real_T altitude)
|
||||
{
|
||||
//derivation: http://www.citycollegiate.com/gravitation_XId.htm
|
||||
if (altitude < 10000 && altitude > -10000) //up to 10 km, difference is too small
|
||||
return EarthUtils::Gravity;
|
||||
if (altitude < 100000 && altitude > -100000) //use first exproximation using binomial expansion
|
||||
return EarthUtils::Gravity * (1 - 2 * altitude / EARTH_RADIUS);
|
||||
else {
|
||||
real_T factor = 1 + altitude / EARTH_RADIUS;
|
||||
return EarthUtils::Gravity / factor / factor;
|
||||
}
|
||||
}
|
||||
|
||||
static Vector3r getMagField(const GeoPoint& geo_point) //return Tesla
|
||||
{
|
||||
double declination, inclination;
|
||||
return getMagField(geo_point, declination, inclination);
|
||||
}
|
||||
|
||||
static Vector3r getMagField(const GeoPoint& geo_point, double& declination, double& inclination) //return Tesla
|
||||
{
|
||||
/*
|
||||
We calculate magnetic field using simple dipol model of Earth, i.e., assume
|
||||
earth as perfect dipole sphere and ignoring all but first order terms.
|
||||
This obviously is inaccurate because of huge amount of irregularities, magnetic pole that is
|
||||
|
|
@ -235,118 +240,117 @@ public:
|
|||
geo: 47.646 -122.134 -378
|
||||
*/
|
||||
|
||||
//ref: The Earth's Magnetism: An Introduction for Geologists, Roberto Lanza, Antonio Meloni
|
||||
//Sec 1.2.5, pg 27-30 https://goo.gl/bRm7wt
|
||||
//some theory at http://www.tulane.edu/~sanelson/eens634/Hmwk6MagneticField.pdf
|
||||
//ref: The Earth's Magnetism: An Introduction for Geologists, Roberto Lanza, Antonio Meloni
|
||||
//Sec 1.2.5, pg 27-30 https://goo.gl/bRm7wt
|
||||
//some theory at http://www.tulane.edu/~sanelson/eens634/Hmwk6MagneticField.pdf
|
||||
|
||||
double lat = Utils::degreesToRadians(geo_point.latitude); //geographic colatitude
|
||||
double lon = Utils::degreesToRadians(geo_point.longitude);
|
||||
double altitude = geo_point.altitude + EARTH_RADIUS;
|
||||
double lat = Utils::degreesToRadians(geo_point.latitude); //geographic colatitude
|
||||
double lon = Utils::degreesToRadians(geo_point.longitude);
|
||||
double altitude = geo_point.altitude + EARTH_RADIUS;
|
||||
|
||||
//cache value
|
||||
double sin_MagPoleLat = sin(MagPoleLat);
|
||||
double cos_MagPoleLat = cos(MagPoleLat);
|
||||
double cos_lat = cos(lat);
|
||||
double sin_lat = sin(lat);
|
||||
//cache value
|
||||
double sin_MagPoleLat = sin(MagPoleLat);
|
||||
double cos_MagPoleLat = cos(MagPoleLat);
|
||||
double cos_lat = cos(lat);
|
||||
double sin_lat = sin(lat);
|
||||
|
||||
//find magnetic colatitude
|
||||
double mag_clat = acos(cos_lat * cos_MagPoleLat +
|
||||
sin_lat * sin_MagPoleLat * cos(lon - MagPoleLon) );
|
||||
//find magnetic colatitude
|
||||
double mag_clat = acos(cos_lat * cos_MagPoleLat +
|
||||
sin_lat * sin_MagPoleLat * cos(lon - MagPoleLon));
|
||||
|
||||
//calculation of magnetic longitude is not needed but just in case if someone wants it
|
||||
//double mag_lon = asin(
|
||||
// (sin(lon - MagPoleLon) * sin(lat)) /
|
||||
// sin(mag_clat));
|
||||
//calculation of magnetic longitude is not needed but just in case if someone wants it
|
||||
//double mag_lon = asin(
|
||||
// (sin(lon - MagPoleLon) * sin(lat)) /
|
||||
// sin(mag_clat));
|
||||
|
||||
//field strength only depends on magnetic colatitude
|
||||
//https://en.wikipedia.org/wiki/Dipole_model_of_the_Earth's_magnetic_field
|
||||
double cos_mag_clat = cos(mag_clat);
|
||||
double field_mag = MeanMagField * pow(EARTH_RADIUS / altitude, 3) *
|
||||
sqrt(1 + 3 * cos_mag_clat * cos_mag_clat);
|
||||
//field strength only depends on magnetic colatitude
|
||||
//https://en.wikipedia.org/wiki/Dipole_model_of_the_Earth's_magnetic_field
|
||||
double cos_mag_clat = cos(mag_clat);
|
||||
double field_mag = MeanMagField * pow(EARTH_RADIUS / altitude, 3) *
|
||||
sqrt(1 + 3 * cos_mag_clat * cos_mag_clat);
|
||||
|
||||
//find inclination and declination
|
||||
//equation of declination in above referenced book is only partial
|
||||
//full equation is (4a) at http://www.tulane.edu/~sanelson/eens634/Hmwk6MagneticField.pdf
|
||||
double lat_test = sin_MagPoleLat * sin_lat;
|
||||
double dec_factor = cos_MagPoleLat / sin(mag_clat);
|
||||
if (cos_mag_clat > lat_test)
|
||||
declination = asin(sin(lon - MagPoleLon) * dec_factor);
|
||||
else
|
||||
declination = asin(cos(lon - MagPoleLon) * dec_factor);
|
||||
inclination = atan(2.0 / tan(mag_clat)); //do not use atan2 here
|
||||
//find inclination and declination
|
||||
//equation of declination in above referenced book is only partial
|
||||
//full equation is (4a) at http://www.tulane.edu/~sanelson/eens634/Hmwk6MagneticField.pdf
|
||||
double lat_test = sin_MagPoleLat * sin_lat;
|
||||
double dec_factor = cos_MagPoleLat / sin(mag_clat);
|
||||
if (cos_mag_clat > lat_test)
|
||||
declination = asin(sin(lon - MagPoleLon) * dec_factor);
|
||||
else
|
||||
declination = asin(cos(lon - MagPoleLon) * dec_factor);
|
||||
inclination = atan(2.0 / tan(mag_clat)); //do not use atan2 here
|
||||
|
||||
//transform magnetic field vector to geographical coordinates
|
||||
//ref: http://www.geo.mtu.edu/~jdiehl/magnotes.html
|
||||
double field_xy = field_mag * cos(inclination);
|
||||
return Vector3r(
|
||||
static_cast<real_T>(field_xy * cos(declination)),
|
||||
static_cast<real_T>(field_xy * sin(declination)),
|
||||
static_cast<real_T>(field_mag * sin(inclination))
|
||||
);
|
||||
}
|
||||
//transform magnetic field vector to geographical coordinates
|
||||
//ref: http://www.geo.mtu.edu/~jdiehl/magnotes.html
|
||||
double field_xy = field_mag * cos(inclination);
|
||||
return Vector3r(
|
||||
static_cast<real_T>(field_xy * cos(declination)),
|
||||
static_cast<real_T>(field_xy * sin(declination)),
|
||||
static_cast<real_T>(field_mag * sin(inclination)));
|
||||
}
|
||||
|
||||
static GeoPoint nedToGeodetic(const Vector3r& v, const HomeGeoPoint& home_geo_point)
|
||||
{
|
||||
double x_rad = v.x() / EARTH_RADIUS;
|
||||
double y_rad = v.y() / EARTH_RADIUS;
|
||||
double c = sqrt(x_rad*x_rad + y_rad*y_rad);
|
||||
double sin_c = sin(c), cos_c = cos(c);
|
||||
double lat_rad, lon_rad;
|
||||
if (!Utils::isApproximatelyZero(c)) { //avoids large changes?
|
||||
lat_rad = asin(cos_c * home_geo_point.sin_lat + (x_rad * sin_c * home_geo_point.cos_lat) / c);
|
||||
lon_rad = (home_geo_point.lon_rad +
|
||||
atan2(y_rad * sin_c, c * home_geo_point.cos_lat * cos_c - x_rad * home_geo_point.sin_lat * sin_c));
|
||||
static GeoPoint nedToGeodetic(const Vector3r& v, const HomeGeoPoint& home_geo_point)
|
||||
{
|
||||
double x_rad = v.x() / EARTH_RADIUS;
|
||||
double y_rad = v.y() / EARTH_RADIUS;
|
||||
double c = sqrt(x_rad * x_rad + y_rad * y_rad);
|
||||
double sin_c = sin(c), cos_c = cos(c);
|
||||
double lat_rad, lon_rad;
|
||||
if (!Utils::isApproximatelyZero(c)) { //avoids large changes?
|
||||
lat_rad = asin(cos_c * home_geo_point.sin_lat + (x_rad * sin_c * home_geo_point.cos_lat) / c);
|
||||
lon_rad = (home_geo_point.lon_rad +
|
||||
atan2(y_rad * sin_c, c * home_geo_point.cos_lat * cos_c - x_rad * home_geo_point.sin_lat * sin_c));
|
||||
|
||||
return GeoPoint(Utils::radiansToDegrees(lat_rad), Utils::radiansToDegrees(lon_rad),
|
||||
home_geo_point.home_geo_point.altitude - v.z());
|
||||
} else
|
||||
return GeoPoint(home_geo_point.home_geo_point.latitude, home_geo_point.home_geo_point.longitude, home_geo_point.home_geo_point.altitude - v.z());
|
||||
}
|
||||
return GeoPoint(Utils::radiansToDegrees(lat_rad), Utils::radiansToDegrees(lon_rad), home_geo_point.home_geo_point.altitude - v.z());
|
||||
}
|
||||
else
|
||||
return GeoPoint(home_geo_point.home_geo_point.latitude, home_geo_point.home_geo_point.longitude, home_geo_point.home_geo_point.altitude - v.z());
|
||||
}
|
||||
|
||||
//below are approximate versions and would produce errors of more than 10m for points farther than 1km
|
||||
//for more accurate versions, please use the version in EarthUtils::nedToGeodetic
|
||||
static Vector3r GeodeticToNedFast(const GeoPoint& geo, const GeoPoint& home)
|
||||
{
|
||||
double d_lat = geo.latitude - home.latitude;
|
||||
double d_lon = geo.longitude - home.longitude;
|
||||
real_T d_alt = static_cast<real_T>(home.altitude - geo.altitude);
|
||||
real_T x = static_cast<real_T>(Utils::degreesToRadians(d_lat) * EARTH_RADIUS);
|
||||
real_T y = static_cast<real_T>(Utils::degreesToRadians(d_lon) * EARTH_RADIUS * cos(Utils::degreesToRadians(geo.latitude)));
|
||||
return Vector3r(x, y, d_alt);
|
||||
}
|
||||
static GeoPoint nedToGeodeticFast(const Vector3r& local, const GeoPoint& home)
|
||||
{
|
||||
GeoPoint r;
|
||||
double d_lat = local.x() / EARTH_RADIUS;
|
||||
double d_lon = local.y() / (EARTH_RADIUS * cos(Utils::degreesToRadians(home.latitude)));
|
||||
r.latitude = home.latitude + Utils::radiansToDegrees(d_lat);
|
||||
r.longitude = home.longitude + Utils::radiansToDegrees(d_lon);
|
||||
r.altitude = home.altitude - local.z();
|
||||
//below are approximate versions and would produce errors of more than 10m for points farther than 1km
|
||||
//for more accurate versions, please use the version in EarthUtils::nedToGeodetic
|
||||
static Vector3r GeodeticToNedFast(const GeoPoint& geo, const GeoPoint& home)
|
||||
{
|
||||
double d_lat = geo.latitude - home.latitude;
|
||||
double d_lon = geo.longitude - home.longitude;
|
||||
real_T d_alt = static_cast<real_T>(home.altitude - geo.altitude);
|
||||
real_T x = static_cast<real_T>(Utils::degreesToRadians(d_lat) * EARTH_RADIUS);
|
||||
real_T y = static_cast<real_T>(Utils::degreesToRadians(d_lon) * EARTH_RADIUS * cos(Utils::degreesToRadians(geo.latitude)));
|
||||
return Vector3r(x, y, d_alt);
|
||||
}
|
||||
static GeoPoint nedToGeodeticFast(const Vector3r& local, const GeoPoint& home)
|
||||
{
|
||||
GeoPoint r;
|
||||
double d_lat = local.x() / EARTH_RADIUS;
|
||||
double d_lon = local.y() / (EARTH_RADIUS * cos(Utils::degreesToRadians(home.latitude)));
|
||||
r.latitude = home.latitude + Utils::radiansToDegrees(d_lat);
|
||||
r.longitude = home.longitude + Utils::radiansToDegrees(d_lon);
|
||||
r.altitude = home.altitude - local.z();
|
||||
|
||||
return r;
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
public: //consts
|
||||
//ref: https://www.ngdc.noaa.gov/geomag/GeomagneticPoles.shtml
|
||||
static constexpr double MagPoleLat = Utils::degreesToRadians(80.31f);
|
||||
static constexpr double MagPoleLon = Utils::degreesToRadians(-72.62f);
|
||||
static constexpr double MeanMagField = 3.12E-5; //Tesla, https://en.wikipedia.org/wiki/Dipole_model_of_the_Earth's_magnetic_field
|
||||
static constexpr float SeaLevelPressure = 101325.0f; //Pascal
|
||||
static constexpr float SeaLevelAirDensity = 1.225f; //kg/m^3
|
||||
static constexpr float Gravity = 9.80665f; //m/s^2
|
||||
static constexpr float Radius = EARTH_RADIUS; //m
|
||||
static constexpr float SpeedOfLight = 299792458.0f; //m
|
||||
static constexpr float Obliquity = Utils::degreesToRadians(23.4397f);
|
||||
static constexpr double Perihelion = Utils::degreesToRadians(102.9372); // perihelion of the Earth
|
||||
static constexpr double DistanceFromSun = 149597870700.0; // meters
|
||||
public: //consts
|
||||
//ref: https://www.ngdc.noaa.gov/geomag/GeomagneticPoles.shtml
|
||||
static constexpr double MagPoleLat = Utils::degreesToRadians(80.31f);
|
||||
static constexpr double MagPoleLon = Utils::degreesToRadians(-72.62f);
|
||||
static constexpr double MeanMagField = 3.12E-5; //Tesla, https://en.wikipedia.org/wiki/Dipole_model_of_the_Earth's_magnetic_field
|
||||
static constexpr float SeaLevelPressure = 101325.0f; //Pascal
|
||||
static constexpr float SeaLevelAirDensity = 1.225f; //kg/m^3
|
||||
static constexpr float Gravity = 9.80665f; //m/s^2
|
||||
static constexpr float Radius = EARTH_RADIUS; //m
|
||||
static constexpr float SpeedOfLight = 299792458.0f; //m
|
||||
static constexpr float Obliquity = Utils::degreesToRadians(23.4397f);
|
||||
static constexpr double Perihelion = Utils::degreesToRadians(102.9372); // perihelion of the Earth
|
||||
static constexpr double DistanceFromSun = 149597870700.0; // meters
|
||||
|
||||
private:
|
||||
/* magnetic field */
|
||||
static float get_mag_lookup_table_val(int lat_index, int lon_index)
|
||||
{
|
||||
return static_cast<float>(DECLINATION_TABLE[lat_index][lon_index]);
|
||||
}
|
||||
};
|
||||
|
||||
}} //namespace
|
||||
private:
|
||||
/* magnetic field */
|
||||
static float get_mag_lookup_table_val(int lat_index, int lon_index)
|
||||
{
|
||||
return static_cast<float>(DECLINATION_TABLE[lat_index][lon_index]);
|
||||
}
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -1,7 +1,6 @@
|
|||
// Copyright (c) Microsoft Corporation. All rights reserved.
|
||||
// Licensed under the MIT License.
|
||||
|
||||
|
||||
#ifndef airsimcore_firstorderfilter_hpp
|
||||
#define airsimcore_firstorderfilter_hpp
|
||||
|
||||
|
|
@ -9,11 +8,15 @@
|
|||
#include "UpdatableObject.hpp"
|
||||
#include "Common.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
template <typename T>
|
||||
class FirstOrderFilter : public UpdatableObject {
|
||||
/*
|
||||
template <typename T>
|
||||
class FirstOrderFilter : public UpdatableObject
|
||||
{
|
||||
/*
|
||||
This class can be used to apply a first order filter on a signal.
|
||||
It allows different acceleration and deceleration time constants.
|
||||
|
||||
|
|
@ -25,65 +28,64 @@ class FirstOrderFilter : public UpdatableObject {
|
|||
discretized system (ZoH):
|
||||
x(k+1) = exp(samplingTime*(-1/tau))*x(k) + (1 - exp(samplingTime*(-1/tau))) * u(k)
|
||||
*/
|
||||
public:
|
||||
FirstOrderFilter()
|
||||
{
|
||||
//allow default constructor with later call for initialize
|
||||
}
|
||||
FirstOrderFilter(float timeConstant, T initial_input, T initial_output)
|
||||
{
|
||||
initialize(timeConstant, initial_input, initial_output);
|
||||
}
|
||||
void initialize(float timeConstant, T initial_input, T initial_output)
|
||||
{
|
||||
timeConstant_ = timeConstant;
|
||||
initial_input_ = initial_input;
|
||||
initial_output_ = initial_output;
|
||||
}
|
||||
public:
|
||||
FirstOrderFilter()
|
||||
{
|
||||
//allow default constructor with later call for initialize
|
||||
}
|
||||
FirstOrderFilter(float timeConstant, T initial_input, T initial_output)
|
||||
{
|
||||
initialize(timeConstant, initial_input, initial_output);
|
||||
}
|
||||
void initialize(float timeConstant, T initial_input, T initial_output)
|
||||
{
|
||||
timeConstant_ = timeConstant;
|
||||
initial_input_ = initial_input;
|
||||
initial_output_ = initial_output;
|
||||
}
|
||||
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
last_time_ = clock()->nowNanos();
|
||||
input_ = initial_input_;
|
||||
output_ = initial_output_;
|
||||
}
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
last_time_ = clock()->nowNanos();
|
||||
input_ = initial_input_;
|
||||
output_ = initial_output_;
|
||||
}
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
UpdatableObject::update();
|
||||
virtual void update() override
|
||||
{
|
||||
UpdatableObject::update();
|
||||
|
||||
TTimeDelta dt = clock()->updateSince(last_time_);
|
||||
TTimeDelta dt = clock()->updateSince(last_time_);
|
||||
|
||||
//lower the weight for previous value if its been long time
|
||||
//TODO: minimize use of exp
|
||||
double alpha = exp(-dt / timeConstant_);
|
||||
// x(k+1) = Ad*x(k) + Bd*u(k)
|
||||
output_ = static_cast<real_T>(output_ * alpha + input_ * (1 - alpha));
|
||||
}
|
||||
//*** End: UpdatableState implementation ***//
|
||||
//lower the weight for previous value if its been long time
|
||||
//TODO: minimize use of exp
|
||||
double alpha = exp(-dt / timeConstant_);
|
||||
// x(k+1) = Ad*x(k) + Bd*u(k)
|
||||
output_ = static_cast<real_T>(output_ * alpha + input_ * (1 - alpha));
|
||||
}
|
||||
//*** End: UpdatableState implementation ***//
|
||||
|
||||
void setInput(T input)
|
||||
{
|
||||
input_ = input;
|
||||
}
|
||||
T getInput() const
|
||||
{
|
||||
return input_;
|
||||
}
|
||||
|
||||
void setInput(T input)
|
||||
{
|
||||
input_ = input;
|
||||
}
|
||||
T getInput() const
|
||||
{
|
||||
return input_;
|
||||
}
|
||||
T getOutput() const
|
||||
{
|
||||
return output_;
|
||||
}
|
||||
|
||||
T getOutput() const
|
||||
{
|
||||
return output_;
|
||||
}
|
||||
|
||||
private:
|
||||
float timeConstant_;
|
||||
T output_, input_;
|
||||
T initial_output_, initial_input_;
|
||||
TTimePoint last_time_;
|
||||
};
|
||||
|
||||
}} //namespace
|
||||
private:
|
||||
float timeConstant_;
|
||||
T output_, input_;
|
||||
T initial_output_, initial_input_;
|
||||
TTimePoint last_time_;
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -8,124 +8,126 @@
|
|||
#include "UpdatableObject.hpp"
|
||||
#include "common/Common.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
class FrequencyLimiter : public UpdatableObject {
|
||||
public:
|
||||
FrequencyLimiter(real_T frequency = Utils::max<float>(), real_T startup_delay = 0)
|
||||
class FrequencyLimiter : public UpdatableObject
|
||||
{
|
||||
initialize(frequency, startup_delay);
|
||||
}
|
||||
|
||||
void initialize(real_T frequency = Utils::max<float>(), real_T startup_delay = 0)
|
||||
{
|
||||
frequency_ = frequency;
|
||||
startup_delay_ = startup_delay;
|
||||
}
|
||||
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
last_time_ = clock()->nowNanos();
|
||||
first_time_ = last_time_;
|
||||
|
||||
if (Utils::isApproximatelyZero(frequency_))
|
||||
interval_size_sec_ = 1E10; //some high number
|
||||
else
|
||||
interval_size_sec_ = 1.0f / frequency_;
|
||||
|
||||
elapsed_total_sec_ = 0;
|
||||
elapsed_interval_sec_ = 0;
|
||||
last_elapsed_interval_sec_ = 0;
|
||||
update_count_ = 0;
|
||||
interval_complete_ = false;
|
||||
startup_complete_ = false;
|
||||
}
|
||||
|
||||
virtual void failResetUpdateOrdering(std::string err) override
|
||||
{
|
||||
// Do nothing.
|
||||
// Disable checks for reset/update sequence because
|
||||
// this object may get created but not used.
|
||||
}
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
UpdatableObject::update();
|
||||
|
||||
elapsed_total_sec_ = clock()->elapsedSince(first_time_);
|
||||
elapsed_interval_sec_ = clock()->elapsedSince(last_time_);
|
||||
++update_count_;
|
||||
|
||||
//if startup_delay_ > 0 then we consider startup_delay_ as the first interval
|
||||
//that needs to be complete
|
||||
if (!startup_complete_) {
|
||||
if (Utils::isDefinitelyGreaterThan(startup_delay_, 0.0f)) {
|
||||
//see if we have spent startup_delay_ time yet
|
||||
interval_complete_ = elapsed_interval_sec_ >= startup_delay_;
|
||||
}
|
||||
else //no special startup delay is needed
|
||||
startup_complete_ = true;
|
||||
public:
|
||||
FrequencyLimiter(real_T frequency = Utils::max<float>(), real_T startup_delay = 0)
|
||||
{
|
||||
initialize(frequency, startup_delay);
|
||||
}
|
||||
|
||||
//if startup is complete, we will do regular intervals from now one
|
||||
if (startup_complete_)
|
||||
interval_complete_ = elapsed_interval_sec_ >= interval_size_sec_;
|
||||
|
||||
//when any interval is done, reset the state and repeat
|
||||
if (interval_complete_) {
|
||||
last_elapsed_interval_sec_ = elapsed_interval_sec_;
|
||||
|
||||
void initialize(real_T frequency = Utils::max<float>(), real_T startup_delay = 0)
|
||||
{
|
||||
frequency_ = frequency;
|
||||
startup_delay_ = startup_delay;
|
||||
}
|
||||
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
last_time_ = clock()->nowNanos();
|
||||
first_time_ = last_time_;
|
||||
|
||||
if (Utils::isApproximatelyZero(frequency_))
|
||||
interval_size_sec_ = 1E10; //some high number
|
||||
else
|
||||
interval_size_sec_ = 1.0f / frequency_;
|
||||
|
||||
elapsed_total_sec_ = 0;
|
||||
elapsed_interval_sec_ = 0;
|
||||
startup_complete_ = true;
|
||||
last_elapsed_interval_sec_ = 0;
|
||||
update_count_ = 0;
|
||||
interval_complete_ = false;
|
||||
startup_complete_ = false;
|
||||
}
|
||||
}
|
||||
//*** End: UpdatableState implementation ***//
|
||||
|
||||
virtual void failResetUpdateOrdering(std::string err) override
|
||||
{
|
||||
// Do nothing.
|
||||
// Disable checks for reset/update sequence because
|
||||
// this object may get created but not used.
|
||||
}
|
||||
|
||||
TTimeDelta getElapsedTotalSec() const
|
||||
{
|
||||
return elapsed_total_sec_;
|
||||
}
|
||||
virtual void update() override
|
||||
{
|
||||
UpdatableObject::update();
|
||||
|
||||
TTimeDelta getElapsedIntervalSec() const
|
||||
{
|
||||
return elapsed_interval_sec_;
|
||||
}
|
||||
elapsed_total_sec_ = clock()->elapsedSince(first_time_);
|
||||
elapsed_interval_sec_ = clock()->elapsedSince(last_time_);
|
||||
++update_count_;
|
||||
|
||||
TTimeDelta getLastElapsedIntervalSec() const
|
||||
{
|
||||
return last_elapsed_interval_sec_;
|
||||
}
|
||||
//if startup_delay_ > 0 then we consider startup_delay_ as the first interval
|
||||
//that needs to be complete
|
||||
if (!startup_complete_) {
|
||||
if (Utils::isDefinitelyGreaterThan(startup_delay_, 0.0f)) {
|
||||
//see if we have spent startup_delay_ time yet
|
||||
interval_complete_ = elapsed_interval_sec_ >= startup_delay_;
|
||||
}
|
||||
else //no special startup delay is needed
|
||||
startup_complete_ = true;
|
||||
}
|
||||
|
||||
bool isWaitComplete() const
|
||||
{
|
||||
return interval_complete_;
|
||||
}
|
||||
//if startup is complete, we will do regular intervals from now one
|
||||
if (startup_complete_)
|
||||
interval_complete_ = elapsed_interval_sec_ >= interval_size_sec_;
|
||||
|
||||
bool isStartupComplete() const
|
||||
{
|
||||
return startup_complete_;
|
||||
}
|
||||
//when any interval is done, reset the state and repeat
|
||||
if (interval_complete_) {
|
||||
last_elapsed_interval_sec_ = elapsed_interval_sec_;
|
||||
last_time_ = clock()->nowNanos();
|
||||
elapsed_interval_sec_ = 0;
|
||||
startup_complete_ = true;
|
||||
}
|
||||
}
|
||||
//*** End: UpdatableState implementation ***//
|
||||
|
||||
uint getUpdateCount() const
|
||||
{
|
||||
return update_count_;
|
||||
}
|
||||
TTimeDelta getElapsedTotalSec() const
|
||||
{
|
||||
return elapsed_total_sec_;
|
||||
}
|
||||
|
||||
private:
|
||||
real_T interval_size_sec_;
|
||||
TTimeDelta elapsed_total_sec_;
|
||||
TTimeDelta elapsed_interval_sec_;
|
||||
TTimeDelta last_elapsed_interval_sec_;
|
||||
uint update_count_;
|
||||
real_T frequency_;
|
||||
real_T startup_delay_;
|
||||
bool interval_complete_;
|
||||
bool startup_complete_;
|
||||
TTimePoint last_time_, first_time_;
|
||||
TTimeDelta getElapsedIntervalSec() const
|
||||
{
|
||||
return elapsed_interval_sec_;
|
||||
}
|
||||
|
||||
};
|
||||
TTimeDelta getLastElapsedIntervalSec() const
|
||||
{
|
||||
return last_elapsed_interval_sec_;
|
||||
}
|
||||
|
||||
}} //namespace
|
||||
#endif
|
||||
bool isWaitComplete() const
|
||||
{
|
||||
return interval_complete_;
|
||||
}
|
||||
|
||||
bool isStartupComplete() const
|
||||
{
|
||||
return startup_complete_;
|
||||
}
|
||||
|
||||
uint getUpdateCount() const
|
||||
{
|
||||
return update_count_;
|
||||
}
|
||||
|
||||
private:
|
||||
real_T interval_size_sec_;
|
||||
TTimeDelta elapsed_total_sec_;
|
||||
TTimeDelta elapsed_interval_sec_;
|
||||
TTimeDelta last_elapsed_interval_sec_;
|
||||
uint update_count_;
|
||||
real_T frequency_;
|
||||
real_T startup_delay_;
|
||||
bool interval_complete_;
|
||||
bool startup_complete_;
|
||||
TTimePoint last_time_, first_time_;
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -9,39 +9,44 @@
|
|||
#include <list>
|
||||
#include "common_utils/RandomGenerator.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
class GaussianMarkov : public UpdatableObject {
|
||||
public:
|
||||
GaussianMarkov()
|
||||
{}
|
||||
GaussianMarkov(real_T tau, real_T sigma, real_T initial_output = 0) //in seconds
|
||||
class GaussianMarkov : public UpdatableObject
|
||||
{
|
||||
initialize(tau, sigma, initial_output);
|
||||
}
|
||||
void initialize(real_T tau, real_T sigma, real_T initial_output = 0) //in seconds
|
||||
{
|
||||
tau_ = tau;
|
||||
sigma_ = sigma;
|
||||
rand_ = RandomGeneratorGausianR(0.0f, 1.0f);
|
||||
|
||||
if (std::isnan(initial_output))
|
||||
initial_output_ = getNextRandom() * sigma_;
|
||||
else
|
||||
initial_output_ = initial_output;
|
||||
}
|
||||
public:
|
||||
GaussianMarkov()
|
||||
{
|
||||
}
|
||||
GaussianMarkov(real_T tau, real_T sigma, real_T initial_output = 0) //in seconds
|
||||
{
|
||||
initialize(tau, sigma, initial_output);
|
||||
}
|
||||
void initialize(real_T tau, real_T sigma, real_T initial_output = 0) //in seconds
|
||||
{
|
||||
tau_ = tau;
|
||||
sigma_ = sigma;
|
||||
rand_ = RandomGeneratorGausianR(0.0f, 1.0f);
|
||||
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
last_time_ = clock()->nowNanos();
|
||||
output_ = initial_output_;
|
||||
rand_.reset();
|
||||
}
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
/*
|
||||
if (std::isnan(initial_output))
|
||||
initial_output_ = getNextRandom() * sigma_;
|
||||
else
|
||||
initial_output_ = initial_output;
|
||||
}
|
||||
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
last_time_ = clock()->nowNanos();
|
||||
output_ = initial_output_;
|
||||
rand_.reset();
|
||||
}
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
/*
|
||||
Ref:
|
||||
A Comparison between Different Error Modeling of MEMS Applied to GPS/INS Integrated Systems
|
||||
Quinchia, sec 3.2, https://www.ncbi.nlm.nih.gov/pmc/articles/PMC3812568/
|
||||
|
|
@ -50,34 +55,31 @@ public:
|
|||
John H Wall, 2007, eq 2.5, pg 13, http://etd.auburn.edu/handle/10415/945
|
||||
*/
|
||||
|
||||
UpdatableObject::update();
|
||||
|
||||
TTimeDelta dt = clock()->updateSince(last_time_);
|
||||
UpdatableObject::update();
|
||||
|
||||
double alpha = exp(-dt / tau_);
|
||||
output_ = static_cast<real_T>(alpha * output_ + (1 - alpha) * getNextRandom() * sigma_);
|
||||
}
|
||||
//*** End: UpdatableState implementation ***//
|
||||
TTimeDelta dt = clock()->updateSince(last_time_);
|
||||
|
||||
double alpha = exp(-dt / tau_);
|
||||
output_ = static_cast<real_T>(alpha * output_ + (1 - alpha) * getNextRandom() * sigma_);
|
||||
}
|
||||
//*** End: UpdatableState implementation ***//
|
||||
|
||||
real_T getNextRandom()
|
||||
{
|
||||
return rand_.next();
|
||||
}
|
||||
real_T getNextRandom()
|
||||
{
|
||||
return rand_.next();
|
||||
}
|
||||
|
||||
real_T getOutput() const
|
||||
{
|
||||
return output_;
|
||||
}
|
||||
real_T getOutput() const
|
||||
{
|
||||
return output_;
|
||||
}
|
||||
|
||||
private:
|
||||
RandomGeneratorGausianR rand_;
|
||||
real_T tau_, sigma_;
|
||||
real_T output_, initial_output_;
|
||||
TTimePoint last_time_;
|
||||
|
||||
};
|
||||
|
||||
|
||||
}} //namespace
|
||||
private:
|
||||
RandomGeneratorGausianR rand_;
|
||||
real_T tau_, sigma_;
|
||||
real_T output_, initial_output_;
|
||||
TTimePoint last_time_;
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -7,220 +7,221 @@
|
|||
#include <cmath>
|
||||
#include "VectorMath.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
class GeodeticConverter
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
public:
|
||||
GeodeticConverter(double home_latitude = 0, double home_longitude = 0, float home_altitude = 0)
|
||||
{
|
||||
setHome(home_latitude, home_longitude, home_altitude);
|
||||
}
|
||||
|
||||
void setHome(double home_latitude, double home_longitude, float home_altitude)
|
||||
{
|
||||
home_latitude_ = home_latitude;
|
||||
home_longitude_ = home_longitude;
|
||||
home_altitude_ = home_altitude;
|
||||
|
||||
// Save NED origin
|
||||
home_latitude_rad_ = deg2Rad(home_latitude);
|
||||
home_longitude_rad_ = deg2Rad(home_longitude);
|
||||
class GeodeticConverter
|
||||
{
|
||||
public:
|
||||
GeodeticConverter(double home_latitude = 0, double home_longitude = 0, float home_altitude = 0)
|
||||
{
|
||||
setHome(home_latitude, home_longitude, home_altitude);
|
||||
}
|
||||
|
||||
// Compute ECEF of NED origin
|
||||
geodetic2Ecef(home_latitude_, home_longitude_, home_altitude_, &home_ecef_x_, &home_ecef_y_, &home_ecef_z_);
|
||||
void setHome(double home_latitude, double home_longitude, float home_altitude)
|
||||
{
|
||||
home_latitude_ = home_latitude;
|
||||
home_longitude_ = home_longitude;
|
||||
home_altitude_ = home_altitude;
|
||||
|
||||
// Compute ECEF to NED and NED to ECEF matrices
|
||||
double phiP = atan2(home_ecef_z_, sqrt(pow(home_ecef_x_, 2) + pow(home_ecef_y_, 2)));
|
||||
// Save NED origin
|
||||
home_latitude_rad_ = deg2Rad(home_latitude);
|
||||
home_longitude_rad_ = deg2Rad(home_longitude);
|
||||
|
||||
ecef_to_ned_matrix_ = nRe(phiP, home_longitude_rad_);
|
||||
ned_to_ecef_matrix_ = nRe(home_latitude_rad_, home_longitude_rad_).transpose();
|
||||
}
|
||||
// Compute ECEF of NED origin
|
||||
geodetic2Ecef(home_latitude_, home_longitude_, home_altitude_, &home_ecef_x_, &home_ecef_y_, &home_ecef_z_);
|
||||
|
||||
void getHome(double* latitude, double* longitude, float* altitude)
|
||||
{
|
||||
*latitude = home_latitude_;
|
||||
*longitude = home_longitude_;
|
||||
*altitude = home_altitude_;
|
||||
}
|
||||
// Compute ECEF to NED and NED to ECEF matrices
|
||||
double phiP = atan2(home_ecef_z_, sqrt(pow(home_ecef_x_, 2) + pow(home_ecef_y_, 2)));
|
||||
|
||||
void geodetic2Ecef(const double latitude, const double longitude, const double altitude, double* x,
|
||||
double* y, double* z)
|
||||
{
|
||||
// Convert geodetic coordinates to ECEF.
|
||||
// http://code.google.com/p/pysatel/source/browse/trunk/coord.py?r=22
|
||||
double lat_rad = deg2Rad(latitude);
|
||||
double lon_rad = deg2Rad(longitude);
|
||||
double xi = sqrt(1 - kFirstEccentricitySquared * sin(lat_rad) * sin(lat_rad));
|
||||
*x = (kSemimajorAxis / xi + altitude) * cos(lat_rad) * cos(lon_rad);
|
||||
*y = (kSemimajorAxis / xi + altitude) * cos(lat_rad) * sin(lon_rad);
|
||||
*z = (kSemimajorAxis / xi * (1 - kFirstEccentricitySquared) + altitude) * sin(lat_rad);
|
||||
}
|
||||
ecef_to_ned_matrix_ = nRe(phiP, home_longitude_rad_);
|
||||
ned_to_ecef_matrix_ = nRe(home_latitude_rad_, home_longitude_rad_).transpose();
|
||||
}
|
||||
|
||||
void ecef2Geodetic(const double x, const double y, const double z, double* latitude,
|
||||
double* longitude, float* altitude)
|
||||
{
|
||||
// Convert ECEF coordinates to geodetic coordinates.
|
||||
// J. Zhu, "Conversion of Earth-centered Earth-fixed coordinates
|
||||
// to geodetic coordinates," IEEE Transactions on Aerospace and
|
||||
// Electronic Systems, vol. 30, pp. 957-961, 1994.
|
||||
void getHome(double* latitude, double* longitude, float* altitude)
|
||||
{
|
||||
*latitude = home_latitude_;
|
||||
*longitude = home_longitude_;
|
||||
*altitude = home_altitude_;
|
||||
}
|
||||
|
||||
double r = sqrt(x * x + y * y);
|
||||
double Esq = kSemimajorAxis * kSemimajorAxis - kSemiminorAxis * kSemiminorAxis;
|
||||
double F = 54 * kSemiminorAxis * kSemiminorAxis * z * z;
|
||||
double G = r * r + (1 - kFirstEccentricitySquared) * z * z - kFirstEccentricitySquared * Esq;
|
||||
double C = (kFirstEccentricitySquared * kFirstEccentricitySquared * F * r * r) / pow(G, 3);
|
||||
double S = cbrt(1 + C + sqrt(C * C + 2 * C));
|
||||
double P = F / (3 * pow((S + 1 / S + 1), 2) * G * G);
|
||||
double Q = sqrt(1 + 2 * kFirstEccentricitySquared * kFirstEccentricitySquared * P);
|
||||
double r_0 = -(P * kFirstEccentricitySquared * r) / (1 + Q)
|
||||
+ sqrt(
|
||||
0.5 * kSemimajorAxis * kSemimajorAxis * (1 + 1.0 / Q)
|
||||
- P * (1 - kFirstEccentricitySquared) * z * z / (Q * (1 + Q)) - 0.5 * P * r * r);
|
||||
double U = sqrt(pow((r - kFirstEccentricitySquared * r_0), 2) + z * z);
|
||||
double V = sqrt(
|
||||
pow((r - kFirstEccentricitySquared * r_0), 2) + (1 - kFirstEccentricitySquared) * z * z);
|
||||
double Z_0 = kSemiminorAxis * kSemiminorAxis * z / (kSemimajorAxis * V);
|
||||
*altitude = static_cast<float>(U * (1 - kSemiminorAxis * kSemiminorAxis / (kSemimajorAxis * V)));
|
||||
*latitude = rad2Deg(atan((z + kSecondEccentricitySquared * Z_0) / r));
|
||||
*longitude = rad2Deg(atan2(y, x));
|
||||
}
|
||||
void geodetic2Ecef(const double latitude, const double longitude, const double altitude, double* x,
|
||||
double* y, double* z)
|
||||
{
|
||||
// Convert geodetic coordinates to ECEF.
|
||||
// http://code.google.com/p/pysatel/source/browse/trunk/coord.py?r=22
|
||||
double lat_rad = deg2Rad(latitude);
|
||||
double lon_rad = deg2Rad(longitude);
|
||||
double xi = sqrt(1 - kFirstEccentricitySquared * sin(lat_rad) * sin(lat_rad));
|
||||
*x = (kSemimajorAxis / xi + altitude) * cos(lat_rad) * cos(lon_rad);
|
||||
*y = (kSemimajorAxis / xi + altitude) * cos(lat_rad) * sin(lon_rad);
|
||||
*z = (kSemimajorAxis / xi * (1 - kFirstEccentricitySquared) + altitude) * sin(lat_rad);
|
||||
}
|
||||
|
||||
void ecef2Ned(const double x, const double y, const double z, double* north, double* east,
|
||||
double* down)
|
||||
{
|
||||
// Converts ECEF coordinate position into local-tangent-plane NED.
|
||||
// Coordinates relative to given ECEF coordinate frame.
|
||||
void ecef2Geodetic(const double x, const double y, const double z, double* latitude,
|
||||
double* longitude, float* altitude)
|
||||
{
|
||||
// Convert ECEF coordinates to geodetic coordinates.
|
||||
// J. Zhu, "Conversion of Earth-centered Earth-fixed coordinates
|
||||
// to geodetic coordinates," IEEE Transactions on Aerospace and
|
||||
// Electronic Systems, vol. 30, pp. 957-961, 1994.
|
||||
|
||||
Vector3d vect, ret;
|
||||
vect(0) = x - home_ecef_x_;
|
||||
vect(1) = y - home_ecef_y_;
|
||||
vect(2) = z - home_ecef_z_;
|
||||
ret = ecef_to_ned_matrix_ * vect;
|
||||
*north = ret(0);
|
||||
*east = ret(1);
|
||||
*down = -ret(2);
|
||||
}
|
||||
double r = sqrt(x * x + y * y);
|
||||
double Esq = kSemimajorAxis * kSemimajorAxis - kSemiminorAxis * kSemiminorAxis;
|
||||
double F = 54 * kSemiminorAxis * kSemiminorAxis * z * z;
|
||||
double G = r * r + (1 - kFirstEccentricitySquared) * z * z - kFirstEccentricitySquared * Esq;
|
||||
double C = (kFirstEccentricitySquared * kFirstEccentricitySquared * F * r * r) / pow(G, 3);
|
||||
double S = cbrt(1 + C + sqrt(C * C + 2 * C));
|
||||
double P = F / (3 * pow((S + 1 / S + 1), 2) * G * G);
|
||||
double Q = sqrt(1 + 2 * kFirstEccentricitySquared * kFirstEccentricitySquared * P);
|
||||
double r_0 = -(P * kFirstEccentricitySquared * r) / (1 + Q) + sqrt(
|
||||
0.5 * kSemimajorAxis * kSemimajorAxis * (1 + 1.0 / Q) - P * (1 - kFirstEccentricitySquared) * z * z / (Q * (1 + Q)) - 0.5 * P * r * r);
|
||||
double U = sqrt(pow((r - kFirstEccentricitySquared * r_0), 2) + z * z);
|
||||
double V = sqrt(
|
||||
pow((r - kFirstEccentricitySquared * r_0), 2) + (1 - kFirstEccentricitySquared) * z * z);
|
||||
double Z_0 = kSemiminorAxis * kSemiminorAxis * z / (kSemimajorAxis * V);
|
||||
*altitude = static_cast<float>(U * (1 - kSemiminorAxis * kSemiminorAxis / (kSemimajorAxis * V)));
|
||||
*latitude = rad2Deg(atan((z + kSecondEccentricitySquared * Z_0) / r));
|
||||
*longitude = rad2Deg(atan2(y, x));
|
||||
}
|
||||
|
||||
void ned2Ecef(const double north, const double east, const float down, double* x, double* y,
|
||||
double* z)
|
||||
{
|
||||
// NED (north/east/down) to ECEF coordinates
|
||||
Vector3d ned, ret;
|
||||
ned(0) = north;
|
||||
ned(1) = east;
|
||||
ned(2) = -down;
|
||||
ret = ned_to_ecef_matrix_ * ned;
|
||||
*x = ret(0) + home_ecef_x_;
|
||||
*y = ret(1) + home_ecef_y_;
|
||||
*z = ret(2) + home_ecef_z_;
|
||||
}
|
||||
void ecef2Ned(const double x, const double y, const double z, double* north, double* east,
|
||||
double* down)
|
||||
{
|
||||
// Converts ECEF coordinate position into local-tangent-plane NED.
|
||||
// Coordinates relative to given ECEF coordinate frame.
|
||||
|
||||
void geodetic2Ned(const double latitude, const double longitude, const float altitude,
|
||||
double* north, double* east, double* down)
|
||||
{
|
||||
// Geodetic position to local NED frame
|
||||
double x, y, z;
|
||||
geodetic2Ecef(latitude, longitude, altitude, &x, &y, &z);
|
||||
ecef2Ned(x, y, z, north, east, down);
|
||||
}
|
||||
Vector3d vect, ret;
|
||||
vect(0) = x - home_ecef_x_;
|
||||
vect(1) = y - home_ecef_y_;
|
||||
vect(2) = z - home_ecef_z_;
|
||||
ret = ecef_to_ned_matrix_ * vect;
|
||||
*north = ret(0);
|
||||
*east = ret(1);
|
||||
*down = -ret(2);
|
||||
}
|
||||
|
||||
void ned2Geodetic(const double north, const double east, const float down, double* latitude,
|
||||
double* longitude, float* altitude)
|
||||
{
|
||||
// Local NED position to geodetic coordinates
|
||||
double x, y, z;
|
||||
ned2Ecef(north, east, down, &x, &y, &z);
|
||||
ecef2Geodetic(x, y, z, latitude, longitude, altitude);
|
||||
void ned2Ecef(const double north, const double east, const float down, double* x, double* y,
|
||||
double* z)
|
||||
{
|
||||
// NED (north/east/down) to ECEF coordinates
|
||||
Vector3d ned, ret;
|
||||
ned(0) = north;
|
||||
ned(1) = east;
|
||||
ned(2) = -down;
|
||||
ret = ned_to_ecef_matrix_ * ned;
|
||||
*x = ret(0) + home_ecef_x_;
|
||||
*y = ret(1) + home_ecef_y_;
|
||||
*z = ret(2) + home_ecef_z_;
|
||||
}
|
||||
|
||||
//TODO: above returns wrong altitude if down was positive. This is because sqrt return value would be -ve
|
||||
//but normal sqrt only return +ve. For now we just override it.
|
||||
*altitude = home_altitude_ - down;
|
||||
}
|
||||
void geodetic2Ned(const double latitude, const double longitude, const float altitude,
|
||||
double* north, double* east, double* down)
|
||||
{
|
||||
// Geodetic position to local NED frame
|
||||
double x, y, z;
|
||||
geodetic2Ecef(latitude, longitude, altitude, &x, &y, &z);
|
||||
ecef2Ned(x, y, z, north, east, down);
|
||||
}
|
||||
|
||||
void geodetic2Enu(const double latitude, const double longitude, const double altitude,
|
||||
double* east, double* north, double* up)
|
||||
{
|
||||
// Geodetic position to local ENU frame
|
||||
double x, y, z;
|
||||
geodetic2Ecef(latitude, longitude, altitude, &x, &y, &z);
|
||||
void ned2Geodetic(const double north, const double east, const float down, double* latitude,
|
||||
double* longitude, float* altitude)
|
||||
{
|
||||
// Local NED position to geodetic coordinates
|
||||
double x, y, z;
|
||||
ned2Ecef(north, east, down, &x, &y, &z);
|
||||
ecef2Geodetic(x, y, z, latitude, longitude, altitude);
|
||||
|
||||
double aux_north, aux_east, aux_down;
|
||||
ecef2Ned(x, y, z, &aux_north, &aux_east, &aux_down);
|
||||
//TODO: above returns wrong altitude if down was positive. This is because sqrt return value would be -ve
|
||||
//but normal sqrt only return +ve. For now we just override it.
|
||||
*altitude = home_altitude_ - down;
|
||||
}
|
||||
|
||||
*east = aux_east;
|
||||
*north = aux_north;
|
||||
*up = -aux_down;
|
||||
}
|
||||
void geodetic2Enu(const double latitude, const double longitude, const double altitude,
|
||||
double* east, double* north, double* up)
|
||||
{
|
||||
// Geodetic position to local ENU frame
|
||||
double x, y, z;
|
||||
geodetic2Ecef(latitude, longitude, altitude, &x, &y, &z);
|
||||
|
||||
void enu2Geodetic(const double east, const double north, const float up, double* latitude,
|
||||
double* longitude, float* altitude)
|
||||
{
|
||||
// Local ENU position to geodetic coordinates
|
||||
double aux_north, aux_east, aux_down;
|
||||
ecef2Ned(x, y, z, &aux_north, &aux_east, &aux_down);
|
||||
|
||||
const double aux_north = north;
|
||||
const double aux_east = east;
|
||||
const float aux_down = -up;
|
||||
double x, y, z;
|
||||
ned2Ecef(aux_north, aux_east, aux_down, &x, &y, &z);
|
||||
ecef2Geodetic(x, y, z, latitude, longitude, altitude);
|
||||
}
|
||||
*east = aux_east;
|
||||
*north = aux_north;
|
||||
*up = -aux_down;
|
||||
}
|
||||
|
||||
private:
|
||||
typedef msr::airlib::VectorMathf VectorMath;
|
||||
typedef VectorMath::Vector3d Vector3d;
|
||||
typedef VectorMath::Matrix3x3d Matrix3x3d;
|
||||
void enu2Geodetic(const double east, const double north, const float up, double* latitude,
|
||||
double* longitude, float* altitude)
|
||||
{
|
||||
// Local ENU position to geodetic coordinates
|
||||
|
||||
// Geodetic system parameters
|
||||
static constexpr double kSemimajorAxis = 6378137;
|
||||
static constexpr double kSemiminorAxis = 6356752.3142;
|
||||
static constexpr double kFirstEccentricitySquared = 6.69437999014 * 0.001;
|
||||
static constexpr double kSecondEccentricitySquared = 6.73949674228 * 0.001;
|
||||
static constexpr double kFlattening = 1 / 298.257223563;
|
||||
const double aux_north = north;
|
||||
const double aux_east = east;
|
||||
const float aux_down = -up;
|
||||
double x, y, z;
|
||||
ned2Ecef(aux_north, aux_east, aux_down, &x, &y, &z);
|
||||
ecef2Geodetic(x, y, z, latitude, longitude, altitude);
|
||||
}
|
||||
|
||||
inline Matrix3x3d nRe(const double lat_radians, const double lon_radians)
|
||||
{
|
||||
const double sLat = sin(lat_radians);
|
||||
const double sLon = sin(lon_radians);
|
||||
const double cLat = cos(lat_radians);
|
||||
const double cLon = cos(lon_radians);
|
||||
private:
|
||||
typedef msr::airlib::VectorMathf VectorMath;
|
||||
typedef VectorMath::Vector3d Vector3d;
|
||||
typedef VectorMath::Matrix3x3d Matrix3x3d;
|
||||
|
||||
Matrix3x3d ret;
|
||||
ret(0, 0) = -sLat * cLon;
|
||||
ret(0, 1) = -sLat * sLon;
|
||||
ret(0, 2) = cLat;
|
||||
ret(1, 0) = -sLon;
|
||||
ret(1, 1) = cLon;
|
||||
ret(1, 2) = 0.0;
|
||||
ret(2, 0) = cLat * cLon;
|
||||
ret(2, 1) = cLat * sLon;
|
||||
ret(2, 2) = sLat;
|
||||
// Geodetic system parameters
|
||||
static constexpr double kSemimajorAxis = 6378137;
|
||||
static constexpr double kSemiminorAxis = 6356752.3142;
|
||||
static constexpr double kFirstEccentricitySquared = 6.69437999014 * 0.001;
|
||||
static constexpr double kSecondEccentricitySquared = 6.73949674228 * 0.001;
|
||||
static constexpr double kFlattening = 1 / 298.257223563;
|
||||
|
||||
return ret;
|
||||
}
|
||||
inline Matrix3x3d nRe(const double lat_radians, const double lon_radians)
|
||||
{
|
||||
const double sLat = sin(lat_radians);
|
||||
const double sLon = sin(lon_radians);
|
||||
const double cLat = cos(lat_radians);
|
||||
const double cLon = cos(lon_radians);
|
||||
|
||||
inline double rad2Deg(const double radians)
|
||||
{
|
||||
return (radians / M_PI) * 180.0;
|
||||
}
|
||||
Matrix3x3d ret;
|
||||
ret(0, 0) = -sLat * cLon;
|
||||
ret(0, 1) = -sLat * sLon;
|
||||
ret(0, 2) = cLat;
|
||||
ret(1, 0) = -sLon;
|
||||
ret(1, 1) = cLon;
|
||||
ret(1, 2) = 0.0;
|
||||
ret(2, 0) = cLat * cLon;
|
||||
ret(2, 1) = cLat * sLon;
|
||||
ret(2, 2) = sLat;
|
||||
|
||||
inline double deg2Rad(const double degrees)
|
||||
{
|
||||
return (degrees / 180.0) * M_PI;
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
double home_latitude_rad_, home_latitude_;
|
||||
double home_longitude_rad_, home_longitude_;
|
||||
float home_altitude_;
|
||||
inline double rad2Deg(const double radians)
|
||||
{
|
||||
return (radians / M_PI) * 180.0;
|
||||
}
|
||||
|
||||
double home_ecef_x_;
|
||||
double home_ecef_y_;
|
||||
double home_ecef_z_;
|
||||
inline double deg2Rad(const double degrees)
|
||||
{
|
||||
return (degrees / 180.0) * M_PI;
|
||||
}
|
||||
|
||||
Matrix3x3d ecef_to_ned_matrix_;
|
||||
Matrix3x3d ned_to_ecef_matrix_;
|
||||
double home_latitude_rad_, home_latitude_;
|
||||
double home_longitude_rad_, home_longitude_;
|
||||
float home_altitude_;
|
||||
|
||||
}; // class GeodeticConverter
|
||||
double home_ecef_x_;
|
||||
double home_ecef_y_;
|
||||
double home_ecef_z_;
|
||||
|
||||
}}
|
||||
Matrix3x3d ecef_to_ned_matrix_;
|
||||
Matrix3x3d ned_to_ecef_matrix_;
|
||||
|
||||
}; // class GeodeticConverter
|
||||
}
|
||||
}
|
||||
#endif // GEODETIC_CONVERTER_H_
|
||||
|
|
|
|||
|
|
@ -7,61 +7,67 @@
|
|||
#include "common/Common.hpp"
|
||||
#include "common/common_utils/EnumFlags.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
// This is an abstraction for cameras associated with a vehicle. Each camera has a unique id.
|
||||
class ImageCaptureBase
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
public: //types
|
||||
enum class ImageType : int { //this indexes to array, -1 is special to indicate main camera
|
||||
Scene = 0,
|
||||
DepthPlanar,
|
||||
DepthPerspective,
|
||||
DepthVis,
|
||||
DisparityNormalized,
|
||||
Segmentation,
|
||||
SurfaceNormals,
|
||||
Infrared,
|
||||
Count //must be last
|
||||
};
|
||||
|
||||
struct ImageRequest {
|
||||
std::string camera_name;
|
||||
ImageCaptureBase::ImageType image_type = ImageCaptureBase::ImageType::Scene;
|
||||
bool pixels_as_float = false;
|
||||
bool compress = true;
|
||||
// This is an abstraction for cameras associated with a vehicle. Each camera has a unique id.
|
||||
class ImageCaptureBase
|
||||
{
|
||||
public: //types
|
||||
enum class ImageType : int
|
||||
{ //this indexes to array, -1 is special to indicate main camera
|
||||
Scene = 0,
|
||||
DepthPlanar,
|
||||
DepthPerspective,
|
||||
DepthVis,
|
||||
DisparityNormalized,
|
||||
Segmentation,
|
||||
SurfaceNormals,
|
||||
Infrared,
|
||||
Count //must be last
|
||||
};
|
||||
|
||||
ImageRequest()
|
||||
{}
|
||||
|
||||
ImageRequest(const std::string& camera_name_val, ImageCaptureBase::ImageType image_type_val, bool pixels_as_float_val = false, bool compress_val = true)
|
||||
struct ImageRequest
|
||||
{
|
||||
camera_name = camera_name_val;
|
||||
image_type = image_type_val;
|
||||
pixels_as_float = pixels_as_float_val;
|
||||
compress = compress_val;
|
||||
}
|
||||
std::string camera_name;
|
||||
ImageCaptureBase::ImageType image_type = ImageCaptureBase::ImageType::Scene;
|
||||
bool pixels_as_float = false;
|
||||
bool compress = true;
|
||||
|
||||
ImageRequest()
|
||||
{
|
||||
}
|
||||
|
||||
ImageRequest(const std::string& camera_name_val, ImageCaptureBase::ImageType image_type_val, bool pixels_as_float_val = false, bool compress_val = true)
|
||||
{
|
||||
camera_name = camera_name_val;
|
||||
image_type = image_type_val;
|
||||
pixels_as_float = pixels_as_float_val;
|
||||
compress = compress_val;
|
||||
}
|
||||
};
|
||||
|
||||
struct ImageResponse
|
||||
{
|
||||
vector<uint8_t> image_data_uint8;
|
||||
vector<float> image_data_float;
|
||||
|
||||
std::string camera_name;
|
||||
Vector3r camera_position = Vector3r::Zero();
|
||||
Quaternionr camera_orientation = Quaternionr::Identity();
|
||||
TTimePoint time_stamp = 0;
|
||||
std::string message;
|
||||
bool pixels_as_float = false;
|
||||
bool compress = true;
|
||||
int width = 0, height = 0;
|
||||
ImageType image_type;
|
||||
};
|
||||
|
||||
public: //methods
|
||||
virtual void getImages(const std::vector<ImageRequest>& requests, std::vector<ImageResponse>& responses) const = 0;
|
||||
};
|
||||
|
||||
struct ImageResponse {
|
||||
vector<uint8_t> image_data_uint8;
|
||||
vector<float> image_data_float;
|
||||
|
||||
std::string camera_name;
|
||||
Vector3r camera_position = Vector3r::Zero();
|
||||
Quaternionr camera_orientation = Quaternionr::Identity();
|
||||
TTimePoint time_stamp = 0;
|
||||
std::string message;
|
||||
bool pixels_as_float = false;
|
||||
bool compress = true;
|
||||
int width = 0, height = 0;
|
||||
ImageType image_type;
|
||||
};
|
||||
|
||||
public: //methods
|
||||
virtual void getImages(const std::vector<ImageRequest>& requests, std::vector<ImageResponse>& responses) const = 0;
|
||||
};
|
||||
|
||||
|
||||
}} //namespace
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
@ -8,68 +8,71 @@
|
|||
#include <fstream>
|
||||
#include "common/Common.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
|
||||
class LogFileWriter {
|
||||
public:
|
||||
LogFileWriter()
|
||||
{}
|
||||
LogFileWriter(const string& file_name, bool enabled = true)
|
||||
class LogFileWriter
|
||||
{
|
||||
open(file_name, enabled);
|
||||
}
|
||||
~LogFileWriter()
|
||||
{
|
||||
close();
|
||||
}
|
||||
void open(const string& file_name, bool enabled = true)
|
||||
{
|
||||
close();
|
||||
file_name_ = file_name;
|
||||
enabled_ = enabled;
|
||||
|
||||
if (enabled_)
|
||||
log_file_ = std::ofstream(file_name);
|
||||
}
|
||||
void close()
|
||||
{
|
||||
if (log_file_.is_open())
|
||||
log_file_.close();
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
void write(const T& val)
|
||||
{
|
||||
if (enabled_)
|
||||
log_file_ << val << "\t";
|
||||
}
|
||||
void write(const Vector3r& vec)
|
||||
{
|
||||
if (enabled_)
|
||||
log_file_ << vec.x() << "\t" << vec.y() << "\t" << vec.z() << "\t";
|
||||
}
|
||||
void write(const Quaternionr& q)
|
||||
{
|
||||
if (enabled_) {
|
||||
real_T p, r, y;
|
||||
VectorMath::toEulerianAngle(q, p, r, y);
|
||||
log_file_ << Utils::radiansToDegrees(r) << "\t" << Utils::radiansToDegrees(p) << "\t" << Utils::radiansToDegrees(y) << "\t";
|
||||
public:
|
||||
LogFileWriter()
|
||||
{
|
||||
}
|
||||
}
|
||||
void endl()
|
||||
{
|
||||
if (enabled_) {
|
||||
log_file_ << std::endl;
|
||||
LogFileWriter(const string& file_name, bool enabled = true)
|
||||
{
|
||||
open(file_name, enabled);
|
||||
}
|
||||
}
|
||||
~LogFileWriter()
|
||||
{
|
||||
close();
|
||||
}
|
||||
void open(const string& file_name, bool enabled = true)
|
||||
{
|
||||
close();
|
||||
file_name_ = file_name;
|
||||
enabled_ = enabled;
|
||||
|
||||
private:
|
||||
std::ofstream log_file_;
|
||||
string file_name_;
|
||||
bool enabled_ = false;
|
||||
};
|
||||
if (enabled_)
|
||||
log_file_ = std::ofstream(file_name);
|
||||
}
|
||||
void close()
|
||||
{
|
||||
if (log_file_.is_open())
|
||||
log_file_.close();
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
void write(const T& val)
|
||||
{
|
||||
if (enabled_)
|
||||
log_file_ << val << "\t";
|
||||
}
|
||||
void write(const Vector3r& vec)
|
||||
{
|
||||
if (enabled_)
|
||||
log_file_ << vec.x() << "\t" << vec.y() << "\t" << vec.z() << "\t";
|
||||
}
|
||||
void write(const Quaternionr& q)
|
||||
{
|
||||
if (enabled_) {
|
||||
real_T p, r, y;
|
||||
VectorMath::toEulerianAngle(q, p, r, y);
|
||||
log_file_ << Utils::radiansToDegrees(r) << "\t" << Utils::radiansToDegrees(p) << "\t" << Utils::radiansToDegrees(y) << "\t";
|
||||
}
|
||||
}
|
||||
void endl()
|
||||
{
|
||||
if (enabled_) {
|
||||
log_file_ << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
}} //namespace
|
||||
private:
|
||||
std::ofstream log_file_;
|
||||
string file_name_;
|
||||
bool enabled_ = false;
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -6,74 +6,77 @@
|
|||
|
||||
#include <chrono>
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
|
||||
// This class implements a pid controller.
|
||||
// First call setPoint to set the desired point and the PID control variables
|
||||
// then call control(x) with new observed values and it will return the updated
|
||||
// control output needed to achieve the setPoint goal. Integration is done using
|
||||
// dt measured using system clock.
|
||||
class PidController
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
private:
|
||||
float set_point_;
|
||||
float kProportional_;
|
||||
float kIntegral_;
|
||||
float kDerivative_;
|
||||
float previous_error_;
|
||||
bool previous_set;
|
||||
float sum_;
|
||||
std::chrono::time_point<std::chrono::system_clock> prev_time_;
|
||||
public:
|
||||
//set desired point.
|
||||
void setPoint(float target, float kProportional, float kIntegral, float kDerivative) {
|
||||
set_point_ = target;
|
||||
kProportional_ = kProportional;
|
||||
kIntegral_ = kIntegral;
|
||||
kDerivative_ = kDerivative;
|
||||
prev_time_ = std::chrono::system_clock::now();
|
||||
previous_error_ = 0;
|
||||
sum_ = 0;
|
||||
previous_set = false;
|
||||
}
|
||||
float control(float processVariable) {
|
||||
auto t = std::chrono::system_clock::now();
|
||||
auto diff = std::chrono::system_clock::now() - prev_time_;
|
||||
|
||||
float error = set_point_ - processVariable;
|
||||
if (!previous_set)
|
||||
{
|
||||
previous_set = true;
|
||||
previous_error_ = error;
|
||||
return 0;
|
||||
}
|
||||
// This class implements a pid controller.
|
||||
// First call setPoint to set the desired point and the PID control variables
|
||||
// then call control(x) with new observed values and it will return the updated
|
||||
// control output needed to achieve the setPoint goal. Integration is done using
|
||||
// dt measured using system clock.
|
||||
class PidController
|
||||
{
|
||||
private:
|
||||
float set_point_;
|
||||
float kProportional_;
|
||||
float kIntegral_;
|
||||
float kDerivative_;
|
||||
float previous_error_;
|
||||
bool previous_set;
|
||||
float sum_;
|
||||
std::chrono::time_point<std::chrono::system_clock> prev_time_;
|
||||
|
||||
float dt = static_cast<float>(std::chrono::duration_cast<std::chrono::microseconds>(diff).count()) * 0.000001f;
|
||||
dt = fmax(dt, 0.01f);
|
||||
float proportionalGain = 0;
|
||||
float derivativeGain = 0;
|
||||
float integralGain = 0;
|
||||
public:
|
||||
//set desired point.
|
||||
void setPoint(float target, float kProportional, float kIntegral, float kDerivative)
|
||||
{
|
||||
set_point_ = target;
|
||||
kProportional_ = kProportional;
|
||||
kIntegral_ = kIntegral;
|
||||
kDerivative_ = kDerivative;
|
||||
prev_time_ = std::chrono::system_clock::now();
|
||||
previous_error_ = 0;
|
||||
sum_ = 0;
|
||||
previous_set = false;
|
||||
}
|
||||
float control(float processVariable)
|
||||
{
|
||||
auto t = std::chrono::system_clock::now();
|
||||
auto diff = std::chrono::system_clock::now() - prev_time_;
|
||||
|
||||
if (kProportional_ != 0) {
|
||||
proportionalGain = error * kProportional_;
|
||||
}
|
||||
if (kDerivative_ != 0) {
|
||||
float derivative = (error - previous_error_) / dt;
|
||||
derivativeGain = derivative * kDerivative_;
|
||||
}
|
||||
if (kIntegral_ != 0) {
|
||||
sum_ += error * dt;
|
||||
integralGain = sum_ * kIntegral_;
|
||||
}
|
||||
float error = set_point_ - processVariable;
|
||||
if (!previous_set) {
|
||||
previous_set = true;
|
||||
previous_error_ = error;
|
||||
return 0;
|
||||
}
|
||||
|
||||
previous_error_ = error;
|
||||
prev_time_ = t;
|
||||
float dt = static_cast<float>(std::chrono::duration_cast<std::chrono::microseconds>(diff).count()) * 0.000001f;
|
||||
dt = fmax(dt, 0.01f);
|
||||
float proportionalGain = 0;
|
||||
float derivativeGain = 0;
|
||||
float integralGain = 0;
|
||||
|
||||
return proportionalGain + derivativeGain + integralGain;
|
||||
}
|
||||
};
|
||||
if (kProportional_ != 0) {
|
||||
proportionalGain = error * kProportional_;
|
||||
}
|
||||
if (kDerivative_ != 0) {
|
||||
float derivative = (error - previous_error_) / dt;
|
||||
derivativeGain = derivative * kDerivative_;
|
||||
}
|
||||
if (kIntegral_ != 0) {
|
||||
sum_ += error * dt;
|
||||
integralGain = sum_ * kIntegral_;
|
||||
}
|
||||
|
||||
previous_error_ = error;
|
||||
prev_time_ = t;
|
||||
|
||||
}} //namespace
|
||||
return proportionalGain + derivativeGain + integralGain;
|
||||
}
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -7,80 +7,82 @@
|
|||
#include "ClockBase.hpp"
|
||||
#include "Common.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
//ScalableClock is a clock that can be scaled (i.e. slowed down or speeded up)
|
||||
class ScalableClock : public ClockBase {
|
||||
public:
|
||||
//scale > 1 slows down the clock, < 1 speeds up the clock
|
||||
ScalableClock(double scale = 1, TTimeDelta latency = 0)
|
||||
: scale_(scale), latency_(latency)
|
||||
//ScalableClock is a clock that can be scaled (i.e. slowed down or speeded up)
|
||||
class ScalableClock : public ClockBase
|
||||
{
|
||||
offset_ = latency * (scale_ - 1);
|
||||
start_ = nowNanos();
|
||||
public:
|
||||
//scale > 1 slows down the clock, < 1 speeds up the clock
|
||||
ScalableClock(double scale = 1, TTimeDelta latency = 0)
|
||||
: scale_(scale), latency_(latency)
|
||||
{
|
||||
offset_ = latency * (scale_ - 1);
|
||||
start_ = nowNanos();
|
||||
|
||||
unused(latency_);
|
||||
}
|
||||
unused(latency_);
|
||||
}
|
||||
|
||||
virtual ~ScalableClock() {}
|
||||
virtual ~ScalableClock() {}
|
||||
|
||||
virtual TTimePoint nowNanos() const override
|
||||
{
|
||||
if (offset_ == 0 && scale_ == 1) //optimized normal route
|
||||
return Utils::getTimeSinceEpochNanos();
|
||||
else {
|
||||
/*
|
||||
virtual TTimePoint nowNanos() const override
|
||||
{
|
||||
if (offset_ == 0 && scale_ == 1) //optimized normal route
|
||||
return Utils::getTimeSinceEpochNanos();
|
||||
else {
|
||||
/*
|
||||
Apply scaling and latency.
|
||||
|
||||
Time point is nanoseconds from some reference r. If latency = 0 then r = 0 .
|
||||
scaled time point is then given by (r + ((now - r) / scale)).
|
||||
This becomes (r*(s-1) + now)/scale or (offset + now / scale).
|
||||
*/
|
||||
return static_cast<TTimePoint>((Utils::getTimeSinceEpochNanos() + offset_) / scale_);
|
||||
return static_cast<TTimePoint>((Utils::getTimeSinceEpochNanos() + offset_) / scale_);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
virtual TTimePoint getStart() const override
|
||||
{
|
||||
return start_;
|
||||
}
|
||||
|
||||
virtual void sleep_for(TTimeDelta dt) override
|
||||
{
|
||||
//for intervals > 2ms just sleep otherwise do spilling otherwise delay won't be accurate
|
||||
if (dt > 2.0 / 1000) {
|
||||
TTimeDelta dt_scaled = fromWallDelta(dt);
|
||||
std::this_thread::sleep_for(std::chrono::duration<double>(dt_scaled));
|
||||
virtual TTimePoint getStart() const override
|
||||
{
|
||||
return start_;
|
||||
}
|
||||
else
|
||||
ClockBase::sleep_for(dt);
|
||||
}
|
||||
|
||||
protected:
|
||||
//converts time interval for wall clock to current clock
|
||||
//For example, if implementation is scaled clock simulating 5X spped then below
|
||||
//will retun dt*5. This functions are required to translate time to operating system
|
||||
//which only has concept of wall clock. For example, to make thread sleep for specific duration.
|
||||
virtual void sleep_for(TTimeDelta dt) override
|
||||
{
|
||||
//for intervals > 2ms just sleep otherwise do spilling otherwise delay won't be accurate
|
||||
if (dt > 2.0 / 1000) {
|
||||
TTimeDelta dt_scaled = fromWallDelta(dt);
|
||||
std::this_thread::sleep_for(std::chrono::duration<double>(dt_scaled));
|
||||
}
|
||||
else
|
||||
ClockBase::sleep_for(dt);
|
||||
}
|
||||
|
||||
//wall clock to sim clock
|
||||
virtual TTimeDelta fromWallDelta(TTimeDelta dt) const
|
||||
{
|
||||
return dt * scale_;
|
||||
}
|
||||
//sim clock to wall clock
|
||||
virtual TTimeDelta toWallDelta(TTimeDelta dt) const
|
||||
{
|
||||
return dt / scale_;
|
||||
}
|
||||
protected:
|
||||
//converts time interval for wall clock to current clock
|
||||
//For example, if implementation is scaled clock simulating 5X spped then below
|
||||
//will retun dt*5. This functions are required to translate time to operating system
|
||||
//which only has concept of wall clock. For example, to make thread sleep for specific duration.
|
||||
|
||||
//wall clock to sim clock
|
||||
virtual TTimeDelta fromWallDelta(TTimeDelta dt) const
|
||||
{
|
||||
return dt * scale_;
|
||||
}
|
||||
//sim clock to wall clock
|
||||
virtual TTimeDelta toWallDelta(TTimeDelta dt) const
|
||||
{
|
||||
return dt / scale_;
|
||||
}
|
||||
|
||||
private:
|
||||
double scale_;
|
||||
TTimeDelta latency_;
|
||||
double offset_;
|
||||
TTimePoint start_;
|
||||
|
||||
};
|
||||
|
||||
}} //namespace
|
||||
private:
|
||||
double scale_;
|
||||
TTimeDelta latency_;
|
||||
double offset_;
|
||||
TTimePoint start_;
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -18,230 +18,234 @@ STRICT_MODE_ON
|
|||
#include <mutex>
|
||||
#include "common_utils/FileSystem.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
class Settings {
|
||||
private:
|
||||
std::string full_filepath_;
|
||||
nlohmann::json doc_;
|
||||
bool load_success_ = false;
|
||||
|
||||
private:
|
||||
static std::mutex& getFileAccessMutex()
|
||||
class Settings
|
||||
{
|
||||
static std::mutex file_access;
|
||||
return file_access;
|
||||
}
|
||||
private:
|
||||
std::string full_filepath_;
|
||||
nlohmann::json doc_;
|
||||
bool load_success_ = false;
|
||||
|
||||
public:
|
||||
static Settings& singleton() {
|
||||
static Settings instance;
|
||||
return instance;
|
||||
}
|
||||
private:
|
||||
static std::mutex& getFileAccessMutex()
|
||||
{
|
||||
static std::mutex file_access;
|
||||
return file_access;
|
||||
}
|
||||
|
||||
std::string getFullFilePath() { return full_filepath_; }
|
||||
public:
|
||||
static Settings& singleton()
|
||||
{
|
||||
static Settings instance;
|
||||
return instance;
|
||||
}
|
||||
|
||||
static std::string getUserDirectoryFullPath(std::string fileName)
|
||||
{
|
||||
std::string path = common_utils::FileSystem::getAppDataFolder();
|
||||
return common_utils::FileSystem::combine(path, fileName);
|
||||
}
|
||||
std::string getFullFilePath() { return full_filepath_; }
|
||||
|
||||
static std::string getExecutableFullPath(std::string fileName)
|
||||
{
|
||||
std::string path = common_utils::FileSystem::getExecutableFolder();
|
||||
return common_utils::FileSystem::combine(path, fileName);
|
||||
}
|
||||
static std::string getUserDirectoryFullPath(std::string fileName)
|
||||
{
|
||||
std::string path = common_utils::FileSystem::getAppDataFolder();
|
||||
return common_utils::FileSystem::combine(path, fileName);
|
||||
}
|
||||
|
||||
static Settings& loadJSonString(const std::string& json_str)
|
||||
{
|
||||
singleton().full_filepath_ = "";
|
||||
singleton().load_success_ = false;
|
||||
static std::string getExecutableFullPath(std::string fileName)
|
||||
{
|
||||
std::string path = common_utils::FileSystem::getExecutableFolder();
|
||||
return common_utils::FileSystem::combine(path, fileName);
|
||||
}
|
||||
|
||||
if (json_str.length() > 0) {
|
||||
static Settings& loadJSonString(const std::string& json_str)
|
||||
{
|
||||
singleton().full_filepath_ = "";
|
||||
singleton().load_success_ = false;
|
||||
|
||||
if (json_str.length() > 0) {
|
||||
std::stringstream ss;
|
||||
ss << json_str;
|
||||
ss >> singleton().doc_;
|
||||
singleton().load_success_ = true;
|
||||
}
|
||||
|
||||
return singleton();
|
||||
}
|
||||
std::string saveJSonString()
|
||||
{
|
||||
std::lock_guard<std::mutex> guard(getFileAccessMutex());
|
||||
std::stringstream ss;
|
||||
ss << json_str;
|
||||
ss >> singleton().doc_;
|
||||
singleton().load_success_ = true;
|
||||
ss << std::setw(2) << singleton().doc_ << std::endl;
|
||||
|
||||
return ss.str();
|
||||
}
|
||||
|
||||
return singleton();
|
||||
}
|
||||
std::string saveJSonString()
|
||||
{
|
||||
std::lock_guard<std::mutex> guard(getFileAccessMutex());
|
||||
std::stringstream ss;
|
||||
ss << std::setw(2) << singleton().doc_ << std::endl;
|
||||
static Settings& loadJSonFile(std::string full_filepath)
|
||||
{
|
||||
std::lock_guard<std::mutex> guard(getFileAccessMutex());
|
||||
singleton().full_filepath_ = full_filepath;
|
||||
|
||||
return ss.str();
|
||||
}
|
||||
|
||||
static Settings& loadJSonFile(std::string full_filepath)
|
||||
{
|
||||
std::lock_guard<std::mutex> guard(getFileAccessMutex());
|
||||
singleton().full_filepath_ = full_filepath;
|
||||
singleton().load_success_ = false;
|
||||
|
||||
singleton().load_success_ = false;
|
||||
std::ifstream s;
|
||||
common_utils::FileSystem::openTextFile(full_filepath, s);
|
||||
if (!s.fail()) {
|
||||
s >> singleton().doc_;
|
||||
singleton().load_success_ = true;
|
||||
}
|
||||
|
||||
std::ifstream s;
|
||||
common_utils::FileSystem::openTextFile(full_filepath, s);
|
||||
if (!s.fail()) {
|
||||
s >> singleton().doc_;
|
||||
singleton().load_success_ = true;
|
||||
return singleton();
|
||||
}
|
||||
|
||||
return singleton();
|
||||
}
|
||||
|
||||
bool isLoadSuccess()
|
||||
{
|
||||
return load_success_;
|
||||
}
|
||||
|
||||
bool hasFileName()
|
||||
{
|
||||
return !getFullFilePath().empty();
|
||||
}
|
||||
|
||||
void saveJSonFile(std::string full_filepath)
|
||||
{
|
||||
std::lock_guard<std::mutex> guard(getFileAccessMutex());
|
||||
singleton().full_filepath_ = full_filepath;
|
||||
std::ofstream s;
|
||||
common_utils::FileSystem::createTextFile(full_filepath, s);
|
||||
s << std::setw(2) << doc_ << std::endl;
|
||||
}
|
||||
|
||||
bool getChild(const std::string& name, Settings& child) const
|
||||
{
|
||||
if (doc_.count(name) == 1 &&
|
||||
( doc_[name].type() == nlohmann::detail::value_t::object ||
|
||||
doc_[name].type() == nlohmann::detail::value_t::array
|
||||
)) {
|
||||
child.doc_ = doc_[name].get<nlohmann::json>();
|
||||
return true;
|
||||
bool isLoadSuccess()
|
||||
{
|
||||
return load_success_;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
size_t size() const {
|
||||
return doc_.size();
|
||||
}
|
||||
bool hasFileName()
|
||||
{
|
||||
return !getFullFilePath().empty();
|
||||
}
|
||||
|
||||
template<typename Container>
|
||||
void getChildNames(Container& c) const
|
||||
{
|
||||
for (auto it = doc_.begin(); it != doc_.end(); ++it) {
|
||||
c.push_back(it.key());
|
||||
void saveJSonFile(std::string full_filepath)
|
||||
{
|
||||
std::lock_guard<std::mutex> guard(getFileAccessMutex());
|
||||
singleton().full_filepath_ = full_filepath;
|
||||
std::ofstream s;
|
||||
common_utils::FileSystem::createTextFile(full_filepath, s);
|
||||
s << std::setw(2) << doc_ << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
bool getChild(size_t index, Settings& child) const
|
||||
{
|
||||
if (doc_.size() > index &&
|
||||
( doc_[index].type() == nlohmann::detail::value_t::object ||
|
||||
doc_[index].type() == nlohmann::detail::value_t::array
|
||||
)) {
|
||||
bool getChild(const std::string& name, Settings& child) const
|
||||
{
|
||||
if (doc_.count(name) == 1 &&
|
||||
(doc_[name].type() == nlohmann::detail::value_t::object ||
|
||||
doc_[name].type() == nlohmann::detail::value_t::array)) {
|
||||
child.doc_ = doc_[name].get<nlohmann::json>();
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
child.doc_ = doc_[index].get<nlohmann::json>();
|
||||
return true;
|
||||
size_t size() const
|
||||
{
|
||||
return doc_.size();
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
std::string getString(const std::string& name, std::string defaultValue) const
|
||||
{
|
||||
if (doc_.count(name) == 1) {
|
||||
return doc_[name].get<std::string>();
|
||||
template <typename Container>
|
||||
void getChildNames(Container& c) const
|
||||
{
|
||||
for (auto it = doc_.begin(); it != doc_.end(); ++it) {
|
||||
c.push_back(it.key());
|
||||
}
|
||||
}
|
||||
else {
|
||||
return defaultValue;
|
||||
}
|
||||
}
|
||||
|
||||
double getDouble(const std::string& name, double defaultValue) const
|
||||
{
|
||||
if (doc_.count(name) == 1) {
|
||||
return doc_[name].get<double>();
|
||||
}
|
||||
else {
|
||||
return defaultValue;
|
||||
}
|
||||
}
|
||||
bool getChild(size_t index, Settings& child) const
|
||||
{
|
||||
if (doc_.size() > index &&
|
||||
(doc_[index].type() == nlohmann::detail::value_t::object ||
|
||||
doc_[index].type() == nlohmann::detail::value_t::array)) {
|
||||
|
||||
float getFloat(const std::string& name, float defaultValue) const
|
||||
{
|
||||
if (doc_.count(name) == 1) {
|
||||
return doc_[name].get<float>();
|
||||
child.doc_ = doc_[index].get<nlohmann::json>();
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
else {
|
||||
return defaultValue;
|
||||
}
|
||||
}
|
||||
|
||||
bool getBool(const std::string& name, bool defaultValue) const
|
||||
{
|
||||
if (doc_.count(name) == 1) {
|
||||
return doc_[name].get<bool>();
|
||||
std::string getString(const std::string& name, std::string defaultValue) const
|
||||
{
|
||||
if (doc_.count(name) == 1) {
|
||||
return doc_[name].get<std::string>();
|
||||
}
|
||||
else {
|
||||
return defaultValue;
|
||||
}
|
||||
}
|
||||
else {
|
||||
return defaultValue;
|
||||
}
|
||||
}
|
||||
|
||||
bool hasKey(const std::string& key) const
|
||||
{
|
||||
return doc_.find(key) != doc_.end();
|
||||
}
|
||||
double getDouble(const std::string& name, double defaultValue) const
|
||||
{
|
||||
if (doc_.count(name) == 1) {
|
||||
return doc_[name].get<double>();
|
||||
}
|
||||
else {
|
||||
return defaultValue;
|
||||
}
|
||||
}
|
||||
|
||||
int getInt(const std::string& name, int defaultValue) const
|
||||
{
|
||||
if (doc_.count(name) == 1) {
|
||||
return doc_[name].get<int>();
|
||||
float getFloat(const std::string& name, float defaultValue) const
|
||||
{
|
||||
if (doc_.count(name) == 1) {
|
||||
return doc_[name].get<float>();
|
||||
}
|
||||
else {
|
||||
return defaultValue;
|
||||
}
|
||||
}
|
||||
else {
|
||||
return defaultValue;
|
||||
}
|
||||
}
|
||||
|
||||
bool setString(const std::string& name, std::string value)
|
||||
{
|
||||
if (doc_.count(name) != 1 || doc_[name].type() != nlohmann::detail::value_t::string || doc_[name] != value) {
|
||||
doc_[name] = value;
|
||||
return true;
|
||||
bool getBool(const std::string& name, bool defaultValue) const
|
||||
{
|
||||
if (doc_.count(name) == 1) {
|
||||
return doc_[name].get<bool>();
|
||||
}
|
||||
else {
|
||||
return defaultValue;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
bool setDouble(const std::string& name, double value)
|
||||
{
|
||||
if (doc_.count(name) != 1 || doc_[name].type() != nlohmann::detail::value_t::number_float || static_cast<double>(doc_[name]) != value) {
|
||||
doc_[name] = value;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
bool setBool(const std::string& name, bool value)
|
||||
{
|
||||
if (doc_.count(name) != 1 || doc_[name].type() != nlohmann::detail::value_t::boolean || static_cast<bool>(doc_[name]) != value) {
|
||||
doc_[name] = value;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
bool setInt(const std::string& name, int value)
|
||||
{
|
||||
if (doc_.count(name) != 1 || doc_[name].type() != nlohmann::detail::value_t::number_integer || static_cast<int>(doc_[name]) != value) {
|
||||
doc_[name] = value;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void setChild(const std::string& name, Settings& value)
|
||||
{
|
||||
doc_[name] = value.doc_;
|
||||
}
|
||||
};
|
||||
bool hasKey(const std::string& key) const
|
||||
{
|
||||
return doc_.find(key) != doc_.end();
|
||||
}
|
||||
|
||||
}} //namespace
|
||||
int getInt(const std::string& name, int defaultValue) const
|
||||
{
|
||||
if (doc_.count(name) == 1) {
|
||||
return doc_[name].get<int>();
|
||||
}
|
||||
else {
|
||||
return defaultValue;
|
||||
}
|
||||
}
|
||||
|
||||
bool setString(const std::string& name, std::string value)
|
||||
{
|
||||
if (doc_.count(name) != 1 || doc_[name].type() != nlohmann::detail::value_t::string || doc_[name] != value) {
|
||||
doc_[name] = value;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
bool setDouble(const std::string& name, double value)
|
||||
{
|
||||
if (doc_.count(name) != 1 || doc_[name].type() != nlohmann::detail::value_t::number_float || static_cast<double>(doc_[name]) != value) {
|
||||
doc_[name] = value;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
bool setBool(const std::string& name, bool value)
|
||||
{
|
||||
if (doc_.count(name) != 1 || doc_[name].type() != nlohmann::detail::value_t::boolean || static_cast<bool>(doc_[name]) != value) {
|
||||
doc_[name] = value;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
bool setInt(const std::string& name, int value)
|
||||
{
|
||||
if (doc_.count(name) != 1 || doc_[name].type() != nlohmann::detail::value_t::number_integer || static_cast<int>(doc_[name]) != value) {
|
||||
doc_[name] = value;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void setChild(const std::string& name, Settings& value)
|
||||
{
|
||||
doc_[name] = value.doc_;
|
||||
}
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
@ -9,119 +9,121 @@
|
|||
#include <iomanip>
|
||||
#include "common/Common.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
/*
|
||||
/*
|
||||
This class is simple key-value reporting provider. It can't inherit from
|
||||
UpdatableObject or we will have circular dependency. This class essentially
|
||||
just provides formatted write APIs over string buffer. The UpdatableObject
|
||||
version is provided by StateReporterWrapper. We expect everyone to use
|
||||
StateReporterWrapper instead of StateReporter directly.
|
||||
*/
|
||||
class StateReporter {
|
||||
public:
|
||||
|
||||
StateReporter(int float_precision = 3, bool is_scientific_notation = false)
|
||||
class StateReporter
|
||||
{
|
||||
initialize(float_precision, is_scientific_notation);
|
||||
}
|
||||
void initialize(int float_precision = 3, bool is_scientific_notation = false)
|
||||
{
|
||||
float_precision_ = float_precision;
|
||||
is_scientific_notation_ = is_scientific_notation;
|
||||
|
||||
if (float_precision_ >= 0) {
|
||||
ss_ << std::setprecision(float_precision_);
|
||||
if (is_scientific_notation_)
|
||||
ss_ << std::scientific;
|
||||
else
|
||||
ss_ << std::fixed;
|
||||
public:
|
||||
StateReporter(int float_precision = 3, bool is_scientific_notation = false)
|
||||
{
|
||||
initialize(float_precision, is_scientific_notation);
|
||||
}
|
||||
}
|
||||
void initialize(int float_precision = 3, bool is_scientific_notation = false)
|
||||
{
|
||||
float_precision_ = float_precision;
|
||||
is_scientific_notation_ = is_scientific_notation;
|
||||
|
||||
void clear()
|
||||
{
|
||||
ss_.str(std::string());
|
||||
ss_.clear();
|
||||
}
|
||||
if (float_precision_ >= 0) {
|
||||
ss_ << std::setprecision(float_precision_);
|
||||
if (is_scientific_notation_)
|
||||
ss_ << std::scientific;
|
||||
else
|
||||
ss_ << std::fixed;
|
||||
}
|
||||
}
|
||||
|
||||
string getOutput() const
|
||||
{
|
||||
return ss_.str();
|
||||
}
|
||||
void clear()
|
||||
{
|
||||
ss_.str(std::string());
|
||||
ss_.clear();
|
||||
}
|
||||
|
||||
//write APIs - heading
|
||||
//TODO: need better line end handling
|
||||
void startHeading(string heading, uint heading_size, uint columns = 20)
|
||||
{
|
||||
unused(heading_size);
|
||||
unused(columns);
|
||||
ss_ << "\n";
|
||||
ss_ << heading;
|
||||
}
|
||||
void endHeading(bool end_line, uint heading_size, uint columns = 20)
|
||||
{
|
||||
if (end_line)
|
||||
string getOutput() const
|
||||
{
|
||||
return ss_.str();
|
||||
}
|
||||
|
||||
//write APIs - heading
|
||||
//TODO: need better line end handling
|
||||
void startHeading(string heading, uint heading_size, uint columns = 20)
|
||||
{
|
||||
unused(heading_size);
|
||||
unused(columns);
|
||||
ss_ << "\n";
|
||||
for(int lines = heading_size; lines > 0; --lines)
|
||||
ss_ << std::string(columns, '_') << "\n";
|
||||
}
|
||||
void writeHeading(string heading, uint heading_size = 0, uint columns = 20)
|
||||
{
|
||||
startHeading(heading, heading_size);
|
||||
endHeading(true, heading_size, columns);
|
||||
}
|
||||
ss_ << heading;
|
||||
}
|
||||
void endHeading(bool end_line, uint heading_size, uint columns = 20)
|
||||
{
|
||||
if (end_line)
|
||||
ss_ << "\n";
|
||||
for (int lines = heading_size; lines > 0; --lines)
|
||||
ss_ << std::string(columns, '_') << "\n";
|
||||
}
|
||||
void writeHeading(string heading, uint heading_size = 0, uint columns = 20)
|
||||
{
|
||||
startHeading(heading, heading_size);
|
||||
endHeading(true, heading_size, columns);
|
||||
}
|
||||
|
||||
//write APIs - specialized objects
|
||||
void writeValue(string name, const Vector3r& vector)
|
||||
{
|
||||
ss_ << name << ": " << "(" << vector.norm() << ") - " << "[" <<
|
||||
vector.x() << ", " << vector.y() << ", " << vector.z() <<
|
||||
"]\n";
|
||||
}
|
||||
void writeValue(string name, const Quaternionr& quat)
|
||||
{
|
||||
real_T pitch, roll, yaw;
|
||||
VectorMath::toEulerianAngle(quat, pitch, roll, yaw);
|
||||
//write APIs - specialized objects
|
||||
void writeValue(string name, const Vector3r& vector)
|
||||
{
|
||||
ss_ << name << ": "
|
||||
<< "(" << vector.norm() << ") - "
|
||||
<< "[" << vector.x() << ", " << vector.y() << ", " << vector.z() << "]\n";
|
||||
}
|
||||
void writeValue(string name, const Quaternionr& quat)
|
||||
{
|
||||
real_T pitch, roll, yaw;
|
||||
VectorMath::toEulerianAngle(quat, pitch, roll, yaw);
|
||||
|
||||
ss_ << name << ":\n" <<
|
||||
" euler: (" << roll << ", " << pitch << ", " << yaw << ")\n" <<
|
||||
" quat: [" << quat.w() << ", " << quat.x() << ", " << quat.y() << ", " << quat.z() << "]\n";
|
||||
}
|
||||
ss_ << name << ":\n"
|
||||
<< " euler: (" << roll << ", " << pitch << ", " << yaw << ")\n"
|
||||
<< " quat: [" << quat.w() << ", " << quat.x() << ", " << quat.y() << ", " << quat.z() << "]\n";
|
||||
}
|
||||
|
||||
//write APIs - generic values
|
||||
template<typename T>
|
||||
void writeValue(string name, const T& r)
|
||||
{
|
||||
writeNameOnly(name);
|
||||
writeValueOnly(r, true);
|
||||
}
|
||||
void writeNameOnly(string name)
|
||||
{
|
||||
ss_ << name << ": ";
|
||||
}
|
||||
template<typename T>
|
||||
void writeValueOnly(const T& r, bool end_line_or_tab = false)
|
||||
{
|
||||
ss_ << r;
|
||||
//write APIs - generic values
|
||||
template <typename T>
|
||||
void writeValue(string name, const T& r)
|
||||
{
|
||||
writeNameOnly(name);
|
||||
writeValueOnly(r, true);
|
||||
}
|
||||
void writeNameOnly(string name)
|
||||
{
|
||||
ss_ << name << ": ";
|
||||
}
|
||||
template <typename T>
|
||||
void writeValueOnly(const T& r, bool end_line_or_tab = false)
|
||||
{
|
||||
ss_ << r;
|
||||
|
||||
if (end_line_or_tab)
|
||||
ss_ << "\n";
|
||||
else
|
||||
ss_ << "\t";
|
||||
}
|
||||
void endl()
|
||||
{
|
||||
ss_ << std::endl;
|
||||
}
|
||||
if (end_line_or_tab)
|
||||
ss_ << "\n";
|
||||
else
|
||||
ss_ << "\t";
|
||||
}
|
||||
void endl()
|
||||
{
|
||||
ss_ << std::endl;
|
||||
}
|
||||
|
||||
private:
|
||||
std::stringstream ss_;
|
||||
|
||||
private:
|
||||
std::stringstream ss_;
|
||||
|
||||
int float_precision_ = 2;
|
||||
bool is_scientific_notation_ = false;
|
||||
};
|
||||
|
||||
}} //namespace
|
||||
int float_precision_ = 2;
|
||||
bool is_scientific_notation_ = false;
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -13,119 +13,122 @@
|
|||
#include "UpdatableObject.hpp"
|
||||
#include "StateReporter.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
class StateReporterWrapper : public UpdatableObject {
|
||||
public:
|
||||
static constexpr real_T DefaultReportFreq = 3.0f;
|
||||
|
||||
StateReporterWrapper(bool enabled = false, int float_precision = 3, bool is_scientific_notation = false)
|
||||
class StateReporterWrapper : public UpdatableObject
|
||||
{
|
||||
initialize(enabled, float_precision, is_scientific_notation);
|
||||
}
|
||||
void initialize(bool enabled = false, int float_precision = 3, bool is_scientific_notation = false)
|
||||
{
|
||||
enabled_ = enabled;
|
||||
report_.initialize(float_precision, is_scientific_notation);
|
||||
report_freq_.initialize(DefaultReportFreq);
|
||||
}
|
||||
public:
|
||||
static constexpr real_T DefaultReportFreq = 3.0f;
|
||||
|
||||
void clearReport()
|
||||
{
|
||||
report_.clear();
|
||||
is_wait_complete = false;
|
||||
}
|
||||
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
last_time_ = clock()->nowNanos();
|
||||
clearReport();
|
||||
dt_stats_.clear();
|
||||
report_freq_.reset();
|
||||
}
|
||||
|
||||
virtual void failResetUpdateOrdering(std::string err) override
|
||||
{
|
||||
// Do nothing.
|
||||
// Disable checks for reset/update sequence because
|
||||
// this object may get created but not used.
|
||||
}
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
UpdatableObject::update();
|
||||
|
||||
TTimeDelta dt = clock()->updateSince(last_time_);
|
||||
|
||||
if (enabled_) {
|
||||
dt_stats_.insert(dt);
|
||||
report_freq_.update();
|
||||
is_wait_complete = is_wait_complete || report_freq_.isWaitComplete();
|
||||
StateReporterWrapper(bool enabled = false, int float_precision = 3, bool is_scientific_notation = false)
|
||||
{
|
||||
initialize(enabled, float_precision, is_scientific_notation);
|
||||
}
|
||||
}
|
||||
virtual void reportState(StateReporter& reporter) override
|
||||
{
|
||||
//TODO: perhaps we should be using supplied reporter?
|
||||
unused(reporter);
|
||||
|
||||
//write dt stats
|
||||
report_.writeNameOnly("dt");
|
||||
report_.writeValueOnly(dt_stats_.mean());
|
||||
report_.writeValueOnly(dt_stats_.variance());
|
||||
report_.writeValueOnly(dt_stats_.size(), true);
|
||||
}
|
||||
//*** End: UpdatableState implementation ***//
|
||||
|
||||
bool canReport()
|
||||
{
|
||||
return enabled_ && is_wait_complete;
|
||||
}
|
||||
|
||||
StateReporter* getReporter()
|
||||
{
|
||||
return &report_;
|
||||
}
|
||||
|
||||
string getOutput()
|
||||
{
|
||||
return report_.getOutput();
|
||||
}
|
||||
|
||||
void setReportFreq(real_T freq)
|
||||
{
|
||||
report_freq_.initialize(freq);
|
||||
}
|
||||
|
||||
void setEnable(bool enable)
|
||||
{
|
||||
if (enable == enabled_)
|
||||
return;
|
||||
|
||||
enabled_ = enable;
|
||||
if (enable)
|
||||
void initialize(bool enabled = false, int float_precision = 3, bool is_scientific_notation = false)
|
||||
{
|
||||
enabled_ = enabled;
|
||||
report_.initialize(float_precision, is_scientific_notation);
|
||||
report_freq_.initialize(DefaultReportFreq);
|
||||
else
|
||||
report_freq_.initialize(0);
|
||||
}
|
||||
bool getEnable()
|
||||
{
|
||||
return enabled_;
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
typedef common_utils::OnlineStats OnlineStats;
|
||||
void clearReport()
|
||||
{
|
||||
report_.clear();
|
||||
is_wait_complete = false;
|
||||
}
|
||||
|
||||
StateReporter report_;
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
last_time_ = clock()->nowNanos();
|
||||
clearReport();
|
||||
dt_stats_.clear();
|
||||
report_freq_.reset();
|
||||
}
|
||||
|
||||
OnlineStats dt_stats_;
|
||||
virtual void failResetUpdateOrdering(std::string err) override
|
||||
{
|
||||
// Do nothing.
|
||||
// Disable checks for reset/update sequence because
|
||||
// this object may get created but not used.
|
||||
}
|
||||
|
||||
FrequencyLimiter report_freq_;
|
||||
bool enabled_;
|
||||
bool is_wait_complete = false;
|
||||
TTimePoint last_time_;
|
||||
virtual void update() override
|
||||
{
|
||||
UpdatableObject::update();
|
||||
|
||||
};
|
||||
TTimeDelta dt = clock()->updateSince(last_time_);
|
||||
|
||||
}} //namespace
|
||||
if (enabled_) {
|
||||
dt_stats_.insert(dt);
|
||||
report_freq_.update();
|
||||
is_wait_complete = is_wait_complete || report_freq_.isWaitComplete();
|
||||
}
|
||||
}
|
||||
virtual void reportState(StateReporter& reporter) override
|
||||
{
|
||||
//TODO: perhaps we should be using supplied reporter?
|
||||
unused(reporter);
|
||||
|
||||
//write dt stats
|
||||
report_.writeNameOnly("dt");
|
||||
report_.writeValueOnly(dt_stats_.mean());
|
||||
report_.writeValueOnly(dt_stats_.variance());
|
||||
report_.writeValueOnly(dt_stats_.size(), true);
|
||||
}
|
||||
//*** End: UpdatableState implementation ***//
|
||||
|
||||
bool canReport()
|
||||
{
|
||||
return enabled_ && is_wait_complete;
|
||||
}
|
||||
|
||||
StateReporter* getReporter()
|
||||
{
|
||||
return &report_;
|
||||
}
|
||||
|
||||
string getOutput()
|
||||
{
|
||||
return report_.getOutput();
|
||||
}
|
||||
|
||||
void setReportFreq(real_T freq)
|
||||
{
|
||||
report_freq_.initialize(freq);
|
||||
}
|
||||
|
||||
void setEnable(bool enable)
|
||||
{
|
||||
if (enable == enabled_)
|
||||
return;
|
||||
|
||||
enabled_ = enable;
|
||||
if (enable)
|
||||
report_freq_.initialize(DefaultReportFreq);
|
||||
else
|
||||
report_freq_.initialize(0);
|
||||
}
|
||||
bool getEnable()
|
||||
{
|
||||
return enabled_;
|
||||
}
|
||||
|
||||
private:
|
||||
typedef common_utils::OnlineStats OnlineStats;
|
||||
|
||||
StateReporter report_;
|
||||
|
||||
OnlineStats dt_stats_;
|
||||
|
||||
FrequencyLimiter report_freq_;
|
||||
bool enabled_;
|
||||
bool is_wait_complete = false;
|
||||
TTimePoint last_time_;
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -8,60 +8,63 @@
|
|||
#include "Common.hpp"
|
||||
#include <atomic>
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
class SteppableClock : public ClockBase {
|
||||
public:
|
||||
static constexpr real_T DefaultStepSize = 20E-3f;
|
||||
|
||||
//Debug clock allows to advance the clock manually in arbitrary way
|
||||
//TTimePoint is nano seconds passed since some fixed point in time
|
||||
//step is how much we would advance the clock by default when calling step()
|
||||
//by default we advance by 20ms
|
||||
SteppableClock(TTimeDelta step = DefaultStepSize, TTimePoint start = 0)
|
||||
: current_(start), step_(step)
|
||||
class SteppableClock : public ClockBase
|
||||
{
|
||||
start_ = current_ = start ? start : Utils::getTimeSinceEpochNanos();
|
||||
}
|
||||
public:
|
||||
static constexpr real_T DefaultStepSize = 20E-3f;
|
||||
|
||||
virtual ~SteppableClock() {}
|
||||
//Debug clock allows to advance the clock manually in arbitrary way
|
||||
//TTimePoint is nano seconds passed since some fixed point in time
|
||||
//step is how much we would advance the clock by default when calling step()
|
||||
//by default we advance by 20ms
|
||||
SteppableClock(TTimeDelta step = DefaultStepSize, TTimePoint start = 0)
|
||||
: current_(start), step_(step)
|
||||
{
|
||||
start_ = current_ = start ? start : Utils::getTimeSinceEpochNanos();
|
||||
}
|
||||
|
||||
TTimePoint stepBy(TTimeDelta amount)
|
||||
{
|
||||
current_ = addTo(current_, amount);
|
||||
return current_;
|
||||
}
|
||||
virtual ~SteppableClock() {}
|
||||
|
||||
virtual TTimePoint step() override
|
||||
{
|
||||
ClockBase::step();
|
||||
TTimePoint stepBy(TTimeDelta amount)
|
||||
{
|
||||
current_ = addTo(current_, amount);
|
||||
return current_;
|
||||
}
|
||||
|
||||
current_ = addTo(current_, step_);
|
||||
return current_;
|
||||
}
|
||||
virtual TTimePoint step() override
|
||||
{
|
||||
ClockBase::step();
|
||||
|
||||
TTimeDelta getStepSize() const
|
||||
{
|
||||
return step_;
|
||||
}
|
||||
current_ = addTo(current_, step_);
|
||||
return current_;
|
||||
}
|
||||
|
||||
virtual TTimePoint nowNanos() const override
|
||||
{
|
||||
return current_;
|
||||
}
|
||||
TTimeDelta getStepSize() const
|
||||
{
|
||||
return step_;
|
||||
}
|
||||
|
||||
virtual TTimePoint getStart() const override
|
||||
{
|
||||
return start_;
|
||||
}
|
||||
virtual TTimePoint nowNanos() const override
|
||||
{
|
||||
return current_;
|
||||
}
|
||||
|
||||
virtual TTimePoint getStart() const override
|
||||
{
|
||||
return start_;
|
||||
}
|
||||
|
||||
private:
|
||||
std::atomic<TTimePoint> current_;
|
||||
std::atomic<TTimePoint> start_;
|
||||
private:
|
||||
std::atomic<TTimePoint> current_;
|
||||
std::atomic<TTimePoint> start_;
|
||||
|
||||
TTimeDelta step_;
|
||||
};
|
||||
|
||||
}} //namespace
|
||||
TTimeDelta step_;
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -7,72 +7,74 @@
|
|||
#include "UpdatableObject.hpp"
|
||||
#include "common/Common.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
template<typename TUpdatableObjectPtr>
|
||||
class UpdatableContainer : public UpdatableObject
|
||||
namespace msr
|
||||
{
|
||||
public: //limited container interface
|
||||
typedef vector<TUpdatableObjectPtr> MembersContainer;
|
||||
typedef typename MembersContainer::iterator iterator;
|
||||
typedef typename MembersContainer::const_iterator const_iterator;
|
||||
typedef typename MembersContainer::value_type value_type;
|
||||
iterator begin() { return members_.begin(); }
|
||||
iterator end() { return members_.end(); }
|
||||
const_iterator begin() const { return members_.begin(); }
|
||||
const_iterator end() const { return members_.end(); }
|
||||
uint size() const { return static_cast<uint>(members_.size()); }
|
||||
const TUpdatableObjectPtr& at(uint index) const { members_.at(index); }
|
||||
TUpdatableObjectPtr& at(uint index) { return members_.at(index); }
|
||||
//allow to override membership modifications
|
||||
virtual void clear()
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
template <typename TUpdatableObjectPtr>
|
||||
class UpdatableContainer : public UpdatableObject
|
||||
{
|
||||
for (auto m : members_) {
|
||||
m->setParent(nullptr);
|
||||
public: //limited container interface
|
||||
typedef vector<TUpdatableObjectPtr> MembersContainer;
|
||||
typedef typename MembersContainer::iterator iterator;
|
||||
typedef typename MembersContainer::const_iterator const_iterator;
|
||||
typedef typename MembersContainer::value_type value_type;
|
||||
iterator begin() { return members_.begin(); }
|
||||
iterator end() { return members_.end(); }
|
||||
const_iterator begin() const { return members_.begin(); }
|
||||
const_iterator end() const { return members_.end(); }
|
||||
uint size() const { return static_cast<uint>(members_.size()); }
|
||||
const TUpdatableObjectPtr& at(uint index) const { members_.at(index); }
|
||||
TUpdatableObjectPtr& at(uint index) { return members_.at(index); }
|
||||
//allow to override membership modifications
|
||||
virtual void clear()
|
||||
{
|
||||
for (auto m : members_) {
|
||||
m->setParent(nullptr);
|
||||
}
|
||||
members_.clear();
|
||||
}
|
||||
virtual void insert(TUpdatableObjectPtr member)
|
||||
{
|
||||
member->setParent(this);
|
||||
members_.push_back(member);
|
||||
}
|
||||
virtual void erase_remove(TUpdatableObjectPtr member)
|
||||
{
|
||||
member->setParent(nullptr);
|
||||
members_.erase(std::remove(members_.begin(), members_.end(), member), members_.end());
|
||||
}
|
||||
members_.clear();
|
||||
}
|
||||
virtual void insert(TUpdatableObjectPtr member)
|
||||
{
|
||||
member->setParent(this);
|
||||
members_.push_back(member);
|
||||
}
|
||||
virtual void erase_remove(TUpdatableObjectPtr member)
|
||||
{
|
||||
member->setParent(nullptr);
|
||||
members_.erase(std::remove(members_.begin(), members_.end(), member), members_.end());
|
||||
}
|
||||
|
||||
public:
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
for (TUpdatableObjectPtr& member : members_)
|
||||
member->reset();
|
||||
}
|
||||
|
||||
public:
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
for (TUpdatableObjectPtr& member : members_)
|
||||
member->reset();
|
||||
}
|
||||
virtual void update() override
|
||||
{
|
||||
UpdatableObject::update();
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
UpdatableObject::update();
|
||||
for (TUpdatableObjectPtr& member : members_)
|
||||
member->update();
|
||||
}
|
||||
|
||||
for (TUpdatableObjectPtr& member : members_)
|
||||
member->update();
|
||||
}
|
||||
virtual void reportState(StateReporter& reporter) override
|
||||
{
|
||||
for (TUpdatableObjectPtr& member : members_)
|
||||
member->reportState(reporter);
|
||||
}
|
||||
|
||||
virtual void reportState(StateReporter& reporter) override
|
||||
{
|
||||
for (TUpdatableObjectPtr& member : members_)
|
||||
member->reportState(reporter);
|
||||
}
|
||||
//*** End: UpdatableState implementation ***//
|
||||
|
||||
//*** End: UpdatableState implementation ***//
|
||||
virtual ~UpdatableContainer() = default;
|
||||
|
||||
virtual ~UpdatableContainer() = default;
|
||||
|
||||
private:
|
||||
MembersContainer members_;
|
||||
};
|
||||
|
||||
}} //namespace
|
||||
private:
|
||||
MembersContainer members_;
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -8,9 +8,12 @@
|
|||
#include "StateReporter.hpp"
|
||||
#include "ClockFactory.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
/*
|
||||
/*
|
||||
UpdatableObject provides generalized framework for things that needs to be "ticked". For example,
|
||||
physics objects that needs to update its position every 10ms.
|
||||
Typically this objects will take their current state, do some processing and produce new state
|
||||
|
|
@ -24,89 +27,89 @@ Do not call reset() from constructor or initialization because that will produce
|
|||
init->reset calls for base-derived class that would be incorrect.
|
||||
*/
|
||||
|
||||
class UpdatableObject
|
||||
{
|
||||
public:
|
||||
void reset()
|
||||
{
|
||||
if (reset_in_progress)
|
||||
return;
|
||||
|
||||
reset_in_progress = true;
|
||||
//TODO: Do we need this check anymore? Maybe reset() should be idempotent.
|
||||
|
||||
if (reset_called && !update_called)
|
||||
failResetUpdateOrdering("Multiple reset() calls detected without call to update()");
|
||||
|
||||
reset_called = true;
|
||||
|
||||
resetImplementation();
|
||||
reset_in_progress = false;
|
||||
}
|
||||
|
||||
virtual void update()
|
||||
class UpdatableObject
|
||||
{
|
||||
if (!reset_called)
|
||||
failResetUpdateOrdering("reset() must be called first before update()");
|
||||
update_called = true;
|
||||
}
|
||||
public:
|
||||
void reset()
|
||||
{
|
||||
if (reset_in_progress)
|
||||
return;
|
||||
|
||||
virtual ~UpdatableObject() = default;
|
||||
reset_in_progress = true;
|
||||
//TODO: Do we need this check anymore? Maybe reset() should be idempotent.
|
||||
|
||||
virtual void reportState(StateReporter& reporter)
|
||||
{
|
||||
unused(reporter);
|
||||
//default implementation doesn't do anything
|
||||
}
|
||||
if (reset_called && !update_called)
|
||||
failResetUpdateOrdering("Multiple reset() calls detected without call to update()");
|
||||
|
||||
virtual UpdatableObject* getPhysicsBody()
|
||||
{
|
||||
return nullptr;
|
||||
}
|
||||
reset_called = true;
|
||||
|
||||
virtual ClockBase* clock()
|
||||
{
|
||||
return ClockFactory::get();
|
||||
}
|
||||
virtual const ClockBase* clock() const
|
||||
{
|
||||
return ClockFactory::get();
|
||||
}
|
||||
resetImplementation();
|
||||
reset_in_progress = false;
|
||||
}
|
||||
|
||||
UpdatableObject* getParent()
|
||||
{
|
||||
return parent_;
|
||||
}
|
||||
virtual void update()
|
||||
{
|
||||
if (!reset_called)
|
||||
failResetUpdateOrdering("reset() must be called first before update()");
|
||||
update_called = true;
|
||||
}
|
||||
|
||||
void setParent(UpdatableObject* container)
|
||||
{
|
||||
parent_ = container;
|
||||
}
|
||||
virtual ~UpdatableObject() = default;
|
||||
|
||||
std::string getName()
|
||||
{
|
||||
return name_;
|
||||
}
|
||||
virtual void reportState(StateReporter& reporter)
|
||||
{
|
||||
unused(reporter);
|
||||
//default implementation doesn't do anything
|
||||
}
|
||||
|
||||
void setName(const std::string& name)
|
||||
{
|
||||
this->name_ = name;
|
||||
}
|
||||
virtual UpdatableObject* getPhysicsBody()
|
||||
{
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
protected:
|
||||
virtual void resetImplementation() = 0;
|
||||
virtual void failResetUpdateOrdering(std::string err)
|
||||
{
|
||||
throw std::runtime_error(err);
|
||||
}
|
||||
virtual ClockBase* clock()
|
||||
{
|
||||
return ClockFactory::get();
|
||||
}
|
||||
virtual const ClockBase* clock() const
|
||||
{
|
||||
return ClockFactory::get();
|
||||
}
|
||||
|
||||
private:
|
||||
bool reset_called = false;
|
||||
bool update_called = false;
|
||||
bool reset_in_progress = false;
|
||||
UpdatableObject* parent_ = nullptr;
|
||||
std::string name_;
|
||||
};
|
||||
UpdatableObject* getParent()
|
||||
{
|
||||
return parent_;
|
||||
}
|
||||
|
||||
}} //namespace
|
||||
void setParent(UpdatableObject* container)
|
||||
{
|
||||
parent_ = container;
|
||||
}
|
||||
|
||||
std::string getName()
|
||||
{
|
||||
return name_;
|
||||
}
|
||||
|
||||
void setName(const std::string& name)
|
||||
{
|
||||
this->name_ = name;
|
||||
}
|
||||
|
||||
protected:
|
||||
virtual void resetImplementation() = 0;
|
||||
virtual void failResetUpdateOrdering(std::string err)
|
||||
{
|
||||
throw std::runtime_error(err);
|
||||
}
|
||||
|
||||
private:
|
||||
bool reset_called = false;
|
||||
bool update_called = false;
|
||||
bool reset_in_progress = false;
|
||||
UpdatableObject* parent_ = nullptr;
|
||||
std::string name_;
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
Разница между файлами не показана из-за своего большого размера
Загрузить разницу
|
|
@ -11,66 +11,70 @@
|
|||
#include "common/ClockFactory.hpp"
|
||||
#include "common/CancelToken.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
class Waiter {
|
||||
public:
|
||||
Waiter(TTimeDelta sleep_duration_seconds, TTimeDelta timeout_sec, CancelToken& cancelable_action)
|
||||
: sleep_duration_(sleep_duration_seconds), timeout_sec_(timeout_sec),
|
||||
cancelable_action_(cancelable_action), is_complete_(false)
|
||||
class Waiter
|
||||
{
|
||||
proc_start_ = loop_start_ = clock()->nowNanos();
|
||||
}
|
||||
public:
|
||||
Waiter(TTimeDelta sleep_duration_seconds, TTimeDelta timeout_sec, CancelToken& cancelable_action)
|
||||
: sleep_duration_(sleep_duration_seconds), timeout_sec_(timeout_sec), cancelable_action_(cancelable_action), is_complete_(false)
|
||||
{
|
||||
proc_start_ = loop_start_ = clock()->nowNanos();
|
||||
}
|
||||
|
||||
bool sleep()
|
||||
{
|
||||
// Sleeps for the time needed to get current running time up to the requested sleep_duration_.
|
||||
// So this can be used to "throttle" any loop to check something every sleep_duration_ seconds.
|
||||
bool sleep()
|
||||
{
|
||||
// Sleeps for the time needed to get current running time up to the requested sleep_duration_.
|
||||
// So this can be used to "throttle" any loop to check something every sleep_duration_ seconds.
|
||||
|
||||
if (isComplete())
|
||||
throw std::domain_error("Process was already complete. This instance of Waiter shouldn't be reused!");
|
||||
if (isTimeout())
|
||||
return false;
|
||||
if (isComplete())
|
||||
throw std::domain_error("Process was already complete. This instance of Waiter shouldn't be reused!");
|
||||
if (isTimeout())
|
||||
return false;
|
||||
|
||||
//measure time spent since last iteration
|
||||
TTimeDelta running_time = clock()->elapsedSince(loop_start_);
|
||||
double remaining = sleep_duration_ - running_time;
|
||||
bool done = cancelable_action_.sleep(remaining);
|
||||
loop_start_ = clock()->nowNanos();
|
||||
return done;
|
||||
}
|
||||
//measure time spent since last iteration
|
||||
TTimeDelta running_time = clock()->elapsedSince(loop_start_);
|
||||
double remaining = sleep_duration_ - running_time;
|
||||
bool done = cancelable_action_.sleep(remaining);
|
||||
loop_start_ = clock()->nowNanos();
|
||||
return done;
|
||||
}
|
||||
|
||||
//call this mark process as complete
|
||||
void complete()
|
||||
{
|
||||
is_complete_ = true;
|
||||
}
|
||||
//call this mark process as complete
|
||||
void complete()
|
||||
{
|
||||
is_complete_ = true;
|
||||
}
|
||||
|
||||
bool isComplete() const
|
||||
{
|
||||
return is_complete_;
|
||||
}
|
||||
bool isComplete() const
|
||||
{
|
||||
return is_complete_;
|
||||
}
|
||||
|
||||
bool isTimeout() const
|
||||
{
|
||||
if (isComplete())
|
||||
return false;
|
||||
else
|
||||
return clock()->elapsedSince(proc_start_) >= timeout_sec_;
|
||||
}
|
||||
private:
|
||||
TTimeDelta sleep_duration_, timeout_sec_;
|
||||
CancelToken& cancelable_action_;
|
||||
bool is_complete_; //each waiter should maintain its own complete status
|
||||
bool isTimeout() const
|
||||
{
|
||||
if (isComplete())
|
||||
return false;
|
||||
else
|
||||
return clock()->elapsedSince(proc_start_) >= timeout_sec_;
|
||||
}
|
||||
|
||||
TTimePoint proc_start_;
|
||||
TTimePoint loop_start_;
|
||||
private:
|
||||
TTimeDelta sleep_duration_, timeout_sec_;
|
||||
CancelToken& cancelable_action_;
|
||||
bool is_complete_; //each waiter should maintain its own complete status
|
||||
|
||||
static ClockBase* clock()
|
||||
{
|
||||
return ClockFactory::get();
|
||||
}
|
||||
};
|
||||
TTimePoint proc_start_;
|
||||
TTimePoint loop_start_;
|
||||
|
||||
}} //namespace
|
||||
static ClockBase* clock()
|
||||
{
|
||||
return ClockFactory::get();
|
||||
}
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -16,296 +16,303 @@
|
|||
#include "ClockFactory.hpp" //TODO: move this out of common_utils
|
||||
#include "CancelToken.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
class CancelableAction : public CancelToken {
|
||||
protected:
|
||||
virtual void executeAction() = 0;
|
||||
|
||||
public:
|
||||
CancelableAction()
|
||||
: is_complete_(false)
|
||||
{
|
||||
}
|
||||
virtual ~CancelableAction();
|
||||
|
||||
void reset()
|
||||
{
|
||||
is_complete_ = false;
|
||||
}
|
||||
|
||||
void execute()
|
||||
{
|
||||
try {
|
||||
executeAction();
|
||||
is_complete_ = true;
|
||||
}
|
||||
catch (...) {
|
||||
is_complete_ = false;
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
bool isComplete() const
|
||||
{
|
||||
return is_complete_;
|
||||
}
|
||||
|
||||
private:
|
||||
std::atomic<bool> is_complete_;
|
||||
|
||||
};
|
||||
|
||||
// This wraps a condition_variable so we can handle the case where we may signal before wait
|
||||
// and implement the semantics that say wait should be a noop in that case.
|
||||
class WorkerThreadSignal
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
std::condition_variable cv_;
|
||||
std::mutex mutex_;
|
||||
std::atomic<bool> signaled_;
|
||||
public:
|
||||
WorkerThreadSignal() : signaled_(false)
|
||||
{
|
||||
}
|
||||
|
||||
void signal()
|
||||
class CancelableAction : public CancelToken
|
||||
{
|
||||
protected:
|
||||
virtual void executeAction() = 0;
|
||||
|
||||
public:
|
||||
CancelableAction()
|
||||
: is_complete_(false)
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
signaled_ = true;
|
||||
}
|
||||
cv_.notify_one();
|
||||
}
|
||||
virtual ~CancelableAction();
|
||||
|
||||
template<class _Predicate>
|
||||
void wait(_Predicate cancel)
|
||||
{
|
||||
// wait for signal or cancel predicate
|
||||
while (!signaled_) {
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
cv_.wait_for(lock, std::chrono::milliseconds(1), [this, cancel] {
|
||||
return cancel();
|
||||
});
|
||||
void reset()
|
||||
{
|
||||
is_complete_ = false;
|
||||
}
|
||||
signaled_ = false;
|
||||
}
|
||||
|
||||
bool waitFor(double timeout_sec)
|
||||
{
|
||||
// wait for signal or timeout or cancel predicate
|
||||
while (!signaled_) {
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
cv_.wait_for(lock, std::chrono::milliseconds(
|
||||
static_cast<long long>(timeout_sec * 1000)));
|
||||
void execute()
|
||||
{
|
||||
try {
|
||||
executeAction();
|
||||
is_complete_ = true;
|
||||
}
|
||||
catch (...) {
|
||||
is_complete_ = false;
|
||||
throw;
|
||||
}
|
||||
}
|
||||
signaled_ = false;
|
||||
return true;
|
||||
}
|
||||
|
||||
void wait()
|
||||
{
|
||||
// wait for signal or timeout or cancel predicate
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
while (!signaled_) {
|
||||
cv_.wait(lock);
|
||||
bool isComplete() const
|
||||
{
|
||||
return is_complete_;
|
||||
}
|
||||
lock.unlock();
|
||||
signaled_ = false;
|
||||
}
|
||||
|
||||
bool waitForRetry(double timeout_sec, int n_times)
|
||||
private:
|
||||
std::atomic<bool> is_complete_;
|
||||
};
|
||||
|
||||
// This wraps a condition_variable so we can handle the case where we may signal before wait
|
||||
// and implement the semantics that say wait should be a noop in that case.
|
||||
class WorkerThreadSignal
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
while (!signaled_ && n_times > 0) {
|
||||
cv_.wait_for(lock, std::chrono::milliseconds(static_cast<long long>(timeout_sec * 1000)));
|
||||
--n_times;
|
||||
std::condition_variable cv_;
|
||||
std::mutex mutex_;
|
||||
std::atomic<bool> signaled_;
|
||||
|
||||
public:
|
||||
WorkerThreadSignal()
|
||||
: signaled_(false)
|
||||
{
|
||||
}
|
||||
lock.unlock();
|
||||
if (n_times == 0 && !signaled_) {
|
||||
return false;
|
||||
|
||||
void signal()
|
||||
{
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
signaled_ = true;
|
||||
}
|
||||
cv_.notify_one();
|
||||
}
|
||||
else {
|
||||
|
||||
template <class _Predicate>
|
||||
void wait(_Predicate cancel)
|
||||
{
|
||||
// wait for signal or cancel predicate
|
||||
while (!signaled_) {
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
cv_.wait_for(lock, std::chrono::milliseconds(1), [this, cancel] {
|
||||
return cancel();
|
||||
});
|
||||
}
|
||||
signaled_ = false;
|
||||
}
|
||||
|
||||
bool waitFor(double timeout_sec)
|
||||
{
|
||||
// wait for signal or timeout or cancel predicate
|
||||
while (!signaled_) {
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
cv_.wait_for(lock, std::chrono::milliseconds(static_cast<long long>(timeout_sec * 1000)));
|
||||
}
|
||||
signaled_ = false;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
};
|
||||
void wait()
|
||||
{
|
||||
// wait for signal or timeout or cancel predicate
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
while (!signaled_) {
|
||||
cv_.wait(lock);
|
||||
}
|
||||
lock.unlock();
|
||||
signaled_ = false;
|
||||
}
|
||||
|
||||
// This class provides a synchronized worker thread that guarantees to execute
|
||||
// cancelable tasks on a background thread one at a time.
|
||||
// It guarantees that previous task is canceled before new task is started.
|
||||
// The queue size is 1, which means it does NOT guarantee all queued tasks are executed.
|
||||
// If enqueue is called very quickly the thread will not have a chance to execute each
|
||||
// task before they get canceled, worst case in a tight loop all tasks are starved and
|
||||
// nothing executes.
|
||||
class WorkerThread {
|
||||
public:
|
||||
WorkerThread()
|
||||
: thread_running_(false), cancel_request_(false) {
|
||||
}
|
||||
bool waitForRetry(double timeout_sec, int n_times)
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
while (!signaled_ && n_times > 0) {
|
||||
cv_.wait_for(lock, std::chrono::milliseconds(static_cast<long long>(timeout_sec * 1000)));
|
||||
--n_times;
|
||||
}
|
||||
lock.unlock();
|
||||
if (n_times == 0 && !signaled_) {
|
||||
return false;
|
||||
}
|
||||
else {
|
||||
signaled_ = false;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
~WorkerThread() {
|
||||
cancel_request_ = true;
|
||||
cancel();
|
||||
}
|
||||
void enqueue(std::shared_ptr<CancelableAction> item) {
|
||||
//cancel previous item
|
||||
// This class provides a synchronized worker thread that guarantees to execute
|
||||
// cancelable tasks on a background thread one at a time.
|
||||
// It guarantees that previous task is canceled before new task is started.
|
||||
// The queue size is 1, which means it does NOT guarantee all queued tasks are executed.
|
||||
// If enqueue is called very quickly the thread will not have a chance to execute each
|
||||
// task before they get canceled, worst case in a tight loop all tasks are starved and
|
||||
// nothing executes.
|
||||
class WorkerThread
|
||||
{
|
||||
public:
|
||||
WorkerThread()
|
||||
: thread_running_(false), cancel_request_(false)
|
||||
{
|
||||
}
|
||||
|
||||
~WorkerThread()
|
||||
{
|
||||
cancel_request_ = true;
|
||||
cancel();
|
||||
}
|
||||
void enqueue(std::shared_ptr<CancelableAction> item)
|
||||
{
|
||||
//cancel previous item
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
std::shared_ptr<CancelableAction> pending = pending_item_;
|
||||
if (pending != nullptr) {
|
||||
pending->cancel();
|
||||
}
|
||||
}
|
||||
|
||||
bool running = false;
|
||||
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
pending_item_ = item;
|
||||
running = thread_running_;
|
||||
}
|
||||
|
||||
if (running) {
|
||||
item_arrived_.signal();
|
||||
}
|
||||
else {
|
||||
start();
|
||||
}
|
||||
}
|
||||
|
||||
bool enqueueAndWait(std::shared_ptr<CancelableAction> item, float timeout_sec)
|
||||
{
|
||||
//cancel previous item
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
std::shared_ptr<CancelableAction> pending = pending_item_;
|
||||
if (pending != nullptr) {
|
||||
pending->cancel();
|
||||
}
|
||||
}
|
||||
|
||||
bool running = false;
|
||||
|
||||
//set new item to run
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
pending_item_ = item;
|
||||
running = thread_running_;
|
||||
}
|
||||
|
||||
if (running) {
|
||||
item_arrived_.signal();
|
||||
}
|
||||
else {
|
||||
start();
|
||||
}
|
||||
|
||||
item->sleep(timeout_sec);
|
||||
|
||||
//after the wait if item is still running then cancel it
|
||||
if (!item->isCancelled() && !item->isComplete())
|
||||
item->cancel();
|
||||
|
||||
return !item->isCancelled();
|
||||
}
|
||||
|
||||
void cancel()
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
std::shared_ptr<CancelableAction> pending = pending_item_;
|
||||
pending_item_ = nullptr;
|
||||
if (pending != nullptr) {
|
||||
pending->cancel();
|
||||
}
|
||||
}
|
||||
|
||||
bool running = false;
|
||||
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
pending_item_ = item;
|
||||
running = thread_running_;
|
||||
}
|
||||
|
||||
if (running) {
|
||||
item_arrived_.signal();
|
||||
}
|
||||
else {
|
||||
start();
|
||||
}
|
||||
}
|
||||
|
||||
bool enqueueAndWait(std::shared_ptr<CancelableAction> item, float timeout_sec)
|
||||
{
|
||||
//cancel previous item
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
std::shared_ptr<CancelableAction> pending = pending_item_;
|
||||
if (pending != nullptr) {
|
||||
pending->cancel();
|
||||
if (thread_.joinable()) {
|
||||
item_arrived_.signal();
|
||||
thread_.join();
|
||||
}
|
||||
}
|
||||
|
||||
bool running = false;
|
||||
|
||||
//set new item to run
|
||||
private:
|
||||
void start()
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
pending_item_ = item;
|
||||
running = thread_running_;
|
||||
//if state == not running
|
||||
if (!thread_running_) {
|
||||
|
||||
//make sure C++ previous thread is done
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
if (thread_.joinable()) {
|
||||
thread_.join();
|
||||
}
|
||||
}
|
||||
Utils::cleanupThread(thread_);
|
||||
|
||||
//start the thread
|
||||
cancel_request_ = false;
|
||||
thread_ = std::thread(&WorkerThread::run, this);
|
||||
|
||||
//wait until thread tells us it has started
|
||||
thread_started_.wait([this] { return static_cast<bool>(cancel_request_); });
|
||||
}
|
||||
}
|
||||
|
||||
if (running) {
|
||||
item_arrived_.signal();
|
||||
}
|
||||
else {
|
||||
start();
|
||||
}
|
||||
void run()
|
||||
{
|
||||
thread_running_ = true;
|
||||
|
||||
item->sleep(timeout_sec);
|
||||
|
||||
//after the wait if item is still running then cancel it
|
||||
if (!item->isCancelled() && !item->isComplete())
|
||||
item->cancel();
|
||||
|
||||
return !item->isCancelled();
|
||||
}
|
||||
|
||||
void cancel()
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
std::shared_ptr<CancelableAction> pending = pending_item_;
|
||||
pending_item_ = nullptr;
|
||||
if (pending != nullptr) {
|
||||
pending->cancel();
|
||||
}
|
||||
if (thread_.joinable()) {
|
||||
item_arrived_.signal();
|
||||
thread_.join();
|
||||
}
|
||||
}
|
||||
private:
|
||||
void start()
|
||||
{
|
||||
//if state == not running
|
||||
if (!thread_running_) {
|
||||
|
||||
//make sure C++ previous thread is done
|
||||
//tell the thread which started this thread that we are on now
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
if (thread_.joinable()) {
|
||||
thread_.join();
|
||||
}
|
||||
}
|
||||
Utils::cleanupThread(thread_);
|
||||
|
||||
//start the thread
|
||||
cancel_request_ = false;
|
||||
thread_ = std::thread(&WorkerThread::run, this);
|
||||
|
||||
//wait until thread tells us it has started
|
||||
thread_started_.wait([this] { return static_cast<bool>(cancel_request_); });
|
||||
}
|
||||
}
|
||||
|
||||
void run()
|
||||
{
|
||||
thread_running_ = true;
|
||||
|
||||
//tell the thread which started this thread that we are on now
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
thread_started_.signal();
|
||||
}
|
||||
|
||||
//until we don't get stopped and have work to do, keep running
|
||||
while (!cancel_request_ && pending_item_ != nullptr) {
|
||||
std::shared_ptr<CancelableAction> pending;
|
||||
|
||||
//get the pending item
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
pending = pending_item_;
|
||||
thread_started_.signal();
|
||||
}
|
||||
|
||||
//if pending item is not yet cancelled
|
||||
if (pending != nullptr && !pending->isCancelled()) {
|
||||
//until we don't get stopped and have work to do, keep running
|
||||
while (!cancel_request_ && pending_item_ != nullptr) {
|
||||
std::shared_ptr<CancelableAction> pending;
|
||||
|
||||
//execute pending item
|
||||
try {
|
||||
pending->execute();
|
||||
//get the pending item
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
pending = pending_item_;
|
||||
}
|
||||
catch (std::exception& e) {
|
||||
//Utils::DebugBreak();
|
||||
Utils::log(Utils::stringf("WorkerThread caught unhandled exception: %s", e.what()), Utils::kLogLevelError);
|
||||
|
||||
//if pending item is not yet cancelled
|
||||
if (pending != nullptr && !pending->isCancelled()) {
|
||||
|
||||
//execute pending item
|
||||
try {
|
||||
pending->execute();
|
||||
}
|
||||
catch (std::exception& e) {
|
||||
//Utils::DebugBreak();
|
||||
Utils::log(Utils::stringf("WorkerThread caught unhandled exception: %s", e.what()), Utils::kLogLevelError);
|
||||
}
|
||||
}
|
||||
|
||||
if (!cancel_request_) {
|
||||
//wait for next item to arrive or thread is stopped
|
||||
item_arrived_.wait([this] { return static_cast<bool>(cancel_request_); });
|
||||
}
|
||||
}
|
||||
|
||||
if (!cancel_request_) {
|
||||
//wait for next item to arrive or thread is stopped
|
||||
item_arrived_.wait([this] { return static_cast<bool>(cancel_request_); });
|
||||
}
|
||||
thread_running_ = false;
|
||||
}
|
||||
|
||||
thread_running_ = false;
|
||||
}
|
||||
private:
|
||||
//this is used to wait until our thread actually gets started
|
||||
WorkerThreadSignal thread_started_;
|
||||
//when new item arrived, we signal this so waiting thread can continue
|
||||
WorkerThreadSignal item_arrived_;
|
||||
|
||||
private:
|
||||
//this is used to wait until our thread actually gets started
|
||||
WorkerThreadSignal thread_started_;
|
||||
//when new item arrived, we signal this so waiting thread can continue
|
||||
WorkerThreadSignal item_arrived_;
|
||||
|
||||
// thread state
|
||||
std::shared_ptr<CancelableAction> pending_item_;
|
||||
std::mutex mutex_;
|
||||
std::thread thread_;
|
||||
//while run() is in progress this is true
|
||||
std::atomic<bool> thread_running_;
|
||||
//has request to stop this worker thread made?
|
||||
std::atomic<bool> cancel_request_;
|
||||
};
|
||||
|
||||
|
||||
}} //namespace
|
||||
// thread state
|
||||
std::shared_ptr<CancelableAction> pending_item_;
|
||||
std::mutex mutex_;
|
||||
std::thread thread_;
|
||||
//while run() is in progress this is true
|
||||
std::atomic<bool> thread_running_;
|
||||
//has request to stop this worker thread made?
|
||||
std::atomic<bool> cancel_request_;
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
@ -7,14 +7,16 @@
|
|||
#include "ctpl_stl.h"
|
||||
#include <functional>
|
||||
|
||||
class AsyncTasker {
|
||||
class AsyncTasker
|
||||
{
|
||||
public:
|
||||
AsyncTasker(unsigned int thread_count = 4)
|
||||
: threads_(thread_count), error_handler_([](std::exception e) {unused(e);})
|
||||
: threads_(thread_count), error_handler_([](std::exception e) { unused(e); })
|
||||
{
|
||||
}
|
||||
|
||||
void setErrorHandler(std::function<void(std::exception&)> errorHandler) {
|
||||
void setErrorHandler(std::function<void(std::exception&)> errorHandler)
|
||||
{
|
||||
error_handler_ = errorHandler;
|
||||
}
|
||||
|
||||
|
|
@ -23,8 +25,7 @@ public:
|
|||
if (iterations < 1)
|
||||
return;
|
||||
|
||||
if (iterations == 1)
|
||||
{
|
||||
if (iterations == 1) {
|
||||
threads_.push([=](int i) {
|
||||
unused(i);
|
||||
try {
|
||||
|
|
@ -56,5 +57,4 @@ private:
|
|||
std::function<void(std::exception&)> error_handler_;
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -1,13 +1,14 @@
|
|||
#ifndef common_utils_OnlineStats_hpp
|
||||
#define common_utils_OnlineStats_hpp
|
||||
|
||||
namespace common_utils {
|
||||
namespace common_utils
|
||||
{
|
||||
|
||||
class ColorUtils {
|
||||
class ColorUtils
|
||||
{
|
||||
public:
|
||||
|
||||
static void valToRGB(double val0To1, unsigned char& r, unsigned char& g, unsigned char& b)
|
||||
{//actual visible spectrum is 375 to 725 but outside of 400-700 things become too black
|
||||
{ //actual visible spectrum is 375 to 725 but outside of 400-700 things become too black
|
||||
wavelengthToRGB(val0To1 * (700 - 400) + 400, r, g, b);
|
||||
}
|
||||
|
||||
|
|
@ -19,7 +20,8 @@ public:
|
|||
* @return RGB color encoded in int. each color is represented with 8 bits and has a layout of
|
||||
* 00000000RRRRRRRRGGGGGGGGBBBBBBBB where MSB is at the leftmost
|
||||
*/
|
||||
static void wavelengthToRGB(double wavelength, unsigned char& r, unsigned char& g, unsigned char& b) {
|
||||
static void wavelengthToRGB(double wavelength, unsigned char& r, unsigned char& g, unsigned char& b)
|
||||
{
|
||||
double x, y, z;
|
||||
cie1931WavelengthToXYZFit(wavelength, x, y, z);
|
||||
double dr, dg, db;
|
||||
|
|
@ -40,8 +42,9 @@ public:
|
|||
* @param xyz XYZ values in a double array in the order of X, Y, Z. each value in the range of [0.0, 1.0]
|
||||
* @return RGB values in a double array, in the order of R, G, B. each value in the range of [0.0, 1.0]
|
||||
*/
|
||||
static void srgbXYZ2RGB(double x, double y, double z, double& r, double& g, double& b) {
|
||||
double rl = 3.2406255 * x + -1.537208 * y + -0.4986286 * z;
|
||||
static void srgbXYZ2RGB(double x, double y, double z, double& r, double& g, double& b)
|
||||
{
|
||||
double rl = 3.2406255 * x + -1.537208 * y + -0.4986286 * z;
|
||||
double gl = -0.9689307 * x + 1.8757561 * y + 0.0415175 * z;
|
||||
double bl = 0.0557101 * x + -0.2040211 * y + 1.0569959 * z;
|
||||
|
||||
|
|
@ -53,7 +56,8 @@ public:
|
|||
/**
|
||||
* helper function for {@link #srgbXYZ2RGB(double[])}
|
||||
*/
|
||||
static double srgbXYZ2RGBPostprocess(double c) {
|
||||
static double srgbXYZ2RGBPostprocess(double c)
|
||||
{
|
||||
// clip if c is out of range
|
||||
c = c > 1 ? 1 : (c < 0 ? 0 : c);
|
||||
|
||||
|
|
@ -73,7 +77,8 @@ public:
|
|||
* @param wavelength wavelength in nm
|
||||
* @return XYZ in a double array in the order of X, Y, Z. each value in the range of [0.0, 1.0]
|
||||
*/
|
||||
static void cie1931WavelengthToXYZFit(double wavelength, double& x, double& y, double& z) {
|
||||
static void cie1931WavelengthToXYZFit(double wavelength, double& x, double& y, double& z)
|
||||
{
|
||||
double wave = wavelength;
|
||||
|
||||
{
|
||||
|
|
@ -81,28 +86,23 @@ public:
|
|||
double t2 = (wave - 599.8) * ((wave < 599.8) ? 0.0264 : 0.0323);
|
||||
double t3 = (wave - 501.1) * ((wave < 501.1) ? 0.0490 : 0.0382);
|
||||
|
||||
x = 0.362 * std::exp(-0.5 * t1 * t1)
|
||||
+ 1.056 * std::exp(-0.5 * t2 * t2)
|
||||
- 0.065 * std::exp(-0.5 * t3 * t3);
|
||||
x = 0.362 * std::exp(-0.5 * t1 * t1) + 1.056 * std::exp(-0.5 * t2 * t2) - 0.065 * std::exp(-0.5 * t3 * t3);
|
||||
}
|
||||
|
||||
{
|
||||
double t1 = (wave - 568.8) * ((wave < 568.8) ? 0.0213 : 0.0247);
|
||||
double t2 = (wave - 530.9) * ((wave < 530.9) ? 0.0613 : 0.0322);
|
||||
|
||||
y = 0.821 * std::exp(-0.5 * t1 * t1)
|
||||
+ 0.286 * std::exp(-0.5 * t2 * t2);
|
||||
y = 0.821 * std::exp(-0.5 * t1 * t1) + 0.286 * std::exp(-0.5 * t2 * t2);
|
||||
}
|
||||
|
||||
{
|
||||
double t1 = (wave - 437.0) * ((wave < 437.0) ? 0.0845 : 0.0278);
|
||||
double t2 = (wave - 459.0) * ((wave < 459.0) ? 0.0385 : 0.0725);
|
||||
|
||||
z = 1.217 * std::exp(-0.5 * t1 * t1)
|
||||
+ 0.681 * std::exp(-0.5 * t2 * t2);
|
||||
z = 1.217 * std::exp(-0.5 * t1 * t1) + 0.681 * std::exp(-0.5 * t2 * t2);
|
||||
}
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
} //namespace
|
||||
|
|
|
|||
|
|
@ -4,110 +4,116 @@
|
|||
#ifndef CommonUtils_EnumFlags_hpp
|
||||
#define CommonUtils_EnumFlags_hpp
|
||||
|
||||
namespace common_utils {
|
||||
namespace common_utils
|
||||
{
|
||||
|
||||
template<typename TEnum,typename TUnderlying=typename std::underlying_type<TEnum>::type>
|
||||
template <typename TEnum, typename TUnderlying = typename std::underlying_type<TEnum>::type>
|
||||
class EnumFlags
|
||||
{
|
||||
protected:
|
||||
TUnderlying flags_;
|
||||
public:
|
||||
EnumFlags()
|
||||
: flags_(0)
|
||||
{}
|
||||
EnumFlags(TEnum singleFlag)
|
||||
: flags_(static_cast<TUnderlying>(singleFlag))
|
||||
{}
|
||||
EnumFlags(TUnderlying flags)
|
||||
: flags_(flags)
|
||||
{}
|
||||
EnumFlags(const EnumFlags& original)
|
||||
: flags_(original.flags_)
|
||||
{}
|
||||
TUnderlying flags_;
|
||||
|
||||
EnumFlags& operator |=(TEnum add_value)
|
||||
{
|
||||
flags_ |= static_cast<TUnderlying>(add_value);
|
||||
return *this;
|
||||
}
|
||||
EnumFlags& operator |=(EnumFlags add_value)
|
||||
{
|
||||
flags_ |= add_value.flags_;
|
||||
return *this;
|
||||
}
|
||||
EnumFlags operator |(TEnum add_value) const
|
||||
public:
|
||||
EnumFlags()
|
||||
: flags_(0)
|
||||
{
|
||||
EnumFlags result(*this);
|
||||
result |= add_value;
|
||||
}
|
||||
EnumFlags(TEnum singleFlag)
|
||||
: flags_(static_cast<TUnderlying>(singleFlag))
|
||||
{
|
||||
}
|
||||
EnumFlags(TUnderlying flags)
|
||||
: flags_(flags)
|
||||
{
|
||||
}
|
||||
EnumFlags(const EnumFlags& original)
|
||||
: flags_(original.flags_)
|
||||
{
|
||||
}
|
||||
|
||||
EnumFlags& operator|=(TEnum add_value)
|
||||
{
|
||||
flags_ |= static_cast<TUnderlying>(add_value);
|
||||
return *this;
|
||||
}
|
||||
EnumFlags& operator|=(EnumFlags add_value)
|
||||
{
|
||||
flags_ |= add_value.flags_;
|
||||
return *this;
|
||||
}
|
||||
EnumFlags operator|(TEnum add_value) const
|
||||
{
|
||||
EnumFlags result(*this);
|
||||
result |= add_value;
|
||||
return result;
|
||||
}
|
||||
EnumFlags operator |(EnumFlags add_value) const
|
||||
EnumFlags operator|(EnumFlags add_value) const
|
||||
{
|
||||
EnumFlags result(*this);
|
||||
result |= add_value.flags_;
|
||||
EnumFlags result(*this);
|
||||
result |= add_value.flags_;
|
||||
return result;
|
||||
}
|
||||
EnumFlags& operator &=(TEnum mask_value)
|
||||
{
|
||||
EnumFlags& operator&=(TEnum mask_value)
|
||||
{
|
||||
flags_ &= static_cast<TUnderlying>(mask_value);
|
||||
return *this;
|
||||
}
|
||||
EnumFlags& operator &=(EnumFlags mask_value)
|
||||
{
|
||||
EnumFlags& operator&=(EnumFlags mask_value)
|
||||
{
|
||||
flags_ &= mask_value.flags_;
|
||||
return *this;
|
||||
}
|
||||
EnumFlags operator &(TEnum mask_value) const
|
||||
{
|
||||
EnumFlags operator&(TEnum mask_value) const
|
||||
{
|
||||
EnumFlags result(*this);
|
||||
result &= mask_value;
|
||||
return result;
|
||||
return result;
|
||||
}
|
||||
EnumFlags operator &(EnumFlags mask_value) const
|
||||
{
|
||||
EnumFlags operator&(EnumFlags mask_value) const
|
||||
{
|
||||
EnumFlags result(*this);
|
||||
result &= mask_value.flags_;
|
||||
return result;
|
||||
}
|
||||
EnumFlags operator ~() const
|
||||
{
|
||||
return result;
|
||||
}
|
||||
EnumFlags operator~() const
|
||||
{
|
||||
EnumFlags result(*this);
|
||||
result.flags_ = ~result.flags_;
|
||||
return result;
|
||||
}
|
||||
EnumFlags& operator ^=(TEnum mask_value)
|
||||
{
|
||||
flags_ ^= mask_value;
|
||||
return *this;
|
||||
}
|
||||
EnumFlags& operator ^=(EnumFlags mask_value)
|
||||
{
|
||||
flags_ ^= mask_value.flags_;
|
||||
return *this;
|
||||
}
|
||||
EnumFlags operator ^(TEnum mask_value) const
|
||||
{
|
||||
EnumFlags result(*this);
|
||||
result.flags_ ^= mask_value;
|
||||
return result;
|
||||
}
|
||||
EnumFlags operator ^(EnumFlags mask_value) const
|
||||
{
|
||||
EnumFlags result(*this);
|
||||
result.flags_ ^= mask_value.flags_;
|
||||
return result;
|
||||
}
|
||||
EnumFlags& operator^=(TEnum mask_value)
|
||||
{
|
||||
flags_ ^= mask_value;
|
||||
return *this;
|
||||
}
|
||||
EnumFlags& operator^=(EnumFlags mask_value)
|
||||
{
|
||||
flags_ ^= mask_value.flags_;
|
||||
return *this;
|
||||
}
|
||||
EnumFlags operator^(TEnum mask_value) const
|
||||
{
|
||||
EnumFlags result(*this);
|
||||
result.flags_ ^= mask_value;
|
||||
return result;
|
||||
}
|
||||
EnumFlags operator^(EnumFlags mask_value) const
|
||||
{
|
||||
EnumFlags result(*this);
|
||||
result.flags_ ^= mask_value.flags_;
|
||||
return result;
|
||||
}
|
||||
|
||||
//EnumFlags& operator ~=(TEnum mask_value)
|
||||
//{
|
||||
// flags_ ~= static_cast<TUnderlying>(mask_value);
|
||||
// return *this;
|
||||
//}
|
||||
//EnumFlags& operator ~=(EnumFlags mask_value)
|
||||
//{
|
||||
// flags_ ~= mask_value.flags_;
|
||||
// return *this;
|
||||
//}
|
||||
//EnumFlags& operator ~=(TEnum mask_value)
|
||||
//{
|
||||
// flags_ ~= static_cast<TUnderlying>(mask_value);
|
||||
// return *this;
|
||||
//}
|
||||
//EnumFlags& operator ~=(EnumFlags mask_value)
|
||||
//{
|
||||
// flags_ ~= mask_value.flags_;
|
||||
// return *this;
|
||||
//}
|
||||
|
||||
//equality operators
|
||||
bool operator==(const EnumFlags& rhs) const
|
||||
|
|
@ -117,26 +123,24 @@ public:
|
|||
inline bool operator!=(const EnumFlags& rhs) const
|
||||
{
|
||||
return !(*this == rhs);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
//type conversion
|
||||
operator bool() const
|
||||
{
|
||||
{
|
||||
return flags_ != 0;
|
||||
}
|
||||
operator TUnderlying() const
|
||||
{
|
||||
return flags_;
|
||||
}
|
||||
|
||||
|
||||
TEnum toEnum() const
|
||||
{
|
||||
return static_cast<TEnum>(flags_);
|
||||
}
|
||||
};
|
||||
|
||||
} //namespace
|
||||
|
||||
} //namespace
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -20,26 +20,29 @@
|
|||
#include <cxxabi.h>
|
||||
#include <cstdlib>
|
||||
|
||||
namespace {
|
||||
void * last_frames[20];
|
||||
size_t last_size;
|
||||
std::string exception_name;
|
||||
namespace
|
||||
{
|
||||
void* last_frames[20];
|
||||
size_t last_size;
|
||||
std::string exception_name;
|
||||
|
||||
std::string demangle(const char *name) {
|
||||
std::string demangle(const char* name)
|
||||
{
|
||||
int status;
|
||||
std::unique_ptr<char,void(*)(void*)> realname(abi::__cxa_demangle(name, 0, 0, &status), &std::free);
|
||||
std::unique_ptr<char, void (*)(void*)> realname(abi::__cxa_demangle(name, 0, 0, &status), &std::free);
|
||||
return status ? "failed" : &*realname;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
extern "C" {
|
||||
void __cxa_throw(void *ex, void *info, void (*dest)(void *)) {
|
||||
void __cxa_throw(void* ex, void* info, void (*dest)(void*))
|
||||
{
|
||||
exception_name = demangle(reinterpret_cast<const std::type_info*>(info)->name());
|
||||
last_size = backtrace(last_frames, sizeof last_frames/sizeof(void*));
|
||||
last_size = backtrace(last_frames, sizeof last_frames / sizeof(void*));
|
||||
|
||||
static void (*const rethrow)(void*,void*,void(*)(void*)) __attribute__ ((noreturn)) = (void (*)(void*,void*,void(*)(void*)))dlsym(RTLD_NEXT, "__cxa_throw");
|
||||
rethrow(ex,info,dest);
|
||||
}
|
||||
static void (*const rethrow)(void*, void*, void (*)(void*)) __attribute__((noreturn)) = (void (*)(void*, void*, void (*)(void*)))dlsym(RTLD_NEXT, "__cxa_throw");
|
||||
rethrow(ex, info, dest);
|
||||
}
|
||||
}
|
||||
|
||||
#pragma GCC diagnostic pop
|
||||
|
|
@ -48,7 +51,7 @@ extern "C" {
|
|||
void printExceptionStack()
|
||||
{
|
||||
#ifdef __GNUC__
|
||||
backtrace_symbols_fd(last_frames, last_size, 2);
|
||||
backtrace_symbols_fd(last_frames, last_size, 2);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -9,23 +9,23 @@
|
|||
#include <string>
|
||||
#include "Utils.hpp"
|
||||
|
||||
// This defines a default folder name for all the files created by AirLib so they
|
||||
// This defines a default folder name for all the files created by AirLib so they
|
||||
// are all gathered nicely in one place in the user's documents folder.
|
||||
#ifndef ProductFolderName
|
||||
#define ProductFolderName "AirSim"
|
||||
#define ProductFolderName "AirSim"
|
||||
#endif
|
||||
|
||||
#ifndef _CRT_SECURE_NO_WARNINGS
|
||||
#define _CRT_SECURE_NO_WARNINGS 1
|
||||
#endif
|
||||
|
||||
namespace common_utils {
|
||||
|
||||
namespace common_utils
|
||||
{
|
||||
class FileSystem
|
||||
{
|
||||
typedef unsigned int uint;
|
||||
|
||||
public:
|
||||
|
||||
// please use the combine() method instead.
|
||||
static const char kPathSeparator =
|
||||
#ifdef _WIN32
|
||||
|
|
@ -38,14 +38,14 @@ public:
|
|||
|
||||
static std::string getUserHomeFolder()
|
||||
{
|
||||
//Windows uses USERPROFILE, Linux uses HOME
|
||||
#ifdef _WIN32
|
||||
//Windows uses USERPROFILE, Linux uses HOME
|
||||
#ifdef _WIN32
|
||||
std::wstring userProfile = _wgetenv(L"USERPROFILE");
|
||||
std::wstring_convert<std::codecvt_utf8<wchar_t>, wchar_t> converter;
|
||||
return converter.to_bytes(userProfile);
|
||||
#else
|
||||
#else
|
||||
return std::getenv("HOME");
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
|
||||
static std::string getUserDocumentsFolder();
|
||||
|
|
@ -78,7 +78,7 @@ public:
|
|||
if (parentFolder.size() > 0 && parentFolder[len - 1] == kPathSeparator) {
|
||||
// parent already ends with '/'
|
||||
return parentFolder + child;
|
||||
}
|
||||
}
|
||||
len = child.size();
|
||||
if (len > 0 && child[0] == kPathSeparator) {
|
||||
// child already starts with '/'
|
||||
|
|
@ -96,7 +96,6 @@ public:
|
|||
}
|
||||
}
|
||||
|
||||
|
||||
static std::string getFileExtension(const std::string& str)
|
||||
{
|
||||
// bugbug: this is not unicode safe.
|
||||
|
|
@ -121,8 +120,8 @@ public:
|
|||
return fullPath;
|
||||
}
|
||||
|
||||
static std::string getLogFileNamePath(const std::string& fullPath, const std::string& prefix, const std::string& suffix, const std::string& extension,
|
||||
bool file_timestamp)
|
||||
static std::string getLogFileNamePath(const std::string& fullPath, const std::string& prefix, const std::string& suffix, const std::string& extension,
|
||||
bool file_timestamp)
|
||||
{
|
||||
//TODO: because this bug we are using alternative code with stringstream
|
||||
//https://answers.unrealengine.com/questions/664905/unreal-crashes-on-two-lines-of-extremely-simple-st.html
|
||||
|
|
@ -143,7 +142,7 @@ public:
|
|||
}
|
||||
|
||||
static void openTextFile(const std::string& filepath, std::ifstream& file)
|
||||
{
|
||||
{
|
||||
#ifdef _WIN32
|
||||
// WIN32 will create the wrong file names if we don't first convert them to UTF-16.
|
||||
std::wstring_convert<std::codecvt_utf8<wchar_t>, wchar_t> converter;
|
||||
|
|
@ -153,9 +152,9 @@ public:
|
|||
file.open(filepath, std::ios::in);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
static void createBinaryFile(const std::string& filepath, std::ofstream& file)
|
||||
{
|
||||
{
|
||||
#ifdef _WIN32
|
||||
// WIN32 will create the wrong file names if we don't first convert them to UTF-16.
|
||||
std::wstring_convert<std::codecvt_utf8<wchar_t>, wchar_t> converter;
|
||||
|
|
@ -165,9 +164,9 @@ public:
|
|||
file.open(filepath, std::ios::binary | std::ios::trunc);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
static void createTextFile(const std::string& filepath, std::ofstream& file)
|
||||
{
|
||||
{
|
||||
#ifdef _WIN32
|
||||
// WIN32 will create the wrong file names if we don't first convert them to UTF-16.
|
||||
std::wstring_convert<std::codecvt_utf8<wchar_t>, wchar_t> converter;
|
||||
|
|
@ -180,7 +179,7 @@ public:
|
|||
if (file.fail())
|
||||
throw std::ios_base::failure(std::strerror(errno));
|
||||
}
|
||||
|
||||
|
||||
static std::string createLogFile(const std::string& suffix, std::ofstream& flog)
|
||||
{
|
||||
std::string log_folderpath = common_utils::FileSystem::getLogFolderPath(false);
|
||||
|
|
@ -198,12 +197,12 @@ public:
|
|||
try {
|
||||
std::getline(file, line);
|
||||
}
|
||||
catch(...) {
|
||||
catch (...) {
|
||||
if (!file.eof())
|
||||
throw;
|
||||
}
|
||||
return line;
|
||||
}
|
||||
}
|
||||
|
||||
static void appendLineToFile(const std::string& filepath, const std::string& line)
|
||||
{
|
||||
|
|
@ -221,8 +220,6 @@ public:
|
|||
file.exceptions(file.exceptions() | std::ios::failbit | std::ifstream::badbit);
|
||||
file << line << std::endl;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
|
@ -8,25 +8,29 @@
|
|||
#include <algorithm>
|
||||
#include <tuple>
|
||||
|
||||
namespace common_utils {
|
||||
namespace common_utils
|
||||
{
|
||||
|
||||
template <typename T>
|
||||
class MedianFilter {
|
||||
private:
|
||||
std::vector<T> buffer_, buffer_copy_;
|
||||
int window_size_, window_size_2x_, window_size_half_;
|
||||
float outlier_factor_;
|
||||
int buffer_index_;
|
||||
public:
|
||||
MedianFilter();
|
||||
MedianFilter(int window_size, float outlier_factor);
|
||||
void initialize(int window_size, float outlier_factor);
|
||||
std::tuple<double,double> filter(T value);
|
||||
class MedianFilter
|
||||
{
|
||||
private:
|
||||
std::vector<T> buffer_, buffer_copy_;
|
||||
int window_size_, window_size_2x_, window_size_half_;
|
||||
float outlier_factor_;
|
||||
int buffer_index_;
|
||||
|
||||
public:
|
||||
MedianFilter();
|
||||
MedianFilter(int window_size, float outlier_factor);
|
||||
void initialize(int window_size, float outlier_factor);
|
||||
std::tuple<double, double> filter(T value);
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
void MedianFilter<T>::initialize(int window_size, float outlier_factor) {
|
||||
buffer_.resize(window_size);
|
||||
void MedianFilter<T>::initialize(int window_size, float outlier_factor)
|
||||
{
|
||||
buffer_.resize(window_size);
|
||||
buffer_copy_.resize(window_size);
|
||||
window_size_ = window_size;
|
||||
window_size_2x_ = window_size_ * 2;
|
||||
|
|
@ -36,47 +40,51 @@ void MedianFilter<T>::initialize(int window_size, float outlier_factor) {
|
|||
}
|
||||
|
||||
template <typename T>
|
||||
MedianFilter<T>::MedianFilter() {
|
||||
MedianFilter<T>::MedianFilter()
|
||||
{
|
||||
initialize(buffer_.capacity(), std::numeric_limits<float>::infinity());
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
MedianFilter<T>::MedianFilter(int window_size, float outlier_factor) {
|
||||
initialize(window_size, std::numeric_limits<float>::infinity());
|
||||
MedianFilter<T>::MedianFilter(int window_size, float outlier_factor)
|
||||
{
|
||||
initialize(window_size, std::numeric_limits<float>::infinity());
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
std::tuple<double,double> MedianFilter<T>::filter(T value){
|
||||
std::tuple<double, double> MedianFilter<T>::filter(T value)
|
||||
{
|
||||
buffer_[buffer_index_++ % window_size_] = value;
|
||||
if (buffer_index_ == window_size_2x_)
|
||||
buffer_index_ = window_size_;
|
||||
|
||||
buffer_index_ = window_size_;
|
||||
|
||||
if (buffer_index_ >= window_size_) {
|
||||
//find median
|
||||
for(auto i = 0; i < window_size_; ++i)
|
||||
for (auto i = 0; i < window_size_; ++i)
|
||||
buffer_copy_[i] = buffer_[i];
|
||||
std::sort(buffer_copy_.begin(), buffer_copy_.end());
|
||||
double median = buffer_copy_[window_size_half_];
|
||||
|
||||
|
||||
//average values that fall between upper and lower bound of median
|
||||
auto lower_bound = median - median * outlier_factor_, upper_bound = median + median * outlier_factor_;
|
||||
double sum = 0; int count = 0;
|
||||
for(auto i = 0; i < window_size_; ++i) {
|
||||
double sum = 0;
|
||||
int count = 0;
|
||||
for (auto i = 0; i < window_size_; ++i) {
|
||||
if (buffer_copy_[i] >= lower_bound && buffer_copy_[i] <= upper_bound) {
|
||||
sum += buffer_copy_[i];
|
||||
++count;
|
||||
}
|
||||
}
|
||||
double mean = sum / count;
|
||||
|
||||
|
||||
double std_dev_sum = 0;
|
||||
for(auto i = 0; i < window_size_; ++i) {
|
||||
for (auto i = 0; i < window_size_; ++i) {
|
||||
if (buffer_copy_[i] >= lower_bound && buffer_copy_[i] <= upper_bound) {
|
||||
double diff = buffer_copy_[i] - mean;
|
||||
sum += diff*diff;
|
||||
double diff = buffer_copy_[i] - mean;
|
||||
sum += diff * diff;
|
||||
}
|
||||
}
|
||||
double variance = std_dev_sum / count;
|
||||
double variance = std_dev_sum / count;
|
||||
|
||||
return std::make_tuple(mean, variance);
|
||||
}
|
||||
|
|
@ -87,6 +95,5 @@ std::tuple<double,double> MedianFilter<T>::filter(T value){
|
|||
}
|
||||
}
|
||||
|
||||
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -13,45 +13,45 @@ you may have to #undef whatever you are actually using.
|
|||
#define WIN32_LEAN_AND_MEAN
|
||||
|
||||
// The below excludes some other unused services from the windows headers -- see windows.h for details.
|
||||
#define NOGDICAPMASKS // CC_*, LC_*, PC_*, CP_*, TC_*, RC_
|
||||
#define NOVIRTUALKEYCODES // VK_*
|
||||
#define NOWINMESSAGES // WM_*, EM_*, LB_*, CB_*
|
||||
#define NOWINSTYLES // WS_*, CS_*, ES_*, LBS_*, SBS_*, CBS_*
|
||||
#define NOSYSMETRICS // SM_*
|
||||
#define NOMENUS // MF_*
|
||||
#define NOICONS // IDI_*
|
||||
#define NOKEYSTATES // MK_*
|
||||
#define NOSYSCOMMANDS // SC_*
|
||||
#define NORASTEROPS // Binary and Tertiary raster ops
|
||||
#define NOSHOWWINDOW // SW_*
|
||||
#define OEMRESOURCE // OEM Resource values
|
||||
#define NOATOM // Atom Manager routines
|
||||
#define NOCLIPBOARD // Clipboard routines
|
||||
#define NOCOLOR // Screen colors
|
||||
#define NOCTLMGR // Control and Dialog routines
|
||||
#define NODRAWTEXT // DrawText() and DT_*
|
||||
#define NOGDI // All GDI #defines and routines
|
||||
#define NOKERNEL // All KERNEL #defines and routines
|
||||
#define NOUSER // All USER #defines and routines
|
||||
#define NONLS // All NLS #defines and routines
|
||||
#define NOMB // MB_* and MessageBox()
|
||||
#define NOMEMMGR // GMEM_*, LMEM_*, GHND, LHND, associated routines
|
||||
#define NOMETAFILE // typedef METAFILEPICT
|
||||
#define NOMINMAX // Macros min(a,b) and max(a,b)
|
||||
#define NOMSG // typedef MSG and associated routines
|
||||
#define NOOPENFILE // OpenFile(), OemToAnsi, AnsiToOem, and OF_*
|
||||
#define NOSCROLL // SB_* and scrolling routines
|
||||
#define NOSERVICE // All Service Controller routines, SERVICE_ equates, etc.
|
||||
#define NOSOUND // Sound driver routines
|
||||
#define NOTEXTMETRIC // typedef TEXTMETRIC and associated routines
|
||||
#define NOWH // SetWindowsHook and WH_*
|
||||
#define NOWINOFFSETS // GWL_*, GCL_*, associated routines
|
||||
#define NOCOMM // COMM driver routines
|
||||
#define NOKANJI // Kanji support stuff.
|
||||
#define NOHELP // Help engine interface.
|
||||
#define NOPROFILER // Profiler interface.
|
||||
#define NODEFERWINDOWPOS // DeferWindowPos routines
|
||||
#define NOMCX // Modem Configuration Extensions
|
||||
#define NOGDICAPMASKS // CC_*, LC_*, PC_*, CP_*, TC_*, RC_
|
||||
#define NOVIRTUALKEYCODES // VK_*
|
||||
#define NOWINMESSAGES // WM_*, EM_*, LB_*, CB_*
|
||||
#define NOWINSTYLES // WS_*, CS_*, ES_*, LBS_*, SBS_*, CBS_*
|
||||
#define NOSYSMETRICS // SM_*
|
||||
#define NOMENUS // MF_*
|
||||
#define NOICONS // IDI_*
|
||||
#define NOKEYSTATES // MK_*
|
||||
#define NOSYSCOMMANDS // SC_*
|
||||
#define NORASTEROPS // Binary and Tertiary raster ops
|
||||
#define NOSHOWWINDOW // SW_*
|
||||
#define OEMRESOURCE // OEM Resource values
|
||||
#define NOATOM // Atom Manager routines
|
||||
#define NOCLIPBOARD // Clipboard routines
|
||||
#define NOCOLOR // Screen colors
|
||||
#define NOCTLMGR // Control and Dialog routines
|
||||
#define NODRAWTEXT // DrawText() and DT_*
|
||||
#define NOGDI // All GDI #defines and routines
|
||||
#define NOKERNEL // All KERNEL #defines and routines
|
||||
#define NOUSER // All USER #defines and routines
|
||||
#define NONLS // All NLS #defines and routines
|
||||
#define NOMB // MB_* and MessageBox()
|
||||
#define NOMEMMGR // GMEM_*, LMEM_*, GHND, LHND, associated routines
|
||||
#define NOMETAFILE // typedef METAFILEPICT
|
||||
#define NOMINMAX // Macros min(a,b) and max(a,b)
|
||||
#define NOMSG // typedef MSG and associated routines
|
||||
#define NOOPENFILE // OpenFile(), OemToAnsi, AnsiToOem, and OF_*
|
||||
#define NOSCROLL // SB_* and scrolling routines
|
||||
#define NOSERVICE // All Service Controller routines, SERVICE_ equates, etc.
|
||||
#define NOSOUND // Sound driver routines
|
||||
#define NOTEXTMETRIC // typedef TEXTMETRIC and associated routines
|
||||
#define NOWH // SetWindowsHook and WH_*
|
||||
#define NOWINOFFSETS // GWL_*, GCL_*, associated routines
|
||||
#define NOCOMM // COMM driver routines
|
||||
#define NOKANJI // Kanji support stuff.
|
||||
#define NOHELP // Help engine interface.
|
||||
#define NOPROFILER // Profiler interface.
|
||||
#define NODEFERWINDOWPOS // DeferWindowPos routines
|
||||
#define NOMCX // Modem Configuration Extensions
|
||||
#define NOCRYPT
|
||||
#define NOTAPE
|
||||
#define NOIMAGE
|
||||
|
|
|
|||
|
|
@ -6,11 +6,13 @@
|
|||
|
||||
#include <cmath>
|
||||
|
||||
namespace common_utils {
|
||||
namespace common_utils
|
||||
{
|
||||
|
||||
class OnlineStats {
|
||||
class OnlineStats
|
||||
{
|
||||
public:
|
||||
OnlineStats()
|
||||
OnlineStats()
|
||||
{
|
||||
clear();
|
||||
}
|
||||
|
|
@ -32,7 +34,7 @@ public:
|
|||
delta_n2 = delta_n * delta_n;
|
||||
term1 = delta * delta_n * n1;
|
||||
m1 += delta_n;
|
||||
m4 += term1 * delta_n2 * (n*n - 3*n + 3) + 6 * delta_n2 * m2 - 4 * delta_n * m3;
|
||||
m4 += term1 * delta_n2 * (n * n - 3 * n + 3) + 6 * delta_n2 * m2 - 4 * delta_n * m3;
|
||||
m3 += term1 * delta_n * (n - 2) - 3 * delta_n * m2;
|
||||
m2 += term1;
|
||||
}
|
||||
|
|
@ -49,7 +51,7 @@ public:
|
|||
|
||||
double variance() const
|
||||
{
|
||||
return m2/(n-1.0f);
|
||||
return m2 / (n - 1.0f);
|
||||
}
|
||||
|
||||
double standardDeviation() const
|
||||
|
|
@ -59,12 +61,12 @@ public:
|
|||
|
||||
double skewness() const
|
||||
{
|
||||
return sqrt(double(n)) * m3/ pow(m2, 1.5f);
|
||||
return sqrt(double(n)) * m3 / pow(m2, 1.5f);
|
||||
}
|
||||
|
||||
double kurtosis() const
|
||||
{
|
||||
return double(n)*m4 / (m2*m2) - 3.0f;
|
||||
return double(n) * m4 / (m2 * m2) - 3.0f;
|
||||
}
|
||||
|
||||
OnlineStats operator+(const OnlineStats& rhs)
|
||||
|
|
@ -74,29 +76,28 @@ public:
|
|||
combined.n = this->n + rhs.n;
|
||||
|
||||
double delta = rhs.m1 - this->m1;
|
||||
double delta2 = delta*delta;
|
||||
double delta3 = delta*delta2;
|
||||
double delta4 = delta2*delta2;
|
||||
double delta2 = delta * delta;
|
||||
double delta3 = delta * delta2;
|
||||
double delta4 = delta2 * delta2;
|
||||
|
||||
combined.m1 = (this->n*this->m1 + rhs.n*rhs.m1) / combined.n;
|
||||
combined.m1 = (this->n * this->m1 + rhs.n * rhs.m1) / combined.n;
|
||||
|
||||
combined.m2 = this->m2 + rhs.m2 +
|
||||
delta2 * this->n * rhs.n / combined.n;
|
||||
combined.m2 = this->m2 + rhs.m2 +
|
||||
delta2 * this->n * rhs.n / combined.n;
|
||||
|
||||
combined.m3 = this->m3 + rhs.m3 +
|
||||
delta3 * this->n * rhs.n * (this->n - rhs.n)/(combined.n*combined.n);
|
||||
combined.m3 += 3.0f*delta * (this->n*rhs.m2 - rhs.n*this->m2) / combined.n;
|
||||
combined.m3 = this->m3 + rhs.m3 +
|
||||
delta3 * this->n * rhs.n * (this->n - rhs.n) / (combined.n * combined.n);
|
||||
combined.m3 += 3.0f * delta * (this->n * rhs.m2 - rhs.n * this->m2) / combined.n;
|
||||
|
||||
combined.m4 = this->m4 + rhs.m4 + delta4*this->n*rhs.n * (this->n*this->n - this->n*rhs.n + rhs.n*rhs.n) /
|
||||
(combined.n*combined.n*combined.n);
|
||||
combined.m4 += 6.0f*delta2 * (this->n*this->n*rhs.m2 + rhs.n*rhs.n*this->m2)/(combined.n*combined.n) +
|
||||
4.0f*delta*(this->n*rhs.m3 - rhs.n*this->m3) / combined.n;
|
||||
combined.m4 = this->m4 + rhs.m4 + delta4 * this->n * rhs.n * (this->n * this->n - this->n * rhs.n + rhs.n * rhs.n) / (combined.n * combined.n * combined.n);
|
||||
combined.m4 += 6.0f * delta2 * (this->n * this->n * rhs.m2 + rhs.n * rhs.n * this->m2) / (combined.n * combined.n) +
|
||||
4.0f * delta * (this->n * rhs.m3 - rhs.n * this->m3) / combined.n;
|
||||
|
||||
return combined;
|
||||
}
|
||||
|
||||
OnlineStats& operator+=(const OnlineStats& rhs)
|
||||
{
|
||||
{
|
||||
OnlineStats combined = *this + rhs;
|
||||
*this = combined;
|
||||
return *this;
|
||||
|
|
@ -108,5 +109,5 @@ private:
|
|||
|
||||
}; //class
|
||||
|
||||
} //namespace
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -10,7 +10,8 @@
|
|||
#include <mutex>
|
||||
#include <condition_variable>
|
||||
|
||||
namespace common_utils {
|
||||
namespace common_utils
|
||||
{
|
||||
|
||||
/*
|
||||
This queue can support multiple producers consumers, but it should be used carefully
|
||||
|
|
@ -39,11 +40,10 @@ public:
|
|||
is_done_ = false;
|
||||
}
|
||||
|
||||
T pop()
|
||||
T pop()
|
||||
{
|
||||
std::unique_lock<std::mutex> global_lock(mutex_);
|
||||
while (queue_.empty())
|
||||
{
|
||||
while (queue_.empty()) {
|
||||
//this may be spurious wake-up
|
||||
//in multi-consumer scenario
|
||||
cond_.wait(global_lock);
|
||||
|
|
@ -56,7 +56,7 @@ public:
|
|||
bool tryPop(T& item)
|
||||
{
|
||||
std::unique_lock<std::mutex> global_lock(mutex_);
|
||||
if(queue_.empty())
|
||||
if (queue_.empty())
|
||||
return false;
|
||||
|
||||
item = queue_.front();
|
||||
|
|
@ -89,7 +89,7 @@ public:
|
|||
global_lock.unlock();
|
||||
cond_.notify_one();
|
||||
}
|
||||
|
||||
|
||||
//is_done_ flag is just convinience flag for external use
|
||||
//its not used by this class
|
||||
bool getIsDone()
|
||||
|
|
@ -101,10 +101,9 @@ public:
|
|||
is_done_ = val;
|
||||
}
|
||||
|
||||
|
||||
// non-copiable
|
||||
ProsumerQueue(const ProsumerQueue&) = delete;
|
||||
ProsumerQueue& operator=(const ProsumerQueue&) = delete;
|
||||
ProsumerQueue(const ProsumerQueue&) = delete;
|
||||
ProsumerQueue& operator=(const ProsumerQueue&) = delete;
|
||||
|
||||
private:
|
||||
std::queue<T> queue_;
|
||||
|
|
@ -112,6 +111,5 @@ private:
|
|||
std::condition_variable cond_;
|
||||
std::atomic<bool> is_done_;
|
||||
};
|
||||
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -6,14 +6,16 @@
|
|||
|
||||
#include <random>
|
||||
|
||||
namespace common_utils {
|
||||
namespace common_utils
|
||||
{
|
||||
|
||||
template<typename TReturn, typename TDistribution, unsigned int Seed=42>
|
||||
class RandomGenerator {
|
||||
template <typename TReturn, typename TDistribution, unsigned int Seed = 42>
|
||||
class RandomGenerator
|
||||
{
|
||||
public:
|
||||
//for uniform distribution supply min and max (inclusive)
|
||||
//for gaussian distribution supply mean and sigma
|
||||
template<typename... DistArgs>
|
||||
template <typename... DistArgs>
|
||||
RandomGenerator(DistArgs... dist_args)
|
||||
: dist_(dist_args...), rand_(Seed)
|
||||
{
|
||||
|
|
@ -47,6 +49,5 @@ typedef RandomGenerator<unsigned int, std::uniform_int_distribution<unsigned int
|
|||
//TODO: below we should have float instead of double but VC++2017 has a bug :(
|
||||
typedef RandomGenerator<float, std::normal_distribution<double>> RandomGeneratorGaussianF;
|
||||
typedef RandomGenerator<double, std::normal_distribution<double>> RandomGeneratorGaussianD;
|
||||
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -12,13 +12,15 @@
|
|||
#include <mutex>
|
||||
#include <cstdint>
|
||||
|
||||
namespace common_utils {
|
||||
namespace common_utils
|
||||
{
|
||||
|
||||
class ScheduledExecutor
|
||||
class ScheduledExecutor
|
||||
{
|
||||
public:
|
||||
ScheduledExecutor()
|
||||
{}
|
||||
{
|
||||
}
|
||||
|
||||
ScheduledExecutor(const std::function<bool(uint64_t)>& callback, uint64_t period_nanos)
|
||||
{
|
||||
|
|
@ -44,7 +46,7 @@ public:
|
|||
is_first_period_ = true;
|
||||
|
||||
initializePauseState();
|
||||
|
||||
|
||||
sleep_time_avg_ = 0;
|
||||
Utils::cleanupThread(th_);
|
||||
th_ = std::thread(&ScheduledExecutor::executorLoop, this);
|
||||
|
|
@ -85,7 +87,7 @@ public:
|
|||
|
||||
void setFrameNumber(uint32_t frameNumber)
|
||||
{
|
||||
currentFrameNumber_ = frameNumber;
|
||||
currentFrameNumber_ = frameNumber;
|
||||
}
|
||||
|
||||
void stop()
|
||||
|
|
@ -99,7 +101,7 @@ public:
|
|||
th_.join();
|
||||
}
|
||||
}
|
||||
catch(const std::system_error& /* e */) {
|
||||
catch (const std::system_error& /* e */) {
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -177,14 +179,14 @@ private:
|
|||
TTimeDelta since_last_call = period_start - call_end;
|
||||
|
||||
if (frame_countdown_enabled_) {
|
||||
if (targetFrameNumber_ <= currentFrameNumber_){
|
||||
if (! isPaused())
|
||||
if (targetFrameNumber_ <= currentFrameNumber_) {
|
||||
if (!isPaused())
|
||||
pause(true);
|
||||
|
||||
frame_countdown_enabled_ = false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if (pause_period_start_ > 0) {
|
||||
if (nanos() - pause_period_start_ >= pause_period_) {
|
||||
pause(!isPaused());
|
||||
|
|
@ -203,10 +205,10 @@ private:
|
|||
started_ = result;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
is_first_period_ = false;
|
||||
|
||||
|
||||
call_end = nanos();
|
||||
|
||||
TTimeDelta elapsed_period = nanos() - period_start;
|
||||
|
|
@ -231,11 +233,10 @@ private:
|
|||
uint32_t currentFrameNumber_;
|
||||
uint32_t targetFrameNumber_;
|
||||
std::atomic_bool frame_countdown_enabled_;
|
||||
|
||||
|
||||
double sleep_time_avg_;
|
||||
|
||||
std::mutex mutex_;
|
||||
};
|
||||
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -33,22 +33,25 @@ return 0;
|
|||
|
||||
*/
|
||||
|
||||
namespace common_utils {
|
||||
|
||||
namespace common_utils
|
||||
{
|
||||
|
||||
template <typename... Args>
|
||||
class Signal {
|
||||
class Signal
|
||||
{
|
||||
|
||||
public:
|
||||
|
||||
Signal() : current_id_(0) {}
|
||||
Signal()
|
||||
: current_id_(0) {}
|
||||
|
||||
// copy creates new signal
|
||||
Signal(Signal const& other) : current_id_(0) {}
|
||||
Signal(Signal const& other)
|
||||
: current_id_(0) {}
|
||||
|
||||
// connects a member function to this Signal
|
||||
template <typename T>
|
||||
int connect_member(T *inst, void (T::*func)(Args...)) {
|
||||
int connect_member(T* inst, void (T::*func)(Args...))
|
||||
{
|
||||
return connect([=](Args... args) {
|
||||
(inst->*func)(args...);
|
||||
});
|
||||
|
|
@ -56,7 +59,8 @@ public:
|
|||
|
||||
// connects a const member function to this Signal
|
||||
template <typename T>
|
||||
int connect_member(T *inst, void (T::*func)(Args...) const) {
|
||||
int connect_member(T* inst, void (T::*func)(Args...) const)
|
||||
{
|
||||
return connect([=](Args... args) {
|
||||
(inst->*func)(args...);
|
||||
});
|
||||
|
|
@ -64,31 +68,36 @@ public:
|
|||
|
||||
// connects a std::function to the signal. The returned
|
||||
// value can be used to disconnect the function again
|
||||
int connect(std::function<void(Args...)> const& slot) const {
|
||||
int connect(std::function<void(Args...)> const& slot) const
|
||||
{
|
||||
slots_.insert(std::make_pair(++current_id_, slot));
|
||||
return current_id_;
|
||||
}
|
||||
|
||||
// disconnects a previously connected function
|
||||
void disconnect(int id) const {
|
||||
void disconnect(int id) const
|
||||
{
|
||||
slots_.erase(id);
|
||||
}
|
||||
|
||||
// disconnects all previously connected functions
|
||||
void disconnect_all() const {
|
||||
void disconnect_all() const
|
||||
{
|
||||
slots_.clear();
|
||||
}
|
||||
|
||||
// calls all connected functions
|
||||
void emit(Args... p) {
|
||||
for(auto it=slots_.begin(); it!=slots_.end(); ) {
|
||||
void emit(Args... p)
|
||||
{
|
||||
for (auto it = slots_.begin(); it != slots_.end();) {
|
||||
// Increment here so that the entry can be erased from inside the method as well
|
||||
(it++)->second(p...);
|
||||
}
|
||||
}
|
||||
|
||||
// assignment creates new Signal
|
||||
Signal& operator=(Signal const& other) {
|
||||
Signal& operator=(Signal const& other)
|
||||
{
|
||||
disconnect_all();
|
||||
}
|
||||
|
||||
|
|
@ -96,7 +105,6 @@ private:
|
|||
mutable std::map<int, std::function<void(Args...)>> slots_;
|
||||
mutable int current_id_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif /* common_utils_Signal_hpp */
|
||||
|
|
@ -8,32 +8,36 @@
|
|||
#include <algorithm>
|
||||
#include <tuple>
|
||||
|
||||
namespace common_utils {
|
||||
namespace common_utils
|
||||
{
|
||||
|
||||
// SmoothingFilter is a filter that removes the outliers that fall outside a given percentage from the min/max
|
||||
// range of numbers over a given window size.
|
||||
// range of numbers over a given window size.
|
||||
template <typename T>
|
||||
class SmoothingFilter {
|
||||
private:
|
||||
std::vector<T> buffer_;
|
||||
int window_size_;
|
||||
int window_size_2x_;
|
||||
float outlier_factor_;
|
||||
int buffer_index_;
|
||||
public:
|
||||
SmoothingFilter();
|
||||
SmoothingFilter(int window_size, float outlier_factor);
|
||||
void initialize(int window_size, float outlier_factor);
|
||||
|
||||
// Each call to the filter method adds to the current window, and returns
|
||||
// the mean and variance over that window, after removing any outliers.
|
||||
// It returns an error variance = -1 until the window is full. So you must
|
||||
// ignore the first window_size values returned by this filter.
|
||||
std::tuple<double,double> filter(T value);
|
||||
class SmoothingFilter
|
||||
{
|
||||
private:
|
||||
std::vector<T> buffer_;
|
||||
int window_size_;
|
||||
int window_size_2x_;
|
||||
float outlier_factor_;
|
||||
int buffer_index_;
|
||||
|
||||
public:
|
||||
SmoothingFilter();
|
||||
SmoothingFilter(int window_size, float outlier_factor);
|
||||
void initialize(int window_size, float outlier_factor);
|
||||
|
||||
// Each call to the filter method adds to the current window, and returns
|
||||
// the mean and variance over that window, after removing any outliers.
|
||||
// It returns an error variance = -1 until the window is full. So you must
|
||||
// ignore the first window_size values returned by this filter.
|
||||
std::tuple<double, double> filter(T value);
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
void SmoothingFilter<T>::initialize(int window_size, float outlier_factor) {
|
||||
void SmoothingFilter<T>::initialize(int window_size, float outlier_factor)
|
||||
{
|
||||
buffer_.resize(window_size);
|
||||
window_size_ = window_size;
|
||||
window_size_2x_ = window_size * 2;
|
||||
|
|
@ -42,54 +46,57 @@ void SmoothingFilter<T>::initialize(int window_size, float outlier_factor) {
|
|||
}
|
||||
|
||||
template <typename T>
|
||||
SmoothingFilter<T>::SmoothingFilter() {
|
||||
SmoothingFilter<T>::SmoothingFilter()
|
||||
{
|
||||
initialize(static_cast<int>(buffer_.capacity()), std::numeric_limits<float>::infinity());
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
SmoothingFilter<T>::SmoothingFilter(int window_size, float outlier_factor) {
|
||||
initialize(window_size, std::numeric_limits<float>::infinity());
|
||||
SmoothingFilter<T>::SmoothingFilter(int window_size, float outlier_factor)
|
||||
{
|
||||
initialize(window_size, std::numeric_limits<float>::infinity());
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
std::tuple<double,double> SmoothingFilter<T>::filter(T value){
|
||||
std::tuple<double, double> SmoothingFilter<T>::filter(T value)
|
||||
{
|
||||
buffer_[buffer_index_++ % window_size_] = value;
|
||||
if (buffer_index_ == window_size_2x_)
|
||||
buffer_index_ = window_size_;
|
||||
|
||||
buffer_index_ = window_size_;
|
||||
|
||||
if (buffer_index_ >= window_size_) {
|
||||
// find min/max and range of the current buffer.
|
||||
double min = *std::min_element(buffer_.begin(), buffer_.end());
|
||||
double max = *std::max_element(buffer_.begin(), buffer_.end());
|
||||
auto range = (max - min);
|
||||
|
||||
// outliers fall outside this lower and upper bound.
|
||||
// outliers fall outside this lower and upper bound.
|
||||
auto lower_bound = min + (range * outlier_factor_);
|
||||
auto upper_bound = max - (range * outlier_factor_);
|
||||
|
||||
double sum = 0; int count = 0;
|
||||
for(auto i = 0; i < window_size_; ++i) {
|
||||
|
||||
double sum = 0;
|
||||
int count = 0;
|
||||
for (auto i = 0; i < window_size_; ++i) {
|
||||
if (buffer_[i] >= lower_bound && buffer_[i] <= upper_bound) {
|
||||
sum += buffer_[i];
|
||||
++count;
|
||||
}
|
||||
}
|
||||
|
||||
if (count == 0)
|
||||
{
|
||||
if (count == 0) {
|
||||
// this can happen if the numbers are oddly clustered around the min/max values.
|
||||
return std::make_tuple(double(min + range / 2), -1);
|
||||
}
|
||||
double mean = sum / count;
|
||||
|
||||
|
||||
double std_dev_sum = 0;
|
||||
for(auto i = 0; i < window_size_; ++i) {
|
||||
for (auto i = 0; i < window_size_; ++i) {
|
||||
if (buffer_[i] >= lower_bound && buffer_[i] <= upper_bound) {
|
||||
double diff = buffer_[i] - mean;
|
||||
std_dev_sum += diff*diff;
|
||||
std_dev_sum += diff * diff;
|
||||
}
|
||||
}
|
||||
double variance = std_dev_sum / count;
|
||||
double variance = std_dev_sum / count;
|
||||
|
||||
return std::make_tuple(mean, variance);
|
||||
}
|
||||
|
|
@ -100,6 +107,5 @@ std::tuple<double,double> SmoothingFilter<T>::filter(T value){
|
|||
}
|
||||
}
|
||||
|
||||
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -3,49 +3,48 @@
|
|||
|
||||
#if defined(_MSC_VER)
|
||||
//'=': conversion from 'double' to 'float', possible loss of data
|
||||
#define STRICT_MODE_OFF \
|
||||
__pragma(warning(push)) \
|
||||
__pragma(warning( disable : 4100 4189 4244 4245 4239 4267 4365 4464 4456 4505 4514 4571 4624 4625 4626 4668 4701 4710 4820 4917 5026 5027 5031))
|
||||
#define STRICT_MODE_ON \
|
||||
__pragma(warning(pop))
|
||||
|
||||
#define STRICT_MODE_OFF \
|
||||
__pragma(warning(push)) \
|
||||
__pragma(warning(disable : 4100 4189 4244 4245 4239 4267 4365 4464 4456 4505 4514 4571 4624 4625 4626 4668 4701 4710 4820 4917 5026 5027 5031))
|
||||
#define STRICT_MODE_ON \
|
||||
__pragma(warning(pop))
|
||||
|
||||
//TODO: limit scope of below statements required to suppress VC++ warnings
|
||||
#define _CRT_SECURE_NO_WARNINGS 1
|
||||
#pragma warning(disable:4996)
|
||||
#pragma warning(disable : 4996)
|
||||
#endif
|
||||
|
||||
// special way to quiet the warning: warning: format string is not a string literal
|
||||
#ifdef __CLANG__
|
||||
#define IGNORE_FORMAT_STRING_ON \
|
||||
_Pragma("clang diagnostic push") \
|
||||
_Pragma("clang diagnostic ignored \"-Wformat-nonliteral\"")
|
||||
#define IGNORE_FORMAT_STRING_ON \
|
||||
_Pragma("clang diagnostic push") \
|
||||
_Pragma("clang diagnostic ignored \"-Wformat-nonliteral\"")
|
||||
|
||||
#define IGNORE_FORMAT_STRING_OFF \
|
||||
_Pragma("clang diagnostic pop")
|
||||
#define IGNORE_FORMAT_STRING_OFF \
|
||||
_Pragma("clang diagnostic pop")
|
||||
#else
|
||||
#define IGNORE_FORMAT_STRING_ON
|
||||
#define IGNORE_FORMAT_STRING_OFF
|
||||
#define IGNORE_FORMAT_STRING_OFF
|
||||
#endif
|
||||
|
||||
// Please keep this list sorted so it is easier to find stuff, also make sure there
|
||||
// Please keep this list sorted so it is easier to find stuff, also make sure there
|
||||
// is no whitespace after the traling \, GCC doesn't like that.
|
||||
#ifdef __CLANG__
|
||||
#define STRICT_MODE_OFF \
|
||||
_Pragma("clang diagnostic push") \
|
||||
_Pragma("clang diagnostic ignored \"-Wctor-dtor-privacy\"") \
|
||||
_Pragma("clang diagnostic ignored \"-Wdelete-non-virtual-dtor\"") \
|
||||
_Pragma("clang diagnostic ignored \"-Wmissing-field-initializers\"") \
|
||||
_Pragma("clang diagnostic ignored \"-Wold-style-cast\"") \
|
||||
_Pragma("clang diagnostic ignored \"-Wredundant-decls\"") \
|
||||
_Pragma("clang diagnostic ignored \"-Wreturn-type\"") \
|
||||
_Pragma("clang diagnostic ignored \"-Wshadow\"") \
|
||||
_Pragma("clang diagnostic ignored \"-Wstrict-overflow\"") \
|
||||
_Pragma("clang diagnostic ignored \"-Wswitch-default\"") \
|
||||
_Pragma("clang diagnostic ignored \"-Wundef\"") \
|
||||
_Pragma("clang diagnostic ignored \"-Wunused-variable\"") \
|
||||
_Pragma("clang diagnostic ignored \"-Wcast-qual\"") \
|
||||
_Pragma("clang diagnostic ignored \"-Wunused-parameter\"")
|
||||
#define STRICT_MODE_OFF \
|
||||
_Pragma("clang diagnostic push") \
|
||||
_Pragma("clang diagnostic ignored \"-Wctor-dtor-privacy\"") \
|
||||
_Pragma("clang diagnostic ignored \"-Wdelete-non-virtual-dtor\"") \
|
||||
_Pragma("clang diagnostic ignored \"-Wmissing-field-initializers\"") \
|
||||
_Pragma("clang diagnostic ignored \"-Wold-style-cast\"") \
|
||||
_Pragma("clang diagnostic ignored \"-Wredundant-decls\"") \
|
||||
_Pragma("clang diagnostic ignored \"-Wreturn-type\"") \
|
||||
_Pragma("clang diagnostic ignored \"-Wshadow\"") \
|
||||
_Pragma("clang diagnostic ignored \"-Wstrict-overflow\"") \
|
||||
_Pragma("clang diagnostic ignored \"-Wswitch-default\"") \
|
||||
_Pragma("clang diagnostic ignored \"-Wundef\"") \
|
||||
_Pragma("clang diagnostic ignored \"-Wunused-variable\"") \
|
||||
_Pragma("clang diagnostic ignored \"-Wcast-qual\"") \
|
||||
_Pragma("clang diagnostic ignored \"-Wunused-parameter\"")
|
||||
|
||||
/* Addition options that can be enabled
|
||||
_Pragma("clang diagnostic ignored \"-Wpedantic\"") \
|
||||
|
|
@ -55,27 +54,26 @@ _Pragma("clang diagnostic ignored \"-Werror=\"") \
|
|||
_Pragma("clang diagnostic ignored \"-Wunused-variable\"") \
|
||||
*/
|
||||
|
||||
#define STRICT_MODE_ON \
|
||||
_Pragma("clang diagnostic pop")
|
||||
|
||||
#define STRICT_MODE_ON \
|
||||
_Pragma("clang diagnostic pop")
|
||||
|
||||
#else
|
||||
#ifdef __GNUC__
|
||||
#define STRICT_MODE_OFF \
|
||||
_Pragma("GCC diagnostic push") \
|
||||
_Pragma("GCC diagnostic ignored \"-Wctor-dtor-privacy\"") \
|
||||
_Pragma("GCC diagnostic ignored \"-Wdelete-non-virtual-dtor\"") \
|
||||
_Pragma("GCC diagnostic ignored \"-Wmissing-field-initializers\"") \
|
||||
_Pragma("GCC diagnostic ignored \"-Wold-style-cast\"") \
|
||||
_Pragma("GCC diagnostic ignored \"-Wredundant-decls\"") \
|
||||
_Pragma("GCC diagnostic ignored \"-Wreturn-type\"") \
|
||||
_Pragma("GCC diagnostic ignored \"-Wshadow\"") \
|
||||
_Pragma("GCC diagnostic ignored \"-Wstrict-overflow\"") \
|
||||
_Pragma("GCC diagnostic ignored \"-Wswitch-default\"") \
|
||||
_Pragma("GCC diagnostic ignored \"-Wundef\"") \
|
||||
_Pragma("GCC diagnostic ignored \"-Wunused-variable\"") \
|
||||
_Pragma("GCC diagnostic ignored \"-Wcast-qual\"") \
|
||||
_Pragma("GCC diagnostic ignored \"-Wunused-parameter\"")
|
||||
#define STRICT_MODE_OFF \
|
||||
_Pragma("GCC diagnostic push") \
|
||||
_Pragma("GCC diagnostic ignored \"-Wctor-dtor-privacy\"") \
|
||||
_Pragma("GCC diagnostic ignored \"-Wdelete-non-virtual-dtor\"") \
|
||||
_Pragma("GCC diagnostic ignored \"-Wmissing-field-initializers\"") \
|
||||
_Pragma("GCC diagnostic ignored \"-Wold-style-cast\"") \
|
||||
_Pragma("GCC diagnostic ignored \"-Wredundant-decls\"") \
|
||||
_Pragma("GCC diagnostic ignored \"-Wreturn-type\"") \
|
||||
_Pragma("GCC diagnostic ignored \"-Wshadow\"") \
|
||||
_Pragma("GCC diagnostic ignored \"-Wstrict-overflow\"") \
|
||||
_Pragma("GCC diagnostic ignored \"-Wswitch-default\"") \
|
||||
_Pragma("GCC diagnostic ignored \"-Wundef\"") \
|
||||
_Pragma("GCC diagnostic ignored \"-Wunused-variable\"") \
|
||||
_Pragma("GCC diagnostic ignored \"-Wcast-qual\"") \
|
||||
_Pragma("GCC diagnostic ignored \"-Wunused-parameter\"")
|
||||
|
||||
/* Addition options that can be enabled
|
||||
_Pragma("GCC diagnostic ignored \"-Wpedantic\"") \
|
||||
|
|
@ -87,8 +85,8 @@ _Pragma("GCC diagnostic ignored \"-Wmaybe-uninitialized\"") \
|
|||
|
||||
*/
|
||||
|
||||
#define STRICT_MODE_ON \
|
||||
_Pragma("GCC diagnostic pop")
|
||||
#define STRICT_MODE_ON \
|
||||
_Pragma("GCC diagnostic pop")
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -3,54 +3,59 @@
|
|||
#include <chrono>
|
||||
#include "Utils.hpp"
|
||||
|
||||
namespace common_utils {
|
||||
class Timer {
|
||||
public:
|
||||
Timer()
|
||||
{
|
||||
started_ = false;
|
||||
}
|
||||
void start()
|
||||
{
|
||||
started_ = true;
|
||||
start_ = now();
|
||||
}
|
||||
void stop()
|
||||
{
|
||||
started_ = false;
|
||||
namespace common_utils
|
||||
{
|
||||
class Timer
|
||||
{
|
||||
public:
|
||||
Timer()
|
||||
{
|
||||
started_ = false;
|
||||
}
|
||||
void start()
|
||||
{
|
||||
started_ = true;
|
||||
start_ = now();
|
||||
}
|
||||
void stop()
|
||||
{
|
||||
started_ = false;
|
||||
end_ = now();
|
||||
}
|
||||
double seconds()
|
||||
{
|
||||
auto diff = static_cast<double>(end() - start_);
|
||||
return diff / 1000000.0;
|
||||
}
|
||||
double milliseconds()
|
||||
{
|
||||
return static_cast<double>(end() - start_) / 1000.0;
|
||||
}
|
||||
double microseconds()
|
||||
{
|
||||
return static_cast<double>(end() - start_);
|
||||
}
|
||||
bool started()
|
||||
{
|
||||
return started_;
|
||||
}
|
||||
|
||||
private:
|
||||
int64_t now()
|
||||
{
|
||||
return std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
|
||||
}
|
||||
int64_t end()
|
||||
{
|
||||
if (started_) {
|
||||
// not stopped yet, so return "elapsed time so far".
|
||||
end_ = now();
|
||||
}
|
||||
double seconds()
|
||||
{
|
||||
auto diff = static_cast<double>(end() - start_);
|
||||
return diff / 1000000.0;
|
||||
}
|
||||
double milliseconds()
|
||||
{
|
||||
return static_cast<double>(end() - start_) / 1000.0;
|
||||
}
|
||||
double microseconds()
|
||||
{
|
||||
return static_cast<double>(end() - start_);
|
||||
}
|
||||
bool started()
|
||||
{
|
||||
return started_;
|
||||
}
|
||||
private:
|
||||
int64_t now() {
|
||||
return std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
|
||||
}
|
||||
int64_t end() {
|
||||
if (started_) {
|
||||
// not stopped yet, so return "elapsed time so far".
|
||||
end_ = now();
|
||||
}
|
||||
return end_;
|
||||
}
|
||||
int64_t start_;
|
||||
int64_t end_;
|
||||
bool started_;
|
||||
};
|
||||
return end_;
|
||||
}
|
||||
int64_t start_;
|
||||
int64_t end_;
|
||||
bool started_;
|
||||
};
|
||||
}
|
||||
#endif
|
||||
|
|
@ -7,12 +7,14 @@
|
|||
#include <set>
|
||||
#include "Utils.hpp"
|
||||
|
||||
namespace common_utils {
|
||||
namespace common_utils
|
||||
{
|
||||
|
||||
//This class allows to maintain unique set of values while
|
||||
//This class allows to maintain unique set of values while
|
||||
//still allowing key to value maps
|
||||
template <class TKey, class TVal>
|
||||
class UniqueValueMap {
|
||||
class UniqueValueMap
|
||||
{
|
||||
public:
|
||||
void insert(const TKey& key, const TVal& val)
|
||||
{
|
||||
|
|
@ -77,7 +79,5 @@ private:
|
|||
std::map<TKey, TVal> map_;
|
||||
std::set<TVal> vals_;
|
||||
};
|
||||
|
||||
|
||||
}
|
||||
#endif
|
||||
|
|
@ -56,8 +56,7 @@
|
|||
*/
|
||||
|
||||
#ifndef _MSC_VER
|
||||
__attribute__((__format__ (__printf__, 1, 0)))
|
||||
static int _vscprintf(const char * format, va_list pargs)
|
||||
__attribute__((__format__(__printf__, 1, 0))) static int _vscprintf(const char* format, va_list pargs)
|
||||
{
|
||||
int retval;
|
||||
va_list argcopy;
|
||||
|
|
@ -71,12 +70,17 @@ static int _vscprintf(const char * format, va_list pargs)
|
|||
#endif
|
||||
|
||||
// Call this on a function parameter to suppress the unused paramter warning
|
||||
template <class T> inline
|
||||
void unused(T const & result) { static_cast<void>(result); }
|
||||
template <class T>
|
||||
inline void unused(T const& result)
|
||||
{
|
||||
static_cast<void>(result);
|
||||
}
|
||||
|
||||
namespace common_utils {
|
||||
namespace common_utils
|
||||
{
|
||||
|
||||
class Utils {
|
||||
class Utils
|
||||
{
|
||||
private:
|
||||
typedef std::chrono::system_clock system_clock;
|
||||
typedef std::chrono::steady_clock steady_clock;
|
||||
|
|
@ -85,11 +89,11 @@ private:
|
|||
//this is not required for most compilers
|
||||
typedef unsigned int uint;
|
||||
template <typename T>
|
||||
using time_point = std::chrono::time_point<T>;
|
||||
|
||||
using time_point = std::chrono::time_point<T>;
|
||||
|
||||
public:
|
||||
class Logger {
|
||||
class Logger
|
||||
{
|
||||
public:
|
||||
virtual void log(int level, const std::string& message)
|
||||
{
|
||||
|
|
@ -99,13 +103,14 @@ public:
|
|||
std::cerr << message << std::endl;
|
||||
}
|
||||
};
|
||||
|
||||
static void enableImmediateConsoleFlush() {
|
||||
|
||||
static void enableImmediateConsoleFlush()
|
||||
{
|
||||
//disable buffering
|
||||
setbuf(stdout, NULL);
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
template <typename T>
|
||||
static T getRandomFromGaussian(T stddev = 1, T mean = 0)
|
||||
{
|
||||
static std::default_random_engine random_gen;
|
||||
|
|
@ -113,25 +118,30 @@ public:
|
|||
|
||||
return gaussian_dist(random_gen) * stddev + mean;
|
||||
}
|
||||
|
||||
static constexpr double degreesToRadians(double degrees) {
|
||||
|
||||
static constexpr double degreesToRadians(double degrees)
|
||||
{
|
||||
return static_cast<double>(M_PIl * degrees / 180.0);
|
||||
}
|
||||
static constexpr float degreesToRadians(float degrees) {
|
||||
static constexpr float degreesToRadians(float degrees)
|
||||
{
|
||||
return static_cast<float>(M_PI * degrees / 180.0f);
|
||||
}
|
||||
static constexpr double radiansToDegrees(double radians) {
|
||||
static constexpr double radiansToDegrees(double radians)
|
||||
{
|
||||
return static_cast<double>(radians * 180.0 / M_PIl);
|
||||
}
|
||||
static constexpr float radiansToDegrees(float radians) {
|
||||
static constexpr float radiansToDegrees(float radians)
|
||||
{
|
||||
return static_cast<float>(radians * 180.0f / M_PI);
|
||||
}
|
||||
|
||||
static bool startsWith(const string& s, const string& prefix) {
|
||||
static bool startsWith(const string& s, const string& prefix)
|
||||
{
|
||||
return s.size() >= prefix.size() && s.compare(0, prefix.size(), prefix) == 0;
|
||||
}
|
||||
|
||||
template <template<class, class, class...> class TContainer, typename TKey, typename TVal, typename... Args>
|
||||
template <template <class, class, class...> class TContainer, typename TKey, typename TVal, typename... Args>
|
||||
static const TVal& findOrDefault(const TContainer<TKey, TVal, Args...>& m, TKey const& key, const TVal& default_val)
|
||||
{
|
||||
typename TContainer<TKey, TVal, Args...>::const_iterator it = m.find(key);
|
||||
|
|
@ -140,7 +150,7 @@ public:
|
|||
return it->second;
|
||||
}
|
||||
|
||||
template <template<class, class, class...> class TContainer, typename TKey, typename TVal, typename... Args>
|
||||
template <template <class, class, class...> class TContainer, typename TKey, typename TVal, typename... Args>
|
||||
static const TVal& findOrDefault(const TContainer<TKey, TVal, Args...>& m, TKey const& key)
|
||||
{
|
||||
static TVal default_val;
|
||||
|
|
@ -168,58 +178,60 @@ public:
|
|||
if (level >= getSetMinLogLevel())
|
||||
getSetLogger()->log(level, message);
|
||||
}
|
||||
static int getSetMinLogLevel(bool set_or_get = false,
|
||||
int set_min_log_level = std::numeric_limits<int>::min())
|
||||
static int getSetMinLogLevel(bool set_or_get = false,
|
||||
int set_min_log_level = std::numeric_limits<int>::min())
|
||||
{
|
||||
static int min_log_level = std::numeric_limits<int>::min();
|
||||
if (set_or_get)
|
||||
min_log_level = set_min_log_level;
|
||||
|
||||
|
||||
return min_log_level;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
static int sign(T val) {
|
||||
static int sign(T val)
|
||||
{
|
||||
return T(0) < val ? 1 : (T(0) > val ? -1 : 0);
|
||||
}
|
||||
}
|
||||
|
||||
/// Limits absolute value whole preserving sign
|
||||
template <typename T>
|
||||
static T limitAbsValue(T val, T min_value, T max_value) {
|
||||
template <typename T>
|
||||
static T limitAbsValue(T val, T min_value, T max_value)
|
||||
{
|
||||
T val_abs = std::abs(val);
|
||||
T val_limited = std::max(val_abs, min_value);
|
||||
val_limited = std::min(val_limited, max_value);
|
||||
return sign(val) * val_limited;
|
||||
}
|
||||
|
||||
|
||||
/// Limits absolute value whole preserving sign
|
||||
template <typename T>
|
||||
static T clip(T val, T min_value, T max_value) {
|
||||
template <typename T>
|
||||
static T clip(T val, T min_value, T max_value)
|
||||
{
|
||||
return std::max(min_value, std::min(val, max_value));
|
||||
}
|
||||
|
||||
template<typename Range>
|
||||
template <typename Range>
|
||||
static const string printRange(Range&& range, const string& delim = ", ",
|
||||
const string& prefix="(", const string& suffix=")")
|
||||
const string& prefix = "(", const string& suffix = ")")
|
||||
{
|
||||
return printRange(std::begin(range), std::end(range), delim, prefix, suffix);
|
||||
}
|
||||
template<typename Iterator>
|
||||
template <typename Iterator>
|
||||
static const string printRange(Iterator start, Iterator last, const string& delim = ", ",
|
||||
const string& prefix="(", const string& suffix=")")
|
||||
const string& prefix = "(", const string& suffix = ")")
|
||||
{
|
||||
stringstream ss;
|
||||
ss << prefix;
|
||||
|
||||
for (Iterator i = start; i != last; ++i)
|
||||
{
|
||||
for (Iterator i = start; i != last; ++i) {
|
||||
if (i != start)
|
||||
ss << delim;
|
||||
ss << *i;
|
||||
}
|
||||
|
||||
ss << suffix;
|
||||
return ss.str();
|
||||
return ss.str();
|
||||
}
|
||||
|
||||
static std::string getFileExtension(const string& str)
|
||||
|
|
@ -227,8 +239,7 @@ public:
|
|||
int len = static_cast<int>(str.size());
|
||||
const char* ptr = str.c_str();
|
||||
int i = 0;
|
||||
for (i = len - 1; i >= 0; i--)
|
||||
{
|
||||
for (i = len - 1; i >= 0; i--) {
|
||||
if (ptr[i] == '.')
|
||||
break;
|
||||
}
|
||||
|
|
@ -236,10 +247,11 @@ public:
|
|||
return str.substr(i, len - i);
|
||||
}
|
||||
|
||||
#ifndef _MSC_VER
|
||||
__attribute__((__format__ (__printf__, 1, 0)))
|
||||
#endif
|
||||
static string stringf(const char* format, ...)
|
||||
#ifndef _MSC_VER
|
||||
__attribute__((__format__(__printf__, 1, 0)))
|
||||
#endif
|
||||
static string
|
||||
stringf(const char* format, ...)
|
||||
{
|
||||
va_list args;
|
||||
va_start(args, format);
|
||||
|
|
@ -247,17 +259,17 @@ public:
|
|||
IGNORE_FORMAT_STRING_ON
|
||||
auto size = _vscprintf(format, args) + 1U;
|
||||
IGNORE_FORMAT_STRING_OFF
|
||||
std::unique_ptr<char[]> buf(new char[size] );
|
||||
std::unique_ptr<char[]> buf(new char[size]);
|
||||
|
||||
#ifndef _MSC_VER
|
||||
IGNORE_FORMAT_STRING_ON
|
||||
vsnprintf(buf.get(), size, format, args);
|
||||
IGNORE_FORMAT_STRING_OFF
|
||||
#else
|
||||
vsnprintf_s(buf.get(), size, _TRUNCATE, format, args);
|
||||
#endif
|
||||
#ifndef _MSC_VER
|
||||
IGNORE_FORMAT_STRING_ON
|
||||
vsnprintf(buf.get(), size, format, args);
|
||||
IGNORE_FORMAT_STRING_OFF
|
||||
#else
|
||||
vsnprintf_s(buf.get(), size, _TRUNCATE, format, args);
|
||||
#endif
|
||||
|
||||
va_end(args);
|
||||
va_end(args);
|
||||
|
||||
return string(buf.get());
|
||||
}
|
||||
|
|
@ -267,14 +279,12 @@ public:
|
|||
int len = static_cast<int>(str.size());
|
||||
const char* ptr = str.c_str();
|
||||
int i = 0;
|
||||
for (i = 0; i < len; i++)
|
||||
{
|
||||
for (i = 0; i < len; i++) {
|
||||
if (ptr[i] != ch)
|
||||
break;
|
||||
}
|
||||
int j = 0;
|
||||
for (j = len - 1; j >= i; j--)
|
||||
{
|
||||
for (j = len - 1; j >= i; j--) {
|
||||
if (ptr[j] != ch)
|
||||
break;
|
||||
}
|
||||
|
|
@ -285,29 +295,24 @@ public:
|
|||
{
|
||||
auto start = s.begin();
|
||||
std::vector<string> result;
|
||||
for (auto it = s.begin(); it != s.end(); it++)
|
||||
{
|
||||
for (auto it = s.begin(); it != s.end(); it++) {
|
||||
char ch = *it;
|
||||
bool split = false;
|
||||
for (int i = 0; i < numSplitChars; i++)
|
||||
{
|
||||
for (int i = 0; i < numSplitChars; i++) {
|
||||
if (ch == splitChars[i]) {
|
||||
split = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (split)
|
||||
{
|
||||
if (start < it)
|
||||
{
|
||||
if (split) {
|
||||
if (start < it) {
|
||||
result.push_back(string(start, it));
|
||||
}
|
||||
start = it;
|
||||
start++;
|
||||
}
|
||||
}
|
||||
if (start < s.end())
|
||||
{
|
||||
if (start < s.end()) {
|
||||
result.push_back(string(start, s.end()));
|
||||
}
|
||||
return result;
|
||||
|
|
@ -321,25 +326,24 @@ public:
|
|||
auto start = line.begin();
|
||||
std::vector<std::string> result;
|
||||
auto end = line.end();
|
||||
for (auto it = line.begin(); it != end; )
|
||||
{
|
||||
for (auto it = line.begin(); it != end;) {
|
||||
bool split = false;
|
||||
char ch = *it;
|
||||
if (ch == '\'' || ch == '"') {
|
||||
// skip quoted literal
|
||||
if (start < it)
|
||||
{
|
||||
if (start < it) {
|
||||
result.push_back(string(start, it));
|
||||
}
|
||||
it++;
|
||||
start = it;
|
||||
for (; it != end; it++) {
|
||||
for (; it != end; it++) {
|
||||
if (*it == ch) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
split = true;
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
for (int i = 0; i < numSeparators; i++) {
|
||||
if (ch == separators[i]) {
|
||||
split = true;
|
||||
|
|
@ -347,10 +351,8 @@ public:
|
|||
}
|
||||
}
|
||||
}
|
||||
if (split)
|
||||
{
|
||||
if (start < it)
|
||||
{
|
||||
if (split) {
|
||||
if (start < it) {
|
||||
result.push_back(string(start, it));
|
||||
}
|
||||
start = it;
|
||||
|
|
@ -360,8 +362,7 @@ public:
|
|||
it++;
|
||||
}
|
||||
}
|
||||
if (start < end)
|
||||
{
|
||||
if (start < end) {
|
||||
result.push_back(string(start, end));
|
||||
}
|
||||
return result;
|
||||
|
|
@ -377,8 +378,7 @@ public:
|
|||
_strlwr_s(buf.get(), len + 1U);
|
||||
#else
|
||||
char* p = buf.get();
|
||||
for (int i = len; i > 0; i--)
|
||||
{
|
||||
for (int i = len; i > 0; i--) {
|
||||
*p = tolower(*p);
|
||||
p++;
|
||||
}
|
||||
|
|
@ -394,8 +394,7 @@ public:
|
|||
// return (onecount != 0)
|
||||
// ? (static_cast<R>(-1) >> ((sizeof(R) * CHAR_BIT) - onecount))
|
||||
// : 0;
|
||||
return static_cast<R>(-(onecount != 0))
|
||||
& (static_cast<R>(-1) >> ((sizeof(R) * CHAR_BIT) - onecount));
|
||||
return static_cast<R>(-(onecount != 0)) & (static_cast<R>(-1) >> ((sizeof(R) * CHAR_BIT) - onecount));
|
||||
}
|
||||
|
||||
static void cleanupThread(std::thread& th)
|
||||
|
|
@ -408,36 +407,39 @@ public:
|
|||
|
||||
static inline int floorToInt(float x)
|
||||
{
|
||||
return static_cast<int> (std::floor(x));
|
||||
return static_cast<int>(std::floor(x));
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
static constexpr T nan() {
|
||||
template <typename T>
|
||||
static constexpr T nan()
|
||||
{
|
||||
return std::numeric_limits<T>::quiet_NaN();
|
||||
}
|
||||
template<typename T>
|
||||
static constexpr T max() {
|
||||
template <typename T>
|
||||
static constexpr T max()
|
||||
{
|
||||
return std::numeric_limits<T>::max();
|
||||
}
|
||||
template<typename T>
|
||||
static constexpr T min() {
|
||||
template <typename T>
|
||||
static constexpr T min()
|
||||
{
|
||||
return std::numeric_limits<T>::min();
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
template <typename T>
|
||||
static void setValue(T arr[], size_t length, const T& val)
|
||||
{
|
||||
std::fill(arr, arr + length, val);
|
||||
}
|
||||
|
||||
template<typename T, size_t N>
|
||||
template <typename T, size_t N>
|
||||
static void setValue(T (&arr)[N], const T& val)
|
||||
{
|
||||
std::fill(arr, arr+N, val);
|
||||
std::fill(arr, arr + N, val);
|
||||
}
|
||||
|
||||
template< class T, size_t N>
|
||||
static std::size_t length(const T(&)[N])
|
||||
template <class T, size_t N>
|
||||
static std::size_t length(const T (&)[N])
|
||||
{
|
||||
return N;
|
||||
}
|
||||
|
|
@ -447,7 +449,7 @@ public:
|
|||
std::ofstream file(file_name, std::ios::binary);
|
||||
file.write(data, size);
|
||||
}
|
||||
template<typename Container>
|
||||
template <typename Container>
|
||||
static typename std::enable_if<type_utils::is_container<Container>::value, void>::type
|
||||
append(Container& to, const Container& from)
|
||||
{
|
||||
|
|
@ -455,15 +457,15 @@ public:
|
|||
using std::end;
|
||||
to.insert(end(to), begin(from), end(from));
|
||||
}
|
||||
template<typename Container>
|
||||
template <typename Container>
|
||||
static typename std::enable_if<type_utils::is_container<Container>::value, void>::type
|
||||
copy(const Container& from, Container& to)
|
||||
copy(const Container& from, Container& to)
|
||||
{
|
||||
using std::begin;
|
||||
using std::end;
|
||||
std::copy(begin(from), end(from), begin(to));
|
||||
}
|
||||
template<typename T>
|
||||
template <typename T>
|
||||
static void copy(const T* from, T* to, uint count)
|
||||
{
|
||||
std::copy(from, from + count, to);
|
||||
|
|
@ -500,7 +502,8 @@ public:
|
|||
char str[1024];
|
||||
if (std::strftime(str, sizeof(str), format, std::localtime(&tt)))
|
||||
return string(str);
|
||||
else return string();
|
||||
else
|
||||
return string();
|
||||
}
|
||||
|
||||
static string to_string(time_point<system_clock> time, const char* format = "%Y-%m-%d-%H-%M-%S")
|
||||
|
|
@ -509,7 +512,8 @@ public:
|
|||
char str[1024];
|
||||
if (std::strftime(str, sizeof(str), format, std::localtime(&tt)))
|
||||
return string(str);
|
||||
else return string();
|
||||
else
|
||||
return string();
|
||||
}
|
||||
static string getLogFileTimeStamp()
|
||||
{
|
||||
|
|
@ -537,27 +541,27 @@ public:
|
|||
static double getTimeSinceEpochSecs(std::chrono::system_clock::time_point* t = nullptr)
|
||||
{
|
||||
using Clock = std::chrono::system_clock; //high res clock has epoch since boot instead of since 1970 for VC++
|
||||
return std::chrono::duration<double>((t != nullptr ? *t : Clock::now() ).time_since_epoch()).count();
|
||||
return std::chrono::duration<double>((t != nullptr ? *t : Clock::now()).time_since_epoch()).count();
|
||||
}
|
||||
static uint64_t getTimeSinceEpochNanos(std::chrono::system_clock::time_point* t = nullptr)
|
||||
{
|
||||
using Clock = std::chrono::system_clock; //high res clock has epoch since boot instead of since 1970 for VC++
|
||||
return std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||
(t != nullptr ? *t : Clock::now())
|
||||
.time_since_epoch()).
|
||||
count();
|
||||
(t != nullptr ? *t : Clock::now())
|
||||
.time_since_epoch())
|
||||
.count();
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
static void clear(std::queue<T> &q, size_t max_elements = SIZE_MAX)
|
||||
template <typename T>
|
||||
static void clear(std::queue<T>& q, size_t max_elements = SIZE_MAX)
|
||||
{
|
||||
while(!q.empty() && max_elements > 0) {
|
||||
while (!q.empty() && max_elements > 0) {
|
||||
q.pop();
|
||||
--max_elements;
|
||||
}
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
template <typename T>
|
||||
static const std::vector<T>& emptyVector()
|
||||
{
|
||||
static const std::vector<T> empty_vector;
|
||||
|
|
@ -579,30 +583,30 @@ public:
|
|||
return celcius + 273.15f;
|
||||
}
|
||||
|
||||
template<typename TReal>
|
||||
template <typename TReal>
|
||||
static constexpr TReal epsilon()
|
||||
{
|
||||
return std::numeric_limits<TReal>::epsilon();
|
||||
}
|
||||
|
||||
//implements relative method - do not use for comparing with zero
|
||||
//use this most of the time, tolerance needs to be meaningful in your context
|
||||
template<typename TReal>
|
||||
static bool isApproximatelyEqual(TReal a, TReal b, TReal tolerance = epsilon<TReal>())
|
||||
{
|
||||
TReal diff = std::fabs(a - b);
|
||||
if (diff <= tolerance)
|
||||
return true;
|
||||
//implements relative method - do not use for comparing with zero
|
||||
//use this most of the time, tolerance needs to be meaningful in your context
|
||||
template <typename TReal>
|
||||
static bool isApproximatelyEqual(TReal a, TReal b, TReal tolerance = epsilon<TReal>())
|
||||
{
|
||||
TReal diff = std::fabs(a - b);
|
||||
if (diff <= tolerance)
|
||||
return true;
|
||||
|
||||
if (diff < std::fmax(std::fabs(a), std::fabs(b)) * tolerance)
|
||||
return true;
|
||||
if (diff < std::fmax(std::fabs(a), std::fabs(b)) * tolerance)
|
||||
return true;
|
||||
|
||||
return false;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
//supply tolerance that is meaningful in your context
|
||||
//for example, default tolerance may not work if you are comparing double with float
|
||||
template<typename TReal>
|
||||
template <typename TReal>
|
||||
static bool isApproximatelyZero(TReal a, TReal tolerance = epsilon<TReal>())
|
||||
{
|
||||
if (std::fabs(a) <= tolerance)
|
||||
|
|
@ -610,10 +614,9 @@ public:
|
|||
return false;
|
||||
}
|
||||
|
||||
|
||||
//use this when you want to be on safe side
|
||||
//for example, don't start rover unless signal is above 1
|
||||
template<typename TReal>
|
||||
template <typename TReal>
|
||||
static bool isDefinitelyLessThan(TReal a, TReal b, TReal tolerance = epsilon<TReal>())
|
||||
{
|
||||
TReal diff = a - b;
|
||||
|
|
@ -625,7 +628,7 @@ public:
|
|||
|
||||
return false;
|
||||
}
|
||||
template<typename TReal>
|
||||
template <typename TReal>
|
||||
static bool isDefinitelyGreaterThan(TReal a, TReal b, TReal tolerance = epsilon<TReal>())
|
||||
{
|
||||
TReal diff = a - b;
|
||||
|
|
@ -642,7 +645,7 @@ public:
|
|||
//use this when you are only concerned about floating point precision issue
|
||||
//for example, if you want to see if a is 1.0 by checking if its within
|
||||
//10 closest representable floating point numbers around 1.0.
|
||||
template<typename TReal>
|
||||
template <typename TReal>
|
||||
static bool isWithinPrecisionInterval(TReal a, TReal b, unsigned int interval_size = 1)
|
||||
{
|
||||
TReal min_a = a - (a - std::nextafter(a, std::numeric_limits<TReal>::lowest())) * interval_size;
|
||||
|
|
@ -656,17 +659,19 @@ public:
|
|||
#ifdef _MSC_VER
|
||||
__debugbreak();
|
||||
#else
|
||||
//TODO: Use GCC and Clang version from https://github.com/scottt/debugbreak
|
||||
//TODO: Use GCC and Clang version from https://github.com/scottt/debugbreak
|
||||
#endif
|
||||
}
|
||||
|
||||
//convert strongly typed enum to underlying scaler types
|
||||
template <typename E>
|
||||
static constexpr typename std::underlying_type<E>::type toNumeric(E e) {
|
||||
static constexpr typename std::underlying_type<E>::type toNumeric(E e)
|
||||
{
|
||||
return static_cast<typename std::underlying_type<E>::type>(e);
|
||||
}
|
||||
template <typename E>
|
||||
static constexpr E toEnum(typename std::underlying_type<E>::type u) {
|
||||
static constexpr E toEnum(typename std::underlying_type<E>::type u)
|
||||
{
|
||||
return static_cast<E>(u);
|
||||
}
|
||||
|
||||
|
|
@ -674,33 +679,33 @@ public:
|
|||
static bool isLittleEndian()
|
||||
{
|
||||
int intval = 1;
|
||||
unsigned char *uval = reinterpret_cast<unsigned char *>(&intval);
|
||||
unsigned char* uval = reinterpret_cast<unsigned char*>(&intval);
|
||||
return uval[0] == 1;
|
||||
}
|
||||
|
||||
static void writePFMfile(const float * const image_data, int width, int height, const std::string& path, float scalef=1)
|
||||
static void writePFMfile(const float* const image_data, int width, int height, const std::string& path, float scalef = 1)
|
||||
{
|
||||
std::ofstream file(path.c_str(), std::ios::binary);
|
||||
|
||||
std::string bands;
|
||||
float fvalue; // scale factor and temp value to hold pixel value
|
||||
bands = "Pf"; // grayscale
|
||||
float fvalue; // scale factor and temp value to hold pixel value
|
||||
bands = "Pf"; // grayscale
|
||||
|
||||
// sign of scalefact indicates endianness, see pfm specs
|
||||
if(isLittleEndian())
|
||||
if (isLittleEndian())
|
||||
scalef = -scalef;
|
||||
|
||||
// insert header information
|
||||
file << bands << "\n";
|
||||
file << width << " ";
|
||||
file << height << "\n";
|
||||
file << scalef << "\n";
|
||||
// insert header information
|
||||
file << bands << "\n";
|
||||
file << width << " ";
|
||||
file << height << "\n";
|
||||
file << scalef << "\n";
|
||||
|
||||
if(bands == "Pf"){ // handle 1-band image
|
||||
for (int i=0; i < height; i++) {
|
||||
for(int j=0; j < width; ++j){
|
||||
if (bands == "Pf") { // handle 1-band image
|
||||
for (int i = 0; i < height; i++) {
|
||||
for (int j = 0; j < width; ++j) {
|
||||
fvalue = image_data[i * width + j];
|
||||
file.write(reinterpret_cast<char *>(&fvalue), sizeof(fvalue));
|
||||
file.write(reinterpret_cast<char*>(&fvalue), sizeof(fvalue));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -713,29 +718,29 @@ public:
|
|||
std::ofstream file(path.c_str(), std::ios::binary);
|
||||
|
||||
// Header information
|
||||
file << "P6\n"; // Magic type for PPM files
|
||||
file << "P6\n"; // Magic type for PPM files
|
||||
file << width << " " << height << "\n";
|
||||
file << "255\n"; // Max color value
|
||||
file << "255\n"; // Max color value
|
||||
|
||||
auto write_binary = [&file](const uint8_t &data) {
|
||||
auto write_binary = [&file](const uint8_t& data) {
|
||||
file.write(reinterpret_cast<const char*>(&data), sizeof(data));
|
||||
};
|
||||
|
||||
for (int i=0; i<height; i++) {
|
||||
for (int j=0; j<width; j++) {
|
||||
int id = (i*width + j)*3; // Pixel index
|
||||
for (int i = 0; i < height; i++) {
|
||||
for (int j = 0; j < width; j++) {
|
||||
int id = (i * width + j) * 3; // Pixel index
|
||||
|
||||
// Image is in BGR, write as RGB
|
||||
write_binary(image_data[id+2]); // R
|
||||
write_binary(image_data[id+1]); // G
|
||||
write_binary(image_data[id]); // B
|
||||
write_binary(image_data[id + 2]); // R
|
||||
write_binary(image_data[id + 1]); // G
|
||||
write_binary(image_data[id]); // B
|
||||
}
|
||||
}
|
||||
|
||||
file.close();
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
template <typename T>
|
||||
static std::string toBinaryString(const T& x)
|
||||
{
|
||||
std::stringstream ss;
|
||||
|
|
|
|||
|
|
@ -11,7 +11,7 @@
|
|||
#include "Windows/AllowWindowsPlatformAtomics.h"
|
||||
//remove warnings for VC++
|
||||
#pragma warning(push)
|
||||
#pragma warning(disable:4191 6000 28251)
|
||||
#pragma warning(disable:4996) //warning C4996: This function or variable may be unsafe. Consider using xxx instead.
|
||||
#pragma warning(disable:4005) //warning C4005: 'TEXT': macro redefinition
|
||||
#pragma warning(disable : 4191 6000 28251)
|
||||
#pragma warning(disable : 4996) //warning C4996: This function or variable may be unsafe. Consider using xxx instead.
|
||||
#pragma warning(disable : 4005) //warning C4005: 'TEXT': macro redefinition
|
||||
#endif
|
||||
|
|
|
|||
Разница между файлами не показана из-за своего большого размера
Загрузить разницу
|
|
@ -16,7 +16,6 @@
|
|||
*
|
||||
*********************************************************/
|
||||
|
||||
|
||||
#ifndef __ctpl_stl_thread_pool_H__
|
||||
#define __ctpl_stl_thread_pool_H__
|
||||
|
||||
|
|
@ -30,225 +29,242 @@
|
|||
#include <mutex>
|
||||
#include <queue>
|
||||
|
||||
|
||||
|
||||
// thread pool to run user's functors with signature
|
||||
// ret func(int id, other_params)
|
||||
// where id is the index of the thread that runs the functor
|
||||
// ret is some return type
|
||||
|
||||
namespace ctpl
|
||||
{
|
||||
|
||||
namespace ctpl {
|
||||
|
||||
namespace detail {
|
||||
template <typename T>
|
||||
class Queue {
|
||||
public:
|
||||
bool push(T const & value) {
|
||||
std::unique_lock<std::mutex> lock(this->mutex);
|
||||
this->q.push(value);
|
||||
return true;
|
||||
}
|
||||
// deletes the retrieved element, do not use for non integral types
|
||||
bool pop(T & v) {
|
||||
std::unique_lock<std::mutex> lock(this->mutex);
|
||||
if (this->q.empty())
|
||||
return false;
|
||||
v = this->q.front();
|
||||
this->q.pop();
|
||||
return true;
|
||||
}
|
||||
bool empty() {
|
||||
std::unique_lock<std::mutex> lock(this->mutex);
|
||||
return this->q.empty();
|
||||
}
|
||||
private:
|
||||
std::queue<T> q;
|
||||
std::mutex mutex;
|
||||
};
|
||||
}
|
||||
|
||||
class thread_pool {
|
||||
|
||||
namespace detail
|
||||
{
|
||||
template <typename T>
|
||||
class Queue
|
||||
{
|
||||
public:
|
||||
|
||||
thread_pool() { this->init(); }
|
||||
thread_pool(int nThreads) { this->init(); this->resize(nThreads); }
|
||||
|
||||
// the destructor waits for all the functions in the queue to be finished
|
||||
~thread_pool() {
|
||||
this->stop(true);
|
||||
}
|
||||
|
||||
// get the number of running threads in the pool
|
||||
int size() { return static_cast<int>(this->threads.size()); }
|
||||
|
||||
// number of idle threads
|
||||
int n_idle() { return this->nWaiting; }
|
||||
std::thread & get_thread(int i) { return *this->threads[i]; }
|
||||
|
||||
// change the number of threads in the pool
|
||||
// should be called from one thread, otherwise be careful to not interleave, also with this->stop()
|
||||
// nThreads must be >= 0
|
||||
void resize(int nThreads) {
|
||||
if (!this->isStop && !this->isDone) {
|
||||
int oldNThreads = static_cast<int>(this->threads.size());
|
||||
if (oldNThreads <= nThreads) { // if the number of threads is increased
|
||||
this->threads.resize(nThreads);
|
||||
this->flags.resize(nThreads);
|
||||
|
||||
for (int i = oldNThreads; i < nThreads; ++i) {
|
||||
this->flags[i] = std::make_shared<std::atomic<bool>>(false);
|
||||
this->set_thread(i);
|
||||
}
|
||||
}
|
||||
else { // the number of threads is decreased
|
||||
for (int i = oldNThreads - 1; i >= nThreads; --i) {
|
||||
*this->flags[i] = true; // this thread will finish
|
||||
this->threads[i]->detach();
|
||||
}
|
||||
{
|
||||
// stop the detached threads that were waiting
|
||||
std::unique_lock<std::mutex> lock(this->mutex);
|
||||
this->cv.notify_all();
|
||||
}
|
||||
this->threads.resize(nThreads); // safe to delete because the threads are detached
|
||||
this->flags.resize(nThreads); // safe to delete because the threads have copies of shared_ptr of the flags, not originals
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// empty the queue
|
||||
void clear_queue() {
|
||||
std::function<void(int id)> * _f;
|
||||
while (this->q.pop(_f))
|
||||
delete _f; // empty the queue
|
||||
}
|
||||
|
||||
// pops a functional wrapper to the original function
|
||||
std::function<void(int)> pop() {
|
||||
std::function<void(int id)> * _f = nullptr;
|
||||
this->q.pop(_f);
|
||||
std::unique_ptr<std::function<void(int id)>> func(_f); // at return, delete the function even if an exception occurred
|
||||
std::function<void(int)> f;
|
||||
if (_f)
|
||||
f = *_f;
|
||||
return f;
|
||||
}
|
||||
|
||||
// wait for all computing threads to finish and stop all threads
|
||||
// may be called asynchronously to not pause the calling thread while waiting
|
||||
// if isWait == true, all the functions in the queue are run, otherwise the queue is cleared without running the functions
|
||||
void stop(bool isWait = false) {
|
||||
if (!isWait) {
|
||||
if (this->isStop)
|
||||
return;
|
||||
this->isStop = true;
|
||||
for (int i = 0, n = this->size(); i < n; ++i) {
|
||||
*this->flags[i] = true; // command the threads to stop
|
||||
}
|
||||
this->clear_queue(); // empty the queue
|
||||
}
|
||||
else {
|
||||
if (this->isDone || this->isStop)
|
||||
return;
|
||||
this->isDone = true; // give the waiting threads a command to finish
|
||||
}
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(this->mutex);
|
||||
this->cv.notify_all(); // stop all waiting threads
|
||||
}
|
||||
for (int i = 0; i < static_cast<int>(this->threads.size()); ++i) { // wait for the computing threads to finish
|
||||
if (this->threads[i]->joinable())
|
||||
this->threads[i]->join();
|
||||
}
|
||||
// if there were no threads in the pool but some functors in the queue, the functors are not deleted by the threads
|
||||
// therefore delete them here
|
||||
this->clear_queue();
|
||||
this->threads.clear();
|
||||
this->flags.clear();
|
||||
}
|
||||
|
||||
template<typename F, typename... Rest>
|
||||
auto push(F && f, Rest&&... rest) ->std::future<decltype(f(0, rest...))> {
|
||||
auto pck = std::make_shared<std::packaged_task<decltype(f(0, rest...))(int)>>(
|
||||
std::bind(std::forward<F>(f), std::placeholders::_1, std::forward<Rest>(rest)...)
|
||||
);
|
||||
auto _f = new std::function<void(int id)>([pck](int id) {
|
||||
(*pck)(id);
|
||||
});
|
||||
this->q.push(_f);
|
||||
bool push(T const& value)
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(this->mutex);
|
||||
this->cv.notify_one();
|
||||
return pck->get_future();
|
||||
this->q.push(value);
|
||||
return true;
|
||||
}
|
||||
|
||||
// run the user's function that excepts argument int - id of the running thread. returned value is templatized
|
||||
// operator returns std::future, where the user can get the result and rethrow the catched exceptins
|
||||
template<typename F>
|
||||
auto push(F && f) ->std::future<decltype(f(0))> {
|
||||
auto pck = std::make_shared<std::packaged_task<decltype(f(0))(int)>>(std::forward<F>(f));
|
||||
auto _f = new std::function<void(int id)>([pck](int id) {
|
||||
(*pck)(id);
|
||||
});
|
||||
this->q.push(_f);
|
||||
// deletes the retrieved element, do not use for non integral types
|
||||
bool pop(T& v)
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(this->mutex);
|
||||
this->cv.notify_one();
|
||||
return pck->get_future();
|
||||
if (this->q.empty())
|
||||
return false;
|
||||
v = this->q.front();
|
||||
this->q.pop();
|
||||
return true;
|
||||
}
|
||||
bool empty()
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(this->mutex);
|
||||
return this->q.empty();
|
||||
}
|
||||
|
||||
|
||||
private:
|
||||
|
||||
// deleted
|
||||
thread_pool(const thread_pool &);// = delete;
|
||||
thread_pool(thread_pool &&);// = delete;
|
||||
thread_pool & operator=(const thread_pool &);// = delete;
|
||||
thread_pool & operator=(thread_pool &&);// = delete;
|
||||
|
||||
void set_thread(int i) {
|
||||
std::shared_ptr<std::atomic<bool>> flag(this->flags[i]); // a copy of the shared ptr to the flag
|
||||
auto f = [this, i, flag/* a copy of the shared ptr to the flag */]() {
|
||||
std::atomic<bool> & _flag = *flag;
|
||||
std::function<void(int id)> * _f;
|
||||
bool isPop = this->q.pop(_f);
|
||||
while (true) {
|
||||
while (isPop) { // if there is anything in the queue
|
||||
std::unique_ptr<std::function<void(int id)>> func(_f); // at return, delete the function even if an exception occurred
|
||||
(*_f)(i);
|
||||
if (_flag)
|
||||
return; // the thread is wanted to stop, return even if the queue is not empty yet
|
||||
else
|
||||
isPop = this->q.pop(_f);
|
||||
}
|
||||
// the queue is empty here, wait for the next command
|
||||
std::unique_lock<std::mutex> lock(this->mutex);
|
||||
++this->nWaiting;
|
||||
this->cv.wait(lock, [this, &_f, &isPop, &_flag](){ isPop = this->q.pop(_f); return isPop || this->isDone || _flag; });
|
||||
--this->nWaiting;
|
||||
if (!isPop)
|
||||
return; // if the queue is empty and this->isDone == true or *flag then return
|
||||
}
|
||||
};
|
||||
|
||||
if (this->threads[i] != nullptr && this->threads[i]->joinable())
|
||||
this->threads[i]->detach();
|
||||
this->threads[i].reset(new std::thread(f)); // compiler may not support std::make_unique()
|
||||
}
|
||||
|
||||
void init() { this->nWaiting = 0; this->isStop = false; this->isDone = false; }
|
||||
|
||||
std::vector<std::unique_ptr<std::thread>> threads;
|
||||
std::vector<std::shared_ptr<std::atomic<bool>>> flags;
|
||||
detail::Queue<std::function<void(int id)> *> q;
|
||||
std::atomic<bool> isDone;
|
||||
std::atomic<bool> isStop;
|
||||
std::atomic<int> nWaiting; // how many threads are waiting
|
||||
|
||||
std::queue<T> q;
|
||||
std::mutex mutex;
|
||||
std::condition_variable cv;
|
||||
};
|
||||
}
|
||||
|
||||
class thread_pool
|
||||
{
|
||||
|
||||
public:
|
||||
thread_pool() { this->init(); }
|
||||
thread_pool(int nThreads)
|
||||
{
|
||||
this->init();
|
||||
this->resize(nThreads);
|
||||
}
|
||||
|
||||
// the destructor waits for all the functions in the queue to be finished
|
||||
~thread_pool()
|
||||
{
|
||||
this->stop(true);
|
||||
}
|
||||
|
||||
// get the number of running threads in the pool
|
||||
int size() { return static_cast<int>(this->threads.size()); }
|
||||
|
||||
// number of idle threads
|
||||
int n_idle() { return this->nWaiting; }
|
||||
std::thread& get_thread(int i) { return *this->threads[i]; }
|
||||
|
||||
// change the number of threads in the pool
|
||||
// should be called from one thread, otherwise be careful to not interleave, also with this->stop()
|
||||
// nThreads must be >= 0
|
||||
void resize(int nThreads)
|
||||
{
|
||||
if (!this->isStop && !this->isDone) {
|
||||
int oldNThreads = static_cast<int>(this->threads.size());
|
||||
if (oldNThreads <= nThreads) { // if the number of threads is increased
|
||||
this->threads.resize(nThreads);
|
||||
this->flags.resize(nThreads);
|
||||
|
||||
for (int i = oldNThreads; i < nThreads; ++i) {
|
||||
this->flags[i] = std::make_shared<std::atomic<bool>>(false);
|
||||
this->set_thread(i);
|
||||
}
|
||||
}
|
||||
else { // the number of threads is decreased
|
||||
for (int i = oldNThreads - 1; i >= nThreads; --i) {
|
||||
*this->flags[i] = true; // this thread will finish
|
||||
this->threads[i]->detach();
|
||||
}
|
||||
{
|
||||
// stop the detached threads that were waiting
|
||||
std::unique_lock<std::mutex> lock(this->mutex);
|
||||
this->cv.notify_all();
|
||||
}
|
||||
this->threads.resize(nThreads); // safe to delete because the threads are detached
|
||||
this->flags.resize(nThreads); // safe to delete because the threads have copies of shared_ptr of the flags, not originals
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// empty the queue
|
||||
void clear_queue()
|
||||
{
|
||||
std::function<void(int id)>* _f;
|
||||
while (this->q.pop(_f))
|
||||
delete _f; // empty the queue
|
||||
}
|
||||
|
||||
// pops a functional wrapper to the original function
|
||||
std::function<void(int)> pop()
|
||||
{
|
||||
std::function<void(int id)>* _f = nullptr;
|
||||
this->q.pop(_f);
|
||||
std::unique_ptr<std::function<void(int id)>> func(_f); // at return, delete the function even if an exception occurred
|
||||
std::function<void(int)> f;
|
||||
if (_f)
|
||||
f = *_f;
|
||||
return f;
|
||||
}
|
||||
|
||||
// wait for all computing threads to finish and stop all threads
|
||||
// may be called asynchronously to not pause the calling thread while waiting
|
||||
// if isWait == true, all the functions in the queue are run, otherwise the queue is cleared without running the functions
|
||||
void stop(bool isWait = false)
|
||||
{
|
||||
if (!isWait) {
|
||||
if (this->isStop)
|
||||
return;
|
||||
this->isStop = true;
|
||||
for (int i = 0, n = this->size(); i < n; ++i) {
|
||||
*this->flags[i] = true; // command the threads to stop
|
||||
}
|
||||
this->clear_queue(); // empty the queue
|
||||
}
|
||||
else {
|
||||
if (this->isDone || this->isStop)
|
||||
return;
|
||||
this->isDone = true; // give the waiting threads a command to finish
|
||||
}
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(this->mutex);
|
||||
this->cv.notify_all(); // stop all waiting threads
|
||||
}
|
||||
for (int i = 0; i < static_cast<int>(this->threads.size()); ++i) { // wait for the computing threads to finish
|
||||
if (this->threads[i]->joinable())
|
||||
this->threads[i]->join();
|
||||
}
|
||||
// if there were no threads in the pool but some functors in the queue, the functors are not deleted by the threads
|
||||
// therefore delete them here
|
||||
this->clear_queue();
|
||||
this->threads.clear();
|
||||
this->flags.clear();
|
||||
}
|
||||
|
||||
template <typename F, typename... Rest>
|
||||
auto push(F&& f, Rest&&... rest) -> std::future<decltype(f(0, rest...))>
|
||||
{
|
||||
auto pck = std::make_shared<std::packaged_task<decltype(f(0, rest...))(int)>>(
|
||||
std::bind(std::forward<F>(f), std::placeholders::_1, std::forward<Rest>(rest)...));
|
||||
auto _f = new std::function<void(int id)>([pck](int id) {
|
||||
(*pck)(id);
|
||||
});
|
||||
this->q.push(_f);
|
||||
std::unique_lock<std::mutex> lock(this->mutex);
|
||||
this->cv.notify_one();
|
||||
return pck->get_future();
|
||||
}
|
||||
|
||||
// run the user's function that excepts argument int - id of the running thread. returned value is templatized
|
||||
// operator returns std::future, where the user can get the result and rethrow the catched exceptins
|
||||
template <typename F>
|
||||
auto push(F&& f) -> std::future<decltype(f(0))>
|
||||
{
|
||||
auto pck = std::make_shared<std::packaged_task<decltype(f(0))(int)>>(std::forward<F>(f));
|
||||
auto _f = new std::function<void(int id)>([pck](int id) {
|
||||
(*pck)(id);
|
||||
});
|
||||
this->q.push(_f);
|
||||
std::unique_lock<std::mutex> lock(this->mutex);
|
||||
this->cv.notify_one();
|
||||
return pck->get_future();
|
||||
}
|
||||
|
||||
private:
|
||||
// deleted
|
||||
thread_pool(const thread_pool&); // = delete;
|
||||
thread_pool(thread_pool&&); // = delete;
|
||||
thread_pool& operator=(const thread_pool&); // = delete;
|
||||
thread_pool& operator=(thread_pool&&); // = delete;
|
||||
|
||||
void set_thread(int i)
|
||||
{
|
||||
std::shared_ptr<std::atomic<bool>> flag(this->flags[i]); // a copy of the shared ptr to the flag
|
||||
auto f = [this, i, flag /* a copy of the shared ptr to the flag */]() {
|
||||
std::atomic<bool>& _flag = *flag;
|
||||
std::function<void(int id)>* _f;
|
||||
bool isPop = this->q.pop(_f);
|
||||
while (true) {
|
||||
while (isPop) { // if there is anything in the queue
|
||||
std::unique_ptr<std::function<void(int id)>> func(_f); // at return, delete the function even if an exception occurred
|
||||
(*_f)(i);
|
||||
if (_flag)
|
||||
return; // the thread is wanted to stop, return even if the queue is not empty yet
|
||||
else
|
||||
isPop = this->q.pop(_f);
|
||||
}
|
||||
// the queue is empty here, wait for the next command
|
||||
std::unique_lock<std::mutex> lock(this->mutex);
|
||||
++this->nWaiting;
|
||||
this->cv.wait(lock, [this, &_f, &isPop, &_flag]() { isPop = this->q.pop(_f); return isPop || this->isDone || _flag; });
|
||||
--this->nWaiting;
|
||||
if (!isPop)
|
||||
return; // if the queue is empty and this->isDone == true or *flag then return
|
||||
}
|
||||
};
|
||||
|
||||
if (this->threads[i] != nullptr && this->threads[i]->joinable())
|
||||
this->threads[i]->detach();
|
||||
this->threads[i].reset(new std::thread(f)); // compiler may not support std::make_unique()
|
||||
}
|
||||
|
||||
void init()
|
||||
{
|
||||
this->nWaiting = 0;
|
||||
this->isStop = false;
|
||||
this->isDone = false;
|
||||
}
|
||||
|
||||
std::vector<std::unique_ptr<std::thread>> threads;
|
||||
std::vector<std::shared_ptr<std::atomic<bool>>> flags;
|
||||
detail::Queue<std::function<void(int id)>*> q;
|
||||
std::atomic<bool> isDone;
|
||||
std::atomic<bool> isStop;
|
||||
std::atomic<int> nWaiting; // how many threads are waiting
|
||||
|
||||
std::mutex mutex;
|
||||
std::condition_variable cv;
|
||||
};
|
||||
}
|
||||
|
||||
#endif // __ctpl_stl_thread_pool_H__
|
||||
|
|
|
|||
Разница между файлами не показана из-за своего большого размера
Загрузить разницу
Разница между файлами не показана из-за своего большого размера
Загрузить разницу
|
|
@ -24,422 +24,486 @@
|
|||
|
||||
namespace pretty_print
|
||||
{
|
||||
namespace detail
|
||||
namespace detail
|
||||
{
|
||||
// SFINAE type trait to detect whether T::const_iterator exists.
|
||||
|
||||
struct sfinae_base
|
||||
{
|
||||
// SFINAE type trait to detect whether T::const_iterator exists.
|
||||
|
||||
struct sfinae_base
|
||||
{
|
||||
using yes = char;
|
||||
using no = yes[2];
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
struct has_const_iterator : private sfinae_base
|
||||
{
|
||||
private:
|
||||
template <typename C> static yes & test(typename C::const_iterator*);
|
||||
template <typename C> static no & test(...);
|
||||
public:
|
||||
static const bool value = sizeof(test<T>(nullptr)) == sizeof(yes);
|
||||
using type = T;
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
struct has_begin_end : private sfinae_base
|
||||
{
|
||||
private:
|
||||
template <typename C>
|
||||
static yes & f(typename std::enable_if<
|
||||
std::is_same<decltype(static_cast<typename C::const_iterator(C::*)() const>(&C::begin)),
|
||||
typename C::const_iterator(C::*)() const>::value>::type *);
|
||||
|
||||
template <typename C> static no & f(...);
|
||||
|
||||
template <typename C>
|
||||
static yes & g(typename std::enable_if<
|
||||
std::is_same<decltype(static_cast<typename C::const_iterator(C::*)() const>(&C::end)),
|
||||
typename C::const_iterator(C::*)() const>::value, void>::type*);
|
||||
|
||||
template <typename C> static no & g(...);
|
||||
|
||||
public:
|
||||
static bool const beg_value = sizeof(f<T>(nullptr)) == sizeof(yes);
|
||||
static bool const end_value = sizeof(g<T>(nullptr)) == sizeof(yes);
|
||||
};
|
||||
|
||||
} // namespace detail
|
||||
|
||||
|
||||
// Holds the delimiter values for a specific character type
|
||||
|
||||
template <typename TChar>
|
||||
struct delimiters_values
|
||||
{
|
||||
using char_type = TChar;
|
||||
const char_type * prefix;
|
||||
const char_type * delimiter;
|
||||
const char_type * postfix;
|
||||
using yes = char;
|
||||
using no = yes[2];
|
||||
};
|
||||
|
||||
|
||||
// Defines the delimiter values for a specific container and character type
|
||||
|
||||
template <typename T, typename TChar>
|
||||
struct delimiters
|
||||
template <typename T>
|
||||
struct has_const_iterator : private sfinae_base
|
||||
{
|
||||
using type = delimiters_values<TChar>;
|
||||
static const type values;
|
||||
private:
|
||||
template <typename C>
|
||||
static yes& test(typename C::const_iterator*);
|
||||
template <typename C>
|
||||
static no& test(...);
|
||||
|
||||
public:
|
||||
static const bool value = sizeof(test<T>(nullptr)) == sizeof(yes);
|
||||
using type = T;
|
||||
};
|
||||
|
||||
|
||||
// Functor to print containers. You can use this directly if you want
|
||||
// to specificy a non-default delimiters type. The printing logic can
|
||||
// be customized by specializing the nested template.
|
||||
|
||||
template <typename T,
|
||||
typename TChar = char,
|
||||
typename TCharTraits = ::std::char_traits<TChar>,
|
||||
typename TDelimiters = delimiters<T, TChar>>
|
||||
struct print_container_helper
|
||||
template <typename T>
|
||||
struct has_begin_end : private sfinae_base
|
||||
{
|
||||
using delimiters_type = TDelimiters;
|
||||
using ostream_type = std::basic_ostream<TChar, TCharTraits>;
|
||||
private:
|
||||
template <typename C>
|
||||
static yes& f(typename std::enable_if<
|
||||
std::is_same<decltype(static_cast<typename C::const_iterator (C::*)() const>(&C::begin)),
|
||||
typename C::const_iterator (C::*)() const>::value>::type*);
|
||||
|
||||
template <typename U>
|
||||
struct printer
|
||||
template <typename C>
|
||||
static no& f(...);
|
||||
|
||||
template <typename C>
|
||||
static yes& g(typename std::enable_if<
|
||||
std::is_same<decltype(static_cast<typename C::const_iterator (C::*)() const>(&C::end)),
|
||||
typename C::const_iterator (C::*)() const>::value,
|
||||
void>::type*);
|
||||
|
||||
template <typename C>
|
||||
static no& g(...);
|
||||
|
||||
public:
|
||||
static bool const beg_value = sizeof(f<T>(nullptr)) == sizeof(yes);
|
||||
static bool const end_value = sizeof(g<T>(nullptr)) == sizeof(yes);
|
||||
};
|
||||
|
||||
} // namespace detail
|
||||
|
||||
// Holds the delimiter values for a specific character type
|
||||
|
||||
template <typename TChar>
|
||||
struct delimiters_values
|
||||
{
|
||||
using char_type = TChar;
|
||||
const char_type* prefix;
|
||||
const char_type* delimiter;
|
||||
const char_type* postfix;
|
||||
};
|
||||
|
||||
// Defines the delimiter values for a specific container and character type
|
||||
|
||||
template <typename T, typename TChar>
|
||||
struct delimiters
|
||||
{
|
||||
using type = delimiters_values<TChar>;
|
||||
static const type values;
|
||||
};
|
||||
|
||||
// Functor to print containers. You can use this directly if you want
|
||||
// to specificy a non-default delimiters type. The printing logic can
|
||||
// be customized by specializing the nested template.
|
||||
|
||||
template <typename T,
|
||||
typename TChar = char,
|
||||
typename TCharTraits = ::std::char_traits<TChar>,
|
||||
typename TDelimiters = delimiters<T, TChar>>
|
||||
struct print_container_helper
|
||||
{
|
||||
using delimiters_type = TDelimiters;
|
||||
using ostream_type = std::basic_ostream<TChar, TCharTraits>;
|
||||
|
||||
template <typename U>
|
||||
struct printer
|
||||
{
|
||||
static void print_body(const U& c, ostream_type& stream)
|
||||
{
|
||||
static void print_body(const U & c, ostream_type & stream)
|
||||
{
|
||||
using std::begin;
|
||||
using std::end;
|
||||
using std::begin;
|
||||
using std::end;
|
||||
|
||||
auto it = begin(c);
|
||||
const auto the_end = end(c);
|
||||
auto it = begin(c);
|
||||
const auto the_end = end(c);
|
||||
|
||||
if (it != the_end)
|
||||
{
|
||||
for ( ; ; )
|
||||
{
|
||||
stream << *it;
|
||||
if (it != the_end) {
|
||||
for (;;) {
|
||||
stream << *it;
|
||||
|
||||
if (++it == the_end) break;
|
||||
|
||||
if (delimiters_type::values.delimiter != NULL)
|
||||
stream << delimiters_type::values.delimiter;
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
}
|
||||
};
|
||||
|
||||
print_container_helper(const T & container)
|
||||
print_container_helper(const T& container)
|
||||
: container_(container)
|
||||
{ }
|
||||
|
||||
inline void operator()(ostream_type & stream) const
|
||||
{
|
||||
if (delimiters_type::values.prefix != NULL)
|
||||
stream << delimiters_type::values.prefix;
|
||||
|
||||
printer<T>::print_body(container_, stream);
|
||||
|
||||
if (delimiters_type::values.postfix != NULL)
|
||||
stream << delimiters_type::values.postfix;
|
||||
}
|
||||
|
||||
private:
|
||||
const T & container_;
|
||||
};
|
||||
|
||||
// Specialization for pairs
|
||||
|
||||
template <typename T, typename TChar, typename TCharTraits, typename TDelimiters>
|
||||
template <typename T1, typename T2>
|
||||
struct print_container_helper<T, TChar, TCharTraits, TDelimiters>::printer<std::pair<T1, T2>>
|
||||
{
|
||||
using ostream_type = typename print_container_helper<T, TChar, TCharTraits, TDelimiters>::ostream_type;
|
||||
|
||||
static void print_body(const std::pair<T1, T2> & c, ostream_type & stream)
|
||||
{
|
||||
stream << c.first;
|
||||
if (print_container_helper<T, TChar, TCharTraits, TDelimiters>::delimiters_type::values.delimiter != NULL)
|
||||
stream << print_container_helper<T, TChar, TCharTraits, TDelimiters>::delimiters_type::values.delimiter;
|
||||
stream << c.second;
|
||||
}
|
||||
};
|
||||
|
||||
// Specialization for tuples
|
||||
|
||||
template <typename T, typename TChar, typename TCharTraits, typename TDelimiters>
|
||||
template <typename ...Args>
|
||||
struct print_container_helper<T, TChar, TCharTraits, TDelimiters>::printer<std::tuple<Args...>>
|
||||
{
|
||||
using ostream_type = typename print_container_helper<T, TChar, TCharTraits, TDelimiters>::ostream_type;
|
||||
using element_type = std::tuple<Args...>;
|
||||
|
||||
template <std::size_t I> struct Int { };
|
||||
|
||||
static void print_body(const element_type & c, ostream_type & stream)
|
||||
{
|
||||
tuple_print(c, stream, Int<0>());
|
||||
}
|
||||
|
||||
static void tuple_print(const element_type &, ostream_type &, Int<sizeof...(Args)>)
|
||||
{
|
||||
}
|
||||
|
||||
static void tuple_print(const element_type & c, ostream_type & stream,
|
||||
typename std::conditional<sizeof...(Args) != 0, Int<0>, std::nullptr_t>::type)
|
||||
{
|
||||
stream << std::get<0>(c);
|
||||
tuple_print(c, stream, Int<1>());
|
||||
}
|
||||
|
||||
template <std::size_t N>
|
||||
static void tuple_print(const element_type & c, ostream_type & stream, Int<N>)
|
||||
{
|
||||
if (print_container_helper<T, TChar, TCharTraits, TDelimiters>::delimiters_type::values.delimiter != NULL)
|
||||
stream << print_container_helper<T, TChar, TCharTraits, TDelimiters>::delimiters_type::values.delimiter;
|
||||
|
||||
stream << std::get<N>(c);
|
||||
|
||||
tuple_print(c, stream, Int<N + 1>());
|
||||
}
|
||||
};
|
||||
|
||||
// Prints a print_container_helper to the specified stream.
|
||||
|
||||
template<typename T, typename TChar, typename TCharTraits, typename TDelimiters>
|
||||
inline std::basic_ostream<TChar, TCharTraits> & operator<<(
|
||||
std::basic_ostream<TChar, TCharTraits> & stream,
|
||||
const print_container_helper<T, TChar, TCharTraits, TDelimiters> & helper)
|
||||
{
|
||||
helper(stream);
|
||||
return stream;
|
||||
}
|
||||
|
||||
inline void operator()(ostream_type& stream) const
|
||||
{
|
||||
if (delimiters_type::values.prefix != NULL)
|
||||
stream << delimiters_type::values.prefix;
|
||||
|
||||
// Basic is_container template; specialize to derive from std::true_type for all desired container types
|
||||
printer<T>::print_body(container_, stream);
|
||||
|
||||
template <typename T>
|
||||
struct is_container : public std::integral_constant<bool,
|
||||
detail::has_const_iterator<T>::value &&
|
||||
detail::has_begin_end<T>::beg_value &&
|
||||
detail::has_begin_end<T>::end_value> { };
|
||||
if (delimiters_type::values.postfix != NULL)
|
||||
stream << delimiters_type::values.postfix;
|
||||
}
|
||||
|
||||
template <typename T, std::size_t N>
|
||||
struct is_container<T[N]> : std::true_type { };
|
||||
private:
|
||||
const T& container_;
|
||||
};
|
||||
|
||||
// Specialization for pairs
|
||||
|
||||
template <typename T, typename TChar, typename TCharTraits, typename TDelimiters>
|
||||
template <typename T1, typename T2>
|
||||
struct print_container_helper<T, TChar, TCharTraits, TDelimiters>::printer<std::pair<T1, T2>>
|
||||
{
|
||||
using ostream_type = typename print_container_helper<T, TChar, TCharTraits, TDelimiters>::ostream_type;
|
||||
|
||||
static void print_body(const std::pair<T1, T2>& c, ostream_type& stream)
|
||||
{
|
||||
stream << c.first;
|
||||
if (print_container_helper<T, TChar, TCharTraits, TDelimiters>::delimiters_type::values.delimiter != NULL)
|
||||
stream << print_container_helper<T, TChar, TCharTraits, TDelimiters>::delimiters_type::values.delimiter;
|
||||
stream << c.second;
|
||||
}
|
||||
};
|
||||
|
||||
// Specialization for tuples
|
||||
|
||||
template <typename T, typename TChar, typename TCharTraits, typename TDelimiters>
|
||||
template <typename... Args>
|
||||
struct print_container_helper<T, TChar, TCharTraits, TDelimiters>::printer<std::tuple<Args...>>
|
||||
{
|
||||
using ostream_type = typename print_container_helper<T, TChar, TCharTraits, TDelimiters>::ostream_type;
|
||||
using element_type = std::tuple<Args...>;
|
||||
|
||||
template <std::size_t I>
|
||||
struct Int
|
||||
{
|
||||
};
|
||||
|
||||
static void print_body(const element_type& c, ostream_type& stream)
|
||||
{
|
||||
tuple_print(c, stream, Int<0>());
|
||||
}
|
||||
|
||||
static void tuple_print(const element_type&, ostream_type&, Int<sizeof...(Args)>)
|
||||
{
|
||||
}
|
||||
|
||||
static void tuple_print(const element_type& c, ostream_type& stream,
|
||||
typename std::conditional<sizeof...(Args) != 0, Int<0>, std::nullptr_t>::type)
|
||||
{
|
||||
stream << std::get<0>(c);
|
||||
tuple_print(c, stream, Int<1>());
|
||||
}
|
||||
|
||||
template <std::size_t N>
|
||||
struct is_container<char[N]> : std::false_type { };
|
||||
|
||||
template <typename T>
|
||||
struct is_container<std::valarray<T>> : std::true_type { };
|
||||
|
||||
template <typename T1, typename T2>
|
||||
struct is_container<std::pair<T1, T2>> : std::true_type { };
|
||||
|
||||
template <typename ...Args>
|
||||
struct is_container<std::tuple<Args...>> : std::true_type { };
|
||||
|
||||
|
||||
// Default delimiters
|
||||
|
||||
template <typename T> struct delimiters<T, char> { static const delimiters_values<char> values; };
|
||||
template <typename T> const delimiters_values<char> delimiters<T, char>::values = { "[", ", ", "]" };
|
||||
template <typename T> struct delimiters<T, wchar_t> { static const delimiters_values<wchar_t> values; };
|
||||
template <typename T> const delimiters_values<wchar_t> delimiters<T, wchar_t>::values = { L"[", L", ", L"]" };
|
||||
|
||||
|
||||
// Delimiters for (multi)set and unordered_(multi)set
|
||||
|
||||
template <typename T, typename TComp, typename TAllocator>
|
||||
struct delimiters< ::std::set<T, TComp, TAllocator>, char> { static const delimiters_values<char> values; };
|
||||
|
||||
template <typename T, typename TComp, typename TAllocator>
|
||||
const delimiters_values<char> delimiters< ::std::set<T, TComp, TAllocator>, char>::values = { "{", ", ", "}" };
|
||||
|
||||
template <typename T, typename TComp, typename TAllocator>
|
||||
struct delimiters< ::std::set<T, TComp, TAllocator>, wchar_t> { static const delimiters_values<wchar_t> values; };
|
||||
|
||||
template <typename T, typename TComp, typename TAllocator>
|
||||
const delimiters_values<wchar_t> delimiters< ::std::set<T, TComp, TAllocator>, wchar_t>::values = { L"{", L", ", L"}" };
|
||||
|
||||
template <typename T, typename TComp, typename TAllocator>
|
||||
struct delimiters< ::std::multiset<T, TComp, TAllocator>, char> { static const delimiters_values<char> values; };
|
||||
|
||||
template <typename T, typename TComp, typename TAllocator>
|
||||
const delimiters_values<char> delimiters< ::std::multiset<T, TComp, TAllocator>, char>::values = { "{", ", ", "}" };
|
||||
|
||||
template <typename T, typename TComp, typename TAllocator>
|
||||
struct delimiters< ::std::multiset<T, TComp, TAllocator>, wchar_t> { static const delimiters_values<wchar_t> values; };
|
||||
|
||||
template <typename T, typename TComp, typename TAllocator>
|
||||
const delimiters_values<wchar_t> delimiters< ::std::multiset<T, TComp, TAllocator>, wchar_t>::values = { L"{", L", ", L"}" };
|
||||
|
||||
template <typename T, typename THash, typename TEqual, typename TAllocator>
|
||||
struct delimiters< ::std::unordered_set<T, THash, TEqual, TAllocator>, char> { static const delimiters_values<char> values; };
|
||||
|
||||
template <typename T, typename THash, typename TEqual, typename TAllocator>
|
||||
const delimiters_values<char> delimiters< ::std::unordered_set<T, THash, TEqual, TAllocator>, char>::values = { "{", ", ", "}" };
|
||||
|
||||
template <typename T, typename THash, typename TEqual, typename TAllocator>
|
||||
struct delimiters< ::std::unordered_set<T, THash, TEqual, TAllocator>, wchar_t> { static const delimiters_values<wchar_t> values; };
|
||||
|
||||
template <typename T, typename THash, typename TEqual, typename TAllocator>
|
||||
const delimiters_values<wchar_t> delimiters< ::std::unordered_set<T, THash, TEqual, TAllocator>, wchar_t>::values = { L"{", L", ", L"}" };
|
||||
|
||||
template <typename T, typename THash, typename TEqual, typename TAllocator>
|
||||
struct delimiters< ::std::unordered_multiset<T, THash, TEqual, TAllocator>, char> { static const delimiters_values<char> values; };
|
||||
|
||||
template <typename T, typename THash, typename TEqual, typename TAllocator>
|
||||
const delimiters_values<char> delimiters< ::std::unordered_multiset<T, THash, TEqual, TAllocator>, char>::values = { "{", ", ", "}" };
|
||||
|
||||
template <typename T, typename THash, typename TEqual, typename TAllocator>
|
||||
struct delimiters< ::std::unordered_multiset<T, THash, TEqual, TAllocator>, wchar_t> { static const delimiters_values<wchar_t> values; };
|
||||
|
||||
template <typename T, typename THash, typename TEqual, typename TAllocator>
|
||||
const delimiters_values<wchar_t> delimiters< ::std::unordered_multiset<T, THash, TEqual, TAllocator>, wchar_t>::values = { L"{", L", ", L"}" };
|
||||
|
||||
|
||||
// Delimiters for pair and tuple
|
||||
|
||||
template <typename T1, typename T2> struct delimiters<std::pair<T1, T2>, char> { static const delimiters_values<char> values; };
|
||||
template <typename T1, typename T2> const delimiters_values<char> delimiters<std::pair<T1, T2>, char>::values = { "(", ", ", ")" };
|
||||
template <typename T1, typename T2> struct delimiters< ::std::pair<T1, T2>, wchar_t> { static const delimiters_values<wchar_t> values; };
|
||||
template <typename T1, typename T2> const delimiters_values<wchar_t> delimiters< ::std::pair<T1, T2>, wchar_t>::values = { L"(", L", ", L")" };
|
||||
|
||||
template <typename ...Args> struct delimiters<std::tuple<Args...>, char> { static const delimiters_values<char> values; };
|
||||
template <typename ...Args> const delimiters_values<char> delimiters<std::tuple<Args...>, char>::values = { "(", ", ", ")" };
|
||||
template <typename ...Args> struct delimiters< ::std::tuple<Args...>, wchar_t> { static const delimiters_values<wchar_t> values; };
|
||||
template <typename ...Args> const delimiters_values<wchar_t> delimiters< ::std::tuple<Args...>, wchar_t>::values = { L"(", L", ", L")" };
|
||||
|
||||
|
||||
// Type-erasing helper class for easy use of custom delimiters.
|
||||
// Requires TCharTraits = std::char_traits<TChar> and TChar = char or wchar_t, and MyDelims needs to be defined for TChar.
|
||||
// Usage: "cout << pretty_print::custom_delims<MyDelims>(x)".
|
||||
|
||||
struct custom_delims_base
|
||||
static void tuple_print(const element_type& c, ostream_type& stream, Int<N>)
|
||||
{
|
||||
virtual ~custom_delims_base() { }
|
||||
virtual std::ostream & stream(::std::ostream &) = 0;
|
||||
virtual std::wostream & stream(::std::wostream &) = 0;
|
||||
};
|
||||
if (print_container_helper<T, TChar, TCharTraits, TDelimiters>::delimiters_type::values.delimiter != NULL)
|
||||
stream << print_container_helper<T, TChar, TCharTraits, TDelimiters>::delimiters_type::values.delimiter;
|
||||
|
||||
template <typename T, typename Delims>
|
||||
struct custom_delims_wrapper : custom_delims_base
|
||||
stream << std::get<N>(c);
|
||||
|
||||
tuple_print(c, stream, Int<N + 1>());
|
||||
}
|
||||
};
|
||||
|
||||
// Prints a print_container_helper to the specified stream.
|
||||
|
||||
template <typename T, typename TChar, typename TCharTraits, typename TDelimiters>
|
||||
inline std::basic_ostream<TChar, TCharTraits>& operator<<(
|
||||
std::basic_ostream<TChar, TCharTraits>& stream,
|
||||
const print_container_helper<T, TChar, TCharTraits, TDelimiters>& helper)
|
||||
{
|
||||
helper(stream);
|
||||
return stream;
|
||||
}
|
||||
|
||||
// Basic is_container template; specialize to derive from std::true_type for all desired container types
|
||||
|
||||
template <typename T>
|
||||
struct is_container : public std::integral_constant<bool, detail::has_const_iterator<T>::value && detail::has_begin_end<T>::beg_value && detail::has_begin_end<T>::end_value>
|
||||
{
|
||||
};
|
||||
|
||||
template <typename T, std::size_t N>
|
||||
struct is_container<T[N]> : std::true_type
|
||||
{
|
||||
};
|
||||
|
||||
template <std::size_t N>
|
||||
struct is_container<char[N]> : std::false_type
|
||||
{
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
struct is_container<std::valarray<T>> : std::true_type
|
||||
{
|
||||
};
|
||||
|
||||
template <typename T1, typename T2>
|
||||
struct is_container<std::pair<T1, T2>> : std::true_type
|
||||
{
|
||||
};
|
||||
|
||||
template <typename... Args>
|
||||
struct is_container<std::tuple<Args...>> : std::true_type
|
||||
{
|
||||
};
|
||||
|
||||
// Default delimiters
|
||||
|
||||
template <typename T>
|
||||
struct delimiters<T, char>
|
||||
{
|
||||
static const delimiters_values<char> values;
|
||||
};
|
||||
template <typename T>
|
||||
const delimiters_values<char> delimiters<T, char>::values = { "[", ", ", "]" };
|
||||
template <typename T>
|
||||
struct delimiters<T, wchar_t>
|
||||
{
|
||||
static const delimiters_values<wchar_t> values;
|
||||
};
|
||||
template <typename T>
|
||||
const delimiters_values<wchar_t> delimiters<T, wchar_t>::values = { L"[", L", ", L"]" };
|
||||
|
||||
// Delimiters for (multi)set and unordered_(multi)set
|
||||
|
||||
template <typename T, typename TComp, typename TAllocator>
|
||||
struct delimiters<::std::set<T, TComp, TAllocator>, char>
|
||||
{
|
||||
static const delimiters_values<char> values;
|
||||
};
|
||||
|
||||
template <typename T, typename TComp, typename TAllocator>
|
||||
const delimiters_values<char> delimiters<::std::set<T, TComp, TAllocator>, char>::values = { "{", ", ", "}" };
|
||||
|
||||
template <typename T, typename TComp, typename TAllocator>
|
||||
struct delimiters<::std::set<T, TComp, TAllocator>, wchar_t>
|
||||
{
|
||||
static const delimiters_values<wchar_t> values;
|
||||
};
|
||||
|
||||
template <typename T, typename TComp, typename TAllocator>
|
||||
const delimiters_values<wchar_t> delimiters<::std::set<T, TComp, TAllocator>, wchar_t>::values = { L"{", L", ", L"}" };
|
||||
|
||||
template <typename T, typename TComp, typename TAllocator>
|
||||
struct delimiters<::std::multiset<T, TComp, TAllocator>, char>
|
||||
{
|
||||
static const delimiters_values<char> values;
|
||||
};
|
||||
|
||||
template <typename T, typename TComp, typename TAllocator>
|
||||
const delimiters_values<char> delimiters<::std::multiset<T, TComp, TAllocator>, char>::values = { "{", ", ", "}" };
|
||||
|
||||
template <typename T, typename TComp, typename TAllocator>
|
||||
struct delimiters<::std::multiset<T, TComp, TAllocator>, wchar_t>
|
||||
{
|
||||
static const delimiters_values<wchar_t> values;
|
||||
};
|
||||
|
||||
template <typename T, typename TComp, typename TAllocator>
|
||||
const delimiters_values<wchar_t> delimiters<::std::multiset<T, TComp, TAllocator>, wchar_t>::values = { L"{", L", ", L"}" };
|
||||
|
||||
template <typename T, typename THash, typename TEqual, typename TAllocator>
|
||||
struct delimiters<::std::unordered_set<T, THash, TEqual, TAllocator>, char>
|
||||
{
|
||||
static const delimiters_values<char> values;
|
||||
};
|
||||
|
||||
template <typename T, typename THash, typename TEqual, typename TAllocator>
|
||||
const delimiters_values<char> delimiters<::std::unordered_set<T, THash, TEqual, TAllocator>, char>::values = { "{", ", ", "}" };
|
||||
|
||||
template <typename T, typename THash, typename TEqual, typename TAllocator>
|
||||
struct delimiters<::std::unordered_set<T, THash, TEqual, TAllocator>, wchar_t>
|
||||
{
|
||||
static const delimiters_values<wchar_t> values;
|
||||
};
|
||||
|
||||
template <typename T, typename THash, typename TEqual, typename TAllocator>
|
||||
const delimiters_values<wchar_t> delimiters<::std::unordered_set<T, THash, TEqual, TAllocator>, wchar_t>::values = { L"{", L", ", L"}" };
|
||||
|
||||
template <typename T, typename THash, typename TEqual, typename TAllocator>
|
||||
struct delimiters<::std::unordered_multiset<T, THash, TEqual, TAllocator>, char>
|
||||
{
|
||||
static const delimiters_values<char> values;
|
||||
};
|
||||
|
||||
template <typename T, typename THash, typename TEqual, typename TAllocator>
|
||||
const delimiters_values<char> delimiters<::std::unordered_multiset<T, THash, TEqual, TAllocator>, char>::values = { "{", ", ", "}" };
|
||||
|
||||
template <typename T, typename THash, typename TEqual, typename TAllocator>
|
||||
struct delimiters<::std::unordered_multiset<T, THash, TEqual, TAllocator>, wchar_t>
|
||||
{
|
||||
static const delimiters_values<wchar_t> values;
|
||||
};
|
||||
|
||||
template <typename T, typename THash, typename TEqual, typename TAllocator>
|
||||
const delimiters_values<wchar_t> delimiters<::std::unordered_multiset<T, THash, TEqual, TAllocator>, wchar_t>::values = { L"{", L", ", L"}" };
|
||||
|
||||
// Delimiters for pair and tuple
|
||||
|
||||
template <typename T1, typename T2>
|
||||
struct delimiters<std::pair<T1, T2>, char>
|
||||
{
|
||||
static const delimiters_values<char> values;
|
||||
};
|
||||
template <typename T1, typename T2>
|
||||
const delimiters_values<char> delimiters<std::pair<T1, T2>, char>::values = { "(", ", ", ")" };
|
||||
template <typename T1, typename T2>
|
||||
struct delimiters<::std::pair<T1, T2>, wchar_t>
|
||||
{
|
||||
static const delimiters_values<wchar_t> values;
|
||||
};
|
||||
template <typename T1, typename T2>
|
||||
const delimiters_values<wchar_t> delimiters<::std::pair<T1, T2>, wchar_t>::values = { L"(", L", ", L")" };
|
||||
|
||||
template <typename... Args>
|
||||
struct delimiters<std::tuple<Args...>, char>
|
||||
{
|
||||
static const delimiters_values<char> values;
|
||||
};
|
||||
template <typename... Args>
|
||||
const delimiters_values<char> delimiters<std::tuple<Args...>, char>::values = { "(", ", ", ")" };
|
||||
template <typename... Args>
|
||||
struct delimiters<::std::tuple<Args...>, wchar_t>
|
||||
{
|
||||
static const delimiters_values<wchar_t> values;
|
||||
};
|
||||
template <typename... Args>
|
||||
const delimiters_values<wchar_t> delimiters<::std::tuple<Args...>, wchar_t>::values = { L"(", L", ", L")" };
|
||||
|
||||
// Type-erasing helper class for easy use of custom delimiters.
|
||||
// Requires TCharTraits = std::char_traits<TChar> and TChar = char or wchar_t, and MyDelims needs to be defined for TChar.
|
||||
// Usage: "cout << pretty_print::custom_delims<MyDelims>(x)".
|
||||
|
||||
struct custom_delims_base
|
||||
{
|
||||
virtual ~custom_delims_base() {}
|
||||
virtual std::ostream& stream(::std::ostream&) = 0;
|
||||
virtual std::wostream& stream(::std::wostream&) = 0;
|
||||
};
|
||||
|
||||
template <typename T, typename Delims>
|
||||
struct custom_delims_wrapper : custom_delims_base
|
||||
{
|
||||
custom_delims_wrapper(const T& t_)
|
||||
: t(t_) {}
|
||||
|
||||
std::ostream& stream(std::ostream& s)
|
||||
{
|
||||
custom_delims_wrapper(const T & t_) : t(t_) { }
|
||||
|
||||
std::ostream & stream(std::ostream & s)
|
||||
{
|
||||
return s << print_container_helper<T, char, std::char_traits<char>, Delims>(t);
|
||||
}
|
||||
|
||||
std::wostream & stream(std::wostream & s)
|
||||
{
|
||||
return s << print_container_helper<T, wchar_t, std::char_traits<wchar_t>, Delims>(t);
|
||||
}
|
||||
|
||||
private:
|
||||
const T & t;
|
||||
};
|
||||
|
||||
template <typename Delims>
|
||||
struct custom_delims
|
||||
{
|
||||
template <typename Container>
|
||||
custom_delims(const Container & c) : base(new custom_delims_wrapper<Container, Delims>(c)) { }
|
||||
|
||||
std::unique_ptr<custom_delims_base> base;
|
||||
};
|
||||
|
||||
template <typename TChar, typename TCharTraits, typename Delims>
|
||||
inline std::basic_ostream<TChar, TCharTraits> & operator<<(std::basic_ostream<TChar, TCharTraits> & s, const custom_delims<Delims> & p)
|
||||
{
|
||||
return p.base->stream(s);
|
||||
return s << print_container_helper<T, char, std::char_traits<char>, Delims>(t);
|
||||
}
|
||||
|
||||
|
||||
// A wrapper for a C-style array given as pointer-plus-size.
|
||||
// Usage: std::cout << pretty_print_array(arr, n) << std::endl;
|
||||
|
||||
template<typename T>
|
||||
struct array_wrapper_n
|
||||
std::wostream& stream(std::wostream& s)
|
||||
{
|
||||
typedef const T * const_iterator;
|
||||
typedef T value_type;
|
||||
return s << print_container_helper<T, wchar_t, std::char_traits<wchar_t>, Delims>(t);
|
||||
}
|
||||
|
||||
array_wrapper_n(const T * const a, size_t n) : _array(a), _n(n) { }
|
||||
inline const_iterator begin() const { return _array; }
|
||||
inline const_iterator end() const { return _array + _n; }
|
||||
private:
|
||||
const T& t;
|
||||
};
|
||||
|
||||
private:
|
||||
const T * const _array;
|
||||
size_t _n;
|
||||
};
|
||||
|
||||
|
||||
// A wrapper for hash-table based containers that offer local iterators to each bucket.
|
||||
// Usage: std::cout << bucket_print(m, 4) << std::endl; (Prints bucket 5 of container m.)
|
||||
|
||||
template <typename T>
|
||||
struct bucket_print_wrapper
|
||||
template <typename Delims>
|
||||
struct custom_delims
|
||||
{
|
||||
template <typename Container>
|
||||
custom_delims(const Container& c)
|
||||
: base(new custom_delims_wrapper<Container, Delims>(c))
|
||||
{
|
||||
typedef typename T::const_local_iterator const_iterator;
|
||||
typedef typename T::size_type size_type;
|
||||
}
|
||||
|
||||
const_iterator begin() const
|
||||
{
|
||||
return m_map.cbegin(n);
|
||||
}
|
||||
std::unique_ptr<custom_delims_base> base;
|
||||
};
|
||||
|
||||
const_iterator end() const
|
||||
{
|
||||
return m_map.cend(n);
|
||||
}
|
||||
template <typename TChar, typename TCharTraits, typename Delims>
|
||||
inline std::basic_ostream<TChar, TCharTraits>& operator<<(std::basic_ostream<TChar, TCharTraits>& s, const custom_delims<Delims>& p)
|
||||
{
|
||||
return p.base->stream(s);
|
||||
}
|
||||
|
||||
bucket_print_wrapper(const T & m, size_type bucket) : m_map(m), n(bucket) { }
|
||||
// A wrapper for a C-style array given as pointer-plus-size.
|
||||
// Usage: std::cout << pretty_print_array(arr, n) << std::endl;
|
||||
|
||||
private:
|
||||
const T & m_map;
|
||||
const size_type n;
|
||||
};
|
||||
template <typename T>
|
||||
struct array_wrapper_n
|
||||
{
|
||||
typedef const T* const_iterator;
|
||||
typedef T value_type;
|
||||
|
||||
} // namespace pretty_print
|
||||
array_wrapper_n(const T* const a, size_t n)
|
||||
: _array(a), _n(n) {}
|
||||
inline const_iterator begin() const { return _array; }
|
||||
inline const_iterator end() const { return _array + _n; }
|
||||
|
||||
private:
|
||||
const T* const _array;
|
||||
size_t _n;
|
||||
};
|
||||
|
||||
// A wrapper for hash-table based containers that offer local iterators to each bucket.
|
||||
// Usage: std::cout << bucket_print(m, 4) << std::endl; (Prints bucket 5 of container m.)
|
||||
|
||||
template <typename T>
|
||||
struct bucket_print_wrapper
|
||||
{
|
||||
typedef typename T::const_local_iterator const_iterator;
|
||||
typedef typename T::size_type size_type;
|
||||
|
||||
const_iterator begin() const
|
||||
{
|
||||
return m_map.cbegin(n);
|
||||
}
|
||||
|
||||
const_iterator end() const
|
||||
{
|
||||
return m_map.cend(n);
|
||||
}
|
||||
|
||||
bucket_print_wrapper(const T& m, size_type bucket)
|
||||
: m_map(m), n(bucket) {}
|
||||
|
||||
private:
|
||||
const T& m_map;
|
||||
const size_type n;
|
||||
};
|
||||
|
||||
} // namespace pretty_print
|
||||
|
||||
// Global accessor functions for the convenience wrappers
|
||||
|
||||
template<typename T>
|
||||
inline pretty_print::array_wrapper_n<T> pretty_print_array(const T * const a, size_t n)
|
||||
template <typename T>
|
||||
inline pretty_print::array_wrapper_n<T> pretty_print_array(const T* const a, size_t n)
|
||||
{
|
||||
return pretty_print::array_wrapper_n<T>(a, n);
|
||||
}
|
||||
|
||||
template <typename T> pretty_print::bucket_print_wrapper<T>
|
||||
bucket_print(const T & m, typename T::size_type n)
|
||||
template <typename T>
|
||||
pretty_print::bucket_print_wrapper<T>
|
||||
bucket_print(const T& m, typename T::size_type n)
|
||||
{
|
||||
return pretty_print::bucket_print_wrapper<T>(m, n);
|
||||
}
|
||||
|
||||
|
||||
// Main magic entry point: An overload snuck into namespace std.
|
||||
// Can we do better?
|
||||
|
||||
namespace std
|
||||
{
|
||||
// Prints a container to the stream using default delimiters
|
||||
// Prints a container to the stream using default delimiters
|
||||
|
||||
template<typename T, typename TChar, typename TCharTraits>
|
||||
inline typename enable_if< ::pretty_print::is_container<T>::value,
|
||||
basic_ostream<TChar, TCharTraits> &>::type
|
||||
operator<<(basic_ostream<TChar, TCharTraits> & stream, const T & container)
|
||||
{
|
||||
return stream << ::pretty_print::print_container_helper<T, TChar, TCharTraits>(container);
|
||||
}
|
||||
template <typename T, typename TChar, typename TCharTraits>
|
||||
inline typename enable_if<::pretty_print::is_container<T>::value,
|
||||
basic_ostream<TChar, TCharTraits>&>::type
|
||||
operator<<(basic_ostream<TChar, TCharTraits>& stream, const T& container)
|
||||
{
|
||||
return stream << ::pretty_print::print_container_helper<T, TChar, TCharTraits>(container);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endif // H_PRETTY_PRINT
|
||||
#endif // H_PRETTY_PRINT
|
||||
|
|
@ -4,7 +4,7 @@
|
|||
#ifndef commn_utils_sincos_hpp
|
||||
#define commn_utils_sincos_hpp
|
||||
|
||||
#include<cmath>
|
||||
#include <cmath>
|
||||
|
||||
/*
|
||||
GCC seem to define sincos already. However there is a bug currently which causes compiler to optimise
|
||||
|
|
@ -14,22 +14,23 @@ http://man7.org/linux/man-pages/man3/sincos.3.html
|
|||
https://android.googlesource.com/platform/bionic/+/master/libm/sincos.c
|
||||
*/
|
||||
|
||||
|
||||
#ifdef _MSC_VER //GNU/Linux compilers have these functions
|
||||
//inline here is forced and is hack. Ideally these definitions needs to be in cpp file. http://stackoverflow.com/a/6469881/207661
|
||||
inline void sincos(double x, double* p_sin, double* p_cos) {
|
||||
*p_sin = sin(x);
|
||||
*p_cos = cos(x);
|
||||
inline void sincos(double x, double* p_sin, double* p_cos)
|
||||
{
|
||||
*p_sin = sin(x);
|
||||
*p_cos = cos(x);
|
||||
}
|
||||
inline void sincosf(float x, float* p_sinf, float* p_cosf) {
|
||||
*p_sinf = sinf(x);
|
||||
*p_cosf = cosf(x);
|
||||
inline void sincosf(float x, float* p_sinf, float* p_cosf)
|
||||
{
|
||||
*p_sinf = sinf(x);
|
||||
*p_cosf = cosf(x);
|
||||
}
|
||||
inline void sincosl(long double x, long double* p_sinl, long double* p_cosl) {
|
||||
*p_sinl = sinl(x);
|
||||
*p_cosl = cosl(x);
|
||||
inline void sincosl(long double x, long double* p_sinl, long double* p_cosl)
|
||||
{
|
||||
*p_sinl = sinl(x);
|
||||
*p_cosl = cosl(x);
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -7,9 +7,12 @@
|
|||
#include <type_traits>
|
||||
#include <valarray>
|
||||
|
||||
namespace common_utils { namespace type_utils {
|
||||
//from: https://raw.githubusercontent.com/louisdx/cxx-prettyprint/master/prettyprint.hpp
|
||||
//also see https://gist.github.com/louisdx/1076849
|
||||
namespace common_utils
|
||||
{
|
||||
namespace type_utils
|
||||
{
|
||||
//from: https://raw.githubusercontent.com/louisdx/cxx-prettyprint/master/prettyprint.hpp
|
||||
//also see https://gist.github.com/louisdx/1076849
|
||||
namespace detail
|
||||
{
|
||||
// SFINAE type trait to detect whether T::const_iterator exists.
|
||||
|
|
@ -17,18 +20,21 @@ namespace common_utils { namespace type_utils {
|
|||
struct sfinae_base
|
||||
{
|
||||
using yes = char;
|
||||
using no = yes[2];
|
||||
using no = yes[2];
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
struct has_const_iterator : private sfinae_base
|
||||
{
|
||||
private:
|
||||
template <typename C> static yes & test(typename C::const_iterator*);
|
||||
template <typename C> static no & test(...);
|
||||
template <typename C>
|
||||
static yes& test(typename C::const_iterator*);
|
||||
template <typename C>
|
||||
static no& test(...);
|
||||
|
||||
public:
|
||||
static const bool value = sizeof(test<T>(nullptr)) == sizeof(yes);
|
||||
using type = T;
|
||||
using type = T;
|
||||
|
||||
void dummy(); //for GCC to suppress -Wctor-dtor-privacy
|
||||
};
|
||||
|
|
@ -38,20 +44,23 @@ namespace common_utils { namespace type_utils {
|
|||
{
|
||||
private:
|
||||
template <typename C>
|
||||
static yes & f(typename std::enable_if<
|
||||
std::is_same<decltype(static_cast<typename C::const_iterator(C::*)() const>(&C::begin)),
|
||||
typename C::const_iterator(C::*)() const>::value>::type *);
|
||||
static yes& f(typename std::enable_if<
|
||||
std::is_same<decltype(static_cast<typename C::const_iterator (C::*)() const>(&C::begin)),
|
||||
typename C::const_iterator (C::*)() const>::value>::type*);
|
||||
|
||||
template <typename C> static no & f(...);
|
||||
template <typename C>
|
||||
static no& f(...);
|
||||
|
||||
// using template parameter symbol 'D' instead of 'C' as a workaround for
|
||||
// VS2017 compiler issue (internal compiler error) starting 15.9.X releases.
|
||||
template <typename D>
|
||||
static yes & g(typename std::enable_if<
|
||||
std::is_same<decltype(static_cast<typename D::const_iterator(D::*)() const>(&D::end)),
|
||||
typename D::const_iterator(D::*)() const>::value, void>::type*);
|
||||
static yes& g(typename std::enable_if<
|
||||
std::is_same<decltype(static_cast<typename D::const_iterator (D::*)() const>(&D::end)),
|
||||
typename D::const_iterator (D::*)() const>::value,
|
||||
void>::type*);
|
||||
|
||||
template <typename D> static no & g(...);
|
||||
template <typename D>
|
||||
static no& g(...);
|
||||
|
||||
public:
|
||||
static bool const beg_value = sizeof(f<T>(nullptr)) == sizeof(yes);
|
||||
|
|
@ -60,30 +69,39 @@ namespace common_utils { namespace type_utils {
|
|||
void dummy(); //for GCC to suppress -Wctor-dtor-privacy
|
||||
};
|
||||
|
||||
} // namespace detail
|
||||
} // namespace detail
|
||||
|
||||
// Basic is_container template; specialize to derive from std::true_type for all desired container types
|
||||
|
||||
template <typename T>
|
||||
struct is_container : public std::integral_constant<bool,
|
||||
detail::has_const_iterator<T>::value &&
|
||||
detail::has_begin_end<T>::beg_value &&
|
||||
detail::has_begin_end<T>::end_value> { };
|
||||
struct is_container : public std::integral_constant<bool, detail::has_const_iterator<T>::value && detail::has_begin_end<T>::beg_value && detail::has_begin_end<T>::end_value>
|
||||
{
|
||||
};
|
||||
|
||||
template <typename T, std::size_t N>
|
||||
struct is_container<T[N]> : std::true_type { };
|
||||
struct is_container<T[N]> : std::true_type
|
||||
{
|
||||
};
|
||||
|
||||
template <std::size_t N>
|
||||
struct is_container<char[N]> : std::false_type { };
|
||||
struct is_container<char[N]> : std::false_type
|
||||
{
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
struct is_container<std::valarray<T>> : std::true_type { };
|
||||
struct is_container<std::valarray<T>> : std::true_type
|
||||
{
|
||||
};
|
||||
|
||||
template <typename T1, typename T2>
|
||||
struct is_container<std::pair<T1, T2>> : std::true_type { };
|
||||
struct is_container<std::pair<T1, T2>> : std::true_type
|
||||
{
|
||||
};
|
||||
|
||||
template <typename ...Args>
|
||||
struct is_container<std::tuple<Args...>> : std::true_type { };
|
||||
|
||||
}} //namespace
|
||||
template <typename... Args>
|
||||
struct is_container<std::tuple<Args...>> : std::true_type
|
||||
{
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -8,154 +8,158 @@
|
|||
#include <exception>
|
||||
#include <iostream>
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
class DebugPhysicsBody : public PhysicsBody
|
||||
{
|
||||
private:
|
||||
class WrenchVertex : public PhysicsBodyVertex
|
||||
{
|
||||
public:
|
||||
WrenchVertex(const Vector3r& position, const Vector3r& normal, const Vector3r& force = Vector3r::Zero())
|
||||
: PhysicsBodyVertex(position, normal), force_(force)
|
||||
{
|
||||
}
|
||||
|
||||
protected:
|
||||
virtual void setWrench(Wrench& wrench) override
|
||||
{
|
||||
wrench.force = force_;
|
||||
}
|
||||
|
||||
private:
|
||||
Vector3r force_;
|
||||
};
|
||||
|
||||
class DebugPhysicsBody : public PhysicsBody {
|
||||
private:
|
||||
class WrenchVertex : public PhysicsBodyVertex {
|
||||
public:
|
||||
WrenchVertex(const Vector3r& position, const Vector3r& normal, const Vector3r& force = Vector3r::Zero())
|
||||
: PhysicsBodyVertex(position, normal), force_(force)
|
||||
void initialize(Kinematics* kinematics, Environment* environment)
|
||||
{
|
||||
computeInertiaMatrix(inertia_, mass_, body_box_);
|
||||
createWrenchVertices(wrench_vertices_, body_box_.x(), body_box_.y(), body_box_.z(), mass_);
|
||||
createDragVertices(drag_vertices_, 1.3f, body_box_.x(), body_box_.y(), body_box_.z());
|
||||
|
||||
PhysicsBody::initialize(mass_, inertia_, kinematics, environment);
|
||||
}
|
||||
protected:
|
||||
virtual void setWrench(Wrench& wrench) override
|
||||
|
||||
virtual void updateKinematics(const Kinematics::State& kinematics) override
|
||||
{
|
||||
wrench.force = force_;
|
||||
PhysicsBody::updateKinematics(kinematics);
|
||||
|
||||
std::cout << " Pos: " << VectorMath::toString(kinematics.pose.position);
|
||||
std::cout << " Ori: " << VectorMath::toString(kinematics.pose.orientation) << std::endl;
|
||||
std::cout << " Lin Vel: " << VectorMath::toString(kinematics.twist.linear);
|
||||
std::cout << " Ang Vel: " << VectorMath::toString(kinematics.twist.angular) << std::endl;
|
||||
std::cout << " ------------------------------------------------" << std::endl;
|
||||
}
|
||||
|
||||
virtual void updateKinematics() override
|
||||
{
|
||||
PhysicsBody::updateKinematics();
|
||||
}
|
||||
|
||||
virtual real_T getRestitution() const override
|
||||
{
|
||||
return restitution_;
|
||||
}
|
||||
|
||||
virtual real_T getFriction() const override
|
||||
{
|
||||
return friction_;
|
||||
}
|
||||
|
||||
virtual uint wrenchVertexCount() const override
|
||||
{
|
||||
return static_cast<uint>(wrench_vertices_.size());
|
||||
}
|
||||
virtual PhysicsBodyVertex& getWrenchVertex(uint index) override
|
||||
{
|
||||
return wrench_vertices_.at(index);
|
||||
}
|
||||
virtual const PhysicsBodyVertex& getWrenchVertex(uint index) const override
|
||||
{
|
||||
return wrench_vertices_.at(index);
|
||||
}
|
||||
|
||||
virtual uint dragVertexCount() const override
|
||||
{
|
||||
return static_cast<uint>(drag_vertices_.size());
|
||||
}
|
||||
virtual PhysicsBodyVertex& getDragVertex(uint index) override
|
||||
{
|
||||
return drag_vertices_.at(index);
|
||||
}
|
||||
virtual const PhysicsBodyVertex& getDragVertex(uint index) const override
|
||||
{
|
||||
return drag_vertices_.at(index);
|
||||
}
|
||||
|
||||
Vector3r getShapeVertex(uint index) const
|
||||
{
|
||||
real_T x = (index & 1) == 0 ? body_box_.x() / 2 : -body_box_.x() / 2;
|
||||
real_T y = (index & 2) == 0 ? body_box_.y() / 2 : -body_box_.y() / 2;
|
||||
real_T z = (index & 4) == 0 ? body_box_.z() / 2 : -body_box_.z() / 2;
|
||||
|
||||
return Vector3r(x, y, z);
|
||||
}
|
||||
uint shapeVertexCount()
|
||||
{
|
||||
return 8; //for box
|
||||
}
|
||||
|
||||
private:
|
||||
Vector3r force_;
|
||||
static void createDragVertices(vector<PhysicsBodyVertex>& drag_vertices, real_T drag_coefficient, real_T body_x, real_T body_y, real_T body_z)
|
||||
{
|
||||
real_T top_bottom_area = body_x * body_y;
|
||||
real_T left_right_area = body_x * body_z;
|
||||
real_T front_back_area = body_y * body_z;
|
||||
Vector3r drag_factor_unit = Vector3r(front_back_area, left_right_area, top_bottom_area) * drag_coefficient / 2; //Vector3r drag_factor_unit = Vector3r::Zero();
|
||||
|
||||
//add six drag vertices representing 6 sides
|
||||
drag_vertices.clear();
|
||||
drag_vertices.emplace_back(Vector3r(0, 0, -body_z), Vector3r(0, 0, -1), drag_factor_unit.z());
|
||||
drag_vertices.emplace_back(Vector3r(0, 0, body_z), Vector3r(0, 0, 1), drag_factor_unit.z());
|
||||
drag_vertices.emplace_back(Vector3r(0, -body_y, 0), Vector3r(0, -1, 0), drag_factor_unit.y());
|
||||
drag_vertices.emplace_back(Vector3r(0, body_y, 0), Vector3r(0, 1, 0), drag_factor_unit.y());
|
||||
drag_vertices.emplace_back(Vector3r(-body_x, 0, 0), Vector3r(-1, 0, 0), drag_factor_unit.x());
|
||||
drag_vertices.emplace_back(Vector3r(body_x, 0, 0), Vector3r(1, 0, 0), drag_factor_unit.x());
|
||||
}
|
||||
|
||||
static void createWrenchVertices(vector<WrenchVertex>& wrench_vertices, real_T body_x, real_T body_y, real_T body_z, real_T mass)
|
||||
{
|
||||
wrench_vertices.clear();
|
||||
wrench_vertices.emplace_back(Vector3r(0, 0, -body_z), Vector3r(0, 0, -1), Vector3r(0, 0, -18 * mass));
|
||||
wrench_vertices.emplace_back(Vector3r(0, 0, body_z), Vector3r(0, 0, 1));
|
||||
wrench_vertices.emplace_back(Vector3r(0, -body_y, 0), Vector3r(0, -1, 0));
|
||||
wrench_vertices.emplace_back(Vector3r(0, body_y, 0), Vector3r(0, 1, 0));
|
||||
wrench_vertices.emplace_back(Vector3r(-body_x, 0, 0), Vector3r(-1, 0, 0));
|
||||
wrench_vertices.emplace_back(Vector3r(body_x, 0, 0), Vector3r(1, 0, 0));
|
||||
}
|
||||
|
||||
//TODO: put in common place?
|
||||
static void computeInertiaMatrix(Matrix3x3r& inertia, real_T box_mass, const Vector3r& body_box)
|
||||
{
|
||||
inertia = Matrix3x3r::Zero();
|
||||
|
||||
//http://farside.ph.utexas.edu/teaching/336k/Newtonhtml/node64.html
|
||||
inertia(0, 0) = box_mass / 12.0f * (body_box.y() * body_box.y() + body_box.z() * body_box.z());
|
||||
inertia(1, 1) = box_mass / 12.0f * (body_box.x() * body_box.x() + body_box.z() * body_box.z());
|
||||
inertia(2, 2) = box_mass / 12.0f * (body_box.x() * body_box.x() + body_box.y() * body_box.y());
|
||||
}
|
||||
|
||||
private:
|
||||
Vector3r body_box_ = Vector3r(0.20f, 0.12f, 0.04f);
|
||||
real_T mass_ = 1.0f;
|
||||
real_T restitution_ = 0.5f;
|
||||
real_T friction_ = 0.7f;
|
||||
|
||||
Matrix3x3r inertia_;
|
||||
|
||||
vector<PhysicsBodyVertex> drag_vertices_;
|
||||
vector<WrenchVertex> wrench_vertices_;
|
||||
};
|
||||
|
||||
public:
|
||||
void initialize(Kinematics* kinematics, Environment* environment)
|
||||
{
|
||||
computeInertiaMatrix(inertia_, mass_, body_box_);
|
||||
createWrenchVertices(wrench_vertices_, body_box_.x(), body_box_.y(), body_box_.z(), mass_);
|
||||
createDragVertices(drag_vertices_, 1.3f, body_box_.x(), body_box_.y(), body_box_.z());
|
||||
|
||||
PhysicsBody::initialize(mass_, inertia_, kinematics, environment);
|
||||
}
|
||||
|
||||
virtual void updateKinematics(const Kinematics::State& kinematics) override
|
||||
{
|
||||
PhysicsBody::updateKinematics(kinematics);
|
||||
|
||||
std::cout << " Pos: " << VectorMath::toString(kinematics.pose.position);
|
||||
std::cout << " Ori: " << VectorMath::toString(kinematics.pose.orientation) << std::endl;
|
||||
std::cout << " Lin Vel: " << VectorMath::toString(kinematics.twist.linear);
|
||||
std::cout << " Ang Vel: " << VectorMath::toString(kinematics.twist.angular) << std::endl;
|
||||
std::cout << " ------------------------------------------------" << std::endl;
|
||||
}
|
||||
|
||||
virtual void updateKinematics() override
|
||||
{
|
||||
PhysicsBody::updateKinematics();
|
||||
}
|
||||
|
||||
virtual real_T getRestitution() const override
|
||||
{
|
||||
return restitution_;
|
||||
}
|
||||
|
||||
virtual real_T getFriction() const override
|
||||
{
|
||||
return friction_;
|
||||
}
|
||||
|
||||
virtual uint wrenchVertexCount() const override
|
||||
{
|
||||
return static_cast<uint>(wrench_vertices_.size());
|
||||
}
|
||||
virtual PhysicsBodyVertex& getWrenchVertex(uint index) override
|
||||
{
|
||||
return wrench_vertices_.at(index);
|
||||
}
|
||||
virtual const PhysicsBodyVertex& getWrenchVertex(uint index) const override
|
||||
{
|
||||
return wrench_vertices_.at(index);
|
||||
}
|
||||
|
||||
virtual uint dragVertexCount() const override
|
||||
{
|
||||
return static_cast<uint>(drag_vertices_.size());
|
||||
}
|
||||
virtual PhysicsBodyVertex& getDragVertex(uint index) override
|
||||
{
|
||||
return drag_vertices_.at(index);
|
||||
}
|
||||
virtual const PhysicsBodyVertex& getDragVertex(uint index) const override
|
||||
{
|
||||
return drag_vertices_.at(index);
|
||||
}
|
||||
|
||||
Vector3r getShapeVertex(uint index) const
|
||||
{
|
||||
real_T x = (index & 1) == 0 ? body_box_.x() / 2 : - body_box_.x() / 2;
|
||||
real_T y = (index & 2) == 0 ? body_box_.y() / 2 : - body_box_.y() / 2;
|
||||
real_T z = (index & 4) == 0 ? body_box_.z() / 2 : - body_box_.z() / 2;
|
||||
|
||||
return Vector3r(x, y, z);
|
||||
}
|
||||
uint shapeVertexCount()
|
||||
{
|
||||
return 8; //for box
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
static void createDragVertices(vector<PhysicsBodyVertex>& drag_vertices, real_T drag_coefficient, real_T body_x, real_T body_y, real_T body_z)
|
||||
{
|
||||
real_T top_bottom_area = body_x * body_y;
|
||||
real_T left_right_area = body_x * body_z;
|
||||
real_T front_back_area = body_y * body_z;
|
||||
Vector3r drag_factor_unit = Vector3r(front_back_area, left_right_area, top_bottom_area) * drag_coefficient / 2; //Vector3r drag_factor_unit = Vector3r::Zero();
|
||||
|
||||
//add six drag vertices representing 6 sides
|
||||
drag_vertices.clear();
|
||||
drag_vertices.emplace_back(Vector3r(0, 0, -body_z), Vector3r(0, 0, -1), drag_factor_unit.z());
|
||||
drag_vertices.emplace_back(Vector3r(0, 0, body_z), Vector3r(0, 0, 1), drag_factor_unit.z());
|
||||
drag_vertices.emplace_back(Vector3r(0, -body_y, 0), Vector3r(0, -1, 0), drag_factor_unit.y());
|
||||
drag_vertices.emplace_back(Vector3r(0, body_y, 0), Vector3r(0, 1, 0), drag_factor_unit.y());
|
||||
drag_vertices.emplace_back(Vector3r(-body_x, 0, 0), Vector3r(-1, 0, 0), drag_factor_unit.x());
|
||||
drag_vertices.emplace_back(Vector3r( body_x, 0, 0), Vector3r( 1, 0, 0), drag_factor_unit.x());
|
||||
|
||||
}
|
||||
|
||||
static void createWrenchVertices(vector<WrenchVertex>& wrench_vertices, real_T body_x, real_T body_y, real_T body_z, real_T mass)
|
||||
{
|
||||
wrench_vertices.clear();
|
||||
wrench_vertices.emplace_back(Vector3r(0, 0, -body_z), Vector3r(0, 0, -1), Vector3r(0, 0, -18 * mass));
|
||||
wrench_vertices.emplace_back(Vector3r(0, 0, body_z), Vector3r(0, 0, 1));
|
||||
wrench_vertices.emplace_back(Vector3r(0, -body_y, 0), Vector3r(0, -1, 0));
|
||||
wrench_vertices.emplace_back(Vector3r(0, body_y, 0), Vector3r(0, 1, 0));
|
||||
wrench_vertices.emplace_back(Vector3r(-body_x, 0, 0), Vector3r(-1, 0, 0));
|
||||
wrench_vertices.emplace_back(Vector3r( body_x, 0, 0), Vector3r( 1, 0, 0));
|
||||
}
|
||||
|
||||
//TODO: put in common place?
|
||||
static void computeInertiaMatrix(Matrix3x3r& inertia, real_T box_mass, const Vector3r& body_box)
|
||||
{
|
||||
inertia = Matrix3x3r::Zero();
|
||||
|
||||
//http://farside.ph.utexas.edu/teaching/336k/Newtonhtml/node64.html
|
||||
inertia(0, 0) = box_mass / 12.0f * (body_box.y()*body_box.y() + body_box.z()*body_box.z());
|
||||
inertia(1, 1) = box_mass / 12.0f * (body_box.x()*body_box.x() + body_box.z()*body_box.z());
|
||||
inertia(2, 2) = box_mass / 12.0f * (body_box.x()*body_box.x() + body_box.y()*body_box.y());
|
||||
}
|
||||
|
||||
private:
|
||||
Vector3r body_box_ = Vector3r(0.20f, 0.12f, 0.04f);
|
||||
real_T mass_ = 1.0f;
|
||||
real_T restitution_ = 0.5f;
|
||||
real_T friction_ = 0.7f;
|
||||
|
||||
Matrix3x3r inertia_;
|
||||
|
||||
vector<PhysicsBodyVertex> drag_vertices_;
|
||||
vector<WrenchVertex> wrench_vertices_;
|
||||
};
|
||||
|
||||
}} //namespace
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -9,113 +9,120 @@
|
|||
#include "common/CommonStructs.hpp"
|
||||
#include "common/EarthUtils.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
class Environment : public UpdatableObject {
|
||||
public:
|
||||
struct State {
|
||||
//these fields must be set at initialization time
|
||||
Vector3r position;
|
||||
GeoPoint geo_point;
|
||||
|
||||
//these fields are computed
|
||||
Vector3r gravity;
|
||||
real_T air_pressure;
|
||||
real_T temperature;
|
||||
real_T air_density;
|
||||
|
||||
State()
|
||||
{}
|
||||
State(const Vector3r& position_val, const GeoPoint& geo_point_val)
|
||||
: position(position_val), geo_point(geo_point_val)
|
||||
class Environment : public UpdatableObject
|
||||
{
|
||||
public:
|
||||
struct State
|
||||
{
|
||||
//these fields must be set at initialization time
|
||||
Vector3r position;
|
||||
GeoPoint geo_point;
|
||||
|
||||
//these fields are computed
|
||||
Vector3r gravity;
|
||||
real_T air_pressure;
|
||||
real_T temperature;
|
||||
real_T air_density;
|
||||
|
||||
State()
|
||||
{
|
||||
}
|
||||
State(const Vector3r& position_val, const GeoPoint& geo_point_val)
|
||||
: position(position_val), geo_point(geo_point_val)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
public:
|
||||
Environment()
|
||||
{
|
||||
//allow default constructor with later call for initialize
|
||||
}
|
||||
Environment(const State& initial)
|
||||
{
|
||||
initialize(initial);
|
||||
}
|
||||
void initialize(const State& initial)
|
||||
{
|
||||
initial_ = initial;
|
||||
|
||||
setHomeGeoPoint(initial_.geo_point);
|
||||
|
||||
updateState(initial_, home_geo_point_);
|
||||
}
|
||||
|
||||
void setHomeGeoPoint(const GeoPoint& home_geo_point)
|
||||
{
|
||||
home_geo_point_ = HomeGeoPoint(home_geo_point);
|
||||
}
|
||||
|
||||
GeoPoint getHomeGeoPoint() const
|
||||
{
|
||||
return home_geo_point_.home_geo_point;
|
||||
}
|
||||
|
||||
//in local NED coordinates
|
||||
void setPosition(const Vector3r& position)
|
||||
{
|
||||
current_.position = position;
|
||||
}
|
||||
|
||||
const State& getInitialState() const
|
||||
{
|
||||
return initial_;
|
||||
}
|
||||
const State& getState() const
|
||||
{
|
||||
return current_;
|
||||
}
|
||||
State& getState()
|
||||
{
|
||||
return current_;
|
||||
}
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
updateState(current_, home_geo_point_);
|
||||
}
|
||||
|
||||
protected:
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
current_ = initial_;
|
||||
}
|
||||
|
||||
virtual void failResetUpdateOrdering(std::string err) override
|
||||
{
|
||||
unused(err);
|
||||
//Do nothing.
|
||||
//The environment gets reset() twice without an update() inbetween,
|
||||
//via MultirotorPawnSimApi::reset() and CarSimApi::reset(), because
|
||||
//those functions directly reset an environment, and also call other reset()s that reset the same environment.
|
||||
}
|
||||
|
||||
private:
|
||||
static void updateState(State& state, const HomeGeoPoint& home_geo_point)
|
||||
{
|
||||
state.geo_point = EarthUtils::nedToGeodetic(state.position, home_geo_point);
|
||||
|
||||
real_T geo_pot = EarthUtils::getGeopotential(state.geo_point.altitude / 1000.0f);
|
||||
state.temperature = EarthUtils::getStandardTemperature(geo_pot);
|
||||
state.air_pressure = EarthUtils::getStandardPressure(geo_pot, state.temperature);
|
||||
state.air_density = EarthUtils::getAirDensity(state.air_pressure, state.temperature);
|
||||
|
||||
//TODO: avoid recalculating square roots
|
||||
state.gravity = Vector3r(0, 0, EarthUtils::getGravity(state.geo_point.altitude));
|
||||
}
|
||||
|
||||
private:
|
||||
State initial_, current_;
|
||||
HomeGeoPoint home_geo_point_;
|
||||
};
|
||||
public:
|
||||
Environment()
|
||||
{
|
||||
//allow default constructor with later call for initialize
|
||||
}
|
||||
Environment(const State& initial)
|
||||
{
|
||||
initialize(initial);
|
||||
}
|
||||
void initialize(const State& initial)
|
||||
{
|
||||
initial_ = initial;
|
||||
|
||||
setHomeGeoPoint(initial_.geo_point);
|
||||
|
||||
updateState(initial_, home_geo_point_);
|
||||
}
|
||||
|
||||
void setHomeGeoPoint(const GeoPoint& home_geo_point)
|
||||
{
|
||||
home_geo_point_ = HomeGeoPoint(home_geo_point);
|
||||
}
|
||||
|
||||
GeoPoint getHomeGeoPoint() const
|
||||
{
|
||||
return home_geo_point_.home_geo_point;
|
||||
}
|
||||
|
||||
//in local NED coordinates
|
||||
void setPosition(const Vector3r& position)
|
||||
{
|
||||
current_.position = position;
|
||||
}
|
||||
|
||||
const State& getInitialState() const
|
||||
{
|
||||
return initial_;
|
||||
}
|
||||
const State& getState() const
|
||||
{
|
||||
return current_;
|
||||
}
|
||||
State& getState()
|
||||
{
|
||||
return current_;
|
||||
}
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
updateState(current_, home_geo_point_);
|
||||
}
|
||||
|
||||
protected:
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
current_ = initial_;
|
||||
}
|
||||
|
||||
virtual void failResetUpdateOrdering(std::string err) override
|
||||
{
|
||||
unused(err);
|
||||
//Do nothing.
|
||||
//The environment gets reset() twice without an update() inbetween,
|
||||
//via MultirotorPawnSimApi::reset() and CarSimApi::reset(), because
|
||||
//those functions directly reset an environment, and also call other reset()s that reset the same environment.
|
||||
}
|
||||
|
||||
private:
|
||||
static void updateState(State& state, const HomeGeoPoint& home_geo_point)
|
||||
{
|
||||
state.geo_point = EarthUtils::nedToGeodetic(state.position, home_geo_point);
|
||||
|
||||
real_T geo_pot = EarthUtils::getGeopotential(state.geo_point.altitude / 1000.0f);
|
||||
state.temperature = EarthUtils::getStandardTemperature(geo_pot);
|
||||
state.air_pressure = EarthUtils::getStandardPressure(geo_pot, state.temperature);
|
||||
state.air_density = EarthUtils::getAirDensity(state.air_pressure, state.temperature);
|
||||
|
||||
//TODO: avoid recalculating square roots
|
||||
state.gravity = Vector3r(0, 0, EarthUtils::getGravity(state.geo_point.altitude));
|
||||
}
|
||||
|
||||
private:
|
||||
State initial_, current_;
|
||||
HomeGeoPoint home_geo_point_;
|
||||
};
|
||||
|
||||
}} //namespace
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -14,45 +14,43 @@
|
|||
#include "common/SteppableClock.hpp"
|
||||
#include <cinttypes>
|
||||
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
class ExternalPhysicsEngine : public PhysicsEngineBase {
|
||||
public:
|
||||
ExternalPhysicsEngine()
|
||||
{
|
||||
}
|
||||
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
class ExternalPhysicsEngine : public PhysicsEngineBase
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
PhysicsEngineBase::update();
|
||||
|
||||
for (PhysicsBody* body_ptr : *this) {
|
||||
body_ptr->updateKinematics();
|
||||
body_ptr->update();
|
||||
public:
|
||||
ExternalPhysicsEngine()
|
||||
{
|
||||
}
|
||||
|
||||
}
|
||||
virtual void reportState(StateReporter& reporter) override
|
||||
{
|
||||
for (PhysicsBody* body_ptr : *this) {
|
||||
reporter.writeValue("ExternalPhysicsEngine",true);
|
||||
reporter.writeValue("Is Grounded", body_ptr->isGrounded());
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
}
|
||||
//call base
|
||||
UpdatableObject::reportState(reporter);
|
||||
}
|
||||
//*** End: UpdatableState implementation ***//
|
||||
|
||||
};
|
||||
virtual void update() override
|
||||
{
|
||||
PhysicsEngineBase::update();
|
||||
|
||||
for (PhysicsBody* body_ptr : *this) {
|
||||
body_ptr->updateKinematics();
|
||||
body_ptr->update();
|
||||
}
|
||||
}
|
||||
virtual void reportState(StateReporter& reporter) override
|
||||
{
|
||||
for (PhysicsBody* body_ptr : *this) {
|
||||
reporter.writeValue("ExternalPhysicsEngine", true);
|
||||
reporter.writeValue("Is Grounded", body_ptr->isGrounded());
|
||||
}
|
||||
//call base
|
||||
UpdatableObject::reportState(reporter);
|
||||
}
|
||||
//*** End: UpdatableState implementation ***//
|
||||
};
|
||||
|
||||
} //namespace
|
||||
} //namespace
|
||||
|
|
|
|||
|
|
@ -14,173 +14,172 @@
|
|||
#include "common/SteppableClock.hpp"
|
||||
#include <cinttypes>
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
class FastPhysicsEngine : public PhysicsEngineBase
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
public:
|
||||
FastPhysicsEngine(bool enable_ground_lock = true, Vector3r wind = Vector3r::Zero())
|
||||
: enable_ground_lock_(enable_ground_lock), wind_(wind)
|
||||
{
|
||||
setName("FastPhysicsEngine");
|
||||
}
|
||||
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
class FastPhysicsEngine : public PhysicsEngineBase
|
||||
{
|
||||
for (PhysicsBody* body_ptr : *this) {
|
||||
public:
|
||||
FastPhysicsEngine(bool enable_ground_lock = true, Vector3r wind = Vector3r::Zero())
|
||||
: enable_ground_lock_(enable_ground_lock), wind_(wind)
|
||||
{
|
||||
setName("FastPhysicsEngine");
|
||||
}
|
||||
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
for (PhysicsBody* body_ptr : *this) {
|
||||
initPhysicsBody(body_ptr);
|
||||
}
|
||||
}
|
||||
|
||||
virtual void insert(PhysicsBody* body_ptr) override
|
||||
{
|
||||
PhysicsEngineBase::insert(body_ptr);
|
||||
|
||||
initPhysicsBody(body_ptr);
|
||||
}
|
||||
}
|
||||
|
||||
virtual void insert(PhysicsBody* body_ptr) override
|
||||
{
|
||||
PhysicsEngineBase::insert(body_ptr);
|
||||
virtual void update() override
|
||||
{
|
||||
PhysicsEngineBase::update();
|
||||
|
||||
initPhysicsBody(body_ptr);
|
||||
}
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
PhysicsEngineBase::update();
|
||||
|
||||
for (PhysicsBody* body_ptr : *this) {
|
||||
updatePhysics(*body_ptr);
|
||||
for (PhysicsBody* body_ptr : *this) {
|
||||
updatePhysics(*body_ptr);
|
||||
}
|
||||
}
|
||||
}
|
||||
virtual void reportState(StateReporter& reporter) override
|
||||
{
|
||||
for (PhysicsBody* body_ptr : *this) {
|
||||
reporter.writeValue("Phys", debug_string_.str());
|
||||
reporter.writeValue("Is Grounded", body_ptr->isGrounded());
|
||||
reporter.writeValue("Force (world)", body_ptr->getWrench().force);
|
||||
reporter.writeValue("Torque (body)", body_ptr->getWrench().torque);
|
||||
virtual void reportState(StateReporter& reporter) override
|
||||
{
|
||||
for (PhysicsBody* body_ptr : *this) {
|
||||
reporter.writeValue("Phys", debug_string_.str());
|
||||
reporter.writeValue("Is Grounded", body_ptr->isGrounded());
|
||||
reporter.writeValue("Force (world)", body_ptr->getWrench().force);
|
||||
reporter.writeValue("Torque (body)", body_ptr->getWrench().torque);
|
||||
}
|
||||
//call base
|
||||
UpdatableObject::reportState(reporter);
|
||||
}
|
||||
//call base
|
||||
UpdatableObject::reportState(reporter);
|
||||
}
|
||||
//*** End: UpdatableState implementation ***//
|
||||
//*** End: UpdatableState implementation ***//
|
||||
|
||||
// Set Wind, for API and Settings implementation
|
||||
void setWind(const Vector3r& wind) override
|
||||
{
|
||||
wind_ = wind;
|
||||
}
|
||||
|
||||
private:
|
||||
void initPhysicsBody(PhysicsBody* body_ptr)
|
||||
{
|
||||
body_ptr->last_kinematics_time = clock()->nowNanos();
|
||||
}
|
||||
|
||||
void updatePhysics(PhysicsBody& body)
|
||||
{
|
||||
TTimeDelta dt = clock()->updateSince(body.last_kinematics_time);
|
||||
|
||||
body.lock();
|
||||
//get current kinematics state of the body - this state existed since last dt seconds
|
||||
const Kinematics::State& current = body.getKinematics();
|
||||
Kinematics::State next;
|
||||
Wrench next_wrench;
|
||||
|
||||
//first compute the response as if there was no collision
|
||||
//this is necessary to take in to account forces and torques generated by body
|
||||
getNextKinematicsNoCollision(dt, body, current, next, next_wrench, wind_);
|
||||
|
||||
//if there is collision, see if we need collision response
|
||||
const CollisionInfo collision_info = body.getCollisionInfo();
|
||||
CollisionResponse& collision_response = body.getCollisionResponseInfo();
|
||||
//if collision was already responded then do not respond to it until we get updated information
|
||||
if (body.isGrounded() || (collision_info.has_collided && collision_response.collision_time_stamp != collision_info.time_stamp)) {
|
||||
bool is_collision_response = getNextKinematicsOnCollision(dt, collision_info, body,
|
||||
current, next, next_wrench, enable_ground_lock_);
|
||||
updateCollisionResponseInfo(collision_info, next, is_collision_response, collision_response);
|
||||
//throttledLogOutput("*** has collision", 0.1);
|
||||
}
|
||||
//else throttledLogOutput("*** no collision", 0.1);
|
||||
|
||||
//Utils::log(Utils::stringf("T-VEL %s %" PRIu64 ": ",
|
||||
// VectorMath::toString(next.twist.linear).c_str(), clock()->getStepCount()));
|
||||
|
||||
body.setWrench(next_wrench);
|
||||
body.updateKinematics(next);
|
||||
body.unlock();
|
||||
|
||||
|
||||
//TODO: this is now being done in PawnSimApi::update. We need to re-think this sequence
|
||||
//with below commented out - Arducopter GPS may not work.
|
||||
//body.getEnvironment().setPosition(next.pose.position);
|
||||
//body.getEnvironment().update();
|
||||
|
||||
}
|
||||
|
||||
static void updateCollisionResponseInfo(const CollisionInfo& collision_info, const Kinematics::State& next,
|
||||
bool is_collision_response, CollisionResponse& collision_response)
|
||||
{
|
||||
collision_response.collision_time_stamp = collision_info.time_stamp;
|
||||
++collision_response.collision_count_raw;
|
||||
|
||||
//increment counter if we didn't collided with high velocity (like resting on ground)
|
||||
if (is_collision_response && next.twist.linear.squaredNorm() > kRestingVelocityMax * kRestingVelocityMax)
|
||||
++collision_response.collision_count_non_resting;
|
||||
|
||||
}
|
||||
|
||||
//return value indicates if collision response was generated
|
||||
static bool getNextKinematicsOnCollision(TTimeDelta dt, const CollisionInfo& collision_info, PhysicsBody& body,
|
||||
const Kinematics::State& current, Kinematics::State& next, Wrench& next_wrench, bool enable_ground_lock)
|
||||
{
|
||||
/************************* Collision response ************************/
|
||||
const real_T dt_real = static_cast<real_T>(dt);
|
||||
|
||||
//are we going away from collision? if so then keep using computed next state
|
||||
if (collision_info.normal.dot(next.twist.linear) >= 0.0f)
|
||||
return false;
|
||||
|
||||
/********** Core collision response ***********/
|
||||
//get avg current velocity
|
||||
const Vector3r vcur_avg = current.twist.linear + current.accelerations.linear * dt_real;
|
||||
|
||||
//get average angular velocity
|
||||
const Vector3r angular_avg = current.twist.angular + current.accelerations.angular * dt_real;
|
||||
|
||||
//contact point vector
|
||||
Vector3r r = collision_info.impact_point - collision_info.position;
|
||||
|
||||
//see if impact is straight at body's surface (assuming its box)
|
||||
const Vector3r normal_body = VectorMath::transformToBodyFrame(collision_info.normal, current.pose.orientation);
|
||||
const bool is_ground_normal = Utils::isApproximatelyEqual(std::abs(normal_body.z()), 1.0f, kAxisTolerance);
|
||||
bool ground_collision = false;
|
||||
const float z_vel = vcur_avg.z();
|
||||
const bool is_landing = z_vel > std::abs(vcur_avg.x()) && z_vel > std::abs(vcur_avg.y());
|
||||
|
||||
real_T restitution = body.getRestitution();
|
||||
real_T friction = body.getFriction();
|
||||
|
||||
if (is_ground_normal && is_landing
|
||||
// So normal_body is the collision normal translated into body coords, why does an x==1 or y==1
|
||||
// mean we are coliding with the ground???
|
||||
// || Utils::isApproximatelyEqual(std::abs(normal_body.x()), 1.0f, kAxisTolerance)
|
||||
// || Utils::isApproximatelyEqual(std::abs(normal_body.y()), 1.0f, kAxisTolerance)
|
||||
) {
|
||||
// looks like we are coliding with the ground. We don't want the ground to be so bouncy
|
||||
// so we reduce the coefficient of restitution. 0 means no bounce.
|
||||
// TODO: it would be better if we did this based on the material we are landing on.
|
||||
// e.g. grass should be inelastic, but a hard surface like the road should be more bouncy.
|
||||
restitution = 0;
|
||||
// crank up friction with the ground so it doesn't try and slide across the ground
|
||||
// again, this should depend on the type of surface we are landing on.
|
||||
friction = 1;
|
||||
|
||||
//we have collided with ground straight on, we will fix orientation later
|
||||
ground_collision = is_ground_normal;
|
||||
// Set Wind, for API and Settings implementation
|
||||
void setWind(const Vector3r& wind) override
|
||||
{
|
||||
wind_ = wind;
|
||||
}
|
||||
|
||||
//velocity at contact point
|
||||
const Vector3r vcur_avg_body = VectorMath::transformToBodyFrame(vcur_avg, current.pose.orientation);
|
||||
const Vector3r contact_vel_body = vcur_avg_body + angular_avg.cross(r);
|
||||
private:
|
||||
void initPhysicsBody(PhysicsBody* body_ptr)
|
||||
{
|
||||
body_ptr->last_kinematics_time = clock()->nowNanos();
|
||||
}
|
||||
|
||||
/*
|
||||
void updatePhysics(PhysicsBody& body)
|
||||
{
|
||||
TTimeDelta dt = clock()->updateSince(body.last_kinematics_time);
|
||||
|
||||
body.lock();
|
||||
//get current kinematics state of the body - this state existed since last dt seconds
|
||||
const Kinematics::State& current = body.getKinematics();
|
||||
Kinematics::State next;
|
||||
Wrench next_wrench;
|
||||
|
||||
//first compute the response as if there was no collision
|
||||
//this is necessary to take in to account forces and torques generated by body
|
||||
getNextKinematicsNoCollision(dt, body, current, next, next_wrench, wind_);
|
||||
|
||||
//if there is collision, see if we need collision response
|
||||
const CollisionInfo collision_info = body.getCollisionInfo();
|
||||
CollisionResponse& collision_response = body.getCollisionResponseInfo();
|
||||
//if collision was already responded then do not respond to it until we get updated information
|
||||
if (body.isGrounded() || (collision_info.has_collided && collision_response.collision_time_stamp != collision_info.time_stamp)) {
|
||||
bool is_collision_response = getNextKinematicsOnCollision(dt, collision_info, body, current, next, next_wrench, enable_ground_lock_);
|
||||
updateCollisionResponseInfo(collision_info, next, is_collision_response, collision_response);
|
||||
//throttledLogOutput("*** has collision", 0.1);
|
||||
}
|
||||
//else throttledLogOutput("*** no collision", 0.1);
|
||||
|
||||
//Utils::log(Utils::stringf("T-VEL %s %" PRIu64 ": ",
|
||||
// VectorMath::toString(next.twist.linear).c_str(), clock()->getStepCount()));
|
||||
|
||||
body.setWrench(next_wrench);
|
||||
body.updateKinematics(next);
|
||||
body.unlock();
|
||||
|
||||
//TODO: this is now being done in PawnSimApi::update. We need to re-think this sequence
|
||||
//with below commented out - Arducopter GPS may not work.
|
||||
//body.getEnvironment().setPosition(next.pose.position);
|
||||
//body.getEnvironment().update();
|
||||
}
|
||||
|
||||
static void updateCollisionResponseInfo(const CollisionInfo& collision_info, const Kinematics::State& next,
|
||||
bool is_collision_response, CollisionResponse& collision_response)
|
||||
{
|
||||
collision_response.collision_time_stamp = collision_info.time_stamp;
|
||||
++collision_response.collision_count_raw;
|
||||
|
||||
//increment counter if we didn't collided with high velocity (like resting on ground)
|
||||
if (is_collision_response && next.twist.linear.squaredNorm() > kRestingVelocityMax * kRestingVelocityMax)
|
||||
++collision_response.collision_count_non_resting;
|
||||
}
|
||||
|
||||
//return value indicates if collision response was generated
|
||||
static bool getNextKinematicsOnCollision(TTimeDelta dt, const CollisionInfo& collision_info, PhysicsBody& body,
|
||||
const Kinematics::State& current, Kinematics::State& next, Wrench& next_wrench, bool enable_ground_lock)
|
||||
{
|
||||
/************************* Collision response ************************/
|
||||
const real_T dt_real = static_cast<real_T>(dt);
|
||||
|
||||
//are we going away from collision? if so then keep using computed next state
|
||||
if (collision_info.normal.dot(next.twist.linear) >= 0.0f)
|
||||
return false;
|
||||
|
||||
/********** Core collision response ***********/
|
||||
//get avg current velocity
|
||||
const Vector3r vcur_avg = current.twist.linear + current.accelerations.linear * dt_real;
|
||||
|
||||
//get average angular velocity
|
||||
const Vector3r angular_avg = current.twist.angular + current.accelerations.angular * dt_real;
|
||||
|
||||
//contact point vector
|
||||
Vector3r r = collision_info.impact_point - collision_info.position;
|
||||
|
||||
//see if impact is straight at body's surface (assuming its box)
|
||||
const Vector3r normal_body = VectorMath::transformToBodyFrame(collision_info.normal, current.pose.orientation);
|
||||
const bool is_ground_normal = Utils::isApproximatelyEqual(std::abs(normal_body.z()), 1.0f, kAxisTolerance);
|
||||
bool ground_collision = false;
|
||||
const float z_vel = vcur_avg.z();
|
||||
const bool is_landing = z_vel > std::abs(vcur_avg.x()) && z_vel > std::abs(vcur_avg.y());
|
||||
|
||||
real_T restitution = body.getRestitution();
|
||||
real_T friction = body.getFriction();
|
||||
|
||||
if (is_ground_normal && is_landing
|
||||
// So normal_body is the collision normal translated into body coords, why does an x==1 or y==1
|
||||
// mean we are coliding with the ground???
|
||||
// || Utils::isApproximatelyEqual(std::abs(normal_body.x()), 1.0f, kAxisTolerance)
|
||||
// || Utils::isApproximatelyEqual(std::abs(normal_body.y()), 1.0f, kAxisTolerance)
|
||||
) {
|
||||
// looks like we are coliding with the ground. We don't want the ground to be so bouncy
|
||||
// so we reduce the coefficient of restitution. 0 means no bounce.
|
||||
// TODO: it would be better if we did this based on the material we are landing on.
|
||||
// e.g. grass should be inelastic, but a hard surface like the road should be more bouncy.
|
||||
restitution = 0;
|
||||
// crank up friction with the ground so it doesn't try and slide across the ground
|
||||
// again, this should depend on the type of surface we are landing on.
|
||||
friction = 1;
|
||||
|
||||
//we have collided with ground straight on, we will fix orientation later
|
||||
ground_collision = is_ground_normal;
|
||||
}
|
||||
|
||||
//velocity at contact point
|
||||
const Vector3r vcur_avg_body = VectorMath::transformToBodyFrame(vcur_avg, current.pose.orientation);
|
||||
const Vector3r contact_vel_body = vcur_avg_body + angular_avg.cross(r);
|
||||
|
||||
/*
|
||||
GafferOnGames - Collision response with columb friction
|
||||
http://gafferongames.com/virtual-go/collision-response-and-coulomb-friction/
|
||||
Assuming collision is with static fixed body,
|
||||
|
|
@ -189,241 +188,240 @@ private:
|
|||
http://chrishecker.com/images/e/e7/Gdmphys3.pdf
|
||||
V(t+1) = V(t) + j*N / m
|
||||
*/
|
||||
const real_T impulse_mag_denom = 1.0f / body.getMass() +
|
||||
(body.getInertiaInv() * r.cross(normal_body))
|
||||
.cross(r)
|
||||
.dot(normal_body);
|
||||
const real_T impulse_mag = -contact_vel_body.dot(normal_body) * (1 + restitution) / impulse_mag_denom;
|
||||
const real_T impulse_mag_denom = 1.0f / body.getMass() +
|
||||
(body.getInertiaInv() * r.cross(normal_body))
|
||||
.cross(r)
|
||||
.dot(normal_body);
|
||||
const real_T impulse_mag = -contact_vel_body.dot(normal_body) * (1 + restitution) / impulse_mag_denom;
|
||||
|
||||
next.twist.linear = vcur_avg + collision_info.normal * (impulse_mag / body.getMass());
|
||||
next.twist.angular = angular_avg + r.cross(normal_body) * impulse_mag;
|
||||
next.twist.linear = vcur_avg + collision_info.normal * (impulse_mag / body.getMass());
|
||||
next.twist.angular = angular_avg + r.cross(normal_body) * impulse_mag;
|
||||
|
||||
//above would modify component in direction of normal
|
||||
//we will use friction to modify component in direction of tangent
|
||||
const Vector3r contact_tang_body = contact_vel_body - normal_body * normal_body.dot(contact_vel_body);
|
||||
const Vector3r contact_tang_unit_body = contact_tang_body.normalized();
|
||||
const real_T friction_mag_denom = 1.0f / body.getMass() +
|
||||
(body.getInertiaInv() * r.cross(contact_tang_unit_body))
|
||||
.cross(r)
|
||||
.dot(contact_tang_unit_body);
|
||||
const real_T friction_mag = -contact_tang_body.norm() * friction / friction_mag_denom;
|
||||
//above would modify component in direction of normal
|
||||
//we will use friction to modify component in direction of tangent
|
||||
const Vector3r contact_tang_body = contact_vel_body - normal_body * normal_body.dot(contact_vel_body);
|
||||
const Vector3r contact_tang_unit_body = contact_tang_body.normalized();
|
||||
const real_T friction_mag_denom = 1.0f / body.getMass() +
|
||||
(body.getInertiaInv() * r.cross(contact_tang_unit_body))
|
||||
.cross(r)
|
||||
.dot(contact_tang_unit_body);
|
||||
const real_T friction_mag = -contact_tang_body.norm() * friction / friction_mag_denom;
|
||||
|
||||
const Vector3r contact_tang_unit = VectorMath::transformToWorldFrame(contact_tang_unit_body, current.pose.orientation);
|
||||
next.twist.linear += contact_tang_unit * friction_mag;
|
||||
next.twist.angular += r.cross(contact_tang_unit_body) * (friction_mag / body.getMass());
|
||||
const Vector3r contact_tang_unit = VectorMath::transformToWorldFrame(contact_tang_unit_body, current.pose.orientation);
|
||||
next.twist.linear += contact_tang_unit * friction_mag;
|
||||
next.twist.angular += r.cross(contact_tang_unit_body) * (friction_mag / body.getMass());
|
||||
|
||||
//TODO: implement better rolling friction
|
||||
next.twist.angular *= 0.9f;
|
||||
//TODO: implement better rolling friction
|
||||
next.twist.angular *= 0.9f;
|
||||
|
||||
// there is no acceleration during collision response, this is a hack, but without it the acceleration cancels
|
||||
// the computed impulse response too much and stops the vehicle from bouncing off the collided object.
|
||||
next.accelerations.linear = Vector3r::Zero();
|
||||
next.accelerations.angular = Vector3r::Zero();
|
||||
|
||||
next.pose = current.pose;
|
||||
if (enable_ground_lock && ground_collision) {
|
||||
float pitch, roll, yaw;
|
||||
VectorMath::toEulerianAngle(next.pose.orientation, pitch, roll, yaw);
|
||||
pitch = roll = 0;
|
||||
next.pose.orientation = VectorMath::toQuaternion(pitch, roll, yaw);
|
||||
|
||||
//there is a lot of random angular velocity when vehicle is on the ground
|
||||
next.twist.angular = Vector3r::Zero();
|
||||
|
||||
// also eliminate any linear velocity due to twist - since we are sitting on the ground there shouldn't be any.
|
||||
next.twist.linear = Vector3r::Zero();
|
||||
next.pose.position = collision_info.position;
|
||||
body.setGrounded(true);
|
||||
|
||||
// but we do want to "feel" the ground when we hit it (we should see a small z-acc bump)
|
||||
// equal and opposite our downward velocity.
|
||||
next.accelerations.linear = -0.5f * body.getMass() * vcur_avg;
|
||||
|
||||
//throttledLogOutput("*** Triggering ground lock", 0.1);
|
||||
}
|
||||
else {
|
||||
//else keep the orientation
|
||||
next.pose.position = collision_info.position + (collision_info.normal * collision_info.penetration_depth) + next.twist.linear * (dt_real * kCollisionResponseCycles);
|
||||
}
|
||||
next_wrench = Wrench::zero();
|
||||
|
||||
//Utils::log(Utils::stringf("*** C-VEL %s: ", VectorMath::toString(next.twist.linear).c_str()));
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void throttledLogOutput(const std::string& msg, double seconds)
|
||||
{
|
||||
TTimeDelta dt = clock()->elapsedSince(last_message_time);
|
||||
const real_T dt_real = static_cast<real_T>(dt);
|
||||
if (dt_real > seconds) {
|
||||
Utils::log(msg);
|
||||
last_message_time = clock()->nowNanos();
|
||||
}
|
||||
}
|
||||
|
||||
static Wrench getDragWrench(const PhysicsBody& body, const Quaternionr& orientation,
|
||||
const Vector3r& linear_vel, const Vector3r& angular_vel_body, const Vector3r& wind_world)
|
||||
{
|
||||
//add linear drag due to velocity we had since last dt seconds + wind
|
||||
//drag vector magnitude is proportional to v^2, direction opposite of velocity
|
||||
//total drag is b*v + c*v*v but we ignore the first term as b << c (pg 44, Classical Mechanics, John Taylor)
|
||||
//To find the drag force, we find the magnitude in the body frame and unit vector direction in world frame
|
||||
//http://physics.stackexchange.com/questions/304742/angular-drag-on-body
|
||||
//similarly calculate angular drag
|
||||
//note that angular velocity, acceleration, torque are already in body frame
|
||||
|
||||
Wrench wrench = Wrench::zero();
|
||||
const real_T air_density = body.getEnvironment().getState().air_density;
|
||||
|
||||
// Use relative velocity of the body wrt wind
|
||||
const Vector3r relative_vel = linear_vel - wind_world;
|
||||
const Vector3r linear_vel_body = VectorMath::transformToBodyFrame(relative_vel, orientation);
|
||||
|
||||
for (uint vi = 0; vi < body.dragVertexCount(); ++vi) {
|
||||
const auto& vertex = body.getDragVertex(vi);
|
||||
const Vector3r vel_vertex = linear_vel_body + angular_vel_body.cross(vertex.getPosition());
|
||||
const real_T vel_comp = vertex.getNormal().dot(vel_vertex);
|
||||
//if vel_comp is -ve then we cull the face. If velocity too low then drag is not generated
|
||||
if (vel_comp > kDragMinVelocity) {
|
||||
const Vector3r drag_force = vertex.getNormal() * (- vertex.getDragFactor() * air_density * vel_comp * vel_comp);
|
||||
const Vector3r drag_torque = vertex.getPosition().cross(drag_force);
|
||||
|
||||
wrench.force += drag_force;
|
||||
wrench.torque += drag_torque;
|
||||
}
|
||||
}
|
||||
|
||||
//convert force to world frame, leave torque to local frame
|
||||
wrench.force = VectorMath::transformToWorldFrame(wrench.force, orientation);
|
||||
|
||||
return wrench;
|
||||
}
|
||||
|
||||
static Wrench getBodyWrench(const PhysicsBody& body, const Quaternionr& orientation)
|
||||
{
|
||||
//set wrench sum to zero
|
||||
Wrench wrench = Wrench::zero();
|
||||
|
||||
//calculate total force on rigid body's center of gravity
|
||||
for (uint i = 0; i < body.wrenchVertexCount(); ++i) {
|
||||
//aggregate total
|
||||
const PhysicsBodyVertex& vertex = body.getWrenchVertex(i);
|
||||
const auto& vertex_wrench = vertex.getWrench();
|
||||
wrench += vertex_wrench;
|
||||
|
||||
//add additional torque due to force applies farther than COG
|
||||
// tau = r X F
|
||||
wrench.torque += vertex.getPosition().cross(vertex_wrench.force);
|
||||
}
|
||||
|
||||
//convert force to world frame, leave torque to local frame
|
||||
wrench.force = VectorMath::transformToWorldFrame(wrench.force, orientation);
|
||||
|
||||
return wrench;
|
||||
}
|
||||
|
||||
static void getNextKinematicsNoCollision(TTimeDelta dt, PhysicsBody& body, const Kinematics::State& current,
|
||||
Kinematics::State& next, Wrench& next_wrench, const Vector3r& wind)
|
||||
{
|
||||
const real_T dt_real = static_cast<real_T>(dt);
|
||||
|
||||
Vector3r avg_linear = Vector3r::Zero();
|
||||
Vector3r avg_angular = Vector3r::Zero();
|
||||
|
||||
/************************* Get force and torque acting on body ************************/
|
||||
//set wrench sum to zero
|
||||
const Wrench body_wrench = getBodyWrench(body, current.pose.orientation);
|
||||
|
||||
if (body.isGrounded()) {
|
||||
// make it stick to the ground until the magnitude of net external force on body exceeds its weight.
|
||||
float external_force_magnitude = body_wrench.force.squaredNorm();
|
||||
Vector3r weight = body.getMass() * body.getEnvironment().getState().gravity;
|
||||
float weight_magnitude = weight.squaredNorm();
|
||||
if (external_force_magnitude >= weight_magnitude) {
|
||||
//throttledLogOutput("*** Losing ground lock due to body_wrench " + VectorMath::toString(body_wrench.force), 0.1);
|
||||
body.setGrounded(false);
|
||||
}
|
||||
next_wrench.force = Vector3r::Zero();
|
||||
next_wrench.torque = Vector3r::Zero();
|
||||
// there is no acceleration during collision response, this is a hack, but without it the acceleration cancels
|
||||
// the computed impulse response too much and stops the vehicle from bouncing off the collided object.
|
||||
next.accelerations.linear = Vector3r::Zero();
|
||||
next.accelerations.angular = Vector3r::Zero();
|
||||
|
||||
next.pose = current.pose;
|
||||
if (enable_ground_lock && ground_collision) {
|
||||
float pitch, roll, yaw;
|
||||
VectorMath::toEulerianAngle(next.pose.orientation, pitch, roll, yaw);
|
||||
pitch = roll = 0;
|
||||
next.pose.orientation = VectorMath::toQuaternion(pitch, roll, yaw);
|
||||
|
||||
//there is a lot of random angular velocity when vehicle is on the ground
|
||||
next.twist.angular = Vector3r::Zero();
|
||||
|
||||
// also eliminate any linear velocity due to twist - since we are sitting on the ground there shouldn't be any.
|
||||
next.twist.linear = Vector3r::Zero();
|
||||
next.pose.position = collision_info.position;
|
||||
body.setGrounded(true);
|
||||
|
||||
// but we do want to "feel" the ground when we hit it (we should see a small z-acc bump)
|
||||
// equal and opposite our downward velocity.
|
||||
next.accelerations.linear = -0.5f * body.getMass() * vcur_avg;
|
||||
|
||||
//throttledLogOutput("*** Triggering ground lock", 0.1);
|
||||
}
|
||||
else {
|
||||
//else keep the orientation
|
||||
next.pose.position = collision_info.position + (collision_info.normal * collision_info.penetration_depth) + next.twist.linear * (dt_real * kCollisionResponseCycles);
|
||||
}
|
||||
next_wrench = Wrench::zero();
|
||||
|
||||
//Utils::log(Utils::stringf("*** C-VEL %s: ", VectorMath::toString(next.twist.linear).c_str()));
|
||||
|
||||
return true;
|
||||
}
|
||||
else {
|
||||
|
||||
void throttledLogOutput(const std::string& msg, double seconds)
|
||||
{
|
||||
TTimeDelta dt = clock()->elapsedSince(last_message_time);
|
||||
const real_T dt_real = static_cast<real_T>(dt);
|
||||
if (dt_real > seconds) {
|
||||
Utils::log(msg);
|
||||
last_message_time = clock()->nowNanos();
|
||||
}
|
||||
}
|
||||
|
||||
static Wrench getDragWrench(const PhysicsBody& body, const Quaternionr& orientation,
|
||||
const Vector3r& linear_vel, const Vector3r& angular_vel_body, const Vector3r& wind_world)
|
||||
{
|
||||
//add linear drag due to velocity we had since last dt seconds + wind
|
||||
//drag vector magnitude is proportional to v^2, direction opposite of velocity
|
||||
//total drag is b*v + c*v*v but we ignore the first term as b << c (pg 44, Classical Mechanics, John Taylor)
|
||||
//To find the drag force, we find the magnitude in the body frame and unit vector direction in world frame
|
||||
avg_linear = current.twist.linear + current.accelerations.linear * (0.5f * dt_real);
|
||||
avg_angular = current.twist.angular + current.accelerations.angular * (0.5f * dt_real);
|
||||
const Wrench drag_wrench = getDragWrench(body, current.pose.orientation, avg_linear, avg_angular, wind);
|
||||
//http://physics.stackexchange.com/questions/304742/angular-drag-on-body
|
||||
//similarly calculate angular drag
|
||||
//note that angular velocity, acceleration, torque are already in body frame
|
||||
|
||||
next_wrench = body_wrench + drag_wrench;
|
||||
Wrench wrench = Wrench::zero();
|
||||
const real_T air_density = body.getEnvironment().getState().air_density;
|
||||
|
||||
//Utils::log(Utils::stringf("B-WRN %s: ", VectorMath::toString(body_wrench.force).c_str()));
|
||||
//Utils::log(Utils::stringf("D-WRN %s: ", VectorMath::toString(drag_wrench.force).c_str()));
|
||||
// Use relative velocity of the body wrt wind
|
||||
const Vector3r relative_vel = linear_vel - wind_world;
|
||||
const Vector3r linear_vel_body = VectorMath::transformToBodyFrame(relative_vel, orientation);
|
||||
|
||||
/************************* Update accelerations due to force and torque ************************/
|
||||
//get new acceleration due to force - we'll use this acceleration in next time step
|
||||
for (uint vi = 0; vi < body.dragVertexCount(); ++vi) {
|
||||
const auto& vertex = body.getDragVertex(vi);
|
||||
const Vector3r vel_vertex = linear_vel_body + angular_vel_body.cross(vertex.getPosition());
|
||||
const real_T vel_comp = vertex.getNormal().dot(vel_vertex);
|
||||
//if vel_comp is -ve then we cull the face. If velocity too low then drag is not generated
|
||||
if (vel_comp > kDragMinVelocity) {
|
||||
const Vector3r drag_force = vertex.getNormal() * (-vertex.getDragFactor() * air_density * vel_comp * vel_comp);
|
||||
const Vector3r drag_torque = vertex.getPosition().cross(drag_force);
|
||||
|
||||
next.accelerations.linear = (next_wrench.force / body.getMass()) + body.getEnvironment().getState().gravity;
|
||||
wrench.force += drag_force;
|
||||
wrench.torque += drag_torque;
|
||||
}
|
||||
}
|
||||
|
||||
//convert force to world frame, leave torque to local frame
|
||||
wrench.force = VectorMath::transformToWorldFrame(wrench.force, orientation);
|
||||
|
||||
return wrench;
|
||||
}
|
||||
|
||||
static Wrench getBodyWrench(const PhysicsBody& body, const Quaternionr& orientation)
|
||||
{
|
||||
//set wrench sum to zero
|
||||
Wrench wrench = Wrench::zero();
|
||||
|
||||
if (body.isGrounded()) {
|
||||
// this stops vehicle from vibrating while it is on the ground doing nothing.
|
||||
next.accelerations.angular = Vector3r::Zero();
|
||||
next.twist.linear = Vector3r::Zero();
|
||||
next.twist.angular = Vector3r::Zero();
|
||||
} else {
|
||||
//get new angular acceleration
|
||||
//Euler's rotation equation: https://en.wikipedia.org/wiki/Euler's_equations_(body_dynamics)
|
||||
//we will use torque to find out the angular acceleration
|
||||
//angular momentum L = I * omega
|
||||
const Vector3r angular_momentum = body.getInertia() * avg_angular;
|
||||
const Vector3r angular_momentum_rate = next_wrench.torque - avg_angular.cross(angular_momentum);
|
||||
//new angular acceleration - we'll use this acceleration in next time step
|
||||
next.accelerations.angular = body.getInertiaInv() * angular_momentum_rate;
|
||||
//calculate total force on rigid body's center of gravity
|
||||
for (uint i = 0; i < body.wrenchVertexCount(); ++i) {
|
||||
//aggregate total
|
||||
const PhysicsBodyVertex& vertex = body.getWrenchVertex(i);
|
||||
const auto& vertex_wrench = vertex.getWrench();
|
||||
wrench += vertex_wrench;
|
||||
|
||||
/************************* Update pose and twist after dt ************************/
|
||||
//Verlet integration: http://www.physics.udel.edu/~bnikolic/teaching/phys660/numerical_ode/node5.html
|
||||
next.twist.linear = current.twist.linear + (current.accelerations.linear + next.accelerations.linear) * (0.5f * dt_real);
|
||||
next.twist.angular = current.twist.angular + (current.accelerations.angular + next.accelerations.angular) * (0.5f * dt_real);
|
||||
//add additional torque due to force applies farther than COG
|
||||
// tau = r X F
|
||||
wrench.torque += vertex.getPosition().cross(vertex_wrench.force);
|
||||
}
|
||||
|
||||
//if controller has bug, velocities can increase idenfinitely
|
||||
//so we need to clip this or everything will turn in to infinity/nans
|
||||
//convert force to world frame, leave torque to local frame
|
||||
wrench.force = VectorMath::transformToWorldFrame(wrench.force, orientation);
|
||||
|
||||
if (next.twist.linear.squaredNorm() > EarthUtils::SpeedOfLight * EarthUtils::SpeedOfLight) { //speed of light
|
||||
next.twist.linear /= (next.twist.linear.norm() / EarthUtils::SpeedOfLight);
|
||||
return wrench;
|
||||
}
|
||||
|
||||
static void getNextKinematicsNoCollision(TTimeDelta dt, PhysicsBody& body, const Kinematics::State& current,
|
||||
Kinematics::State& next, Wrench& next_wrench, const Vector3r& wind)
|
||||
{
|
||||
const real_T dt_real = static_cast<real_T>(dt);
|
||||
|
||||
Vector3r avg_linear = Vector3r::Zero();
|
||||
Vector3r avg_angular = Vector3r::Zero();
|
||||
|
||||
/************************* Get force and torque acting on body ************************/
|
||||
//set wrench sum to zero
|
||||
const Wrench body_wrench = getBodyWrench(body, current.pose.orientation);
|
||||
|
||||
if (body.isGrounded()) {
|
||||
// make it stick to the ground until the magnitude of net external force on body exceeds its weight.
|
||||
float external_force_magnitude = body_wrench.force.squaredNorm();
|
||||
Vector3r weight = body.getMass() * body.getEnvironment().getState().gravity;
|
||||
float weight_magnitude = weight.squaredNorm();
|
||||
if (external_force_magnitude >= weight_magnitude) {
|
||||
//throttledLogOutput("*** Losing ground lock due to body_wrench " + VectorMath::toString(body_wrench.force), 0.1);
|
||||
body.setGrounded(false);
|
||||
}
|
||||
next_wrench.force = Vector3r::Zero();
|
||||
next_wrench.torque = Vector3r::Zero();
|
||||
next.accelerations.linear = Vector3r::Zero();
|
||||
}
|
||||
//
|
||||
//for disc of 1m radius which angular velocity translates to speed of light on tangent?
|
||||
if (next.twist.angular.squaredNorm() > EarthUtils::SpeedOfLight * EarthUtils::SpeedOfLight) { //speed of light
|
||||
next.twist.angular /= (next.twist.angular.norm() / EarthUtils::SpeedOfLight);
|
||||
next.accelerations.angular = Vector3r::Zero();
|
||||
else {
|
||||
//add linear drag due to velocity we had since last dt seconds + wind
|
||||
//drag vector magnitude is proportional to v^2, direction opposite of velocity
|
||||
//total drag is b*v + c*v*v but we ignore the first term as b << c (pg 44, Classical Mechanics, John Taylor)
|
||||
//To find the drag force, we find the magnitude in the body frame and unit vector direction in world frame
|
||||
avg_linear = current.twist.linear + current.accelerations.linear * (0.5f * dt_real);
|
||||
avg_angular = current.twist.angular + current.accelerations.angular * (0.5f * dt_real);
|
||||
const Wrench drag_wrench = getDragWrench(body, current.pose.orientation, avg_linear, avg_angular, wind);
|
||||
|
||||
next_wrench = body_wrench + drag_wrench;
|
||||
|
||||
//Utils::log(Utils::stringf("B-WRN %s: ", VectorMath::toString(body_wrench.force).c_str()));
|
||||
//Utils::log(Utils::stringf("D-WRN %s: ", VectorMath::toString(drag_wrench.force).c_str()));
|
||||
|
||||
/************************* Update accelerations due to force and torque ************************/
|
||||
//get new acceleration due to force - we'll use this acceleration in next time step
|
||||
|
||||
next.accelerations.linear = (next_wrench.force / body.getMass()) + body.getEnvironment().getState().gravity;
|
||||
}
|
||||
|
||||
if (body.isGrounded()) {
|
||||
// this stops vehicle from vibrating while it is on the ground doing nothing.
|
||||
next.accelerations.angular = Vector3r::Zero();
|
||||
next.twist.linear = Vector3r::Zero();
|
||||
next.twist.angular = Vector3r::Zero();
|
||||
}
|
||||
else {
|
||||
//get new angular acceleration
|
||||
//Euler's rotation equation: https://en.wikipedia.org/wiki/Euler's_equations_(body_dynamics)
|
||||
//we will use torque to find out the angular acceleration
|
||||
//angular momentum L = I * omega
|
||||
const Vector3r angular_momentum = body.getInertia() * avg_angular;
|
||||
const Vector3r angular_momentum_rate = next_wrench.torque - avg_angular.cross(angular_momentum);
|
||||
//new angular acceleration - we'll use this acceleration in next time step
|
||||
next.accelerations.angular = body.getInertiaInv() * angular_momentum_rate;
|
||||
|
||||
/************************* Update pose and twist after dt ************************/
|
||||
//Verlet integration: http://www.physics.udel.edu/~bnikolic/teaching/phys660/numerical_ode/node5.html
|
||||
next.twist.linear = current.twist.linear + (current.accelerations.linear + next.accelerations.linear) * (0.5f * dt_real);
|
||||
next.twist.angular = current.twist.angular + (current.accelerations.angular + next.accelerations.angular) * (0.5f * dt_real);
|
||||
|
||||
//if controller has bug, velocities can increase idenfinitely
|
||||
//so we need to clip this or everything will turn in to infinity/nans
|
||||
|
||||
if (next.twist.linear.squaredNorm() > EarthUtils::SpeedOfLight * EarthUtils::SpeedOfLight) { //speed of light
|
||||
next.twist.linear /= (next.twist.linear.norm() / EarthUtils::SpeedOfLight);
|
||||
next.accelerations.linear = Vector3r::Zero();
|
||||
}
|
||||
//
|
||||
//for disc of 1m radius which angular velocity translates to speed of light on tangent?
|
||||
if (next.twist.angular.squaredNorm() > EarthUtils::SpeedOfLight * EarthUtils::SpeedOfLight) { //speed of light
|
||||
next.twist.angular /= (next.twist.angular.norm() / EarthUtils::SpeedOfLight);
|
||||
next.accelerations.angular = Vector3r::Zero();
|
||||
}
|
||||
}
|
||||
|
||||
computeNextPose(dt, current.pose, avg_linear, avg_angular, next);
|
||||
|
||||
//Utils::log(Utils::stringf("N-VEL %s %f: ", VectorMath::toString(next.twist.linear).c_str(), dt));
|
||||
//Utils::log(Utils::stringf("N-POS %s %f: ", VectorMath::toString(next.pose.position).c_str(), dt));
|
||||
}
|
||||
|
||||
computeNextPose(dt, current.pose, avg_linear, avg_angular, next);
|
||||
static void computeNextPose(TTimeDelta dt, const Pose& current_pose, const Vector3r& avg_linear, const Vector3r& avg_angular, Kinematics::State& next)
|
||||
{
|
||||
real_T dt_real = static_cast<real_T>(dt);
|
||||
|
||||
//Utils::log(Utils::stringf("N-VEL %s %f: ", VectorMath::toString(next.twist.linear).c_str(), dt));
|
||||
//Utils::log(Utils::stringf("N-POS %s %f: ", VectorMath::toString(next.pose.position).c_str(), dt));
|
||||
next.pose.position = current_pose.position + avg_linear * dt_real;
|
||||
|
||||
}
|
||||
|
||||
static void computeNextPose(TTimeDelta dt, const Pose& current_pose, const Vector3r& avg_linear, const Vector3r& avg_angular, Kinematics::State& next)
|
||||
{
|
||||
real_T dt_real = static_cast<real_T>(dt);
|
||||
|
||||
next.pose.position = current_pose.position + avg_linear * dt_real;
|
||||
|
||||
//use angular velocty in body frame to calculate angular displacement in last dt seconds
|
||||
real_T angle_per_unit = avg_angular.norm();
|
||||
if (Utils::isDefinitelyGreaterThan(angle_per_unit, 0.0f)) {
|
||||
//convert change in angle to unit quaternion
|
||||
AngleAxisr angle_dt_aa = AngleAxisr(angle_per_unit * dt_real, avg_angular / angle_per_unit);
|
||||
Quaternionr angle_dt_q = Quaternionr(angle_dt_aa);
|
||||
/*
|
||||
//use angular velocty in body frame to calculate angular displacement in last dt seconds
|
||||
real_T angle_per_unit = avg_angular.norm();
|
||||
if (Utils::isDefinitelyGreaterThan(angle_per_unit, 0.0f)) {
|
||||
//convert change in angle to unit quaternion
|
||||
AngleAxisr angle_dt_aa = AngleAxisr(angle_per_unit * dt_real, avg_angular / angle_per_unit);
|
||||
Quaternionr angle_dt_q = Quaternionr(angle_dt_aa);
|
||||
/*
|
||||
Add change in angle to previous orientation.
|
||||
Proof that this is q0 * q1:
|
||||
If rotated vector is qx*v*qx' then qx is attitude
|
||||
|
|
@ -438,30 +436,30 @@ private:
|
|||
q0(q1(v)q1')q0'
|
||||
Thus new attitude is q0q1
|
||||
*/
|
||||
next.pose.orientation = current_pose.orientation * angle_dt_q;
|
||||
if (VectorMath::hasNan(next.pose.orientation)) {
|
||||
//Utils::DebugBreak();
|
||||
Utils::log("orientation had NaN!", Utils::kLogLevelError);
|
||||
next.pose.orientation = current_pose.orientation * angle_dt_q;
|
||||
if (VectorMath::hasNan(next.pose.orientation)) {
|
||||
//Utils::DebugBreak();
|
||||
Utils::log("orientation had NaN!", Utils::kLogLevelError);
|
||||
}
|
||||
|
||||
//re-normalize quaternion to avoid accumulating error
|
||||
next.pose.orientation.normalize();
|
||||
}
|
||||
else //no change in angle, because angular velocity is zero (normalized vector is undefined)
|
||||
next.pose.orientation = current_pose.orientation;
|
||||
}
|
||||
|
||||
//re-normalize quaternion to avoid accumulating error
|
||||
next.pose.orientation.normalize();
|
||||
}
|
||||
else //no change in angle, because angular velocity is zero (normalized vector is undefined)
|
||||
next.pose.orientation = current_pose.orientation;
|
||||
}
|
||||
private:
|
||||
static constexpr uint kCollisionResponseCycles = 1;
|
||||
static constexpr float kAxisTolerance = 0.25f;
|
||||
static constexpr float kRestingVelocityMax = 0.1f;
|
||||
static constexpr float kDragMinVelocity = 0.1f;
|
||||
|
||||
private:
|
||||
static constexpr uint kCollisionResponseCycles = 1;
|
||||
static constexpr float kAxisTolerance = 0.25f;
|
||||
static constexpr float kRestingVelocityMax = 0.1f;
|
||||
static constexpr float kDragMinVelocity = 0.1f;
|
||||
|
||||
std::stringstream debug_string_;
|
||||
bool enable_ground_lock_;
|
||||
TTimePoint last_message_time;
|
||||
Vector3r wind_;
|
||||
};
|
||||
|
||||
}} //namespace
|
||||
std::stringstream debug_string_;
|
||||
bool enable_ground_lock_;
|
||||
TTimePoint last_message_time;
|
||||
Vector3r wind_;
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -8,98 +8,103 @@
|
|||
#include "common/UpdatableObject.hpp"
|
||||
#include "common/CommonStructs.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
class Kinematics : public UpdatableObject {
|
||||
public:
|
||||
struct State {
|
||||
Pose pose;
|
||||
Twist twist;
|
||||
Accelerations accelerations;
|
||||
|
||||
static State zero()
|
||||
class Kinematics : public UpdatableObject
|
||||
{
|
||||
public:
|
||||
struct State
|
||||
{
|
||||
State zero_state;
|
||||
zero_state.pose.position = Vector3r::Zero();
|
||||
zero_state.pose.orientation = Quaternionr::Identity();
|
||||
zero_state.twist = Twist::zero();
|
||||
zero_state.accelerations = Accelerations::zero();
|
||||
Pose pose;
|
||||
Twist twist;
|
||||
Accelerations accelerations;
|
||||
|
||||
return zero_state;
|
||||
static State zero()
|
||||
{
|
||||
State zero_state;
|
||||
zero_state.pose.position = Vector3r::Zero();
|
||||
zero_state.pose.orientation = Quaternionr::Identity();
|
||||
zero_state.twist = Twist::zero();
|
||||
zero_state.accelerations = Accelerations::zero();
|
||||
|
||||
return zero_state;
|
||||
}
|
||||
};
|
||||
|
||||
Kinematics(const State& initial = State::zero())
|
||||
{
|
||||
initialize(initial);
|
||||
}
|
||||
void initialize(const State& initial)
|
||||
{
|
||||
initial_ = initial;
|
||||
}
|
||||
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
current_ = initial_;
|
||||
}
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
UpdatableObject::update();
|
||||
|
||||
//nothing to do because next state should be updated
|
||||
//by physics engine. The reason is that final state
|
||||
//needs to take in to account state of other objects as well,
|
||||
//for example, if collision occurs
|
||||
}
|
||||
virtual void reportState(StateReporter& reporter) override
|
||||
{
|
||||
//call base
|
||||
UpdatableObject::reportState(reporter);
|
||||
|
||||
reporter.writeValue("Position", current_.pose.position);
|
||||
reporter.writeValue("Orientation", current_.pose.orientation);
|
||||
reporter.writeValue("Lin-Vel", current_.twist.linear);
|
||||
reporter.writeValue("Lin-Accl", current_.accelerations.linear);
|
||||
reporter.writeValue("Ang-Vel", current_.twist.angular);
|
||||
reporter.writeValue("Ang-Accl", current_.accelerations.angular);
|
||||
}
|
||||
//*** End: UpdatableState implementation ***//
|
||||
|
||||
const Pose& getPose() const
|
||||
{
|
||||
return current_.pose;
|
||||
}
|
||||
void setPose(const Pose& pose)
|
||||
{
|
||||
current_.pose = pose;
|
||||
}
|
||||
const Twist& getTwist() const
|
||||
{
|
||||
return current_.twist;
|
||||
}
|
||||
void setTwist(const Twist& twist)
|
||||
{
|
||||
current_.twist = twist;
|
||||
}
|
||||
|
||||
const State& getState() const
|
||||
{
|
||||
return current_;
|
||||
}
|
||||
void setState(const State& state)
|
||||
{
|
||||
current_ = state;
|
||||
}
|
||||
const State& getInitialState() const
|
||||
{
|
||||
return initial_;
|
||||
}
|
||||
|
||||
private: //fields
|
||||
State initial_;
|
||||
State current_;
|
||||
};
|
||||
|
||||
Kinematics(const State& initial = State::zero())
|
||||
{
|
||||
initialize(initial);
|
||||
}
|
||||
void initialize(const State& initial)
|
||||
{
|
||||
initial_ = initial;
|
||||
}
|
||||
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
current_ = initial_;
|
||||
}
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
UpdatableObject::update();
|
||||
|
||||
//nothing to do because next state should be updated
|
||||
//by physics engine. The reason is that final state
|
||||
//needs to take in to account state of other objects as well,
|
||||
//for example, if collision occurs
|
||||
}
|
||||
virtual void reportState(StateReporter& reporter) override
|
||||
{
|
||||
//call base
|
||||
UpdatableObject::reportState(reporter);
|
||||
|
||||
reporter.writeValue("Position", current_.pose.position);
|
||||
reporter.writeValue("Orientation", current_.pose.orientation);
|
||||
reporter.writeValue("Lin-Vel", current_.twist.linear);
|
||||
reporter.writeValue("Lin-Accl", current_.accelerations.linear);
|
||||
reporter.writeValue("Ang-Vel", current_.twist.angular);
|
||||
reporter.writeValue("Ang-Accl", current_.accelerations.angular);
|
||||
}
|
||||
//*** End: UpdatableState implementation ***//
|
||||
|
||||
const Pose& getPose() const
|
||||
{
|
||||
return current_.pose;
|
||||
}
|
||||
void setPose(const Pose& pose)
|
||||
{
|
||||
current_.pose = pose;
|
||||
}
|
||||
const Twist& getTwist() const
|
||||
{
|
||||
return current_.twist;
|
||||
}
|
||||
void setTwist(const Twist& twist)
|
||||
{
|
||||
current_.twist = twist;
|
||||
}
|
||||
|
||||
const State& getState() const
|
||||
{
|
||||
return current_;
|
||||
}
|
||||
void setState(const State& state)
|
||||
{
|
||||
current_ = state;
|
||||
}
|
||||
const State& getInitialState() const
|
||||
{
|
||||
return initial_;
|
||||
}
|
||||
|
||||
private: //fields
|
||||
State initial_;
|
||||
State current_;
|
||||
};
|
||||
|
||||
}} //namespace
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -13,258 +13,257 @@
|
|||
#include <unordered_set>
|
||||
#include <exception>
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
class PhysicsBody : public UpdatableObject
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
public: //interface
|
||||
virtual real_T getRestitution() const = 0;
|
||||
virtual real_T getFriction() const = 0;
|
||||
|
||||
//derived class may return covariant type
|
||||
virtual uint wrenchVertexCount() const
|
||||
class PhysicsBody : public UpdatableObject
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
virtual PhysicsBodyVertex& getWrenchVertex(uint index)
|
||||
{
|
||||
unused(index);
|
||||
throw std::out_of_range("no physics vertex are available");
|
||||
}
|
||||
virtual const PhysicsBodyVertex& getWrenchVertex(uint index) const
|
||||
{
|
||||
unused(index);
|
||||
throw std::out_of_range("no physics vertex are available");
|
||||
}
|
||||
public: //interface
|
||||
virtual real_T getRestitution() const = 0;
|
||||
virtual real_T getFriction() const = 0;
|
||||
|
||||
virtual uint dragVertexCount() const
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
virtual PhysicsBodyVertex& getDragVertex(uint index)
|
||||
{
|
||||
unused(index);
|
||||
throw std::out_of_range("no physics vertex are available");
|
||||
}
|
||||
virtual const PhysicsBodyVertex& getDragVertex(uint index) const
|
||||
{
|
||||
unused(index);
|
||||
throw std::out_of_range("no physics vertex are available");
|
||||
}
|
||||
virtual void setCollisionInfo(const CollisionInfo& collision_info)
|
||||
{
|
||||
collision_info_ = collision_info;
|
||||
}
|
||||
|
||||
virtual void updateKinematics(const Kinematics::State& state)
|
||||
{
|
||||
if (VectorMath::hasNan(state.twist.linear)) {
|
||||
//Utils::DebugBreak();
|
||||
Utils::log("Linear velocity had NaN!", Utils::kLogLevelError);
|
||||
//derived class may return covariant type
|
||||
virtual uint wrenchVertexCount() const
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
virtual PhysicsBodyVertex& getWrenchVertex(uint index)
|
||||
{
|
||||
unused(index);
|
||||
throw std::out_of_range("no physics vertex are available");
|
||||
}
|
||||
virtual const PhysicsBodyVertex& getWrenchVertex(uint index) const
|
||||
{
|
||||
unused(index);
|
||||
throw std::out_of_range("no physics vertex are available");
|
||||
}
|
||||
|
||||
kinematics_->setState(state);
|
||||
kinematics_->update();
|
||||
}
|
||||
/**
|
||||
virtual uint dragVertexCount() const
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
virtual PhysicsBodyVertex& getDragVertex(uint index)
|
||||
{
|
||||
unused(index);
|
||||
throw std::out_of_range("no physics vertex are available");
|
||||
}
|
||||
virtual const PhysicsBodyVertex& getDragVertex(uint index) const
|
||||
{
|
||||
unused(index);
|
||||
throw std::out_of_range("no physics vertex are available");
|
||||
}
|
||||
virtual void setCollisionInfo(const CollisionInfo& collision_info)
|
||||
{
|
||||
collision_info_ = collision_info;
|
||||
}
|
||||
|
||||
virtual void updateKinematics(const Kinematics::State& state)
|
||||
{
|
||||
if (VectorMath::hasNan(state.twist.linear)) {
|
||||
//Utils::DebugBreak();
|
||||
Utils::log("Linear velocity had NaN!", Utils::kLogLevelError);
|
||||
}
|
||||
|
||||
kinematics_->setState(state);
|
||||
kinematics_->update();
|
||||
}
|
||||
/**
|
||||
* Update kinematics without a state
|
||||
*/
|
||||
virtual void updateKinematics()
|
||||
{
|
||||
kinematics_->update();
|
||||
}
|
||||
|
||||
|
||||
public: //methods
|
||||
//constructors
|
||||
PhysicsBody()
|
||||
{
|
||||
//allow default constructor with later call for initialize
|
||||
}
|
||||
PhysicsBody(real_T mass, const Matrix3x3r& inertia, Kinematics* kinematics, Environment* environment)
|
||||
{
|
||||
initialize(mass, inertia, kinematics, environment);
|
||||
}
|
||||
void initialize(real_T mass, const Matrix3x3r& inertia, Kinematics* kinematics, Environment* environment)
|
||||
{
|
||||
mass_ = mass;
|
||||
mass_inv_ = 1.0f / mass;
|
||||
inertia_ = inertia;
|
||||
inertia_inv_ = inertia_.inverse();
|
||||
environment_ = environment;
|
||||
environment_->setParent(this);
|
||||
kinematics_ = kinematics;
|
||||
kinematics_->setParent(this);
|
||||
}
|
||||
|
||||
//enable physics body detection
|
||||
virtual UpdatableObject* getPhysicsBody() override
|
||||
{
|
||||
return this;
|
||||
}
|
||||
|
||||
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
if (environment_)
|
||||
environment_->reset();
|
||||
wrench_ = Wrench::zero();
|
||||
collision_info_ = CollisionInfo();
|
||||
collision_response_ = CollisionResponse();
|
||||
grounded_ = false;
|
||||
|
||||
//update individual vertices
|
||||
for (uint vertex_index = 0; vertex_index < wrenchVertexCount(); ++vertex_index) {
|
||||
getWrenchVertex(vertex_index).reset();
|
||||
virtual void updateKinematics()
|
||||
{
|
||||
kinematics_->update();
|
||||
}
|
||||
for (uint vertex_index = 0; vertex_index < dragVertexCount(); ++vertex_index) {
|
||||
getDragVertex(vertex_index).reset();
|
||||
|
||||
public: //methods
|
||||
//constructors
|
||||
PhysicsBody()
|
||||
{
|
||||
//allow default constructor with later call for initialize
|
||||
}
|
||||
}
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
UpdatableObject::update();
|
||||
|
||||
//update individual vertices - each vertex takes control signal as input and
|
||||
//produces force and thrust as output
|
||||
for (uint vertex_index = 0; vertex_index < wrenchVertexCount(); ++vertex_index) {
|
||||
getWrenchVertex(vertex_index).update();
|
||||
PhysicsBody(real_T mass, const Matrix3x3r& inertia, Kinematics* kinematics, Environment* environment)
|
||||
{
|
||||
initialize(mass, inertia, kinematics, environment);
|
||||
}
|
||||
for (uint vertex_index = 0; vertex_index < dragVertexCount(); ++vertex_index) {
|
||||
getDragVertex(vertex_index).update();
|
||||
void initialize(real_T mass, const Matrix3x3r& inertia, Kinematics* kinematics, Environment* environment)
|
||||
{
|
||||
mass_ = mass;
|
||||
mass_inv_ = 1.0f / mass;
|
||||
inertia_ = inertia;
|
||||
inertia_inv_ = inertia_.inverse();
|
||||
environment_ = environment;
|
||||
environment_->setParent(this);
|
||||
kinematics_ = kinematics;
|
||||
kinematics_->setParent(this);
|
||||
}
|
||||
}
|
||||
|
||||
virtual void reportState(StateReporter& reporter) override
|
||||
{
|
||||
//call base
|
||||
UpdatableObject::reportState(reporter);
|
||||
//enable physics body detection
|
||||
virtual UpdatableObject* getPhysicsBody() override
|
||||
{
|
||||
return this;
|
||||
}
|
||||
|
||||
reporter.writeHeading("Kinematics");
|
||||
}
|
||||
//*** End: UpdatableState implementation ***//
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
if (environment_)
|
||||
environment_->reset();
|
||||
wrench_ = Wrench::zero();
|
||||
collision_info_ = CollisionInfo();
|
||||
collision_response_ = CollisionResponse();
|
||||
grounded_ = false;
|
||||
|
||||
//update individual vertices
|
||||
for (uint vertex_index = 0; vertex_index < wrenchVertexCount(); ++vertex_index) {
|
||||
getWrenchVertex(vertex_index).reset();
|
||||
}
|
||||
for (uint vertex_index = 0; vertex_index < dragVertexCount(); ++vertex_index) {
|
||||
getDragVertex(vertex_index).reset();
|
||||
}
|
||||
}
|
||||
|
||||
//getters
|
||||
real_T getMass() const
|
||||
{
|
||||
return mass_;
|
||||
}
|
||||
real_T getMassInv() const
|
||||
{
|
||||
return mass_inv_;
|
||||
}
|
||||
const Matrix3x3r& getInertia() const
|
||||
{
|
||||
return inertia_;
|
||||
}
|
||||
const Matrix3x3r& getInertiaInv() const
|
||||
{
|
||||
return inertia_inv_;
|
||||
}
|
||||
virtual void update() override
|
||||
{
|
||||
UpdatableObject::update();
|
||||
|
||||
const Pose& getPose() const
|
||||
{
|
||||
return kinematics_->getPose();
|
||||
}
|
||||
void setPose(const Pose& pose)
|
||||
{
|
||||
return kinematics_->setPose(pose);
|
||||
}
|
||||
const Twist& getTwist() const
|
||||
{
|
||||
return kinematics_->getTwist();
|
||||
}
|
||||
void setTwist(const Twist& twist)
|
||||
{
|
||||
return kinematics_->setTwist(twist);
|
||||
}
|
||||
//update individual vertices - each vertex takes control signal as input and
|
||||
//produces force and thrust as output
|
||||
for (uint vertex_index = 0; vertex_index < wrenchVertexCount(); ++vertex_index) {
|
||||
getWrenchVertex(vertex_index).update();
|
||||
}
|
||||
for (uint vertex_index = 0; vertex_index < dragVertexCount(); ++vertex_index) {
|
||||
getDragVertex(vertex_index).update();
|
||||
}
|
||||
}
|
||||
|
||||
virtual void reportState(StateReporter& reporter) override
|
||||
{
|
||||
//call base
|
||||
UpdatableObject::reportState(reporter);
|
||||
|
||||
const Kinematics::State& getKinematics() const
|
||||
{
|
||||
return kinematics_->getState();
|
||||
}
|
||||
reporter.writeHeading("Kinematics");
|
||||
}
|
||||
//*** End: UpdatableState implementation ***//
|
||||
|
||||
const Kinematics::State& getInitialKinematics() const
|
||||
{
|
||||
return kinematics_->getInitialState();
|
||||
}
|
||||
const Environment& getEnvironment() const
|
||||
{
|
||||
return *environment_;
|
||||
}
|
||||
Environment& getEnvironment()
|
||||
{
|
||||
return *environment_;
|
||||
}
|
||||
bool hasEnvironment() const
|
||||
{
|
||||
return environment_ != nullptr;
|
||||
}
|
||||
const Wrench& getWrench() const
|
||||
{
|
||||
return wrench_;
|
||||
}
|
||||
void setWrench(const Wrench& wrench)
|
||||
{
|
||||
wrench_ = wrench;
|
||||
}
|
||||
const CollisionInfo& getCollisionInfo() const
|
||||
{
|
||||
return collision_info_;
|
||||
}
|
||||
//getters
|
||||
real_T getMass() const
|
||||
{
|
||||
return mass_;
|
||||
}
|
||||
real_T getMassInv() const
|
||||
{
|
||||
return mass_inv_;
|
||||
}
|
||||
const Matrix3x3r& getInertia() const
|
||||
{
|
||||
return inertia_;
|
||||
}
|
||||
const Matrix3x3r& getInertiaInv() const
|
||||
{
|
||||
return inertia_inv_;
|
||||
}
|
||||
|
||||
const CollisionResponse& getCollisionResponseInfo() const
|
||||
{
|
||||
return collision_response_;
|
||||
}
|
||||
CollisionResponse& getCollisionResponseInfo()
|
||||
{
|
||||
return collision_response_;
|
||||
}
|
||||
const Pose& getPose() const
|
||||
{
|
||||
return kinematics_->getPose();
|
||||
}
|
||||
void setPose(const Pose& pose)
|
||||
{
|
||||
return kinematics_->setPose(pose);
|
||||
}
|
||||
const Twist& getTwist() const
|
||||
{
|
||||
return kinematics_->getTwist();
|
||||
}
|
||||
void setTwist(const Twist& twist)
|
||||
{
|
||||
return kinematics_->setTwist(twist);
|
||||
}
|
||||
|
||||
bool isGrounded() const
|
||||
{
|
||||
return grounded_;
|
||||
}
|
||||
void setGrounded(bool grounded)
|
||||
{
|
||||
grounded_ = grounded;
|
||||
}
|
||||
const Kinematics::State& getKinematics() const
|
||||
{
|
||||
return kinematics_->getState();
|
||||
}
|
||||
|
||||
void lock()
|
||||
{
|
||||
mutex_.lock();
|
||||
}
|
||||
const Kinematics::State& getInitialKinematics() const
|
||||
{
|
||||
return kinematics_->getInitialState();
|
||||
}
|
||||
const Environment& getEnvironment() const
|
||||
{
|
||||
return *environment_;
|
||||
}
|
||||
Environment& getEnvironment()
|
||||
{
|
||||
return *environment_;
|
||||
}
|
||||
bool hasEnvironment() const
|
||||
{
|
||||
return environment_ != nullptr;
|
||||
}
|
||||
const Wrench& getWrench() const
|
||||
{
|
||||
return wrench_;
|
||||
}
|
||||
void setWrench(const Wrench& wrench)
|
||||
{
|
||||
wrench_ = wrench;
|
||||
}
|
||||
const CollisionInfo& getCollisionInfo() const
|
||||
{
|
||||
return collision_info_;
|
||||
}
|
||||
|
||||
void unlock()
|
||||
{
|
||||
mutex_.unlock();
|
||||
}
|
||||
const CollisionResponse& getCollisionResponseInfo() const
|
||||
{
|
||||
return collision_response_;
|
||||
}
|
||||
CollisionResponse& getCollisionResponseInfo()
|
||||
{
|
||||
return collision_response_;
|
||||
}
|
||||
|
||||
public:
|
||||
//for use in physics engine: //TODO: use getter/setter or friend method?
|
||||
TTimePoint last_kinematics_time;
|
||||
bool isGrounded() const
|
||||
{
|
||||
return grounded_;
|
||||
}
|
||||
void setGrounded(bool grounded)
|
||||
{
|
||||
grounded_ = grounded;
|
||||
}
|
||||
|
||||
private:
|
||||
real_T mass_, mass_inv_;
|
||||
Matrix3x3r inertia_, inertia_inv_;
|
||||
void lock()
|
||||
{
|
||||
mutex_.lock();
|
||||
}
|
||||
|
||||
Kinematics* kinematics_ = nullptr;
|
||||
Environment* environment_ = nullptr;
|
||||
void unlock()
|
||||
{
|
||||
mutex_.unlock();
|
||||
}
|
||||
|
||||
//force is in world frame but torque is not
|
||||
Wrench wrench_;
|
||||
public:
|
||||
//for use in physics engine: //TODO: use getter/setter or friend method?
|
||||
TTimePoint last_kinematics_time;
|
||||
|
||||
CollisionInfo collision_info_;
|
||||
CollisionResponse collision_response_;
|
||||
private:
|
||||
real_T mass_, mass_inv_;
|
||||
Matrix3x3r inertia_, inertia_inv_;
|
||||
|
||||
bool grounded_ = false;
|
||||
std::mutex mutex_;
|
||||
};
|
||||
Kinematics* kinematics_ = nullptr;
|
||||
Environment* environment_ = nullptr;
|
||||
|
||||
}} //namespace
|
||||
//force is in world frame but torque is not
|
||||
Wrench wrench_;
|
||||
|
||||
CollisionInfo collision_info_;
|
||||
CollisionResponse collision_response_;
|
||||
|
||||
bool grounded_ = false;
|
||||
std::mutex mutex_;
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -8,91 +8,93 @@
|
|||
#include "common/Common.hpp"
|
||||
#include "common/CommonStructs.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
class PhysicsBodyVertex : public UpdatableObject {
|
||||
protected:
|
||||
virtual void setWrench(Wrench& wrench)
|
||||
class PhysicsBodyVertex : public UpdatableObject
|
||||
{
|
||||
unused(wrench);
|
||||
//derived class should override if this is force/torque
|
||||
//generating vertex
|
||||
}
|
||||
public:
|
||||
real_T getDragFactor() const
|
||||
{
|
||||
return drag_factor_;
|
||||
}
|
||||
void setDragFactor(real_T val)
|
||||
{
|
||||
drag_factor_ = val;
|
||||
}
|
||||
protected:
|
||||
virtual void setWrench(Wrench& wrench)
|
||||
{
|
||||
unused(wrench);
|
||||
//derived class should override if this is force/torque
|
||||
//generating vertex
|
||||
}
|
||||
|
||||
PhysicsBodyVertex()
|
||||
{
|
||||
//allow default constructor with later call for initialize
|
||||
}
|
||||
PhysicsBodyVertex(const Vector3r& position, const Vector3r& normal, real_T drag_factor = 0)
|
||||
{
|
||||
initialize(position, normal, drag_factor);
|
||||
}
|
||||
void initialize(const Vector3r& position, const Vector3r& normal, real_T drag_factor = 0)
|
||||
{
|
||||
initial_position_ = position;
|
||||
initial_normal_ = normal;
|
||||
drag_factor_ = drag_factor;
|
||||
}
|
||||
public:
|
||||
real_T getDragFactor() const
|
||||
{
|
||||
return drag_factor_;
|
||||
}
|
||||
void setDragFactor(real_T val)
|
||||
{
|
||||
drag_factor_ = val;
|
||||
}
|
||||
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
position_ = initial_position_;
|
||||
normal_ = initial_normal_;
|
||||
PhysicsBodyVertex()
|
||||
{
|
||||
//allow default constructor with later call for initialize
|
||||
}
|
||||
PhysicsBodyVertex(const Vector3r& position, const Vector3r& normal, real_T drag_factor = 0)
|
||||
{
|
||||
initialize(position, normal, drag_factor);
|
||||
}
|
||||
void initialize(const Vector3r& position, const Vector3r& normal, real_T drag_factor = 0)
|
||||
{
|
||||
initial_position_ = position;
|
||||
initial_normal_ = normal;
|
||||
drag_factor_ = drag_factor;
|
||||
}
|
||||
|
||||
current_wrench_ = Wrench::zero();
|
||||
}
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
position_ = initial_position_;
|
||||
normal_ = initial_normal_;
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
UpdatableObject::update();
|
||||
current_wrench_ = Wrench::zero();
|
||||
}
|
||||
|
||||
setWrench(current_wrench_);
|
||||
}
|
||||
//*** End: UpdatableState implementation ***//
|
||||
virtual void update() override
|
||||
{
|
||||
UpdatableObject::update();
|
||||
|
||||
setWrench(current_wrench_);
|
||||
}
|
||||
//*** End: UpdatableState implementation ***//
|
||||
|
||||
//getters, setters
|
||||
Vector3r getPosition() const
|
||||
{
|
||||
return position_;
|
||||
}
|
||||
void setPosition(const Vector3r& position)
|
||||
{
|
||||
position_ = position;
|
||||
}
|
||||
//getters, setters
|
||||
Vector3r getPosition() const
|
||||
{
|
||||
return position_;
|
||||
}
|
||||
void setPosition(const Vector3r& position)
|
||||
{
|
||||
position_ = position;
|
||||
}
|
||||
|
||||
Vector3r getNormal() const
|
||||
{
|
||||
return normal_;
|
||||
}
|
||||
void setNormal(const Vector3r& normal)
|
||||
{
|
||||
normal_ = normal;
|
||||
}
|
||||
|
||||
Vector3r getNormal() const
|
||||
{
|
||||
return normal_;
|
||||
}
|
||||
void setNormal(const Vector3r& normal)
|
||||
{
|
||||
normal_ = normal;
|
||||
}
|
||||
Wrench getWrench() const
|
||||
{
|
||||
return current_wrench_;
|
||||
}
|
||||
|
||||
|
||||
Wrench getWrench() const
|
||||
{
|
||||
return current_wrench_;
|
||||
}
|
||||
|
||||
private:
|
||||
Vector3r initial_position_, position_;
|
||||
Vector3r initial_normal_, normal_;
|
||||
Wrench current_wrench_;
|
||||
real_T drag_factor_;
|
||||
};
|
||||
|
||||
}} //namespace
|
||||
private:
|
||||
Vector3r initial_position_, position_;
|
||||
Vector3r initial_normal_, normal_;
|
||||
Wrench current_wrench_;
|
||||
real_T drag_factor_;
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -8,25 +8,27 @@
|
|||
#include "common/Common.hpp"
|
||||
#include "PhysicsBody.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
class PhysicsEngineBase : public UpdatableContainer<PhysicsBody*>
|
||||
namespace msr
|
||||
{
|
||||
public:
|
||||
virtual void update() override
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
class PhysicsEngineBase : public UpdatableContainer<PhysicsBody*>
|
||||
{
|
||||
UpdatableObject::update();
|
||||
}
|
||||
public:
|
||||
virtual void update() override
|
||||
{
|
||||
UpdatableObject::update();
|
||||
}
|
||||
|
||||
virtual void reportState(StateReporter& reporter) override
|
||||
{
|
||||
unused(reporter);
|
||||
//default nothing to report for physics engine
|
||||
}
|
||||
virtual void reportState(StateReporter& reporter) override
|
||||
{
|
||||
unused(reporter);
|
||||
//default nothing to report for physics engine
|
||||
}
|
||||
|
||||
virtual void setWind(const Vector3r& wind) { unused(wind); };
|
||||
};
|
||||
|
||||
|
||||
}} //namespace
|
||||
virtual void setWind(const Vector3r& wind) { unused(wind); };
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -11,129 +11,129 @@
|
|||
#include "World.hpp"
|
||||
#include "common/StateReporterWrapper.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
class PhysicsWorld : UpdatableObject
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
public:
|
||||
PhysicsWorld(std::unique_ptr<PhysicsEngineBase> physics_engine, const std::vector<UpdatableObject*>& bodies,
|
||||
uint64_t update_period_nanos = 3000000LL, bool state_reporter_enabled = false,
|
||||
bool start_async_updator = true
|
||||
)
|
||||
: world_(std::move(physics_engine))
|
||||
{
|
||||
setName("PhysicsWorld");
|
||||
enableStateReport(state_reporter_enabled);
|
||||
update_period_nanos_ = update_period_nanos;
|
||||
initializeWorld(bodies, start_async_updator);
|
||||
}
|
||||
|
||||
void lock()
|
||||
class PhysicsWorld : UpdatableObject
|
||||
{
|
||||
world_.lock();
|
||||
}
|
||||
|
||||
void unlock()
|
||||
{
|
||||
world_.unlock();
|
||||
}
|
||||
|
||||
void reset()
|
||||
{
|
||||
lock();
|
||||
world_.reset();
|
||||
unlock();
|
||||
}
|
||||
|
||||
void addBody(UpdatableObject* body)
|
||||
{
|
||||
lock();
|
||||
world_.insert(body);
|
||||
unlock();
|
||||
}
|
||||
|
||||
uint64_t getUpdatePeriodNanos() const
|
||||
{
|
||||
return update_period_nanos_;
|
||||
}
|
||||
|
||||
void startAsyncUpdator()
|
||||
{
|
||||
world_.startAsyncUpdator(update_period_nanos_);
|
||||
}
|
||||
void stopAsyncUpdator()
|
||||
{
|
||||
world_.stopAsyncUpdator();
|
||||
}
|
||||
|
||||
|
||||
void enableStateReport(bool is_enabled)
|
||||
{
|
||||
reporter_.setEnable(is_enabled);
|
||||
}
|
||||
|
||||
void updateStateReport()
|
||||
{
|
||||
if (reporter_.canReport()) {
|
||||
reporter_.clearReport();
|
||||
world_.reportState(*reporter_.getReporter());
|
||||
public:
|
||||
PhysicsWorld(std::unique_ptr<PhysicsEngineBase> physics_engine, const std::vector<UpdatableObject*>& bodies,
|
||||
uint64_t update_period_nanos = 3000000LL, bool state_reporter_enabled = false,
|
||||
bool start_async_updator = true)
|
||||
: world_(std::move(physics_engine))
|
||||
{
|
||||
setName("PhysicsWorld");
|
||||
enableStateReport(state_reporter_enabled);
|
||||
update_period_nanos_ = update_period_nanos;
|
||||
initializeWorld(bodies, start_async_updator);
|
||||
}
|
||||
}
|
||||
|
||||
std::string getDebugReport()
|
||||
{
|
||||
return reporter_.getOutput();
|
||||
}
|
||||
void lock()
|
||||
{
|
||||
world_.lock();
|
||||
}
|
||||
|
||||
void pause(bool is_paused)
|
||||
{
|
||||
world_.pause(is_paused);
|
||||
}
|
||||
void unlock()
|
||||
{
|
||||
world_.unlock();
|
||||
}
|
||||
|
||||
bool isPaused() const
|
||||
{
|
||||
return world_.isPaused();
|
||||
}
|
||||
void reset()
|
||||
{
|
||||
lock();
|
||||
world_.reset();
|
||||
unlock();
|
||||
}
|
||||
|
||||
void continueForTime(double seconds)
|
||||
{
|
||||
world_.continueForTime(seconds);
|
||||
}
|
||||
void addBody(UpdatableObject* body)
|
||||
{
|
||||
lock();
|
||||
world_.insert(body);
|
||||
unlock();
|
||||
}
|
||||
|
||||
void continueForFrames(uint32_t frames)
|
||||
{
|
||||
world_.continueForFrames(frames);
|
||||
}
|
||||
uint64_t getUpdatePeriodNanos() const
|
||||
{
|
||||
return update_period_nanos_;
|
||||
}
|
||||
|
||||
void setFrameNumber(uint32_t frameNumber)
|
||||
{
|
||||
world_.setFrameNumber(frameNumber);
|
||||
}
|
||||
|
||||
void resetImplementation() override {}
|
||||
|
||||
private:
|
||||
void initializeWorld(const std::vector<UpdatableObject*>& bodies, bool start_async_updator)
|
||||
{
|
||||
reporter_.initialize(false);
|
||||
world_.insert(&reporter_);
|
||||
|
||||
for(size_t bi = 0; bi < bodies.size(); bi++)
|
||||
world_.insert(bodies.at(bi));
|
||||
|
||||
world_.reset();
|
||||
|
||||
if (start_async_updator)
|
||||
void startAsyncUpdator()
|
||||
{
|
||||
world_.startAsyncUpdator(update_period_nanos_);
|
||||
}
|
||||
}
|
||||
void stopAsyncUpdator()
|
||||
{
|
||||
world_.stopAsyncUpdator();
|
||||
}
|
||||
|
||||
private:
|
||||
std::vector<UpdatableObject*> bodies_;
|
||||
StateReporterWrapper reporter_;
|
||||
World world_;
|
||||
uint64_t update_period_nanos_;
|
||||
};
|
||||
void enableStateReport(bool is_enabled)
|
||||
{
|
||||
reporter_.setEnable(is_enabled);
|
||||
}
|
||||
|
||||
void updateStateReport()
|
||||
{
|
||||
if (reporter_.canReport()) {
|
||||
reporter_.clearReport();
|
||||
world_.reportState(*reporter_.getReporter());
|
||||
}
|
||||
}
|
||||
|
||||
}} //namespace
|
||||
std::string getDebugReport()
|
||||
{
|
||||
return reporter_.getOutput();
|
||||
}
|
||||
|
||||
void pause(bool is_paused)
|
||||
{
|
||||
world_.pause(is_paused);
|
||||
}
|
||||
|
||||
bool isPaused() const
|
||||
{
|
||||
return world_.isPaused();
|
||||
}
|
||||
|
||||
void continueForTime(double seconds)
|
||||
{
|
||||
world_.continueForTime(seconds);
|
||||
}
|
||||
|
||||
void continueForFrames(uint32_t frames)
|
||||
{
|
||||
world_.continueForFrames(frames);
|
||||
}
|
||||
|
||||
void setFrameNumber(uint32_t frameNumber)
|
||||
{
|
||||
world_.setFrameNumber(frameNumber);
|
||||
}
|
||||
|
||||
void resetImplementation() override {}
|
||||
|
||||
private:
|
||||
void initializeWorld(const std::vector<UpdatableObject*>& bodies, bool start_async_updator)
|
||||
{
|
||||
reporter_.initialize(false);
|
||||
world_.insert(&reporter_);
|
||||
|
||||
for (size_t bi = 0; bi < bodies.size(); bi++)
|
||||
world_.insert(bodies.at(bi));
|
||||
|
||||
world_.reset();
|
||||
|
||||
if (start_async_updator)
|
||||
world_.startAsyncUpdator(update_period_nanos_);
|
||||
}
|
||||
|
||||
private:
|
||||
std::vector<UpdatableObject*> bodies_;
|
||||
StateReporterWrapper reporter_;
|
||||
World world_;
|
||||
uint64_t update_period_nanos_;
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -12,158 +12,161 @@
|
|||
#include "common/common_utils/ScheduledExecutor.hpp"
|
||||
#include "common/ClockFactory.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
class World : public UpdatableContainer<UpdatableObject*>
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
public:
|
||||
World(std::unique_ptr<PhysicsEngineBase> physics_engine)
|
||||
: physics_engine_(std::move(physics_engine))
|
||||
{
|
||||
World::clear();
|
||||
setName("World");
|
||||
physics_engine_->setParent(this);
|
||||
if (physics_engine)
|
||||
physics_engine_->clear();
|
||||
}
|
||||
|
||||
//override updatable interface so we can synchronize physics engine
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
class World : public UpdatableContainer<UpdatableObject*>
|
||||
{
|
||||
UpdatableContainer::resetImplementation();
|
||||
|
||||
if (physics_engine_)
|
||||
physics_engine_->reset();
|
||||
}
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
ClockFactory::get()->step();
|
||||
|
||||
//first update our objects
|
||||
UpdatableContainer::update();
|
||||
|
||||
//now update kinematics state
|
||||
if (physics_engine_)
|
||||
physics_engine_->update();
|
||||
}
|
||||
|
||||
virtual void reportState(StateReporter& reporter) override
|
||||
{
|
||||
reporter.writeValue("Sleep", 1.0f / executor_.getSleepTimeAvg());
|
||||
if (physics_engine_)
|
||||
physics_engine_->reportState(reporter);
|
||||
|
||||
//call base
|
||||
UpdatableContainer::reportState(reporter);
|
||||
}
|
||||
//*** End: UpdatableState implementation ***//
|
||||
|
||||
//override membership modification methods so we can synchronize physics engine
|
||||
virtual void clear() override
|
||||
{
|
||||
if (physics_engine_)
|
||||
physics_engine_->clear();
|
||||
UpdatableContainer::clear();
|
||||
}
|
||||
|
||||
virtual void insert(UpdatableObject* member) override
|
||||
{
|
||||
if (physics_engine_ && member->getPhysicsBody() != nullptr)
|
||||
physics_engine_->insert(static_cast<PhysicsBody*>(member->getPhysicsBody()));
|
||||
|
||||
UpdatableContainer::insert(member);
|
||||
}
|
||||
|
||||
virtual void erase_remove(UpdatableObject* member) override
|
||||
{
|
||||
if (physics_engine_ && member->getPhysicsBody() != nullptr)
|
||||
physics_engine_->erase_remove(static_cast<PhysicsBody*>(
|
||||
member->getPhysicsBody()));
|
||||
|
||||
UpdatableContainer::erase_remove(member);
|
||||
}
|
||||
|
||||
//async updater thread
|
||||
void startAsyncUpdator(uint64_t period)
|
||||
{
|
||||
//TODO: probably we shouldn't be passing around fixed period
|
||||
executor_.initialize(std::bind(&World::worldUpdatorAsync, this, std::placeholders::_1), period);
|
||||
executor_.start();
|
||||
}
|
||||
void stopAsyncUpdator()
|
||||
{
|
||||
executor_.stop();
|
||||
}
|
||||
void lock()
|
||||
{
|
||||
executor_.lock();
|
||||
}
|
||||
void unlock()
|
||||
{
|
||||
executor_.unlock();
|
||||
}
|
||||
|
||||
virtual ~World()
|
||||
{
|
||||
executor_.stop();
|
||||
}
|
||||
|
||||
void pause(bool is_paused)
|
||||
{
|
||||
executor_.pause(is_paused);
|
||||
}
|
||||
|
||||
bool isPaused() const
|
||||
{
|
||||
return executor_.isPaused();
|
||||
}
|
||||
|
||||
void pauseForTime(double seconds)
|
||||
{
|
||||
executor_.pauseForTime(seconds);
|
||||
}
|
||||
|
||||
void continueForTime(double seconds)
|
||||
{
|
||||
executor_.continueForTime(seconds);
|
||||
}
|
||||
|
||||
void continueForFrames(uint32_t frames)
|
||||
{
|
||||
executor_.continueForFrames(frames);
|
||||
}
|
||||
|
||||
void setFrameNumber(uint32_t frameNumber)
|
||||
{
|
||||
executor_.setFrameNumber(frameNumber);
|
||||
}
|
||||
|
||||
private:
|
||||
bool worldUpdatorAsync(uint64_t dt_nanos)
|
||||
{
|
||||
unused(dt_nanos);
|
||||
|
||||
try {
|
||||
update();
|
||||
}
|
||||
catch(const std::exception& ex) {
|
||||
//Utils::DebugBreak();
|
||||
Utils::log(Utils::stringf("Exception occurred while updating world: %s", ex.what()), Utils::kLogLevelError);
|
||||
}
|
||||
catch(...) {
|
||||
//Utils::DebugBreak();
|
||||
Utils::log("Exception occurred while updating world", Utils::kLogLevelError);
|
||||
public:
|
||||
World(std::unique_ptr<PhysicsEngineBase> physics_engine)
|
||||
: physics_engine_(std::move(physics_engine))
|
||||
{
|
||||
World::clear();
|
||||
setName("World");
|
||||
physics_engine_->setParent(this);
|
||||
if (physics_engine)
|
||||
physics_engine_->clear();
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
//override updatable interface so we can synchronize physics engine
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
UpdatableContainer::resetImplementation();
|
||||
|
||||
private:
|
||||
std::unique_ptr<PhysicsEngineBase> physics_engine_ = nullptr;
|
||||
common_utils::ScheduledExecutor executor_;
|
||||
};
|
||||
if (physics_engine_)
|
||||
physics_engine_->reset();
|
||||
}
|
||||
|
||||
}} //namespace
|
||||
virtual void update() override
|
||||
{
|
||||
ClockFactory::get()->step();
|
||||
|
||||
//first update our objects
|
||||
UpdatableContainer::update();
|
||||
|
||||
//now update kinematics state
|
||||
if (physics_engine_)
|
||||
physics_engine_->update();
|
||||
}
|
||||
|
||||
virtual void reportState(StateReporter& reporter) override
|
||||
{
|
||||
reporter.writeValue("Sleep", 1.0f / executor_.getSleepTimeAvg());
|
||||
if (physics_engine_)
|
||||
physics_engine_->reportState(reporter);
|
||||
|
||||
//call base
|
||||
UpdatableContainer::reportState(reporter);
|
||||
}
|
||||
//*** End: UpdatableState implementation ***//
|
||||
|
||||
//override membership modification methods so we can synchronize physics engine
|
||||
virtual void clear() override
|
||||
{
|
||||
if (physics_engine_)
|
||||
physics_engine_->clear();
|
||||
UpdatableContainer::clear();
|
||||
}
|
||||
|
||||
virtual void insert(UpdatableObject* member) override
|
||||
{
|
||||
if (physics_engine_ && member->getPhysicsBody() != nullptr)
|
||||
physics_engine_->insert(static_cast<PhysicsBody*>(member->getPhysicsBody()));
|
||||
|
||||
UpdatableContainer::insert(member);
|
||||
}
|
||||
|
||||
virtual void erase_remove(UpdatableObject* member) override
|
||||
{
|
||||
if (physics_engine_ && member->getPhysicsBody() != nullptr)
|
||||
physics_engine_->erase_remove(static_cast<PhysicsBody*>(
|
||||
member->getPhysicsBody()));
|
||||
|
||||
UpdatableContainer::erase_remove(member);
|
||||
}
|
||||
|
||||
//async updater thread
|
||||
void startAsyncUpdator(uint64_t period)
|
||||
{
|
||||
//TODO: probably we shouldn't be passing around fixed period
|
||||
executor_.initialize(std::bind(&World::worldUpdatorAsync, this, std::placeholders::_1), period);
|
||||
executor_.start();
|
||||
}
|
||||
void stopAsyncUpdator()
|
||||
{
|
||||
executor_.stop();
|
||||
}
|
||||
void lock()
|
||||
{
|
||||
executor_.lock();
|
||||
}
|
||||
void unlock()
|
||||
{
|
||||
executor_.unlock();
|
||||
}
|
||||
|
||||
virtual ~World()
|
||||
{
|
||||
executor_.stop();
|
||||
}
|
||||
|
||||
void pause(bool is_paused)
|
||||
{
|
||||
executor_.pause(is_paused);
|
||||
}
|
||||
|
||||
bool isPaused() const
|
||||
{
|
||||
return executor_.isPaused();
|
||||
}
|
||||
|
||||
void pauseForTime(double seconds)
|
||||
{
|
||||
executor_.pauseForTime(seconds);
|
||||
}
|
||||
|
||||
void continueForTime(double seconds)
|
||||
{
|
||||
executor_.continueForTime(seconds);
|
||||
}
|
||||
|
||||
void continueForFrames(uint32_t frames)
|
||||
{
|
||||
executor_.continueForFrames(frames);
|
||||
}
|
||||
|
||||
void setFrameNumber(uint32_t frameNumber)
|
||||
{
|
||||
executor_.setFrameNumber(frameNumber);
|
||||
}
|
||||
|
||||
private:
|
||||
bool worldUpdatorAsync(uint64_t dt_nanos)
|
||||
{
|
||||
unused(dt_nanos);
|
||||
|
||||
try {
|
||||
update();
|
||||
}
|
||||
catch (const std::exception& ex) {
|
||||
//Utils::DebugBreak();
|
||||
Utils::log(Utils::stringf("Exception occurred while updating world: %s", ex.what()), Utils::kLogLevelError);
|
||||
}
|
||||
catch (...) {
|
||||
//Utils::DebugBreak();
|
||||
Utils::log("Exception occurred while updating world", Utils::kLogLevelError);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
private:
|
||||
std::unique_ptr<PhysicsEngineBase> physics_engine_ = nullptr;
|
||||
common_utils::ScheduledExecutor executor_;
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -6,65 +6,68 @@
|
|||
|
||||
#include "common/Common.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
class CubeGeoFence : public IGeoFence {
|
||||
private:
|
||||
Vector3r point_min_, point_max_, point_center_;
|
||||
float distance_accuracy_;
|
||||
|
||||
public:
|
||||
CubeGeoFence(const Vector3r& point_min, const Vector3r& point_max, float distance_accuracy)
|
||||
: point_min_(point_min), point_max_(point_max), distance_accuracy_(distance_accuracy)
|
||||
class CubeGeoFence : public IGeoFence
|
||||
{
|
||||
calculateCenter();
|
||||
private:
|
||||
Vector3r point_min_, point_max_, point_center_;
|
||||
float distance_accuracy_;
|
||||
|
||||
Utils::logMessage("CubeGeoFence: %s", toString().c_str());
|
||||
}
|
||||
public:
|
||||
CubeGeoFence(const Vector3r& point_min, const Vector3r& point_max, float distance_accuracy)
|
||||
: point_min_(point_min), point_max_(point_max), distance_accuracy_(distance_accuracy)
|
||||
{
|
||||
calculateCenter();
|
||||
|
||||
void setBoundry(const Vector3r& origin, float xy_length, float max_z, float min_z) override
|
||||
{
|
||||
point_min_ = Vector3r(-xy_length, -xy_length, 0) + origin;
|
||||
point_min_[2] = max_z;
|
||||
|
||||
point_max_ = Vector3r(xy_length, xy_length, 0) + origin;
|
||||
point_max_[2] = min_z;
|
||||
|
||||
calculateCenter();
|
||||
|
||||
Utils::logMessage("CubeGeoFence: %s", toString().c_str());
|
||||
}
|
||||
|
||||
void checkFence(const Vector3r& cur_loc, const Vector3r& dest_loc,
|
||||
bool& in_fence, bool& allow) override
|
||||
{
|
||||
in_fence = dest_loc[0] >= point_min_[0] && dest_loc[1] >= point_min_[1] && dest_loc[2] >= point_min_[2] &&
|
||||
dest_loc[0] <= point_max_[0] && dest_loc[1] <= point_max_[1] && dest_loc[2] <= point_max_[2];
|
||||
|
||||
if (!in_fence) {
|
||||
//are we better off with dest than cur location?
|
||||
float dest_dist = (dest_loc - point_center_).norm();
|
||||
float cur_dist = (cur_loc - point_center_).norm();
|
||||
allow = cur_dist - dest_dist >= -distance_accuracy_;
|
||||
Utils::logMessage("CubeGeoFence: %s", toString().c_str());
|
||||
}
|
||||
else
|
||||
allow = true;
|
||||
}
|
||||
|
||||
string toString() const override
|
||||
{
|
||||
return Utils::stringf("min=%s, max=%s", VectorMath::toString(point_min_).c_str(), VectorMath::toString(point_max_).c_str());
|
||||
}
|
||||
|
||||
virtual ~CubeGeoFence() {};
|
||||
void setBoundry(const Vector3r& origin, float xy_length, float max_z, float min_z) override
|
||||
{
|
||||
point_min_ = Vector3r(-xy_length, -xy_length, 0) + origin;
|
||||
point_min_[2] = max_z;
|
||||
|
||||
private:
|
||||
void calculateCenter()
|
||||
{
|
||||
point_center_ = (point_max_ + point_min_) / 2;
|
||||
}
|
||||
};
|
||||
point_max_ = Vector3r(xy_length, xy_length, 0) + origin;
|
||||
point_max_[2] = min_z;
|
||||
|
||||
calculateCenter();
|
||||
|
||||
}} //namespace
|
||||
Utils::logMessage("CubeGeoFence: %s", toString().c_str());
|
||||
}
|
||||
|
||||
void checkFence(const Vector3r& cur_loc, const Vector3r& dest_loc,
|
||||
bool& in_fence, bool& allow) override
|
||||
{
|
||||
in_fence = dest_loc[0] >= point_min_[0] && dest_loc[1] >= point_min_[1] && dest_loc[2] >= point_min_[2] &&
|
||||
dest_loc[0] <= point_max_[0] && dest_loc[1] <= point_max_[1] && dest_loc[2] <= point_max_[2];
|
||||
|
||||
if (!in_fence) {
|
||||
//are we better off with dest than cur location?
|
||||
float dest_dist = (dest_loc - point_center_).norm();
|
||||
float cur_dist = (cur_loc - point_center_).norm();
|
||||
allow = cur_dist - dest_dist >= -distance_accuracy_;
|
||||
}
|
||||
else
|
||||
allow = true;
|
||||
}
|
||||
|
||||
string toString() const override
|
||||
{
|
||||
return Utils::stringf("min=%s, max=%s", VectorMath::toString(point_min_).c_str(), VectorMath::toString(point_max_).c_str());
|
||||
}
|
||||
|
||||
virtual ~CubeGeoFence(){};
|
||||
|
||||
private:
|
||||
void calculateCenter()
|
||||
{
|
||||
point_center_ = (point_max_ + point_min_) / 2;
|
||||
}
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -6,17 +6,21 @@
|
|||
|
||||
#include "common/Common.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
class IGeoFence {
|
||||
public:
|
||||
virtual void setBoundry(const Vector3r& origin, float xy_length, float max_z, float min_z) = 0;
|
||||
virtual void checkFence(const Vector3r& cur_loc, const Vector3r& dest_loc,
|
||||
bool& in_fence, bool& allow) = 0;
|
||||
virtual string toString() const = 0;
|
||||
class IGeoFence
|
||||
{
|
||||
public:
|
||||
virtual void setBoundry(const Vector3r& origin, float xy_length, float max_z, float min_z) = 0;
|
||||
virtual void checkFence(const Vector3r& cur_loc, const Vector3r& dest_loc,
|
||||
bool& in_fence, bool& allow) = 0;
|
||||
virtual string toString() const = 0;
|
||||
|
||||
virtual ~IGeoFence() {};
|
||||
};
|
||||
|
||||
}} //namespace
|
||||
virtual ~IGeoFence(){};
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -7,9 +7,12 @@
|
|||
#include <mutex>
|
||||
#include "common/Common.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
/*
|
||||
/*
|
||||
ObstacleMap implements 2D map of obstacles in circular disk around the vehicle. The main
|
||||
design criteria is to make insert/delete/queries very fast. This is typically
|
||||
not possible in grid based approach because these operations may have large
|
||||
|
|
@ -35,63 +38,68 @@ namespace msr { namespace airlib {
|
|||
We fully expect one thread to continuously update the obstacles while another to query the map.
|
||||
*/
|
||||
|
||||
class ObstacleMap {
|
||||
private:
|
||||
//stores distances for each tick segment
|
||||
vector<float> distances_;
|
||||
//what is the confidence in these values? This should typically be the standard deviation
|
||||
vector<float> confidences_;
|
||||
//number of ticks, this decides reolution
|
||||
int ticks_;
|
||||
//blind spots don't get updated so we get its value from neighbours
|
||||
vector<bool> blindspots_;
|
||||
public:
|
||||
//this will be return result of the queries
|
||||
struct ObstacleInfo {
|
||||
int tick; //at what tick we found obstacle
|
||||
float distance; //what is the distance from obstacle
|
||||
float confidence;
|
||||
class ObstacleMap
|
||||
{
|
||||
private:
|
||||
//stores distances for each tick segment
|
||||
vector<float> distances_;
|
||||
//what is the confidence in these values? This should typically be the standard deviation
|
||||
vector<float> confidences_;
|
||||
//number of ticks, this decides reolution
|
||||
int ticks_;
|
||||
//blind spots don't get updated so we get its value from neighbours
|
||||
vector<bool> blindspots_;
|
||||
|
||||
string toString() const
|
||||
public:
|
||||
//this will be return result of the queries
|
||||
struct ObstacleInfo
|
||||
{
|
||||
return Utils::stringf("Obs: tick=%i, distance=%f, confidence=%f", tick, distance, confidence);
|
||||
}
|
||||
int tick; //at what tick we found obstacle
|
||||
float distance; //what is the distance from obstacle
|
||||
float confidence;
|
||||
|
||||
string toString() const
|
||||
{
|
||||
return Utils::stringf("Obs: tick=%i, distance=%f, confidence=%f", tick, distance, confidence);
|
||||
}
|
||||
};
|
||||
|
||||
//private version of hasObstacle doesn't do lock or check inputs
|
||||
ObstacleInfo hasObstacle_(int from_tick, int to_tick) const;
|
||||
|
||||
private:
|
||||
int wrap(int tick) const;
|
||||
|
||||
//currently we employ simple thread safe model: just serialize queries and updates
|
||||
std::mutex mutex_;
|
||||
|
||||
public:
|
||||
//if odd_blindspots = true then set all odd ticks as blind spots
|
||||
ObstacleMap(int ticks, bool odd_blindspots = false);
|
||||
|
||||
//update the map for tick direction within +/-window ticks
|
||||
void update(float distance, int tick, int window, float confidence);
|
||||
void update(float distances[], float confidences[]);
|
||||
|
||||
void setBlindspot(int tick, bool blindspot);
|
||||
|
||||
//query if we have obstacle in segment that starts at from to segment that starts at to
|
||||
ObstacleInfo hasObstacle(int from_tick, int to_tick);
|
||||
|
||||
//search entire map to find obstacle at minimum distance
|
||||
ObstacleInfo getClosestObstacle();
|
||||
|
||||
//number of ticks the map was initialized with
|
||||
int getTicks() const;
|
||||
//convert angle (in body frame) in radians to tick number
|
||||
int angleToTick(float angle_rad) const;
|
||||
//convert tick to start of angle (in body frame) in radians
|
||||
float tickToAngleStart(int tick) const;
|
||||
//convert tick to end of angle (in body frame) in radians
|
||||
float tickToAngleEnd(int tick) const;
|
||||
//convert tick to mid of the cone in radians
|
||||
float tickToAngleMid(int tick) const;
|
||||
};
|
||||
|
||||
//private version of hasObstacle doesn't do lock or check inputs
|
||||
ObstacleInfo hasObstacle_(int from_tick, int to_tick) const;
|
||||
private:
|
||||
int wrap(int tick) const;
|
||||
|
||||
//currently we employ simple thread safe model: just serialize queries and updates
|
||||
std::mutex mutex_;
|
||||
public:
|
||||
//if odd_blindspots = true then set all odd ticks as blind spots
|
||||
ObstacleMap(int ticks, bool odd_blindspots = false);
|
||||
|
||||
//update the map for tick direction within +/-window ticks
|
||||
void update(float distance, int tick, int window, float confidence);
|
||||
void update(float distances[], float confidences[]);
|
||||
|
||||
void setBlindspot(int tick, bool blindspot);
|
||||
|
||||
//query if we have obstacle in segment that starts at from to segment that starts at to
|
||||
ObstacleInfo hasObstacle(int from_tick, int to_tick);
|
||||
|
||||
//search entire map to find obstacle at minimum distance
|
||||
ObstacleInfo getClosestObstacle();
|
||||
|
||||
//number of ticks the map was initialized with
|
||||
int getTicks() const;
|
||||
//convert angle (in body frame) in radians to tick number
|
||||
int angleToTick(float angle_rad) const;
|
||||
//convert tick to start of angle (in body frame) in radians
|
||||
float tickToAngleStart(int tick) const;
|
||||
//convert tick to end of angle (in body frame) in radians
|
||||
float tickToAngleEnd(int tick) const;
|
||||
//convert tick to mid of the cone in radians
|
||||
float tickToAngleMid(int tick) const;
|
||||
};
|
||||
|
||||
}} //namespace
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -14,89 +14,106 @@
|
|||
#include "vehicles/multirotor/api/MultirotorCommon.hpp"
|
||||
#include "common/common_utils/EnumFlags.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
//this class takes all inputs and outputs in NEU world coordinates in metric system
|
||||
class SafetyEval {
|
||||
public:
|
||||
enum class SafetyViolationType_ :uint {
|
||||
NoSafetyViolation = 0,
|
||||
GeoFence = 1 << 0,
|
||||
Obstacle = 1 << 1,
|
||||
VelocityLimit = 1 << 2,
|
||||
All = Utils::max<uint>()
|
||||
};
|
||||
//add bitwise operators for enum
|
||||
typedef common_utils::EnumFlags<SafetyViolationType_> SafetyViolationType;
|
||||
|
||||
//to disable obstacle avoidance entirely, disable it in SafetyViolationType
|
||||
enum class ObsAvoidanceStrategy : uint {
|
||||
RaiseException = 0, //will return suggested velocity vector = 0
|
||||
ClosestMove, //find closest obstacle free destination along desired destination
|
||||
OppositeMove //find closest obstacle free destination along opposite of desired destination
|
||||
};
|
||||
|
||||
//result of the safety evaluation
|
||||
struct EvalResult {
|
||||
//information in data structure may only be partially filled if is_safe = true
|
||||
bool is_safe;
|
||||
SafetyViolationType reason;
|
||||
//obstacle info around cur and towards dest, if no obs found then cur might not be evaluated
|
||||
ObstacleMap::ObstacleInfo cur_obs, dest_obs, suggested_obs;
|
||||
//locations that were considered while evaluation
|
||||
Vector3r cur_pos, dest_pos;
|
||||
//transformed cur to destination vector in body frame
|
||||
Vector3r cur_dest_body;
|
||||
string message;
|
||||
//suggested unit vector without obstacle, must be zero if no suggestions available
|
||||
Vector3r suggested_vec;
|
||||
//risk distances indicates how far we are in to risk zone, lower (<= 0) better than higher
|
||||
//cur is for risk distance around current position
|
||||
//dest if risk distance towards destination
|
||||
float cur_risk_dist, dest_risk_dist;
|
||||
|
||||
//setup default result
|
||||
EvalResult()
|
||||
: is_safe(true), reason(SafetyViolationType_::NoSafetyViolation), suggested_vec(Vector3r::Zero()),
|
||||
cur_risk_dist(Utils::nan<float>()), dest_risk_dist(Utils::nan<float>())
|
||||
{}
|
||||
|
||||
string toString() const
|
||||
//this class takes all inputs and outputs in NEU world coordinates in metric system
|
||||
class SafetyEval
|
||||
{
|
||||
public:
|
||||
enum class SafetyViolationType_ : uint
|
||||
{
|
||||
return Utils::stringf("SafetyEval: is_safe=%i, reason=%X, cur_risk_dist=%f, dest_risk_dist=%f, suggested_vec=%s,"
|
||||
" cur=%s, dest=%s, cur_dest_body=%s, cur_obs=[%s], dest_obs=[%s], suggested_obs=[%s], message=%s",
|
||||
is_safe, uint(reason), cur_risk_dist, dest_risk_dist, VectorMath::toString(suggested_vec).c_str(),
|
||||
VectorMath::toString(cur_pos).c_str(), VectorMath::toString(dest_pos).c_str(), VectorMath::toString(cur_dest_body).c_str(),
|
||||
cur_obs.toString().c_str(), dest_obs.toString().c_str(), suggested_obs.toString().c_str(), message.c_str());
|
||||
}
|
||||
NoSafetyViolation = 0,
|
||||
GeoFence = 1 << 0,
|
||||
Obstacle = 1 << 1,
|
||||
VelocityLimit = 1 << 2,
|
||||
All = Utils::max<uint>()
|
||||
};
|
||||
//add bitwise operators for enum
|
||||
typedef common_utils::EnumFlags<SafetyViolationType_> SafetyViolationType;
|
||||
|
||||
//to disable obstacle avoidance entirely, disable it in SafetyViolationType
|
||||
enum class ObsAvoidanceStrategy : uint
|
||||
{
|
||||
RaiseException = 0, //will return suggested velocity vector = 0
|
||||
ClosestMove, //find closest obstacle free destination along desired destination
|
||||
OppositeMove //find closest obstacle free destination along opposite of desired destination
|
||||
};
|
||||
|
||||
//result of the safety evaluation
|
||||
struct EvalResult
|
||||
{
|
||||
//information in data structure may only be partially filled if is_safe = true
|
||||
bool is_safe;
|
||||
SafetyViolationType reason;
|
||||
//obstacle info around cur and towards dest, if no obs found then cur might not be evaluated
|
||||
ObstacleMap::ObstacleInfo cur_obs, dest_obs, suggested_obs;
|
||||
//locations that were considered while evaluation
|
||||
Vector3r cur_pos, dest_pos;
|
||||
//transformed cur to destination vector in body frame
|
||||
Vector3r cur_dest_body;
|
||||
string message;
|
||||
//suggested unit vector without obstacle, must be zero if no suggestions available
|
||||
Vector3r suggested_vec;
|
||||
//risk distances indicates how far we are in to risk zone, lower (<= 0) better than higher
|
||||
//cur is for risk distance around current position
|
||||
//dest if risk distance towards destination
|
||||
float cur_risk_dist, dest_risk_dist;
|
||||
|
||||
//setup default result
|
||||
EvalResult()
|
||||
: is_safe(true), reason(SafetyViolationType_::NoSafetyViolation), suggested_vec(Vector3r::Zero()), cur_risk_dist(Utils::nan<float>()), dest_risk_dist(Utils::nan<float>())
|
||||
{
|
||||
}
|
||||
|
||||
string toString() const
|
||||
{
|
||||
return Utils::stringf("SafetyEval: is_safe=%i, reason=%X, cur_risk_dist=%f, dest_risk_dist=%f, suggested_vec=%s,"
|
||||
" cur=%s, dest=%s, cur_dest_body=%s, cur_obs=[%s], dest_obs=[%s], suggested_obs=[%s], message=%s",
|
||||
is_safe,
|
||||
uint(reason),
|
||||
cur_risk_dist,
|
||||
dest_risk_dist,
|
||||
VectorMath::toString(suggested_vec).c_str(),
|
||||
VectorMath::toString(cur_pos).c_str(),
|
||||
VectorMath::toString(dest_pos).c_str(),
|
||||
VectorMath::toString(cur_dest_body).c_str(),
|
||||
cur_obs.toString().c_str(),
|
||||
dest_obs.toString().c_str(),
|
||||
suggested_obs.toString().c_str(),
|
||||
message.c_str());
|
||||
}
|
||||
};
|
||||
|
||||
private:
|
||||
MultirotorApiParams vehicle_params_;
|
||||
shared_ptr<IGeoFence> fence_ptr_;
|
||||
shared_ptr<ObstacleMap> obs_xy_ptr_;
|
||||
SafetyViolationType enable_reasons_ = SafetyEval::SafetyViolationType_::GeoFence;
|
||||
ObsAvoidanceStrategy obs_strategy_ = SafetyEval::ObsAvoidanceStrategy::RaiseException;
|
||||
|
||||
void checkFence(const Vector3r& cur_pos, const Vector3r& dest_pos, EvalResult& appendToResult);
|
||||
void isSafeDestination(const Vector3r& dest, const Vector3r& cur_pos, const Quaternionr& quaternion, SafetyEval::EvalResult& result);
|
||||
Vector3r getDestination(const Vector3r& cur_pos, const Vector3r& velocity) const;
|
||||
bool isThisRiskDistLess(float this_risk_dist, float other_risk_dist) const;
|
||||
void isCurrentSafer(SafetyEval::EvalResult& result);
|
||||
void setSuggestedVelocity(SafetyEval::EvalResult& result, const Quaternionr& quaternion);
|
||||
float adjustClearanceForPrStl(float base_clearance, float obs_confidence);
|
||||
|
||||
public:
|
||||
SafetyEval(MultirotorApiParams vehicle_params, shared_ptr<IGeoFence> fence_ptr, shared_ptr<ObstacleMap> obs_xy);
|
||||
EvalResult isSafeVelocity(const Vector3r& cur_pos, const Vector3r& velocity, const Quaternionr& quaternion);
|
||||
EvalResult isSafeVelocityZ(const Vector3r& cur_pos, float vx, float vy, float z, const Quaternionr& quaternion);
|
||||
EvalResult isSafeDestination(const Vector3r& dest, const Vector3r& cur_pos, const Quaternionr& quaternion);
|
||||
EvalResult isSafePosition(const Vector3r& cur_pos, const Quaternionr& quaternion);
|
||||
|
||||
void setSafety(SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_strategy,
|
||||
const Vector3r& origin, float xy_length, float max_z, float min_z);
|
||||
void setObsAvoidanceStrategy(SafetyEval::ObsAvoidanceStrategy obs_strategy);
|
||||
SafetyEval::ObsAvoidanceStrategy getObsAvoidanceStrategy();
|
||||
};
|
||||
|
||||
private:
|
||||
MultirotorApiParams vehicle_params_;
|
||||
shared_ptr<IGeoFence> fence_ptr_;
|
||||
shared_ptr<ObstacleMap> obs_xy_ptr_;
|
||||
SafetyViolationType enable_reasons_ = SafetyEval::SafetyViolationType_::GeoFence;
|
||||
ObsAvoidanceStrategy obs_strategy_ = SafetyEval::ObsAvoidanceStrategy::RaiseException;
|
||||
|
||||
void checkFence(const Vector3r& cur_pos, const Vector3r& dest_pos, EvalResult& appendToResult);
|
||||
void isSafeDestination(const Vector3r& dest,const Vector3r& cur_pos, const Quaternionr& quaternion, SafetyEval::EvalResult& result);
|
||||
Vector3r getDestination(const Vector3r& cur_pos, const Vector3r& velocity) const;
|
||||
bool isThisRiskDistLess(float this_risk_dist, float other_risk_dist) const;
|
||||
void isCurrentSafer(SafetyEval::EvalResult& result);
|
||||
void setSuggestedVelocity(SafetyEval::EvalResult& result, const Quaternionr& quaternion);
|
||||
float adjustClearanceForPrStl(float base_clearance, float obs_confidence);
|
||||
public:
|
||||
SafetyEval(MultirotorApiParams vehicle_params, shared_ptr<IGeoFence> fence_ptr, shared_ptr<ObstacleMap> obs_xy);
|
||||
EvalResult isSafeVelocity(const Vector3r& cur_pos, const Vector3r& velocity, const Quaternionr& quaternion);
|
||||
EvalResult isSafeVelocityZ(const Vector3r& cur_pos, float vx, float vy, float z, const Quaternionr& quaternion);
|
||||
EvalResult isSafeDestination(const Vector3r& dest,const Vector3r& cur_pos, const Quaternionr& quaternion);
|
||||
EvalResult isSafePosition(const Vector3r& cur_pos, const Quaternionr& quaternion);
|
||||
|
||||
void setSafety(SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_strategy,
|
||||
const Vector3r& origin, float xy_length, float max_z, float min_z);
|
||||
void setObsAvoidanceStrategy(SafetyEval::ObsAvoidanceStrategy obs_strategy);
|
||||
SafetyEval::ObsAvoidanceStrategy getObsAvoidanceStrategy();
|
||||
};
|
||||
|
||||
}} //namespace
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -6,54 +6,57 @@
|
|||
|
||||
#include "common/Common.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
class SphereGeoFence : public IGeoFence {
|
||||
private:
|
||||
Vector3r center_;
|
||||
float radius_, distance_accuracy_, min_height_, max_height_;
|
||||
|
||||
public:
|
||||
SphereGeoFence(const Vector3r& center, float radius, float max_height, float min_height, float distance_accuracy)
|
||||
: center_(center), radius_(radius), distance_accuracy_(distance_accuracy), min_height_(min_height), max_height_(max_height)
|
||||
class SphereGeoFence : public IGeoFence
|
||||
{
|
||||
Utils::logMessage("SphereGeoFence: %s", toString().c_str());
|
||||
}
|
||||
private:
|
||||
Vector3r center_;
|
||||
float radius_, distance_accuracy_, min_height_, max_height_;
|
||||
|
||||
void setBoundry(const Vector3r& origin, float xy_length, float max_z, float min_z) override
|
||||
{
|
||||
center_ = origin;
|
||||
radius_ = xy_length;
|
||||
max_height_ = min_z;
|
||||
min_height_ = max_z;
|
||||
|
||||
Utils::logMessage("SphereGeoFence: %s", toString().c_str());
|
||||
}
|
||||
|
||||
void checkFence(const Vector3r& cur_loc, const Vector3r& dest_loc,
|
||||
bool& in_fence, bool& allow) override
|
||||
{
|
||||
Vector3r center_dest = dest_loc - center_;
|
||||
Vector3r center_dest_xy(center_dest[0], center_dest[1], 0);
|
||||
|
||||
in_fence = center_dest_xy.norm() <= radius_ && dest_loc[2] >= min_height_ && dest_loc[2] <= max_height_;
|
||||
|
||||
if (!in_fence) {
|
||||
float cur_center_norm = (cur_loc - center_).norm();
|
||||
allow = cur_center_norm - center_dest.norm() >= -distance_accuracy;
|
||||
public:
|
||||
SphereGeoFence(const Vector3r& center, float radius, float max_height, float min_height, float distance_accuracy)
|
||||
: center_(center), radius_(radius), distance_accuracy_(distance_accuracy), min_height_(min_height), max_height_(max_height)
|
||||
{
|
||||
Utils::logMessage("SphereGeoFence: %s", toString().c_str());
|
||||
}
|
||||
else
|
||||
allow = true;
|
||||
}
|
||||
|
||||
string toString() const override
|
||||
{
|
||||
return Utils::stringf("CubeGeoFence: radius=%f, min_height=%f, max_height=%f, center=%s", radius_, min_height_, max_height_, Vector3r::toString(center_).c_str());
|
||||
}
|
||||
void setBoundry(const Vector3r& origin, float xy_length, float max_z, float min_z) override
|
||||
{
|
||||
center_ = origin;
|
||||
radius_ = xy_length;
|
||||
max_height_ = min_z;
|
||||
min_height_ = max_z;
|
||||
|
||||
|
||||
virtual ~SphereGeoFence() {};
|
||||
};
|
||||
Utils::logMessage("SphereGeoFence: %s", toString().c_str());
|
||||
}
|
||||
|
||||
}} //namespace
|
||||
void checkFence(const Vector3r& cur_loc, const Vector3r& dest_loc,
|
||||
bool& in_fence, bool& allow) override
|
||||
{
|
||||
Vector3r center_dest = dest_loc - center_;
|
||||
Vector3r center_dest_xy(center_dest[0], center_dest[1], 0);
|
||||
|
||||
in_fence = center_dest_xy.norm() <= radius_ && dest_loc[2] >= min_height_ && dest_loc[2] <= max_height_;
|
||||
|
||||
if (!in_fence) {
|
||||
float cur_center_norm = (cur_loc - center_).norm();
|
||||
allow = cur_center_norm - center_dest.norm() >= -distance_accuracy;
|
||||
}
|
||||
else
|
||||
allow = true;
|
||||
}
|
||||
|
||||
string toString() const override
|
||||
{
|
||||
return Utils::stringf("CubeGeoFence: radius=%f, min_height=%f, max_height=%f, center=%s", radius_, min_height_, max_height_, Vector3r::toString(center_).c_str());
|
||||
}
|
||||
|
||||
virtual ~SphereGeoFence(){};
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -10,61 +10,65 @@
|
|||
#include "physics/Environment.hpp"
|
||||
#include "physics/Kinematics.hpp"
|
||||
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
/*
|
||||
/*
|
||||
Derived classes should not do any work in constructor which requires ground truth.
|
||||
After construction of the derived class an initialize(...) must be made which would
|
||||
set the sensor in good-to-use state by call to reset.
|
||||
*/
|
||||
class SensorBase : public UpdatableObject
|
||||
{
|
||||
public:
|
||||
enum class SensorType : uint {
|
||||
Barometer = 1,
|
||||
Imu = 2,
|
||||
Gps = 3,
|
||||
Magnetometer = 4,
|
||||
Distance = 5,
|
||||
Lidar = 6
|
||||
class SensorBase : public UpdatableObject
|
||||
{
|
||||
public:
|
||||
enum class SensorType : uint
|
||||
{
|
||||
Barometer = 1,
|
||||
Imu = 2,
|
||||
Gps = 3,
|
||||
Magnetometer = 4,
|
||||
Distance = 5,
|
||||
Lidar = 6
|
||||
};
|
||||
|
||||
SensorBase(const std::string& sensor_name = "")
|
||||
: name_(sensor_name)
|
||||
{
|
||||
}
|
||||
|
||||
protected:
|
||||
struct GroundTruth
|
||||
{
|
||||
const Kinematics::State* kinematics;
|
||||
const Environment* environment;
|
||||
};
|
||||
|
||||
public:
|
||||
virtual void initialize(const Kinematics::State* kinematics, const Environment* environment)
|
||||
{
|
||||
ground_truth_.kinematics = kinematics;
|
||||
ground_truth_.environment = environment;
|
||||
}
|
||||
|
||||
const GroundTruth& getGroundTruth() const
|
||||
{
|
||||
return ground_truth_;
|
||||
}
|
||||
|
||||
const std::string& getName() const
|
||||
{
|
||||
return name_;
|
||||
}
|
||||
|
||||
virtual ~SensorBase() = default;
|
||||
|
||||
private:
|
||||
//ground truth can be shared between many sensors
|
||||
GroundTruth ground_truth_ = { nullptr, nullptr };
|
||||
std::string name_ = "";
|
||||
};
|
||||
|
||||
SensorBase(const std::string& sensor_name = "")
|
||||
: name_(sensor_name)
|
||||
{}
|
||||
|
||||
protected:
|
||||
struct GroundTruth {
|
||||
const Kinematics::State* kinematics;
|
||||
const Environment* environment;
|
||||
};
|
||||
public:
|
||||
virtual void initialize(const Kinematics::State* kinematics, const Environment* environment)
|
||||
{
|
||||
ground_truth_.kinematics = kinematics;
|
||||
ground_truth_.environment = environment;
|
||||
}
|
||||
|
||||
const GroundTruth& getGroundTruth() const
|
||||
{
|
||||
return ground_truth_;
|
||||
}
|
||||
|
||||
const std::string& getName() const
|
||||
{
|
||||
return name_;
|
||||
}
|
||||
|
||||
virtual ~SensorBase() = default;
|
||||
|
||||
private:
|
||||
//ground truth can be shared between many sensors
|
||||
GroundTruth ground_truth_ = { nullptr, nullptr };
|
||||
std::string name_ = "";
|
||||
};
|
||||
|
||||
|
||||
|
||||
}} //namespace
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -9,93 +9,97 @@
|
|||
#include "common/UpdatableContainer.hpp"
|
||||
#include "common/Common.hpp"
|
||||
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
class SensorCollection : public UpdatableObject {
|
||||
public: //types
|
||||
typedef SensorBase* SensorBasePtr;
|
||||
public:
|
||||
void insert(SensorBasePtr sensor, SensorBase::SensorType type)
|
||||
class SensorCollection : public UpdatableObject
|
||||
{
|
||||
auto type_int = static_cast<uint>(type);
|
||||
const auto& it = sensors_.find(type_int);
|
||||
if (it == sensors_.end()) {
|
||||
const auto& pair = sensors_.emplace(type_int, unique_ptr<SensorBaseContainer>(new SensorBaseContainer()));
|
||||
pair.first->second->insert(sensor);
|
||||
}
|
||||
else {
|
||||
it->second->insert(sensor);
|
||||
}
|
||||
}
|
||||
public: //types
|
||||
typedef SensorBase* SensorBasePtr;
|
||||
|
||||
const SensorBase* getByType(SensorBase::SensorType type, uint index = 0) const
|
||||
{
|
||||
auto type_int = static_cast<uint>(type);
|
||||
const auto& it = sensors_.find(type_int);
|
||||
if (it == sensors_.end()) {
|
||||
return nullptr;
|
||||
}
|
||||
else {
|
||||
return it->second->at(index);
|
||||
}
|
||||
}
|
||||
|
||||
uint size(SensorBase::SensorType type) const
|
||||
{
|
||||
auto type_int = static_cast<uint>(type);
|
||||
const auto& it = sensors_.find(type_int);
|
||||
if (it == sensors_.end()) {
|
||||
return 0;
|
||||
}
|
||||
else {
|
||||
return it->second->size();
|
||||
}
|
||||
}
|
||||
|
||||
void initialize(const Kinematics::State* kinematics, const Environment* environment)
|
||||
{
|
||||
for (auto& pair : sensors_) {
|
||||
for (auto& sensor : *pair.second) {
|
||||
sensor->initialize(kinematics, environment);
|
||||
public:
|
||||
void insert(SensorBasePtr sensor, SensorBase::SensorType type)
|
||||
{
|
||||
auto type_int = static_cast<uint>(type);
|
||||
const auto& it = sensors_.find(type_int);
|
||||
if (it == sensors_.end()) {
|
||||
const auto& pair = sensors_.emplace(type_int, unique_ptr<SensorBaseContainer>(new SensorBaseContainer()));
|
||||
pair.first->second->insert(sensor);
|
||||
}
|
||||
else {
|
||||
it->second->insert(sensor);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void clear()
|
||||
{
|
||||
sensors_.clear();
|
||||
}
|
||||
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
for (auto& pair : sensors_) {
|
||||
pair.second->reset();
|
||||
const SensorBase* getByType(SensorBase::SensorType type, uint index = 0) const
|
||||
{
|
||||
auto type_int = static_cast<uint>(type);
|
||||
const auto& it = sensors_.find(type_int);
|
||||
if (it == sensors_.end()) {
|
||||
return nullptr;
|
||||
}
|
||||
else {
|
||||
return it->second->at(index);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
UpdatableObject::update();
|
||||
|
||||
for (auto& pair : sensors_) {
|
||||
pair.second->update();
|
||||
uint size(SensorBase::SensorType type) const
|
||||
{
|
||||
auto type_int = static_cast<uint>(type);
|
||||
const auto& it = sensors_.find(type_int);
|
||||
if (it == sensors_.end()) {
|
||||
return 0;
|
||||
}
|
||||
else {
|
||||
return it->second->size();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
virtual void reportState(StateReporter& reporter) override
|
||||
{
|
||||
for (auto& pair : sensors_) {
|
||||
pair.second->reportState(reporter);
|
||||
void initialize(const Kinematics::State* kinematics, const Environment* environment)
|
||||
{
|
||||
for (auto& pair : sensors_) {
|
||||
for (auto& sensor : *pair.second) {
|
||||
sensor->initialize(kinematics, environment);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
//*** End: UpdatableState implementation ***//
|
||||
|
||||
private:
|
||||
typedef UpdatableContainer<SensorBasePtr> SensorBaseContainer;
|
||||
unordered_map<uint, unique_ptr<SensorBaseContainer>> sensors_;
|
||||
};
|
||||
void clear()
|
||||
{
|
||||
sensors_.clear();
|
||||
}
|
||||
|
||||
}} //namespace
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
for (auto& pair : sensors_) {
|
||||
pair.second->reset();
|
||||
}
|
||||
}
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
UpdatableObject::update();
|
||||
|
||||
for (auto& pair : sensors_) {
|
||||
pair.second->update();
|
||||
}
|
||||
}
|
||||
|
||||
virtual void reportState(StateReporter& reporter) override
|
||||
{
|
||||
for (auto& pair : sensors_) {
|
||||
pair.second->reportState(reporter);
|
||||
}
|
||||
}
|
||||
//*** End: UpdatableState implementation ***//
|
||||
|
||||
private:
|
||||
typedef UpdatableContainer<SensorBasePtr> SensorBaseContainer;
|
||||
unordered_map<uint, unique_ptr<SensorBaseContainer>> sensors_;
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -1,7 +1,6 @@
|
|||
#ifndef msr_airlib_SensorFactoryBase_hpp
|
||||
#define msr_airlib_SensorFactoryBase_hpp
|
||||
|
||||
|
||||
#include "SensorBase.hpp"
|
||||
#include "SensorCollection.hpp"
|
||||
#include <memory>
|
||||
|
|
@ -12,55 +11,55 @@
|
|||
#include "sensors/gps/GpsSimple.hpp"
|
||||
#include "sensors/barometer/BarometerSimple.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
|
||||
class SensorFactory
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
public:
|
||||
|
||||
// creates one sensor from settings
|
||||
virtual std::shared_ptr<SensorBase> createSensorFromSettings(
|
||||
const AirSimSettings::SensorSetting* sensor_setting) const
|
||||
class SensorFactory
|
||||
{
|
||||
switch (sensor_setting->sensor_type) {
|
||||
case SensorBase::SensorType::Imu:
|
||||
return std::shared_ptr<ImuSimple>(new ImuSimple(*static_cast<const AirSimSettings::ImuSetting*>(sensor_setting)));
|
||||
case SensorBase::SensorType::Magnetometer:
|
||||
return std::shared_ptr<MagnetometerSimple>(new MagnetometerSimple(*static_cast<const AirSimSettings::MagnetometerSetting*>(sensor_setting)));
|
||||
case SensorBase::SensorType::Gps:
|
||||
return std::shared_ptr<GpsSimple>(new GpsSimple(*static_cast<const AirSimSettings::GpsSetting*>(sensor_setting)));
|
||||
case SensorBase::SensorType::Barometer:
|
||||
return std::shared_ptr<BarometerSimple>(new BarometerSimple(*static_cast<const AirSimSettings::BarometerSetting*>(sensor_setting)));
|
||||
default:
|
||||
throw new std::invalid_argument("Unexpected sensor type");
|
||||
}
|
||||
}
|
||||
|
||||
// creates sensor-collection
|
||||
virtual void createSensorsFromSettings(
|
||||
const std::map<std::string, std::shared_ptr<AirSimSettings::SensorSetting>>& sensors_settings,
|
||||
SensorCollection& sensors,
|
||||
vector<shared_ptr<SensorBase>>& sensor_storage) const
|
||||
{
|
||||
for (const auto& sensor_setting_pair : sensors_settings) {
|
||||
const AirSimSettings::SensorSetting* sensor_setting = sensor_setting_pair.second.get();
|
||||
|
||||
// ignore sensors that are marked "disabled" in settings
|
||||
if (sensor_setting == nullptr || !sensor_setting->enabled)
|
||||
continue;
|
||||
|
||||
std::shared_ptr<SensorBase> sensor = createSensorFromSettings(sensor_setting);
|
||||
if (sensor) {
|
||||
sensor_storage.push_back(sensor);
|
||||
sensors.insert(sensor.get(), sensor_setting->sensor_type);
|
||||
public:
|
||||
// creates one sensor from settings
|
||||
virtual std::shared_ptr<SensorBase> createSensorFromSettings(
|
||||
const AirSimSettings::SensorSetting* sensor_setting) const
|
||||
{
|
||||
switch (sensor_setting->sensor_type) {
|
||||
case SensorBase::SensorType::Imu:
|
||||
return std::shared_ptr<ImuSimple>(new ImuSimple(*static_cast<const AirSimSettings::ImuSetting*>(sensor_setting)));
|
||||
case SensorBase::SensorType::Magnetometer:
|
||||
return std::shared_ptr<MagnetometerSimple>(new MagnetometerSimple(*static_cast<const AirSimSettings::MagnetometerSetting*>(sensor_setting)));
|
||||
case SensorBase::SensorType::Gps:
|
||||
return std::shared_ptr<GpsSimple>(new GpsSimple(*static_cast<const AirSimSettings::GpsSetting*>(sensor_setting)));
|
||||
case SensorBase::SensorType::Barometer:
|
||||
return std::shared_ptr<BarometerSimple>(new BarometerSimple(*static_cast<const AirSimSettings::BarometerSetting*>(sensor_setting)));
|
||||
default:
|
||||
throw new std::invalid_argument("Unexpected sensor type");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
virtual ~SensorFactory() = default;
|
||||
};
|
||||
// creates sensor-collection
|
||||
virtual void createSensorsFromSettings(
|
||||
const std::map<std::string, std::shared_ptr<AirSimSettings::SensorSetting>>& sensors_settings,
|
||||
SensorCollection& sensors,
|
||||
vector<shared_ptr<SensorBase>>& sensor_storage) const
|
||||
{
|
||||
for (const auto& sensor_setting_pair : sensors_settings) {
|
||||
const AirSimSettings::SensorSetting* sensor_setting = sensor_setting_pair.second.get();
|
||||
|
||||
// ignore sensors that are marked "disabled" in settings
|
||||
if (sensor_setting == nullptr || !sensor_setting->enabled)
|
||||
continue;
|
||||
|
||||
}} //namespace
|
||||
std::shared_ptr<SensorBase> sensor = createSensorFromSettings(sensor_setting);
|
||||
if (sensor) {
|
||||
sensor_storage.push_back(sensor);
|
||||
sensors.insert(sensor.get(), sensor_setting->sensor_type);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
virtual ~SensorFactory() = default;
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
@ -4,53 +4,54 @@
|
|||
#ifndef msr_airlib_BarometerBase_hpp
|
||||
#define msr_airlib_BarometerBase_hpp
|
||||
|
||||
|
||||
#include "sensors/SensorBase.hpp"
|
||||
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
class BarometerBase : public SensorBase
|
||||
{
|
||||
public:
|
||||
BarometerBase(const std::string& sensor_name = "")
|
||||
: SensorBase(sensor_name)
|
||||
{
|
||||
}
|
||||
|
||||
class BarometerBase : public SensorBase {
|
||||
public:
|
||||
BarometerBase(const std::string& sensor_name = "")
|
||||
: SensorBase(sensor_name)
|
||||
{}
|
||||
public: //types
|
||||
struct Output
|
||||
{ //same fields as ROS message
|
||||
TTimePoint time_stamp;
|
||||
real_T altitude; //meters
|
||||
real_T pressure; //Pascal
|
||||
real_T qnh;
|
||||
};
|
||||
|
||||
public: //types
|
||||
struct Output { //same fields as ROS message
|
||||
TTimePoint time_stamp;
|
||||
real_T altitude; //meters
|
||||
real_T pressure; //Pascal
|
||||
real_T qnh;
|
||||
public:
|
||||
virtual void reportState(StateReporter& reporter) override
|
||||
{
|
||||
//call base
|
||||
UpdatableObject::reportState(reporter);
|
||||
|
||||
reporter.writeValue("Baro-Alt", output_.altitude);
|
||||
reporter.writeValue("Baro-Prs", output_.pressure);
|
||||
}
|
||||
|
||||
const Output& getOutput() const
|
||||
{
|
||||
return output_;
|
||||
}
|
||||
|
||||
protected:
|
||||
void setOutput(const Output& output)
|
||||
{
|
||||
output_ = output;
|
||||
}
|
||||
|
||||
private:
|
||||
Output output_;
|
||||
};
|
||||
|
||||
|
||||
public:
|
||||
virtual void reportState(StateReporter& reporter) override
|
||||
{
|
||||
//call base
|
||||
UpdatableObject::reportState(reporter);
|
||||
|
||||
reporter.writeValue("Baro-Alt", output_.altitude);
|
||||
reporter.writeValue("Baro-Prs", output_.pressure);
|
||||
}
|
||||
|
||||
const Output& getOutput() const
|
||||
{
|
||||
return output_;
|
||||
}
|
||||
|
||||
protected:
|
||||
void setOutput(const Output& output)
|
||||
{
|
||||
output_ = output;
|
||||
}
|
||||
|
||||
|
||||
private:
|
||||
Output output_;
|
||||
};
|
||||
|
||||
|
||||
}} //namespace
|
||||
#endif
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -13,100 +13,102 @@
|
|||
#include "common/DelayLine.hpp"
|
||||
#include "common/FrequencyLimiter.hpp"
|
||||
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
class BarometerSimple : public BarometerBase
|
||||
namespace msr
|
||||
{
|
||||
public:
|
||||
BarometerSimple(const AirSimSettings::BarometerSetting& setting = AirSimSettings::BarometerSetting())
|
||||
: BarometerBase(setting.sensor_name)
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
class BarometerSimple : public BarometerBase
|
||||
{
|
||||
// initialize params
|
||||
params_.initializeFromSettings(setting);
|
||||
public:
|
||||
BarometerSimple(const AirSimSettings::BarometerSetting& setting = AirSimSettings::BarometerSetting())
|
||||
: BarometerBase(setting.sensor_name)
|
||||
{
|
||||
// initialize params
|
||||
params_.initializeFromSettings(setting);
|
||||
|
||||
//GM process that would do random walk for pressure factor
|
||||
pressure_factor_.initialize(params_.pressure_factor_tau, params_.pressure_factor_sigma, 0);
|
||||
//GM process that would do random walk for pressure factor
|
||||
pressure_factor_.initialize(params_.pressure_factor_tau, params_.pressure_factor_sigma, 0);
|
||||
|
||||
uncorrelated_noise_ = RandomGeneratorGausianR(0.0f, params_.uncorrelated_noise_sigma);
|
||||
//correlated_noise_.initialize(params_.correlated_noise_tau, params_.correlated_noise_sigma, 0.0f);
|
||||
uncorrelated_noise_ = RandomGeneratorGausianR(0.0f, params_.uncorrelated_noise_sigma);
|
||||
//correlated_noise_.initialize(params_.correlated_noise_tau, params_.correlated_noise_sigma, 0.0f);
|
||||
|
||||
//initialize frequency limiter
|
||||
freq_limiter_.initialize(params_.update_frequency, params_.startup_delay);
|
||||
delay_line_.initialize(params_.update_latency);
|
||||
}
|
||||
//initialize frequency limiter
|
||||
freq_limiter_.initialize(params_.update_frequency, params_.startup_delay);
|
||||
delay_line_.initialize(params_.update_latency);
|
||||
}
|
||||
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
pressure_factor_.reset();
|
||||
//correlated_noise_.reset();
|
||||
uncorrelated_noise_.reset();
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
pressure_factor_.reset();
|
||||
//correlated_noise_.reset();
|
||||
uncorrelated_noise_.reset();
|
||||
|
||||
freq_limiter_.reset();
|
||||
delay_line_.reset();
|
||||
freq_limiter_.reset();
|
||||
delay_line_.reset();
|
||||
|
||||
delay_line_.push_back(getOutputInternal());
|
||||
}
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
BarometerBase::update();
|
||||
|
||||
freq_limiter_.update();
|
||||
|
||||
if (freq_limiter_.isWaitComplete()) {
|
||||
delay_line_.push_back(getOutputInternal());
|
||||
}
|
||||
|
||||
delay_line_.update();
|
||||
virtual void update() override
|
||||
{
|
||||
BarometerBase::update();
|
||||
|
||||
if (freq_limiter_.isWaitComplete())
|
||||
setOutput(delay_line_.getOutput());
|
||||
}
|
||||
//*** End: UpdatableState implementation ***//
|
||||
freq_limiter_.update();
|
||||
|
||||
virtual ~BarometerSimple() = default;
|
||||
if (freq_limiter_.isWaitComplete()) {
|
||||
delay_line_.push_back(getOutputInternal());
|
||||
}
|
||||
|
||||
private: //methods
|
||||
Output getOutputInternal()
|
||||
{
|
||||
Output output;
|
||||
const GroundTruth& ground_truth = getGroundTruth();
|
||||
delay_line_.update();
|
||||
|
||||
auto altitude = ground_truth.environment->getState().geo_point.altitude;
|
||||
auto pressure = EarthUtils::getStandardPressure(altitude);
|
||||
if (freq_limiter_.isWaitComplete())
|
||||
setOutput(delay_line_.getOutput());
|
||||
}
|
||||
//*** End: UpdatableState implementation ***//
|
||||
|
||||
//add drift in pressure, about 10m change per hour using default settings.
|
||||
pressure_factor_.update();
|
||||
pressure += pressure * pressure_factor_.getOutput();
|
||||
virtual ~BarometerSimple() = default;
|
||||
|
||||
//add noise in pressure (about 0.2m sigma)
|
||||
pressure += uncorrelated_noise_.next();
|
||||
private: //methods
|
||||
Output getOutputInternal()
|
||||
{
|
||||
Output output;
|
||||
const GroundTruth& ground_truth = getGroundTruth();
|
||||
|
||||
output.pressure = pressure - EarthUtils::SeaLevelPressure + params_.qnh*100.0f;
|
||||
auto altitude = ground_truth.environment->getState().geo_point.altitude;
|
||||
auto pressure = EarthUtils::getStandardPressure(altitude);
|
||||
|
||||
//apply altimeter formula
|
||||
//https://en.wikipedia.org/wiki/Pressure_altitude
|
||||
//TODO: use same formula as in driver code?
|
||||
output.altitude = (1 - pow(pressure / EarthUtils::SeaLevelPressure, 0.190284f)) * 145366.45f * 0.3048f;
|
||||
output.qnh = params_.qnh;
|
||||
//add drift in pressure, about 10m change per hour using default settings.
|
||||
pressure_factor_.update();
|
||||
pressure += pressure * pressure_factor_.getOutput();
|
||||
|
||||
output.time_stamp = clock()->nowNanos();
|
||||
//add noise in pressure (about 0.2m sigma)
|
||||
pressure += uncorrelated_noise_.next();
|
||||
|
||||
return output;
|
||||
}
|
||||
|
||||
private:
|
||||
BarometerSimpleParams params_;
|
||||
output.pressure = pressure - EarthUtils::SeaLevelPressure + params_.qnh * 100.0f;
|
||||
|
||||
GaussianMarkov pressure_factor_;
|
||||
//GaussianMarkov correlated_noise_;
|
||||
RandomGeneratorGausianR uncorrelated_noise_;
|
||||
//apply altimeter formula
|
||||
//https://en.wikipedia.org/wiki/Pressure_altitude
|
||||
//TODO: use same formula as in driver code?
|
||||
output.altitude = (1 - pow(pressure / EarthUtils::SeaLevelPressure, 0.190284f)) * 145366.45f * 0.3048f;
|
||||
output.qnh = params_.qnh;
|
||||
|
||||
FrequencyLimiter freq_limiter_;
|
||||
DelayLine<Output> delay_line_;
|
||||
};
|
||||
output.time_stamp = clock()->nowNanos();
|
||||
|
||||
}} //namespace
|
||||
#endif
|
||||
return output;
|
||||
}
|
||||
|
||||
private:
|
||||
BarometerSimpleParams params_;
|
||||
|
||||
GaussianMarkov pressure_factor_;
|
||||
//GaussianMarkov correlated_noise_;
|
||||
RandomGeneratorGausianR uncorrelated_noise_;
|
||||
|
||||
FrequencyLimiter freq_limiter_;
|
||||
DelayLine<Output> delay_line_;
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -7,26 +7,27 @@
|
|||
#include "common/Common.hpp"
|
||||
#include "common/AirSimSettings.hpp"
|
||||
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
|
||||
struct BarometerSimpleParams
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
//user specified sea level pressure is specified in hPa units
|
||||
real_T qnh = EarthUtils::SeaLevelPressure / 100.0f; // hPa
|
||||
|
||||
//sea level min,avh,max = 950,1013,1050 ie approx 3.65% variation
|
||||
//regular pressure changes in quiet conditions are taken as 1/20th of this
|
||||
//Mariner's Pressure Atlas, David Burch, 2014
|
||||
//GM process may generate ~70% of sigma in tau interval
|
||||
//This means below config may produce ~10m variance per hour
|
||||
struct BarometerSimpleParams
|
||||
{
|
||||
//user specified sea level pressure is specified in hPa units
|
||||
real_T qnh = EarthUtils::SeaLevelPressure / 100.0f; // hPa
|
||||
|
||||
//https://www.starpath.com/ebooksamples/9780914025382_sample.pdf
|
||||
real_T pressure_factor_sigma = 0.0365f / 20;
|
||||
real_T pressure_factor_tau = 3600;
|
||||
//sea level min,avh,max = 950,1013,1050 ie approx 3.65% variation
|
||||
//regular pressure changes in quiet conditions are taken as 1/20th of this
|
||||
//Mariner's Pressure Atlas, David Burch, 2014
|
||||
//GM process may generate ~70% of sigma in tau interval
|
||||
//This means below config may produce ~10m variance per hour
|
||||
|
||||
/*
|
||||
//https://www.starpath.com/ebooksamples/9780914025382_sample.pdf
|
||||
real_T pressure_factor_sigma = 0.0365f / 20;
|
||||
real_T pressure_factor_tau = 3600;
|
||||
|
||||
/*
|
||||
Ref: A Stochastic Approach to Noise Modeling for Barometric Altimeters
|
||||
Angelo Maria Sabatini* and Vincenzo Genovese
|
||||
Sample values are from Table 1
|
||||
|
|
@ -38,29 +39,28 @@ struct BarometerSimpleParams
|
|||
real_T uncorrelated_noise_sigma = 0.24f;
|
||||
*/
|
||||
|
||||
//Experiments for MEAS MS56112 sensor shows 0.021mbar, datasheet has resoultion of 0.027mbar @ 1024
|
||||
//http://www.te.com/commerce/DocumentDelivery/DDEController?Action=srchrtrv&DocNm=MS5611-01BA03&DocType=Data+Sheet&DocLang=English
|
||||
real_T uncorrelated_noise_sigma = 0.027f * 100;
|
||||
//jMavSim uses below
|
||||
//real_T uncorrelated_noise_sigma = 0.1f;
|
||||
//Experiments for MEAS MS56112 sensor shows 0.021mbar, datasheet has resoultion of 0.027mbar @ 1024
|
||||
//http://www.te.com/commerce/DocumentDelivery/DDEController?Action=srchrtrv&DocNm=MS5611-01BA03&DocType=Data+Sheet&DocLang=English
|
||||
real_T uncorrelated_noise_sigma = 0.027f * 100;
|
||||
//jMavSim uses below
|
||||
//real_T uncorrelated_noise_sigma = 0.1f;
|
||||
|
||||
//see PX4 param reference for EKF: https://dev.px4.io/en/advanced/parameter_reference.html
|
||||
real_T update_latency = 0.0f; //sec
|
||||
real_T update_frequency = 50; //Hz
|
||||
real_T startup_delay = 0; //sec
|
||||
//see PX4 param reference for EKF: https://dev.px4.io/en/advanced/parameter_reference.html
|
||||
real_T update_latency = 0.0f; //sec
|
||||
real_T update_frequency = 50; //Hz
|
||||
real_T startup_delay = 0; //sec
|
||||
|
||||
void initializeFromSettings(const AirSimSettings::BarometerSetting& settings)
|
||||
{
|
||||
const auto& json = settings.settings;
|
||||
pressure_factor_sigma = json.getFloat("PressureFactorSigma", pressure_factor_sigma);
|
||||
pressure_factor_tau = json.getFloat("PressureFactorTau", pressure_factor_tau);
|
||||
uncorrelated_noise_sigma = json.getFloat("UncorrelatedNoiseSigma", uncorrelated_noise_sigma);
|
||||
update_latency = json.getFloat("UpdateLatency", update_latency);
|
||||
update_frequency = json.getFloat("UpdateFrequency", update_frequency);
|
||||
startup_delay = json.getFloat("StartupDelay", startup_delay);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
}} //namespace
|
||||
void initializeFromSettings(const AirSimSettings::BarometerSetting& settings)
|
||||
{
|
||||
const auto& json = settings.settings;
|
||||
pressure_factor_sigma = json.getFloat("PressureFactorSigma", pressure_factor_sigma);
|
||||
pressure_factor_tau = json.getFloat("PressureFactorTau", pressure_factor_tau);
|
||||
uncorrelated_noise_sigma = json.getFloat("UncorrelatedNoiseSigma", uncorrelated_noise_sigma);
|
||||
update_latency = json.getFloat("UpdateLatency", update_latency);
|
||||
update_frequency = json.getFloat("UpdateFrequency", update_frequency);
|
||||
startup_delay = json.getFloat("StartupDelay", startup_delay);
|
||||
}
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -4,43 +4,44 @@
|
|||
#ifndef msr_airlib_DistanceBase_hpp
|
||||
#define msr_airlib_DistanceBase_hpp
|
||||
|
||||
|
||||
#include "sensors/SensorBase.hpp"
|
||||
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
class DistanceBase : public SensorBase {
|
||||
public:
|
||||
DistanceBase(const std::string& sensor_name = "")
|
||||
: SensorBase(sensor_name)
|
||||
{}
|
||||
|
||||
public:
|
||||
virtual void reportState(StateReporter& reporter) override
|
||||
class DistanceBase : public SensorBase
|
||||
{
|
||||
//call base
|
||||
UpdatableObject::reportState(reporter);
|
||||
public:
|
||||
DistanceBase(const std::string& sensor_name = "")
|
||||
: SensorBase(sensor_name)
|
||||
{
|
||||
}
|
||||
|
||||
reporter.writeValue("Dist-Curr", output_.distance);
|
||||
}
|
||||
public:
|
||||
virtual void reportState(StateReporter& reporter) override
|
||||
{
|
||||
//call base
|
||||
UpdatableObject::reportState(reporter);
|
||||
|
||||
const DistanceSensorData& getOutput() const
|
||||
{
|
||||
return output_;
|
||||
}
|
||||
reporter.writeValue("Dist-Curr", output_.distance);
|
||||
}
|
||||
|
||||
protected:
|
||||
void setOutput(const DistanceSensorData& output)
|
||||
{
|
||||
output_ = output;
|
||||
}
|
||||
const DistanceSensorData& getOutput() const
|
||||
{
|
||||
return output_;
|
||||
}
|
||||
|
||||
protected:
|
||||
void setOutput(const DistanceSensorData& output)
|
||||
{
|
||||
output_ = output;
|
||||
}
|
||||
|
||||
private:
|
||||
DistanceSensorData output_;
|
||||
};
|
||||
|
||||
|
||||
}} //namespace
|
||||
private:
|
||||
DistanceSensorData output_;
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -12,97 +12,99 @@
|
|||
#include "common/DelayLine.hpp"
|
||||
#include "common/FrequencyLimiter.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
class DistanceSimple : public DistanceBase
|
||||
namespace msr
|
||||
{
|
||||
public:
|
||||
DistanceSimple(const AirSimSettings::DistanceSetting& setting = AirSimSettings::DistanceSetting())
|
||||
: DistanceBase(setting.sensor_name)
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
class DistanceSimple : public DistanceBase
|
||||
{
|
||||
// initialize params
|
||||
params_.initializeFromSettings(setting);
|
||||
public:
|
||||
DistanceSimple(const AirSimSettings::DistanceSetting& setting = AirSimSettings::DistanceSetting())
|
||||
: DistanceBase(setting.sensor_name)
|
||||
{
|
||||
// initialize params
|
||||
params_.initializeFromSettings(setting);
|
||||
|
||||
uncorrelated_noise_ = RandomGeneratorGausianR(0.0f, params_.uncorrelated_noise_sigma);
|
||||
//correlated_noise_.initialize(params_.correlated_noise_tau, params_.correlated_noise_sigma, 0.0f);
|
||||
uncorrelated_noise_ = RandomGeneratorGausianR(0.0f, params_.uncorrelated_noise_sigma);
|
||||
//correlated_noise_.initialize(params_.correlated_noise_tau, params_.correlated_noise_sigma, 0.0f);
|
||||
|
||||
//initialize frequency limiter
|
||||
freq_limiter_.initialize(params_.update_frequency, params_.startup_delay);
|
||||
delay_line_.initialize(params_.update_latency);
|
||||
}
|
||||
|
||||
//initialize frequency limiter
|
||||
freq_limiter_.initialize(params_.update_frequency, params_.startup_delay);
|
||||
delay_line_.initialize(params_.update_latency);
|
||||
}
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
//correlated_noise_.reset();
|
||||
uncorrelated_noise_.reset();
|
||||
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
//correlated_noise_.reset();
|
||||
uncorrelated_noise_.reset();
|
||||
freq_limiter_.reset();
|
||||
delay_line_.reset();
|
||||
|
||||
freq_limiter_.reset();
|
||||
delay_line_.reset();
|
||||
|
||||
delay_line_.push_back(getOutputInternal());
|
||||
}
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
DistanceBase::update();
|
||||
|
||||
freq_limiter_.update();
|
||||
|
||||
if (freq_limiter_.isWaitComplete()) {
|
||||
delay_line_.push_back(getOutputInternal());
|
||||
}
|
||||
|
||||
delay_line_.update();
|
||||
virtual void update() override
|
||||
{
|
||||
DistanceBase::update();
|
||||
|
||||
if (freq_limiter_.isWaitComplete())
|
||||
setOutput(delay_line_.getOutput());
|
||||
}
|
||||
//*** End: UpdatableState implementation ***//
|
||||
freq_limiter_.update();
|
||||
|
||||
virtual ~DistanceSimple() = default;
|
||||
if (freq_limiter_.isWaitComplete()) {
|
||||
delay_line_.push_back(getOutputInternal());
|
||||
}
|
||||
|
||||
const DistanceSimpleParams& getParams() const
|
||||
{
|
||||
return params_;
|
||||
}
|
||||
delay_line_.update();
|
||||
|
||||
protected:
|
||||
virtual real_T getRayLength(const Pose& pose) = 0;
|
||||
if (freq_limiter_.isWaitComplete())
|
||||
setOutput(delay_line_.getOutput());
|
||||
}
|
||||
//*** End: UpdatableState implementation ***//
|
||||
|
||||
private: //methods
|
||||
DistanceSensorData getOutputInternal()
|
||||
{
|
||||
DistanceSensorData output;
|
||||
const GroundTruth& ground_truth = getGroundTruth();
|
||||
virtual ~DistanceSimple() = default;
|
||||
|
||||
//order of Pose addition is important here because it also adds quaternions which is not commutative!
|
||||
auto distance = getRayLength(params_.relative_pose + ground_truth.kinematics->pose);
|
||||
const DistanceSimpleParams& getParams() const
|
||||
{
|
||||
return params_;
|
||||
}
|
||||
|
||||
//add noise in distance (about 0.2m sigma)
|
||||
distance += uncorrelated_noise_.next();
|
||||
protected:
|
||||
virtual real_T getRayLength(const Pose& pose) = 0;
|
||||
|
||||
output.distance = distance;
|
||||
output.min_distance = params_.min_distance;
|
||||
output.max_distance = params_.max_distance;
|
||||
output.relative_pose = params_.relative_pose;
|
||||
output.time_stamp = clock()->nowNanos();
|
||||
private: //methods
|
||||
DistanceSensorData getOutputInternal()
|
||||
{
|
||||
DistanceSensorData output;
|
||||
const GroundTruth& ground_truth = getGroundTruth();
|
||||
|
||||
return output;
|
||||
}
|
||||
//order of Pose addition is important here because it also adds quaternions which is not commutative!
|
||||
auto distance = getRayLength(params_.relative_pose + ground_truth.kinematics->pose);
|
||||
|
||||
private:
|
||||
DistanceSimpleParams params_;
|
||||
//add noise in distance (about 0.2m sigma)
|
||||
distance += uncorrelated_noise_.next();
|
||||
|
||||
//GaussianMarkov correlated_noise_;
|
||||
RandomGeneratorGausianR uncorrelated_noise_;
|
||||
output.distance = distance;
|
||||
output.min_distance = params_.min_distance;
|
||||
output.max_distance = params_.max_distance;
|
||||
output.relative_pose = params_.relative_pose;
|
||||
output.time_stamp = clock()->nowNanos();
|
||||
|
||||
FrequencyLimiter freq_limiter_;
|
||||
DelayLine<DistanceSensorData> delay_line_;
|
||||
return output;
|
||||
}
|
||||
|
||||
//start time
|
||||
};
|
||||
private:
|
||||
DistanceSimpleParams params_;
|
||||
|
||||
}} //namespace
|
||||
//GaussianMarkov correlated_noise_;
|
||||
RandomGeneratorGausianR uncorrelated_noise_;
|
||||
|
||||
FrequencyLimiter freq_limiter_;
|
||||
DelayLine<DistanceSensorData> delay_line_;
|
||||
|
||||
//start time
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -7,24 +7,25 @@
|
|||
#include "common/Common.hpp"
|
||||
#include "common/AirSimSettings.hpp"
|
||||
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
|
||||
struct DistanceSimpleParams
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
real_T min_distance = 20.0f / 100; //m
|
||||
real_T max_distance = 4000.0f / 100; //m
|
||||
|
||||
Pose relative_pose {
|
||||
Vector3r(0,0,-1), // position - a little above vehicle (especially for cars) or Vector3r::Zero()
|
||||
Quaternionr::Identity() // orientation - by default Quaternionr(1, 0, 0, 0)
|
||||
};
|
||||
struct DistanceSimpleParams
|
||||
{
|
||||
real_T min_distance = 20.0f / 100; //m
|
||||
real_T max_distance = 4000.0f / 100; //m
|
||||
|
||||
bool draw_debug_points = false;
|
||||
bool external_controller = true;
|
||||
Pose relative_pose{
|
||||
Vector3r(0, 0, -1), // position - a little above vehicle (especially for cars) or Vector3r::Zero()
|
||||
Quaternionr::Identity() // orientation - by default Quaternionr(1, 0, 0, 0)
|
||||
};
|
||||
|
||||
/*
|
||||
bool draw_debug_points = false;
|
||||
bool external_controller = true;
|
||||
|
||||
/*
|
||||
Ref: A Stochastic Approach to Noise Modeling for Barometric Altimeters
|
||||
Angelo Maria Sabatini* and Vincenzo Genovese
|
||||
Sample values are from Table 1
|
||||
|
|
@ -36,52 +37,51 @@ struct DistanceSimpleParams
|
|||
real_T uncorrelated_noise_sigma = 0.24f;
|
||||
|
||||
*/
|
||||
//TODO: update sigma based on documentation, maybe as a function increasing with measured distance
|
||||
real_T uncorrelated_noise_sigma = 0.002f * 100;
|
||||
//jMavSim uses below
|
||||
//real_T uncorrelated_noise_sigma = 0.1f;
|
||||
//TODO: update sigma based on documentation, maybe as a function increasing with measured distance
|
||||
real_T uncorrelated_noise_sigma = 0.002f * 100;
|
||||
//jMavSim uses below
|
||||
//real_T uncorrelated_noise_sigma = 0.1f;
|
||||
|
||||
//see PX4 param reference for EKF: https://dev.px4.io/en/advanced/parameter_reference.html
|
||||
real_T update_latency = 0.0f; //sec
|
||||
real_T update_frequency = 50; //Hz
|
||||
real_T startup_delay = 0; //sec
|
||||
//see PX4 param reference for EKF: https://dev.px4.io/en/advanced/parameter_reference.html
|
||||
real_T update_latency = 0.0f; //sec
|
||||
real_T update_frequency = 50; //Hz
|
||||
real_T startup_delay = 0; //sec
|
||||
|
||||
void initializeFromSettings(const AirSimSettings::DistanceSetting& settings)
|
||||
{
|
||||
const auto& settings_json = settings.settings;
|
||||
min_distance = settings_json.getFloat("MinDistance", min_distance);
|
||||
max_distance = settings_json.getFloat("MaxDistance", max_distance);
|
||||
draw_debug_points = settings_json.getBool("DrawDebugPoints", draw_debug_points);
|
||||
external_controller = settings_json.getBool("ExternalController", external_controller);
|
||||
void initializeFromSettings(const AirSimSettings::DistanceSetting& settings)
|
||||
{
|
||||
const auto& settings_json = settings.settings;
|
||||
min_distance = settings_json.getFloat("MinDistance", min_distance);
|
||||
max_distance = settings_json.getFloat("MaxDistance", max_distance);
|
||||
draw_debug_points = settings_json.getBool("DrawDebugPoints", draw_debug_points);
|
||||
external_controller = settings_json.getBool("ExternalController", external_controller);
|
||||
|
||||
auto position = AirSimSettings::createVectorSetting(settings_json, VectorMath::nanVector());
|
||||
auto rotation = AirSimSettings::createRotationSetting(settings_json, AirSimSettings::Rotation::nanRotation());
|
||||
auto position = AirSimSettings::createVectorSetting(settings_json, VectorMath::nanVector());
|
||||
auto rotation = AirSimSettings::createRotationSetting(settings_json, AirSimSettings::Rotation::nanRotation());
|
||||
|
||||
std::string simmode_name = AirSimSettings::singleton().simmode_name;
|
||||
std::string simmode_name = AirSimSettings::singleton().simmode_name;
|
||||
|
||||
relative_pose.position = position;
|
||||
if (std::isnan(relative_pose.position.x()))
|
||||
relative_pose.position.x() = 0;
|
||||
if (std::isnan(relative_pose.position.y()))
|
||||
relative_pose.position.y() = 0;
|
||||
if (std::isnan(relative_pose.position.z())) {
|
||||
if (simmode_name == AirSimSettings::kSimModeTypeMultirotor)
|
||||
relative_pose.position.z() = 0;
|
||||
else
|
||||
relative_pose.position.z() = -1; // a little bit above for cars
|
||||
relative_pose.position = position;
|
||||
if (std::isnan(relative_pose.position.x()))
|
||||
relative_pose.position.x() = 0;
|
||||
if (std::isnan(relative_pose.position.y()))
|
||||
relative_pose.position.y() = 0;
|
||||
if (std::isnan(relative_pose.position.z())) {
|
||||
if (simmode_name == AirSimSettings::kSimModeTypeMultirotor)
|
||||
relative_pose.position.z() = 0;
|
||||
else
|
||||
relative_pose.position.z() = -1; // a little bit above for cars
|
||||
}
|
||||
|
||||
float pitch, roll, yaw;
|
||||
pitch = !std::isnan(rotation.pitch) ? rotation.pitch : 0;
|
||||
roll = !std::isnan(rotation.roll) ? rotation.roll : 0;
|
||||
yaw = !std::isnan(rotation.yaw) ? rotation.yaw : 0;
|
||||
relative_pose.orientation = VectorMath::toQuaternion(
|
||||
Utils::degreesToRadians(pitch), //pitch - rotation around Y axis
|
||||
Utils::degreesToRadians(roll), //roll - rotation around X axis
|
||||
Utils::degreesToRadians(yaw)); //yaw - rotation around Z axis
|
||||
}
|
||||
|
||||
float pitch, roll, yaw;
|
||||
pitch = !std::isnan(rotation.pitch) ? rotation.pitch : 0;
|
||||
roll = !std::isnan(rotation.roll) ? rotation.roll : 0;
|
||||
yaw = !std::isnan(rotation.yaw) ? rotation.yaw : 0;
|
||||
relative_pose.orientation = VectorMath::toQuaternion(
|
||||
Utils::degreesToRadians(pitch), //pitch - rotation around Y axis
|
||||
Utils::degreesToRadians(roll), //roll - rotation around X axis
|
||||
Utils::degreesToRadians(yaw)); //yaw - rotation around Z axis
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
}} //namespace
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -4,126 +4,137 @@
|
|||
#ifndef msr_airlib_GpsBase_hpp
|
||||
#define msr_airlib_GpsBase_hpp
|
||||
|
||||
|
||||
#include "sensors/SensorBase.hpp"
|
||||
#include "common/CommonStructs.hpp"
|
||||
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
class GpsBase : public SensorBase {
|
||||
public:
|
||||
GpsBase(const std::string& sensor_name = "")
|
||||
: SensorBase(sensor_name)
|
||||
{}
|
||||
|
||||
public: //types
|
||||
//TODO: cleanup GPS structures that are not needed
|
||||
struct GpsPoint {
|
||||
public:
|
||||
double latitude, longitude;
|
||||
float height, altitude;
|
||||
int health;
|
||||
|
||||
GpsPoint()
|
||||
{}
|
||||
|
||||
GpsPoint(double latitude_val, double longitude_val, float altitude_val, int health_val = -1, float height_val = std::numeric_limits<float>::quiet_NaN())
|
||||
class GpsBase : public SensorBase
|
||||
{
|
||||
public:
|
||||
GpsBase(const std::string& sensor_name = "")
|
||||
: SensorBase(sensor_name)
|
||||
{
|
||||
latitude = latitude_val; longitude = longitude_val;
|
||||
height = height_val, altitude = altitude_val;
|
||||
health = health_val;
|
||||
}
|
||||
|
||||
string to_string()
|
||||
public: //types
|
||||
//TODO: cleanup GPS structures that are not needed
|
||||
struct GpsPoint
|
||||
{
|
||||
return Utils::stringf("latitude=%f, longitude=%f, altitude=%f, height=%f, health=%d", latitude, longitude, altitude, height, health);
|
||||
public:
|
||||
double latitude, longitude;
|
||||
float height, altitude;
|
||||
int health;
|
||||
|
||||
GpsPoint()
|
||||
{
|
||||
}
|
||||
|
||||
GpsPoint(double latitude_val, double longitude_val, float altitude_val, int health_val = -1, float height_val = std::numeric_limits<float>::quiet_NaN())
|
||||
{
|
||||
latitude = latitude_val;
|
||||
longitude = longitude_val;
|
||||
height = height_val, altitude = altitude_val;
|
||||
health = health_val;
|
||||
}
|
||||
|
||||
string to_string()
|
||||
{
|
||||
return Utils::stringf("latitude=%f, longitude=%f, altitude=%f, height=%f, health=%d", latitude, longitude, altitude, height, health);
|
||||
}
|
||||
};
|
||||
|
||||
enum NavSatStatusType : char
|
||||
{
|
||||
STATUS_NO_FIX = 80, //unable to fix position
|
||||
STATUS_FIX = 0, //unaugmented fix
|
||||
STATUS_SBAS_FIX = 1, //with satellite-based augmentation
|
||||
STATUS_GBAS_FIX = 2 //with ground-based augmentation
|
||||
};
|
||||
|
||||
enum NavSatStatusServiceType : unsigned short int
|
||||
{
|
||||
SERVICE_GPS = 1,
|
||||
SERVICE_GLONASS = 2,
|
||||
SERVICE_COMPASS = 4, //includes BeiDou.
|
||||
SERVICE_GALILEO = 8
|
||||
};
|
||||
|
||||
struct NavSatStatus
|
||||
{
|
||||
NavSatStatusType status;
|
||||
NavSatStatusServiceType service;
|
||||
};
|
||||
|
||||
enum PositionCovarianceType : unsigned char
|
||||
{
|
||||
COVARIANCE_TYPE_UNKNOWN = 0,
|
||||
COVARIANCE_TYPE_APPROXIMATED = 1,
|
||||
COVARIANCE_TYPE_DIAGONAL_KNOWN = 2,
|
||||
COVARIANCE_TYPE_KNOWN = 3
|
||||
};
|
||||
|
||||
enum GnssFixType : unsigned char
|
||||
{
|
||||
GNSS_FIX_NO_FIX = 0,
|
||||
GNSS_FIX_TIME_ONLY = 1,
|
||||
GNSS_FIX_2D_FIX = 2,
|
||||
GNSS_FIX_3D_FIX = 3
|
||||
};
|
||||
|
||||
struct GnssReport
|
||||
{
|
||||
GeoPoint geo_point;
|
||||
real_T eph, epv; //GPS HDOP/VDOP horizontal/vertical dilution of position (unitless), 0-100%
|
||||
Vector3r velocity;
|
||||
GnssFixType fix_type;
|
||||
uint64_t time_utc = 0;
|
||||
};
|
||||
|
||||
struct NavSatFix
|
||||
{
|
||||
GeoPoint geo_point;
|
||||
double position_covariance[9] = {};
|
||||
NavSatStatus status;
|
||||
PositionCovarianceType position_covariance_type;
|
||||
};
|
||||
|
||||
struct Output
|
||||
{ //same as ROS message
|
||||
TTimePoint time_stamp;
|
||||
GnssReport gnss;
|
||||
bool is_valid = false;
|
||||
};
|
||||
|
||||
public:
|
||||
virtual void reportState(StateReporter& reporter) override
|
||||
{
|
||||
//call base
|
||||
UpdatableObject::reportState(reporter);
|
||||
|
||||
reporter.writeValue("GPS-Loc", output_.gnss.geo_point);
|
||||
reporter.writeValue("GPS-Vel", output_.gnss.velocity);
|
||||
reporter.writeValue("GPS-Eph", output_.gnss.eph);
|
||||
reporter.writeValue("GPS-Epv", output_.gnss.epv);
|
||||
}
|
||||
|
||||
const Output& getOutput() const
|
||||
{
|
||||
return output_;
|
||||
}
|
||||
|
||||
protected:
|
||||
void setOutput(const Output& output)
|
||||
{
|
||||
output_ = output;
|
||||
}
|
||||
|
||||
private:
|
||||
Output output_;
|
||||
};
|
||||
|
||||
enum NavSatStatusType : char {
|
||||
STATUS_NO_FIX = 80, //unable to fix position
|
||||
STATUS_FIX = 0, //unaugmented fix
|
||||
STATUS_SBAS_FIX = 1, //with satellite-based augmentation
|
||||
STATUS_GBAS_FIX = 2 //with ground-based augmentation
|
||||
};
|
||||
|
||||
enum NavSatStatusServiceType : unsigned short int {
|
||||
SERVICE_GPS = 1,
|
||||
SERVICE_GLONASS = 2,
|
||||
SERVICE_COMPASS = 4, //includes BeiDou.
|
||||
SERVICE_GALILEO = 8
|
||||
};
|
||||
|
||||
|
||||
struct NavSatStatus {
|
||||
NavSatStatusType status;
|
||||
NavSatStatusServiceType service;
|
||||
};
|
||||
|
||||
enum PositionCovarianceType : unsigned char {
|
||||
COVARIANCE_TYPE_UNKNOWN = 0,
|
||||
COVARIANCE_TYPE_APPROXIMATED = 1,
|
||||
COVARIANCE_TYPE_DIAGONAL_KNOWN = 2,
|
||||
COVARIANCE_TYPE_KNOWN = 3
|
||||
};
|
||||
|
||||
enum GnssFixType : unsigned char {
|
||||
GNSS_FIX_NO_FIX = 0,
|
||||
GNSS_FIX_TIME_ONLY = 1,
|
||||
GNSS_FIX_2D_FIX = 2,
|
||||
GNSS_FIX_3D_FIX = 3
|
||||
};
|
||||
|
||||
struct GnssReport {
|
||||
GeoPoint geo_point;
|
||||
real_T eph, epv; //GPS HDOP/VDOP horizontal/vertical dilution of position (unitless), 0-100%
|
||||
Vector3r velocity;
|
||||
GnssFixType fix_type;
|
||||
uint64_t time_utc = 0;
|
||||
};
|
||||
|
||||
struct NavSatFix {
|
||||
GeoPoint geo_point;
|
||||
double position_covariance[9] = {};
|
||||
NavSatStatus status;
|
||||
PositionCovarianceType position_covariance_type;
|
||||
};
|
||||
|
||||
struct Output { //same as ROS message
|
||||
TTimePoint time_stamp;
|
||||
GnssReport gnss;
|
||||
bool is_valid = false;
|
||||
};
|
||||
|
||||
|
||||
public:
|
||||
virtual void reportState(StateReporter& reporter) override
|
||||
{
|
||||
//call base
|
||||
UpdatableObject::reportState(reporter);
|
||||
|
||||
reporter.writeValue("GPS-Loc", output_.gnss.geo_point);
|
||||
reporter.writeValue("GPS-Vel", output_.gnss.velocity);
|
||||
reporter.writeValue("GPS-Eph", output_.gnss.eph);
|
||||
reporter.writeValue("GPS-Epv", output_.gnss.epv);
|
||||
}
|
||||
|
||||
const Output& getOutput() const
|
||||
{
|
||||
return output_;
|
||||
}
|
||||
|
||||
protected:
|
||||
void setOutput(const Output& output)
|
||||
{
|
||||
output_ = output;
|
||||
}
|
||||
|
||||
|
||||
private:
|
||||
Output output_;
|
||||
};
|
||||
|
||||
}} //namespace
|
||||
#endif
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -12,94 +12,96 @@
|
|||
#include "common/FrequencyLimiter.hpp"
|
||||
#include "common/DelayLine.hpp"
|
||||
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
class GpsSimple : public GpsBase
|
||||
namespace msr
|
||||
{
|
||||
public: //methods
|
||||
GpsSimple(const AirSimSettings::GpsSetting& setting = AirSimSettings::GpsSetting())
|
||||
: GpsBase(setting.sensor_name)
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
class GpsSimple : public GpsBase
|
||||
{
|
||||
// initialize params
|
||||
params_.initializeFromSettings(setting);
|
||||
public: //methods
|
||||
GpsSimple(const AirSimSettings::GpsSetting& setting = AirSimSettings::GpsSetting())
|
||||
: GpsBase(setting.sensor_name)
|
||||
{
|
||||
// initialize params
|
||||
params_.initializeFromSettings(setting);
|
||||
|
||||
//initialize frequency limiter
|
||||
freq_limiter_.initialize(params_.update_frequency, params_.startup_delay);
|
||||
delay_line_.initialize(params_.update_latency);
|
||||
//initialize frequency limiter
|
||||
freq_limiter_.initialize(params_.update_frequency, params_.startup_delay);
|
||||
delay_line_.initialize(params_.update_latency);
|
||||
|
||||
//initialize filters
|
||||
eph_filter.initialize(params_.eph_time_constant, params_.eph_final, params_.eph_initial); //starting dilution set to 100 which we will reduce over time to targeted 0.3f, with 45% accuracy within 100 updates, each update occurring at 0.2s interval
|
||||
epv_filter.initialize(params_.epv_time_constant, params_.epv_final, params_.epv_initial);
|
||||
}
|
||||
//initialize filters
|
||||
eph_filter.initialize(params_.eph_time_constant, params_.eph_final, params_.eph_initial); //starting dilution set to 100 which we will reduce over time to targeted 0.3f, with 45% accuracy within 100 updates, each update occurring at 0.2s interval
|
||||
epv_filter.initialize(params_.epv_time_constant, params_.epv_final, params_.epv_initial);
|
||||
}
|
||||
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
freq_limiter_.reset();
|
||||
delay_line_.reset();
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
freq_limiter_.reset();
|
||||
delay_line_.reset();
|
||||
|
||||
eph_filter.reset();
|
||||
epv_filter.reset();
|
||||
eph_filter.reset();
|
||||
epv_filter.reset();
|
||||
|
||||
addOutputToDelayLine(eph_filter.getOutput(), epv_filter.getOutput());
|
||||
}
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
GpsBase::update();
|
||||
|
||||
freq_limiter_.update();
|
||||
eph_filter.update();
|
||||
epv_filter.update();
|
||||
|
||||
if (freq_limiter_.isWaitComplete()) { //update output
|
||||
addOutputToDelayLine(eph_filter.getOutput(), epv_filter.getOutput());
|
||||
}
|
||||
|
||||
delay_line_.update();
|
||||
virtual void update() override
|
||||
{
|
||||
GpsBase::update();
|
||||
|
||||
if (freq_limiter_.isWaitComplete())
|
||||
setOutput(delay_line_.getOutput());
|
||||
}
|
||||
freq_limiter_.update();
|
||||
eph_filter.update();
|
||||
epv_filter.update();
|
||||
|
||||
//*** End: UpdatableState implementation ***//
|
||||
if (freq_limiter_.isWaitComplete()) { //update output
|
||||
addOutputToDelayLine(eph_filter.getOutput(), epv_filter.getOutput());
|
||||
}
|
||||
|
||||
virtual ~GpsSimple() = default;
|
||||
private:
|
||||
void addOutputToDelayLine(real_T eph, real_T epv)
|
||||
{
|
||||
Output output;
|
||||
const GroundTruth& ground_truth = getGroundTruth();
|
||||
delay_line_.update();
|
||||
|
||||
//GNSS
|
||||
output.gnss.time_utc = static_cast<uint64_t>(clock()->nowNanos() / 1.0E3);
|
||||
output.gnss.geo_point = ground_truth.environment->getState().geo_point;
|
||||
output.gnss.eph = eph;
|
||||
output.gnss.epv = epv;
|
||||
output.gnss.velocity = ground_truth.kinematics->twist.linear;
|
||||
output.is_valid = true;
|
||||
if (freq_limiter_.isWaitComplete())
|
||||
setOutput(delay_line_.getOutput());
|
||||
}
|
||||
|
||||
output.gnss.fix_type =
|
||||
output.gnss.eph <= params_.eph_min_3d ? GnssFixType::GNSS_FIX_3D_FIX
|
||||
: output.gnss.eph <= params_.eph_min_2d ? GnssFixType::GNSS_FIX_2D_FIX
|
||||
: GnssFixType::GNSS_FIX_NO_FIX;
|
||||
//*** End: UpdatableState implementation ***//
|
||||
|
||||
output.time_stamp = clock()->nowNanos();
|
||||
virtual ~GpsSimple() = default;
|
||||
|
||||
delay_line_.push_back(output);
|
||||
}
|
||||
private:
|
||||
void addOutputToDelayLine(real_T eph, real_T epv)
|
||||
{
|
||||
Output output;
|
||||
const GroundTruth& ground_truth = getGroundTruth();
|
||||
|
||||
//GNSS
|
||||
output.gnss.time_utc = static_cast<uint64_t>(clock()->nowNanos() / 1.0E3);
|
||||
output.gnss.geo_point = ground_truth.environment->getState().geo_point;
|
||||
output.gnss.eph = eph;
|
||||
output.gnss.epv = epv;
|
||||
output.gnss.velocity = ground_truth.kinematics->twist.linear;
|
||||
output.is_valid = true;
|
||||
|
||||
private:
|
||||
typedef std::normal_distribution<> NormalDistribution;
|
||||
output.gnss.fix_type =
|
||||
output.gnss.eph <= params_.eph_min_3d ? GnssFixType::GNSS_FIX_3D_FIX
|
||||
: output.gnss.eph <= params_.eph_min_2d ? GnssFixType::GNSS_FIX_2D_FIX
|
||||
: GnssFixType::GNSS_FIX_NO_FIX;
|
||||
|
||||
GpsSimpleParams params_;
|
||||
output.time_stamp = clock()->nowNanos();
|
||||
|
||||
FirstOrderFilter<real_T> eph_filter, epv_filter;
|
||||
FrequencyLimiter freq_limiter_;
|
||||
DelayLine<Output> delay_line_;
|
||||
};
|
||||
delay_line_.push_back(output);
|
||||
}
|
||||
|
||||
}} //namespace
|
||||
private:
|
||||
typedef std::normal_distribution<> NormalDistribution;
|
||||
|
||||
GpsSimpleParams params_;
|
||||
|
||||
FirstOrderFilter<real_T> eph_filter, epv_filter;
|
||||
FrequencyLimiter freq_limiter_;
|
||||
DelayLine<Output> delay_line_;
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -6,36 +6,38 @@
|
|||
|
||||
#include "common/Common.hpp"
|
||||
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
struct GpsSimpleParams
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
real_T eph_time_constant = 0.9f, epv_time_constant = 0.9f;
|
||||
real_T eph_initial = 100.0f, epv_initial = 100.0f; //initially fully diluted positions
|
||||
real_T eph_final = 0.1f, epv_final = 0.1f; // PX4 won't converge GPS sensor fusion until we get to 10% accuracty.
|
||||
real_T eph_min_3d = 3.0f, eph_min_2d = 4.0f;
|
||||
|
||||
real_T update_latency = 0.2f; //sec
|
||||
real_T update_frequency = 50; //Hz
|
||||
real_T startup_delay = 1; //sec
|
||||
|
||||
void initializeFromSettings(const AirSimSettings::GpsSetting& settings)
|
||||
struct GpsSimpleParams
|
||||
{
|
||||
const auto& json = settings.settings;
|
||||
eph_time_constant = json.getFloat("EPH_TimeConstant", eph_time_constant);
|
||||
epv_time_constant = json.getFloat("EPV_TimeConstant", epv_time_constant);
|
||||
eph_initial = json.getFloat("EphInitial", eph_initial);
|
||||
epv_initial = json.getFloat("EpvInitial", epv_initial);
|
||||
eph_final = json.getFloat("EphFinal", eph_final);
|
||||
epv_final = json.getFloat("EpvFinal", epv_final);
|
||||
eph_min_3d = json.getFloat("EphMin3d", eph_min_3d);
|
||||
eph_min_2d = json.getFloat("EphMin2d", eph_min_2d);
|
||||
update_latency = json.getFloat("UpdateLatency", update_latency);
|
||||
update_frequency = json.getFloat("UpdateFrequency", update_frequency);
|
||||
startup_delay = json.getFloat("StartupDelay", startup_delay);
|
||||
}
|
||||
};
|
||||
real_T eph_time_constant = 0.9f, epv_time_constant = 0.9f;
|
||||
real_T eph_initial = 100.0f, epv_initial = 100.0f; //initially fully diluted positions
|
||||
real_T eph_final = 0.1f, epv_final = 0.1f; // PX4 won't converge GPS sensor fusion until we get to 10% accuracty.
|
||||
real_T eph_min_3d = 3.0f, eph_min_2d = 4.0f;
|
||||
|
||||
}} //namespace
|
||||
real_T update_latency = 0.2f; //sec
|
||||
real_T update_frequency = 50; //Hz
|
||||
real_T startup_delay = 1; //sec
|
||||
|
||||
void initializeFromSettings(const AirSimSettings::GpsSetting& settings)
|
||||
{
|
||||
const auto& json = settings.settings;
|
||||
eph_time_constant = json.getFloat("EPH_TimeConstant", eph_time_constant);
|
||||
epv_time_constant = json.getFloat("EPV_TimeConstant", epv_time_constant);
|
||||
eph_initial = json.getFloat("EphInitial", eph_initial);
|
||||
epv_initial = json.getFloat("EpvInitial", epv_initial);
|
||||
eph_final = json.getFloat("EphFinal", eph_final);
|
||||
epv_final = json.getFloat("EpvFinal", epv_final);
|
||||
eph_min_3d = json.getFloat("EphMin3d", eph_min_3d);
|
||||
eph_min_2d = json.getFloat("EphMin2d", eph_min_2d);
|
||||
update_latency = json.getFloat("UpdateLatency", update_latency);
|
||||
update_frequency = json.getFloat("UpdateFrequency", update_frequency);
|
||||
startup_delay = json.getFloat("StartupDelay", startup_delay);
|
||||
}
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -4,54 +4,55 @@
|
|||
#ifndef msr_airlib_ImuBase_hpp
|
||||
#define msr_airlib_ImuBase_hpp
|
||||
|
||||
|
||||
#include "sensors/SensorBase.hpp"
|
||||
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
class ImuBase : public SensorBase
|
||||
{
|
||||
public:
|
||||
ImuBase(const std::string& sensor_name = "")
|
||||
: SensorBase(sensor_name)
|
||||
{
|
||||
}
|
||||
|
||||
class ImuBase : public SensorBase {
|
||||
public:
|
||||
ImuBase(const std::string& sensor_name = "")
|
||||
: SensorBase(sensor_name)
|
||||
{}
|
||||
public: //types
|
||||
struct Output
|
||||
{ //structure is same as ROS IMU message
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
TTimePoint time_stamp;
|
||||
Quaternionr orientation;
|
||||
Vector3r angular_velocity;
|
||||
Vector3r linear_acceleration;
|
||||
};
|
||||
|
||||
public: //types
|
||||
struct Output { //structure is same as ROS IMU message
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
TTimePoint time_stamp;
|
||||
Quaternionr orientation;
|
||||
Vector3r angular_velocity;
|
||||
Vector3r linear_acceleration;
|
||||
public:
|
||||
virtual void reportState(StateReporter& reporter) override
|
||||
{
|
||||
//call base
|
||||
UpdatableObject::reportState(reporter);
|
||||
|
||||
reporter.writeValue("IMU-Ang", output_.angular_velocity);
|
||||
reporter.writeValue("IMU-Lin", output_.linear_acceleration);
|
||||
}
|
||||
|
||||
const Output& getOutput() const
|
||||
{
|
||||
return output_;
|
||||
}
|
||||
|
||||
protected:
|
||||
void setOutput(const Output& output)
|
||||
{
|
||||
output_ = output;
|
||||
}
|
||||
|
||||
private:
|
||||
Output output_;
|
||||
};
|
||||
|
||||
|
||||
public:
|
||||
virtual void reportState(StateReporter& reporter) override
|
||||
{
|
||||
//call base
|
||||
UpdatableObject::reportState(reporter);
|
||||
|
||||
reporter.writeValue("IMU-Ang", output_.angular_velocity);
|
||||
reporter.writeValue("IMU-Lin", output_.linear_acceleration);
|
||||
}
|
||||
|
||||
const Output& getOutput() const
|
||||
{
|
||||
return output_;
|
||||
}
|
||||
|
||||
protected:
|
||||
void setOutput(const Output& output)
|
||||
{
|
||||
output_ = output;
|
||||
}
|
||||
|
||||
|
||||
private:
|
||||
Output output_;
|
||||
};
|
||||
|
||||
|
||||
}} //namespace
|
||||
#endif
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -1,7 +1,6 @@
|
|||
// Copyright (c) Microsoft Corporation. All rights reserved.
|
||||
// Licensed under the MIT License.
|
||||
|
||||
|
||||
#ifndef msr_airlib_SimpleImu_hpp
|
||||
#define msr_airlib_SimpleImu_hpp
|
||||
|
||||
|
|
@ -9,108 +8,111 @@
|
|||
#include "ImuSimpleParams.hpp"
|
||||
#include "ImuBase.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
class ImuSimple : public ImuBase
|
||||
namespace msr
|
||||
{
|
||||
public:
|
||||
//constructors
|
||||
ImuSimple(const AirSimSettings::ImuSetting& setting = AirSimSettings::ImuSetting())
|
||||
: ImuBase(setting.sensor_name)
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
class ImuSimple : public ImuBase
|
||||
{
|
||||
// initialize params
|
||||
params_.initializeFromSettings(setting);
|
||||
public:
|
||||
//constructors
|
||||
ImuSimple(const AirSimSettings::ImuSetting& setting = AirSimSettings::ImuSetting())
|
||||
: ImuBase(setting.sensor_name)
|
||||
{
|
||||
// initialize params
|
||||
params_.initializeFromSettings(setting);
|
||||
|
||||
gyro_bias_stability_norm = params_.gyro.bias_stability / sqrt(params_.gyro.tau);
|
||||
accel_bias_stability_norm = params_.accel.bias_stability / sqrt(params_.accel.tau);
|
||||
}
|
||||
gyro_bias_stability_norm = params_.gyro.bias_stability / sqrt(params_.gyro.tau);
|
||||
accel_bias_stability_norm = params_.accel.bias_stability / sqrt(params_.accel.tau);
|
||||
}
|
||||
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
last_time_ = clock()->nowNanos();
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
last_time_ = clock()->nowNanos();
|
||||
|
||||
state_.gyroscope_bias = params_.gyro.turn_on_bias;
|
||||
state_.accelerometer_bias = params_.accel.turn_on_bias;
|
||||
gauss_dist.reset();
|
||||
updateOutput();
|
||||
}
|
||||
state_.gyroscope_bias = params_.gyro.turn_on_bias;
|
||||
state_.accelerometer_bias = params_.accel.turn_on_bias;
|
||||
gauss_dist.reset();
|
||||
updateOutput();
|
||||
}
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
ImuBase::update();
|
||||
virtual void update() override
|
||||
{
|
||||
ImuBase::update();
|
||||
|
||||
updateOutput();
|
||||
}
|
||||
//*** End: UpdatableState implementation ***//
|
||||
updateOutput();
|
||||
}
|
||||
//*** End: UpdatableState implementation ***//
|
||||
|
||||
virtual ~ImuSimple() = default;
|
||||
virtual ~ImuSimple() = default;
|
||||
|
||||
private: //methods
|
||||
void updateOutput()
|
||||
{
|
||||
Output output;
|
||||
const GroundTruth& ground_truth = getGroundTruth();
|
||||
private: //methods
|
||||
void updateOutput()
|
||||
{
|
||||
Output output;
|
||||
const GroundTruth& ground_truth = getGroundTruth();
|
||||
|
||||
output.angular_velocity = ground_truth.kinematics->twist.angular;
|
||||
output.linear_acceleration = ground_truth.kinematics->accelerations.linear - ground_truth.environment->getState().gravity;
|
||||
output.orientation = ground_truth.kinematics->pose.orientation;
|
||||
output.angular_velocity = ground_truth.kinematics->twist.angular;
|
||||
output.linear_acceleration = ground_truth.kinematics->accelerations.linear - ground_truth.environment->getState().gravity;
|
||||
output.orientation = ground_truth.kinematics->pose.orientation;
|
||||
|
||||
//acceleration is in world frame so transform to body frame
|
||||
output.linear_acceleration = VectorMath::transformToBodyFrame(output.linear_acceleration,
|
||||
ground_truth.kinematics->pose.orientation, true);
|
||||
//acceleration is in world frame so transform to body frame
|
||||
output.linear_acceleration = VectorMath::transformToBodyFrame(output.linear_acceleration,
|
||||
ground_truth.kinematics->pose.orientation,
|
||||
true);
|
||||
|
||||
//add noise
|
||||
addNoise(output.linear_acceleration, output.angular_velocity);
|
||||
// TODO: Add noise in orientation?
|
||||
//add noise
|
||||
addNoise(output.linear_acceleration, output.angular_velocity);
|
||||
// TODO: Add noise in orientation?
|
||||
|
||||
output.time_stamp = clock()->nowNanos();
|
||||
output.time_stamp = clock()->nowNanos();
|
||||
|
||||
setOutput(output);
|
||||
}
|
||||
setOutput(output);
|
||||
}
|
||||
|
||||
void addNoise(Vector3r& linear_acceleration, Vector3r& angular_velocity)
|
||||
{
|
||||
TTimeDelta dt = clock()->updateSince(last_time_);
|
||||
void addNoise(Vector3r& linear_acceleration, Vector3r& angular_velocity)
|
||||
{
|
||||
TTimeDelta dt = clock()->updateSince(last_time_);
|
||||
|
||||
//ref: An introduction to inertial navigation, Oliver J. Woodman, Sec 3.2, pp 10-12
|
||||
//https://www.cl.cam.ac.uk/techreports/UCAM-CL-TR-696.pdf
|
||||
//ref: An introduction to inertial navigation, Oliver J. Woodman, Sec 3.2, pp 10-12
|
||||
//https://www.cl.cam.ac.uk/techreports/UCAM-CL-TR-696.pdf
|
||||
|
||||
real_T sqrt_dt = static_cast<real_T>(sqrt(std::max<TTimeDelta>(dt, params_.min_sample_time)));
|
||||
real_T sqrt_dt = static_cast<real_T>(sqrt(std::max<TTimeDelta>(dt, params_.min_sample_time)));
|
||||
|
||||
// Gyrosocpe
|
||||
//convert arw to stddev
|
||||
real_T gyro_sigma_arw = params_.gyro.arw / sqrt_dt;
|
||||
angular_velocity += gauss_dist.next() * gyro_sigma_arw + state_.gyroscope_bias;
|
||||
//update bias random walk
|
||||
real_T gyro_sigma_bias = gyro_bias_stability_norm * sqrt_dt;
|
||||
state_.gyroscope_bias += gauss_dist.next() * gyro_sigma_bias;
|
||||
// Gyrosocpe
|
||||
//convert arw to stddev
|
||||
real_T gyro_sigma_arw = params_.gyro.arw / sqrt_dt;
|
||||
angular_velocity += gauss_dist.next() * gyro_sigma_arw + state_.gyroscope_bias;
|
||||
//update bias random walk
|
||||
real_T gyro_sigma_bias = gyro_bias_stability_norm * sqrt_dt;
|
||||
state_.gyroscope_bias += gauss_dist.next() * gyro_sigma_bias;
|
||||
|
||||
//accelerometer
|
||||
//convert vrw to stddev
|
||||
real_T accel_sigma_vrw = params_.accel.vrw / sqrt_dt;
|
||||
linear_acceleration += gauss_dist.next() * accel_sigma_vrw + state_.accelerometer_bias;
|
||||
//update bias random walk
|
||||
real_T accel_sigma_bias = accel_bias_stability_norm * sqrt_dt;
|
||||
state_.accelerometer_bias += gauss_dist.next() * accel_sigma_bias;
|
||||
}
|
||||
//accelerometer
|
||||
//convert vrw to stddev
|
||||
real_T accel_sigma_vrw = params_.accel.vrw / sqrt_dt;
|
||||
linear_acceleration += gauss_dist.next() * accel_sigma_vrw + state_.accelerometer_bias;
|
||||
//update bias random walk
|
||||
real_T accel_sigma_bias = accel_bias_stability_norm * sqrt_dt;
|
||||
state_.accelerometer_bias += gauss_dist.next() * accel_sigma_bias;
|
||||
}
|
||||
|
||||
private: //fields
|
||||
ImuSimpleParams params_;
|
||||
RandomVectorGaussianR gauss_dist = RandomVectorGaussianR(0, 1);
|
||||
|
||||
private: //fields
|
||||
ImuSimpleParams params_;
|
||||
RandomVectorGaussianR gauss_dist = RandomVectorGaussianR(0, 1);
|
||||
//cached calculated values
|
||||
real_T gyro_bias_stability_norm, accel_bias_stability_norm;
|
||||
|
||||
//cached calculated values
|
||||
real_T gyro_bias_stability_norm, accel_bias_stability_norm;
|
||||
struct State
|
||||
{
|
||||
Vector3r gyroscope_bias;
|
||||
Vector3r accelerometer_bias;
|
||||
} state_;
|
||||
|
||||
struct State {
|
||||
Vector3r gyroscope_bias;
|
||||
Vector3r accelerometer_bias;
|
||||
} state_;
|
||||
|
||||
TTimePoint last_time_;
|
||||
};
|
||||
|
||||
|
||||
}} //namespace
|
||||
TTimePoint last_time_;
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -9,15 +9,16 @@
|
|||
#include "common/AirSimSettings.hpp"
|
||||
#include <cmath>
|
||||
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
|
||||
// A description of the parameters:
|
||||
// https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model-and-Intrinsics
|
||||
struct ImuSimpleParams
|
||||
namespace msr
|
||||
{
|
||||
/* ref: Parameter values are for MPU 6000 IMU from InvenSense
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
// A description of the parameters:
|
||||
// https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model-and-Intrinsics
|
||||
struct ImuSimpleParams
|
||||
{
|
||||
/* ref: Parameter values are for MPU 6000 IMU from InvenSense
|
||||
Design and Characterization of a Low Cost MEMS IMU Cluster for Precision Navigation
|
||||
Daniel R. Greenheck, 2009, sec 2.2, pp 17
|
||||
http://epublications.marquette.edu/cgi/viewcontent.cgi?article=1326&context=theses_open
|
||||
|
|
@ -25,52 +26,51 @@ struct ImuSimpleParams
|
|||
https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Datasheet1.pdf
|
||||
For Allan Variance/Deviation plots see http://www.invensense.com/wp-content/uploads/2015/02/MPU-3300-Datasheet.pdf
|
||||
*/
|
||||
struct Gyroscope
|
||||
{
|
||||
//angular random walk (ARW)
|
||||
real_T arw = 0.30f / sqrt(3600.0f) * M_PIf / 180; //deg/sqrt(hour) converted to rad/sqrt(sec)
|
||||
//Bias Stability (tau = 500s)
|
||||
real_T tau = 500;
|
||||
real_T bias_stability = 4.6f / 3600 * M_PIf / 180; //deg/hr converted to rad/sec
|
||||
Vector3r turn_on_bias = Vector3r::Zero(); //assume calibration is done
|
||||
} gyro;
|
||||
struct Gyroscope
|
||||
{
|
||||
//angular random walk (ARW)
|
||||
real_T arw = 0.30f / sqrt(3600.0f) * M_PIf / 180; //deg/sqrt(hour) converted to rad/sqrt(sec)
|
||||
//Bias Stability (tau = 500s)
|
||||
real_T tau = 500;
|
||||
real_T bias_stability = 4.6f / 3600 * M_PIf / 180; //deg/hr converted to rad/sec
|
||||
Vector3r turn_on_bias = Vector3r::Zero(); //assume calibration is done
|
||||
} gyro;
|
||||
|
||||
struct Accelerometer
|
||||
{
|
||||
//velocity random walk (ARW)
|
||||
real_T vrw = 0.24f * EarthUtils::Gravity / 1.0E3f; //mg converted to m/s^2
|
||||
//Bias Stability (tau = 800s)
|
||||
real_T tau = 800;
|
||||
real_T bias_stability = 36.0f * 1E-6f * EarthUtils::Gravity; //ug converted to m/s^2
|
||||
Vector3r turn_on_bias = Vector3r::Zero(); //assume calibration is done
|
||||
} accel;
|
||||
struct Accelerometer
|
||||
{
|
||||
//velocity random walk (ARW)
|
||||
real_T vrw = 0.24f * EarthUtils::Gravity / 1.0E3f; //mg converted to m/s^2
|
||||
//Bias Stability (tau = 800s)
|
||||
real_T tau = 800;
|
||||
real_T bias_stability = 36.0f * 1E-6f * EarthUtils::Gravity; //ug converted to m/s^2
|
||||
Vector3r turn_on_bias = Vector3r::Zero(); //assume calibration is done
|
||||
} accel;
|
||||
|
||||
real_T min_sample_time = 1 / 1000.0f; //internal IMU frequency
|
||||
real_T min_sample_time = 1 / 1000.0f; //internal IMU frequency
|
||||
|
||||
void initializeFromSettings(const AirSimSettings::ImuSetting& settings)
|
||||
{
|
||||
const auto& json = settings.settings;
|
||||
float arw = json.getFloat("AngularRandomWalk", Utils::nan<float>());
|
||||
if (!std::isnan(arw)) {
|
||||
gyro.arw = arw / sqrt(3600.0f) * M_PIf / 180; // //deg/sqrt(hour) converted to rad/sqrt(sec)
|
||||
void initializeFromSettings(const AirSimSettings::ImuSetting& settings)
|
||||
{
|
||||
const auto& json = settings.settings;
|
||||
float arw = json.getFloat("AngularRandomWalk", Utils::nan<float>());
|
||||
if (!std::isnan(arw)) {
|
||||
gyro.arw = arw / sqrt(3600.0f) * M_PIf / 180; // //deg/sqrt(hour) converted to rad/sqrt(sec)
|
||||
}
|
||||
gyro.tau = json.getFloat("GyroBiasStabilityTau", gyro.tau);
|
||||
float bias_stability = json.getFloat("GyroBiasStability", Utils::nan<float>());
|
||||
if (!std::isnan(bias_stability)) {
|
||||
gyro.bias_stability = bias_stability / 3600 * M_PIf / 180; //deg/hr converted to rad/sec
|
||||
}
|
||||
auto vrw = json.getFloat("VelocityRandomWalk", Utils::nan<float>());
|
||||
if (!std::isnan(vrw)) {
|
||||
accel.vrw = vrw * EarthUtils::Gravity / 1.0E3f; //mg converted to m/s^2
|
||||
}
|
||||
accel.tau = json.getFloat("AccelBiasStabilityTau", accel.tau);
|
||||
bias_stability = json.getFloat("AccelBiasStability", Utils::nan<float>());
|
||||
if (!std::isnan(bias_stability)) {
|
||||
accel.bias_stability = bias_stability * 1E-6f * EarthUtils::Gravity; //ug converted to m/s^2
|
||||
}
|
||||
}
|
||||
gyro.tau = json.getFloat("GyroBiasStabilityTau", gyro.tau);
|
||||
float bias_stability = json.getFloat("GyroBiasStability", Utils::nan<float>());
|
||||
if (!std::isnan(bias_stability)) {
|
||||
gyro.bias_stability = bias_stability / 3600 * M_PIf / 180; //deg/hr converted to rad/sec
|
||||
}
|
||||
auto vrw = json.getFloat("VelocityRandomWalk", Utils::nan<float>());
|
||||
if (!std::isnan(vrw)) {
|
||||
accel.vrw = vrw * EarthUtils::Gravity / 1.0E3f; //mg converted to m/s^2
|
||||
}
|
||||
accel.tau = json.getFloat("AccelBiasStabilityTau", accel.tau);
|
||||
bias_stability = json.getFloat("AccelBiasStability", Utils::nan<float>());
|
||||
if (!std::isnan(bias_stability)) {
|
||||
accel.bias_stability = bias_stability * 1E-6f * EarthUtils::Gravity; //ug converted to m/s^2
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
}} //namespace
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -6,38 +6,43 @@
|
|||
|
||||
#include "sensors/SensorBase.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
class LidarBase : public SensorBase {
|
||||
public:
|
||||
LidarBase(const std::string& sensor_name = "")
|
||||
: SensorBase(sensor_name)
|
||||
{}
|
||||
|
||||
public:
|
||||
virtual void reportState(StateReporter& reporter) override
|
||||
class LidarBase : public SensorBase
|
||||
{
|
||||
//call base
|
||||
UpdatableObject::reportState(reporter);
|
||||
public:
|
||||
LidarBase(const std::string& sensor_name = "")
|
||||
: SensorBase(sensor_name)
|
||||
{
|
||||
}
|
||||
|
||||
reporter.writeValue("Lidar-Timestamp", output_.time_stamp);
|
||||
reporter.writeValue("Lidar-NumPoints", static_cast<int>(output_.point_cloud.size() / 3));
|
||||
}
|
||||
public:
|
||||
virtual void reportState(StateReporter& reporter) override
|
||||
{
|
||||
//call base
|
||||
UpdatableObject::reportState(reporter);
|
||||
|
||||
const LidarData& getOutput() const
|
||||
{
|
||||
return output_;
|
||||
}
|
||||
reporter.writeValue("Lidar-Timestamp", output_.time_stamp);
|
||||
reporter.writeValue("Lidar-NumPoints", static_cast<int>(output_.point_cloud.size() / 3));
|
||||
}
|
||||
|
||||
protected:
|
||||
void setOutput(const LidarData& output)
|
||||
{
|
||||
output_ = output;
|
||||
}
|
||||
const LidarData& getOutput() const
|
||||
{
|
||||
return output_;
|
||||
}
|
||||
|
||||
private:
|
||||
LidarData output_;
|
||||
};
|
||||
protected:
|
||||
void setOutput(const LidarData& output)
|
||||
{
|
||||
output_ = output;
|
||||
}
|
||||
|
||||
}} //namespace
|
||||
private:
|
||||
LidarData output_;
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -11,109 +11,110 @@
|
|||
#include "common/DelayLine.hpp"
|
||||
#include "common/FrequencyLimiter.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
class LidarSimple : public LidarBase
|
||||
namespace msr
|
||||
{
|
||||
public:
|
||||
LidarSimple(const AirSimSettings::LidarSetting& setting = AirSimSettings::LidarSetting())
|
||||
: LidarBase(setting.sensor_name)
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
class LidarSimple : public LidarBase
|
||||
{
|
||||
// initialize params
|
||||
params_.initializeFromSettings(setting);
|
||||
public:
|
||||
LidarSimple(const AirSimSettings::LidarSetting& setting = AirSimSettings::LidarSetting())
|
||||
: LidarBase(setting.sensor_name)
|
||||
{
|
||||
// initialize params
|
||||
params_.initializeFromSettings(setting);
|
||||
|
||||
//initialize frequency limiter
|
||||
freq_limiter_.initialize(params_.update_frequency, params_.startup_delay);
|
||||
}
|
||||
//initialize frequency limiter
|
||||
freq_limiter_.initialize(params_.update_frequency, params_.startup_delay);
|
||||
}
|
||||
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
freq_limiter_.reset();
|
||||
last_time_ = clock()->nowNanos();
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
freq_limiter_.reset();
|
||||
last_time_ = clock()->nowNanos();
|
||||
|
||||
updateOutput();
|
||||
}
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
LidarBase::update();
|
||||
|
||||
freq_limiter_.update();
|
||||
|
||||
if (freq_limiter_.isWaitComplete()) {
|
||||
updateOutput();
|
||||
}
|
||||
}
|
||||
|
||||
virtual void reportState(StateReporter& reporter) override
|
||||
{
|
||||
//call base
|
||||
LidarBase::reportState(reporter);
|
||||
virtual void update() override
|
||||
{
|
||||
LidarBase::update();
|
||||
|
||||
reporter.writeValue("Lidar-NumChannels", params_.number_of_channels);
|
||||
reporter.writeValue("Lidar-Range", params_.range);
|
||||
reporter.writeValue("Lidar-FOV-Upper", params_.vertical_FOV_upper);
|
||||
reporter.writeValue("Lidar-FOV-Lower", params_.vertical_FOV_lower);
|
||||
}
|
||||
//*** End: UpdatableState implementation ***//
|
||||
freq_limiter_.update();
|
||||
|
||||
virtual ~LidarSimple() = default;
|
||||
if (freq_limiter_.isWaitComplete()) {
|
||||
updateOutput();
|
||||
}
|
||||
}
|
||||
|
||||
const LidarSimpleParams& getParams() const
|
||||
{
|
||||
return params_;
|
||||
}
|
||||
virtual void reportState(StateReporter& reporter) override
|
||||
{
|
||||
//call base
|
||||
LidarBase::reportState(reporter);
|
||||
|
||||
protected:
|
||||
virtual void getPointCloud(const Pose& lidar_pose, const Pose& vehicle_pose,
|
||||
TTimeDelta delta_time, vector<real_T>& point_cloud, vector<int>& segmentation_cloud) = 0;
|
||||
reporter.writeValue("Lidar-NumChannels", params_.number_of_channels);
|
||||
reporter.writeValue("Lidar-Range", params_.range);
|
||||
reporter.writeValue("Lidar-FOV-Upper", params_.vertical_FOV_upper);
|
||||
reporter.writeValue("Lidar-FOV-Lower", params_.vertical_FOV_lower);
|
||||
}
|
||||
//*** End: UpdatableState implementation ***//
|
||||
|
||||
|
||||
private: //methods
|
||||
void updateOutput()
|
||||
{
|
||||
TTimeDelta delta_time = clock()->updateSince(last_time_);
|
||||
virtual ~LidarSimple() = default;
|
||||
|
||||
point_cloud_.clear();
|
||||
const LidarSimpleParams& getParams() const
|
||||
{
|
||||
return params_;
|
||||
}
|
||||
|
||||
const GroundTruth& ground_truth = getGroundTruth();
|
||||
protected:
|
||||
virtual void getPointCloud(const Pose& lidar_pose, const Pose& vehicle_pose,
|
||||
TTimeDelta delta_time, vector<real_T>& point_cloud, vector<int>& segmentation_cloud) = 0;
|
||||
|
||||
// calculate the pose before obtaining the point-cloud. Before/after is a bit arbitrary
|
||||
// decision here. If the pose can change while obtaining the point-cloud (could happen for drones)
|
||||
// then the pose won't be very accurate either way.
|
||||
//
|
||||
// TODO: Seems like pose is in vehicle inertial-frame (NOT in Global NED frame).
|
||||
// That could be a bit unintuitive but seems consistent with the position/orientation returned as part of
|
||||
// ImageResponse for cameras and pose returned by getCameraInfo API.
|
||||
// Do we need to convert pose to Global NED frame before returning to clients?
|
||||
Pose lidar_pose = params_.relative_pose + ground_truth.kinematics->pose;
|
||||
getPointCloud(params_.relative_pose, // relative lidar pose
|
||||
ground_truth.kinematics->pose, // relative vehicle pose
|
||||
delta_time,
|
||||
point_cloud_,
|
||||
segmentation_cloud_
|
||||
);
|
||||
private: //methods
|
||||
void updateOutput()
|
||||
{
|
||||
TTimeDelta delta_time = clock()->updateSince(last_time_);
|
||||
|
||||
LidarData output;
|
||||
output.point_cloud = point_cloud_;
|
||||
output.time_stamp = clock()->nowNanos();
|
||||
output.pose = lidar_pose;
|
||||
output.segmentation = segmentation_cloud_;
|
||||
point_cloud_.clear();
|
||||
|
||||
last_time_ = output.time_stamp;
|
||||
const GroundTruth& ground_truth = getGroundTruth();
|
||||
|
||||
setOutput(output);
|
||||
}
|
||||
// calculate the pose before obtaining the point-cloud. Before/after is a bit arbitrary
|
||||
// decision here. If the pose can change while obtaining the point-cloud (could happen for drones)
|
||||
// then the pose won't be very accurate either way.
|
||||
//
|
||||
// TODO: Seems like pose is in vehicle inertial-frame (NOT in Global NED frame).
|
||||
// That could be a bit unintuitive but seems consistent with the position/orientation returned as part of
|
||||
// ImageResponse for cameras and pose returned by getCameraInfo API.
|
||||
// Do we need to convert pose to Global NED frame before returning to clients?
|
||||
Pose lidar_pose = params_.relative_pose + ground_truth.kinematics->pose;
|
||||
getPointCloud(params_.relative_pose, // relative lidar pose
|
||||
ground_truth.kinematics->pose, // relative vehicle pose
|
||||
delta_time,
|
||||
point_cloud_,
|
||||
segmentation_cloud_);
|
||||
|
||||
private:
|
||||
LidarSimpleParams params_;
|
||||
vector<real_T> point_cloud_;
|
||||
vector<int> segmentation_cloud_;
|
||||
LidarData output;
|
||||
output.point_cloud = point_cloud_;
|
||||
output.time_stamp = clock()->nowNanos();
|
||||
output.pose = lidar_pose;
|
||||
output.segmentation = segmentation_cloud_;
|
||||
|
||||
FrequencyLimiter freq_limiter_;
|
||||
TTimePoint last_time_;
|
||||
};
|
||||
last_time_ = output.time_stamp;
|
||||
|
||||
}} //namespace
|
||||
setOutput(output);
|
||||
}
|
||||
|
||||
private:
|
||||
LidarSimpleParams params_;
|
||||
vector<real_T> point_cloud_;
|
||||
vector<int> segmentation_cloud_;
|
||||
|
||||
FrequencyLimiter freq_limiter_;
|
||||
TTimePoint last_time_;
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -7,100 +7,101 @@
|
|||
#include "common/Common.hpp"
|
||||
#include "common/AirSimSettings.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
struct LidarSimpleParams
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
// Velodyne VLP-16 Puck config
|
||||
// https://velodynelidar.com/vlp-16.html
|
||||
|
||||
// default settings
|
||||
// TODO: enable reading of these params from AirSim settings
|
||||
|
||||
uint number_of_channels = 16;
|
||||
real_T range = 10000.0f / 100; // meters
|
||||
uint points_per_second = 100000;
|
||||
uint horizontal_rotation_frequency = 10; // rotations/sec
|
||||
real_T horizontal_FOV_start = 0;
|
||||
real_T horizontal_FOV_end = 359;
|
||||
real_T vertical_FOV_upper = -15; // drones -15, car +10
|
||||
real_T vertical_FOV_lower = -45; // drones -45, car -10
|
||||
|
||||
Pose relative_pose {
|
||||
Vector3r(0,0,-1), // position - a little above vehicle (especially for cars) or Vector3r::Zero()
|
||||
Quaternionr::Identity() // orientation - by default Quaternionr(1, 0, 0, 0)
|
||||
};
|
||||
|
||||
bool draw_debug_points = false;
|
||||
std::string data_frame = AirSimSettings::kVehicleInertialFrame;
|
||||
|
||||
bool external_controller = true;
|
||||
|
||||
real_T update_frequency = 10; // Hz
|
||||
real_T startup_delay = 0; // sec
|
||||
|
||||
void initializeFromSettings(const AirSimSettings::LidarSetting& settings)
|
||||
struct LidarSimpleParams
|
||||
{
|
||||
std::string simmode_name = AirSimSettings::singleton().simmode_name;
|
||||
|
||||
const auto& settings_json = settings.settings;
|
||||
number_of_channels = settings_json.getInt("NumberOfChannels", number_of_channels);
|
||||
range = settings_json.getFloat("Range", range);
|
||||
points_per_second = settings_json.getInt("PointsPerSecond", points_per_second);
|
||||
horizontal_rotation_frequency = settings_json.getInt("RotationsPerSecond", horizontal_rotation_frequency);
|
||||
draw_debug_points = settings_json.getBool("DrawDebugPoints", draw_debug_points);
|
||||
data_frame = settings_json.getString("DataFrame", data_frame);
|
||||
external_controller = settings_json.getBool("ExternalController", external_controller);
|
||||
// Velodyne VLP-16 Puck config
|
||||
// https://velodynelidar.com/vlp-16.html
|
||||
|
||||
vertical_FOV_upper = settings_json.getFloat("VerticalFOVUpper", Utils::nan<float>());
|
||||
// default settings
|
||||
// TODO: enable reading of these params from AirSim settings
|
||||
|
||||
// By default, for multirotors the lidars FOV point downwards;
|
||||
// for cars, the lidars FOV is more forward facing.
|
||||
if (std::isnan(vertical_FOV_upper)) {
|
||||
if (simmode_name == AirSimSettings::kSimModeTypeMultirotor)
|
||||
vertical_FOV_upper = -15;
|
||||
else
|
||||
vertical_FOV_upper = +10;
|
||||
uint number_of_channels = 16;
|
||||
real_T range = 10000.0f / 100; // meters
|
||||
uint points_per_second = 100000;
|
||||
uint horizontal_rotation_frequency = 10; // rotations/sec
|
||||
real_T horizontal_FOV_start = 0;
|
||||
real_T horizontal_FOV_end = 359;
|
||||
real_T vertical_FOV_upper = -15; // drones -15, car +10
|
||||
real_T vertical_FOV_lower = -45; // drones -45, car -10
|
||||
|
||||
Pose relative_pose{
|
||||
Vector3r(0, 0, -1), // position - a little above vehicle (especially for cars) or Vector3r::Zero()
|
||||
Quaternionr::Identity() // orientation - by default Quaternionr(1, 0, 0, 0)
|
||||
};
|
||||
|
||||
bool draw_debug_points = false;
|
||||
std::string data_frame = AirSimSettings::kVehicleInertialFrame;
|
||||
|
||||
bool external_controller = true;
|
||||
|
||||
real_T update_frequency = 10; // Hz
|
||||
real_T startup_delay = 0; // sec
|
||||
|
||||
void initializeFromSettings(const AirSimSettings::LidarSetting& settings)
|
||||
{
|
||||
std::string simmode_name = AirSimSettings::singleton().simmode_name;
|
||||
|
||||
const auto& settings_json = settings.settings;
|
||||
number_of_channels = settings_json.getInt("NumberOfChannels", number_of_channels);
|
||||
range = settings_json.getFloat("Range", range);
|
||||
points_per_second = settings_json.getInt("PointsPerSecond", points_per_second);
|
||||
horizontal_rotation_frequency = settings_json.getInt("RotationsPerSecond", horizontal_rotation_frequency);
|
||||
draw_debug_points = settings_json.getBool("DrawDebugPoints", draw_debug_points);
|
||||
data_frame = settings_json.getString("DataFrame", data_frame);
|
||||
external_controller = settings_json.getBool("ExternalController", external_controller);
|
||||
|
||||
vertical_FOV_upper = settings_json.getFloat("VerticalFOVUpper", Utils::nan<float>());
|
||||
|
||||
// By default, for multirotors the lidars FOV point downwards;
|
||||
// for cars, the lidars FOV is more forward facing.
|
||||
if (std::isnan(vertical_FOV_upper)) {
|
||||
if (simmode_name == AirSimSettings::kSimModeTypeMultirotor)
|
||||
vertical_FOV_upper = -15;
|
||||
else
|
||||
vertical_FOV_upper = +10;
|
||||
}
|
||||
|
||||
vertical_FOV_lower = settings_json.getFloat("VerticalFOVLower", Utils::nan<float>());
|
||||
if (std::isnan(vertical_FOV_lower)) {
|
||||
if (simmode_name == AirSimSettings::kSimModeTypeMultirotor)
|
||||
vertical_FOV_lower = -45;
|
||||
else
|
||||
vertical_FOV_lower = -10;
|
||||
}
|
||||
|
||||
horizontal_FOV_start = settings_json.getFloat("HorizontalFOVStart", horizontal_FOV_start);
|
||||
horizontal_FOV_end = settings_json.getFloat("HorizontalFOVEnd", horizontal_FOV_end);
|
||||
|
||||
relative_pose.position = AirSimSettings::createVectorSetting(settings_json, VectorMath::nanVector());
|
||||
auto rotation = AirSimSettings::createRotationSetting(settings_json, AirSimSettings::Rotation::nanRotation());
|
||||
|
||||
if (std::isnan(relative_pose.position.x()))
|
||||
relative_pose.position.x() = 0;
|
||||
if (std::isnan(relative_pose.position.y()))
|
||||
relative_pose.position.y() = 0;
|
||||
if (std::isnan(relative_pose.position.z())) {
|
||||
if (simmode_name == AirSimSettings::kSimModeTypeMultirotor)
|
||||
relative_pose.position.z() = 0;
|
||||
else
|
||||
relative_pose.position.z() = -1; // a little bit above for cars
|
||||
}
|
||||
|
||||
float pitch, roll, yaw;
|
||||
pitch = !std::isnan(rotation.pitch) ? rotation.pitch : 0;
|
||||
roll = !std::isnan(rotation.roll) ? rotation.roll : 0;
|
||||
yaw = !std::isnan(rotation.yaw) ? rotation.yaw : 0;
|
||||
relative_pose.orientation = VectorMath::toQuaternion(
|
||||
Utils::degreesToRadians(pitch), // pitch - rotation around Y axis
|
||||
Utils::degreesToRadians(roll), // roll - rotation around X axis
|
||||
Utils::degreesToRadians(yaw)); // yaw - rotation around Z axis
|
||||
}
|
||||
|
||||
|
||||
vertical_FOV_lower = settings_json.getFloat("VerticalFOVLower", Utils::nan<float>());
|
||||
if (std::isnan(vertical_FOV_lower)) {
|
||||
if (simmode_name == AirSimSettings::kSimModeTypeMultirotor)
|
||||
vertical_FOV_lower = -45;
|
||||
else
|
||||
vertical_FOV_lower = -10;
|
||||
}
|
||||
|
||||
|
||||
horizontal_FOV_start = settings_json.getFloat("HorizontalFOVStart", horizontal_FOV_start);
|
||||
horizontal_FOV_end = settings_json.getFloat("HorizontalFOVEnd", horizontal_FOV_end);
|
||||
|
||||
relative_pose.position = AirSimSettings::createVectorSetting(settings_json, VectorMath::nanVector());
|
||||
auto rotation = AirSimSettings::createRotationSetting(settings_json, AirSimSettings::Rotation::nanRotation());
|
||||
|
||||
if (std::isnan(relative_pose.position.x()))
|
||||
relative_pose.position.x() = 0;
|
||||
if (std::isnan(relative_pose.position.y()))
|
||||
relative_pose.position.y() = 0;
|
||||
if (std::isnan(relative_pose.position.z())) {
|
||||
if (simmode_name == AirSimSettings::kSimModeTypeMultirotor)
|
||||
relative_pose.position.z() = 0;
|
||||
else
|
||||
relative_pose.position.z() = -1; // a little bit above for cars
|
||||
}
|
||||
|
||||
float pitch, roll, yaw;
|
||||
pitch = !std::isnan(rotation.pitch) ? rotation.pitch : 0;
|
||||
roll = !std::isnan(rotation.roll) ? rotation.roll : 0;
|
||||
yaw = !std::isnan(rotation.yaw) ? rotation.yaw : 0;
|
||||
relative_pose.orientation = VectorMath::toQuaternion(
|
||||
Utils::degreesToRadians(pitch), // pitch - rotation around Y axis
|
||||
Utils::degreesToRadians(roll), // roll - rotation around X axis
|
||||
Utils::degreesToRadians(yaw)); // yaw - rotation around Z axis
|
||||
}
|
||||
};
|
||||
|
||||
}} //namespace
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -4,50 +4,52 @@
|
|||
#ifndef msr_airlib_MagnetometerBase_hpp
|
||||
#define msr_airlib_MagnetometerBase_hpp
|
||||
|
||||
|
||||
#include "sensors/SensorBase.hpp"
|
||||
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
class MagnetometerBase : public SensorBase
|
||||
{
|
||||
public:
|
||||
MagnetometerBase(const std::string& sensor_name = "")
|
||||
: SensorBase(sensor_name)
|
||||
{
|
||||
}
|
||||
|
||||
class MagnetometerBase : public SensorBase {
|
||||
public:
|
||||
MagnetometerBase(const std::string& sensor_name = "")
|
||||
: SensorBase(sensor_name)
|
||||
{}
|
||||
public: //types
|
||||
struct Output
|
||||
{ //same fields as ROS message
|
||||
TTimePoint time_stamp;
|
||||
Vector3r magnetic_field_body; //in Gauss
|
||||
vector<real_T> magnetic_field_covariance; //9 elements 3x3 matrix
|
||||
};
|
||||
|
||||
public: //types
|
||||
struct Output { //same fields as ROS message
|
||||
TTimePoint time_stamp;
|
||||
Vector3r magnetic_field_body; //in Gauss
|
||||
vector<real_T> magnetic_field_covariance; //9 elements 3x3 matrix
|
||||
public:
|
||||
virtual void reportState(StateReporter& reporter) override
|
||||
{
|
||||
//call base
|
||||
UpdatableObject::reportState(reporter);
|
||||
|
||||
reporter.writeValue("Mag-Vec", output_.magnetic_field_body);
|
||||
}
|
||||
|
||||
const Output& getOutput() const
|
||||
{
|
||||
return output_;
|
||||
}
|
||||
|
||||
protected:
|
||||
void setOutput(const Output& output)
|
||||
{
|
||||
output_ = output;
|
||||
}
|
||||
|
||||
private:
|
||||
Output output_;
|
||||
};
|
||||
|
||||
|
||||
public:
|
||||
virtual void reportState(StateReporter& reporter) override
|
||||
{
|
||||
//call base
|
||||
UpdatableObject::reportState(reporter);
|
||||
|
||||
reporter.writeValue("Mag-Vec", output_.magnetic_field_body);
|
||||
}
|
||||
|
||||
const Output& getOutput() const
|
||||
{
|
||||
return output_;
|
||||
}
|
||||
|
||||
protected:
|
||||
void setOutput(const Output& output)
|
||||
{
|
||||
output_ = output;
|
||||
}
|
||||
|
||||
private:
|
||||
Output output_;
|
||||
};
|
||||
|
||||
|
||||
}} //namespace
|
||||
#endif
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -12,104 +12,106 @@
|
|||
#include "common/FrequencyLimiter.hpp"
|
||||
#include "common/DelayLine.hpp"
|
||||
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
class MagnetometerSimple : public MagnetometerBase
|
||||
namespace msr
|
||||
{
|
||||
public:
|
||||
MagnetometerSimple(const AirSimSettings::MagnetometerSetting& setting = AirSimSettings::MagnetometerSetting())
|
||||
: MagnetometerBase(setting.sensor_name)
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
class MagnetometerSimple : public MagnetometerBase
|
||||
{
|
||||
// initialize params
|
||||
params_.initializeFromSettings(setting);
|
||||
public:
|
||||
MagnetometerSimple(const AirSimSettings::MagnetometerSetting& setting = AirSimSettings::MagnetometerSetting())
|
||||
: MagnetometerBase(setting.sensor_name)
|
||||
{
|
||||
// initialize params
|
||||
params_.initializeFromSettings(setting);
|
||||
|
||||
noise_vec_ = RandomVectorGaussianR(Vector3r::Zero(), params_.noise_sigma);
|
||||
bias_vec_ = RandomVectorR(-params_.noise_bias, params_.noise_bias).next();
|
||||
noise_vec_ = RandomVectorGaussianR(Vector3r::Zero(), params_.noise_sigma);
|
||||
bias_vec_ = RandomVectorR(-params_.noise_bias, params_.noise_bias).next();
|
||||
|
||||
//initialize frequency limiter
|
||||
freq_limiter_.initialize(params_.update_frequency, params_.startup_delay);
|
||||
delay_line_.initialize(params_.update_latency);
|
||||
}
|
||||
//initialize frequency limiter
|
||||
freq_limiter_.initialize(params_.update_frequency, params_.startup_delay);
|
||||
delay_line_.initialize(params_.update_latency);
|
||||
}
|
||||
|
||||
//*** Start: UpdatableObject implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
//Ground truth is reset before sensors are reset
|
||||
updateReference(getGroundTruth());
|
||||
noise_vec_.reset();
|
||||
//*** Start: UpdatableObject implementation ***//
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
//Ground truth is reset before sensors are reset
|
||||
updateReference(getGroundTruth());
|
||||
noise_vec_.reset();
|
||||
|
||||
freq_limiter_.reset();
|
||||
delay_line_.reset();
|
||||
freq_limiter_.reset();
|
||||
delay_line_.reset();
|
||||
|
||||
delay_line_.push_back(getOutputInternal());
|
||||
}
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
MagnetometerBase::update();
|
||||
|
||||
freq_limiter_.update();
|
||||
|
||||
if (freq_limiter_.isWaitComplete()) {
|
||||
delay_line_.push_back(getOutputInternal());
|
||||
}
|
||||
|
||||
delay_line_.update();
|
||||
virtual void update() override
|
||||
{
|
||||
MagnetometerBase::update();
|
||||
|
||||
if (freq_limiter_.isWaitComplete())
|
||||
setOutput(delay_line_.getOutput());
|
||||
}
|
||||
//*** End: UpdatableObject implementation ***//
|
||||
freq_limiter_.update();
|
||||
|
||||
virtual ~MagnetometerSimple() = default;
|
||||
if (freq_limiter_.isWaitComplete()) {
|
||||
delay_line_.push_back(getOutputInternal());
|
||||
}
|
||||
|
||||
private: //methods
|
||||
void updateReference(const GroundTruth& ground_truth)
|
||||
{
|
||||
switch (params_.ref_source) {
|
||||
case MagnetometerSimpleParams::ReferenceSource::ReferenceSource_Constant:
|
||||
// Constant magnetic field for Seattle
|
||||
magnetic_field_true_ = Vector3r(0.34252f, 0.09805f, 0.93438f);
|
||||
break;
|
||||
case MagnetometerSimpleParams::ReferenceSource::ReferenceSource_DipoleModel:
|
||||
magnetic_field_true_ = EarthUtils::getMagField(ground_truth.environment->getState().geo_point) * 1E4f; //Tesla to Gauss
|
||||
break;
|
||||
default:
|
||||
throw std::invalid_argument("magnetic reference source type is not recognized");
|
||||
delay_line_.update();
|
||||
|
||||
if (freq_limiter_.isWaitComplete())
|
||||
setOutput(delay_line_.getOutput());
|
||||
}
|
||||
}
|
||||
Output getOutputInternal()
|
||||
{
|
||||
Output output;
|
||||
const GroundTruth& ground_truth = getGroundTruth();
|
||||
//*** End: UpdatableObject implementation ***//
|
||||
|
||||
if (params_.dynamic_reference_source)
|
||||
updateReference(ground_truth);
|
||||
virtual ~MagnetometerSimple() = default;
|
||||
|
||||
// Calculate the magnetic field noise.
|
||||
output.magnetic_field_body = VectorMath::transformToBodyFrame(magnetic_field_true_,
|
||||
ground_truth.kinematics->pose.orientation, true) * params_.scale_factor
|
||||
+ noise_vec_.next()
|
||||
+ bias_vec_;
|
||||
private: //methods
|
||||
void updateReference(const GroundTruth& ground_truth)
|
||||
{
|
||||
switch (params_.ref_source) {
|
||||
case MagnetometerSimpleParams::ReferenceSource::ReferenceSource_Constant:
|
||||
// Constant magnetic field for Seattle
|
||||
magnetic_field_true_ = Vector3r(0.34252f, 0.09805f, 0.93438f);
|
||||
break;
|
||||
case MagnetometerSimpleParams::ReferenceSource::ReferenceSource_DipoleModel:
|
||||
magnetic_field_true_ = EarthUtils::getMagField(ground_truth.environment->getState().geo_point) * 1E4f; //Tesla to Gauss
|
||||
break;
|
||||
default:
|
||||
throw std::invalid_argument("magnetic reference source type is not recognized");
|
||||
}
|
||||
}
|
||||
Output getOutputInternal()
|
||||
{
|
||||
Output output;
|
||||
const GroundTruth& ground_truth = getGroundTruth();
|
||||
|
||||
// todo output.magnetic_field_covariance ?
|
||||
output.time_stamp = clock()->nowNanos();
|
||||
if (params_.dynamic_reference_source)
|
||||
updateReference(ground_truth);
|
||||
|
||||
return output;
|
||||
}
|
||||
// Calculate the magnetic field noise.
|
||||
output.magnetic_field_body = VectorMath::transformToBodyFrame(magnetic_field_true_,
|
||||
ground_truth.kinematics->pose.orientation,
|
||||
true) *
|
||||
params_.scale_factor +
|
||||
noise_vec_.next() + bias_vec_;
|
||||
|
||||
private:
|
||||
RandomVectorGaussianR noise_vec_;
|
||||
Vector3r bias_vec_;
|
||||
// todo output.magnetic_field_covariance ?
|
||||
output.time_stamp = clock()->nowNanos();
|
||||
|
||||
Vector3r magnetic_field_true_;
|
||||
MagnetometerSimpleParams params_;
|
||||
return output;
|
||||
}
|
||||
|
||||
private:
|
||||
RandomVectorGaussianR noise_vec_;
|
||||
Vector3r bias_vec_;
|
||||
|
||||
FrequencyLimiter freq_limiter_;
|
||||
DelayLine<Output> delay_line_;
|
||||
};
|
||||
Vector3r magnetic_field_true_;
|
||||
MagnetometerSimpleParams params_;
|
||||
|
||||
}} //namespace
|
||||
#endif
|
||||
FrequencyLimiter freq_limiter_;
|
||||
DelayLine<Output> delay_line_;
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -6,47 +6,48 @@
|
|||
|
||||
#include "common/Common.hpp"
|
||||
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
|
||||
struct MagnetometerSimpleParams
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
enum ReferenceSource {
|
||||
ReferenceSource_Constant,
|
||||
ReferenceSource_DipoleModel
|
||||
};
|
||||
|
||||
Vector3r noise_sigma = Vector3r(0.005f, 0.005f, 0.005f); //5 mgauss as per specs sheet (RMS is same as stddev) https://goo.gl/UOz6FT
|
||||
real_T scale_factor = 1.0f;
|
||||
Vector3r noise_bias = Vector3r(0.0f, 0.0f, 0.0f); //no offset as per specsheet (zero gauss level) https://goo.gl/UOz6FT
|
||||
float ref_update_frequency = 0.2f; //Hz
|
||||
|
||||
//use dipole model if there is enough compute power available
|
||||
bool dynamic_reference_source = true;
|
||||
ReferenceSource ref_source = ReferenceSource::ReferenceSource_DipoleModel;
|
||||
//bool dynamic_reference_source = false;
|
||||
//ReferenceSource ref_source = ReferenceSource::ReferenceSource_Constant;
|
||||
|
||||
//see PX4 param reference for EKF: https://dev.px4.io/en/advanced/parameter_reference.html
|
||||
real_T update_latency = 0.0f; //sec: from PX4 doc
|
||||
real_T update_frequency = 50; //Hz
|
||||
real_T startup_delay = 0; //sec
|
||||
|
||||
void initializeFromSettings(const AirSimSettings::MagnetometerSetting& settings)
|
||||
struct MagnetometerSimpleParams
|
||||
{
|
||||
const auto& json = settings.settings;
|
||||
float noise = json.getFloat("NoiseSigma", noise_sigma.x());
|
||||
noise_sigma = Vector3r(noise, noise, noise);
|
||||
scale_factor = json.getFloat("ScaleFactor", scale_factor);
|
||||
float bias = json.getFloat("NoiseBias", noise_bias.x());
|
||||
noise_bias = Vector3r(bias, bias, bias);
|
||||
update_latency = json.getFloat("UpdateLatency", update_latency);
|
||||
update_frequency = json.getFloat("UpdateFrequency", update_frequency);
|
||||
startup_delay = json.getFloat("StartupDelay", startup_delay);
|
||||
}
|
||||
};
|
||||
enum ReferenceSource
|
||||
{
|
||||
ReferenceSource_Constant,
|
||||
ReferenceSource_DipoleModel
|
||||
};
|
||||
|
||||
Vector3r noise_sigma = Vector3r(0.005f, 0.005f, 0.005f); //5 mgauss as per specs sheet (RMS is same as stddev) https://goo.gl/UOz6FT
|
||||
real_T scale_factor = 1.0f;
|
||||
Vector3r noise_bias = Vector3r(0.0f, 0.0f, 0.0f); //no offset as per specsheet (zero gauss level) https://goo.gl/UOz6FT
|
||||
float ref_update_frequency = 0.2f; //Hz
|
||||
|
||||
}} //namespace
|
||||
//use dipole model if there is enough compute power available
|
||||
bool dynamic_reference_source = true;
|
||||
ReferenceSource ref_source = ReferenceSource::ReferenceSource_DipoleModel;
|
||||
//bool dynamic_reference_source = false;
|
||||
//ReferenceSource ref_source = ReferenceSource::ReferenceSource_Constant;
|
||||
|
||||
//see PX4 param reference for EKF: https://dev.px4.io/en/advanced/parameter_reference.html
|
||||
real_T update_latency = 0.0f; //sec: from PX4 doc
|
||||
real_T update_frequency = 50; //Hz
|
||||
real_T startup_delay = 0; //sec
|
||||
|
||||
void initializeFromSettings(const AirSimSettings::MagnetometerSetting& settings)
|
||||
{
|
||||
const auto& json = settings.settings;
|
||||
float noise = json.getFloat("NoiseSigma", noise_sigma.x());
|
||||
noise_sigma = Vector3r(noise, noise, noise);
|
||||
scale_factor = json.getFloat("ScaleFactor", scale_factor);
|
||||
float bias = json.getFloat("NoiseBias", noise_bias.x());
|
||||
noise_bias = Vector3r(bias, bias, bias);
|
||||
update_latency = json.getFloat("UpdateLatency", update_latency);
|
||||
update_frequency = json.getFloat("UpdateFrequency", update_frequency);
|
||||
startup_delay = json.getFloat("StartupDelay", startup_delay);
|
||||
}
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -7,31 +7,33 @@
|
|||
#include "vehicles/car/firmwares/physxcar/PhysXCarApi.hpp"
|
||||
#include "vehicles/car/firmwares/ardurover/ArduRoverApi.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
class CarApiFactory {
|
||||
public:
|
||||
static std::unique_ptr<CarApiBase> createApi(const AirSimSettings::VehicleSetting* vehicle_setting,
|
||||
std::shared_ptr<SensorFactory> sensor_factory,
|
||||
const Kinematics::State& state, const Environment& environment,
|
||||
const msr::airlib::GeoPoint& home_geopoint)
|
||||
class CarApiFactory
|
||||
{
|
||||
if (vehicle_setting->vehicle_type == AirSimSettings::kVehicleTypeArduRover) {
|
||||
return std::unique_ptr<CarApiBase>(new ArduRoverApi(vehicle_setting, sensor_factory,
|
||||
state, environment, home_geopoint));
|
||||
}
|
||||
else if (vehicle_setting->vehicle_type == "" || //default config
|
||||
vehicle_setting->vehicle_type == AirSimSettings::kVehicleTypePhysXCar) {
|
||||
return std::unique_ptr<CarApiBase>(new PhysXCarApi(vehicle_setting, sensor_factory,
|
||||
state, environment, home_geopoint));
|
||||
}
|
||||
else
|
||||
throw std::runtime_error(Utils::stringf(
|
||||
public:
|
||||
static std::unique_ptr<CarApiBase> createApi(const AirSimSettings::VehicleSetting* vehicle_setting,
|
||||
std::shared_ptr<SensorFactory> sensor_factory,
|
||||
const Kinematics::State& state, const Environment& environment,
|
||||
const msr::airlib::GeoPoint& home_geopoint)
|
||||
{
|
||||
if (vehicle_setting->vehicle_type == AirSimSettings::kVehicleTypeArduRover) {
|
||||
return std::unique_ptr<CarApiBase>(new ArduRoverApi(vehicle_setting, sensor_factory, state, environment, home_geopoint));
|
||||
}
|
||||
else if (vehicle_setting->vehicle_type == "" || //default config
|
||||
vehicle_setting->vehicle_type == AirSimSettings::kVehicleTypePhysXCar) {
|
||||
return std::unique_ptr<CarApiBase>(new PhysXCarApi(vehicle_setting, sensor_factory, state, environment, home_geopoint));
|
||||
}
|
||||
else
|
||||
throw std::runtime_error(Utils::stringf(
|
||||
"Cannot create vehicle config because vehicle name '%s' is not recognized",
|
||||
vehicle_setting->vehicle_name.c_str()));
|
||||
}
|
||||
};
|
||||
|
||||
}} // namespace
|
||||
}
|
||||
};
|
||||
}
|
||||
} // namespace
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -12,150 +12,151 @@
|
|||
#include "sensors/SensorCollection.hpp"
|
||||
#include "sensors/SensorFactory.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
class CarApiBase : public VehicleApiBase
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
public:
|
||||
struct CarControls {
|
||||
float throttle = 0; /* 1 to -1 */
|
||||
float steering = 0; /* 1 to -1 */
|
||||
float brake = 0; /* 1 to -1 */
|
||||
bool handbrake = false;
|
||||
bool is_manual_gear = false;
|
||||
int manual_gear = 0;
|
||||
bool gear_immediate = true;
|
||||
|
||||
CarControls()
|
||||
class CarApiBase : public VehicleApiBase
|
||||
{
|
||||
public:
|
||||
struct CarControls
|
||||
{
|
||||
}
|
||||
CarControls(float throttle_val, float steering_val, float brake_val, bool handbrake_val,
|
||||
bool is_manual_gear_val, int manual_gear_val, bool gear_immediate_val)
|
||||
: throttle(throttle_val), steering(steering_val), brake(brake_val), handbrake(handbrake_val),
|
||||
is_manual_gear(is_manual_gear_val), manual_gear(manual_gear_val), gear_immediate(gear_immediate_val)
|
||||
{
|
||||
}
|
||||
void set_throttle(float throttle_val, bool forward)
|
||||
{
|
||||
if (forward) {
|
||||
is_manual_gear = false;
|
||||
manual_gear = 0;
|
||||
throttle = std::abs(throttle_val);
|
||||
float throttle = 0; /* 1 to -1 */
|
||||
float steering = 0; /* 1 to -1 */
|
||||
float brake = 0; /* 1 to -1 */
|
||||
bool handbrake = false;
|
||||
bool is_manual_gear = false;
|
||||
int manual_gear = 0;
|
||||
bool gear_immediate = true;
|
||||
|
||||
CarControls()
|
||||
{
|
||||
}
|
||||
else {
|
||||
is_manual_gear = false;
|
||||
manual_gear = -1;
|
||||
throttle = - std::abs(throttle_val);
|
||||
CarControls(float throttle_val, float steering_val, float brake_val, bool handbrake_val,
|
||||
bool is_manual_gear_val, int manual_gear_val, bool gear_immediate_val)
|
||||
: throttle(throttle_val), steering(steering_val), brake(brake_val), handbrake(handbrake_val), is_manual_gear(is_manual_gear_val), manual_gear(manual_gear_val), gear_immediate(gear_immediate_val)
|
||||
{
|
||||
}
|
||||
void set_throttle(float throttle_val, bool forward)
|
||||
{
|
||||
if (forward) {
|
||||
is_manual_gear = false;
|
||||
manual_gear = 0;
|
||||
throttle = std::abs(throttle_val);
|
||||
}
|
||||
else {
|
||||
is_manual_gear = false;
|
||||
manual_gear = -1;
|
||||
throttle = -std::abs(throttle_val);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
struct CarState
|
||||
{
|
||||
float speed;
|
||||
int gear;
|
||||
float rpm;
|
||||
float maxrpm;
|
||||
bool handbrake;
|
||||
Kinematics::State kinematics_estimated;
|
||||
uint64_t timestamp;
|
||||
|
||||
CarState()
|
||||
{
|
||||
}
|
||||
|
||||
CarState(float speed_val, int gear_val, float rpm_val, float maxrpm_val, bool handbrake_val,
|
||||
const Kinematics::State& kinematics_estimated_val, uint64_t timestamp_val)
|
||||
: speed(speed_val), gear(gear_val), rpm(rpm_val), maxrpm(maxrpm_val), handbrake(handbrake_val), kinematics_estimated(kinematics_estimated_val), timestamp(timestamp_val)
|
||||
{
|
||||
}
|
||||
|
||||
//shortcuts
|
||||
const Vector3r& getPosition() const
|
||||
{
|
||||
return kinematics_estimated.pose.position;
|
||||
}
|
||||
const Quaternionr& getOrientation() const
|
||||
{
|
||||
return kinematics_estimated.pose.orientation;
|
||||
}
|
||||
};
|
||||
|
||||
public:
|
||||
// TODO: Temporary constructor for the Unity implementation which does not use the new Sensor Configuration Settings implementation.
|
||||
//CarApiBase() {}
|
||||
|
||||
CarApiBase(const AirSimSettings::VehicleSetting* vehicle_setting,
|
||||
std::shared_ptr<SensorFactory> sensor_factory,
|
||||
const Kinematics::State& state, const Environment& environment)
|
||||
{
|
||||
initialize(vehicle_setting, sensor_factory, state, environment);
|
||||
}
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
VehicleApiBase::update();
|
||||
|
||||
getSensors().update();
|
||||
}
|
||||
|
||||
void reportState(StateReporter& reporter) override
|
||||
{
|
||||
getSensors().reportState(reporter);
|
||||
}
|
||||
|
||||
// sensor helpers
|
||||
virtual const SensorCollection& getSensors() const override
|
||||
{
|
||||
return sensors_;
|
||||
}
|
||||
|
||||
SensorCollection& getSensors()
|
||||
{
|
||||
return sensors_;
|
||||
}
|
||||
|
||||
void initialize(const AirSimSettings::VehicleSetting* vehicle_setting,
|
||||
std::shared_ptr<SensorFactory> sensor_factory,
|
||||
const Kinematics::State& state, const Environment& environment)
|
||||
{
|
||||
sensor_factory_ = sensor_factory;
|
||||
|
||||
sensor_storage_.clear();
|
||||
sensors_.clear();
|
||||
|
||||
addSensorsFromSettings(vehicle_setting);
|
||||
|
||||
getSensors().initialize(&state, &environment);
|
||||
}
|
||||
|
||||
void addSensorsFromSettings(const AirSimSettings::VehicleSetting* vehicle_setting)
|
||||
{
|
||||
const auto& sensor_settings = vehicle_setting->sensors;
|
||||
|
||||
sensor_factory_->createSensorsFromSettings(sensor_settings, sensors_, sensor_storage_);
|
||||
}
|
||||
|
||||
virtual void setCarControls(const CarControls& controls) = 0;
|
||||
virtual void updateCarState(const CarState& state) = 0;
|
||||
virtual const CarState& getCarState() const = 0;
|
||||
virtual const CarControls& getCarControls() const = 0;
|
||||
|
||||
virtual ~CarApiBase() = default;
|
||||
|
||||
std::shared_ptr<const SensorFactory> sensor_factory_;
|
||||
SensorCollection sensors_; //maintains sensor type indexed collection of sensors
|
||||
vector<shared_ptr<SensorBase>> sensor_storage_; //RAII for created sensors
|
||||
|
||||
protected:
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
//reset sensors last after their ground truth has been reset
|
||||
getSensors().reset();
|
||||
}
|
||||
};
|
||||
|
||||
struct CarState {
|
||||
float speed;
|
||||
int gear;
|
||||
float rpm;
|
||||
float maxrpm;
|
||||
bool handbrake;
|
||||
Kinematics::State kinematics_estimated;
|
||||
uint64_t timestamp;
|
||||
|
||||
CarState()
|
||||
{
|
||||
}
|
||||
|
||||
CarState(float speed_val, int gear_val, float rpm_val, float maxrpm_val, bool handbrake_val,
|
||||
const Kinematics::State& kinematics_estimated_val, uint64_t timestamp_val)
|
||||
: speed(speed_val), gear(gear_val), rpm(rpm_val), maxrpm(maxrpm_val), handbrake(handbrake_val),
|
||||
kinematics_estimated(kinematics_estimated_val), timestamp(timestamp_val)
|
||||
{
|
||||
}
|
||||
|
||||
//shortcuts
|
||||
const Vector3r& getPosition() const
|
||||
{
|
||||
return kinematics_estimated.pose.position;
|
||||
}
|
||||
const Quaternionr& getOrientation() const
|
||||
{
|
||||
return kinematics_estimated.pose.orientation;
|
||||
}
|
||||
};
|
||||
|
||||
public:
|
||||
|
||||
// TODO: Temporary constructor for the Unity implementation which does not use the new Sensor Configuration Settings implementation.
|
||||
//CarApiBase() {}
|
||||
|
||||
CarApiBase(const AirSimSettings::VehicleSetting* vehicle_setting,
|
||||
std::shared_ptr<SensorFactory> sensor_factory,
|
||||
const Kinematics::State& state, const Environment& environment)
|
||||
{
|
||||
initialize(vehicle_setting, sensor_factory, state, environment);
|
||||
}
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
VehicleApiBase::update();
|
||||
|
||||
getSensors().update();
|
||||
}
|
||||
|
||||
void reportState(StateReporter& reporter) override
|
||||
{
|
||||
getSensors().reportState(reporter);
|
||||
}
|
||||
|
||||
// sensor helpers
|
||||
virtual const SensorCollection& getSensors() const override
|
||||
{
|
||||
return sensors_;
|
||||
}
|
||||
|
||||
SensorCollection& getSensors()
|
||||
{
|
||||
return sensors_;
|
||||
}
|
||||
|
||||
void initialize(const AirSimSettings::VehicleSetting* vehicle_setting,
|
||||
std::shared_ptr<SensorFactory> sensor_factory,
|
||||
const Kinematics::State& state, const Environment& environment)
|
||||
{
|
||||
sensor_factory_ = sensor_factory;
|
||||
|
||||
sensor_storage_.clear();
|
||||
sensors_.clear();
|
||||
|
||||
addSensorsFromSettings(vehicle_setting);
|
||||
|
||||
getSensors().initialize(&state, &environment);
|
||||
}
|
||||
|
||||
void addSensorsFromSettings(const AirSimSettings::VehicleSetting* vehicle_setting)
|
||||
{
|
||||
const auto& sensor_settings = vehicle_setting->sensors;
|
||||
|
||||
sensor_factory_->createSensorsFromSettings(sensor_settings, sensors_, sensor_storage_);
|
||||
}
|
||||
|
||||
virtual void setCarControls(const CarControls& controls) = 0;
|
||||
virtual void updateCarState(const CarState& state) = 0;
|
||||
virtual const CarState& getCarState() const = 0;
|
||||
virtual const CarControls& getCarControls() const = 0;
|
||||
|
||||
virtual ~CarApiBase() = default;
|
||||
|
||||
std::shared_ptr<const SensorFactory> sensor_factory_;
|
||||
SensorCollection sensors_; //maintains sensor type indexed collection of sensors
|
||||
vector<shared_ptr<SensorBase>> sensor_storage_; //RAII for created sensors
|
||||
|
||||
protected:
|
||||
virtual void resetImplementation() override
|
||||
{
|
||||
//reset sensors last after their ground truth has been reset
|
||||
getSensors().reset();
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
}} //namespace
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -14,74 +14,80 @@
|
|||
#include "rpc/msgpack.hpp"
|
||||
#include "common/common_utils/WindowsApisCommonPost.hpp"
|
||||
|
||||
namespace msr { namespace airlib_rpclib {
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib_rpclib
|
||||
{
|
||||
|
||||
class CarRpcLibAdaptors : public RpcLibAdaptorsBase {
|
||||
public:
|
||||
struct CarControls {
|
||||
float throttle = 0;
|
||||
float steering = 0;
|
||||
float brake = 0;
|
||||
bool handbrake = false;
|
||||
bool is_manual_gear = false;
|
||||
int manual_gear = 0;
|
||||
bool gear_immediate = true;
|
||||
|
||||
MSGPACK_DEFINE_MAP(throttle, steering, brake, handbrake, is_manual_gear, manual_gear, gear_immediate);
|
||||
|
||||
CarControls()
|
||||
{}
|
||||
|
||||
CarControls(const msr::airlib::CarApiBase::CarControls& s)
|
||||
class CarRpcLibAdaptors : public RpcLibAdaptorsBase
|
||||
{
|
||||
public:
|
||||
struct CarControls
|
||||
{
|
||||
throttle = s.throttle;
|
||||
steering = s.steering;
|
||||
brake = s.brake;
|
||||
handbrake = s.handbrake;
|
||||
is_manual_gear = s.is_manual_gear;
|
||||
manual_gear = s.manual_gear;
|
||||
gear_immediate = s.gear_immediate;
|
||||
}
|
||||
msr::airlib::CarApiBase::CarControls to() const
|
||||
float throttle = 0;
|
||||
float steering = 0;
|
||||
float brake = 0;
|
||||
bool handbrake = false;
|
||||
bool is_manual_gear = false;
|
||||
int manual_gear = 0;
|
||||
bool gear_immediate = true;
|
||||
|
||||
MSGPACK_DEFINE_MAP(throttle, steering, brake, handbrake, is_manual_gear, manual_gear, gear_immediate);
|
||||
|
||||
CarControls()
|
||||
{
|
||||
}
|
||||
|
||||
CarControls(const msr::airlib::CarApiBase::CarControls& s)
|
||||
{
|
||||
throttle = s.throttle;
|
||||
steering = s.steering;
|
||||
brake = s.brake;
|
||||
handbrake = s.handbrake;
|
||||
is_manual_gear = s.is_manual_gear;
|
||||
manual_gear = s.manual_gear;
|
||||
gear_immediate = s.gear_immediate;
|
||||
}
|
||||
msr::airlib::CarApiBase::CarControls to() const
|
||||
{
|
||||
return msr::airlib::CarApiBase::CarControls(throttle, steering, brake, handbrake, is_manual_gear, manual_gear, gear_immediate);
|
||||
}
|
||||
};
|
||||
|
||||
struct CarState
|
||||
{
|
||||
return msr::airlib::CarApiBase::CarControls(throttle, steering, brake, handbrake,
|
||||
is_manual_gear, manual_gear, gear_immediate);
|
||||
}
|
||||
float speed;
|
||||
int gear;
|
||||
float rpm;
|
||||
float maxrpm;
|
||||
bool handbrake;
|
||||
KinematicsState kinematics_estimated;
|
||||
uint64_t timestamp;
|
||||
|
||||
MSGPACK_DEFINE_MAP(speed, gear, rpm, maxrpm, handbrake, kinematics_estimated, timestamp);
|
||||
|
||||
CarState()
|
||||
{
|
||||
}
|
||||
|
||||
CarState(const msr::airlib::CarApiBase::CarState& s)
|
||||
{
|
||||
speed = s.speed;
|
||||
gear = s.gear;
|
||||
rpm = s.rpm;
|
||||
maxrpm = s.maxrpm;
|
||||
handbrake = s.handbrake;
|
||||
timestamp = s.timestamp;
|
||||
kinematics_estimated = s.kinematics_estimated;
|
||||
}
|
||||
msr::airlib::CarApiBase::CarState to() const
|
||||
{
|
||||
return msr::airlib::CarApiBase::CarState(
|
||||
speed, gear, rpm, maxrpm, handbrake, kinematics_estimated.to(), timestamp);
|
||||
}
|
||||
};
|
||||
};
|
||||
|
||||
struct CarState {
|
||||
float speed;
|
||||
int gear;
|
||||
float rpm;
|
||||
float maxrpm;
|
||||
bool handbrake;
|
||||
KinematicsState kinematics_estimated;
|
||||
uint64_t timestamp;
|
||||
|
||||
MSGPACK_DEFINE_MAP(speed, gear, rpm, maxrpm, handbrake, kinematics_estimated, timestamp);
|
||||
|
||||
CarState()
|
||||
{}
|
||||
|
||||
CarState(const msr::airlib::CarApiBase::CarState& s)
|
||||
{
|
||||
speed = s.speed;
|
||||
gear = s.gear;
|
||||
rpm = s.rpm;
|
||||
maxrpm = s.maxrpm;
|
||||
handbrake = s.handbrake;
|
||||
timestamp = s.timestamp;
|
||||
kinematics_estimated = s.kinematics_estimated;
|
||||
}
|
||||
msr::airlib::CarApiBase::CarState to() const
|
||||
{
|
||||
return msr::airlib::CarApiBase::CarState(
|
||||
speed, gear, rpm, maxrpm, handbrake, kinematics_estimated.to(), timestamp);
|
||||
}
|
||||
};
|
||||
};
|
||||
|
||||
}} //namespace
|
||||
|
||||
}
|
||||
} //namespace
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -11,18 +11,21 @@
|
|||
#include "api/RpcLibClientBase.hpp"
|
||||
#include "common/ImageCaptureBase.hpp"
|
||||
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
class CarRpcLibClient : public RpcLibClientBase
|
||||
{
|
||||
public:
|
||||
CarRpcLibClient(const string& ip_address = "localhost", uint16_t port = RpcLibPort, float timeout_sec = 60);
|
||||
|
||||
class CarRpcLibClient : public RpcLibClientBase {
|
||||
public:
|
||||
CarRpcLibClient(const string& ip_address = "localhost", uint16_t port = RpcLibPort, float timeout_sec = 60);
|
||||
|
||||
void setCarControls(const CarApiBase::CarControls& controls, const std::string& vehicle_name = "");
|
||||
CarApiBase::CarState getCarState(const std::string& vehicle_name = "");
|
||||
CarApiBase::CarControls getCarControls(const std::string& vehicle_name = "");
|
||||
virtual ~CarRpcLibClient(); //required for pimpl
|
||||
};
|
||||
|
||||
}} //namespace
|
||||
void setCarControls(const CarApiBase::CarControls& controls, const std::string& vehicle_name = "");
|
||||
CarApiBase::CarState getCarState(const std::string& vehicle_name = "");
|
||||
CarApiBase::CarControls getCarControls(const std::string& vehicle_name = "");
|
||||
virtual ~CarRpcLibClient(); //required for pimpl
|
||||
};
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -11,22 +11,25 @@
|
|||
#include "api/RpcLibServerBase.hpp"
|
||||
#include "vehicles/car/api/CarApiBase.hpp"
|
||||
|
||||
namespace msr
|
||||
{
|
||||
namespace airlib
|
||||
{
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
class CarRpcLibServer : public RpcLibServerBase {
|
||||
public:
|
||||
CarRpcLibServer(ApiProvider* api_provider, string server_address, uint16_t port = RpcLibPort);
|
||||
virtual ~CarRpcLibServer();
|
||||
|
||||
protected:
|
||||
virtual CarApiBase* getVehicleApi(const std::string& vehicle_name) override
|
||||
class CarRpcLibServer : public RpcLibServerBase
|
||||
{
|
||||
return static_cast<CarApiBase*>(RpcLibServerBase::getVehicleApi(vehicle_name));
|
||||
}
|
||||
};
|
||||
public:
|
||||
CarRpcLibServer(ApiProvider* api_provider, string server_address, uint16_t port = RpcLibPort);
|
||||
virtual ~CarRpcLibServer();
|
||||
|
||||
protected:
|
||||
virtual CarApiBase* getVehicleApi(const std::string& vehicle_name) override
|
||||
{
|
||||
return static_cast<CarApiBase*>(RpcLibServerBase::getVehicleApi(vehicle_name));
|
||||
}
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
}} //namespace
|
||||
}
|
||||
} //namespace
|
||||
#endif
|
||||
Некоторые файлы не были показаны из-за слишком большого количества измененных файлов Показать больше
Загрузка…
Ссылка в новой задаче