void network()
{
  ENetAddress address;
  ENetHost* client;
  ENetPeer* peer;
  std::string message;
  ENetEvent event;
  int eventStatus;

  if (enet_initialize () != 0)
  {	
  		std::cout << "Could not start ENet." << std::endl;
      return;
  }else
  	std::cout << "Started ENet." << std::endl; 
  atexit(enet_deinitialize);

  client = enet_host_create(NULL, 1, 2, 57600 / 8, 14400 / 8);

  enet_address_set_host(&address, "localhost");
  char hej[10];
  enet_address_get_host(&address, hej, 10);
  std::cout << hej << std::endl;
  address.port = 1234;
  

  peer = enet_host_connect(client, &address, 2, 0);

  while(true)
  {
    while (enet_host_service(client, &event, 1000) > 0)
    {
      switch (event.type)
      {
      case ENET_EVENT_TYPE_CONNECT:
        std::cout << "We got a new connection " << event.peer->address.host << std::endl;
        break;

      case ENET_EVENT_TYPE_RECEIVE:
      {
          std::cout << "Message from servv: ";
          //enet_peer_disconnect(peer, 3);
          char header[] = {((char*) event.packet->data)[0],
                            ((char*) event.packet->data)[1],
                            ((char*) event.packet->data)[2],
                            ((char*) event.packet->data)[3]};
          int* headerInt = (int*) header;
          std::cout << *headerInt << std::endl;

          if (*headerInt == PLAYER_POSITION)
          {
            Package<PLAYER_POSITION_TYPE>* message = (Package<PLAYER_POSITION_TYPE>*) event.packet->data;
            std::cout << "player: " << message->_player << std::endl;
            std::cout << "x: " << message->_data.x << std::endl;
            std::cout << "y: " << message->_data.y << std::endl;
            std::cout << "z: " << message->_data.z << std::endl;
          }else if (*headerInt == ASSIGN_PLAYER_NUMBER)
          {
            Package<int>* message = (Package<int>*) event.packet->data;
            std::cout << "Player number: " << message->_data << std::endl;
          }
          
          // int message = ((char*) event.packet->data)
          enet_packet_destroy(event.packet);
        
        

        break;
      }
      case ENET_EVENT_TYPE_DISCONNECT:
        
        std::cout << "Disconnected from serv: " << event.peer->data << std::endl;
        event.peer->data = NULL;
        break;
      }
    }

    posMutex.lock();
    glm::vec3 hej = pos;
    posMutex.unlock();
    // std::cout << "Position sent: (" << hej.x << ", " << hej.y << ", " << hej.x << ")" << std::endl;
    ENetPacket* packet = enet_packet_create(&hej, sizeof(glm::vec3), ENET_PACKET_FLAG_RELIABLE);
    enet_peer_send(peer, 0, packet);

    std::cout << "ping: " << peer->roundTripTime << " ms" << std::endl;
		offMutex.lock();
    if (turnOfNetwork)
    {
    	offMutex.unlock();
    	std::cout << "Network recived shoudown command." << std::endl;
    	break;
    }else
    	offMutex.unlock();
  }
  std::cout << "Turning of network." << std::endl;

  enet_host_destroy(client);
}
Exemple #2
0
 void lock()
 { m.lock(); }
Exemple #3
0
 std::mutex::native_handle_type primitive()
 { return m.native_handle(); }
Exemple #4
0
int main(){
	Uavcam *camera; 
 	bool camera_ok, frame_ok;
	int n_saved;
	
	std::stringstream filename;
	std::stringstream directory;

	std::vector<int> jpg_params;
	cv::Mat frame, preview;

	parseConfig();
	jpg_params.push_back(CV_IMWRITE_JPEG_QUALITY);
	jpg_params.push_back(90);

	//Set Signals
	std::signal(SIGINT,exit_signal); 	//Set up Ctrl+C signal

	//Construct Cameras
	if (cameratype == 1){
#ifdef USE_ARAVIS
		camera = new AravisCam();
		std::cout<<"Using Aravis camera"<<std::endl;
#else
		camera = new WebCam();
#endif
	}
	
	if(view)	
		cv::namedWindow("Camera Viewer", cv::WINDOW_AUTOSIZE);
	
	n_saved = checkLog(); 			//Check the log and open it
	openLogtoWrite(n_saved);

	ublox = new Gps();			//Initialize the GPS
	std::thread gps_thread(gpsUpdate);
	
	camera_ok = camera->initializeCam();  	//Initialize the camera

	if (camera_ok) {

		std::cout << "Start camera acquisition in " << start_delay << " seconds" << std::endl;
		std::this_thread::sleep_for(std::chrono::seconds(start_delay));	

		while(!finish){  //--Main Acquisition Loop

			filename.str(""); directory.str(""); //Update filenames
			filename<<"im"<<std::setfill('0')<<std::setw(4)<<++n_saved<<".jpg";
			directory<<FOLDER<<filename.str();
		
			camera->trigger(); //Send camera trigger

			if (usegps){
				if(ublox->data_is_good)	
					std::cout<<"GPS up to date"<<std::endl;
				else	
					std::cout<<"No GPS available" <<std::endl;
			}
			mtx.lock();
			writeImageInfo(ublox->current_loc, filename.str()); //Record GPS 
			mtx.unlock();

			frame_ok = camera->getImage(frame); //Acquire the image

			if(frame_ok){ //Acquired Image

				cv::resize(frame,preview,cv::Size(),sizefac,sizefac,cv::INTER_NEAREST);

				if(saveimg) {
					cv::imwrite(directory.str(), preview, jpg_params);
					std::cout<<"Saved to " << filename.str() <<std::endl;
				}

				if(view) {
					cv::imshow("Camera Viewer", preview); 
					cv::waitKey(50);
				}	
			}
			
			std::this_thread::sleep_for(std::chrono::milliseconds(250));
		} 
		//Finished photographing
		delete camera;
	}

	gps_thread.join();
	closeLog();

	return 0;
}
int main(int argc, const char* argv[])
{
	#ifdef _WIN32
		//Start up Winsock…
		WSADATA wsadata;

		int error = WSAStartup(0x0202, &wsadata);
		if (error)	//Did something happen?
		{
			std::cerr<<"Unable to start winsock"<<std::endl;
			return -1;
		}

		if (wsadata.wVersion != 0x0202)//Did we get the right Winsock version?
		{
			Die("version mismatch");
		}
	#endif


	std::cout << "Enter Server URL: ";
	char url[0x100];
	std::cin.getline(url,sizeof(url));

	std::cout << "Enter your Nickname: ";
	char nickname[0x100] = ":";
	std::cin.getline(nickname+1,sizeof(nickname)-1);

	char*colon = strchr(url,':');
	std::string surl,sport;
	
	if (colon != NULL)
	{
		*colon = 0;
		sport = colon+1;
	}
	else
	{
		sport = ToString(DEFAULT_PORT);
	}
	surl = url;
	
	PrintLine("Attempting to connect to "+surl);

	ADDRINFOA hints;
	memset(&hints,0,sizeof(hints));
	hints.ai_protocol = IPPROTO_TCP;
    hints.ai_family = AF_UNSPEC;
	//hints.
	ADDRINFOA*result;
	if (getaddrinfo(surl.c_str(),sport.c_str(),&hints,&result) != 0)
	{
		Die("Call to getaddrinfo() failed");
	}

	bool connected = false;
	while (result)		//search through all available results until one allows connection. 
						//Chances are we get IPv4 and IPv6 results here but the server will only answer to one of those
	{
		sock = socket(result->ai_family,result->ai_socktype,result->ai_protocol);
		if (sock != INVALID_SOCKET)	//if we can create a socket then we can attempt a connection. It would be rather unusual for this not to work but...
		{
			if (!connect(sock,result->ai_addr,result->ai_addrlen))	//attempt connnection
			{
				//connected
				PrintLine("Connected to "+ToString(*result));	//yay, it worked
				connected = true;
				break;
			}
			else
			{
				closesocket(sock);		//these aren't the droids we're looking for.
				sock = INVALID_SOCKET;
			}

		}
		
		result = result->ai_next;	//moving on.
	}
	if (!connected)
		Die("Failed to connect to "+surl+":"+sport);	//so, yeah, none of them worked.

	Submit(nickname);	//includes leading ':', so that the server knows this is a name, not a message

	netThread = std::thread(NetLoop);	//start read-thread
	
	while (sock != INVALID_SOCKET)
	{
		char c = _getch();	//keep reading characters
		{
			if (c == 3)	//this only works on windows, but ctrl-c is handled better on linux anyway
			{
				Die("Ctrl+C");
				break;
			}
			consoleLock.lock();
			if (c == '\n' || c == '\r')							//user pressed enter/return:
			{
				std::string submit = inputBuffer;				//copy buffer to string
				std::cout << '\r';								//move cursor to line beginning
				for (size_t i = 0; i < inputBufferUsage+1; i++)	//overwrite line with as many blanks as there were characters
					std::cout << ' ';
				std::cout << '\r';								//move cursor to line beginning again
				inputBufferUsage = 0;							//reset character pointer
				inputBuffer[0] = 0;								//write terminating zero to first char
				consoleLock.unlock();							//release console lock
			
				Submit(submit);									//process input
			}
			else
			{
				if (c == Backspace)										//user pressed backspace
				{
					if (inputBufferUsage > 0)
					{
						inputBuffer[--inputBufferUsage] = 0;	//decrement character pointer and overwrite with 0
						std::cout << '\r'<<':'<<inputBuffer<<" \r"<<':'<<inputBuffer;
					}
				}
				else
				{
					if (c == '!' || c == '?' || ( c != -32 && (isalnum(c) || c == ' ')))	//ordinary character
					{
						if (inputBufferUsage+1 < maxInputBufferUsage)	//only allow up to 255 characters though
						{
							inputBuffer[inputBufferUsage++] = c;	//write to the end of the buffer
							inputBuffer[inputBufferUsage] = 0;		//and terminate properly
						}
						std::cout << c;		//update console
					}
				}
				consoleLock.unlock();
			}
		}
	}
	netThread.join();
	netThread.detach();
	netThread.swap(std::thread());
	#ifdef _WIN32
		WSACleanup();
	#endif
	return 0;
}
Exemple #6
0
void gl::globalRelease(bool finish) {
    if (finish) {
        glFinish();
    }
    _globalOpenGLLock.unlock();
}
Exemple #7
0
	static void log(const std::stringstream &message) {
		m.lock();
		std::cout << message.str() << std::endl << std::flush;
		m.unlock();
	}
