/// \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"; }