示例#1
0
文件: main.cpp 项目: smeyel/M2
void init(char *inifilename, int argc, char **argv)
{
	// Init config, allows overrides (including the ini file name)
	configManager.init(inifilename, argc, argv);

	// Init logger (singleton)
	Logger *logger = new StdoutLogger();
	//logger->SetLogLevel(Logger::LOGLEVEL_INFO);
	logger->SetLogLevel(Logger::LOGLEVEL_ERROR);
	Logger::getInstance()->Log(Logger::LOGLEVEL_VERBOSE,"CamClient","CamClient started\n");
    time_t t = time(0);   // get time now
    struct tm * now = localtime( & t );
	Logger::getInstance()->Log(Logger::LOGLEVEL_INFO,"CamClient","Current time: %d-%d-%d, %d:%d:%d\n",
		(now->tm_year + 1900),(now->tm_mon + 1),now->tm_mday,now->tm_hour,now->tm_min,now->tm_sec );
	Logger::getInstance()->Log(Logger::LOGLEVEL_VERBOSE,"CamClient","Ini file: %s\n",inifilename);

	// Init time measurement
	timeMeasurement.init();
	CamClient::TimeMeasurementCodeDefs::setnames(&timeMeasurement);

	// Initialize camera
	if (configManager.usePs3eye)
	{
		videoInput = VideoInputFactory::CreateVideoInput(VIDEOINPUTTYPE_PS3EYE);
	}
	else
	{
		videoInput = VideoInputFactory::CreateVideoInput(VIDEOINPUTTYPE_GENERIC);
	}
	if (configManager.camID>=0)
	{
		videoInput->init(configManager.camID);
	}
	else
	{
		videoInput->init(configManager.camSourceFilename.data());
	}

	// Set auto gain, exposure and white balance
	videoInput->SetNormalizedGain(-1);
	videoInput->SetNormalizedExposure(-1);
	videoInput->SetNormalizedWhiteBalance(-1,-1,-1);

	if (configManager.showImage)
	{
		namedWindow(imageWindowName, CV_WINDOW_AUTOSIZE);
	}

	// Init matrices
	frameCaptured = new Mat(480,640,CV_8UC4);	// Warning: this assumes the resolution and 4 channel color depth

	// Initialize socket communication and server socket
	server.InitServer(configManager.serverPort);
}
RRT::RRT(const State &_state, const double _time): time(_time)
{
    /// Tree initialization
    nodes.reserve(RRT_MAX_ITER);
    states.reserve(RRT_MAX_ITER);
    states.push_back(_state);

    const int dimension = 2;
    int *topology = new int[dimension];
    MPNN::ANNcoord *scale = new MPNN::ANNcoord[dimension];
    for(int i=0;i<dimension;i++) {
        scale[i] = 1.0;
        topology[i] = 1;
    }

    kdTree = new MPNN::MultiANN<int>(dimension,1,topology,(MPNN::ANNpoint)scale);
    generate_input_seed();
    TimeMeasurement tm;
    tm.start();
    construct_tree();
    find_trajectory();
    tm.end();
}
示例#3
0
文件: main.cpp 项目: smeyel/M2
int main(int argc, char *argv[])
{
	init(inifilename,argc,argv);

	if (server.RegisterNode("avalon.aut.bme.hu","~kristof/smeyel/smeyel_reg.php?IP=127.0.0.1:6000"))	// TODO: do not hardwire the port!
	{
		cout << "Error: could not register the node..." << endl;
	}

	server.ListenServerSocket();

	// Enter main loop: wait for commands and execute them.
	while(running)
	{
		// Wait for connection
		Logger::getInstance()->Log(Logger::LOGLEVEL_INFO,"CamClient","Waiting for connection\n");
		cout << "Waiting for connection." << endl;
		struct sockaddr_in addr;
		SOCKET sock = accept(server.GetServerSocket(), (struct sockaddr *) &addr, NULL);
		if (sock < 0 || sock == INVALID_SOCKET)
		{
//			WSAGetLastError
			Logger::getInstance()->Log(Logger::LOGLEVEL_ERROR,"CamClient","Error on accept(), trying to re-initialize server.\n");
			server.DisconnectServer();
			server.InitServer(configManager.serverPort);
			server.ListenServerSocket();
			cout << "Server rebinded, waiting for connection." << endl;
			sock = accept(server.GetServerSocket(), (struct sockaddr *) &addr, NULL);
			if (sock < 0 || sock == INVALID_SOCKET)
			{
				Logger::getInstance()->Log(Logger::LOGLEVEL_ERROR,"CamClient","Error on accept(), 2nd try failed, exiting.\n");
				server.DisconnectServer();
				exit(1);
			}
		}
		// Convert IP address to string
		char ipAddressStr[INET_ADDRSTRLEN];
		inet_ntop( AF_INET, &addr.sin_addr, ipAddressStr, INET_ADDRSTRLEN );
		Logger::getInstance()->Log(Logger::LOGLEVEL_INFO,"CamClient","Connection received from %s\n",ipAddressStr);
		cout << "Connected." << endl;

		// TODO: while socket is not closed by remote size, repeat waiting for commands and execute them...
		bool connectionOpen = true;
		server.SetSock(sock);
		while(connectionOpen)
		{
			// Handle connection

			// TODO: wait for the whole message
			timeMeasurement.start(CamClient::TimeMeasurementCodeDefs::ReceiveCommand);
			JsonMessage *msg = server.ReceiveNew();
			timeMeasurement.finish(CamClient::TimeMeasurementCodeDefs::ReceiveCommand);
			
			if (!msg)
			{
				// The connection was closed from the remote side.
				// (Or a malformed JSON was received...)
				Logger::getInstance()->Log(Logger::LOGLEVEL_ERROR,"CamClient","Error on reading from socket. Closing connection.\n");
				connectionOpen=false;
				break;
			}

			// ---------- Message received, now handle it
			timeMeasurement.start(CamClient::TimeMeasurementCodeDefs::HandleJson);
			handleJSON(msg, &server);
			timeMeasurement.finish(CamClient::TimeMeasurementCodeDefs::HandleJson);
		}

		// Close connection
		server.Disconnect();
		Logger::getInstance()->Log(Logger::LOGLEVEL_INFO,"CamClient","Connection closed\n");
	}

	server.DisconnectServer();

	return 0;
}
void RRT::optimize_trajectory(){
    Trajectory traj;
    double length, new_length;
    double v_max = world.leader->get_limits().vMax*0.8;
    double c_max = world.leader->get_limits().cMax;
    double c_min = world.leader->get_limits().cMin;
    State st = states[0],
            st2;
    for (int i = 0; i < out_trajectory.size(); ++i) {
        traj.push_back(Desc(st,out_trajectory[i]));
        st2 = world.leader->calculate_state(st, out_trajectory[i]);
        st = st2;
    }
    traj.push_back(Desc(st,Control()));

#ifdef DEBUG_RRT
    length = 0;
    for (int i = 0; i < traj.size(); ++i) {
        length += traj[i].control.t * traj[i].control.v;
    }
    TimeMeasurement tm;
    tm.start();
    std::cout << "RRT: original trajectory length: " << length <<  std::endl;
    /*
    std::cout << std::endl << "LEADER PLANNED POSITIONS:" << std::endl;
    for (int i = 0; i < traj.size(); ++i) {
        std::cout << "	X = [ " << traj[i].state.X(0) << ", " << traj[i].state.X(1) << ", " << traj[i].state.X(2) << " ]" << std::endl;
    }
*/
#endif
    int loop = 0;
    bool isValid;
    bool endPoint;
    while(loop < 100){
        ++loop;
        int n_first = rand() % traj.size();
        int n_second = rand() % traj.size();
        while(n_first == n_second){
            n_second = rand() % traj.size();
        }

        if (n_first > n_second){
            const int p = n_first;
            n_first = n_second;
            n_second = p;
        }

        length = 0;
        for (int i = n_first; i < n_second; ++i) {
            length += traj[i].control.t * fabs(traj[i].control.v);
        }
        endPoint = false;
        if (n_second != traj.size()-1){
            dubins = new geom::Dubins(geom::Position(geom::Point(traj[n_first].state.X(0),traj[n_first].state.X(1)), traj[n_first].state.phi),
                                      geom::Position(geom::Point(traj[n_second].state.X(0),traj[n_second].state.X(1)), traj[n_second].state.phi),
                                      1/world.leader->cMax());
        }else{
            endPoint = true;
            dubins = new geom::Dubins(geom::Position(geom::Point(traj[n_first].state.X(0),traj[n_first].state.X(1)), traj[n_first].state.phi),
                                      geom::Point(goal[0],goal[1]),
                    1/world.leader->cMax());
            length += dist2D(traj.back().state.X,Vector(goal[0],goal[1],0));
        }
        new_length = dubins->getLength();

        if (new_length < length){
#ifdef DEBUG_RRT_DUBINS
            if (endPoint){
            std::cout << "DUBINS: original part length   : " << length <<  std::endl;
            std::cout << "        simplified part length : " << new_length <<  std::endl;
            std::cout << "        type of Dubins maneuver: " << dubins->getTypeOfManeuver() << " [RSR, LSL, LSR, RSL, RLR, LRL, LS, RS]" << std::endl;
            }
#endif

            Trajectory new_traj;
            for (int i = 0; i < n_first; ++i) {
                new_traj.push_back(traj[i]);
            }
            double t1, t2, t3, c1, c2, c3;
            State state_first = traj[n_first].state;
            Control cnt;
            switch (dubins->getTypeOfManeuver()) {
            case geom::RSR:
                t1 = dubins->getRadius()*fabs(dubins->getLen1())/v_max;
                t2 = dubins->getLen2()/v_max;
                t3 = dubins->getRadius()*fabs(dubins->getLen3())/v_max;
                c1 = c_min;
                c2 = 0;
                c3 = c_min;
                break;
            case geom::LSL:
                t1 = dubins->getRadius()*fabs(dubins->getLen1())/v_max;
                t2 = dubins->getLen2()/v_max;
                t3 = dubins->getRadius()*fabs(dubins->getLen3())/v_max;
                c1 = c_max;
                c2 = 0;
                c3 = c_max;
                break;
            case geom::LSR:
                t1 = dubins->getRadius()*fabs(dubins->getLen1())/v_max;
                t2 = dubins->getLen2()/v_max;
                t3 = dubins->getRadius()*fabs(dubins->getLen3())/v_max;
                c1 = c_max;
                c2 = 0;
                c3 = c_min;
                break;
            case geom::RSL:
                t1 = dubins->getRadius()*fabs(dubins->getLen1())/v_max;
                t2 = dubins->getLen2()/v_max;
                t3 = dubins->getRadius()*fabs(dubins->getLen3())/v_max;
                c1 = c_min;
                c2 = 0;
                c3 = c_max;
                break;
            case geom::RLR:
                t1 = dubins->getRadius()*fabs(dubins->getLen1())/v_max;
                t2 = dubins->getRadius()*fabs(dubins->getLen2())/v_max;
                t3 = dubins->getRadius()*fabs(dubins->getLen3())/v_max;
                c1 = c_min;
                c2 = c_max;
                c3 = c_min;
                break;
            case geom::LRL:
                t1 = dubins->getRadius()*fabs(dubins->getLen1())/v_max;
                t2 = dubins->getRadius()*fabs(dubins->getLen2())/v_max;
                t3 = dubins->getRadius()*fabs(dubins->getLen3())/v_max;
                c1 = c_max;
                c2 = c_min;
                c3 = c_max;
                break;
            default:
                break;
            }

            State s_new, s_old = state_first;
            int n_added = 0;
            if (t1 > eps){
                while (t1-RRT_TIME_STEP>eps){
                    cnt = Control(v_max,0,c1,RRT_TIME_STEP);
                    new_traj.push_back(Desc(s_old,cnt));
                    t1 -= RRT_TIME_STEP;

                    s_new = world.leader->calculate_state(s_old, cnt);
                    s_old = s_new;
                    ++n_added;
                }
                cnt = Control(v_max,0,c1,t1);
                new_traj.push_back(Desc(s_old,cnt));
                s_new = world.leader->calculate_state(s_old, cnt);
                s_old = s_new;
                ++n_added;
            }

            if (t2 > eps){
                while (t2-RRT_TIME_STEP>eps){
                    cnt = Control(v_max,0,c2,RRT_TIME_STEP);
                    new_traj.push_back(Desc(s_old,cnt));
                    t2 -= RRT_TIME_STEP;

                    s_new = world.leader->calculate_state(s_old, cnt);
                    s_old = s_new;
                    ++n_added;
                }
                cnt = Control(v_max,0,c2,t2);
                new_traj.push_back(Desc(s_old,cnt));
                s_new = world.leader->calculate_state(s_old, cnt);
                s_old = s_new;
                ++n_added;
            }
            if (t3 > eps){
                while (t3-RRT_TIME_STEP>eps){
                    cnt = Control(v_max,0,c3,RRT_TIME_STEP);
                    new_traj.push_back(Desc(s_old,cnt));
                    t3 -= RRT_TIME_STEP;

                    s_new = world.leader->calculate_state(s_old, cnt);
                    s_old = s_new;
                    ++n_added;
                }
                cnt = Control(v_max,0,c3,t3);
                new_traj.push_back(Desc(s_old,cnt));
                ++n_added;
            }

            isValid = true;
            for (int i = 0; i < n_added; ++i) {
                const int index = new_traj.size()-n_added+i;
                Points pts = world.leader->getSampledTrajectory(new_traj[index].state,&new_traj[index].control,1);
                for (int j = 0; j < pts.size(); ++j) {
                    isValid &= world.map->wall_distance(pts[j]) > world.leader->get_ra();
                    if (!isValid)
                        break;
                }
                if (!isValid)
                    break;
            }
            if (!isValid){
                continue;
            }

            loop = 0;
            if (!endPoint){
                for (int i = n_second; i < traj.size(); ++i) {
                    new_traj.push_back(traj[i]);
                }
            }else{
                s_new = world.leader->calculate_state(new_traj.back().state, new_traj.back().control);
                new_traj.push_back(Desc(State(goal[0],goal[1],0,0),Control()));
            }
            traj = new_traj;

        }
    }
    out_trajectory.clear();
    for (int i = 0; i < traj.size()-1; ++i) {
        out_trajectory.push_back(traj[i].control);
    }

#ifdef DEBUG_RRT
    length = 0;
    for (int i = 0; i < traj.size(); ++i) {
        length += traj[i].control.t * traj[i].control.v;
    }
    tm.end();
    std::cout << "RRT: optimized trajectory length: " << length <<  std::endl;
    std::cout << "                            size: " << traj.size()-1 <<  std::endl;

    /*
    std::cout << std::endl;
    for (int i = 0; i < traj.size(); ++i) {
        std::cout << "   v = " << traj[i].control.v << ", w = " << traj[i].control.w << ", c = " << traj[i].control.c << ", t = " << traj[i].control.t << std::endl;
    }
    */
#endif

}