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); }
void lock() { m.lock(); }
std::mutex::native_handle_type primitive() { return m.native_handle(); }
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; }
void gl::globalRelease(bool finish) { if (finish) { glFinish(); } _globalOpenGLLock.unlock(); }
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(); }
// 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; } }
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(); }
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(); }
void gl::globalLock() { _globalOpenGLLock.lock(); }
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
void func(int i) { mutex.lock(); std::cout << "func clled, i = " << i << " /" << std::this_thread::get_id() << std::endl; mutex.unlock(); }
// 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(); }
void exit_signal(int param){ mtx.lock(); finish = 1; std::cout << "Exiting on next frame completion.." << std::endl; mtx.unlock(); }
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(); //###################################################################################### }
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; }
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); } }
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); } }
void unlock() { m.unlock(); }
void add_adj(int other) { mutex.lock(); adj.push_back(other); mutex.unlock(); }
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
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 ); } }