/// \brief Primary route call.
void route(RouteNet& inNet) {
mRouteTimer.restart();
routeNet(inNet);
double routeTime = mRouteTimer.elapsed();
mTotalRouteTime += routeTime;
inNet.mProperties[eRouteTime] = routeTime;
}
void CMainDlg::OnTimer(UINT_PTR nIDEvent)
{
if (nIDEvent == 1 && gStop)
{
static boost::timer tmr;
// 延迟3秒关闭
if (tmr.elapsed() > 3)
{
// 检测关闭
trayIcon.Hide();
CloseDialog(0);
TerminateProcess(GetCurrentProcess(), 0);
}
}
else if (nIDEvent == 2 && exiting && !gStop)
{
static int times = 0;
if (times < 3)
{
times++;
// L"正在退出太一，请稍候.."
trayIcon.SetBalloonDetails(GetLocalWStr(strTrayNowExiting), L"", CTrayNotifyIcon::None, 100);
}
}
else
{
SetMsgHandled(false);
}
}
void Reconstruct::wait_idle_time( boost::timer& timer, int idle_time )
{
timer.restart( );
while( boost::posix_time::seconds( timer.elapsed( ) ) < boost::posix_time::milliseconds( idle_time ) && !m_abort_scan )
{
boost::this_thread::sleep( boost::posix_time::milliseconds( 1 ) );
QApplication::processEvents( );
}
}
bool sufficient_time()
{
// return true if enough time has passed since the tests began:
static boost::static_mutex mut = BOOST_STATIC_MUTEX_INIT ;
boost::static_mutex::scoped_lock guard(mut);
static boost::timer t;
// is 10 seconds enough?
return t.elapsed() >= 10.0;
}
virtual boost::unique_future<bool> send(const safe_ptr<core::read_frame>& frame) override
{
CASPAR_VERIFY(format_desc_.height * format_desc_.width * 4 == static_cast<unsigned>(frame->image_data().size()));
return executor_.begin_invoke([=]() -> bool
{
graph_->set_value("tick-time", tick_timer_.elapsed() * format_desc_.fps * 0.5);
tick_timer_.restart();
frame_timer_.restart();
// AUDIO
std::vector<int16_t, tbb::cache_aligned_allocator<int16_t>> audio_buffer;
if (core::needs_rearranging(
frame->multichannel_view(),
channel_layout_,
channel_layout_.num_channels))
{
core::audio_buffer downmixed;
downmixed.resize(
frame->multichannel_view().num_samples()
* channel_layout_.num_channels,
0);
auto dest_view = core::make_multichannel_view<int32_t>(
downmixed.begin(), downmixed.end(), channel_layout_);
core::rearrange_or_rearrange_and_mix(
frame->multichannel_view(),
dest_view,
core::default_mix_config_repository());
audio_buffer = core::audio_32_to_16(downmixed);
}
else
{
audio_buffer = core::audio_32_to_16(frame->audio_data());
}
airsend::add_audio(air_send_.get(), audio_buffer.data(), audio_buffer.size() / channel_layout_.num_channels);
// VIDEO
connected_ = airsend::add_frame_bgra(air_send_.get(), frame->image_data().begin());
graph_->set_text(print());
graph_->set_value("frame-time", frame_timer_.elapsed() * format_desc_.fps * 0.5);
return true;
});
}
float testQuickSort(int vectorSize, int numberOfIterations)
{
vector<int> quick;
quick.reserve(vectorSize);
float elapsedTime = 0;
for (int iterations = 0; iterations < numberOfIterations; ++iterations)
{
quick = fillVectorWithRandomInts(quick.size());
testTimer.restart();
quick = sorter.quickSort(quick);
elapsedTime += testTimer.elapsed();
}
return (elapsedTime / numberOfIterations);
}
float testInsertionSort(int vectorSize, int numberOfIterations)
{
vector<int> insertion;
insertion.reserve(vectorSize);
float elapsedTime = 0;
for (int iterations = 0; iterations < numberOfIterations; ++iterations)
{
insertion = fillVectorWithRandomInts(insertion.size());
testTimer.restart();
insertion = sorter.insertionSort(insertion);
elapsedTime += testTimer.elapsed();
}
return (elapsedTime / numberOfIterations);
}
int64_t get_and_record_age_millis()
{
if (recorded_frame_age_ == -1)
recorded_frame_age_ = static_cast<int64_t>(
since_created_timer_.elapsed() * 1000.0);
return recorded_frame_age_;
}
virtual bool OnGetData(sf::SoundStream::Chunk& data) override
{
win32_exception::ensure_handler_installed_for_thread(
"sfml-audio-thread");
std::pair<std::shared_ptr<core::read_frame>, std::shared_ptr<audio_buffer_16>> audio_data;
input_.pop(audio_data); // Block until available
graph_->set_value("tick-time", perf_timer_.elapsed()*format_desc_.fps*0.5);
perf_timer_.restart();
container_.push_back(std::move(*audio_data.second));
data.Samples = container_.back().data();
data.NbSamples = container_.back().size();
if (audio_data.first)
presentation_age_ = audio_data.first->get_age_millis();
return is_running_;
}
void run_test(int NUM_TESTS, std::vector<int> NUM_POINTS, std::vector<int> NUM_RUNS) {
for (int i = 0; i < NUM_TESTS; ++i) {
kdtree::KDTree<Point> tr(2);
for(int k=0; k<NUM_POINTS[i]; k++) {
double x = ((float)rand() / RAND_MAX) * 200.0 - 100.0;
double y = ((float)rand() / RAND_MAX) * 200.0 - 100.0;
Point p(x,y);
tr.insert( p );
}
timer.restart();
for (int j = 0; j < NUM_RUNS[i]; ++j) {
double x = ((float)rand() / RAND_MAX) * 200.0 - 100.0;
double y = ((float)rand() / RAND_MAX) * 200.0 - 100.0;
Point ps(x,y);
std::pair<Point,double> pt = tr.nearest( ps );
}
double total_time = timer.elapsed();
double time_per_test = total_time / NUM_RUNS[i];
format_line(NUM_POINTS[i], NUM_RUNS[i], total_time, time_per_test);
}
}
std::string StartGate::Execute()
{
static boost::timer _timestamp;
std::cout << "===========================================================" << std::endl;
std::cout << "State: " << ID << "::" << GetStateName(mState) << "\tTime: " << _timestamp.elapsed() << std::endl;
std::cout << std::endl;
bool pathExists = false;
std::vector<Path> paths;
switch (mState)
{
case GoToDepth:
mDesiredYaw = mStartYaw;
mDesiredDepth = mInitDepth;
if(fabs(AI::Utility::DepthDiff(mCurrentDepth, mInitDepth)) < mDepthThresh && fabs(Zebulon::AI::Utility::AngleDiff(mCurrentYaw, mStartYaw)) <= mYawThresh)
{
mInitTravel.Start();
mState = InitTravel;
}
break;
case InitTravel:
mDesiredYaw = mStartYaw;
mDesiredDepth = mInitDepth;
mDesiredAxialThrust = mInitTravelSpeed;
if(mInitTravel.IsFinished())
{
mPathTravel.Start();
mState = PathTravel;
}
std::cout << " \tDesired:\tCurrent:\tDiff" << std::endl;
std::cout << "Timer:"
<< "\t" << mInitTravel.GetRunTime()
<< "\t" << mInitTravel.TimeElapsed()
<< "\t" << mInitTravel.GetRunTime()-mInitTravel.TimeElapsed()
<< std::endl;
break;
case PathTravel:
mDesiredYaw = mStartYaw;
mDesiredDepth = mInitDepth;
mDesiredAxialThrust = mPathTravelSpeed;
pathExists = GetPaths(paths , mCurrentYaw, mDWFrame, mDWProcFrame);
if(mPathTravel.IsFinished() || pathExists == true)
{
mState = LeaveMission;
}
std::cout << " \tDesired:\tCurrent:\tDiff" << std::endl;
std::cout << "Timer:"
<< "\t" << mPathTravel.GetRunTime()
<< "\t" << mPathTravel.TimeElapsed()
<< "\t" << mPathTravel.GetRunTime()-mPathTravel.TimeElapsed()
<< std::endl;
break;
case LeaveMission:
if(mLeaveMission == false)
{
mGlobalInfo->SetInfo(GlobalInfo::FixedYaw,mStartYaw);
//mGlobalInfo->SetInfo(GlobalInfo::StartYaw,mStartYaw);
Utility::Display::CloseAllWindows();
mLeaveMission = true;
}
return NextMission;
break;
default:
std::cout << "ERROR::" << ID << " state " << mState << " does not exist!" << std::endl;
break;
}
//if (mState == PathTravel)
{
AI::Utility::HeadingDisplay(mDWProcFrame, mCurrentYaw, mDesiredYaw, 0, 255, 255);
AI::Utility::DepthDisplay(mDWProcFrame, mCurrentDepth, mDesiredDepth, 0, 192);
AI::Utility::ThrustDisplay(mDWProcFrame, mDesiredAxialThrust, mDesiredLateralThrust);
Utility::Display::DisplayImage("DW Frame",mDWFrame);
Utility::Display::DisplayImage("Processed DW Frame",mDWProcFrame);
}
std::cout << std::endl;
std::cout << " \tDesired:\tCurrent:\tDiff" << std::endl;
std::cout << "Yaw:"
<< "\t" << mDesiredYaw
<< "\t" << mCurrentYaw
<< "\t" << Zebulon::AI::Utility::AngleDiff(mCurrentYaw, mDesiredYaw)
<< std::endl;
std::cout << "Pitch:"
<< "\t" << mDesiredPitch
<< "\t" << mCurrentPitch
<< "\t" << Zebulon::AI::Utility::AngleDiff(mCurrentPitch, mDesiredPitch)
<< std::endl;
std::cout << "Roll:"
<< "\t" << mDesiredRoll
<< "\t" << mCurrentRoll
<< "\t" << Zebulon::AI::Utility::AngleDiff(mCurrentRoll, mDesiredRoll)
<< std::endl;
std::cout << "Depth:"
<< "\t" << mDesiredDepth
<< "\t" << mCurrentDepth
<< "\t" << Zebulon::AI::Utility::AngleDiff(mCurrentDepth, mDesiredDepth)
<< std::endl;
std::cout << std::endl;
std::cout << "Forward:"
<< "\t" << mDesiredAxialThrust
<< std::endl;
std::cout << "Lateral:"
<< "\t" << mDesiredLateralThrust
<< std::endl;
mGlobalCommand->SetDesiredDepth(mDesiredDepth);
mGlobalCommand->SetDesiredYaw(mDesiredYaw);
mGlobalCommand->SetDesiredAxialVel(mDesiredAxialThrust);
mGlobalCommand->SetDesiredPitch(mDesiredPitch);
mGlobalCommand->SetDesiredRoll(mDesiredRoll);
return "KeepRunning";
}
void timeElapsed(const boost::timer &time){
cout<<"finished in "<<floor(time.elapsed()/60)<<"min "
<<fmod(time.elapsed(),60) <<"s"<<endl;
}
virtual HRESULT STDMETHODCALLTYPE VideoInputFrameArrived(IDeckLinkVideoInputFrame* video, IDeckLinkAudioInputPacket* audio)
{
if(!video)
return S_OK;
try
{
graph_->set_value("tick-time", tick_timer_.elapsed()*format_desc_.fps*0.5);
tick_timer_.restart();
frame_timer_.restart();
// PUSH
void* bytes = nullptr;
if(FAILED(video->GetBytes(&bytes)) || !bytes)
return S_OK;
safe_ptr<AVFrame> av_frame(avcodec_alloc_frame(), av_free);
avcodec_get_frame_defaults(av_frame.get());
av_frame->data[0] = reinterpret_cast<uint8_t*>(bytes);
av_frame->linesize[0] = video->GetRowBytes();
av_frame->format = PIX_FMT_UYVY422;
av_frame->width = video->GetWidth();
av_frame->height = video->GetHeight();
av_frame->interlaced_frame = format_desc_.field_mode != core::field_mode::progressive;
av_frame->top_field_first = format_desc_.field_mode == core::field_mode::upper ? 1 : 0;
std::shared_ptr<core::audio_buffer> audio_buffer;
// It is assumed that audio is always equal or ahead of video.
if(audio && SUCCEEDED(audio->GetBytes(&bytes)) && bytes)
{
auto sample_frame_count = audio->GetSampleFrameCount();
auto audio_data = reinterpret_cast<int32_t*>(bytes);
audio_buffer = std::make_shared<core::audio_buffer>(audio_data, audio_data + sample_frame_count*format_desc_.audio_channels);
}
else
audio_buffer = std::make_shared<core::audio_buffer>(audio_cadence_.front(), 0);
// Note: Uses 1 step rotated cadence for 1001 modes (1602, 1602, 1601, 1602, 1601)
// This cadence fills the audio mixer most optimally.
sync_buffer_.push_back(audio_buffer->size());
if(!boost::range::equal(sync_buffer_, audio_cadence_))
{
CASPAR_LOG(trace) << print() << L" Syncing audio.";
return S_OK;
}
muxer_.push(audio_buffer);
muxer_.push(av_frame, hints_);
boost::range::rotate(audio_cadence_, std::begin(audio_cadence_)+1);
// POLL
for(auto frame = muxer_.poll(); frame; frame = muxer_.poll())
{
if(!frame_buffer_.try_push(make_safe_ptr(frame)))
{
auto dummy = core::basic_frame::empty();
frame_buffer_.try_pop(dummy);
frame_buffer_.try_push(make_safe_ptr(frame));
graph_->set_tag("dropped-frame");
}
}
graph_->set_value("frame-time", frame_timer_.elapsed()*format_desc_.fps*0.5);
graph_->set_value("output-buffer", static_cast<float>(frame_buffer_.size())/static_cast<float>(frame_buffer_.capacity()));
}
catch(...)
{
exception_ = std::current_exception();
return E_FAIL;
}
return S_OK;
}
~bench_no_lock() {
threads.join_all();
std::cerr << "bench_no_lock: x = " << x << ", elapsed time is " << timer.elapsed()
<< "\n";
}
~bench_atomic() {
threads.join_all();
std::cerr << "bench_atomic: x = " << x << ", elapsed time is " << timer.elapsed()
<< "\n";
}
std::string EtTuBrute::Execute()
{
static boost::timer _timestamp;
std::cout << "===========================================================" << std::endl;
std::cout << "State: " << ID << "::" << GetStateName(mState) << "\tTime: " << _timestamp.elapsed() << " " << mStateTimeout.TimeElapsed() << std::endl;
std::cout << std::endl;
bool pathExists = false;
std::vector<Path> paths;
switch (mState)
{
// Go to Path Depth, leave immediatley if path is found
case GoToPathDepth:
mDesiredYaw = mFixedYaw;
mDesiredDepth = mPathDepth;
mDesiredAxialThrust = 0;
mDesiredLateralThrust = 0;
pathExists = GetPaths(paths, mCurrentYaw, mDWFrame, mDWProcFrame);
if((fabs(AI::Utility::DepthDiff(mCurrentDepth, mPathDepth)) < mDepthThresh
&& fabs(Zebulon::AI::Utility::AngleDiff(mCurrentYaw, mFixedYaw)) <= mYawThresh))
// Add a check if the path is seen to go directly to path found ... || )
{
mPathSearchTimer.Start();
InitSearch();
mState = PathSearch;
}
break;
//Search for a path, if found center, if not timeout and look at buoys
case PathSearch:
mDesiredYaw = mFixedYaw;
mDesiredDepth = mPathDepth;
mDesiredAxialThrust = 0;
mDesiredLateralThrust = 0;
pathExists = GetPaths(paths, mCurrentYaw, mDWFrame, mDWProcFrame);
//Add some kind of debouncing to get here, in fetch probably
if(pathExists)
{
mState = DoPath;
}
else if(mPathSearchTimer.IsFinished())
{
mState = GoToObstacleDepth;
}
else
{
SearchStep();
//mPathSearchPattern.SearchStep(mDesiredLateralThrust, mDesiredAxialThrust);
}
std::cout << "Number of Paths: " << paths.size() << std::endl;
std::cout << "Timer:"
<< "\t" << mPathSearchTimer.GetRunTime()
<< "\t" << mPathSearchTimer.TimeElapsed()
<< "\t" << mPathSearchTimer.GetRunTime()-mPathSearchTimer.TimeElapsed()
<< std::endl;
break;
//center in on a path
case DoPath:
//Don't debounce in DoPath
GetPaths(paths, mCurrentYaw, mDWFrame, mDWProcFrame);
if(paths.size()>0)
{
//if path is found change thrust
if(mPathCenteredDebounce.Bounce(mPathFinder.StepPath(&paths[0], mCurrentYaw, mDesiredYaw, mDesiredAxialThrust, mDesiredLateralThrust)))
{
mPathTimer.Initialize(mPathExitTime);
mPathTimer.Start();
//mFixedYaw = mDesiredYaw;
//mDesiredYaw = paths[0].mAngle;
mGlobalInfo->SetInfo(GlobalInfo::FixedYaw, mDesiredYaw);
cvCircle(mDWProcFrame, cvPoint(mDWProcFrame->width/2,mDWProcFrame->height/2), 200, cvScalar(0,255,0), 4);
mState = ExitPath;
}
else
{
std::cout << "NOT CENTERED SEEN" << std::endl;
//mPathCenteredDebounce.Miss();
}
std::cout << "PATHS SEEN" << std::endl;
}
else
{
//can't be centered if didn't see it
mPathCenteredDebounce.Miss();
//mState=PathSearch;
std::cout << "NO PATHS" << std::endl;
}
break;
//lock the path heading for awhile
//could be messy if did not find a path
case ExitPath:
//mDesiredYaw = mFixedYaw;
mDesiredAxialThrust = mExitPathThrust;
mDesiredDepth = mPathDepth;
mDesiredLateralThrust = 0;
if(mPathTimer.IsFinished())
{
mState = GoToObstacleDepth;
}
std::cout << "Timer:"
<< "\t" << mPathTimer.GetRunTime()
<< "\t" << mPathTimer.TimeElapsed()
<< "\t" << mPathTimer.GetRunTime()-mPathTimer.TimeElapsed()
<< std::endl;
break;
case GoToObstacleDepth:
mDesiredYaw = mFixedYaw;
mDesiredDepth = mObstacleDepth;
mDesiredAxialThrust = 0;
mDesiredLateralThrust = 0;
pathExists = GetPaths(paths, mCurrentYaw, mDWFrame, mDWProcFrame);
if((fabs(AI::Utility::DepthDiff(mCurrentDepth, mObstacleDepth)) < mDepthThresh
&& fabs(Zebulon::AI::Utility::AngleDiff(mCurrentYaw, mFixedYaw)) <= mYawThresh))
// Add a check if the path is seen to go directly to path found ... || )
{
//mPathSearchTimer.Start();
mSetupTimer.Start();
mState = Setup;
}
break;
case Setup:
mDesiredYaw = mSetupYaw;
mDesiredDepth = mObstacleDepth;
mDesiredAxialThrust = mSetupAxial;
mDesiredLateralThrust = mSetupLateral;
//pathExists = GetPaths(paths , mCurrentYaw, mDWFrame, mDWProcFrame);
if(mSetupTimer.IsFinished())
{
mFireTimer.Start();
mState = Fire;
}
std::cout << " \tDesired:\tCurrent:\tDiff" << std::endl;
std::cout << "Timer:"
<< "\t" << mSetupTimer.GetRunTime()
<< "\t" << mSetupTimer.TimeElapsed()
<< "\t" << mSetupTimer.GetRunTime()-mSetupTimer.TimeElapsed()
<< std::endl;
break;
/*if(mDriveForwardTimer.IsFinished())
{
mState = LeaveMission;
}
break;*/
case Fire:
mDesiredYaw = mFixedYaw;
mDesiredDepth = mObstacleDepth;
mDesiredAxialThrust = 0;
mDesiredLateralThrust = 0;
mGlobalCommand->SetTorpedo1(true);
mGlobalCommand->SetTorpedo2(true);
if(mFireTimer.IsFinished())
{
mGlobalCommand->SetTorpedo1(false);
mGlobalCommand->SetTorpedo2(false);
mState = LeaveMission;
}
break;
case LeaveMission:
if(mLeaveMission == false)
{
Utility::Display::CloseAllWindows();
mLeaveMission = true;
}
return NextMission;
default:
std::cout << "ERROR::" << ID << " state " << mState << " does not exist" << std::endl;
break;
}
std::cout << std::endl;
std::cout << " \tDesired:\tCurrent:\tDiff" << std::endl;
std::cout << "Yaw:"
<< "\t" << mDesiredYaw
<< "\t" << mCurrentYaw
<< "\t" << Zebulon::AI::Utility::AngleDiff(mCurrentYaw, mDesiredYaw)
<< std::endl;
std::cout << "Pitch:"
<< "\t" << mDesiredPitch
<< "\t" << mCurrentPitch
<< "\t" << Zebulon::AI::Utility::AngleDiff(mCurrentPitch, mDesiredPitch)
<< std::endl;
std::cout << "Roll:"
<< "\t" << mDesiredRoll
<< "\t" << mCurrentRoll
<< "\t" << Zebulon::AI::Utility::AngleDiff(mCurrentRoll, mDesiredRoll)
<< std::endl;
std::cout << "Depth:"
<< "\t" << mDesiredDepth
<< "\t" << mCurrentDepth
<< "\t" << Zebulon::AI::Utility::AngleDiff(mCurrentDepth, mDesiredDepth)
<< std::endl;
std::cout << std::endl;
std::cout << "Forward:"
<< "\t" << mDesiredAxialThrust
<< std::endl;
std::cout << "Lateral:"
<< "\t" << mDesiredLateralThrust
<< std::endl;
if (mState == PathSearch || mState == DoPath || mState == ExitPath)
{
AI::Utility::HeadingDisplay(mDWProcFrame, mCurrentYaw, mDesiredYaw, 0, 255, 255);
AI::Utility::DepthDisplay(mDWProcFrame, mCurrentDepth, mDesiredDepth, 0, 192);
AI::Utility::ThrustDisplay(mDWProcFrame, mDesiredAxialThrust, mDesiredLateralThrust);
}
else
{
AI::Utility::HeadingDisplay(mFWProcFrame, mCurrentYaw, mDesiredYaw, 0, 255, 255);
AI::Utility::DepthDisplay(mFWProcFrame, mCurrentDepth, mDesiredDepth, 0, 192);
AI::Utility::ThrustDisplay(mFWProcFrame, mDesiredAxialThrust, mDesiredLateralThrust);
}
Utility::Display::DisplayImage("Post Processing FW",mFWProcFrame);
Utility::Display::DisplayImage("Post Processing DW",mDWProcFrame);
mGlobalCommand->SetDesiredPitch(mDesiredPitch);
mGlobalCommand->SetDesiredRoll(mDesiredRoll);
mGlobalCommand->SetDesiredYaw(mDesiredYaw);
mGlobalCommand->SetDesiredAxialVel(mDesiredAxialThrust);
mGlobalCommand->SetDesiredLateralVel(mDesiredLateralThrust);
mGlobalCommand->SetDesiredDepth(mDesiredDepth);
return "KeepRunning";
}
~bench_boost_mutex() {
threads.join_all();
std::cerr << "bench_boost_mutex: x = " << x << ", elapsed time is " << timer.elapsed()
<< "\n";
}
bool poll()
{
bool done = (t_.elapsed() > md_seconds);
if(done) Done();
return done;
}
double elapsed(){return t_.elapsed();}
std::string SimpleStartGate::Execute()
{
static boost::timer _timestamp;
std::cout << "===========================================================" << std::endl;
std::cout << "State: " << ID << "::" << GetStateName(mState)<< "\tTime: " << _timestamp.elapsed() << std::endl;
std::cout << std::endl;
switch (mState)
{
case GoToDepth:
mDesiredYaw = mStartYaw;
mDesiredDepth = mStartDepth;
if(fabs(AI::Utility::DepthDiff(mCurrentDepth, mStartDepth)) < mDepthThresh && fabs(Zebulon::AI::Utility::AngleDiff(mCurrentYaw, mStartYaw)) <= mYawThresh)
{
mState = ThroughGate;
mStopWatch.Start();
}
break;
case ThroughGate:
mDesiredYaw = mStartYaw;
mDesiredDepth = mStartDepth;
mDesiredAxialThrust = mGateSpeed;
if(mStopWatch.IsFinished())
{
mState = LeaveMission;
}
std::cout << " \tDesired:\tCurrent:\tDiff" << std::endl;
//std::cout << std::endl;
std::cout << "Timer:"
<< "\t" << mStopWatch.GetRunTime()
<< "\t" << mStopWatch.TimeElapsed()
<< "\t" << mStopWatch.GetRunTime()-mStopWatch.TimeElapsed()
<< std::endl;
break;
case LeaveMission:
return NextMission;
}
std::cout << std::endl;
std::cout << " \tDesired:\tCurrent:\tDiff" << std::endl;
//std::cout << std::endl;
std::cout << "Yaw:"
<< "\t" << mDesiredYaw
<< "\t" << mCurrentYaw
<< "\t" << Zebulon::AI::Utility::AngleDiff(mCurrentYaw, mDesiredYaw)
<< std::endl;
std::cout << "Pitch:"
<< "\t" << mDesiredPitch
<< "\t" << mCurrentPitch
<< "\t" << Zebulon::AI::Utility::AngleDiff(mCurrentPitch, mDesiredPitch)
<< std::endl;
std::cout << "Roll:"
<< "\t" << mDesiredRoll
<< "\t" << mCurrentRoll
<< "\t" << Zebulon::AI::Utility::AngleDiff(mCurrentRoll, mDesiredRoll)
<< std::endl;
std::cout << "Depth:"
<< "\t" << mDesiredDepth
<< "\t" << mCurrentDepth
<< "\t" << Zebulon::AI::Utility::AngleDiff(mCurrentDepth, mDesiredDepth)
<< std::endl;
std::cout << std::endl;
std::cout << "Forward:"
<< "\t" << mDesiredAxialThrust
<< std::endl;
std::cout << "Lateral:"
<< "\t" << mDesiredLateralThrust
<< std::endl;
mGlobalCommand->SetDesiredPitch(mDesiredPitch);
mGlobalCommand->SetDesiredRoll(mDesiredRoll);
mGlobalCommand->SetDesiredYaw(mDesiredYaw);
mGlobalCommand->SetDesiredAxialVel(mDesiredAxialThrust);
mGlobalCommand->SetDesiredLateralVel(mDesiredLateralThrust);
mGlobalCommand->SetDesiredDepth(mDesiredDepth);
return "KeepRunning";
}
virtual void reportStart() {
timer.restart();
customStart();
}
int main(int argc, char **argv)
{
std::string environment_uri;
std::string robot_name;
std::string plugin_name;
size_t num_trials;
bool self = false;
bool profile = false;
// Parse arguments.
po::options_description desc("Profile OpenRAVE's memory usage.");
desc.add_options()
("num-samples", po::value<size_t>(&num_trials)->default_value(10000),
"number of samples to run")
("self", po::value<bool>(&self)->zero_tokens(),
"run self-collision checks")
("profile", po::value<bool>(&profile)->zero_tokens(),
"remove objects from environment")
("environment_uri",
po::value<std::string>(&environment_uri)->required(),
"number of samples to run")
("robot", po::value<std::string>(&robot_name)->required(),
"robot_name")
("collision_checker",
po::value<std::string>(&plugin_name)->required(),
"collision checker name")
("help",
"print usage information")
;
po::positional_options_description pd;
pd.add("environment_uri", 1);
pd.add("robot", 1);
pd.add("collision_checker", 1);
po::variables_map vm;
po::store(
po::command_line_parser(argc, argv)
.options(desc)
.positional(pd).run(),
vm
);
po::notify(vm);
if (vm.count("help")) {
std::cout << desc << std::endl;
return 1;
}
// Create the OpenRAVE environment.
RaveInitialize(true);
EnvironmentBasePtr const env = RaveCreateEnvironment();
CollisionCheckerBasePtr const collision_checker
= RaveCreateCollisionChecker(env, plugin_name);
env->SetCollisionChecker(collision_checker);
env->StopSimulation();
// "/usr/share/openrave-0.9/data/wamtest1.env.xml"
env->Load(environment_uri);
KinBodyPtr const body = env->GetKinBody(robot_name);
// Generate random configuations.
std::vector<OpenRAVE::dReal> lower;
std::vector<OpenRAVE::dReal> upper;
body->GetDOFLimits(lower, upper);
std::vector<std::vector<double> > data;
data = benchmarks::DataUtils::GenerateRandomConfigurations(
num_trials, lower, upper);
//
RAVELOG_INFO("Running %d collision checks.\n", num_trials);
boost::timer const timer;
if (profile) {
std::string const prof_name = str(
format("CheckCollision.%s.prof") % plugin_name);
RAVELOG_INFO("Writing gperftools information to '%s'.\n",
prof_name.c_str()
);
ProfilerStart(prof_name.c_str());
}
size_t num_collision = 0;
for (size_t i = 0; i < num_trials; ++i) {
body->SetDOFValues(data[i]);
bool is_collision;
if (self) {
is_collision = body->CheckSelfCollision();
} else {
is_collision = env->CheckCollision(body);
}
num_collision += !!is_collision;
}
if (profile) {
ProfilerStop();
}
double const duration = timer.elapsed();
RAVELOG_INFO(
"Ran %d collision checks (%d in collision) in %f seconds (%f checks per second).\n",
num_trials, num_collision, duration, num_trials / duration
);
return 0;
}
void restart() { t.restart(); }
double elapsed() { return t.elapsed() * 1000.0; }
void stop() { elapsed_ = timer_.elapsed(); }
std::string Training::Execute()
{
static boost::timer _timestamp;
std::cout << "===========================================================" << std::endl;
std::cout << "State: " << ID << "::" << GetStateName(mState) << "\tTime: " << _timestamp.elapsed() << std::endl;
std::cout << std::endl;
bool pathExists = false;
std::vector<Path> paths;
switch (mState)
{
// Go to Path Depth, leave immediatley if path is found
case GoToPathDepth:
mDesiredYaw = mFixedYaw;
mDesiredDepth = mPathDepth;
mDesiredAxialThrust = 0;
mDesiredLateralThrust = 0;
//pathExists = GetPaths(paths, mCurrentYaw, mDWFrame, mDWProcFrame);
/*if(pathExists)
{
}*/
if((fabs(AI::Utility::DepthDiff(mCurrentDepth, mPathDepth)) < mDepthThresh
&& fabs(Zebulon::AI::Utility::AngleDiff(mCurrentYaw, mFixedYaw)) <= mYawThresh))
// Add a check if the path is seen to go directly to path found ... || )
{
mPathSearchTimer.Start();
mState = PathSearch;
}
break;
//Search for a path, if found center, if not timeout and look at buoys
case PathSearch:
mDesiredYaw = mFixedYaw;
mDesiredDepth = mPathDepth;
pathExists = GetPaths(paths, mCurrentYaw, mDWFrame, mDWProcFrame);
//Add some kind of debouncing to get here, in fetch probably
if(pathExists)
{
mState = DoPath;
}
else if(mPathSearchTimer.IsFinished())
{
mState = BuoySearch;
}
else
{
mPathSearchPattern.SearchStep(mDesiredLateralThrust, mDesiredAxialThrust);
}
std::cout << "Number of Paths: " << paths.size() << std::endl;
std::cout << "Timer:"
<< "\t" << mPathSearchTimer.GetRunTime()
<< "\t" << mPathSearchTimer.TimeElapsed()
<< "\t" << mPathSearchTimer.GetRunTime()-mPathSearchTimer.TimeElapsed()
<< std::endl;
break;
//center in on a path
case DoPath:
//Don't debounce in DoPath
GetPaths(paths, mCurrentYaw, mDWFrame, mDWProcFrame);
if(paths.size()>0)
{
//if path is found change thrust
if(mPathCenteredDebounce.Bounce(mPathFinder.StepPath(&paths[0], mCurrentYaw, mDesiredYaw, mDesiredAxialThrust, mDesiredLateralThrust)))
{
mPathTimer.Initialize(mPathExitTime);
mPathTimer.Start();
//mFixedYaw = mDesiredYaw;
//mDesiredYaw = paths[0].mAngle;
mGlobalInfo->SetInfo(GlobalInfo::FixedYaw, mDesiredYaw); // should be current yaw
cvCircle(mDWProcFrame, cvPoint(mDWProcFrame->width/2,mDWProcFrame->height/2), 200, cvScalar(0,255,0), 4);
mState = ExitPath;
}
else
{
std::cout << "NOT CENTERED SEEN" << std::endl;
//mPathCenteredDebounce.Miss();
}
std::cout << "PATHS SEEN" << std::endl;
}
else
{
//can't be centered if didn't see it
mPathCenteredDebounce.Miss();
//mState=PathSearch;
std::cout << "NO PATHS" << std::endl;
}
break;
//lock the path heading for awhile
//could be messy if did not find a path
case ExitPath:
//mDesiredYaw = mFixedYaw;
mDesiredAxialThrust = mExitPathThrust;
mDesiredDepth = mPathDepth;
mDesiredLateralThrust = 0;
if(mPathTimer.IsFinished())
{
//InitSearch();
mState = GoToBuoyDepth;
}
std::cout << "Timer:"
<< "\t" << mPathTimer.GetRunTime()
<< "\t" << mPathTimer.TimeElapsed()
<< "\t" << mPathTimer.GetRunTime()-mPathTimer.TimeElapsed()
<< std::endl;
break;
//go to a good depth for buoys
case GoToBuoyDepth:
mDesiredYaw = mFixedYaw;
mDesiredDepth = mBuoyDepth;
mDesiredAxialThrust = 0;
mDesiredLateralThrust = 0;
if((fabs(AI::Utility::DepthDiff(mCurrentDepth, mBuoyDepth)) < mDepthThresh
&& fabs(Zebulon::AI::Utility::AngleDiff(mCurrentYaw, mDesiredYaw)) <= mYawThresh))
// Add a check if the path is seen to go directly to path found ... || )
{
mState = BuoySearch;
}
break;
//start searching for one color buoy
case BuoySearch:
mDesiredYaw = mFixedYaw;
mDesiredDepth = mBuoyDepth;
mDesiredAxialThrust = 0;
mDesiredLateralThrust = 0;
//if found attack it
if(mDebounce[mBuoyToAttack[mAttackingBuoy]].GetDebounced())
{
mInitAttackTimer.Start();
mState = InitAttack;
//mState = Attack;
}
//or search
else
{
SearchStep();
}
std::cout << "Current Buoy Debounce: " << mDebounce[mBuoyToAttack[mAttackingBuoy]].GetCount() << std::endl;
break;
case InitAttack:
mCenterI=mFWProcFrame->width/2;
mCenterJ=mFWProcFrame->height/2;
if(mDebounce[mBuoyToAttack[mAttackingBuoy]].GetDebounced())
{
mCenterI = mReturn[mBuoyToAttack[mAttackingBuoy]].mCenterI;
mCenterJ = mReturn[mBuoyToAttack[mAttackingBuoy]].mCenterJ;
mArea = mReturn[mBuoyToAttack[mAttackingBuoy]].mArea;
// if (mCenteredDebounce.Bounce( fabs(mCenterI - mFWProcFrame->width/2) < mIThresh &&
// fabs(mCenterJ - mFWProcFrame->height/2) < mJThresh &&
// (mTargetArea - mArea) < mAreaCloseThresh ))
/*if (mCurrLaserDist < mAttackLaserDist || mArea > mAreaCloseThresh)
{
mState = LockAttack;
mLockAttackTimer.Initialize(mLockAttackTime);
mLockAttackTimer.Start();
if(mYawAttack)
{
mLockAttackYaw = mCurrentYaw;
}
else if(mLateralAttack)
{
mLockAttackYaw = mFixedYaw;
}
mLockAttackDepth = mCurrentDepth;
}*/
mLostTargetDebounce.Miss();
}
else
{
if(mLostTargetDebounce.Hit())
{
//InitSearch();
//mSearchPattern.InitSearch(mYawFix, mSearchSweepDegrees, mSearchSweepSpeed, mSearchForwardTime, mSearchForwardSpeed, 1, mSearchSmoothSpeed);
mState = BuoySearch;
break;
}
}
if(mYawAttack)
{
mDesiredYaw = mCurrentYaw + (mCenterI - mFWProcFrame->width/2)*mYawPixelScale;
mDesiredLateralThrust = 0;
}
else if(mLateralAttack)
{
mDesiredYaw = mFixedYaw;
mDesiredLateralThrust = (mCenterI - mFWProcFrame->width/2)*mLatPixelScale;;
}
if(mInitAttackTimer.IsFinished())
{
mState = Attack;
}
//mDesiredDepth = mCurrentDepth + (mCenterJ - mFWProcFrame->height/2 + 100)*mDepthPixelScale;
mDesiredDepth = mCurrentDepth + (mCenterJ - mFWProcFrame->height/2)*mDepthPixelScale;
//mDesired.mAxialThrust = (mTargetArea + 1000 - mArea)*mAxialPixelScale;
mDesiredAxialThrust = mInitAxialThrust;
// or constant axial thrust?
std::cout << "Target Data i: " << mCenterI << " j: " << mCenterJ << " area: " << mArea << std::endl;
std::cout << "Laser Dist: " << mCurrLaserDist << std::endl;
break;
//attack the buoy through either L/A or A/Y
//if laser sees it, lock heading
//or if the area is big, lock heading
case Attack:
mCenterI=mFWProcFrame->width/2;
mCenterJ=mFWProcFrame->height/2;
if(mDebounce[mBuoyToAttack[mAttackingBuoy]].GetDebounced())
{
mCenterI = mReturn[mBuoyToAttack[mAttackingBuoy]].mCenterI;
mCenterJ = mReturn[mBuoyToAttack[mAttackingBuoy]].mCenterJ;
mArea = mReturn[mBuoyToAttack[mAttackingBuoy]].mArea;
if (mCenteredDebounce.Bounce( fabs(mCenterI - mFWProcFrame->width/2) < mIThresh &&
fabs(mCenterJ - mFWProcFrame->height/2) < mJThresh && mArea > mTargetArea))
// (mTargetArea - mArea) < mAreaCloseThresh ))
{
mState = LockAttack;
mLockAttackTimer.Initialize(mLockAttackTime);
mLockAttackTimer.Start();
if(mYawAttack)
{
mLockAttackYaw = mCurrentYaw;
}
else if(mLateralAttack)
{
mLockAttackYaw = mFixedYaw;
}
mLockAttackDepth = mCurrentDepth;
}
mLostTargetDebounce.Miss();
}
else
{
if(mLostTargetDebounce.Hit())
{
//InitSearch();
//mSearchPattern.InitSearch(mYawFix, mSearchSweepDegrees, mSearchSweepSpeed, mSearchForwardTime, mSearchForwardSpeed, 1, mSearchSmoothSpeed);
mState = BuoySearch;
break;
}
}
if(mYawAttack)
{
mDesiredYaw = mCurrentYaw + (mCenterI - mFWProcFrame->width/2)*mYawPixelScale;
mDesiredLateralThrust = 0;
}
else if(mLateralAttack)
{
mDesiredYaw = mFixedYaw;
mDesiredLateralThrust = (mCenterI - mFWProcFrame->width/2)*mLatPixelScale;;
}
//mDesiredDepth = mCurrentDepth + (mCenterJ - mFWProcFrame->height/2 + 100)*mDepthPixelScale;
mDesiredDepth = mCurrentDepth + (mCenterJ - mFWProcFrame->height/2)*mDepthPixelScale;
//mDesired.mAxialThrust = (mTargetArea + 1000 - mArea)*mAxialPixelScale;
mDesiredAxialThrust = mAttackThrust;
// or constant axial thrust?
std::cout << "Target Data i: " << mCenterI << " j: " << mCenterJ << " area: " << mArea << std::endl;
std::cout << "Laser Dist: " << mCurrLaserDist << std::endl;
break;
//drive at it for a fixed time
//if first buoy do unhit
//if second buoy do unhitandleave
case LockAttack:
mDesiredAxialThrust = mLockAttackThrust;
mDesiredYaw = mLockAttackYaw;
mDesiredLateralThrust = 0;
mDesiredDepth = mLockAttackDepth;
if(mLockAttackTimer.IsFinished())
{
//second buoy, leave
if(mAttackingBuoy)
{
mUnhitLeaveTimer.Initialize(mUnhitLeaveTime);
mUnhitLeaveTimer.Start();
//GoToLeaveDepth, change depth first?
mState = UnhitLeave;
}
//if first buoy, unhit
else
{
mUnhitTimer.Initialize(mUnhitTime);
mUnhitTimer.Start();
mState = Unhit;
}
}
std::cout << "Timer:"
<< "\t" << mLockAttackTimer.GetRunTime()
<< "\t" << mLockAttackTimer.TimeElapsed()
<< "\t" << mLockAttackTimer.GetRunTime()-mLockAttackTimer.TimeElapsed()
<< std::endl;
break;
case Unhit:
mDesiredAxialThrust = mUnhitThrust;
mDesiredYaw = mLockAttackYaw;
mDesiredLateralThrust = 0;
// Maybe change to buoy depth
mDesiredDepth = mLockAttackDepth;
if(mUnhitTimer.IsFinished())
{
mDesiredYaw = mFixedYaw;
mAttackingBuoy++;
InitSearch();
mState = BuoySearch;
}
std::cout << "Timer:"
<< "\t" << mUnhitTimer.GetRunTime()
<< "\t" << mUnhitTimer.TimeElapsed()
<< "\t" << mUnhitTimer.GetRunTime()-mUnhitTimer.TimeElapsed()
<< std::endl;
break;
case UnhitLeave:
mDesiredAxialThrust = mUnhitLeaveThrust;
mDesiredYaw = mLockAttackYaw;
//mDesiredYaw = mFixedYaw;
mDesiredLateralThrust = 0;
// Maybe change to buoy depth
mDesiredDepth = mLockAttackDepth;
if(mUnhitLeaveTimer.IsFinished())
{
mDesiredYaw = mFixedYaw;
mAttackingBuoy++;
mState = GoToLeaveDepth;
}
std::cout << "Timer:"
<< "\t" << mUnhitLeaveTimer.GetRunTime()
<< "\t" << mUnhitLeaveTimer.TimeElapsed()
<< "\t" << mUnhitLeaveTimer.GetRunTime()-mUnhitLeaveTimer.TimeElapsed()
<< std::endl;
break;
case GoToLeaveDepth:
mDesiredYaw = mFixedYaw;
mDesiredDepth = mLeaveDepth;
mDesiredAxialThrust = 0;
mDesiredLateralThrust = 0;
if((fabs(AI::Utility::DepthDiff(mCurrentDepth, mLeaveDepth)) < mDepthThresh
&& fabs(Zebulon::AI::Utility::AngleDiff(mCurrentYaw, mFixedYaw)) <= mYawThresh))
// Add a check if the path is seen to go directly to path found ... || )
{
mLeaveTimer.Initialize(mLeaveTime);
mLeaveTimer.Start();
mState = Leave;
}
break;
case Leave:
mDesiredDepth = mLeaveDepth;
mDesiredAxialThrust = mLeaveThrust;
mDesiredYaw = mFixedYaw;
if (mLeaveTimer.IsFinished() || mPathDebounce.Bounce(mPathFinder.GetPathsDavid(mCurrentYaw, mDWFrame, mDWProcFrame).size() > 0))
{
//mDesiredDepth = mDefaultDepth;
mState = LeaveMission;
}
break;
case LeaveMission:
//mGlobalInfo->SetInfo(GlobalInfo::FixedYaw, mFixedYaw);
if(mLeaveMission == false)
{
Utility::Display::CloseAllWindows();
mLeaveMission = true;
}
return NextMission;
default:
std::cout << "ERROR::" << ID << " state " << mState << " does not exist" << std::endl;
break;
}
std::cout << "Num Buoys Attacked: " << mAttackingBuoy << std::endl;
std::cout << std::endl;
std::cout << " \tDesired:\tCurrent:\tDiff" << std::endl;
std::cout << "Yaw:"
<< "\t" << mDesiredYaw
<< "\t" << mCurrentYaw
<< "\t" << Zebulon::AI::Utility::AngleDiff(mCurrentYaw, mDesiredYaw)
<< std::endl;
std::cout << "Pitch:"
<< "\t" << mDesiredPitch
<< "\t" << mCurrentPitch
<< "\t" << Zebulon::AI::Utility::AngleDiff(mCurrentPitch, mDesiredPitch)
<< std::endl;
std::cout << "Roll:"
<< "\t" << mDesiredRoll
<< "\t" << mCurrentRoll
<< "\t" << Zebulon::AI::Utility::AngleDiff(mCurrentRoll, mDesiredRoll)
<< std::endl;
std::cout << "Depth:"
<< "\t" << mDesiredDepth
<< "\t" << mCurrentDepth
<< "\t" << Zebulon::AI::Utility::AngleDiff(mCurrentDepth, mDesiredDepth)
<< std::endl;
std::cout << std::endl;
std::cout << "Forward:"
<< "\t" << mDesiredAxialThrust
<< std::endl;
std::cout << "Lateral:"
<< "\t" << mDesiredLateralThrust
<< std::endl;
/*std::cout << " Yaw " << mCurrentYaw << " -> " << mDesiredYaw
<< " Depth " << mCurrentDepth << " -> " << mDesiredDepth << std::endl
<< " Axial " << mDesiredAxialThrust << " Lat " << mDesiredLateralThrust << std::endl
<< std::endl;*/
std::cout << "mCurrLaserDist: " << mCurrLaserDist << std::endl;
cvZero(mDisplayLaser);
double x, y;
for(int i = 0; i < mRawLaserData.size(); i++)
{
//cout << mRawLaserData[i].mZ << ", ";
//cvPoint(mRawLaserData[i].mX, mRawLaserData[i].mY);
/*x = (mRawLaserData[i].mX*sin(mRawLaserData[i].mZ))*30;
y = (mRawLaserData[i].mX*cos(mRawLaserData[i].mZ))*30;
cvCircle(mDisplayLaser, cvPoint(x+320, y+240), 1, cvScalar(0,255,0), 1);*/
x = (mRawLaserData[i].mX*(0) - mRawLaserData[i].mY*(-1))*500;
y = (mRawLaserData[i].mX*(-1) + mRawLaserData[i].mY*(0))*500;
//if(sqrt(mRawLaserData[i].mX*mRawLaserData[i].mX - mRawLaserData[i].mY*mRawLaserData[i].mY) > 0.1)
{
cvCircle(mDisplayLaser, cvPoint(x+320, y+470), 1, cvScalar(0,255,0), 1);
}
//cout << mRawLaserData[i].mZ << ", ";
}
//cout << endl;
//std::cout << "mLaserDist: " << mLaserDist << std::endl;
for(int i = 0; i < mLaserObjects.size(); i++)
{
x = (mLaserObjects[i].mX*(0) - mLaserObjects[i].mY*(-1))*500;
y = (mLaserObjects[i].mX*(-1) + mLaserObjects[i].mY*(0))*500;
CvFont font;
cvInitFont(&font,CV_FONT_HERSHEY_SIMPLEX, 0.5,0.5,0,1);
//std::cout << "mCurrLaserDist: " << mCurrLaserDist << std::endl;
if(mCurrLaserDist > 0)
{
cvLine(mDisplayLaser, cvPoint(320, 470), cvPoint(x+320, y+470), cvScalar(0,0,255), 1);
char dist[10];
sprintf(dist, "Dist: %lf", mCurrLaserDist);
cvPutText (mDisplayLaser, dist, cvPoint((x+320)/2, (y+470)/2), &font, cvScalar(255,255,0));
}
cvCircle(mDisplayLaser, cvPoint(x+320, y+470), mLaserObjects[i].mZ*500, cvScalar(0,0,255), 2);
char radius[10];
sprintf(radius, "Rad: %lf", mLaserObjects[i].mZ*39.37007874015748);
cvPutText (mDisplayLaser, radius, cvPoint(x+320, y+470), &font, cvScalar(255,255,0));
}
//////////////////////////////////////////////////////
// DO NOT USE THIS THIS WILL CRASH THE PROGRAM
// WHEN IT IS IN AUTONOMOUS MODE
//////
//cvShowImage("LaserData", mDisplayLaser);
//cvWaitKey(10);
///////////////////////////////////////////////////////
///////////////////////////////////////////////////////
// USE THIS
Utility::Display::DisplayImage("LaserData", mDisplayLaser);
///////////////////////////////////////////////////////
if (mState == DoPath || mState == PathSearch || mState == Leave)
{
AI::Utility::HeadingDisplay(mDWProcFrame, mCurrentYaw, mDesiredYaw, 0, 255, 255);
AI::Utility::DepthDisplay(mDWProcFrame, mCurrentDepth, mDesiredDepth, 0, 192);
AI::Utility::ThrustDisplay(mDWProcFrame, mDesiredAxialThrust, mDesiredLateralThrust);
}
else
{
AI::Utility::HeadingDisplay(mFWProcFrame, mCurrentYaw, mDesiredYaw, 0, 255, 255);
AI::Utility::DepthDisplay(mFWProcFrame, mCurrentDepth, mDesiredDepth, 0, 192);
AI::Utility::ThrustDisplay(mFWProcFrame, mDesiredAxialThrust, mDesiredLateralThrust);
}
Utility::Display::DisplayImage("Post Processing FW",mFWProcFrame);
Utility::Display::DisplayImage("Post Processing DW",mDWProcFrame);
mGlobalCommand->SetDesiredPitch(mDesiredPitch);
mGlobalCommand->SetDesiredRoll(mDesiredRoll);
mGlobalCommand->SetDesiredYaw(mDesiredYaw);
mGlobalCommand->SetDesiredAxialVel(mDesiredAxialThrust);
mGlobalCommand->SetDesiredLateralVel(mDesiredLateralThrust);
mGlobalCommand->SetDesiredDepth(mDesiredDepth);
return "KeepRunning";
}