Skip to content

Commit

Permalink
Fix AsyncTasker. Fix restart of RpcLibServer.
Browse files Browse the repository at this point in the history
  • Loading branch information
Chris Lovett committed Feb 28, 2017
1 parent 70da873 commit 76c3384
Show file tree
Hide file tree
Showing 6 changed files with 43 additions and 34 deletions.
16 changes: 5 additions & 11 deletions AirLib/include/common/common_utils/AsyncTasker.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@
class AsyncTasker {
public:
AsyncTasker(unsigned int thread_count = 4)
: threads_(thread_count), current_([] {}), error_handler_([](std::exception e) {})
: threads_(thread_count), error_handler_([](std::exception e) {})
{
}

Expand All @@ -20,29 +20,24 @@ class AsyncTasker {

void execute(std::function<void()> func, unsigned int iterations = 1)
{
// must keep the func alive between now and the thread start, right now it is just
// on the stack, and so between here and the thread start it goes away and then
// the thread tries to operate on random memory. But this AsyncTasker stays alive
// so it can keep the func state alive as a member.
current_ = func;
if (iterations < 1)
return;

if (iterations == 1)
{
threads_.push([&](int i) {
threads_.push([=](int i) {
try {
current_();
func();
}
catch (std::exception& e) {
error_handler_(e);
};
});
} else {
threads_.push([&](int i) {
threads_.push([=](int i) {
for (unsigned int itr = 0; itr < iterations; ++itr) {
try {
current_();
func();
}
catch (std::exception& e) {
error_handler_(e);
Expand All @@ -55,7 +50,6 @@ class AsyncTasker {

private:
ctpl::thread_pool threads_;
std::function<void()> current_;
std::function<void(std::exception&)> error_handler_;
};

Expand Down
2 changes: 1 addition & 1 deletion AirLib/src/controllers/MavLinkDroneController.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -98,7 +98,7 @@ struct MavLinkDroneController::impl {
if (phys_vehicle_ != nullptr)
mav_vehicle_.reset(new mavlinkcom::MavLinkVehicle(connection_info_.vehicle_sysid, connection_info_.vehicle_compid));
else
mav_vehicle_.release();
delete mav_vehicle_.release();
}

ConnectionInfo getMavConnectionInfo()
Expand Down
9 changes: 8 additions & 1 deletion AirLib/src/rpc/RpcLibServer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,10 @@ struct RpcLibServer::impl {
: server(server_address, port)
{}

~impl() {
server.close_sessions();
}

rpc::server server;
};

Expand Down Expand Up @@ -102,7 +106,9 @@ RpcLibServer::RpcLibServer(DroneControllerCancelable* drone, string server_addre

//required for pimpl
RpcLibServer::~RpcLibServer()
{}
{
stop();
}

void RpcLibServer::start(bool block)
{
Expand All @@ -115,6 +121,7 @@ void RpcLibServer::start(bool block)
void RpcLibServer::stop()
{
pimpl_->server.stop();
pimpl_->server.close_sessions();
}

}} //namespace
Expand Down
34 changes: 17 additions & 17 deletions DroneShell/src/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -202,7 +202,7 @@ class MoveToZCommand : public DroneCommand {
float adaptive_lookahead = std::stof(getSwitch("-adaptive_lookahead").value);
CommandContext* context = params.context;

context->tasker.execute([&]() {
context->tasker.execute([=]() {
context->client.moveToZ(z, velocity, getYawMode(), lookahead, adaptive_lookahead);
});

Expand All @@ -224,7 +224,7 @@ class RotateByYawRateCommand : public DroneCommand {
float duration = std::stof(getSwitch("-duration").value);
CommandContext* context = params.context;

context->tasker.execute([&]() {
context->tasker.execute([=]() {
context->client.rotateByYawRate(yaw_rate, duration);
});

Expand All @@ -246,7 +246,7 @@ class RotateToYawCommand : public DroneCommand {
float margin = std::stof(getSwitch("-yaw_margin").value);
CommandContext* context = params.context;

context->tasker.execute([&]() {
context->tasker.execute([=]() {
context->client.rotateToYaw(yaw, margin);
});

Expand All @@ -263,7 +263,7 @@ class HoverCommand : public DroneCommand {
bool execute(const DroneCommandParameters& params)
{
CommandContext* context = params.context;
context->tasker.execute([&]() {
context->tasker.execute([=]() {
context->client.hover();
});

Expand Down Expand Up @@ -335,7 +335,7 @@ class MoveByManualCommand : public DroneCommand {
auto yawMode = getYawMode();
CommandContext* context = params.context;

context->tasker.execute([&]() {
context->tasker.execute([=]() {
context->client.moveByManual(vx, vy, z, drivetrain, yawMode, duration);
});

Expand Down Expand Up @@ -364,7 +364,7 @@ class MoveByAngleCommand : public DroneCommand {
float duration = std::stof(getSwitch("-duration").value);
CommandContext* context = params.context;

context->tasker.execute([&]() {
context->tasker.execute([=]() {
context->client.moveByAngle(pitch, roll, z, yaw, duration);
});

Expand Down Expand Up @@ -395,7 +395,7 @@ class MoveByVelocityCommand : public DroneCommand {
auto yawMode = getYawMode();
CommandContext* context = params.context;

context->tasker.execute([&]() {
context->tasker.execute([=]() {
context->client.moveByVelocity(vx, vy, vz, duration, drivetrain, yawMode);
});

Expand Down Expand Up @@ -425,7 +425,7 @@ class MoveByVelocityZCommand : public DroneCommand {
auto yawMode = getYawMode();
CommandContext* context = params.context;

context->tasker.execute([&]() {
context->tasker.execute([=]() {
context->client.moveByVelocityZ(vx, vy, z, duration, drivetrain, yawMode);
});

Expand Down Expand Up @@ -490,7 +490,7 @@ class SetSafetyCommand : public DroneCommand {
}
}

context->tasker.execute([&]() {
context->tasker.execute([=]() {
context->client.setSafety(SafetyEval::SafetyViolationType(safety_flags), obs_clearance,
SafetyEval::ObsAvoidanceStrategy(obs_strategy), obs_avoidance_vel, origin, xy_length, min_z, max_z);
});
Expand Down Expand Up @@ -524,7 +524,7 @@ class BackForthByAngleCommand : public DroneCommand {
int iterations = getSwitchInt("-iterations");
CommandContext* context = params.context;

context->tasker.execute([&]() {
context->tasker.execute([=]() {
context->client.moveByAngle(pitch, roll, z, yaw, duration);
context->client.hover();
std::this_thread::sleep_for(std::chrono::duration<double>(pause_time));
Expand Down Expand Up @@ -563,7 +563,7 @@ class BackForthByPositionCommand : public DroneCommand {
auto yawMode = getYawMode();
CommandContext* context = params.context;

context->tasker.execute([&]() {
context->tasker.execute([=]() {
context->client.moveToPosition(length, 0, z, velocity, drivetrain,
yawMode, lookahead, adaptive_lookahead);
context->client.hover();
Expand Down Expand Up @@ -603,7 +603,7 @@ class SquareByAngleCommand : public DroneCommand {
int iterations = getSwitchInt("-iterations");
CommandContext* context = params.context;

context->tasker.execute([&]() {
context->tasker.execute([=]() {
context->client.moveByAngle(pitch, -roll, z, yaw, duration);
context->client.hover();
std::this_thread::sleep_for(std::chrono::duration<double>(pause_time));
Expand Down Expand Up @@ -650,7 +650,7 @@ class SquareByPositionCommand : public DroneCommand {
auto yawMode = getYawMode();
CommandContext* context = params.context;

context->tasker.execute([&]() {
context->tasker.execute([=]() {
context->client.moveToPosition(length, -length, z, velocity, drivetrain,
yawMode, lookahead, adaptive_lookahead);
context->client.hover();
Expand Down Expand Up @@ -711,7 +711,7 @@ class SquareByPathCommand : public DroneCommand {
path.push_back(Vector3r(length, length, z));
}

context->tasker.execute([&]() {
context->tasker.execute([=]() {
context->client.moveOnPath(path, velocity, drivetrain, yawMode, lookahead, adaptive_lookahead);
}, iterations);

Expand Down Expand Up @@ -757,7 +757,7 @@ class CircleByPositionCommand : public DroneCommand {
float seg_angle = 2*M_PIf / seg_count;
CommandContext* context = params.context;

context->tasker.execute([&]() {
context->tasker.execute([=]() {
for(float seg = 0; seg < seg_count; ++seg) {
float x = std::cos(seg_angle * seg) * radius;
float y = std::sin(seg_angle * seg) * radius;
Expand Down Expand Up @@ -833,7 +833,7 @@ class CircleByPathCommand : public DroneCommand {
}
}

context->tasker.execute([&]() {
context->tasker.execute([=]() {
context->client.moveOnPath(path, velocity, drivetrain, yawMode, lookahead, adaptive_lookahead);
}, iterations);

Expand Down Expand Up @@ -891,7 +891,7 @@ See RecordPose for information about log file format")
float adaptive_lookahead = std::stof(getSwitch("-adaptive_lookahead").value);
CommandContext* context = params.context;

context->tasker.execute([&]() {
context->tasker.execute([=]() {
std::ifstream file;
string file_path_name = Utils::getLogFileNamePath("rec_pos", "", ".log", false);
file.exceptions(file.exceptions() | std::ios::failbit);
Expand Down
14 changes: 11 additions & 3 deletions Unreal/Plugins/AirSim/Source/MavMultiRotorConnector.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,12 @@ void MavMultiRotorConnector::initialize(AFlyingPawn* vehicle_pawn)
&environment_, controller_.get());
}

MavMultiRotorConnector::~MavMultiRotorConnector()
{
stopApiServer();
delete controller_.release();
}

void MavMultiRotorConnector::createController(MultiRotor& vehicle)
{
controller_.reset(new msr::airlib::MavLinkDroneController());
Expand Down Expand Up @@ -132,9 +138,11 @@ void MavMultiRotorConnector::startApiServer()
}
void MavMultiRotorConnector::stopApiServer()
{
rpclib_server_->stop();
rpclib_server_.release();
controller_cancelable_.release();
if (rpclib_server_ != nullptr) {
rpclib_server_->stop();
delete rpclib_server_.release();
delete controller_cancelable_.release();
}
}

bool MavMultiRotorConnector::isApiServerStarted()
Expand Down
2 changes: 1 addition & 1 deletion Unreal/Plugins/AirSim/Source/MavMultiRotorConnector.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@ class MavMultiRotorConnector : public VehicleConnectorBase
typedef msr::airlib::UpdatableObject UpdatableObject;

public:
virtual ~MavMultiRotorConnector() = default;
virtual ~MavMultiRotorConnector();

//VehicleConnectorBase interface
//implements game interface to update pawn
Expand Down

0 comments on commit 76c3384

Please sign in to comment.