void CommandReceiver::addCommands(const vector<Command>& commands) {

    m_stackLock.lock();
    m_commandStack.insert(m_commandStack.end(), commands.begin(), commands.end());
    m_stackLock.unlock();
}
void lockScripts()
{
	scriptLock.lock();
}
Exemple #10
0
// Worker thread
static void networkThread(void)
{    
    HttpRequest *request = NULL;
    
    while (true) 
    {
        if (s_need_quit)
        {
            break;
        }
        
        // step 1: send http request if the requestQueue isn't empty
        request = NULL;
        
        s_requestQueueMutex.lock();
        
        //Get request task from queue
        
        if (0 != s_requestQueue->count())
        {
            request = dynamic_cast<HttpRequest*>(s_requestQueue->objectAtIndex(0));
            s_requestQueue->removeObjectAtIndex(0);
        }
        
        s_requestQueueMutex.unlock();
        
        if (NULL == request)
        {
            // Wait for http request tasks from main thread
            std::unique_lock<std::mutex> lk(s_SleepMutex); 
            s_SleepCondition.wait(lk);
            continue;
        }
        
        // step 2: libcurl sync access
        
        // Create a HttpResponse object, the default setting is http access failed
        HttpResponse *response = new HttpResponse(request);
        
        // request's refcount = 2 here, it's retained by HttpRespose constructor
        request->release();
        // ok, refcount = 1 now, only HttpResponse hold it.
        
        int32_t responseCode = -1;
        int retValue = 0;

        // Process the request -> get response packet
        switch (request->getRequestType())
        {
            case HttpRequest::kHttpGet: // HTTP GET
                retValue = processGetTask(request,
                                          writeData, 
                                          response->getResponseData(), 
                                          &responseCode,
                                          writeHeaderData,
                                          response->getResponseHeader());
                break;
            
            case HttpRequest::kHttpPost: // HTTP POST
                retValue = processPostTask(request,
                                           writeData, 
                                           response->getResponseData(), 
                                           &responseCode,
                                           writeHeaderData,
                                           response->getResponseHeader());
                break;

            case HttpRequest::kHttpPut:
                retValue = processPutTask(request,
                                          writeData,
                                          response->getResponseData(),
                                          &responseCode,
                                          writeHeaderData,
                                          response->getResponseHeader());
                break;

            case HttpRequest::kHttpDelete:
                retValue = processDeleteTask(request,
                                             writeData,
                                             response->getResponseData(),
                                             &responseCode,
                                             writeHeaderData,
                                             response->getResponseHeader());
                break;
            
            default:
                CCASSERT(true, "CCHttpClient: unkown request type, only GET and POSt are supported");
                break;
        }
                
        // write data to HttpResponse
        response->setResponseCode(responseCode);
        
        if (retValue != 0) 
        {
            response->setSucceed(false);
            response->setErrorBuffer(s_errorBuffer);
        }
        else
        {
            response->setSucceed(true);
        }

        
        // add response packet into queue
        s_responseQueueMutex.lock();
        s_responseQueue->addObject(response);
        s_responseQueueMutex.unlock();
        
        // resume dispatcher selector
        Director::getInstance()->getScheduler()->resumeTarget(HttpClient::getInstance());
    }
    
    // cleanup: if worker thread received quit signal, clean up un-completed request queue
    s_requestQueueMutex.lock();
    s_requestQueue->removeAllObjects();
    s_requestQueueMutex.unlock();
    
    s_asyncRequestCount -= s_requestQueue->count();
    
    if (s_requestQueue != NULL) {

        s_requestQueue->release();
        s_requestQueue = NULL;
        s_responseQueue->release();
        s_responseQueue = NULL;
    }
    
}
Exemple #11
0
namespace clog {

std::mutex m;

struct logger2 {

