зеркало из https://github.com/microsoft/AirSim.git
AirLib compilable
This commit is contained in:
Shital Shah
Родитель
413e44da1e
Коммит
536fc3a548
|
|
@ -80,7 +80,6 @@
|
|||
<ClInclude Include="include\vehicles\multirotor\api\MultirotorRpcLibAdapators.hpp" />
|
||||
<ClInclude Include="include\vehicles\multirotor\api\MultirotorRpcLibClient.hpp" />
|
||||
<ClInclude Include="include\vehicles\multirotor\api\MultirotorRpcLibServer.hpp" />
|
||||
<ClInclude Include="include\vehicles\multirotor\api\RealMultirotorConnector.hpp" />
|
||||
<ClInclude Include="include\vehicles\multirotor\firmwares\simple_flight\AirSimSimpleFlightBoard.hpp" />
|
||||
<ClInclude Include="include\vehicles\multirotor\firmwares\simple_flight\AirSimSimpleFlightCommLink.hpp" />
|
||||
<ClInclude Include="include\vehicles\multirotor\firmwares\simple_flight\AirSimSimpleFlightEstimator.hpp" />
|
||||
|
|
@ -154,8 +153,8 @@
|
|||
<ClInclude Include="include\sensors\magnetometer\MagnetometerSimpleParams.hpp" />
|
||||
<ClInclude Include="include\sensors\SensorBase.hpp" />
|
||||
<ClInclude Include="include\sensors\SensorCollection.hpp" />
|
||||
<ClInclude Include="include\vehicles\multirotor\firmwares\mavlink\Px4MultiRotor.hpp" />
|
||||
<ClInclude Include="include\vehicles\multirotor\firmwares\simple_flight\SimpleFlightQuadX.hpp" />
|
||||
<ClInclude Include="include\vehicles\multirotor\firmwares\mavlink\Px4MultiRotorParams.hpp" />
|
||||
<ClInclude Include="include\vehicles\multirotor\firmwares\simple_flight\SimpleFlightQuadXParams.hpp" />
|
||||
<ClInclude Include="include\vehicles\multirotor\MultiRotor.hpp" />
|
||||
<ClInclude Include="include\vehicles\multirotor\MultiRotorParams.hpp" />
|
||||
<ClInclude Include="include\vehicles\multirotor\MultiRotorParamsFactory.hpp" />
|
||||
|
|
|
|||
|
|
@ -411,9 +411,6 @@
|
|||
<ClInclude Include="include\vehicles\multirotor\api\MultirotorApiBase.h">
|
||||
<Filter>Header Files</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="include\vehicles\multirotor\api\RealMultirotorConnector.hpp">
|
||||
<Filter>Header Files</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="include\api\VehicleConnectorBase.hpp">
|
||||
<Filter>Header Files</Filter>
|
||||
</ClInclude>
|
||||
|
|
@ -426,10 +423,10 @@
|
|||
<ClInclude Include="include\vehicles\multirotor\firmwares\simple_flight\SimpleFlightApi.hpp">
|
||||
<Filter>Header Files</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="include\vehicles\multirotor\firmwares\mavlink\Px4MultiRotor.hpp">
|
||||
<ClInclude Include="include\vehicles\multirotor\firmwares\mavlink\Px4MultiRotorParams.hpp">
|
||||
<Filter>Header Files</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="include\vehicles\multirotor\firmwares\simple_flight\SimpleFlightQuadX.hpp">
|
||||
<ClInclude Include="include\vehicles\multirotor\firmwares\simple_flight\SimpleFlightQuadXParams.hpp">
|
||||
<Filter>Header Files</Filter>
|
||||
</ClInclude>
|
||||
</ItemGroup>
|
||||
|
|
|
|||
|
|
@ -28,17 +28,20 @@ public:
|
|||
return const_cast<WorldSimApiBase*>(getWorldSimApi());
|
||||
}
|
||||
|
||||
virtual const VehicleSimApiBase* getVehicleSimApi(const std::string& vehicle_name = "") const = 0;
|
||||
virtual VehicleSimApiBase* getVehicleSimApi(const std::string& vehicle_name = "")
|
||||
virtual const VehicleSimApiBase* getVehicleSimApi(const std::string& vehicle_name = "") const
|
||||
{
|
||||
auto* world_sim_api = getWorldSimApi();
|
||||
if (world_sim_api) {
|
||||
auto* vehicle_sim_api = getWorldSimApi()->getVehicleSimApi(vehicle_name);
|
||||
return const_cast<VehicleSimApiBase*>(vehicle_sim_api);
|
||||
return vehicle_sim_api;
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
virtual VehicleSimApiBase* getVehicleSimApi(const std::string& vehicle_name = "")
|
||||
{
|
||||
return const_cast<VehicleSimApiBase*>(getVehicleSimApi(vehicle_name));
|
||||
|
||||
}
|
||||
|
||||
virtual ~ApiServerBase() = default;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -27,6 +27,7 @@ public:
|
|||
void resetVehicle();
|
||||
void simResetWorld();
|
||||
bool armDisarm(bool arm);
|
||||
void cancelPendingTasks();
|
||||
|
||||
ConnectionState getConnectionState();
|
||||
bool ping();
|
||||
|
|
@ -59,11 +60,8 @@ public:
|
|||
|
||||
|
||||
protected:
|
||||
void* getClient();
|
||||
const void* getClient() const;
|
||||
void* getClient()
|
||||
{
|
||||
return const_cast<void*>(getClient());
|
||||
}
|
||||
|
||||
private:
|
||||
struct impl;
|
||||
|
|
|
|||
|
|
@ -31,37 +31,58 @@ public:
|
|||
virtual void cancelPendingTasks() = 0;
|
||||
virtual GeoPoint getHomeGeoPoint() const = 0;
|
||||
|
||||
virtual bool isReady(std::string& message) const
|
||||
{
|
||||
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);
|
||||
}
|
||||
|
||||
//below APIs are used by FastPhysicsEngine
|
||||
virtual real_T getActuation(unsigned int actuator_index) const
|
||||
{
|
||||
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
|
||||
}
|
||||
|
||||
virtual CameraInfo getCameraInfo(int camera_id) const = 0;
|
||||
virtual void setCameraOrientation(int camera_id, const Quaternionr& orientation) = 0;
|
||||
|
||||
virtual ~VehicleApiBase() = default;
|
||||
};
|
||||
|
||||
class VehicleControllerException : public std::runtime_error {
|
||||
public:
|
||||
VehicleControllerException(const std::string& message)
|
||||
: runtime_error(message) {
|
||||
}
|
||||
};
|
||||
//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 VehicleCommandNotImplementedException : public VehicleControllerException {
|
||||
public:
|
||||
VehicleCommandNotImplementedException(const std::string& message)
|
||||
: VehicleControllerException(message) {
|
||||
}
|
||||
};
|
||||
|
||||
class VehicleMoveException : public VehicleControllerException {
|
||||
public:
|
||||
VehicleMoveException(const std::string& message)
|
||||
: VehicleControllerException(message) {
|
||||
}
|
||||
class VehicleMoveException : public VehicleControllerException {
|
||||
public:
|
||||
VehicleMoveException(const std::string& message)
|
||||
: VehicleControllerException(message) {
|
||||
}
|
||||
};
|
||||
};
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -7,10 +7,13 @@
|
|||
#include "VehicleApiBase.hpp"
|
||||
#include "common/ImageCaptureBase.hpp"
|
||||
#include "common/UpdatableObject.hpp"
|
||||
#include "physics/Kinematics.hpp"
|
||||
#include "physics/Environment.hpp"
|
||||
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
class VehicleConnectorBase : public UpdatableObject
|
||||
class VehicleSimBridgeBase : public UpdatableObject
|
||||
{
|
||||
public:
|
||||
//pure abstract methods in addition to UpdatableObject
|
||||
|
|
@ -20,16 +23,23 @@ public:
|
|||
//called when render changes are required
|
||||
virtual void updateRendering(float dt) = 0;
|
||||
|
||||
virtual VehicleApiBase* getController() = 0;
|
||||
virtual const VehicleApiBase* getVehicleApi() const = 0;
|
||||
virtual VehicleApiBase* getVehicleApi()
|
||||
{
|
||||
return const_cast<VehicleApiBase*>(getVehicleApi());
|
||||
}
|
||||
|
||||
virtual ImageCaptureBase* getImageCapture() = 0;
|
||||
|
||||
virtual void setPose(const Pose& pose, bool ignore_collision) = 0;
|
||||
virtual Pose getPose() = 0;
|
||||
virtual Pose getPose() const = 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) = 0;
|
||||
virtual int getSegmentationObjectID(const std::string& mesh_name) const = 0;
|
||||
virtual void printLogMessage(const std::string& message, std::string message_param = "", unsigned char severity = 0) = 0;
|
||||
virtual Pose getActorPose(const std::string& actor_name) = 0;
|
||||
virtual Kinematics::State getTrueKinematics() = 0;
|
||||
virtual Pose getActorPose(const std::string& actor_name) const = 0;
|
||||
virtual const Kinematics::State* getGroundTruthKinematics() const = 0;
|
||||
virtual const Environment* getGroundTruthEnvironment() const = 0;
|
||||
virtual CameraInfo getCameraInfo(int camera_id) const = 0;
|
||||
virtual void setCameraOrientation(int camera_id, const Quaternionr& orientation) = 0;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -23,9 +23,11 @@ public:
|
|||
virtual Pose getPose() const = 0;
|
||||
|
||||
virtual CollisionInfo getCollisionInfo() const = 0;
|
||||
virtual const Kinematics::State& getTrueKinematics() const = 0;
|
||||
virtual Kinematics::State getGroundTruthKinematics() const = 0;
|
||||
virtual int getRemoteControlID() const { return -1; }
|
||||
virtual void setRCData(const RCData& rcData) = 0;
|
||||
|
||||
virtual CameraInfo getCameraInfo(int camera_id) const = 0;
|
||||
virtual void setCameraOrientation(int camera_id, const Quaternionr& orientation) = 0;
|
||||
};
|
||||
|
||||
} } //namespace
|
||||
|
|
|
|||
|
|
@ -43,7 +43,10 @@ public:
|
|||
{
|
||||
return ClockFactory::get();
|
||||
}
|
||||
|
||||
virtual const ClockBase* clock() const
|
||||
{
|
||||
return ClockFactory::get();
|
||||
}
|
||||
|
||||
protected:
|
||||
void clearResetUpdateAsserts()
|
||||
|
|
|
|||
|
|
@ -17,7 +17,7 @@ namespace msr { namespace airlib {
|
|||
template <class Vector3T, class QuaternionT, class RealT>
|
||||
class VectorMathT {
|
||||
public:
|
||||
//IMPORTANT: make sure fixed size vectorizable types have no alignment assumption
|
||||
//IMPORTANT: make sure fixed size vectorization types have no alignment assumption
|
||||
//https://eigen.tuxfamily.org/dox/group__TopicUnalignedArrayAssert.html
|
||||
typedef Eigen::Matrix<float, 1, 1> Vector1f;
|
||||
typedef Eigen::Matrix<double, 1, 1> Vector1d;
|
||||
|
|
@ -36,7 +36,7 @@ public:
|
|||
|
||||
typedef common_utils::Utils Utils;
|
||||
//use different seeds for each component
|
||||
//TODO: below we are using double instead of RealT becaise of VC++2017 bug in random implementation
|
||||
//TODO: below we are using double instead of RealT because of VC++2017 bug in random implementation
|
||||
typedef common_utils::RandomGenerator<RealT, std::normal_distribution<double>, 1> RandomGeneratorGausianXT;
|
||||
typedef common_utils::RandomGenerator<RealT, std::normal_distribution<double>, 2> RandomGeneratorGausianYT;
|
||||
typedef common_utils::RandomGenerator<RealT, std::normal_distribution<double>, 3> RandomGeneratorGausianZT;
|
||||
|
|
|
|||
|
|
@ -12,7 +12,7 @@
|
|||
#include <exception>
|
||||
#include <future>
|
||||
#include <mutex>
|
||||
#include "Utils.hpp"
|
||||
#include "common/common_utils/Utils.hpp"
|
||||
#include "ClockFactory.hpp" //TODO: move this out of common_utils
|
||||
#include "CancelToken.hpp"
|
||||
|
||||
|
|
@ -35,13 +35,14 @@ public:
|
|||
|
||||
void execute()
|
||||
{
|
||||
is_complete_ = false;
|
||||
try {
|
||||
executeAction();
|
||||
}
|
||||
finally{
|
||||
is_complete_ = true;
|
||||
}
|
||||
catch(...) {
|
||||
is_complete_ = false;
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
bool isComplete() const
|
||||
|
|
@ -249,9 +250,6 @@ private:
|
|||
//Utils::DebugBreak();
|
||||
Utils::log(Utils::stringf("WorkerThread caught unhandled exception: %s", e.what()), Utils::kLogLevelError);
|
||||
}
|
||||
|
||||
//we use cancel here to communicate to enqueueAndWait that the task is complete.
|
||||
pending->complete();
|
||||
}
|
||||
|
||||
if (!cancel_request_) {
|
||||
|
|
|
|||
|
|
@ -11,7 +11,7 @@
|
|||
#include "common/common_utils/Utils.hpp"
|
||||
#include "IGeoFence.hpp"
|
||||
#include "common/Common.hpp"
|
||||
#include "vehicles/multirotor/controllers/MultirotorCommon.hpp"
|
||||
#include "vehicles/multirotor/api/MultirotorCommon.hpp"
|
||||
#include "common/common_utils/EnumFlags.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
|
@ -45,7 +45,7 @@ public:
|
|||
ObstacleMap::ObstacleInfo cur_obs, dest_obs, suggested_obs;
|
||||
//locations that were considered while evaluation
|
||||
Vector3r cur_pos, dest_pos;
|
||||
//transoformed cur to destination vector in body frame
|
||||
//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
|
||||
|
|
|
|||
|
|
@ -17,8 +17,8 @@ namespace msr { namespace airlib {
|
|||
class CarRpcLibClient : public RpcLibClientBase {
|
||||
public:
|
||||
CarRpcLibClient(const string& ip_address = "localhost", uint16_t port = 41451, uint timeout_ms = 60000);
|
||||
|
||||
void setCarControls(const CarApiBase::CarControls& controls);
|
||||
void reset();
|
||||
CarApiBase::CarState getCarState();
|
||||
|
||||
virtual ~CarRpcLibClient(); //required for pimpl
|
||||
|
|
|
|||
|
|
@ -13,11 +13,29 @@ namespace msr { namespace airlib {
|
|||
|
||||
class CarRpcLibServer : public RpcLibServerBase {
|
||||
public:
|
||||
CarRpcLibServer(WorldSimApiBase* simmode_api, string server_address, uint16_t port = 41451);
|
||||
CarRpcLibServer(VehicleApiBase* vehicle_api, WorldSimApiBase* world_sim_api, string server_address, uint16_t port = 41451);
|
||||
virtual ~CarRpcLibServer();
|
||||
|
||||
virtual const CarApiBase* getVehicleApi(const std::string& vehicle_name = "") const override
|
||||
{
|
||||
return vehicle_api_;
|
||||
}
|
||||
virtual CarApiBase* getVehicleApi(const std::string& vehicle_name = "") override
|
||||
{
|
||||
return const_cast<CarApiBase*>(getVehicleApi(vehicle_name));
|
||||
}
|
||||
virtual const WorldSimApiBase* getWorldSimApi() const override
|
||||
{
|
||||
return world_sim_api_;
|
||||
}
|
||||
virtual WorldSimApiBase* getWorldSimApi() override
|
||||
{
|
||||
return const_cast<WorldSimApiBase*>(getWorldSimApi());
|
||||
}
|
||||
|
||||
private:
|
||||
CarApiBase* getCarApi() const;
|
||||
CarApiBase* vehicle_api_;
|
||||
WorldSimApiBase* world_sim_api_;
|
||||
};
|
||||
|
||||
}} //namespace
|
||||
|
|
|
|||
|
|
@ -8,6 +8,7 @@
|
|||
#include "common/CommonStructs.hpp"
|
||||
#include "Rotor.hpp"
|
||||
#include "api/VehicleApiBase.hpp"
|
||||
#include "api/VehicleSimApiBase.hpp"
|
||||
#include "MultiRotorParams.hpp"
|
||||
#include <vector>
|
||||
#include "physics/PhysicsBody.hpp"
|
||||
|
|
@ -17,44 +18,23 @@ namespace msr { namespace airlib {
|
|||
|
||||
class MultiRotor : public PhysicsBody {
|
||||
public:
|
||||
MultiRotor()
|
||||
{
|
||||
//allow default constructor with later call for initialize
|
||||
}
|
||||
MultiRotor(MultiRotorParams* params, const Kinematics::State& initial_kinematic_state, Environment* environment)
|
||||
MultiRotor(MultiRotorParams* params, VehicleApiBase* vehicle_api,
|
||||
const Kinematics::State& initial_kinematic_state, Environment* environment)
|
||||
: params_(params), vehicle_api_(vehicle_api)
|
||||
{
|
||||
initialize(params, initial_kinematic_state, environment);
|
||||
}
|
||||
void initialize(MultiRotorParams* params, const Pose& initial_pose, const GeoPoint& home_point,
|
||||
std::unique_ptr<Environment>& environment)
|
||||
|
||||
MultiRotor(MultiRotorParams* params, VehicleApiBase* vehicle_api,
|
||||
const Pose& initial_pose, const GeoPoint& home_point, std::unique_ptr<Environment>& environment)
|
||||
: params_(params), vehicle_api_(vehicle_api)
|
||||
{
|
||||
//init physics vehicle
|
||||
auto initial_kinematics = Kinematics::State::zero();
|
||||
initial_kinematics.pose = initial_pose;
|
||||
Environment::State initial_environment;
|
||||
initial_environment.position = initial_kinematics.pose.position;
|
||||
initial_environment.geo_point = home_point;
|
||||
environment.reset(new Environment(initial_environment));
|
||||
|
||||
initialize(params, initial_kinematics, environment.get());
|
||||
}
|
||||
void initialize(MultiRotorParams* params, const Kinematics::State& initial_kinematic_state, Environment* environment)
|
||||
{
|
||||
params_ = params;
|
||||
|
||||
PhysicsBody::initialize(params_->getParams().mass, params_->getParams().inertia, initial_kinematic_state, environment);
|
||||
|
||||
createRotors(*params_, rotors_, environment);
|
||||
createDragVertices();
|
||||
|
||||
initSensors(*params_, getKinematics(), getEnvironment());
|
||||
|
||||
getController()->setSimulatedGroundTruth(& getKinematics(), & getEnvironment());
|
||||
}
|
||||
|
||||
MultirotorApiBase* getController()
|
||||
{
|
||||
return params_->getController();
|
||||
}
|
||||
|
||||
//*** Start: UpdatableState implementation ***//
|
||||
|
|
@ -64,8 +44,8 @@ public:
|
|||
PhysicsBody::reset();
|
||||
|
||||
//reset inputs
|
||||
if (getController())
|
||||
getController()->reset();
|
||||
if (vehicle_api_)
|
||||
vehicle_api_->reset();
|
||||
|
||||
//reset sensors last after their ground truth has been reset
|
||||
resetSensors();
|
||||
|
|
@ -101,12 +81,12 @@ public:
|
|||
{
|
||||
updateSensors(*params_, getKinematics(), getEnvironment());
|
||||
|
||||
getController()->update();
|
||||
vehicle_api_->update();
|
||||
|
||||
//transfer new input values from controller to rotors
|
||||
for (uint rotor_index = 0; rotor_index < rotors_.size(); ++rotor_index) {
|
||||
rotors_.at(rotor_index).setControlSignal(
|
||||
getController()->getRotorActuation(rotor_index));
|
||||
vehicle_api_->getActuation(rotor_index));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -157,15 +137,19 @@ public:
|
|||
return rotors_.at(rotor_index).getOutput();
|
||||
}
|
||||
|
||||
virtual void setCollisionInfo(const CollisionInfo& collision_info) override
|
||||
{
|
||||
PhysicsBody::setCollisionInfo(collision_info);
|
||||
getController()->setCollisionInfo(collision_info);
|
||||
}
|
||||
|
||||
virtual ~MultiRotor() = default;
|
||||
|
||||
private: //methods
|
||||
void initialize(MultiRotorParams* params, const Kinematics::State& initial_kinematic_state, Environment* environment)
|
||||
{
|
||||
PhysicsBody::initialize(params_->getParams().mass, params_->getParams().inertia, initial_kinematic_state, environment);
|
||||
|
||||
createRotors(*params_, rotors_, environment);
|
||||
createDragVertices();
|
||||
|
||||
initSensors(*params_, getKinematics(), getEnvironment());
|
||||
}
|
||||
|
||||
static void createRotors(const MultiRotorParams& params, vector<Rotor>& rotors, const Environment* environment)
|
||||
{
|
||||
rotors.clear();
|
||||
|
|
@ -234,6 +218,8 @@ private: //fields
|
|||
//let us be the owner of rotors object
|
||||
vector<Rotor> rotors_;
|
||||
vector<PhysicsBodyVertex> drag_vertices_;
|
||||
|
||||
VehicleApiBase* vehicle_api_;
|
||||
};
|
||||
|
||||
}} //namespace
|
||||
|
|
|
|||
|
|
@ -7,9 +7,7 @@
|
|||
#include "common/Common.hpp"
|
||||
#include "RotorParams.hpp"
|
||||
#include "sensors/SensorCollection.hpp"
|
||||
#include "api/MultirotorApiBase.h"
|
||||
|
||||
|
||||
#include "vehicles/multirotor/api/MultirotorApiBase.h"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
|
|
@ -57,7 +55,15 @@ public: //types
|
|||
RotorParams rotor_params;
|
||||
};
|
||||
|
||||
|
||||
protected: //must override by derived class
|
||||
virtual void setupParams() = 0;
|
||||
virtual std::unique_ptr<SensorBase> createSensor(SensorBase::SensorType sensor_type) = 0;
|
||||
|
||||
public: //interface
|
||||
virtual std::unique_ptr<MultirotorApiBase> createMultirotorApi() = 0;
|
||||
|
||||
virtual ~MultiRotorParams() = default;
|
||||
virtual void initialize()
|
||||
{
|
||||
sensor_storage_.clear();
|
||||
|
|
@ -66,7 +72,6 @@ public: //interface
|
|||
setupParams();
|
||||
|
||||
addEnabledSensors(params_.enabled_sensors);
|
||||
controller_ = createController();
|
||||
}
|
||||
|
||||
const Params& getParams() const
|
||||
|
|
@ -85,14 +90,6 @@ public: //interface
|
|||
{
|
||||
return sensors_;
|
||||
}
|
||||
MultirotorApiBase* getController()
|
||||
{
|
||||
return controller_.get();
|
||||
}
|
||||
const MultirotorApiBase* getController() const
|
||||
{
|
||||
return controller_.get();
|
||||
}
|
||||
|
||||
void addEnabledSensors(const EnabledSensors& enabled_sensors)
|
||||
{
|
||||
|
|
@ -120,14 +117,6 @@ public: //interface
|
|||
return nullptr;
|
||||
}
|
||||
|
||||
|
||||
virtual ~MultiRotorParams() = default;
|
||||
|
||||
protected: //must override by derived class
|
||||
virtual void setupParams() = 0;
|
||||
virtual std::unique_ptr<SensorBase> createSensor(SensorBase::SensorType sensor_type) = 0;
|
||||
virtual std::unique_ptr<MultirotorApiBase> createController() = 0;
|
||||
|
||||
protected: //static utility functions for derived classes to use
|
||||
|
||||
/// Initializes 4 rotors in the usual QuadX pattern: http://ardupilot.org/copter/_images/MOTORS_QuadX_QuadPlus.jpg
|
||||
|
|
@ -214,7 +203,7 @@ protected: //static utility functions for derived classes to use
|
|||
}
|
||||
|
||||
/// Initialize the rotor_poses given the rotor_count, the arm lengths and the arm angles (relative to forwards vector).
|
||||
/// Also provide the direction you want to spin each rotor and the z-offsetof the rotors relative to the center of gravity.
|
||||
/// Also provide the direction you want to spin each rotor and the z-offset of the rotors relative to the center of gravity.
|
||||
static void initializeRotors(vector<RotorPose>& rotor_poses, uint rotor_count, real_T arm_lengths[], real_T arm_angles[], RotorTurningDirection rotor_directions[], real_T rotor_z /* z relative to center of gravity */)
|
||||
{
|
||||
Vector3r unit_z(0, 0, -1); //NED frame
|
||||
|
|
@ -249,7 +238,6 @@ private:
|
|||
Params params_;
|
||||
SensorCollection sensors_; //maintains sensor type indexed collection of sensors
|
||||
vector<unique_ptr<SensorBase>> sensor_storage_; //RAII for created sensors
|
||||
std::unique_ptr<MultirotorApiBase> controller_;
|
||||
};
|
||||
|
||||
}} //namespace
|
||||
|
|
|
|||
|
|
@ -5,8 +5,8 @@
|
|||
#define msr_airlib_vehicles_MultiRotorParamsFactory_hpp
|
||||
|
||||
#include "vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp"
|
||||
#include "vehicles/multirotor/firmwares/mavlink/Px4MultiRotor.hpp"
|
||||
#include "vehicles/multirotor/firmwares/simple_flight/SimpleFlightQuadX.hpp"
|
||||
#include "vehicles/multirotor/firmwares/mavlink/Px4MultiRotorParams.hpp"
|
||||
#include "vehicles/multirotor/firmwares/simple_flight/SimpleFlightQuadXParams.hpp"
|
||||
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
|
@ -19,11 +19,11 @@ public:
|
|||
std::unique_ptr<MultiRotorParams> config;
|
||||
|
||||
if (vehicle_setting->vehicle_type == AirSimSettings::kVehicleTypePX4) {
|
||||
config.reset(new Px4MultiRotor(* static_cast<const AirSimSettings::PX4VehicleSetting*>(vehicle_setting),
|
||||
config.reset(new Px4MultiRotorParams(* static_cast<const AirSimSettings::PX4VehicleSetting*>(vehicle_setting),
|
||||
sensor_factory));
|
||||
} else if (vehicle_setting->vehicle_type == "" || //default config
|
||||
vehicle_setting->vehicle_type == AirSimSettings::kVehicleTypeSimpleFlight) {
|
||||
config.reset(new SimpleFlightQuadX(vehicle_setting, sensor_factory));
|
||||
config.reset(new SimpleFlightQuadXParams(vehicle_setting, sensor_factory));
|
||||
} else
|
||||
throw std::runtime_error(Utils::stringf(
|
||||
"Cannot create vehicle config because vehicle name '%s' is not recognized",
|
||||
|
|
|
|||
|
|
@ -50,7 +50,7 @@ public: //methods
|
|||
PhysicsBodyVertex::initialize(position, normal); //call base initializer
|
||||
}
|
||||
|
||||
//0 to 1 - will be scalled to 0 to max_speed
|
||||
//0 to 1 - will be scaled to 0 to max_speed
|
||||
void setControlSignal(real_T control_signal)
|
||||
{
|
||||
control_signal_filter_.setInput(Utils::clip(control_signal, 0.0f, 1.0f));
|
||||
|
|
|
|||
|
|
@ -37,7 +37,7 @@ namespace msr {
|
|||
real_T air_density = 1.225f; // kg/m^3
|
||||
real_T max_rpm = 6396.667f; // revolutions per minute
|
||||
real_T propeller_diameter = 0.2286f; //diameter in meters, default is for DJI Phantom 2
|
||||
real_T propeller_height = 1 / 100.0f; //height of cylinderical area when propeller rotates, 1 cm
|
||||
real_T propeller_height = 1 / 100.0f; //height of cylindrical area when propeller rotates, 1 cm
|
||||
real_T control_signal_filter_tc = 0.005f; //time constant for low pass filter
|
||||
|
||||
real_T revolutions_per_second;
|
||||
|
|
@ -46,7 +46,7 @@ namespace msr {
|
|||
real_T max_thrust = 4.179446268f; //computed from above formula for the given constants
|
||||
real_T max_torque = 0.055562f; //computed from above formula
|
||||
|
||||
// call this method to recaculate thrust if you want to use different numbers for C_T, C_P, max_rpm, etc.
|
||||
// call this method to recalculate thrust if you want to use different numbers for C_T, C_P, max_rpm, etc.
|
||||
void calculateMaxThrust() {
|
||||
revolutions_per_second = max_rpm / 60;
|
||||
max_speed = revolutions_per_second * 2 * M_PIf; // radians / sec
|
||||
|
|
|
|||
|
|
@ -5,9 +5,10 @@
|
|||
#define air_DroneControlServer_hpp
|
||||
|
||||
#include "common/Common.hpp"
|
||||
#include "MultirotorCommon.hpp";
|
||||
#include "MultirotorCommon.hpp"
|
||||
#include "safety/SafetyEval.hpp"
|
||||
#include "physics/Kinematics.hpp"
|
||||
#include "physics/Environment.hpp"
|
||||
#include "api/VehicleApiBase.hpp"
|
||||
|
||||
#include <atomic>
|
||||
|
|
@ -20,7 +21,7 @@ namespace msr { namespace airlib {
|
|||
|
||||
class MultirotorApiBase : public VehicleApiBase {
|
||||
|
||||
public: //must be implemented
|
||||
protected: //must be implemented
|
||||
|
||||
/************************* low level move APIs *********************************/
|
||||
virtual void commandRollPitchZ(float pitch, float roll, float z, float yaw) = 0;
|
||||
|
|
@ -33,9 +34,8 @@ public: //must be implemented
|
|||
virtual Kinematics::State getKinematicsEstimated() const = 0;
|
||||
virtual LandedState getLandedState() const = 0;
|
||||
virtual GeoPoint getGpsLocation() const = 0;
|
||||
virtual const MultirotorApiParams& getVehicleParams() const = 0;
|
||||
virtual real_T getRotorActuation(unsigned int rotor_index) const = 0;
|
||||
virtual size_t getRotorCount() const = 0;
|
||||
virtual const MultirotorApiParams& getMultirotorApiParams() const = 0;
|
||||
|
||||
virtual RCData getRCData() const = 0; //get reading from RC bound to vehicle
|
||||
|
||||
/************************* basic config APIs *********************************/
|
||||
|
|
@ -45,6 +45,32 @@ public: //must be implemented
|
|||
//the difference between two position cancels out transitional errors. Typically this would be 0.1m or lower.
|
||||
virtual float getDistanceAccuracy() const = 0;
|
||||
|
||||
protected: //optional overrides but recommended, default values may work
|
||||
virtual float getAutoLookahead(float velocity, float adaptive_lookahead,
|
||||
float max_factor = 40, float min_factor = 30) const;
|
||||
virtual float getObsAvoidanceVelocity(float risk_dist, float max_obs_avoidance_vel) const;
|
||||
|
||||
//below methods gets called by default implementations of move-related commands that would use a long
|
||||
//running loop. These can be used by derived classes to do some init/cleanup.
|
||||
virtual void beforeTask()
|
||||
{
|
||||
//default is do nothing
|
||||
}
|
||||
virtual void afterTask()
|
||||
{
|
||||
//default is do nothing
|
||||
}
|
||||
|
||||
public: //optional overrides
|
||||
virtual void moveByRC(const RCData& rc_data);
|
||||
|
||||
//below method exist for any firmwares that may want to use ground truth for debugging purposes
|
||||
virtual void setSimulatedGroundTruth(const Kinematics::State* kinematics, const Environment* environment)
|
||||
{
|
||||
unused(kinematics);
|
||||
unused(environment);
|
||||
}
|
||||
|
||||
public: //these APIs uses above low level APIs
|
||||
virtual ~MultirotorApiBase() = default;
|
||||
|
||||
|
|
@ -69,7 +95,7 @@ public: //these APIs uses above low level APIs
|
|||
virtual bool rotateByYawRate(float yaw_rate, float duration);
|
||||
virtual bool hover();
|
||||
virtual RCData estimateRCTrims(float trimduration = 1, float minCountForTrim = 10, float maxTrim = 100);
|
||||
|
||||
|
||||
/************************* Safety APIs *********************************/
|
||||
virtual void setSafetyEval(const shared_ptr<SafetyEval> safety_eval_ptr);
|
||||
virtual bool setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy,
|
||||
|
|
@ -95,29 +121,6 @@ public: //these APIs uses above low level APIs
|
|||
token_.cancel();
|
||||
}
|
||||
|
||||
//below method exist for any firmwares that may want to use ground truth for debugging purposes
|
||||
virtual void setSimulatedGroundTruth(const Kinematics::State* kinematics, const Environment* environment)
|
||||
{
|
||||
unused(kinematics);
|
||||
unused(environment);
|
||||
}
|
||||
|
||||
protected: //optional overrides but recommended, default values may work
|
||||
virtual float getAutoLookahead(float velocity, float adaptive_lookahead,
|
||||
float max_factor = 40, float min_factor = 30) const;
|
||||
virtual float getObsAvoidanceVelocity(float risk_dist, float max_obs_avoidance_vel) const;
|
||||
|
||||
//below methods gets called by default implementations of move-related commands that would use a long
|
||||
//running loop. These can be used by derived classes to do some init/cleanup.
|
||||
virtual void beforeTask()
|
||||
{
|
||||
//default is do nothing
|
||||
}
|
||||
virtual void afterTask()
|
||||
{
|
||||
//default is do nothing
|
||||
}
|
||||
|
||||
protected: //utility methods
|
||||
typedef std::function<bool()> WaitFunction;
|
||||
|
||||
|
|
@ -155,11 +158,6 @@ protected: //utility methods
|
|||
{
|
||||
return getKinematicsEstimated().pose.orientation;
|
||||
}
|
||||
ClockBase* clock() const
|
||||
{
|
||||
return ClockFactory::get();
|
||||
}
|
||||
|
||||
|
||||
CancelToken& getCancelToken()
|
||||
{
|
||||
|
|
@ -222,6 +220,7 @@ protected: //types
|
|||
|
||||
class SingleTaskCall : public SingleCall
|
||||
{
|
||||
public:
|
||||
SingleTaskCall(MultirotorApiBase* api)
|
||||
: SingleCall(api)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -77,14 +77,24 @@ struct MultirotorState {
|
|||
|
||||
MultirotorState()
|
||||
{}
|
||||
MultirotorState(const CollisionInfo& collision_val, const Kinematics::State& kinematics_true_val,
|
||||
const Kinematics::State& kinematics_estimated_val, const GeoPoint& gps_location_val, uint64_t timestamp_val,
|
||||
LandedState landed_state_val, const RCData& rc_data_val, const std::vector<std::string>& controller_messages_val)
|
||||
MultirotorState(const CollisionInfo& collision_val, const Kinematics::State& kinematics_estimated_val,
|
||||
const GeoPoint& gps_location_val, uint64_t timestamp_val,
|
||||
LandedState landed_state_val, const RCData& rc_data_val)
|
||||
: collision(collision_val), kinematics_estimated(kinematics_estimated_val),
|
||||
gps_location(gps_location_val), timestamp(timestamp_val),
|
||||
landed_state(landed_state_val), rc_data(rc_data_val)
|
||||
{
|
||||
}
|
||||
|
||||
//shortcuts
|
||||
const Vector3r& getPosition() const
|
||||
{
|
||||
return kinematics_estimated.pose.position;
|
||||
}
|
||||
const Quaternionr& getOrientation() const
|
||||
{
|
||||
return kinematics_estimated.pose.orientation;
|
||||
}
|
||||
};
|
||||
|
||||
}} //namespace
|
||||
|
|
|
|||
|
|
@ -7,8 +7,8 @@
|
|||
#include "common/Common.hpp"
|
||||
#include "common/CommonStructs.hpp"
|
||||
#include "api/RpcLibAdapatorsBase.hpp"
|
||||
#include "vehicles/multirotor/controllers/MultirotorCommon.hpp"
|
||||
#include "vehicles/multirotor/controllers/MultirotorApiBase.h"
|
||||
#include "vehicles/multirotor/api/MultirotorCommon.hpp"
|
||||
#include "vehicles/multirotor/api/MultirotorApiBase.h"
|
||||
#include "common/ImageCaptureBase.hpp"
|
||||
#include "safety/SafetyEval.hpp"
|
||||
#include "rpc/msgpack.hpp"
|
||||
|
|
@ -48,7 +48,7 @@ public:
|
|||
std::vector<std::string> controller_messages;
|
||||
|
||||
|
||||
MSGPACK_DEFINE_MAP(collision, kinematics_estimated, kinematics_true, gps_location, timestamp, landed_state, rc_data, controller_messages);
|
||||
MSGPACK_DEFINE_MAP(collision, kinematics_estimated, gps_location, timestamp, landed_state, rc_data);
|
||||
|
||||
MultirotorState()
|
||||
{}
|
||||
|
|
@ -57,18 +57,16 @@ public:
|
|||
{
|
||||
collision = s.collision;
|
||||
kinematics_estimated = s.kinematics_estimated;
|
||||
kinematics_true = s.kinematics_true;
|
||||
gps_location = s.gps_location;
|
||||
timestamp = s.timestamp;
|
||||
landed_state = s.landed_state;
|
||||
rc_data = RCData(s.rc_data);
|
||||
controller_messages = s.controller_messages;
|
||||
}
|
||||
|
||||
msr::airlib::MultirotorState to() const
|
||||
{
|
||||
return msr::airlib::MultirotorState(collision.to(), kinematics_estimated.to(),
|
||||
kinematics_true.to(), gps_location.to(), timestamp, landed_state, rc_data.to(), controller_messages);
|
||||
gps_location.to(), timestamp, landed_state, rc_data.to());
|
||||
}
|
||||
};
|
||||
};
|
||||
|
|
|
|||
|
|
@ -8,9 +8,9 @@
|
|||
#include <functional>
|
||||
#include "common/CommonStructs.hpp"
|
||||
#include "common/ImageCaptureBase.hpp"
|
||||
#include "vehicles/multirotor/controllers/MultirotorApiBase.h"
|
||||
#include "vehicles/multirotor/api/MultirotorApiBase.h"
|
||||
#include "api/RpcLibClientBase.hpp"
|
||||
#include "vehicles/multirotor/controllers/MultirotorCommon.hpp"
|
||||
#include "vehicles/multirotor/api/MultirotorCommon.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
|
|
@ -39,9 +39,9 @@ public:
|
|||
bool rotateToYaw(float yaw, float max_wait_seconds = 60, float margin = 5);
|
||||
bool rotateByYawRate(float yaw_rate, float duration);
|
||||
bool hover();
|
||||
void moveByRC(const RCData& rc_data);
|
||||
|
||||
MultirotorState getMultirotorState();
|
||||
void setRCData(const RCData& rc_data);
|
||||
|
||||
bool setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy,
|
||||
float obs_avoidance_vel, const Vector3r& origin, float xy_length, float max_z, float min_z);
|
||||
|
|
|
|||
|
|
@ -6,19 +6,37 @@
|
|||
|
||||
#include "common/Common.hpp"
|
||||
#include <functional>
|
||||
#include "vehicles/multirotor/api/MultirotorApiBase.h"
|
||||
#include "api/RpcLibServerBase.hpp"
|
||||
#include "vehicles/multirotor/api/MultirotorApiBase.h"
|
||||
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
class MultirotorRpcLibServer : public RpcLibServerBase {
|
||||
public:
|
||||
MultirotorRpcLibServer(WorldSimApiBase* simmode_api, string server_address, uint16_t port = 41451);
|
||||
MultirotorRpcLibServer(MultirotorApiBase* vehicle_api, WorldSimApiBase* world_sim_api, string server_address, uint16_t port = 41451);
|
||||
virtual ~MultirotorRpcLibServer();
|
||||
|
||||
virtual const MultirotorApiBase* getVehicleApi(const std::string& vehicle_name = "") const override
|
||||
{
|
||||
return vehicle_api_;
|
||||
}
|
||||
virtual MultirotorApiBase* getVehicleApi(const std::string& vehicle_name = "") override
|
||||
{
|
||||
return const_cast<MultirotorApiBase*>(getVehicleApi(vehicle_name));
|
||||
}
|
||||
virtual const WorldSimApiBase* getWorldSimApi() const override
|
||||
{
|
||||
return world_sim_api_;
|
||||
}
|
||||
virtual WorldSimApiBase* getWorldSimApi() override
|
||||
{
|
||||
return const_cast<WorldSimApiBase*>(getWorldSimApi());
|
||||
}
|
||||
|
||||
private:
|
||||
MultirotorApiBase* getDroneApi() const;
|
||||
MultirotorApiBase* vehicle_api_;
|
||||
WorldSimApiBase* world_sim_api_;
|
||||
};
|
||||
|
||||
}} //namespace
|
||||
|
|
|
|||
|
|
@ -1,104 +0,0 @@
|
|||
// Copyright (c) Microsoft Corporation. All rights reserved.
|
||||
// Licensed under the MIT License.
|
||||
|
||||
#ifndef air_RealMultirotorConnector_hpp
|
||||
#define air_RealMultirotorConnector_hpp
|
||||
|
||||
#include "controllers/VehicleConnectorBase.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
class RealMultirotorConnector : public VehicleConnectorBase
|
||||
{
|
||||
public:
|
||||
RealMultirotorConnector(VehicleApiBase* controller)
|
||||
: controller_(controller)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void updateRenderedState(float dt) override
|
||||
{
|
||||
unused(dt);
|
||||
}
|
||||
|
||||
virtual void updateRendering(float dt) override
|
||||
{
|
||||
unused(dt);
|
||||
}
|
||||
|
||||
virtual VehicleApiBase* getController() override
|
||||
{
|
||||
return controller_;
|
||||
}
|
||||
|
||||
virtual ImageCaptureBase* getImageCapture() override
|
||||
{
|
||||
//TODO: we need to support this but with only scene image type
|
||||
throw std::logic_error("getImageCapture() call is only supported for simulation");
|
||||
}
|
||||
|
||||
virtual void setPose(const Pose& pose, bool ignore_collision) override
|
||||
{
|
||||
unused(pose);
|
||||
unused(ignore_collision);
|
||||
throw std::logic_error("setPose() call is only supported for simulation");
|
||||
}
|
||||
|
||||
virtual Pose getPose() override
|
||||
{
|
||||
throw std::logic_error("getPose() call is only supported for simulation");
|
||||
}
|
||||
|
||||
virtual bool setSegmentationObjectID(const std::string& mesh_name, int object_id,
|
||||
bool is_name_regex = false) override
|
||||
{
|
||||
unused(mesh_name);
|
||||
unused(object_id);
|
||||
unused(is_name_regex);
|
||||
throw std::logic_error("setSegmentationObjectID() call is only supported for simulation");
|
||||
}
|
||||
virtual int getSegmentationObjectID(const std::string& mesh_name) override
|
||||
{
|
||||
unused(mesh_name);
|
||||
throw std::logic_error("getSegmentationObjectID() call is only supported for simulation");
|
||||
}
|
||||
|
||||
virtual Kinematics::State getTrueKinematics() override
|
||||
{
|
||||
throw std::logic_error("getTrueKinematics() call is only supported for simulation");
|
||||
}
|
||||
|
||||
virtual void printLogMessage(const std::string& message, std::string message_param = "", unsigned char severity = 0) override
|
||||
{
|
||||
unused(message);
|
||||
unused(message_param);
|
||||
unused(severity);
|
||||
}
|
||||
|
||||
virtual Pose getActorPose(const std::string& actor_name) override
|
||||
{
|
||||
unused(actor_name);
|
||||
return msr::airlib::Pose();
|
||||
}
|
||||
|
||||
virtual CameraInfo getCameraInfo(int camera_id) const override
|
||||
{
|
||||
unused(camera_id);
|
||||
throw std::logic_error("getCameraInfo() call is not implemented for this vehicle");
|
||||
}
|
||||
|
||||
virtual void setCameraOrientation(int camera_id, const Quaternionr& orientation) override
|
||||
{
|
||||
unused(camera_id);
|
||||
unused(orientation);
|
||||
|
||||
throw std::logic_error("setCameraOrientation() call is not implemented for this vehicle");
|
||||
}
|
||||
|
||||
private:
|
||||
VehicleApiBase* controller_;
|
||||
};
|
||||
|
||||
|
||||
}} //namespace
|
||||
#endif
|
||||
|
|
@ -51,7 +51,14 @@ public: //methods
|
|||
sensors_ = sensors;
|
||||
is_simulation_mode_ = is_simulation;
|
||||
|
||||
openAllConnections();
|
||||
try {
|
||||
openAllConnections();
|
||||
is_ready_ = true;
|
||||
}
|
||||
catch (std::exception& ex) {
|
||||
is_ready_ = false;
|
||||
is_ready_message_ = Utils::stringf("Failed to connect: %s", ex.what());
|
||||
}
|
||||
}
|
||||
|
||||
Pose getMocapPose()
|
||||
|
|
@ -129,6 +136,13 @@ public: //methods
|
|||
was_reset_ = false;
|
||||
}
|
||||
|
||||
virtual bool isReady(std::string& message) const override
|
||||
{
|
||||
if (!is_ready_)
|
||||
message = is_ready_message_;
|
||||
return is_ready_;
|
||||
}
|
||||
|
||||
//TODO: this method can't be const yet because it clears previous messages
|
||||
virtual void getStatusMessages(std::vector<std::string>& messages) override
|
||||
{
|
||||
|
|
@ -200,7 +214,7 @@ public: //methods
|
|||
return current_state_.controls.landed ? LandedState::Landed : LandedState::Flying;
|
||||
}
|
||||
|
||||
virtual real_T getRotorActuation(unsigned int rotor_index) const override
|
||||
virtual real_T getActuation(unsigned int rotor_index) const override
|
||||
{
|
||||
if (!is_simulation_mode_)
|
||||
throw std::logic_error("Attempt to read motor controls while not in simulation mode");
|
||||
|
|
@ -208,7 +222,7 @@ public: //methods
|
|||
std::lock_guard<std::mutex> guard(hil_controls_mutex_);
|
||||
return rotor_controls_[rotor_index];
|
||||
}
|
||||
virtual size_t getRotorCount() const override
|
||||
virtual size_t getActuatorCount() const override
|
||||
{
|
||||
return RotorControlsCount;
|
||||
}
|
||||
|
|
@ -330,8 +344,7 @@ public: //methods
|
|||
return GeoPoint(current_state_.global_est.pos.lat, current_state_.global_est.pos.lon, current_state_.global_est.pos.alt);
|
||||
}
|
||||
|
||||
//TODO: remove this method?
|
||||
virtual void reportTelemetry(float renderTime)
|
||||
virtual void sendTelemetry(float last_interval = -1) override
|
||||
{
|
||||
if (logviewer_proxy_ == nullptr || connection_ == nullptr || mav_vehicle_ == nullptr) {
|
||||
return;
|
||||
|
|
@ -375,7 +388,7 @@ public: //methods
|
|||
}
|
||||
}
|
||||
|
||||
data.renderTime = static_cast<int64_t>(renderTime * 1000000);// microseconds
|
||||
data.renderTime = static_cast<int64_t>(last_interval * 1000000);// microseconds
|
||||
logviewer_proxy_->sendMessage(data);
|
||||
}
|
||||
|
||||
|
|
@ -434,7 +447,7 @@ protected: //methods
|
|||
}
|
||||
|
||||
//TODO: decouple MultirotorApiBase, VehicalParams and SafetyEval
|
||||
virtual const MultirotorApiParams& getVehicleParams() const override
|
||||
virtual const MultirotorApiParams& getMultirotorApiParams() const override
|
||||
{
|
||||
//defaults are good for PX4 generic quadcopter.
|
||||
static const MultirotorApiParams vehicle_params_;
|
||||
|
|
@ -1199,6 +1212,8 @@ private: //variables
|
|||
const SensorCollection* sensors_;
|
||||
uint64_t last_gps_time_;
|
||||
bool was_reset_;
|
||||
bool is_ready_;
|
||||
std::string is_ready_message_;
|
||||
Pose mocap_pose_;
|
||||
|
||||
//additional variables required for MultirotorApiBase implementation
|
||||
|
|
|
|||
|
|
@ -4,22 +4,31 @@
|
|||
#ifndef msr_airlib_vehicles_Px4MultiRotor_hpp
|
||||
#define msr_airlib_vehicles_Px4MultiRotor_hpp
|
||||
|
||||
#include "vehicles/multirotor/controllers/MavLinkMultirotorApi.hpp"
|
||||
#include "vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp"
|
||||
#include "common/AirSimSettings.hpp"
|
||||
#include "sensors/SensorFactory.hpp"
|
||||
|
||||
#include "vehicles/multirotor/MultiRotorParams.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
class Px4MultiRotor : public MultiRotorParams {
|
||||
class Px4MultiRotorParams : public MultiRotorParams {
|
||||
public:
|
||||
Px4MultiRotor(const AirSimSettings::PX4VehicleSetting& vehicle_setting, std::shared_ptr<const SensorFactory> sensor_factory)
|
||||
Px4MultiRotorParams(const AirSimSettings::PX4VehicleSetting& vehicle_setting, std::shared_ptr<const SensorFactory> sensor_factory)
|
||||
: sensor_factory_(sensor_factory)
|
||||
{
|
||||
connection_info_ = getConnectionInfo(vehicle_setting);
|
||||
}
|
||||
|
||||
virtual ~Px4MultiRotor() = default;
|
||||
virtual ~Px4MultiRotorParams() = default;
|
||||
|
||||
virtual std::unique_ptr<MultirotorApiBase> createMultirotorApi() override
|
||||
{
|
||||
unique_ptr<MultirotorApiBase> api(new MavLinkMultirotorApi());
|
||||
auto api_ptr = static_cast<MavLinkMultirotorApi*>(api.get());
|
||||
api_ptr->initialize(connection_info_, &getSensors(), true);
|
||||
|
||||
return api;
|
||||
}
|
||||
|
||||
virtual void setupParams() override
|
||||
{
|
||||
|
|
@ -47,16 +56,6 @@ protected:
|
|||
return sensor_factory_->createSensor(sensor_type);
|
||||
}
|
||||
|
||||
virtual std::unique_ptr<MultirotorApiBase> createController() override
|
||||
{
|
||||
unique_ptr<MultirotorApiBase> controller(new MavLinkMultirotorApi());
|
||||
auto mav_controller = static_cast<MavLinkMultirotorApi*>(controller.get());
|
||||
mav_controller->initialize(connection_info_, & getSensors(), true);
|
||||
|
||||
return controller;
|
||||
}
|
||||
|
||||
|
||||
private:
|
||||
void setupFrameGenericQuad(Params& params)
|
||||
{
|
||||
|
|
@ -244,14 +243,14 @@ private:
|
|||
}
|
||||
|
||||
|
||||
static const MavLinkMultirotorApi::MavLinkConnectionInfo& getConnectionInfo(const AirSimSettings::PX4VehicleSetting& vehicle_setting)
|
||||
static const AirSimSettings::MavLinkConnectionInfo& getConnectionInfo(const AirSimSettings::PX4VehicleSetting& vehicle_setting)
|
||||
{
|
||||
return vehicle_setting.connection_info;
|
||||
}
|
||||
|
||||
|
||||
private:
|
||||
MavLinkMultirotorApi::MavLinkConnectionInfo connection_info_;
|
||||
AirSimSettings::MavLinkConnectionInfo connection_info_;
|
||||
std::shared_ptr<const SensorFactory> sensor_factory_;
|
||||
|
||||
};
|
||||
|
|
@ -65,47 +65,23 @@ public:
|
|||
}
|
||||
}
|
||||
|
||||
public:
|
||||
//*** Start: VehicleApiBase implementation ***//
|
||||
public: //VehicleApiBase implementation
|
||||
virtual void reset() override
|
||||
{
|
||||
MultirotorApiBase::reset();
|
||||
|
||||
firmware_->reset();
|
||||
}
|
||||
|
||||
virtual void update() override
|
||||
{
|
||||
MultirotorApiBase::update();
|
||||
|
||||
firmware_->update();
|
||||
}
|
||||
|
||||
virtual real_T getRotorActuation(unsigned int rotor_index) const override
|
||||
{
|
||||
auto control_signal = board_->getMotorControlSignal(rotor_index);
|
||||
return control_signal;
|
||||
}
|
||||
virtual size_t getRotorCount() const override
|
||||
{
|
||||
return vehicle_params_->getParams().rotor_count;
|
||||
}
|
||||
virtual void setSimulatedGroundTruth(const Kinematics::State* kinematics, const Environment* environment) override
|
||||
{
|
||||
board_->setKinematics(kinematics);
|
||||
estimator_->setKinematics(kinematics, environment);
|
||||
}
|
||||
|
||||
virtual void getStatusMessages(std::vector<std::string>& messages) override
|
||||
{
|
||||
comm_link_->getStatusMessages(messages);
|
||||
}
|
||||
|
||||
virtual bool isApiControlEnabled() const override
|
||||
{
|
||||
return firmware_->offboardApi().hasApiControl();
|
||||
}
|
||||
|
||||
virtual void enableApiControl(bool is_enabled) override
|
||||
{
|
||||
if (is_enabled) {
|
||||
|
|
@ -116,11 +92,44 @@ public:
|
|||
else
|
||||
firmware_->offboardApi().releaseApiControl();
|
||||
}
|
||||
|
||||
//*** End: VehicleApiBase implementation ***//
|
||||
virtual bool armDisarm(bool arm) override
|
||||
{
|
||||
std::string message;
|
||||
if (arm)
|
||||
return firmware_->offboardApi().arm(message);
|
||||
else
|
||||
return firmware_->offboardApi().disarm(message);
|
||||
}
|
||||
virtual GeoPoint getHomeGeoPoint() const override
|
||||
{
|
||||
return AirSimSimpleFlightCommon::toGeoPoint(firmware_->offboardApi().getHomeGeoPoint());
|
||||
}
|
||||
virtual void getStatusMessages(std::vector<std::string>& messages) override
|
||||
{
|
||||
comm_link_->getStatusMessages(messages);
|
||||
}
|
||||
|
||||
//*** Start: MultirotorApiBase implementation ***//
|
||||
public:
|
||||
public: //MultirotorApiBase implementation
|
||||
virtual real_T getActuation(unsigned int rotor_index) const override
|
||||
{
|
||||
auto control_signal = board_->getMotorControlSignal(rotor_index);
|
||||
return control_signal;
|
||||
}
|
||||
virtual size_t getActuatorCount() const override
|
||||
{
|
||||
return vehicle_params_->getParams().rotor_count;
|
||||
}
|
||||
virtual void moveByRC(const RCData& rc_data) override
|
||||
{
|
||||
setRCData(rc_data);
|
||||
}
|
||||
virtual void setSimulatedGroundTruth(const Kinematics::State* kinematics, const Environment* environment) override
|
||||
{
|
||||
board_->setKinematics(kinematics);
|
||||
estimator_->setKinematics(kinematics, environment);
|
||||
}
|
||||
|
||||
protected:
|
||||
virtual Kinematics::State getKinematicsEstimated() const override
|
||||
{
|
||||
return AirSimSimpleFlightCommon::toKinematicsState3r(firmware_->offboardApi().
|
||||
|
|
@ -142,33 +151,19 @@ public:
|
|||
virtual Quaternionr getOrientation() const override
|
||||
{
|
||||
const auto& val = firmware_->offboardApi().getStateEstimator().getOrientation();
|
||||
return AirSimSimpleFlightCommon::toQuaternion(val);
|
||||
return AirSimSimpleFlightCommon::toQuaternion(val);
|
||||
}
|
||||
|
||||
virtual LandedState getLandedState() const override
|
||||
{
|
||||
return firmware_->offboardApi().getLandedState() ? LandedState::Landed : LandedState::Flying;
|
||||
}
|
||||
|
||||
|
||||
virtual RCData getRCData() const override
|
||||
{
|
||||
return last_rcData_;
|
||||
}
|
||||
|
||||
virtual bool armDisarm(bool arm) override
|
||||
{
|
||||
std::string message;
|
||||
if (arm)
|
||||
return firmware_->offboardApi().arm(message);
|
||||
else
|
||||
return firmware_->offboardApi().disarm(message);
|
||||
}
|
||||
|
||||
virtual GeoPoint getHomeGeoPoint() const override
|
||||
{
|
||||
return AirSimSimpleFlightCommon::toGeoPoint(firmware_->offboardApi().getHomeGeoPoint());
|
||||
}
|
||||
|
||||
virtual GeoPoint getGpsLocation() const override
|
||||
{
|
||||
return AirSimSimpleFlightCommon::toGeoPoint(firmware_->offboardApi().getGeoPoint());
|
||||
|
|
@ -176,7 +171,7 @@ public:
|
|||
|
||||
virtual float getCommandPeriod() const override
|
||||
{
|
||||
return 1.0f/50; //50hz
|
||||
return 1.0f / 50; //50hz
|
||||
}
|
||||
|
||||
virtual float getTakeoffZ() const override
|
||||
|
|
@ -191,7 +186,6 @@ public:
|
|||
return 0.5f; //measured in simulator by firing commands "MoveToLocation -x 0 -y 0" multiple times and looking at distance traveled
|
||||
}
|
||||
|
||||
protected:
|
||||
virtual void commandRollPitchThrottle(float pitch, float roll, float throttle, float yaw_rate) override
|
||||
{
|
||||
//Utils::log(Utils::stringf("commandRollPitchThrottle %f, %f, %f, %f", pitch, roll, throttle, yaw_rate));
|
||||
|
|
@ -263,7 +257,7 @@ protected:
|
|||
firmware_->offboardApi().setGoalAndMode(&goal, &mode, message);
|
||||
}
|
||||
|
||||
virtual const MultirotorApiParams& getVehicleParams() const override
|
||||
virtual const MultirotorApiParams& getMultirotorApiParams() const override
|
||||
{
|
||||
return safety_params_;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -12,14 +12,19 @@
|
|||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
class SimpleFlightQuadX : public MultiRotorParams {
|
||||
class SimpleFlightQuadXParams : public MultiRotorParams {
|
||||
public:
|
||||
SimpleFlightQuadX(const AirSimSettings::VehicleSetting* vehicle_setting, std::shared_ptr<const SensorFactory> sensor_factory)
|
||||
SimpleFlightQuadXParams(const AirSimSettings::VehicleSetting* vehicle_setting, std::shared_ptr<const SensorFactory> sensor_factory)
|
||||
: vehicle_setting_(vehicle_setting), sensor_factory_(sensor_factory)
|
||||
{
|
||||
}
|
||||
|
||||
virtual ~SimpleFlightQuadX() = default;
|
||||
virtual ~SimpleFlightQuadXParams() = default;
|
||||
|
||||
virtual std::unique_ptr<MultirotorApiBase> createMultirotorApi() override
|
||||
{
|
||||
return std::unique_ptr<MultirotorApiBase>(new SimpleFlightApi(this, vehicle_setting_));
|
||||
}
|
||||
|
||||
protected:
|
||||
virtual void setupParams() override
|
||||
|
|
@ -60,11 +65,6 @@ protected:
|
|||
return sensor_factory_->createSensor(sensor_type);
|
||||
}
|
||||
|
||||
virtual std::unique_ptr<MultirotorApiBase> createController() override
|
||||
{
|
||||
return std::unique_ptr<MultirotorApiBase>(new SimpleFlightApi(this, vehicle_setting_));
|
||||
}
|
||||
|
||||
|
||||
private:
|
||||
vector<unique_ptr<SensorBase>> sensor_storage_;
|
||||
|
|
@ -230,11 +230,15 @@ void RpcLibClientBase::setCameraOrientation(int camera_id, const Quaternionr& or
|
|||
pimpl_->client.call("setCameraOrientation", camera_id, RpcLibAdapatorsBase::Quaternionr(orientation));
|
||||
}
|
||||
|
||||
void RpcLibClientBase::cancelPendingTasks()
|
||||
{
|
||||
pimpl_->client.call("cancelPendingTasks");
|
||||
}
|
||||
|
||||
void* RpcLibClientBase::getClient()
|
||||
{
|
||||
return &pimpl_->client;
|
||||
}
|
||||
|
||||
const void* RpcLibClientBase::getClient() const
|
||||
{
|
||||
return &pimpl_->client;
|
||||
|
|
|
|||
|
|
@ -127,13 +127,13 @@ RpcLibServerBase::RpcLibServerBase(const std::string& server_address, uint16_t p
|
|||
return RpcLibAdapatorsBase::GeoPoint(geo_point);
|
||||
});
|
||||
|
||||
pimpl_->server.bind("getCameraInfo", [&](int camera_id) -> RpcLibAdapatorsBase::CameraInfo {
|
||||
const auto& camera_info = getVehicleApi()->getCameraInfo(camera_id);
|
||||
pimpl_->server.bind("simGetCameraInfo", [&](int camera_id) -> RpcLibAdapatorsBase::CameraInfo {
|
||||
const auto& camera_info = getVehicleSimApi()->getCameraInfo(camera_id);
|
||||
return RpcLibAdapatorsBase::CameraInfo(camera_info);
|
||||
});
|
||||
|
||||
pimpl_->server.bind("setCameraOrientation", [&](int camera_id, const RpcLibAdapatorsBase::Quaternionr& orientation) -> void {
|
||||
getVehicleApi()->setCameraOrientation(camera_id, orientation.to());
|
||||
pimpl_->server.bind("simSetCameraOrientation", [&](int camera_id, const RpcLibAdapatorsBase::Quaternionr& orientation) -> void {
|
||||
getVehicleSimApi()->setCameraOrientation(camera_id, orientation.to());
|
||||
});
|
||||
|
||||
pimpl_->server.bind("simGetCollisionInfo", [&]() -> RpcLibAdapatorsBase::CollisionInfo {
|
||||
|
|
@ -146,6 +146,9 @@ RpcLibServerBase::RpcLibServerBase(const std::string& server_address, uint16_t p
|
|||
return RpcLibAdapatorsBase::Pose(pose);
|
||||
});
|
||||
|
||||
pimpl_->server.bind("cancelPendingTasks", [&]() -> void {
|
||||
getVehicleApi()->cancelPendingTasks();
|
||||
});
|
||||
|
||||
//if we don't suppress then server will bomb out for exceptions raised by any method
|
||||
pimpl_->server.suppress_exceptions(true);
|
||||
|
|
|
|||
|
|
@ -57,9 +57,9 @@ void ObstacleMap::setBlindspot(int tick, bool blindspot)
|
|||
|
||||
ObstacleMap::ObstacleInfo ObstacleMap::hasObstacle_(int from_tick, int to_tick) const
|
||||
{
|
||||
//make dure from <= to
|
||||
//make sure from <= to
|
||||
if (from_tick > to_tick) {
|
||||
//normalize the ticks so bothe are valid indices
|
||||
//normalize the ticks so both are valid indices
|
||||
from_tick = wrap(from_tick);
|
||||
to_tick = wrap(to_tick);
|
||||
|
||||
|
|
|
|||
|
|
@ -5,6 +5,7 @@
|
|||
#ifndef AIRLIB_HEADER_ONLY
|
||||
|
||||
#include "safety/SafetyEval.hpp"
|
||||
#include "vehicles/multirotor/api/MultirotorCommon.hpp"
|
||||
|
||||
#include <cmath>
|
||||
#include <sstream>
|
||||
|
|
@ -138,7 +139,7 @@ void SafetyEval::isSafeDestination(const Vector3r& dest_pos, const Vector3r& cur
|
|||
|
||||
float SafetyEval::adjustClearanceForPrStl(float base_clearance, float obs_confidence)
|
||||
{
|
||||
//3.2 comes from inverse CDF for epsilone = 0.05 (i.e. 95% confidence), author: akapoor
|
||||
//3.2 comes from inverse CDF for epsilon = 0.05 (i.e. 95% confidence), author: akapoor
|
||||
float additional_clearance = (1 - obs_confidence) * 3.2f;
|
||||
if (additional_clearance != 0)
|
||||
Utils::log(Utils::stringf("additional_clearance=%f", additional_clearance));
|
||||
|
|
|
|||
|
|
@ -30,17 +30,17 @@ namespace msr { namespace airlib {
|
|||
|
||||
typedef msr::airlib_rpclib::CarRpcLibAdapators CarRpcLibAdapators;
|
||||
|
||||
CarRpcLibServer::CarRpcLibServer(WorldSimApiBase* simmode_api, string server_address, uint16_t port)
|
||||
: RpcLibServerBase(simmode_api, server_address, port)
|
||||
CarRpcLibServer::CarRpcLibServer(VehicleApiBase* vehicle_api, WorldSimApiBase* world_sim_api, string server_address, uint16_t port)
|
||||
: RpcLibServerBase(server_address, port)
|
||||
{
|
||||
(static_cast<rpc::server*>(getServer()))->
|
||||
bind("getCarState", [&]() -> CarRpcLibAdapators::CarState {
|
||||
return CarRpcLibAdapators::CarState(getCarApi()->getCarState());
|
||||
return CarRpcLibAdapators::CarState(getVehicleApi()->getCarState());
|
||||
});
|
||||
|
||||
(static_cast<rpc::server*>(getServer()))->
|
||||
bind("setCarControls", [&](const CarRpcLibAdapators::CarControls& controls) -> void {
|
||||
getCarApi()->setCarControls(controls.to());
|
||||
getVehicleApi()->setCarControls(controls.to());
|
||||
});
|
||||
|
||||
}
|
||||
|
|
@ -50,12 +50,6 @@ CarRpcLibServer::~CarRpcLibServer()
|
|||
{
|
||||
}
|
||||
|
||||
CarApiBase* CarRpcLibServer::getCarApi() const
|
||||
{
|
||||
return static_cast<CarApiBase*>(getSimModeApi()->getVehicleApi());
|
||||
}
|
||||
|
||||
|
||||
}} //namespace
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -66,7 +66,6 @@ bool MultirotorApiBase::moveByVelocityZ(float vx, float vy, float z, float durat
|
|||
return !waitForFunction([&]() {
|
||||
return !moveByVelocityZInternal(vx, vy, z, adj_yaw_mode);
|
||||
}, duration);
|
||||
|
||||
}
|
||||
|
||||
bool MultirotorApiBase::moveOnPath(const vector<Vector3r>& path, float velocity, DrivetrainType drivetrain, const YawMode& yaw_mode,
|
||||
|
|
@ -121,9 +120,9 @@ bool MultirotorApiBase::moveOnPath(const vector<Vector3r>& path, float velocity,
|
|||
|
||||
//when path ends, we want to slow down
|
||||
float breaking_dist = 0;
|
||||
if (velocity > getVehicleParams().breaking_vel) {
|
||||
breaking_dist = Utils::clip(velocity * getVehicleParams().vel_to_breaking_dist,
|
||||
getVehicleParams().min_breaking_dist, getVehicleParams().max_breaking_dist);
|
||||
if (velocity > getMultirotorApiParams().breaking_vel) {
|
||||
breaking_dist = Utils::clip(velocity * getMultirotorApiParams().vel_to_breaking_dist,
|
||||
getMultirotorApiParams().min_breaking_dist, getMultirotorApiParams().max_breaking_dist);
|
||||
}
|
||||
//else no need to change velocities for last segments
|
||||
|
||||
|
|
@ -148,8 +147,8 @@ bool MultirotorApiBase::moveOnPath(const vector<Vector3r>& path, float velocity,
|
|||
|
||||
float seg_velocity = path_segs.at(next_path_loc.seg_index).seg_velocity;
|
||||
float path_length_remaining = path_length - path_segs.at(cur_path_loc.seg_index).seg_path_length - cur_path_loc.offset;
|
||||
if (seg_velocity > getVehicleParams().min_vel_for_breaking && path_length_remaining <= breaking_dist) {
|
||||
seg_velocity = getVehicleParams().breaking_vel;
|
||||
if (seg_velocity > getMultirotorApiParams().min_vel_for_breaking && path_length_remaining <= breaking_dist) {
|
||||
seg_velocity = getMultirotorApiParams().breaking_vel;
|
||||
//Utils::logMessage("path_length_remaining = %f, Switched to breaking vel %f", path_length_remaining, seg_velocity);
|
||||
}
|
||||
|
||||
|
|
@ -262,7 +261,7 @@ bool MultirotorApiBase::moveToZ(float z, float velocity, const YawMode& yaw_mode
|
|||
{
|
||||
SingleTaskCall lock(this);
|
||||
|
||||
Vector2r cur_xy(getPosition().x, getPosition().y);
|
||||
Vector2r cur_xy(getPosition().x(), getPosition().y());
|
||||
vector<Vector3r> path { Vector3r(cur_xy.x(), cur_xy.y(), z) };
|
||||
return moveOnPath(path, velocity, DrivetrainType::MaxDegreeOfFreedom, yaw_mode, lookahead, adaptive_lookahead);
|
||||
}
|
||||
|
|
@ -348,7 +347,13 @@ bool MultirotorApiBase::hover()
|
|||
{
|
||||
SingleTaskCall lock(this);
|
||||
|
||||
return moveToZ(getPosition().z, 0.5f, YawMode{ true,0 }, 1.0f, false);
|
||||
return moveToZ(getPosition().z(), 0.5f, YawMode{ true,0 }, 1.0f, false);
|
||||
}
|
||||
|
||||
void MultirotorApiBase::moveByRC(const RCData& rc_data)
|
||||
{
|
||||
//by default we say that this command is not supported
|
||||
throw VehicleCommandNotImplementedException("moveByRC API is not implemented for this multirotor");
|
||||
}
|
||||
|
||||
bool MultirotorApiBase::moveByVelocityInternal(float vx, float vy, float vz, const YawMode& yaw_mode)
|
||||
|
|
@ -476,7 +481,7 @@ bool MultirotorApiBase::moveByManual(float vx_max, float vy_max, float z_min, fl
|
|||
|
||||
//execute command
|
||||
try {
|
||||
float vz = (rc_data.throttle / kMaxRCValue) * z_min + getPosition().z;
|
||||
float vz = (rc_data.throttle / kMaxRCValue) * z_min + getPosition().z();
|
||||
moveByVelocityZInternal(vel_body.x(), vel_body.y(), vz, adj_yaw_mode);
|
||||
}
|
||||
catch(const MultirotorApiBase::UnsafeMoveException& ex) {
|
||||
|
|
@ -519,7 +524,7 @@ bool MultirotorApiBase::waitForZ(float max_wait_seconds, float z, float margin)
|
|||
{
|
||||
float cur_z = 100000;
|
||||
if (!waitForFunction([&]() {
|
||||
cur_z = getPosition().z;
|
||||
cur_z = getPosition().z();
|
||||
return (std::abs(cur_z - z) <= margin);
|
||||
}, max_wait_seconds))
|
||||
{
|
||||
|
|
@ -541,7 +546,7 @@ bool MultirotorApiBase::emergencyManeuverIfUnsafe(const SafetyEval::EvalResult&
|
|||
Vector3r avoidance_vel = getObsAvoidanceVelocity(result.cur_risk_dist, obs_avoidance_vel_) * result.suggested_vec;
|
||||
|
||||
//use the unchecked command
|
||||
commandVelocityZ(avoidance_vel.x(), avoidance_vel.y(), getPosition().z, YawMode::Zero());
|
||||
commandVelocityZ(avoidance_vel.x(), avoidance_vel.y(), getPosition().z(), YawMode::Zero());
|
||||
|
||||
//tell caller not to execute planned command
|
||||
return false;
|
||||
|
|
@ -698,8 +703,5 @@ float MultirotorApiBase::getObsAvoidanceVelocity(float risk_dist, float max_obs_
|
|||
return max_obs_avoidance_vel;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
}} //namespace
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -124,37 +124,9 @@ MultirotorState MultirotorRpcLibClient::getMultirotorState()
|
|||
as<MultirotorRpcLibAdapators::MultirotorState>().to();
|
||||
}
|
||||
|
||||
Vector3r MultirotorRpcLibClient::getPosition()
|
||||
void MultirotorRpcLibClient::moveByRC(const RCData& rc_data)
|
||||
{
|
||||
return static_cast<rpc::client*>(getClient())->call("getPosition").as<MultirotorRpcLibAdapators::Vector3r>().to();
|
||||
}
|
||||
Vector3r MultirotorRpcLibClient::getVelocity()
|
||||
{
|
||||
return static_cast<rpc::client*>(getClient())->call("getVelocity").as<MultirotorRpcLibAdapators::Vector3r>().to();
|
||||
}
|
||||
Quaternionr MultirotorRpcLibClient::getOrientation()
|
||||
{
|
||||
return static_cast<rpc::client*>(getClient())->call("getOrientation").as<MultirotorRpcLibAdapators::Quaternionr>().to();
|
||||
}
|
||||
|
||||
MultirotorApiBase::LandedState MultirotorRpcLibClient::getLandedState()
|
||||
{
|
||||
int result = static_cast<rpc::client*>(getClient())->call("getLandedState").as<int>();
|
||||
return static_cast<MultirotorApiBase::LandedState>(result);
|
||||
}
|
||||
|
||||
RCData MultirotorRpcLibClient::getRCData()
|
||||
{
|
||||
return static_cast<rpc::client*>(getClient())->call("getRCData").as<MultirotorRpcLibAdapators::RCData>().to();
|
||||
}
|
||||
void MultirotorRpcLibClient::setRCData(const RCData& rc_data)
|
||||
{
|
||||
static_cast<rpc::client*>(getClient())->call("setRCData", MultirotorRpcLibAdapators::RCData(rc_data));
|
||||
}
|
||||
|
||||
GeoPoint MultirotorRpcLibClient::getGpsLocation()
|
||||
{
|
||||
return static_cast<rpc::client*>(getClient())->call("getGpsLocation").as<MultirotorRpcLibAdapators::GeoPoint>().to();
|
||||
static_cast<rpc::client*>(getClient())->call("moveByRC", MultirotorRpcLibAdapators::RCData(rc_data));
|
||||
}
|
||||
|
||||
}} //namespace
|
||||
|
|
|
|||
|
|
@ -5,7 +5,7 @@
|
|||
#ifndef AIRLIB_HEADER_ONLY
|
||||
//RPC code requires C++14. If build system like Unreal doesn't support it then use compiled binaries
|
||||
#ifndef AIRLIB_NO_RPC
|
||||
//if using Unreal Build system then include precompiled header file first
|
||||
//if using Unreal Build system then include pre-compiled header file first
|
||||
|
||||
#include "vehicles/multirotor/api/MultirotorRpcLibServer.hpp"
|
||||
|
||||
|
|
@ -30,97 +30,70 @@ namespace msr { namespace airlib {
|
|||
|
||||
typedef msr::airlib_rpclib::MultirotorRpcLibAdapators MultirotorRpcLibAdapators;
|
||||
|
||||
MultirotorRpcLibServer::MultirotorRpcLibServer(WorldSimApiBase* simmode_api, string server_address, uint16_t port)
|
||||
: RpcLibServerBase(simmode_api, server_address, port)
|
||||
MultirotorRpcLibServer::MultirotorRpcLibServer(MultirotorApiBase* vehicle_api, WorldSimApiBase* world_sim_api, string server_address, uint16_t port)
|
||||
: RpcLibServerBase(server_address, port), vehicle_api_(vehicle_api), world_sim_api_(world_sim_api)
|
||||
{
|
||||
(static_cast<rpc::server*>(getServer()))->
|
||||
bind("takeoff", [&](float max_wait_seconds) -> bool { return getDroneApi()->takeoff(max_wait_seconds); });
|
||||
bind("takeoff", [&](float max_wait_seconds) -> bool { return getVehicleApi()->takeoff(max_wait_seconds); });
|
||||
(static_cast<rpc::server*>(getServer()))->
|
||||
bind("land", [&](float max_wait_seconds) -> bool { return getDroneApi()->land(max_wait_seconds); });
|
||||
bind("land", [&](float max_wait_seconds) -> bool { return getVehicleApi()->land(max_wait_seconds); });
|
||||
(static_cast<rpc::server*>(getServer()))->
|
||||
bind("goHome", [&]() -> bool { return getDroneApi()->goHome(); });
|
||||
|
||||
bind("goHome", [&]() -> bool { return getVehicleApi()->goHome(); });
|
||||
|
||||
(static_cast<rpc::server*>(getServer()))->
|
||||
bind("moveByAngleZ", [&](float pitch, float roll, float z, float yaw, float duration) ->
|
||||
bool { return getDroneApi()->moveByAngleZ(pitch, roll, z, yaw, duration); });
|
||||
bool { return getVehicleApi()->moveByAngleZ(pitch, roll, z, yaw, duration); });
|
||||
(static_cast<rpc::server*>(getServer()))->
|
||||
bind("moveByAngleThrottle", [&](float pitch, float roll, float throttle, float yaw_rate, float duration) ->
|
||||
bool { return getDroneApi()->moveByAngleThrottle(pitch, roll, throttle, yaw_rate, duration); });
|
||||
bool { return getVehicleApi()->moveByAngleThrottle(pitch, roll, throttle, yaw_rate, duration); });
|
||||
(static_cast<rpc::server*>(getServer()))->
|
||||
bind("moveByVelocity", [&](float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const MultirotorRpcLibAdapators::YawMode& yaw_mode) ->
|
||||
bool { return getDroneApi()->moveByVelocity(vx, vy, vz, duration, drivetrain, yaw_mode.to()); });
|
||||
bool { return getVehicleApi()->moveByVelocity(vx, vy, vz, duration, drivetrain, yaw_mode.to()); });
|
||||
(static_cast<rpc::server*>(getServer()))->
|
||||
bind("moveByVelocityZ", [&](float vx, float vy, float z, float duration, DrivetrainType drivetrain, const MultirotorRpcLibAdapators::YawMode& yaw_mode) ->
|
||||
bool { return getDroneApi()->moveByVelocityZ(vx, vy, z, duration, drivetrain, yaw_mode.to()); });
|
||||
bool { return getVehicleApi()->moveByVelocityZ(vx, vy, z, duration, drivetrain, yaw_mode.to()); });
|
||||
(static_cast<rpc::server*>(getServer()))->
|
||||
bind("moveOnPath", [&](const vector<MultirotorRpcLibAdapators::Vector3r>& path, float velocity, float max_wait_seconds, DrivetrainType drivetrain, const MultirotorRpcLibAdapators::YawMode& yaw_mode,
|
||||
float lookahead, float adaptive_lookahead) ->
|
||||
bool {
|
||||
vector<Vector3r> conv_path;
|
||||
MultirotorRpcLibAdapators::to(path, conv_path);
|
||||
return getDroneApi()->moveOnPath(conv_path, velocity, max_wait_seconds, drivetrain, yaw_mode.to(), lookahead, adaptive_lookahead);
|
||||
return getVehicleApi()->moveOnPath(conv_path, velocity, drivetrain, yaw_mode.to(), lookahead, adaptive_lookahead);
|
||||
});
|
||||
(static_cast<rpc::server*>(getServer()))->
|
||||
bind("moveToPosition", [&](float x, float y, float z, float velocity, float max_wait_seconds, DrivetrainType drivetrain,
|
||||
const MultirotorRpcLibAdapators::YawMode& yaw_mode, float lookahead, float adaptive_lookahead) ->
|
||||
bool { return getDroneApi()->moveToPosition(x, y, z, velocity, max_wait_seconds, drivetrain, yaw_mode.to(), lookahead, adaptive_lookahead); });
|
||||
bool { return getVehicleApi()->moveToPosition(x, y, z, velocity, drivetrain, yaw_mode.to(), lookahead, adaptive_lookahead); });
|
||||
(static_cast<rpc::server*>(getServer()))->
|
||||
bind("moveToZ", [&](float z, float velocity, float max_wait_seconds, const MultirotorRpcLibAdapators::YawMode& yaw_mode, float lookahead, float adaptive_lookahead) ->
|
||||
bool { return getDroneApi()->moveToZ(z, velocity, max_wait_seconds, yaw_mode.to(), lookahead, adaptive_lookahead); });
|
||||
bool { return getVehicleApi()->moveToZ(z, velocity, yaw_mode.to(), lookahead, adaptive_lookahead); });
|
||||
(static_cast<rpc::server*>(getServer()))->
|
||||
bind("moveByManual", [&](float vx_max, float vy_max, float z_min, float duration, DrivetrainType drivetrain, const MultirotorRpcLibAdapators::YawMode& yaw_mode) ->
|
||||
bool { return getDroneApi()->moveByManual(vx_max, vy_max, z_min, duration, drivetrain, yaw_mode.to()); });
|
||||
bool { return getVehicleApi()->moveByManual(vx_max, vy_max, z_min, duration, drivetrain, yaw_mode.to()); });
|
||||
|
||||
(static_cast<rpc::server*>(getServer()))->
|
||||
bind("rotateToYaw", [&](float yaw, float max_wait_seconds, float margin) ->
|
||||
bool { return getDroneApi()->rotateToYaw(yaw, max_wait_seconds, margin); });
|
||||
bool { return getVehicleApi()->rotateToYaw(yaw, margin); });
|
||||
(static_cast<rpc::server*>(getServer()))->
|
||||
bind("rotateByYawRate", [&](float yaw_rate, float duration) ->
|
||||
bool { return getDroneApi()->rotateByYawRate(yaw_rate, duration); });
|
||||
bool { return getVehicleApi()->rotateByYawRate(yaw_rate, duration); });
|
||||
(static_cast<rpc::server*>(getServer()))->
|
||||
bind("hover", [&]() -> bool { return getDroneApi()->hover(); });
|
||||
bind("hover", [&]() -> bool { return getVehicleApi()->hover(); });
|
||||
(static_cast<rpc::server*>(getServer()))->
|
||||
bind("moveByRC", [&](const MultirotorRpcLibAdapators::RCData& data) ->
|
||||
void { getVehicleApi()->moveByRC(data.to()); });
|
||||
|
||||
(static_cast<rpc::server*>(getServer()))->
|
||||
bind("setSafety", [&](uint enable_reasons, float obs_clearance, const SafetyEval::ObsAvoidanceStrategy& obs_startegy,
|
||||
float obs_avoidance_vel, const MultirotorRpcLibAdapators::Vector3r& origin, float xy_length, float max_z, float min_z) ->
|
||||
bool { return getDroneApi()->setSafety(SafetyEval::SafetyViolationType(enable_reasons), obs_clearance, obs_startegy,
|
||||
bool { return getVehicleApi()->setSafety(SafetyEval::SafetyViolationType(enable_reasons), obs_clearance, obs_startegy,
|
||||
obs_avoidance_vel, origin.to(), xy_length, max_z, min_z); });
|
||||
(static_cast<rpc::server*>(getServer()))->
|
||||
bind("setRCData", [&](const MultirotorRpcLibAdapators::RCData& data) ->
|
||||
void { getDroneApi()->setRCData(data.to()); });
|
||||
|
||||
|
||||
//getters
|
||||
(static_cast<rpc::server*>(getServer()))->
|
||||
bind("getMultirotorState", [&]() -> MultirotorRpcLibAdapators::MultirotorState {
|
||||
return MultirotorRpcLibAdapators::MultirotorState(getDroneApi()->getMultirotorState());
|
||||
return MultirotorRpcLibAdapators::MultirotorState(getVehicleApi()->getMultirotorState());
|
||||
});
|
||||
(static_cast<rpc::server*>(getServer()))->
|
||||
bind("getPosition", [&]() -> MultirotorRpcLibAdapators::Vector3r {
|
||||
return MultirotorRpcLibAdapators::Vector3r(getDroneApi()->getPosition());
|
||||
});
|
||||
(static_cast<rpc::server*>(getServer()))->
|
||||
bind("getVelocity", [&]() -> MultirotorRpcLibAdapators::Vector3r {
|
||||
return MultirotorRpcLibAdapators::Vector3r(getDroneApi()->getVelocity());
|
||||
});
|
||||
(static_cast<rpc::server*>(getServer()))->
|
||||
bind("getOrientation", [&]() -> MultirotorRpcLibAdapators::Quaternionr {
|
||||
return MultirotorRpcLibAdapators::Quaternionr(getDroneApi()->getOrientation());
|
||||
});
|
||||
(static_cast<rpc::server*>(getServer()))->
|
||||
bind("getLandedState", [&]() -> int {
|
||||
return static_cast<int>(getDroneApi()->getLandedState());
|
||||
});
|
||||
(static_cast<rpc::server*>(getServer()))->
|
||||
bind("getRCData", [&]() -> MultirotorRpcLibAdapators::RCData {
|
||||
return MultirotorRpcLibAdapators::RCData(getDroneApi()->getRCData());
|
||||
});
|
||||
(static_cast<rpc::server*>(getServer()))->
|
||||
bind("getGpsLocation", [&]() -> MultirotorRpcLibAdapators::GeoPoint {
|
||||
return MultirotorRpcLibAdapators::GeoPoint(getDroneApi()->getGpsLocation());
|
||||
});
|
||||
|
||||
}
|
||||
|
||||
//required for pimpl
|
||||
|
|
@ -128,10 +101,6 @@ MultirotorRpcLibServer::~MultirotorRpcLibServer()
|
|||
{
|
||||
}
|
||||
|
||||
MultirotorApiBase* MultirotorRpcLibServer::getDroneApi() const
|
||||
{
|
||||
return static_cast<MultirotorApiBase*>(getSimModeApi()->getVehicleApi());
|
||||
}
|
||||
|
||||
}} //namespace
|
||||
|
||||
|
|
|
|||
|
|
@ -189,7 +189,6 @@
|
|||
<ItemGroup>
|
||||
<ClInclude Include="CelestialTests.hpp" />
|
||||
<ClInclude Include="QuaternionTest.hpp" />
|
||||
<ClInclude Include="RosFlightTest.hpp" />
|
||||
<ClInclude Include="SettingsTest.hpp" />
|
||||
<ClInclude Include="SimpleFlightTest.hpp" />
|
||||
<ClInclude Include="TestBase.hpp" />
|
||||
|
|
|
|||
|
|
@ -30,9 +30,6 @@
|
|||
<ClInclude Include="PixhawkTest.hpp">
|
||||
<Filter>Header Files</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="RosFlightTest.hpp">
|
||||
<Filter>Header Files</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="SimpleFlightTest.hpp">
|
||||
<Filter>Header Files</Filter>
|
||||
</ClInclude>
|
||||
|
|
|
|||
|
|
@ -12,10 +12,10 @@ public:
|
|||
virtual void run() override
|
||||
{
|
||||
//Test PX4 based drones
|
||||
auto pixhawk = MultiRotorParamsFactory::createConfig("Pixhawk", std::make_shared<SensorFactory>());
|
||||
|
||||
MultirotorApiBase* controller = pixhawk->getController();
|
||||
testAssert(controller != nullptr, "Couldn't get pixhawk controller");
|
||||
auto pixhawk = MultiRotorParamsFactory::createConfig(AirSimSettings::singleton().getVehicleSetting("Pixhawk"),
|
||||
std::make_shared<SensorFactory>());
|
||||
auto api = pixhawk->createMultirotorApi();
|
||||
testAssert(api != nullptr, "Couldn't get pixhawk controller");
|
||||
|
||||
try {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
|
|
|
|||
|
|
@ -1,26 +0,0 @@
|
|||
|
||||
#ifndef msr_AirLibUnitTests_RosFlightTest_hpp
|
||||
#define msr_AirLibUnitTests_RosFlightTest_hpp
|
||||
|
||||
#include "vehicles/multirotor/MultiRotorParamsFactory.hpp"
|
||||
#include "TestBase.hpp"
|
||||
#include "sensors/SensorFactory.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
class RosFlightTest : public TestBase {
|
||||
public:
|
||||
virtual void run() override
|
||||
{
|
||||
auto rosFlight = MultiRotorParamsFactory::createConfig("RosFlight", std::make_shared<SensorFactory>());
|
||||
|
||||
MultirotorApiBase* controller = rosFlight->getController();
|
||||
testAssert(controller != nullptr, "Couldn't get controller");
|
||||
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
}}
|
||||
#endif
|
||||
|
|
@ -8,6 +8,7 @@
|
|||
#include "physics/FastPhysicsEngine.hpp"
|
||||
#include "vehicles/multirotor/api/MultirotorApiBase.h"
|
||||
#include "common/SteppableClock.hpp"
|
||||
#include "vehicles/multirotor/MultiRotor.hpp"
|
||||
|
||||
namespace msr { namespace airlib {
|
||||
|
||||
|
|
@ -22,10 +23,13 @@ public:
|
|||
|
||||
SensorFactory sensor_factory;
|
||||
|
||||
std::unique_ptr<MultiRotorParams> params = MultiRotorParamsFactory::createConfig("SimpleFlight", std::make_shared<SensorFactory>());
|
||||
MultiRotor vehicle;
|
||||
std::unique_ptr<MultiRotorParams> params = MultiRotorParamsFactory::createConfig(
|
||||
AirSimSettings::singleton().getVehicleSetting("SimpleFlight"),
|
||||
std::make_shared<SensorFactory>());
|
||||
auto api = params->createMultirotorApi();
|
||||
|
||||
std::unique_ptr<Environment> environment;
|
||||
vehicle.initialize(params.get(), Pose(),
|
||||
MultiRotor vehicle(params.get(), api.get(), Pose(),
|
||||
GeoPoint(), environment);
|
||||
|
||||
std::vector<UpdatableObject*> vehicles = { &vehicle };
|
||||
|
|
@ -33,32 +37,30 @@ public:
|
|||
PhysicsWorld physics_world(physics_engine.get(), vehicles,
|
||||
static_cast<uint64_t>(clock->getStepSize() * 1E9));
|
||||
|
||||
MultirotorApiBase* controller = params->getController();
|
||||
testAssert(controller != nullptr, "Controller was null");
|
||||
testAssert(api != nullptr, "api was null");
|
||||
std::string message;
|
||||
testAssert(controller->isAvailable(message), message);
|
||||
testAssert(api->isReady(message), message);
|
||||
|
||||
clock->sleep_for(0.04f);
|
||||
|
||||
Utils::getSetMinLogLevel(true, 100);
|
||||
|
||||
DirectCancelableBase cancellable(controller, &vehicle);
|
||||
controller->enableApiControl(true);
|
||||
controller->armDisarm(true, cancellable);
|
||||
controller->takeoff(10, cancellable);
|
||||
api->enableApiControl(true);
|
||||
api->armDisarm(true);
|
||||
api->takeoff(10);
|
||||
|
||||
clock->sleep_for(2.0f);
|
||||
|
||||
Utils::getSetMinLogLevel(true);
|
||||
|
||||
controller->moveToPosition(-5, -5, -5, 5, DrivetrainType::MaxDegreeOfFreedom, YawMode(true, 0), -1, 0, cancellable);
|
||||
api->moveToPosition(-5, -5, -5, 5, DrivetrainType::MaxDegreeOfFreedom, YawMode(true, 0), -1, 0);
|
||||
|
||||
clock->sleep_for(2.0f);
|
||||
|
||||
|
||||
while (true) {
|
||||
clock->sleep_for(0.1f);
|
||||
controller->getStatusMessages(messages_);
|
||||
api->getStatusMessages(messages_);
|
||||
for (const auto& status_message : messages_) {
|
||||
std::cout << status_message << std::endl;
|
||||
}
|
||||
|
|
@ -68,34 +70,6 @@ public:
|
|||
|
||||
private:
|
||||
std::vector<std::string> messages_;
|
||||
|
||||
private:
|
||||
class DirectCancelableBase : public CancelableAction
|
||||
{
|
||||
public:
|
||||
DirectCancelableBase(MultirotorApiBase* controller, const MultiRotor* vehicle)
|
||||
: controller_(controller), vehicle_(vehicle)
|
||||
{
|
||||
}
|
||||
virtual void execute() override
|
||||
{}
|
||||
|
||||
virtual bool sleep(double secs) override
|
||||
{
|
||||
controller_->getStatusMessages(messages_);
|
||||
for (const auto& status_message : messages_) {
|
||||
std::cout << status_message << std::endl;
|
||||
}
|
||||
messages_.clear();
|
||||
|
||||
return CancelableAction::sleep(secs);
|
||||
};
|
||||
|
||||
private:
|
||||
MultirotorApiBase* controller_;
|
||||
const MultiRotor* vehicle_;
|
||||
std::vector<std::string> messages_;
|
||||
};
|
||||
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -1,7 +1,7 @@
|
|||
#ifndef msr_AirLibUnitTests_WorkerThreadThreadTest_hpp
|
||||
#define msr_AirLibUnitTests_WorkerThreadThreadTest_hpp
|
||||
|
||||
#include "common/common_utils/WorkerThread.hpp"
|
||||
#include "common/WorkerThread.hpp"
|
||||
#include "common/common_utils/Timer.hpp"
|
||||
#include <chrono>
|
||||
#include "TestBase.hpp"
|
||||
|
|
@ -14,7 +14,7 @@ class WorkerThreadTest : public TestBase {
|
|||
public:
|
||||
double runTime = 2; // seconds
|
||||
|
||||
virtual void execute() {
|
||||
virtual void executeAction() override {
|
||||
common_utils::Timer timer;
|
||||
timer.start();
|
||||
while (!this->isCancelled())
|
||||
|
|
@ -26,12 +26,12 @@ class WorkerThreadTest : public TestBase {
|
|||
}
|
||||
}
|
||||
}
|
||||
void reset(bool reset_counter = true) {
|
||||
void reset(bool reset_counter = true)
|
||||
{
|
||||
// reset for next test
|
||||
if (reset_counter)
|
||||
counter_ = 0;
|
||||
is_cancelled_ = false;
|
||||
is_complete_ = false;
|
||||
CancelableAction::reset();
|
||||
}
|
||||
unsigned int getCount() {
|
||||
return counter_;
|
||||
|
|
|
|||
|
|
@ -1,6 +1,5 @@
|
|||
|
||||
#include "SettingsTest.hpp"
|
||||
#include "RosFlightTest.hpp"
|
||||
#include "PixhawkTest.hpp"
|
||||
#include "SimpleFlightTest.hpp"
|
||||
#include "WorkerThreadTest.hpp"
|
||||
|
|
@ -18,7 +17,6 @@ int main()
|
|||
std::unique_ptr<TestBase>(new SimpleFlightTest())
|
||||
//,
|
||||
//std::unique_ptr<TestBase>(new PixhawkTest()),
|
||||
//std::unique_ptr<TestBase>(new RosFlightTest()),
|
||||
//std::unique_ptr<TestBase>(new WorkerThreadTest())
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -4,59 +4,24 @@
|
|||
#include <iostream>
|
||||
#include <string>
|
||||
#include "vehicles/multirotor/api/MultirotorRpcLibServer.hpp"
|
||||
#include "vehicles/multirotor/controllers/MavLinkMultirotorApi.hpp"
|
||||
#include "vehicles/multirotor/controllers/RealMultirotorConnector.hpp"
|
||||
#include "vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp"
|
||||
#include "common/Settings.hpp"
|
||||
|
||||
using namespace std;
|
||||
using namespace msr::airlib;
|
||||
|
||||
void printUsage() {
|
||||
cout << "Usage: DroneServer" << endl;
|
||||
cout << "Start the DroneServer using the 'PX4' settings in ~/Documents/AirSim/settings.json." << endl;
|
||||
}
|
||||
|
||||
class DroneServerSimModeApi : public WorldSimApiBase {
|
||||
public:
|
||||
DroneServerSimModeApi(MultirotorApiBase* api)
|
||||
: api_(api)
|
||||
{
|
||||
}
|
||||
|
||||
virtual VehicleApiBase* getVehicleApi() override
|
||||
{
|
||||
return api_;
|
||||
}
|
||||
|
||||
virtual void reset() override
|
||||
{
|
||||
throw std::domain_error("reset is not supported");
|
||||
}
|
||||
|
||||
virtual bool isPaused() const override
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
virtual void pause(bool is_paused) override
|
||||
{
|
||||
throw std::domain_error("pause is not supported");
|
||||
}
|
||||
|
||||
virtual void continueForTime(double seconds) override
|
||||
{
|
||||
throw std::domain_error("continueForTime is not supported");
|
||||
}
|
||||
|
||||
private:
|
||||
MultirotorApiBase* api_;
|
||||
};
|
||||
/*
|
||||
This is a sample code demonstrating how to deploy rpc server on-board
|
||||
real drone so we can use same APIs on real vehicle that we used in simulation.
|
||||
This demonstration is designed for PX4 powered drone.
|
||||
*/
|
||||
|
||||
int main(int argc, const char* argv[])
|
||||
{
|
||||
if (argc != 2) {
|
||||
std::cout << "Usage: " << argv[0] << " is_simulation" << std::endl;
|
||||
std::cout << "\t where is_simulation = 0 or 1" << std::endl;
|
||||
cout << "Start the DroneServer using the 'PX4' settings in ~/Documents/AirSim/settings.json." << endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
|
@ -66,7 +31,7 @@ int main(int argc, const char* argv[])
|
|||
else
|
||||
std::cout << "WARNING: This is not simulation!" << std::endl;
|
||||
|
||||
MavLinkMultirotorApi::ConnectionInfo connection_info;
|
||||
AirSimSettings::MavLinkConnectionInfo connection_info;
|
||||
|
||||
// read settings and override defaults
|
||||
auto settings_full_filepath = Settings::getUserDirectoryFullPath("settings.json");
|
||||
|
|
@ -110,15 +75,11 @@ int main(int argc, const char* argv[])
|
|||
|
||||
}
|
||||
|
||||
MavLinkMultirotorApi mav_drone;
|
||||
mav_drone.initialize(connection_info, nullptr, is_simulation);
|
||||
mav_drone.reset();
|
||||
MavLinkMultirotorApi api;
|
||||
api.initialize(connection_info, nullptr, is_simulation);
|
||||
api.reset();
|
||||
|
||||
RealMultirotorConnector connector(& mav_drone);
|
||||
|
||||
MultirotorApiBase api(& connector);
|
||||
DroneServerSimModeApi simmode_api(&api);
|
||||
msr::airlib::MultirotorRpcLibServer server(&simmode_api, connection_info.local_host_ip);
|
||||
msr::airlib::MultirotorRpcLibServer server(&api, nullptr, connection_info.local_host_ip);
|
||||
|
||||
//start server in async mode
|
||||
server.start(false);
|
||||
|
|
@ -139,7 +100,7 @@ int main(int argc, const char* argv[])
|
|||
constexpr static std::chrono::milliseconds MessageCheckDurationMillis(100);
|
||||
std::this_thread::sleep_for(MessageCheckDurationMillis);
|
||||
|
||||
mav_drone.reportTelemetry(100);
|
||||
api.sendTelemetry();
|
||||
}
|
||||
|
||||
return 0;
|
||||
|
|
|
|||
|
|
@ -14,8 +14,8 @@
|
|||
#include "common/common_utils/AsyncTasker.hpp"
|
||||
#include "vehicles/multirotor/api/MultirotorRpcLibClient.hpp"
|
||||
#include "common/EarthUtils.hpp"
|
||||
#include "vehicles/multirotor/controllers/MultirotorCommon.hpp"
|
||||
#include "vehicles/multirotor/controllers/MultirotorApiBase.h"
|
||||
#include "vehicles/multirotor/api/MultirotorCommon.hpp"
|
||||
#include "vehicles/multirotor/api/MultirotorApiBase.h"
|
||||
#include "safety/SafetyEval.hpp"
|
||||
#include "common/common_utils/Timer.hpp"
|
||||
|
||||
|
|
@ -108,7 +108,7 @@ public:
|
|||
|
||||
class DisarmCommand : public DroneCommand {
|
||||
public:
|
||||
DisarmCommand() : DroneCommand("Disarm", "Disarm the motors so we can safly approach the drone")
|
||||
DisarmCommand() : DroneCommand("Disarm", "Disarm the motors so we can safely approach the drone")
|
||||
{
|
||||
}
|
||||
|
||||
|
|
@ -308,8 +308,8 @@ public:
|
|||
{
|
||||
CommandContext* context = params.context;
|
||||
context->tasker.execute([=]() {
|
||||
auto pos = context->client.getPosition();
|
||||
auto gps = context->client.getGpsLocation();
|
||||
auto pos = context->client.getMultirotorState().getPosition();
|
||||
auto gps = context->client.getMultirotorState().gps_location;
|
||||
|
||||
std::cout << "Local position: x=" << pos.x() << ", y=" << pos.y() << ", z=" << pos.z() << std::endl;
|
||||
std::cout << "Global position: lat=" << gps.latitude << ", lon=" << gps.longitude << ", alt=" << gps.altitude << std::endl;
|
||||
|
|
@ -325,7 +325,7 @@ public:
|
|||
|
||||
MoveOnPathCommand() : DroneCommand("MoveOnPath", "Move along the given series of x,y,z coordinates with the specified velocity")
|
||||
{
|
||||
this->addSwitch({ "-path", "", "a series of x,y,z cordinates separated by commas, e.g. 0,0,-10,100,0,-10,100,100,-10,0,100,-10,0,0,-10 will fly a square box pattern" });
|
||||
this->addSwitch({ "-path", "", "a series of x,y,z coordinates separated by commas, e.g. 0,0,-10,100,0,-10,100,100,-10,0,100,-10,0,0,-10 will fly a square box pattern" });
|
||||
this->addSwitch({ "-velocity", "2.5", "the velocity in meters per second (default 2.5)" });
|
||||
this->addSwitch({ "-duration", "0", "maximum time to wait to reach end of path (default no wait)" });
|
||||
addYawModeSwitches();
|
||||
|
|
@ -374,7 +374,7 @@ public:
|
|||
return false;
|
||||
}
|
||||
|
||||
auto pos = context->client.getPosition();
|
||||
auto pos = context->client.getMultirotorState().getPosition();
|
||||
if (getSwitch("-r").toInt() == 1) {
|
||||
for (size_t i = 0, n = path.size(); i < n; i++) {
|
||||
Vector3r v = path[i];
|
||||
|
|
@ -427,7 +427,7 @@ public:
|
|||
float z = getSwitch("-z").toFloat();
|
||||
|
||||
if (getSwitch("-r").toInt() == 1) {
|
||||
auto pos = context->client.getPosition();
|
||||
auto pos = context->client.getMultirotorState().getPosition();
|
||||
x += pos.x();
|
||||
y += pos.y();
|
||||
z += pos.z();
|
||||
|
|
@ -652,7 +652,7 @@ public:
|
|||
origin[1] = std::stof(parts[1]);
|
||||
origin[2] = std::stof(parts[2]);
|
||||
} else {
|
||||
throw std::invalid_argument("-origin argument is expectin 'x,y,z' (separated by commas and no spaces in between)");
|
||||
throw std::invalid_argument("-origin argument is expecting 'x,y,z' (separated by commas and no spaces in between)");
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -692,12 +692,12 @@ public:
|
|||
|
||||
context->tasker.execute([=]() {
|
||||
if (!context->client.moveByAngleZ(pitch, roll, z, yaw, duration)) {
|
||||
throw std::runtime_error("BackForthByAngleCommand cancelled");
|
||||
throw std::runtime_error("BackForthByAngleCommand canceled");
|
||||
}
|
||||
context->client.hover();
|
||||
context->sleep_for(pause_time);
|
||||
if (!context->client.moveByAngleZ(-pitch, -roll, z, yaw, duration)){
|
||||
throw std::runtime_error("BackForthByAngleCommand cancelled");
|
||||
throw std::runtime_error("BackForthByAngleCommand canceled");
|
||||
}
|
||||
}, iterations);
|
||||
|
||||
|
|
@ -736,13 +736,13 @@ public:
|
|||
context->tasker.execute([=]() {
|
||||
if (!context->client.moveToPosition(length, 0, z, velocity, 0, drivetrain,
|
||||
yawMode, lookahead, adaptive_lookahead)) {
|
||||
throw std::runtime_error("BackForthByPositionCommand cancelled");
|
||||
throw std::runtime_error("BackForthByPositionCommand canceled");
|
||||
}
|
||||
context->client.hover();
|
||||
context->sleep_for(pause_time);
|
||||
if (!context->client.moveToPosition(-length, 0, z, velocity, 0, drivetrain,
|
||||
yawMode, lookahead, adaptive_lookahead)){
|
||||
throw std::runtime_error("BackForthByPositionCommand cancelled");
|
||||
throw std::runtime_error("BackForthByPositionCommand canceled");
|
||||
}
|
||||
context->client.hover();
|
||||
context->sleep_for(pause_time);
|
||||
|
|
@ -777,22 +777,22 @@ public:
|
|||
|
||||
context->tasker.execute([=]() {
|
||||
if (!context->client.moveByAngleZ(pitch, -roll, z, yaw, 0)) {
|
||||
throw std::runtime_error("SquareByAngleCommand cancelled");
|
||||
throw std::runtime_error("SquareByAngleCommand canceled");
|
||||
}
|
||||
context->client.hover();
|
||||
context->sleep_for(pause_time);
|
||||
if (!context->client.moveByAngleZ(-pitch, -roll, z, yaw, 0)) {
|
||||
throw std::runtime_error("SquareByAngleCommand cancelled");
|
||||
throw std::runtime_error("SquareByAngleCommand canceled");
|
||||
}
|
||||
context->client.hover();
|
||||
context->sleep_for(pause_time);
|
||||
if (!context->client.moveByAngleZ(-pitch, roll, z, yaw, 0)) {
|
||||
throw std::runtime_error("SquareByAngleCommand cancelled");
|
||||
throw std::runtime_error("SquareByAngleCommand canceled");
|
||||
}
|
||||
context->client.hover();
|
||||
context->sleep_for(pause_time);
|
||||
if (!context->client.moveByAngleZ(-pitch, -roll, z, yaw, 0)){
|
||||
throw std::runtime_error("SquareByAngleCommand cancelled");
|
||||
throw std::runtime_error("SquareByAngleCommand canceled");
|
||||
}
|
||||
context->client.hover();
|
||||
context->sleep_for(pause_time);
|
||||
|
|
@ -833,7 +833,7 @@ public:
|
|||
|
||||
float x = 0, y = 0;
|
||||
if (getSwitch("-r").toInt() == 1) {
|
||||
auto pos = context->client.getPosition();
|
||||
auto pos = context->client.getMultirotorState().getPosition();
|
||||
x += pos.x();
|
||||
y += pos.y();
|
||||
z += pos.z();
|
||||
|
|
@ -842,25 +842,25 @@ public:
|
|||
context->tasker.execute([=]() {
|
||||
if (!context->client.moveToPosition(x + length, y - length, z, velocity, 0, drivetrain,
|
||||
yawMode, lookahead, adaptive_lookahead)){
|
||||
throw std::runtime_error("SquareByPositionCommand cancelled");
|
||||
throw std::runtime_error("SquareByPositionCommand canceled");
|
||||
}
|
||||
context->client.hover();
|
||||
context->sleep_for(pause_time);
|
||||
if (!context->client.moveToPosition(x - length, y - length, z, velocity, 0, drivetrain,
|
||||
yawMode, lookahead, adaptive_lookahead)){
|
||||
throw std::runtime_error("SquareByPositionCommand cancelled");
|
||||
throw std::runtime_error("SquareByPositionCommand canceled");
|
||||
}
|
||||
context->client.hover();
|
||||
context->sleep_for(pause_time);
|
||||
if (!context->client.moveToPosition(x - length, y + length, z, velocity, 0, drivetrain,
|
||||
yawMode, lookahead, adaptive_lookahead)){
|
||||
throw std::runtime_error("SquareByPositionCommand cancelled");
|
||||
throw std::runtime_error("SquareByPositionCommand canceled");
|
||||
}
|
||||
context->client.hover();
|
||||
context->sleep_for(pause_time);
|
||||
if (!context->client.moveToPosition(x + length, y + length, z, velocity, 0, drivetrain,
|
||||
yawMode, lookahead, adaptive_lookahead)){
|
||||
throw std::runtime_error("SquareByPositionCommand cancelled");
|
||||
throw std::runtime_error("SquareByPositionCommand canceled");
|
||||
}
|
||||
context->client.hover();
|
||||
context->sleep_for(pause_time);
|
||||
|
|
@ -902,7 +902,7 @@ public:
|
|||
float x = 0, y = 0;
|
||||
CommandContext* context = params.context;
|
||||
if (getSwitch("-r").toInt() == 1) {
|
||||
auto pos = context->client.getPosition();
|
||||
auto pos = context->client.getMultirotorState().getPosition();
|
||||
x += pos.x();
|
||||
y += pos.y();
|
||||
z += pos.z();
|
||||
|
|
@ -924,7 +924,7 @@ public:
|
|||
throw std::runtime_error("SquareByPathCommand -duration reached");
|
||||
}
|
||||
if (!context->client.moveOnPath(path, velocity, duration, drivetrain, yawMode, lookahead, adaptive_lookahead)){
|
||||
throw std::runtime_error("SquareByPathCommand cancelled");
|
||||
throw std::runtime_error("SquareByPathCommand canceled");
|
||||
}
|
||||
}, iterations);
|
||||
|
||||
|
|
@ -969,7 +969,7 @@ public:
|
|||
auto yawMode = getYawMode();
|
||||
float cx = 0, cy = 0;
|
||||
if (getSwitch("-r").toInt() == 1) {
|
||||
auto pos = context->client.getPosition();
|
||||
auto pos = context->client.getMultirotorState().getPosition();
|
||||
cx += pos.x();
|
||||
cy += pos.y();
|
||||
z += pos.z();
|
||||
|
|
@ -988,7 +988,7 @@ public:
|
|||
float y = cy + std::sin(seg_angle * seg) * radius;
|
||||
if (!context->client.moveToPosition(x, y, z, velocity, duration, drivetrain,
|
||||
yawMode, lookahead, adaptive_lookahead)){
|
||||
throw std::runtime_error("CircleByPositionCommand cancelled");
|
||||
throw std::runtime_error("CircleByPositionCommand canceled");
|
||||
}
|
||||
context->client.hover();
|
||||
context->sleep_for(pause_time);
|
||||
|
|
@ -1037,7 +1037,7 @@ public:
|
|||
CommandContext* context = params.context;
|
||||
float cx = 0, cy = 0;
|
||||
if (getSwitch("-r").toInt() == 1) {
|
||||
auto pos = context->client.getPosition();
|
||||
auto pos = context->client.getMultirotorState().getPosition();
|
||||
cx += pos.x();
|
||||
cy += pos.y();
|
||||
z_path += pos.z();
|
||||
|
|
@ -1077,7 +1077,7 @@ public:
|
|||
throw std::runtime_error("CircleByPath -duration reached");
|
||||
}
|
||||
if (!context->client.moveOnPath(path, velocity, duration, drivetrain, yawMode, lookahead, adaptive_lookahead)) {
|
||||
throw std::runtime_error("CircleByPath cancelled");
|
||||
throw std::runtime_error("CircleByPath canceled");
|
||||
}
|
||||
}, iterations);
|
||||
|
||||
|
|
@ -1096,9 +1096,9 @@ Each record is tab separated floating point numbers containing GPS lat,lon,alt,z
|
|||
bool execute(const DroneCommandParameters& params)
|
||||
{
|
||||
//TODO: get these in one call
|
||||
Vector3r position = params.context->client.getPosition();
|
||||
Quaternionr quaternion = params.context->client.getOrientation();
|
||||
GeoPoint gps_point = params.context->client.getGpsLocation();
|
||||
Vector3r position = params.context->client.getMultirotorState().getPosition();
|
||||
Quaternionr quaternion = params.context->client.getMultirotorState().getOrientation();
|
||||
GeoPoint gps_point = params.context->client.getMultirotorState().gps_location;
|
||||
|
||||
params.shell_ptr->showMessage(gps_point.to_string());
|
||||
params.shell_ptr->showMessage(VectorMath::toString(position));
|
||||
|
|
@ -1275,7 +1275,7 @@ public:
|
|||
};
|
||||
|
||||
|
||||
// std::string beforeScriptStartCallback(const DroneCommandParameters& params, std::string scriptFilePath)
|
||||
// std::string beforeScriptStartCallback(const DroneCommandParameters& param, std::string scriptFilePath)
|
||||
// {
|
||||
// return "";
|
||||
// }
|
||||
|
|
|
|||
|
|
@ -10,7 +10,7 @@
|
|||
#include "common/common_utils/FileSystem.hpp"
|
||||
#include "common/ClockFactory.hpp"
|
||||
#include "vehicles/multirotor/api/MultirotorRpcLibClient.hpp"
|
||||
#include "vehicles/multirotor/controllers/MultirotorApiBase.h"
|
||||
#include "vehicles/multirotor/api/MultirotorApiBase.h"
|
||||
#include "RandomPointPoseGenerator.hpp"
|
||||
STRICT_MODE_OFF
|
||||
#ifndef RPCLIB_MSGPACK
|
||||
|
|
@ -50,7 +50,7 @@ public:
|
|||
// pose_generator.next();
|
||||
// client.simSetPose(pose_generator.position, pose_generator.orientation);
|
||||
|
||||
// std::cout << "Collison at " << VectorMath::toString(collision_info.position)
|
||||
// std::cout << "Collision at " << VectorMath::toString(collision_info.position)
|
||||
// << "Moving to next pose: " << VectorMath::toString(pose_generator.position)
|
||||
// << std::endl;
|
||||
|
||||
|
|
@ -67,7 +67,7 @@ public:
|
|||
};
|
||||
const std::vector<ImageResponse>& response = client.simGetImages(request);
|
||||
if (response.size() != 3) {
|
||||
std::cout << "Images were not recieved!" << std::endl;
|
||||
std::cout << "Images were not received!" << std::endl;
|
||||
start_nanos = clock->nowNanos();
|
||||
continue;
|
||||
}
|
||||
|
|
@ -85,7 +85,7 @@ public:
|
|||
results.push(result);
|
||||
|
||||
pose_generator.next();
|
||||
client.simSetPose(Pose(pose_generator.position, pose_generator.orientation), true);
|
||||
client.simSetVehiclePose(Pose(pose_generator.position, pose_generator.orientation), true);
|
||||
}
|
||||
} catch (rpc::timeout &t) {
|
||||
// will display a message like
|
||||
|
|
@ -135,7 +135,7 @@ private:
|
|||
if (file_list.eof())
|
||||
file_list.clear(); //otherwise we can't do any further I/O
|
||||
else if (file_list.bad()) {
|
||||
throw std::runtime_error("Error occured while reading files_list.txt");
|
||||
throw std::runtime_error("Error occurred while reading files_list.txt");
|
||||
}
|
||||
|
||||
return sample;
|
||||
|
|
|
|||
|
|
@ -74,7 +74,7 @@ int main()
|
|||
std::cout << "Press Enter to fly in a 10m box pattern at 3 m/s velocity" << std::endl; std::cin.get();
|
||||
// moveByVelocityZ is an offboard operation, so we need to set offboard mode.
|
||||
client.enableApiControl(true);
|
||||
auto position = client.getPosition();
|
||||
auto position = client.getMultirotorState().getPosition();
|
||||
float z = position.z(); // current position (NED coordinate system).
|
||||
const float speed = 3.0f;
|
||||
const float size = 10.0f;
|
||||
|
|
|
|||
|
|
@ -252,7 +252,7 @@ void ASimModeCar::updateReport()
|
|||
reporter.writeHeading(std::string("Vehicle: ").append(
|
||||
vehicle_name == "" ? "(default)" : vehicle_name));
|
||||
|
||||
const msr::airlib::Kinematics::State* kinematics = wrapper->getTrueKinematics();
|
||||
const msr::airlib::Kinematics::State* kinematics = wrapper->getGroundTruthKinematics();
|
||||
|
||||
reporter.writeValue("Position", kinematics->pose.position);
|
||||
reporter.writeValue("Orientation", kinematics->pose.orientation);
|
||||
|
|
|
|||
|
|
@ -24,7 +24,7 @@ MultiRotorConnector::MultiRotorConnector(VehiclePawnWrapper* wrapper,
|
|||
vehicle_.initialize(vehicle_params_, wrapper_->getPose(),
|
||||
wrapper_->getHomePoint(), environment_);
|
||||
|
||||
controller_ = static_cast<msr::airlib::MultirotorApiBase*>(vehicle_.getController());
|
||||
controller_ = static_cast<msr::airlib::MultirotorApiBase*>(vehicle_.getVehicleApi());
|
||||
|
||||
if (controller_->getRemoteControlID() >= 0)
|
||||
detectUsbRc();
|
||||
|
|
@ -41,7 +41,7 @@ MultiRotorConnector::MultiRotorConnector(VehiclePawnWrapper* wrapper,
|
|||
new MultirotorApiBase(this)));
|
||||
|
||||
std::string message;
|
||||
if (!vehicle_.getController()->isAvailable(message)) {
|
||||
if (!vehicle_.getApi()->isAvailable(message)) {
|
||||
UAirBlueprintLib::LogMessage(FString("Vehicle was not initialized: "), FString(message.c_str()), LogDebugLevel::Failure);
|
||||
UAirBlueprintLib::LogMessage("Tip: check connection info in settings.json", "", LogDebugLevel::Informational);
|
||||
}
|
||||
|
|
@ -52,16 +52,16 @@ msr::airlib::ImageCaptureBase* MultiRotorConnector::getImageCapture()
|
|||
return wrapper_->getImageCapture();
|
||||
}
|
||||
|
||||
Kinematics::State MultiRotorConnector::getTrueKinematics()
|
||||
Kinematics::State MultiRotorConnector::getGroundTruthKinematics()
|
||||
{
|
||||
return * wrapper_->getTrueKinematics();
|
||||
return * wrapper_->getGroundTruthKinematics();
|
||||
}
|
||||
|
||||
MultiRotorConnector::~MultiRotorConnector()
|
||||
{
|
||||
}
|
||||
|
||||
msr::airlib::VehicleApiBase* MultiRotorConnector::getController()
|
||||
msr::airlib::VehicleApiBase* MultiRotorConnector::getVehicleApi()
|
||||
{
|
||||
return controller_;
|
||||
}
|
||||
|
|
@ -324,7 +324,7 @@ void MultiRotorConnector::reset()
|
|||
|
||||
void MultiRotorConnector::resetPrivate()
|
||||
{
|
||||
VehicleConnectorBase::reset();
|
||||
VehicleSimBridgeBase::reset();
|
||||
|
||||
//TODO: should this be done in MultiRotor.hpp?
|
||||
//controller_->reset();
|
||||
|
|
@ -336,7 +336,7 @@ void MultiRotorConnector::resetPrivate()
|
|||
|
||||
void MultiRotorConnector::update()
|
||||
{
|
||||
VehicleConnectorBase::update();
|
||||
VehicleSimBridgeBase::update();
|
||||
|
||||
//this is high frequency physics tick, flier gets ticked at rendering frame rate
|
||||
vehicle_.update();
|
||||
|
|
|
|||
|
|
@ -10,7 +10,7 @@
|
|||
#include "physics//Kinematics.hpp"
|
||||
#include "common/Common.hpp"
|
||||
#include "common/CommonStructs.hpp"
|
||||
#include "controllers/VehicleConnectorBase.hpp"
|
||||
#include "controllers/VehicleSimBridgeBase.hpp"
|
||||
#include "ManualPoseController.h"
|
||||
#include <chrono>
|
||||
#include "api/ApiServerBase.hpp"
|
||||
|
|
@ -18,7 +18,7 @@
|
|||
#include <future>
|
||||
|
||||
|
||||
class MultiRotorConnector : public msr::airlib::VehicleConnectorBase
|
||||
class MultiRotorConnector : public msr::airlib::VehicleSimBridgeBase
|
||||
{
|
||||
public:
|
||||
typedef msr::airlib::real_T real_T;
|
||||
|
|
@ -33,14 +33,14 @@ public:
|
|||
public:
|
||||
virtual ~MultiRotorConnector();
|
||||
|
||||
//VehicleConnectorBase interface
|
||||
//VehicleSimBridgeBase interface
|
||||
//implements game interface to update pawn
|
||||
MultiRotorConnector(VehiclePawnWrapper* vehicle_paw_wrapper, msr::airlib::MultiRotorParams* vehicle_params,
|
||||
UManualPoseController* manual_pose_controller);
|
||||
virtual void updateRenderedState(float dt) override;
|
||||
virtual void updateRendering(float dt) override;
|
||||
|
||||
virtual msr::airlib::VehicleApiBase* getController() override;
|
||||
virtual msr::airlib::VehicleApiBase* getVehicleApi() override;
|
||||
|
||||
//PhysicsBody interface
|
||||
//this just wrapped around MultiRotor physics body
|
||||
|
|
@ -51,7 +51,7 @@ public:
|
|||
|
||||
virtual void setPose(const Pose& pose, bool ignore_collision) override;
|
||||
virtual Pose getPose() override;
|
||||
virtual Kinematics::State getTrueKinematics() override;
|
||||
virtual Kinematics::State getGroundTruthKinematics() override;
|
||||
|
||||
|
||||
virtual bool setSegmentationObjectID(const std::string& mesh_name, int object_id,
|
||||
|
|
|
|||
|
|
@ -180,7 +180,7 @@ void ASimModeWorldMultiRotor::Tick(float DeltaSeconds)
|
|||
|
||||
std::string ASimModeWorldMultiRotor::getLogString() const
|
||||
{
|
||||
const msr::airlib::Kinematics::State* kinematics = getFpvVehiclePawnWrapper()->getTrueKinematics();
|
||||
const msr::airlib::Kinematics::State* kinematics = getFpvVehiclePawnWrapper()->getGroundTruthKinematics();
|
||||
uint64_t timestamp_millis = static_cast<uint64_t>(msr::airlib::ClockFactory::get()->nowNanos() / 1.0E6);
|
||||
|
||||
//TODO: because this bug we are using alternative code with stringstream
|
||||
|
|
@ -226,7 +226,7 @@ ASimModeWorldBase::VehiclePtr ASimModeWorldMultiRotor::createVehicle(VehiclePawn
|
|||
if (vehicle->getPhysicsBody() != nullptr)
|
||||
wrapper->setKinematics(&(static_cast<PhysicsBody*>(vehicle->getPhysicsBody())->getKinematics()));
|
||||
|
||||
return std::static_pointer_cast<VehicleConnectorBase>(vehicle);
|
||||
return std::static_pointer_cast<VehicleSimBridgeBase>(vehicle);
|
||||
}
|
||||
|
||||
void ASimModeWorldMultiRotor::setupClockSpeed()
|
||||
|
|
|
|||
|
|
@ -50,5 +50,5 @@ private:
|
|||
TArray<AActor*> spawned_actors_;
|
||||
|
||||
VehiclePawnWrapper* fpv_vehicle_pawn_wrapper_;
|
||||
TArray <std::shared_ptr<VehicleConnectorBase> > fpv_vehicle_connectors_;
|
||||
TArray <std::shared_ptr<VehicleSimBridgeBase> > fpv_vehicle_connectors_;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -283,7 +283,7 @@ ECameraDirectorMode ASimModeBase::getInitialViewMode() const
|
|||
void ASimModeBase::startRecording()
|
||||
{
|
||||
FRecordingThread::startRecording(getFpvVehiclePawnWrapper()->getImageCapture(),
|
||||
getFpvVehiclePawnWrapper()->getTrueKinematics(), getSettings().recording_settings, getFpvVehiclePawnWrapper());
|
||||
getFpvVehiclePawnWrapper()->getGroundTruthKinematics(), getSettings().recording_settings, getFpvVehiclePawnWrapper());
|
||||
}
|
||||
|
||||
const AirSimSettings& ASimModeBase::getSettings() const
|
||||
|
|
|
|||
|
|
@ -3,7 +3,7 @@
|
|||
#include "CoreMinimal.h"
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
#include "controllers/VehicleConnectorBase.hpp"
|
||||
#include "controllers/VehicleSimBridgeBase.hpp"
|
||||
#include "physics/FastPhysicsEngine.hpp"
|
||||
#include "physics/World.hpp"
|
||||
#include "physics/PhysicsWorld.hpp"
|
||||
|
|
@ -33,7 +33,7 @@ public:
|
|||
virtual void continueForTime(double seconds) override;
|
||||
|
||||
protected:
|
||||
typedef std::shared_ptr<msr::airlib::VehicleConnectorBase> VehiclePtr;
|
||||
typedef std::shared_ptr<msr::airlib::VehicleSimBridgeBase> VehiclePtr;
|
||||
virtual void createVehicles(std::vector<VehiclePtr>& vehicles);
|
||||
size_t getVehicleCount() const;
|
||||
|
||||
|
|
|
|||
|
|
@ -85,7 +85,7 @@ void VehiclePawnWrapper::displayCollisionEffect(FVector hit_location, const FHit
|
|||
}
|
||||
}
|
||||
|
||||
const msr::airlib::Kinematics::State* VehiclePawnWrapper::getTrueKinematics()
|
||||
const msr::airlib::Kinematics::State* VehiclePawnWrapper::getGroundTruthKinematics()
|
||||
{
|
||||
return kinematics_;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -13,10 +13,10 @@
|
|||
#include "NedTransform.h"
|
||||
#include "common/AirSimSettings.hpp"
|
||||
#include "api/VehicleApiBase.hpp"
|
||||
#include "controllers/VehicleConnectorBase.hpp"
|
||||
#include "controllers/VehicleSimBridgeBase.hpp"
|
||||
|
||||
|
||||
class VehiclePawnWrapper : public msr::airlib::VehicleConnectorBase
|
||||
class VehiclePawnWrapper : public msr::airlib::VehicleSimBridgeBase
|
||||
{
|
||||
public: //types
|
||||
typedef msr::airlib::GeoPoint GeoPoint;
|
||||
|
|
@ -70,7 +70,7 @@ public: //interface
|
|||
void setDebugPose(const Pose& debug_pose);
|
||||
|
||||
void setKinematics(const msr::airlib::Kinematics::State* kinematics);
|
||||
const msr::airlib::Kinematics::State* getTrueKinematics();
|
||||
const msr::airlib::Kinematics::State* getGroundTruthKinematics();
|
||||
|
||||
const GeoPoint& getHomePoint() const;
|
||||
const CollisionInfo& getCollisionInfo() const;
|
||||
|
|
|
|||
Загрузка…
Ссылка в новой задаче