Skip to content

Commit

Permalink
[variable_renames] see full commit msg for details (#2538)
Browse files Browse the repository at this point in the history
multirotor->multirotorphysicsbody, rotor->rotoractuator
rotor_info -> rotor actuator info, phys_vehicle->multirotor_physics_body_
  • Loading branch information
madratman authored Apr 8, 2020
1 parent 0505f46 commit 25a628e
Show file tree
Hide file tree
Showing 12 changed files with 55 additions and 56 deletions.
4 changes: 2 additions & 2 deletions AirLib/AirLib.vcxproj
Original file line number Diff line number Diff line change
Expand Up @@ -165,10 +165,10 @@
<ClInclude Include="include\sensors\SensorCollection.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\MultiRotorPhysicsBody.hpp" />
<ClInclude Include="include\vehicles\multirotor\MultiRotorParams.hpp" />
<ClInclude Include="include\vehicles\multirotor\MultiRotorParamsFactory.hpp" />
<ClInclude Include="include\vehicles\multirotor\Rotor.hpp" />
<ClInclude Include="include\vehicles\multirotor\RotorActuator.hpp" />
<ClInclude Include="include\vehicles\multirotor\RotorParams.hpp" />
</ItemGroup>
<ItemGroup>
Expand Down
4 changes: 2 additions & 2 deletions AirLib/AirLib.vcxproj.filters
Original file line number Diff line number Diff line change
Expand Up @@ -213,7 +213,7 @@
<ClInclude Include="include\common\common_utils\MinWinDefines.hpp">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="include\vehicles\multirotor\MultiRotor.hpp">
<ClInclude Include="include\vehicles\multirotor\MultiRotorPhysicsBody.hpp">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="include\vehicles\multirotor\MultiRotorParams.hpp">
Expand All @@ -222,7 +222,7 @@
<ClInclude Include="include\vehicles\multirotor\MultiRotorParamsFactory.hpp">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="include\vehicles\multirotor\Rotor.hpp">
<ClInclude Include="include\vehicles\multirotor\RotorActuator.hpp">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="include\vehicles\multirotor\RotorParams.hpp">
Expand Down
Original file line number Diff line number Diff line change
@@ -1,12 +1,12 @@
// Copyright (c) Microsoft Corporation. All rights reserved.
// Licensed under the MIT License.

#ifndef msr_airlib_multirotor_hpp
#define msr_airlib_multirotor_hpp
#ifndef msr_airlib_multirotorphysicsbody_hpp
#define msr_airlib_multirotorphysicsbody_hpp

#include "common/Common.hpp"
#include "common/CommonStructs.hpp"
#include "Rotor.hpp"
#include "RotorActuator.hpp"
#include "api/VehicleApiBase.hpp"
#include "api/VehicleSimApiBase.hpp"
#include "MultiRotorParams.hpp"
Expand All @@ -16,9 +16,9 @@

namespace msr { namespace airlib {

class MultiRotor : public PhysicsBody {
class MultiRotorPhysicsBody : public PhysicsBody {
public:
MultiRotor(MultiRotorParams* params, VehicleApiBase* vehicle_api,
MultiRotorPhysicsBody(MultiRotorParams* params, VehicleApiBase* vehicle_api,
Kinematics* kinematics, Environment* environment)
: params_(params), vehicle_api_(vehicle_api)
{
Expand Down Expand Up @@ -101,15 +101,15 @@ class MultiRotor : public PhysicsBody {

virtual uint dragVertexCount() const override
{
return static_cast<uint>(drag_vertices_.size());
return static_cast<uint>(drag_faces_.size());
}
virtual PhysicsBodyVertex& getDragVertex(uint index) override
{
return drag_vertices_.at(index);
return drag_faces_.at(index);
}
virtual const PhysicsBodyVertex& getDragVertex(uint index) const override
{
return drag_vertices_.at(index);
return drag_faces_.at(index);
}

virtual real_T getRestitution() const override
Expand All @@ -121,12 +121,12 @@ class MultiRotor : public PhysicsBody {
return params_->getParams().friction;
}

Rotor::Output getRotorOutput(uint rotor_index) const
RotorActuator::Output getRotorOutput(uint rotor_index) const
{
return rotors_.at(rotor_index).getOutput();
}

virtual ~MultiRotor() = default;
virtual ~MultiRotorPhysicsBody() = default;

private: //methods
void initialize(Kinematics* kinematics, Environment* environment)
Expand All @@ -139,7 +139,7 @@ class MultiRotor : public PhysicsBody {
initSensors(*params_, getKinematics(), getEnvironment());
}

static void createRotors(const MultiRotorParams& params, vector<Rotor>& rotors, const Environment* environment)
static void createRotors(const MultiRotorParams& params, vector<RotorActuator>& rotors, const Environment* environment)
{
rotors.clear();
//for each rotor pose
Expand Down Expand Up @@ -191,22 +191,22 @@ class MultiRotor : public PhysicsBody {
* params.linear_drag_coefficient / 2;

//add six drag vertices representing 6 sides
drag_vertices_.clear();
drag_vertices_.emplace_back(Vector3r(0, 0, -params.body_box.z()), Vector3r(0, 0, -1), drag_factor_unit.z());
drag_vertices_.emplace_back(Vector3r(0, 0, params.body_box.z()), Vector3r(0, 0, 1), drag_factor_unit.z());
drag_vertices_.emplace_back(Vector3r(0, -params.body_box.y(), 0), Vector3r(0, -1, 0), drag_factor_unit.y());
drag_vertices_.emplace_back(Vector3r(0, params.body_box.y(), 0), Vector3r(0, 1, 0), drag_factor_unit.y());
drag_vertices_.emplace_back(Vector3r(-params.body_box.x(), 0, 0), Vector3r(-1, 0, 0), drag_factor_unit.x());
drag_vertices_.emplace_back(Vector3r( params.body_box.x(), 0, 0), Vector3r( 1, 0, 0), drag_factor_unit.x());
drag_faces_.clear();
drag_faces_.emplace_back(Vector3r(0, 0, -params.body_box.z()), Vector3r(0, 0, -1), drag_factor_unit.z());
drag_faces_.emplace_back(Vector3r(0, 0, params.body_box.z()), Vector3r(0, 0, 1), drag_factor_unit.z());
drag_faces_.emplace_back(Vector3r(0, -params.body_box.y(), 0), Vector3r(0, -1, 0), drag_factor_unit.y());
drag_faces_.emplace_back(Vector3r(0, params.body_box.y(), 0), Vector3r(0, 1, 0), drag_factor_unit.y());
drag_faces_.emplace_back(Vector3r(-params.body_box.x(), 0, 0), Vector3r(-1, 0, 0), drag_factor_unit.x());
drag_faces_.emplace_back(Vector3r( params.body_box.x(), 0, 0), Vector3r( 1, 0, 0), drag_factor_unit.x());

}

private: //fields
MultiRotorParams* params_;

//let us be the owner of rotors object
vector<Rotor> rotors_;
vector<PhysicsBodyVertex> drag_vertices_;
vector<RotorActuator> rotors_;
vector<PhysicsBodyVertex> drag_faces_;

std::unique_ptr<Environment> environment_;
VehicleApiBase* vehicle_api_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -3,8 +3,8 @@



#ifndef airsimcore_motor_hpp
#define airsimcore_motor_hpp
#ifndef rotor_actuator_hpp
#define rotor_actuator_hpp

#include <limits>
#include "common/Common.hpp"
Expand All @@ -18,7 +18,7 @@ namespace msr { namespace airlib {
//Rotor gets control signal as input (PWM or voltage represented from 0 to 1) which causes
//change in rotation speed and turning direction and ultimately produces force and thrust as
//output
class Rotor : public PhysicsBodyVertex {
class RotorActuator : public PhysicsBodyVertex {
public: //types
struct Output {
real_T thrust;
Expand All @@ -30,11 +30,11 @@ class Rotor : public PhysicsBodyVertex {
};

public: //methods
Rotor()
RotorActuator()
{
//allow default constructor with later call for initialize
}
Rotor(const Vector3r& position, const Vector3r& normal, RotorTurningDirection turning_direction,
RotorActuator(const Vector3r& position, const Vector3r& normal, RotorTurningDirection turning_direction,
const RotorParams& params, const Environment* environment, uint id = -1)
{
initialize(position, normal, turning_direction, params, environment, id);
Expand Down
1 change: 0 additions & 1 deletion AirLib/include/vehicles/multirotor/RotorParams.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,6 @@
#ifndef msr_airlib_RotorParams_hpp
#define msr_airlib_RotorParams_hpp


#include "common/Common.hpp"

namespace msr {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
#define msr_airlib_ArduCopterSoloApi_h

#include "AdHocConnection.hpp"
#include "vehicles/multirotor/MultiRotor.hpp"
#include "vehicles/multirotor/MultiRotorPhysicsBody.hpp"
#include "vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp"

namespace msr { namespace airlib {
Expand Down
4 changes: 2 additions & 2 deletions AirLibUnitTests/SimpleFlightTest.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@
#include "physics/FastPhysicsEngine.hpp"
#include "vehicles/multirotor/api/MultirotorApiBase.hpp"
#include "common/SteppableClock.hpp"
#include "vehicles/multirotor/MultiRotor.hpp"
#include "vehicles/multirotor/MultiRotorPhysicsBody.hpp"

namespace msr { namespace airlib {

Expand Down Expand Up @@ -39,7 +39,7 @@ class SimpleFlightTest : public TestBase
initial_environment.geo_point = GeoPoint();
environment.reset(new Environment(initial_environment));

MultiRotor vehicle(params.get(), api.get(), kinematics.get(), environment.get());
MultiRotorPhysicsBody vehicle(params.get(), api.get(), kinematics.get(), environment.get());

std::vector<UpdatableObject*> vehicles = { &vehicle };
std::unique_ptr<PhysicsEngineBase> physics_engine(new FastPhysicsEngine());
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -79,7 +79,7 @@ void AFlyingPawn::NotifyHit(class UPrimitiveComponent* MyComp, class AActor* Oth
HitNormal, NormalImpulse, Hit);
}

void AFlyingPawn::setRotorSpeed(const std::vector<MultirotorPawnEvents::RotorInfo>& rotor_infos)
void AFlyingPawn::setRotorSpeed(const std::vector<MultirotorPawnEvents::RotorActuatorInfo>& rotor_infos)
{
for (auto rotor_index = 0; rotor_index < rotor_infos.size(); ++rotor_index) {
auto comp = rotating_movements_[rotor_index];
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@ class AIRSIM_API AFlyingPawn : public APawn
return &pawn_events_;
}
//called by API to set rotor speed
void setRotorSpeed(const std::vector<MultirotorPawnEvents::RotorInfo>& rotor_infos);
void setRotorSpeed(const std::vector<MultirotorPawnEvents::RotorActuatorInfo>& rotor_infos);


private: //variables
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -11,14 +11,14 @@
class MultirotorPawnEvents : public PawnEvents {
public: //types
typedef msr::airlib::real_T real_T;
struct RotorInfo {
struct RotorActuatorInfo {
real_T rotor_speed = 0;
int rotor_direction = 0;
real_T rotor_thrust = 0;
real_T rotor_control_filtered = 0;
};

typedef common_utils::Signal<const std::vector<RotorInfo>&> ActuatorsSignal;
typedef common_utils::Signal<const std::vector<RotorActuatorInfo>&> ActuatorsSignal;

public:
ActuatorsSignal& getActuatorSignal();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -27,10 +27,10 @@ void MultirotorPawnSimApi::initialize()
vehicle_params_ = MultiRotorParamsFactory::createConfig(getVehicleSetting(), sensor_factory);
vehicle_api_ = vehicle_params_->createMultirotorApi();
//setup physics vehicle
phys_vehicle_ = std::unique_ptr<MultiRotor>(new MultiRotor(vehicle_params_.get(), vehicle_api_.get(),
multirotor_physics_body_ = std::unique_ptr<MultiRotor>(new MultiRotorPhysicsBody(vehicle_params_.get(), vehicle_api_.get(),
getKinematics(), getEnvironment()));
rotor_count_ = phys_vehicle_->wrenchVertexCount();
rotor_info_.assign(rotor_count_, RotorInfo());
rotor_count_ = multirotor_physics_body_->wrenchVertexCount();
rotor_actuator_info_.assign(rotor_count_, RotorActuatorInfo());

vehicle_api_->setSimulatedGroundTruth(getGroundTruthKinematics(), getGroundTruthEnvironment());

Expand Down Expand Up @@ -60,21 +60,21 @@ void MultirotorPawnSimApi::updateRenderedState(float dt)

//move collision info from rendering engine to vehicle
const CollisionInfo& collision_info = getCollisionInfo();
phys_vehicle_->setCollisionInfo(collision_info);
multirotor_physics_body_->setCollisionInfo(collision_info);

if (pending_pose_status_ == PendingPoseStatus::RenderStatePending) {
phys_vehicle_->setPose(pending_phys_pose_);
multirotor_physics_body_->setPose(pending_phys_pose_);
pending_pose_status_ = PendingPoseStatus::RenderPending;
}

last_phys_pose_ = phys_vehicle_->getPose();
last_phys_pose_ = multirotor_physics_body_->getPose();

collision_response = phys_vehicle_->getCollisionResponseInfo();
collision_response = multirotor_physics_body_->getCollisionResponseInfo();

//update rotor poses
for (unsigned int i = 0; i < rotor_count_; ++i) {
const auto& rotor_output = phys_vehicle_->getRotorOutput(i);
RotorInfo* info = &rotor_info_[i];
const auto& rotor_output = multirotor_physics_body_->getRotorOutput(i);
RotorActuatorInfo* info = &rotor_actuator_info_[i];
info->rotor_speed = rotor_output.speed;
info->rotor_direction = static_cast<int>(rotor_output.turning_direction);
info->rotor_thrust = rotor_output.thrust;
Expand Down Expand Up @@ -126,7 +126,7 @@ void MultirotorPawnSimApi::updateRendering(float dt)
UAirBlueprintLib::LogMessage(FString(e.what()), TEXT(""), LogDebugLevel::Failure, 30);
}

pawn_events_->getActuatorSignal().emit(rotor_info_);
pawn_events_->getActuatorSignal().emit(rotor_actuator_info_);
}

void MultirotorPawnSimApi::setPose(const Pose& pose, bool ignore_collision)
Expand All @@ -142,7 +142,7 @@ void MultirotorPawnSimApi::resetImplementation()
PawnSimApi::resetImplementation();

vehicle_api_->reset();
phys_vehicle_->reset();
multirotor_physics_body_->reset();
vehicle_api_messages_.clear();
}

Expand All @@ -153,7 +153,7 @@ void MultirotorPawnSimApi::update()
PawnSimApi::update();

//update forces on vertices
phys_vehicle_->update();
multirotor_physics_body_->update();

//update to controller must be done after kinematics have been updated by physics engine
}
Expand All @@ -162,12 +162,12 @@ void MultirotorPawnSimApi::reportState(StateReporter& reporter)
{
PawnSimApi::reportState(reporter);

phys_vehicle_->reportState(reporter);
multirotor_physics_body_->reportState(reporter);
}

MultirotorPawnSimApi::UpdatableObject* MultirotorPawnSimApi::getPhysicsBody()
{
return phys_vehicle_->getPhysicsBody();
return multirotor_physics_body_->getPhysicsBody();
}
//*** End: UpdatableState implementation ***//

Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@
#include "CoreMinimal.h"

#include "PawnSimApi.h"
#include "vehicles/multirotor/MultiRotor.hpp"
#include "vehicles/multirotor/MultiRotorPhysicsBody.hpp"
#include "vehicles/multirotor/MultiRotorParams.hpp"
#include "physics//Kinematics.hpp"
#include "common/Common.hpp"
Expand All @@ -18,12 +18,12 @@ class MultirotorPawnSimApi : public PawnSimApi
public:
typedef msr::airlib::real_T real_T;
typedef msr::airlib::Utils Utils;
typedef msr::airlib::MultiRotor MultiRotor;
typedef msr::airlib::MultiRotorPhysicsBody MultiRotor;
typedef msr::airlib::StateReporter StateReporter;
typedef msr::airlib::UpdatableObject UpdatableObject;
typedef msr::airlib::Pose Pose;

typedef MultirotorPawnEvents::RotorInfo RotorInfo;
typedef MultirotorPawnEvents::RotorActuatorInfo RotorActuatorInfo;

public:
virtual void initialize() override;
Expand Down Expand Up @@ -60,9 +60,9 @@ class MultirotorPawnSimApi : public PawnSimApi
std::unique_ptr<msr::airlib::MultirotorApiBase> vehicle_api_;
std::unique_ptr<msr::airlib::MultiRotorParams> vehicle_params_;

std::unique_ptr<MultiRotor> phys_vehicle_;
std::unique_ptr<MultiRotor> multirotor_physics_body_;
unsigned int rotor_count_;
std::vector<RotorInfo> rotor_info_;
std::vector<RotorActuatorInfo> rotor_actuator_info_;

//show info on collision response from physics engine
CollisionResponse collision_response;
Expand Down

0 comments on commit 25a628e

Please sign in to comment.