	std::stringstream ss;
	const char *level;

	std::string now()
	{
		time_t t = time(NULL);
		struct tm *tm_info;
		tm_info = localtime(&t);

		char buf[20];
		strftime(buf, 20, "%F %T", tm_info);
		return std::string(buf);
	}

	logger2(const char *llevel)
	{
		level = llevel;
	}

	void flush(std::ostream &out)
	{
		m.lock();
		out << level << ": " << now() << " " 
			<< ss.str() << std::endl;
		m.unlock();
	}

	virtual ~logger2()
	{
	}

	template <typename t>
	logger2& operator<<(const t s)
	{
		ss << s;
		return *this;
	}
};

/* TODO Copy and pasted code. */
struct err : public logger2 {

	#ifdef CPP_LOG_ERR_FILE
	std::ofstream ofs;
	#endif

	err() : logger2("Error") {
		#ifdef CPP_LOG_ERR_FILE
		ofs.open(CPP_LOG_ERR_FILE, 
			std::ios_base::app | std::ios_base::out);
		#endif
	};

	~err() {
		#ifdef CPP_LOG_ERR_COUT
		flush(std::cout);
		#endif

		#ifdef CPP_LOG_ERR_CERR
		flush(std::cerr);
		#endif

		#ifdef CPP_LOG_ERR_FILE
		flush(ofs);
		ofs.close();
		#endif
	}
};

struct warn : public logger2 {

	#ifdef CPP_LOG_WARN_FILE
	std::ofstream ofs;
	#endif

	warn() : logger2("Warning") {
		#ifdef CPP_LOG_WARN_FILE
		ofs.open(CPP_LOG_WARN_FILE, 
			std::ios_base::app | std::ios_base::out);
		#endif
	};

	~warn() {
		#ifdef CPP_LOG_WARN_COUT
		flush(std::cout);
		#endif

		#ifdef CPP_LOG_WARN_CERR
		flush(std::cerr);
		#endif

		#ifdef CPP_LOG_WARN_FILE
		flush(ofs);
		ofs.close();
		#endif
	}
};

struct info : public logger2 {

	#ifdef CPP_LOG_INFO_FILE
	std::ofstream ofs;
	#endif

	info() : logger2("Info") {
		#ifdef CPP_LOG_INFO_FILE
		ofs.open(CPP_LOG_INFO_FILE, 
			std::ios_base::app | std::ios_base::out);
		#endif
	};

	~info() {
		#ifdef CPP_LOG_INFO_COUT
		flush(std::cout);
		#endif

		#ifdef CPP_LOG_INFO_CERR
		flush(std::cerr);
		#endif

		#ifdef CPP_LOG_INFO_FILE
		flush(ofs);
		ofs.close();
		#endif
	}
};

}
void withLock(std::mutex & m, Function f) {
  m.lock();
  f();
  m.unlock();
}
Exemple #13
0
Fichier : IO.cpp Projet : cran/rtk
void smplVec::rarefy(vector<double> depts, string ofile, int rep,
        DivEsts* divs, std::vector<vector<rare_map>> & RareSample,
        vector<string>& retCntsSampleName, string& skippedSample,
        vector<vector<vector<uint>>>* abundInRow, vector<vector<vector<uint>>>* occuencesInRow,
        int writes,bool write, bool fillret){
    bool doShuffle = true;
    long curIdx = 0;
    long dep;
    // resize divvs
    divs->richness.resize(depts.size());
    divs->shannon.resize(depts.size());
    divs->simpson.resize(depts.size());
    divs->invsimpson.resize(depts.size());
    divs->chao1.resize(depts.size());
    divs->eve.resize(depts.size());
    
    for(uint i = 0; i < depts.size(); i++){
        dep = (long)depts[i];

        if (dep > totSum){
            skippedSample = divs->SampleName;
            #ifdef notRpackage
            if (verbose){cout<<"skipped sample, because rowSums < depth \n";}
            #endif
            return;
        }
        //long curIdx=(long)totSum+1;
        for (int curRep=0;curRep<rep;curRep++){
            if(curIdx+dep >= (long) totSum || doShuffle == true){
                shuffle_singl();	
                curIdx = 0;
                doShuffle = false;
            }


            //count up
            vector<uint> cnts(numFeatures, 0);
            for (long i=(0+curIdx);i<(dep+curIdx);i++){
                cnts[arr[i]]++;
            }


            curIdx += dep;
            string t_out = ofile;
            if (rep!=1){
                std::ostringstream oss;
                oss<<curRep;
                t_out += "_" +oss.str();
            }

            if (curRep < writes && fillret) {
                rare_map cntsMap;
                // fill map:
                for(uint i = 0; i < cnts.size(); i++){
                    if(cnts[i] != 0){
                        cntsMap.insert( std::make_pair(i, cnts[i]) );
                    }
                }
                RareSample[i].push_back(cntsMap);
                if(curRep == 0){
                    retCntsSampleName[i] = divs->SampleName; // safe the sample name as well
                }
            }
            richness = 0;
            divs->richness[i].push_back(this->getRichness(cnts));
            vector<double> three = this->calc_div(cnts, 4);
            divs->shannon[i].push_back(three[0]);
            divs->simpson[i].push_back(three[1]);
            divs->invsimpson[i].push_back(three[2]);
            divs->chao1[i].push_back(this->calc_chao1(cnts,1));
            divs->eve[i].push_back(this->calc_eveness(cnts));
            richness = 0;

            // save abundance for chao2 calculations later
            rarefyMutex.lock();
            for(uint im = 0; im < IDs.size(); im++){
                //sparse convertions in swap mode
                int id = std::stoi(IDs[im]);
                if(cnts[im] != 0){
                    abundInRow->at(i)[curRep][id]++;	
                    occuencesInRow->at(i)[curRep][id] += cnts[im];
                }
            }
            rarefyMutex.unlock();
        }
    }
}
 void increment(){
     mutex.lock();
     counter.increment();
     mutex.unlock();
 }
 void add_inj(int other) {
     mutex.lock();
     inj.push_back(other);
     mutex.unlock();
 }
void unlockScripts()
{
	scriptLock.unlock();
}
Exemple #17
0
void gl::globalLock() {
    _globalOpenGLLock.lock();
}
Exemple #18
0
namespace bch {

class alignas(4) Test
{
public:
    Test()
    {
#if BCH_SMART_PTR_UNITTEST
        std::cout << "Test ctor(1)" << std::endl;
#endif
        Fill(1);
    }
    explicit Test(int v)
    {
#if BCH_SMART_PTR_UNITTEST
        std::cout << "Test ctor(2)" << std::endl;
#endif
        Fill(v);
    }
    ~Test()
    {
#if BCH_SMART_PTR_UNITTEST
        std::cout << "Test dtor" << std::endl;
#endif
    }
    
