Exemple #1
0
		/// \brief Primary route call.
		void route(RouteNet& inNet) {
			mRouteTimer.restart();
			routeNet(inNet);
			double routeTime = mRouteTimer.elapsed();
			mTotalRouteTime += routeTime;
			inNet.mProperties[eRouteTime] = routeTime;
		}
Exemple #2
0
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);
	}
}
Exemple #3
0
 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;
		});
	}
Exemple #6
0
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);
}
Exemple #7
0
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);
}
Exemple #8
0
	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_;
	}
Exemple #10
0
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);
    }
}
Exemple #11
0
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";
}
Exemple #12
0
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;
	}
Exemple #14
0
            ~bench_no_lock() {
			    threads.join_all();
                std::cerr << "bench_no_lock: x = " << x << ", elapsed time is " << timer.elapsed()
                          << "\n";
		    }
Exemple #15
0
            ~bench_atomic() {
			    threads.join_all();
                std::cerr << "bench_atomic: x = " << x << ", elapsed time is " << timer.elapsed()
                          << "\n";
		    }
Exemple #16
0
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";
}
Exemple #17
0
            ~bench_boost_mutex() {
			    threads.join_all();
                std::cerr << "bench_boost_mutex: x = " << x << ", elapsed time is " << timer.elapsed()
                          << "\n";
		    }
Exemple #18
0
		bool poll()
		{ 
			bool done = (t_.elapsed() > md_seconds);
			if(done) Done();
			return done; 
		}
Exemple #19
0
		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";
}
Exemple #21
0
 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;
}
Exemple #23
0
	void restart()		{ t.restart(); }
Exemple #24
0
	double elapsed()	{ return t.elapsed() * 1000.0; }
 void stop() { elapsed_ = timer_.elapsed(); }
Exemple #26
0
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";
}