    void Baz()
    {
    }

private:
    void Fill(int v)
    {
        for (int i = 0; i < sizeof(mValue)/sizeof(*mValue); ++i)
            mValue[i] = v++;
    }
    char mValue[4];
};

class TestDer: public Test
{
public:
    TestDer()
    {
#if BCH_SMART_PTR_UNITTEST
        std::cout << "TestDer ctor" << std::endl;
#endif
    }
    ~TestDer()
    {
#if BCH_SMART_PTR_UNITTEST
        std::cout << "TestDer dtor" << std::endl;
#endif
    }
};

template <typename T>
void Foo(T value)
{
    value->Baz();
}

const unsigned int kTestCount = 100000000;
//    const unsigned int kTestCount = 1;

std::mutex mutex;
double stdPtrTime = 0;
double noThreadTime = 0;

void std_poiner()
{
    typedef std::chrono::time_point<std::chrono::system_clock> TimerType;
    TimerType start = std::chrono::system_clock::now();


    std::shared_ptr<Test> stdValue(new Test);
    for (unsigned int i = 0; i < kTestCount; ++i)
    {
        Foo(stdValue);
    }
    TimerType end = std::chrono::system_clock::now();
 
    std::chrono::duration<double> elapsed_seconds = end-start;

    double elapsed = elapsed_seconds.count();;
    {
        mutex.lock();
        stdPtrTime += elapsed;
        std::cout << "Time with std::shared_ptr is " << elapsed << std::endl;
        mutex.unlock();
    }
}

void nothread_poiner()
{
    typedef std::chrono::time_point<std::chrono::system_clock> TimerType;
    TimerType start = std::chrono::system_clock::now();

    shared_ptr_nc<Test> stdValue(new Test);
    for (unsigned int i = 0; i < kTestCount; ++i)
    {
        Foo(stdValue);
    }
    TimerType end = std::chrono::system_clock::now();
 
    std::chrono::duration<double> elapsed_seconds = end-start;
    double elapsed = elapsed_seconds.count();

    {
        mutex.lock();
        noThreadTime += elapsed;
        std::cout << "Time with no-thread::shared_ptr_nc is " << elapsed << std::endl;
        mutex.unlock();
    }
}
class SizeTest
{
    char x;
};
}   // namespace bch
Exemple #19
0
void func(int i) {
	mutex.lock();
	std::cout << "func clled, i = " << i << " /" << std::this_thread::get_id() << std::endl;
	mutex.unlock();
}
Exemple #20
0
// Will analyse every buffer from
void detection(uint nDetect, int nStorage) {
    bool isDetected=false;
    int nCurrent=0;
    short *storage_short;
    storage_short=new short [nStorage];
    short *p=0;
    float power;
    int i2=0,count_trans=0;
    std::vector<std::thread> processingV;
    int count=0;
    while(1) {
        std::this_thread::yield();
        if (isDetected) {
            // Store the elements in storage_short
            if (nCurrent<nStorage) {
                // Fill storage_short
                i2=0;
                int conj_rx;
                while ((nCurrent<nStorage) && (i2<2*((int) nDetect))) {
                    conj_rx=pow(-1,nCurrent);
                    //DispVal(conj_rx);
                    storage_short[nCurrent]=(p[i2])*conj_rx;
                    nCurrent++;
                    i2++;
                };
                // Relase the just stored element
                std::this_thread::yield();
                delete[] (usrpQ.front());
                mtxUsrp.lock();
                usrpQ.pop();
                mtxUsrp.unlock();
                std::this_thread::yield();

                //Wait for a new element
                sem_wait(&usrpReady);
                p=usrpQ.front();
            }
            else {
                // End of transmission
                //count_trans=0;
                isDetected=false;
                nCurrent=0;

                processingV.push_back(std::thread(processing, storage_short, nStorage, count));
                count++;
                storage_short=new short[nStorage];
            }
        }
        else {
            // Wait for new element
            sem_wait(&usrpReady);
            p=usrpQ.front();
            // Test of detection
            power=powerTotArray(p, (int)2*nDetect);
            //if(power>50000)DispVal(power);

            count_trans++;//Avoid transient power amplification

            //Power threshold 60GHz=1000

            if ( power>600 && count_trans>10000 && count<10) {
                // If detection, keep the element
                std::cout<<"Detected Transmission\n";
                //exit(1);
                isDetected=true;
            }
            else {
                // Release the element
                std::this_thread::yield();
                delete[] (usrpQ.front());
                mtxUsrp.lock();
                usrpQ.pop();
                mtxUsrp.unlock();
                std::this_thread::yield();
            }
        }
    }
    for (auto& th : processingV) th.join();
}
Exemple #21
0
void exit_signal(int param){
	mtx.lock();
	finish  = 1;
	std::cout << "Exiting on next frame completion.." << std::endl;
	mtx.unlock();
}
Exemple #22
0
  void cloudViewer()
  {
    ros::Publisher pub = nh.advertise<pcl::PointCloud<pcl::PointXYZRGBA> > ("/original_pointcloud", 100);

    //######################################################################################
    // if you want to visualize original pc uncomment the following
    //######################################################################################
    // cv::Mat color, depth;
    // pcl::visualization::PCLVisualizer::Ptr visualizer(new pcl::visualization::PCLVisualizer("Cloud Viewer"));
    // const std::string cloudName = "rendered";
    //
    // lock.lock();
    // color = this->color;
    // depth = this->depth;
    // updateCloud = false;
    // lock.unlock();
    //
    // createCloud(depth, color, cloud);
    //
    // visualizer->addPointCloud(cloud, cloudName);
    // visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, cloudName);
    // visualizer->initCameraParameters();
    // visualizer->setBackgroundColor(0, 0, 0);
    // visualizer->setPosition(mode == BOTH ? color.cols : 0, 0);
    // visualizer->setSize(color.cols, color.rows);
    // visualizer->setShowFPS(true);
    // visualizer->setCameraPosition(0, 0, 0, 0, -1, 0);
    // // visualizer->registerKeyboardCallback(&Receiver::keyboardEvent, *this);
    //######################################################################################

    for(; running && ros::ok();)
    {
      if(updateCloud)
      {
        lock.lock();
        color = this->color;
        depth = this->depth;
        updateCloud = false;
        lock.unlock();

        createCloud(depth, color, cloud);

        //######################################################################################
        // visualizer->updatePointCloud(cloud, cloudName);
        //######################################################################################

        pcl::PointCloud<pcl::PointXYZRGBA>::Ptr msg (new pcl::PointCloud<pcl::PointXYZRGBA>);
        msg->header.frame_id = "kinect2_rgb_optical_frame";
        *msg += *cloud;
        msg->header.stamp = ros::Time::now().toNSec();
        pub.publish(msg);
      }
    //   if(save)
    //   {
    //     save = false;
    //     cv::Mat depthDisp;
    //     dispDepth(depth, depthDisp, 12000.0f);
    //     saveCloudAndImages(cloud, color, depth, depthDisp);
    //   }
      //######################################################################################
    //   visualizer->spinOnce(10);
      //######################################################################################
    }
    //######################################################################################
    // visualizer->close();
    //######################################################################################
  }
Exemple #23
0
int main(int argc, char** argv)
{
  ros::init(argc, argv, "pathplanner");

  ros::NodeHandle nh;

  ros::Subscriber map_sub = nh.subscribe("/map", 1, map_callback);

  ros::Subscriber pose_sub = nh.subscribe("/odometry/filtered", 1, position_callback);

  ros::Subscriber waypoint_sub = nh.subscribe("/waypoint", 1, waypoint_callback);

  disp_path_pub = nh.advertise<nav_msgs::Path>("/path_display", 1);

  act_path_pub = nh.advertise<igvc_msgs::action_path>("/path", 1);

  expanded_pub = nh.advertise<pcl::PointCloud<pcl::PointXYZ>>("/expanded", 1);

  path_planner_map_pub = nh.advertise<pcl::PointCloud<pcl::PointXYZ>>("/path_planner_incremental", 1);

  double baseline = 0.93;

  ros::NodeHandle pNh("~");

  search_problem.Map = pcl::PointCloud<pcl::PointXYZ>().makeShared();
  search_problem.Map->header.frame_id = "/odom";
  search_problem.Octree = boost::make_shared<pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>>(0.1);
  search_problem.Octree->setInputCloud(search_problem.Map);

  if (!pNh.hasParam("goal_threshold") || !pNh.hasParam("threshold") || !pNh.hasParam("speed") ||
      !pNh.hasParam("baseline") || !pNh.hasParam("minimum_omega") || !pNh.hasParam("maximum_omega") ||
      !pNh.hasParam("delta_omega") || !pNh.hasParam("point_turns_enabled") || !pNh.hasParam("reverse_enabled") ||
      !pNh.hasParam("max_obstacle_delta_t") || !pNh.hasParam("alpha") || !pNh.hasParam("beta") ||
      !pNh.hasParam("bounding_distance"))
  {
    ROS_ERROR_STREAM("path planner does not have all required parameters");
    return 0;
  }

  pNh.getParam("goal_threshold", search_problem.GoalThreshold);
  pNh.getParam("threshold", search_problem.Threshold);
  pNh.getParam("speed", search_problem.Speed);
  pNh.getParam("baseline", search_problem.Baseline);
  search_problem.DeltaT = [](double distToStart, double distToGoal) -> double {
    return -((distToStart + distToGoal) / 7 / (pow((distToStart + distToGoal) / 2, 2)) *
             pow(distToStart - (distToStart + distToGoal) / 2, 2)) +
           (distToStart + distToGoal) / 7 + 0.3;
  };
  pNh.getParam("minimum_omega", search_problem.MinimumOmega);
  pNh.getParam("maximum_omega", search_problem.MaximumOmega);
  pNh.getParam("delta_omega", search_problem.DeltaOmega);
  pNh.getParam("point_turns_enabled", search_problem.PointTurnsEnabled);
  pNh.getParam("reverse_enabled", search_problem.ReverseEnabled);
  pNh.getParam("max_obstacle_delta_t", search_problem.MaxObstacleDeltaT);
  pNh.getParam("alpha", search_problem.Alpha);
  pNh.getParam("beta", search_problem.Beta);
  pNh.getParam("bounding_distance", search_problem.BoundingDistance);

  ros::Rate rate(3);
  while (ros::ok())
  {
    ros::spinOnce();

    /* Do not attempt to plan a path if the path length would be greater than 100ft (~30m).
     * This should only happen when we have received either a waypoint or position estimate, but not both.
     * Long paths take forever to compute, and will freeze up this node.
     */
    auto distance_to_goal = search_problem.Start.distTo(search_problem.Goal);
    if (!received_waypoint || distance_to_goal == 0 || distance_to_goal > 60)
      continue;

    planning_mutex.lock();
    Path<SearchLocation, SearchMove> path;
    path = GraphSearch::AStar(search_problem, expanded_callback);
    if (act_path_pub.getNumSubscribers() > 0)
    {
      nav_msgs::Path disp_path_msg;
      disp_path_msg.header.stamp = ros::Time::now();
      disp_path_msg.header.frame_id = "odom";
      if (path.getStates()->empty())
        path.getStates()->push_back(search_problem.Start);
      for (auto loc : *(path.getStates()))
      {
        geometry_msgs::PoseStamped pose;
        pose.header.stamp = disp_path_msg.header.stamp;
        pose.header.frame_id = disp_path_msg.header.frame_id;
        pose.pose.position.x = loc.x;
        pose.pose.position.y = loc.y;
        disp_path_msg.poses.push_back(pose);
      }
      disp_path_pub.publish(disp_path_msg);
      igvc_msgs::action_path act_path_msg;
      act_path_msg.header.stamp = ros::Time::now();
      act_path_msg.header.frame_id = "odom";
      for (auto action : *(path.getActions()))
      {
        igvc_msgs::velocity_pair vels;
        vels.header.stamp = act_path_msg.header.stamp;
        vels.header.frame_id = act_path_msg.header.frame_id;
        if (action.W != 0)
        {
          double radius = action.V / action.W;
          vels.right_velocity = (radius - baseline / 2.) * action.W;
          vels.left_velocity = (radius + baseline / 2.) * action.W;
        }
        else
        {
          vels.right_velocity = 1.0;
          vels.left_velocity = 1.0;
        }
        vels.duration = action.DeltaT;
        act_path_msg.actions.push_back(vels);
      }
      act_path_pub.publish(act_path_msg);
    }

    planning_mutex.unlock();

    rate.sleep();
  }

  return 0;
}
Exemple #24
0
void elog::setCallbackLog(const elog::callbackLog& _callback) {
	// TODO : Check atomicity ...
	g_lock.lock();
	callbackUserLog = _callback;
	g_lock.unlock();
}
//发送数据线程
void netSocketManger::sendThread()
{

    //bool connect = cSocket.Connect(kServerIP,kServerPort);
    bool connect = cSocket.Connect(netSocketManger::sharednetSocketManger()->server.IP.c_str(),netSocketManger::sharednetSocketManger()->server.port);
   // cSocket.Send("bbb",strlen("bbb")+1,0);

    if (connect) {
        isConnect = true;
        SocketData *errorData = newSocketData();
        errorData->eventType = CONNECT_SUCCEED;
        m_mainQueue.Push(errorData);
        //创建接收线程
        m_mutexx.lock();
        netSocketManger::sharednetSocketManger()->createReciveThread();
        m_mutexx.unlock();
        m_sendEvent->Lock();
        while (true) {
            while (!m_sendQueue.IsEmpty()) {
                SocketData *data = m_sendQueue.Pop();
                uLong comprLen = data->bodyLen;
                const char *compr = data->sendData;
                
                T_MSGHEAD_T msgHead;
                
                msgHead.cmd = (unsigned short)data->module.cmd;
                msgHead.com = 0;
                msgHead.enc = 0;
                msgHead.eno = 0;
                msgHead.idx = (uInt)data->sn;
                msgHead.len = (unsigned short)comprLen;
                msgHead.tea = 0;//s_tea;
                
                unsigned char *sendData = (unsigned char*)malloc(comprLen);
                unsigned int pos = 0;
                
                memcpy(&sendData[pos], compr, comprLen);//body
                pos += comprLen;
                
                int ret = cSocket.Send((char*)sendData,pos,0);
                log("发送:%s",compr);
                if (ret <= 0) {
                    m_sendEvent->Unlock();
                    free(sendData);
                    SocketData *errorData = newSocketData();
                    errorData->eventType = DISCONNECT;
                    m_mainQueue.Push(errorData);
                    return;
                }

                free(data->sendData);
                free(data);
                free(sendData);

                log("-----发送数据长度len:%d------",msgHead.len);
                log("-----------");
                
            }
            
            m_sendEvent->Wait();
        }
        m_sendEvent->Unlock();
    }else {
        isConnect = false;
        SocketData *errorData = newSocketData();
        errorData->eventType = CONNECT_FAIL;
        m_mainQueue.Push(errorData);
    }
}
Exemple #26
0
void elog::logChar(int32_t _id, int32_t _level, int32_t _ligne, const char* _funcName, const char* _log) {
	// special callback mode:
	if (callbackUserLog != nullptr) {
		const char* libName = "";
		if (_id >= 0) {
			libName = getList()[_id].first.c_str();
		}
		g_lock.lock();
		if (callbackUserLog != nullptr) {
			callbackUserLog(libName, elog::level(_level), _ligne, _funcName, _log);
		}
		g_lock.unlock();
		return;
	}
	char handle[LENGHT_MAX_LOG] = "";
	memset(handle, ' ', LENGHT_MAX_LOG);
	handle[0] = '\0';
	char* pointer = handle;
	if(getColor() == true) {
		switch(_level) {
			default:
				// nothing to do ...
				break;
			case elog::level_critical:
				strcat(pointer, ETK_BASH_COLOR_BOLD_RED);
				break;
			case elog::level_error:
				strcat(pointer, ETK_BASH_COLOR_RED);
				break;
			case elog::level_warning:
				strcat(pointer, ETK_BASH_COLOR_MAGENTA);
				break;
			case elog::level_info:
				strcat(pointer, ETK_BASH_COLOR_CYAN);
				break;
			case elog::level_debug:
				strcat(pointer, ETK_BASH_COLOR_YELLOW);
				break;
			case elog::level_verbose:
				strcat(pointer, ETK_BASH_COLOR_WHITE);
				break;
			case elog::level_print:
				strcat(pointer, ETK_BASH_COLOR_WHITE);
				break;
		}
		pointer = handle+strlen(handle);
	}
	if(getTime() == true) {
		getDisplayTime(pointer);
		pointer = handle+strlen(handle);
	}
	#ifndef __TARGET_OS__Android
		switch(_level) {
			default:
				strcat(pointer, "[?] ");
				break;
			case elog::level_print:
				strcat(pointer, "[P] ");
				break;
			case elog::level_critical:
				strcat(pointer, "[C] ");
				break;
			case elog::level_error:
				strcat(pointer, "[E] ");
				break;
			case elog::level_warning:
				strcat(pointer, "[W] ");
				break;
			case elog::level_info:
				strcat(pointer, "[I] ");
				break;
			case elog::level_debug:
				strcat(pointer, "[D] ");
				break;
			case elog::level_verbose:
				strcat(pointer, "[V] ");
				break;
		}
		pointer = handle+strlen(handle);
	#endif
	if (getLibName() == true) {
		if (_id >= 0) {
			int32_t len = strlen(handle);
			strcat(pointer, getList()[_id].first.c_str());
			pointer = handle+strlen(handle);
			while (strlen(handle) - len < getNameSizeLog()) {
				*pointer++ = ' ';
				*pointer = '\0';
			}
			*pointer++ = '|';
			*pointer++ = ' ';
			*pointer = '\0';
		}
	}
	#ifdef ELOG_BUILD_ETHREAD
		if(getThreadId() == true) {
			// display thread ID
			uint32_t iddd = ethread::getId();
			sprintf(pointer, "%3d", iddd);
			pointer = handle+strlen(handle);
			*pointer++ = ' ';
			*pointer++ = '|';
			*pointer++ = ' ';
			*pointer = '\0';
		}
		if(getThreadNameEnable() == true) {
			// display thread ID
			std::string name = ethread::getName();
			if (name.size() >= getThreadSizeLog() ) {
				getThreadSizeLog() = name.size() + 1;
			}
			sprintf(pointer, "%s", name.c_str());
			pointer = handle+strlen(handle);
			size_t nbSpaceToAdd = getThreadSizeLog()-name.size();
			for (size_t iii=0; iii<nbSpaceToAdd; ++iii) {
				*pointer++ = ' ';
				*pointer = '\0';
			}
			*pointer++ = '|';
			*pointer++ = ' ';
			*pointer = '\0';
		}
	#endif
	if(getLine() == true) {
		if (_ligne >= 0) {
			sprintf(pointer, "(l=%5d)", _ligne);
			pointer = handle+strlen(handle);
			*pointer++ = ' ';
			*pointer = '\0';
		}
	}
	// TODO :Maybe optimize this one ...
	if(getFunction() == true) {
		int32_t len = strlen(handle);
		char tmpName[1024];
		char *tmpPointer = tmpName;
		if (_funcName != nullptr) {
			// cleen for android :
			char* startPos = strchr((char*)_funcName, ' ');
			char* stopPos = strchr((char*)_funcName, '(');
			if (startPos != nullptr) {
				if (stopPos != nullptr) {
					char* startPos2 = strchr(startPos+1, ' ');
					while (    startPos2 != nullptr
					        && startPos2 < stopPos) {
						startPos = startPos2;
						startPos2 = strchr(startPos+1, ' ');
					}
					if(uint64_t(stopPos) < uint64_t(startPos)) {
						snprintf(tmpPointer, std::min(uint64_t(1024), uint64_t(stopPos)-uint64_t(_funcName)), "%s", _funcName);
					} else {
						snprintf(tmpPointer, std::min(uint64_t(1024), uint64_t(stopPos)-uint64_t(startPos)), "%s", startPos+1);
					}
				} else {
					snprintf(tmpPointer, 1024, "%s", startPos);
				}
			} else {
				if (stopPos != nullptr) {
					snprintf(tmpPointer, std::min(uint64_t(1024), uint64_t(stopPos)-uint64_t(_funcName)+1), "%s", _funcName);
				} else {
					snprintf(tmpPointer, 1024, "%s", _funcName);
				}
			}
			tmpPointer = tmpPointer+strlen(tmpPointer);
		}
		size_t lenFunc = strlen(tmpName);
		if (lenFunc >= getFunctionSizeLog()) {
			getFunctionSizeLog() = lenFunc+1;
		}
		size_t nbSpaceToAdd = getFunctionSizeLog() - lenFunc;
		for (size_t iii=0; iii<nbSpaceToAdd; ++iii) {
			*tmpPointer++ = ' ';
			*tmpPointer = '\0';
		}
		*tmpPointer++ = '|';
		*tmpPointer++ = ' ';
		*tmpPointer = '\0';
		strcat(pointer, tmpName);
		pointer += strlen(tmpName);
	}
	if (strlen(_log) > LENGHT_MAX_LOG - strlen(handle)-20) {
		memcpy(pointer, _log, LENGHT_MAX_LOG - strlen(handle)-21);
		handle[1024-25] = ' ';
		handle[1024-24] = '.';
		handle[1024-23] = '.';
		handle[1024-22] = '.';
		handle[1024-21] = '\0';
	} else {
		strcat(pointer, _log);
	}
	pointer = handle+strlen(handle);
	if(getColor() == true) {
		strcat(pointer, ETK_BASH_COLOR_NORMAL);
	}
	
	g_lock.lock();
	{
		FILE*& file = getLogFile();
		// close file only if needed ...
		if (file != nullptr) {
			*pointer++ = '\n';
			*pointer = '\0';
			fprintf(file, handle);
			switch(_level) {
				default:
					break;
				case elog::level_critical:
				case elog::level_error:
					fflush(file);
					break;
			}
			// if we log in file, we have no need to log otherwise ... just "tail -f log.txt"
			g_lock.unlock();
			return;
		}
	}
	#if defined(__TARGET_OS__Android)
		switch(_level) {
			default:
				__android_log_print(ANDROID_LOG_VERBOSE, "EWOL", "%s", handle);
				break;
			case elog::level_print:
				__android_log_print(ANDROID_LOG_INFO, "EWOL", "%s", handle);
				break;
			case elog::level_critical:
				__android_log_print(ANDROID_LOG_FATAL, "EWOL", "%s", handle);
				break;
			case elog::level_error:
				__android_log_print(ANDROID_LOG_ERROR, "EWOL", "%s", handle);
				break;
			case elog::level_warning:
				__android_log_print(ANDROID_LOG_WARN, "EWOL", "%s", handle);
				break;
			case elog::level_info:
				__android_log_print(ANDROID_LOG_INFO, "EWOL", "%s", handle);
				break;
			case elog::level_debug:
				__android_log_print(ANDROID_LOG_DEBUG, "EWOL", "%s", handle);
				break;
			case elog::level_verbose:
				__android_log_print(ANDROID_LOG_VERBOSE, "EWOL", "%s", handle);
				break;
		}
	#elif defined(__TARGET_OS__IOs)
		iosNSLog(handle);
	#else
		std::cout << handle << std::endl;
	#endif
	g_lock.unlock();
	if (_level == level_critical) {
		std::this_thread::sleep_for(std::chrono::milliseconds(700));
		displayBacktrace(true, 2);
	}
	// Display backtrace to facilitate the error problems
	if (    _level == level_error
	     && getDisplayBackTrace() == true) {
		displayBacktrace(false, 2);
	}
}
Exemple #27
0
 void unlock()
 { m.unlock(); }
 void add_adj(int other) {
     mutex.lock();
     adj.push_back(other);
     mutex.unlock();
 }
Exemple #29
0
namespace BB
{

// Global variable for now
std::mutex boundTightenerMutex;

OBBT::OBBT(double threshold, unsigned int maxIterations)
    : BoundsTightener(threshold, maxIterations),
      doParallelComputing(false),
      doAggressiveBoundsTightening(false)
{
}

bool OBBT::doTightening(ConstraintPtr constraints)
{
    // Get convex relaxation
    ConstraintPtr convexConstraints = constraints->getConvexRelaxation();

    assert(convexConstraints != nullptr);
    assert(convexConstraints->isConstraintConvex());

    // Tighten bounds of all complicating variables
    std::vector<VariablePtr> variables;
    for (auto var : constraints->getComplicatingVariables())
    {
        if (assertNear(var->getUpperBound(), var->getLowerBound()))
            continue;

        variables.push_back(var);
    }

    // Check if there are any variables to tighten
    if (variables.size() < 1)
        return true;

    // Tighten bounds
    return tightenBoundsSequential(convexConstraints, variables);
//    bool success = true;

//    if (doParallelComputing)
//    {
//        tightenBoundsParallel(convexConstraints, variables);
//    }
//    else
//    {
//        success = tightenBoundsSequential(convexConstraints, variables);
//    }

//    return success;
}

bool OBBT::tightenBoundsSequential(ConstraintPtr cs, std::vector<VariablePtr> variables)
{
    for (auto var : variables)
    {
        // Tighten bounds of single variable
        if (!tightenVariableBound(cs, var))
            return false;
    }

    return true;
}

void OBBT::tightenBoundsParallel(ConstraintPtr cs, std::vector<VariablePtr> variables)
{
    assert(variables.size() > 0);

    std::vector<std::thread> threads;
    int numThreads = std::thread::hardware_concurrency();
    if (numThreads < 1) numThreads = 1;

    int varsPerThread = variables.size()/numThreads;

//    printVec(variables);
//    cout << "Variables: " << variables.size() << endl;
//    cout << "Threads: " << numThreads << endl;

    for (int i = 0; i < numThreads; i++)
    {
        int start = i*varsPerThread;
        int end = (i+1)*varsPerThread;
        if (i == numThreads - 1) end = variables.size();

        //cout << "Thread " << i << ": " << start << " to " << end << endl;

        std::vector<VariablePtr>::const_iterator varStart = variables.begin() + start;
        std::vector<VariablePtr>::const_iterator varEnd = variables.begin() + end;
        std::vector<VariablePtr> threadVariables(varStart, varEnd);
        //printVec(threadVariables);

        threads.push_back(std::thread(&OBBT::tightenVariableBounds, this, cs, threadVariables));
//        threads.push_back(thread(&BoundTightener::test, this, convexConstraints, z0_aug, ref(threadVariables)));
    }

    for (int i = 0; i < numThreads; i++)
    {
        threads.at(i).join();
    }
}

void OBBT::tightenVariableBounds(ConstraintPtr cs, std::vector<VariablePtr> variables)
{
    for (auto var : variables)
    {
        // Read variable bounds
        boundTightenerMutex.lock();
        tightenVariableBound(cs, var);
        boundTightenerMutex.unlock();
    }
}

bool OBBT::tightenVariableBound(ConstraintPtr cs, VariablePtr variable)
{
    assert(cs->hasVariable(variable));

    auto vars = cs->getVariables();

    // Store and set objective costs to zero
    std::vector<double> costs;

    for (auto var : vars)
    {
        costs.push_back(var->getCost());
    }

    // Set costs for lower bound problem
    for (auto var : vars)
    {
        if (var == variable)
            var->setCost(1); // Min. variable
        else
            var->setCost(0);
    }

    //SolverIpopt solver_min(cs);
    SolverGurobi solver_min(cs);
    SolverResult result_min = solver_min.optimize();

    // Set costs for upper bound problem
    for (auto var : vars)
    {
        if (var == variable)
            var->setCost(-1); // Max. variable
        else
            var->setCost(0);
    }

    //SolverIpopt solver_max(cs);
    SolverGurobi solver_max(cs);
    SolverResult result_max = solver_max.optimize();

    // Reset costs
    int counter = 0;
    for (auto var : vars)
    {
        var->setCost(costs.at(counter));
        counter++;
    }

    // Check for infeasibility
    if (result_min.status == SolverStatus::INFEASIBLE
        || result_max.status == SolverStatus::INFEASIBLE)
        return false;

    // Update lower bound
    if (result_min.status == SolverStatus::OPTIMAL)
    {
        if (!variable->updateLowerBound(result_min.objectiveValue))
        {
            // This should never happen!
            cout << "Min bound" << endl;
            cout << *variable << endl;
            cout << result_min << endl;
            return false;
        }
    }

    // Update upper bound
    if (result_max.status == SolverStatus::OPTIMAL)
    {
        if (!variable->updateUpperBound(-result_max.objectiveValue))
        {
            cout << std::setprecision(10) << -result_max.objectiveValue << endl;
            cout << std::setprecision(10) << variable->getLowerBound() << endl;
            cout << std::setprecision(10) << variable->getLowerBound() + result_max.objectiveValue << endl;
            // This should never happen!
            cout << "Max bound" << endl;
            cout << *variable << endl;
            cout << result_max << endl;
            return false;
        }
    }

    // No update
    return true;
}

//void FBBT::tightenBoundsAggressive(ConstraintPtr cs,
//                                             std::vector<double> z0,
//                                             std::vector<int> variables,
//                                             std::vector<double> &lb,
//                                             std::vector<double> &ub)
//{
//    cout << "Performing aggressive bound tightening..." << endl;

//    // z0 is assumed to be the lower bound solution

//    assert(variables.size() > 0);

//    for (unsigned int i = 0; i < variables.size(); i++)
//    {
//        int var = variables.at(i);

//        double varopt = z0.at(var);
//        double varlb = lb.at(var);
//        double varub = ub.at(var);
//        double dist = varub - varlb;
//        //varopt = (varub + varlb)/2.0;

//        if (dist < 0.1) continue;

//        cout << "varlb: " << varlb << endl;
//        cout << "varlb: " << varub << endl;
//        cout << "var: " << varopt << endl;

//        // Try to update lower bound by introducing a fake upper bound
//        double ratio = 2.0/3.0; // 0 < ratio < 1
//        double fakeub = varlb + ratio*(varopt - varlb);
//        if ((fakeub - varlb)/dist > 0.1)
//        {
//            ConstraintPtr cs_copy(cs->clone());
//            std::vector<double> lb_copy;
//            std::vector<double> ub_copy;
//            cs_copy->getDomainBounds(lb_copy,ub_copy);

//            ub_copy.at(var) = fakeub;

//            cs_copy->setDomainBounds(lb_copy, ub_copy);

//            // Solve for new lower bound on variable
//            DenseMatrix cmin; cmin.setZero(1,z0.size()); cmin(var) = 1;
//            ObjectivePtr minObj(new ObjectiveLinear(cmin));
//            //SolverIpopt ip_min(minObj, cs_copy, z0);
//            SolverGurobi solver_min(minObj, cs_copy, z0);
//            solver_min.initialize();
//            int status_min = solver_min.optimize();

//            if (status_min != 1)
//            {
//                cout << "Able to tighten lower bound by aggressive FBBT!" << endl;
//                cout << "old lb: " << lb.at(var) << endl;
//                cout << "new lb: " << fakeub << endl;
//                cout << "ub: " << ub.at(var) << endl;
//                lb.at(var) = fakeub;
//                varlb = fakeub;
//                dist = varub - varlb;
//            }
//        }

//        // Try to update upper bound by introducing a fake lower bound
//        double fakelb = varub - ratio*(varub - varopt);
//        if ((varub - fakelb)/dist > 0.1)
//        {
//            ConstraintPtr cs_copy(cs->clone());
//            std::vector<double> lb_copy;
//            std::vector<double> ub_copy;
//            cs_copy->getDomainBounds(lb_copy,ub_copy);

//            lb_copy.at(var) = fakelb;

//            cs_copy->setDomainBounds(lb_copy, ub_copy);

//            // Solve for new lower bound on variable
//            DenseMatrix cmin; cmin.setZero(1,z0.size()); cmin(var) = 1;
//            ObjectivePtr minObj(new ObjectiveLinear(cmin));
//            //SolverIpopt ip_min(minObj, cs_copy, z0);
//            SolverGurobi solver_min(minObj, cs_copy, z0);
//            solver_min.initialize();
//            int status_min = solver_min.optimize();

//            if (status_min != 1)
//            {
//                cout << "Able to tighten upper bound by aggressive FBBT!" << endl;
//                ub.at(var) = fakelb;

//            }
//        }
//    }
//}

} // namespace BB
Exemple #30
0
void myPreSyncFun()
{
	if( gEngine->isMaster() )
	{
		// if( mouseLeftButton )
		// {
			//double tmpYPos;
			//get the mouse pos from first window
			sgct::Engine::getMousePos( gEngine->getFocusedWindowIndex(), &mouseXPos[0], &mouseYPos[0] );
			// mouseDx = mouseXPos[0] - mouseXPos[1];
			// mouseDy = mouseYPos[0] - mouseYPos[1];

			mouseDx = mouseXPos[0] - 960/2;
			mouseDy = mouseYPos[0] - 540/2;
		// }
		// else
		// {
		// 	mouseDx = 0.0;
		// 	mouseDy = 0.0;
		// }


		sgct::Engine::setMousePos( gEngine->getFocusedWindowIndex(), 960/2, 540/2);

		static float panRot = 0.0f;
		panRot += (static_cast<float>(mouseDx) * rotationSpeed * static_cast<float>(gEngine->getDt()));
		static float vertRot = 0.0f;
		vertRot += (static_cast<float>(mouseDy) * rotationSpeed * static_cast<float>(gEngine->getDt()));


		glm::mat4 ViewRotateX = glm::rotate(
			glm::mat4(1.0f),
			panRot,
			glm::vec3(0.0f, 1.0f, 0.0f)); //rotation around the y-axis

		glm::mat4 ViewRotateY = glm::rotate(
			glm::mat4(1.0f),
			vertRot,
			glm::vec3(1.0f, 0.0f, 0.0f)); //rotation around the x-axis

		view = glm::inverse(glm::mat3(ViewRotateX)) * glm::vec3(0.0f, 0.0f, 1.0f);


		glm::vec3 right = glm::cross(view, up);

		posMutex.lock();
		if( arrowButtons[FORWARD] )
			pos += (walkingSpeed * static_cast<float>(gEngine->getDt()) * view);
		if( arrowButtons[BACKWARD] )
			pos -= (walkingSpeed * static_cast<float>(gEngine->getDt()) * view);
		if( arrowButtons[LEFT] )
			pos -= (walkingSpeed * static_cast<float>(gEngine->getDt()) * right);
		if( arrowButtons[RIGHT] )
			pos += (walkingSpeed * static_cast<float>(gEngine->getDt()) * right);
		posMutex.unlock();
		/*
			To get a first person camera, the world needs
			to be transformed around the users head.

			This is done by:
			1, Transform the user to coordinate system origin
			2, Apply transformation
			3, Transform the user back to original position

			However, mathwise this process need to be reversed
			due to the matrix multiplication order.
		*/
		// std::cout << "Position: (" << pos.x << ", " << pos.y << ", " << pos.x << ")" << std::endl;
		//3. transform user back to original position
		glm::mat4 result;
		result = glm::translate( glm::mat4(1.0f), sgct::Engine::getUserPtr()->getPos() );
		//2. apply transformation
		result *= (ViewRotateY * ViewRotateX *  glm::translate( glm::mat4(1.0f), pos ));
		//1. transform user to coordinate system origin
		result *= glm::translate( glm::mat4(1.0f), -sgct::Engine::getUserPtr()->getPos() );

		xform.setVal( result );
	}
}