コード例 #1
0
ファイル: main.cpp プロジェクト: penghou620/SocketPiping
int main()
{
	//std::string buf ("Hello");
	char const* addr = "127.0.0.1";
	SocketPipe mySocket(addr,4500);
	mySocket.createServer();
	mySocket.receive();
	return 0;
}
コード例 #2
0
ファイル: serverTcp.c プロジェクト: VincentYQ/learnGit
int main(int argc,char *argv[])
{
    int myfd;
    int myport;
    int buffLen = 0;
    char * readBuff = NULL;
    struct sockaddr_in myaddr;

    configuration *conf = config_new();
    config_init(conf);//相当于config_load("conf.ini");

    if (argc < 2 || (myport = atoi(argv[1])) < 1) {
        if (!config_get_int(conf, "http", "listen", &myport)) {
            myport = 80;
        }
    }
//	printf("port :%d\n",myport);

    if((myfd = socket(AF_INET, SOCK_STREAM, 0)) == -1)
    {
        perror("server socket() error!");
        exit(1);
    }

    myaddr.sin_family      = AF_INET;
    myaddr.sin_port        = htons(myport);
    myaddr.sin_addr.s_addr = inet_addr("0.0.0.0");

    if(bind(myfd, (struct sockaddr *)&myaddr, sizeof(struct sockaddr)) == -1)
    {
        perror("server bind() error!");
        exit(1);
    }

    if(listen(myfd, 10) == -1)
    {
        perror("server listen() error!");
        exit(1);
    }

    while(1)
    {
        mySocket(myfd,&readBuff,&buffLen);
        printf("\nreadBuff:\n%s\n buffLen:\n%d\n",readBuff,buffLen);
    }

    free(readBuff);
    close(myfd);
    return 0;
}
コード例 #3
0
int main(int argC,char **argV)
{

	ros::init(argC,argV,"startDepth");
	ros::NodeHandle n(cameraName);
	image_transport::ImageTransport imT(n);
	std::string serverAddress;
	n.getParam("/serverNameOrIP",serverAddress);

	Socket mySocket(serverAddress.c_str(),"9001",streamSize);


	image_transport::Publisher imagePublisher = imT.advertise(imageTopicSubName,1);
	ros::Publisher cameraInfoPub = n.advertise<sensor_msgs::CameraInfo>(cameraInfoSubName,1);
	camera_info_manager::CameraInfoManager camInfoMgr(n,cameraName);
	camInfoMgr.loadCameraInfo("");
	cv::Mat frame;
	cv_bridge::CvImage cvImage;
	sensor_msgs::Image rosImage;

	while(ros::ok())
	{
		mySocket.readData();
		frame = cv::Mat(cv::Size(512,424),CV_16UC1,mySocket.mBuffer);
		cv::flip(frame,frame,1);

		double utcTime;
		memcpy(&utcTime,&mySocket.mBuffer[imageSize],sizeof(double));
		cvImage.header.stamp = ros::Time(utcTime);
		cvImage.header.frame_id =  ros::this_node::getNamespace() + "/depthFrame";
		cvImage.encoding = "mono16";
		cvImage.image = frame;
		cvImage.toImageMsg(rosImage);

		sensor_msgs::CameraInfo camInfo = camInfoMgr.getCameraInfo();
		std::cout << "Hello23 " << std::endl;
		camInfo.header.stamp = cvImage.header.stamp;
		camInfo.header.frame_id = cvImage.header.frame_id;
		cameraInfoPub.publish(camInfo);
		imagePublisher.publish(rosImage);
		ros::spinOnce();
	}

	return 0;
}
コード例 #4
0
ファイル: startRGB.cpp プロジェクト: psigen/k2_client
int main(int argC,char **argV)
{
	ros::init(argC,argV,"startRGB");
	ros::NodeHandle n(cameraName);
	image_transport::ImageTransport imT(n);
	std::string serverAddress;
	n.getParam("/serverNameOrIP",serverAddress);
    n.getParam(ros::this_node::getNamespace().substr(1,std::string::npos) +
            "/rgb_frame", cameraFrame);
	Socket mySocket(serverAddress.c_str(),"9000",streamSize);
    image_transport::CameraPublisher cameraPublisher = imT.advertiseCamera(
            imageTopicSubName, 1);
	camera_info_manager::CameraInfoManager camInfoMgr(n,cameraName);
	camInfoMgr.loadCameraInfo("");
	cv::Mat frame;
	cv_bridge::CvImage cvImage;
	sensor_msgs::Image rosImage;
	while(ros::ok())
	{
        printf("Got a frame.\n");

		mySocket.readData();
        printf("Creating mat.\n");
        frame = cv::Mat(cv::Size(1920, 1080), CV_8UC4, mySocket.mBuffer);
        cv::cvtColor(frame, frame, CV_BGRA2BGR);
		cv::flip(frame,frame,1);
        printf("Getting time.\n");
		double utcTime;
		memcpy(&utcTime,&mySocket.mBuffer[imageSize],sizeof(double));
        cvImage.header.frame_id = cameraFrame.c_str();
        printf("%s\n", cameraFrame.c_str());
		cvImage.encoding = "bgr8";
		cvImage.image = frame;
		cvImage.toImageMsg(rosImage);
		sensor_msgs::CameraInfo camInfo = camInfoMgr.getCameraInfo();
		camInfo.header.frame_id = cvImage.header.frame_id;
        printf("Updating.\n");
        cameraPublisher.publish(rosImage, camInfo, ros::Time(utcTime));
		ros::spinOnce();
	}
	return 0;
}
コード例 #5
0
ファイル: serveur1-ChouChinois.c プロジェクト: Wait4Code/PRS
int main(int argc, char * argv[]){

	//first, check the number of argument
    if (argc != 2){
        printf("Usage: serveur.exe <port number>\n");
        exit(1);
    }

    //trap SIGINT
    signal(SIGINT,SIGINT_handler);

    //variable definition
    int sock_Data, addr_len,pid, seq, cwnd,normal_wnd, advanced_wnd;
    int test =-1;
    int SEG_SIZE,size,inc,descriptorToCheck[2];
    int reuse = 1;
    int ctrlBuff_SIZE = 12;
    int isSYN=0;
    int seq_advanced, seq_expected,checkBreak=0;

    char SYN_ACK[12];
    char FIN[4]="FIN";
    char ctrlBuffer[ctrlBuff_SIZE];
    char * dataBuffer;

    FILE * file;
    DATA DATA = NULL;
    struct timeval ZERO = {0, 0};


    // struct sockaddr_in
    SOCKADDR_IN * my_addr_UDP;//Adress for listening UDP
    SOCKADDR_IN * my_client_addr;//Adress client, for control
    SOCKADDR_IN * data_addr;//Adress client for Data

    int port_nb = atoi(argv[1]);
    int sock_Control= mySocket(AF_INET,SOCK_DGRAM,0); //socketUDP

	//display the descriptor value and pid of server
	printf("Id Socket: %i\n",sock_Control);
	printf("PID father: %i\n",getpid());

	//allow to reuse immediately the socket
	setsockopt( sock_Control, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse));

	// structure sockaddr_in initialization
    my_addr_UDP = conf_sock_addr(AF_INET, port_nb, "0.0.0.0");
	my_client_addr = init_sock_addr();
	size = sizeof(*my_client_addr);

	//link the socket to the structure
	myBind(sock_Control,my_addr_UDP,sizeof(*my_addr_UDP));

	descriptorToCheck[0] = sock_Control;

	//init fd set
	fd_set * my_read_set = init_fd_set();

	//loop to be a daemon
	while (isSIGINT==0){

		//set the fd_set
		set_fd_set(my_read_set,descriptorToCheck[0]);
		select(sock_Control+1,my_read_set,NULL,NULL,&ZERO);

		if ( FD_ISSET(sock_Control, my_read_set) ){
			// message reception
			myReceiveFrom(sock_Control, ctrlBuffer, ctrlBuff_SIZE,my_client_addr,&size);
            printf("Reception UDP by %d: %s\n",getpid(),ctrlBuffer);

            // if message of type SYN
            if ( check_SYN(ctrlBuffer) == 1){
                port_nb= (port_nb+1)%8975+1025 ;
                sprintf(SYN_ACK,"SYN-ACK%04d",port_nb);

                //new socket for receiving data
                sock_Data = mySocket(AF_INET,SOCK_DGRAM,0);
                data_addr = conf_sock_addr(AF_INET, port_nb, "0.0.0.0");
                addr_len = sizeof(* data_addr);
                descriptorToCheck[1]=sock_Data;

                //binding sock on addr client
                myBind(sock_Data,data_addr,addr_len);
                printf("Connexion Asked by client, dialoguing on port %d \n", ntohs(data_addr->sin_port));
                printf("Adresse client %s\n",inet_ntoa(my_client_addr->sin_addr));
                data_addr = conf_sock_addr(AF_INET, port_nb, inet_ntoa(my_client_addr->sin_addr));

                // variable for receiving
                SEG_SIZE= getMTU(data_addr)-28; // (MTU - header UDP + header IP)
                SEG_SIZE=(SEG_SIZE> 5000)?5000:SEG_SIZE;
                dataBuffer = (char *)malloc(SEG_SIZE*sizeof(char));


                isSYN=1;
                mySendTo(sock_Control, SYN_ACK,12,my_client_addr);
            }

            if ( check_ACK(ctrlBuffer) == 1 && isSYN ==1){
                printf("Connexion Accept\n");
                isSYN = 0;
                //a little fork
                pid = fork();

                //if error
                if (pid == -1){
                    perror("Fork Error\n");
                    exit(-1);
                }

                /*======================================================================================*/
                /*===================================== child code =====================================*/
                /*======================================================================================*/
				else if (pid == 0){
                    myClose(sock_Control);
                    // reception of file name
                    myReceiveFrom(sock_Data,dataBuffer, SEG_SIZE,data_addr,&addr_len);
                    printf("Client Asking for file %s\n",dataBuffer);
                    //retrieve file name
                    file=myOpen(dataBuffer,"r");

                    seq=1, seq_advanced=1, cwnd =400,normal_wnd=320, advanced_wnd=cwnd-normal_wnd;

                    //while something to send, send and wait for ack
                    do {

                        checkBreak=0;
                        seq_expected=(seq == seq_advanced)?seq+cwnd:seq_advanced+advanced_wnd;
                        printf("seq :%d, seq_expected %d, seq_advanced %d\n",seq, seq_expected, seq_advanced);

                        //construct DATA to send from file
                        if (test == -1) test= readFileToBUFFER(file,&DATA,seq,(seq+cwnd==seq_expected)?cwnd:seq_advanced-seq+advanced_wnd,SEG_SIZE);
                        else readFileToBUFFER(file,&DATA,seq,test-seq,SEG_SIZE);

                        //send dataBuffer to client
                        if(seq+normal_wnd==seq_expected || seq == 1) {
                            sendData(sock_Data, cwnd, DATA, data_addr);
                        }else{
                            printf("fast retransmit, resend sequence presumed lost\n");
                            sendData(sock_Data, normal_wnd, DATA, data_addr);
                            printf("fast retransmit, send selected data\n");
                            sendSelectedData(sock_Data,seq_advanced,advanced_wnd,DATA,data_addr);
                        }

                        //while there is something to read
                        set_fd_set(my_read_set, descriptorToCheck[1]);
                        select(sock_Data + 1, my_read_set, NULL, NULL, &ZERO);
                        while (FD_ISSET(sock_Data,my_read_set) ) {


                            //reception
                            myReceiveFrom(sock_Data, ctrlBuffer, ctrlBuff_SIZE, data_addr, &addr_len);
                            //number of sequences acknowledged, (-1 si duplicate ACK)
                            inc = check_ACK_Seq(seq,cwnd,ctrlBuffer);

                            if (inc == -1) // if duplicate ACK
                            {
                                printf("ACK Duplicate ");
                                sendSelectedData(sock_Data,seq,1,DATA,data_addr);
                                if (checkBreak==0){
                                    usleep(100000);
                                    checkBreak=1;
                                }

                            }
                            else if (inc > 0){// if good ACK
                                checkBreak=0;
                                seq += inc;
                                if (seq==seq_expected)break;
                            }

                            //set the descriptor to check
                            set_fd_set(my_read_set, descriptorToCheck[1]);
                            select(sock_Data + 1, my_read_set, NULL, NULL, &ZERO);
                        }


                        if ( test == -1)
                            seq_advanced=(seq_advanced+advanced_wnd > seq+2000 || seq_advanced < seq+normal_wnd)?seq+normal_wnd:seq_advanced+advanced_wnd;
                        else
                            seq_advanced=(seq_advanced+advanced_wnd > seq + 2000 || seq_advanced+advanced_wnd> test+1)?seq+normal_wnd:seq_advanced+advanced_wnd;


                    } while(seq != test+1 );

                    printf("client dialoguing on port %d\n",ntohs(data_addr->sin_port));
                    for(reuse=0;reuse<1000000;reuse++)
                       mySendTo(sock_Data, FIN, 4, data_addr); //Send FIN to client

                    //to do a clean exit
                    myClose(sock_Data);
                    free(my_addr_UDP);
                    free(my_client_addr);
                    free(dataBuffer);
                    free_Data(DATA);
                    fclose(file);
					printf("\e[1;3%dmEnd of connexion. Son with pid:%d is closing, communicate on %d\e[0m\n", sock_Data % 8, getpid(),port_nb);
                    exit(1);


				}
                /*======================================================================================*/
                /*==================================== end of child ====================================*/
                /*======================================================================================*/
			}
		}
コード例 #6
0
ファイル: startBody.cpp プロジェクト: Somahtr/robotic_surgery
int main(int argC,char **argV)
{
	ros::init(argC,argV,"startBody");
	ros::NodeHandle n;
	std::string serverAddress;
	n.getParam("/serverNameOrIP",serverAddress);
	Socket mySocket(serverAddress.c_str(),"9003",streamSize);
	iconv_t charConverter = iconv_open("UTF-8","UTF-16");
	ros::Publisher bodyPub = n.advertise<k2_client::BodyArray>(topicName,1);
	char jsonCharArray[readSkipSize];
	while(ros::ok())
	{
		mySocket.readData();
		char *jsonCharArrayPtr;
		char *socketCharArrayPtr;
		jsonCharArrayPtr = jsonCharArray;
		socketCharArrayPtr = mySocket.mBuffer;
		iconv(charConverter,&socketCharArrayPtr,&readSkipSize,&jsonCharArrayPtr,&stringSize);
		double utcTime;
		memcpy(&utcTime,&mySocket.mBuffer[readSkipSize],sizeof(double));
		std::string jsonString(jsonCharArray);
		//std::cout<<jsonCharArray<<std::endl<<"***"<<std::endl;
		Json::Value jsonObject;
		Json::Reader jsonReader;
		bool parsingSuccessful = jsonReader.parse(jsonString,jsonObject,false);
		if(!parsingSuccessful)
		{
			std::cout<<"Failure to parse: "<<parsingSuccessful<<std::endl;
			continue;
		}
		k2_client::BodyArray bodyArray;
		try
		{
			for(int i=0;i<6;i++)
			{
				k2_client::Body body;
				body.header.stamp = ros::Time(utcTime);
				body.header.frame_id =  ros::this_node::getNamespace().substr(1,std::string::npos) + "/depthFrame";
				body.leanTrackingState = jsonObject[i]["LeanTrackingState"].asInt();
				body.lean.leanX = jsonObject[i]["Lean"]["X"].asDouble();
				body.lean.leanY = jsonObject[i]["Lean"]["Y"].asDouble();
				body.isTracked = jsonObject[i]["IsTracked"].asBool();
				body.trackingId = jsonObject[i]["TrackingId"].asUInt64();
				body.clippedEdges = jsonObject[i]["ClippedEdges"].asInt();
				body.engaged = jsonObject[i]["Engaged"].asBool();
				body.handRightConfidence = jsonObject[i]["HandRightConfidence"].asInt();
				body.handRightState = jsonObject[i]["HandRightState"].asInt();
				body.handLeftConfidence = jsonObject[i]["HandLeftConfidence"].asInt();
				body.handLeftState = jsonObject[i]["HandLeftState"].asInt();
				body.appearance.wearingGlasses = jsonObject[i]["Appearance"]["WearingGlasses"].asBool();
				body.activities.eyeLeftClosed = jsonObject[i]["Activities"]["EyeLeftClosed"].asBool();
				body.activities.eyeRightClosed = jsonObject[i]["Activities"]["EyeRightClosed"].asBool();
				body.activities.mouthOpen = jsonObject[i]["Activities"]["MouthOpen"].asBool();
				body.activities.mouthMoved = jsonObject[i]["Activities"]["MouthMoved"].asBool();
				body.activities.lookingAway = jsonObject[i]["Activities"]["LookingAway"].asBool();
				body.expressions.neutral = jsonObject[i]["Expressions"]["Neutral"].asBool();
				body.expressions.neutral = jsonObject[i]["Expressions"]["Happy"].asBool();
				for(int j=0;j<25;j++)
				{
					k2_client::JointOrientationAndType JOAT;
					k2_client::JointPositionAndState JPAS;
					std::string fieldName;
					switch (j)
					{
						case 0: fieldName = "SpineBase";break;
						case 1: fieldName = "SpineMid";break;
						case 2: fieldName = "Neck";break;
						case 3: fieldName = "Head";break;
						case 4: fieldName = "ShoulderLeft";break;
						case 5: fieldName = "ElbowLeft";break;
						case 6: fieldName = "WristLeft";break;
						case 7: fieldName = "HandLeft";break;
						case 8: fieldName = "ShoulderRight";break;
						case 9: fieldName = "ElbowRight";break;
						case 10: fieldName = "WristRight";break;
						case 11: fieldName = "HandRight";break;
						case 12: fieldName = "HipLeft";break;
						case 13: fieldName = "KneeLeft";break;
						case 14: fieldName = "AnkleLeft";break;
						case 15: fieldName = "SpineBase";break;
						case 16: fieldName = "HipRight";break;
						case 17: fieldName = "KneeRight";break;
						case 18: fieldName = "AnkleRight";break;
						case 19: fieldName = "FootRight";break;
						case 20: fieldName = "SpineShoulder";break;
						case 21: fieldName = "HandTipLeft";break;
						case 22: fieldName = "ThumbLeft";break;
						case 23: fieldName = "HandTipRight";break;
						case 24: fieldName = "ThumbRight";break;
					}
					
					JOAT.orientation.x = jsonObject[i][fieldName]["Orientation"]["X"].asDouble();
					JOAT.orientation.y = jsonObject[i][fieldName]["Orientation"]["Y"].asDouble();
					JOAT.orientation.z = jsonObject[i][fieldName]["Orientation"]["Z"].asDouble();
					JOAT.orientation.w = jsonObject[i][fieldName]["Orientation"]["W"].asDouble();
					JOAT.jointType = jsonObject[i][fieldName]["JointType"].asInt();

					JPAS.trackingState = jsonObject[i][fieldName]["TrackingState"].asBool();
					JPAS.position.x = jsonObject[i][fieldName]["Position"]["X"].asDouble();
					JPAS.position.y = jsonObject[i][fieldName]["Position"]["Y"].asDouble();
					JPAS.position.z = jsonObject[i][fieldName]["Position"]["Z"].asDouble();
					JPAS.jointType = jsonObject[i][fieldName]["JointType"].asInt();
					
					body.jointOrientations.push_back(JOAT);
					body.jointPositions.push_back(JPAS);
				}
				bodyArray.bodies.push_back(body);
			}
		}
		catch (...)
		{
			std::cout<<"An exception occured"<<std::endl;
			continue;
		}
		bodyPub.publish(bodyArray);
	}
	return 0;
}
コード例 #7
0
ファイル: startBody.cpp プロジェクト: roboticsgroup/k2_client
int main(int argC,char **argV)
{
	ros::init(argC,argV,"startBody");
	ros::NodeHandle n;
	std::string serverAddress;
	n.getParam("/serverNameOrIP",serverAddress);
    Socket mySocket(serverAddress.c_str(),const_cast<char*>("9003"), streamSize);
	ros::Publisher bodyPub = n.advertise<k2_client::BodyArray>(topicName,1);
    char utf16Array[readSkipSize];
    char jsonCharArray[stringSize];
    char *jsonCharArrayPtr;
    char *utf16ArrayPtr;
    ros::Rate r(30);
	while(ros::ok())
	{
		mySocket.readData();
        jsonCharArrayPtr = jsonCharArray;
        utf16ArrayPtr = utf16Array;
        memset(utf16Array, 0, sizeof(utf16Array));
        memcpy(utf16Array, mySocket.mBuffer, sizeof(utf16Array));
        size_t iconv_in = static_cast<size_t>(readSkipSize);
        size_t iconv_out = static_cast<size_t>(stringSize);
        iconv_t charConverter = iconv_open("UTF-8","UTF-16");
        size_t nconv = iconv(charConverter, &utf16ArrayPtr, &iconv_in, &jsonCharArrayPtr, &iconv_out);
        iconv_close(charConverter);
        if (nconv == (size_t) - 1)
        {
            if (errno == EINVAL)
                ROS_ERROR("An incomplete multibyte sequence has been encountered in the input.");
            else if (errno == EILSEQ)
                ROS_ERROR("An invalid multibyte sequence has been encountered in the input.");
            else
                ROS_ERROR("There is not sufficient room at jsonCharArray");
        }
        double utcTime = 0.0;
        memcpy(&utcTime,&mySocket.mBuffer[readSkipSize], sizeof(double));
		std::string jsonString(jsonCharArray);
		Json::Value jsonObject;
		Json::Reader jsonReader;
        bool parsingSuccessful = jsonReader.parse(jsonString,jsonObject,false);
        if(!parsingSuccessful)
        {
            ROS_ERROR("Failure to parse");
            continue;
        }
        k2_client::BodyArray bodyArray;
        try
        {
            for(int i=0;i<6;i++)
            {
                k2_client::Body body;
                body.header.stamp = ros::Time(utcTime);
                body.header.frame_id =  ros::this_node::getNamespace().substr(1,std::string::npos) + "/depthFrame";
                body.leanTrackingState = jsonObject[i]["LeanTrackingState"].asInt();
                body.lean.leanX = jsonObject[i]["Lean"]["X"].asDouble();
                body.lean.leanY = jsonObject[i]["Lean"]["Y"].asDouble();
                body.isTracked = jsonObject[i]["IsTracked"].asBool();
                body.trackingId = jsonObject[i]["TrackingId"].asUInt64();
                body.clippedEdges = jsonObject[i]["ClippedEdges"].asInt();
                body.engaged = jsonObject[i]["Engaged"].asBool();
                body.handRightConfidence = jsonObject[i]["HandRightConfidence"].asInt();
                body.handRightState = jsonObject[i]["HandRightState"].asInt();
                body.handLeftConfidence = jsonObject[i]["HandLeftConfidence"].asInt();
                body.handLeftState = jsonObject[i]["HandLeftState"].asInt();
                body.appearance.wearingGlasses = jsonObject[i]["Appearance"]["WearingGlasses"].asBool();
                body.activities.eyeLeftClosed = jsonObject[i]["Activities"]["EyeLeftClosed"].asBool();
                body.activities.eyeRightClosed = jsonObject[i]["Activities"]["EyeRightClosed"].asBool();
                body.activities.mouthOpen = jsonObject[i]["Activities"]["MouthOpen"].asBool();
                body.activities.mouthMoved = jsonObject[i]["Activities"]["MouthMoved"].asBool();
                body.activities.lookingAway = jsonObject[i]["Activities"]["LookingAway"].asBool();
                body.expressions.neutral = jsonObject[i]["Expressions"]["Neutral"].asBool();
                body.expressions.neutral = jsonObject[i]["Expressions"]["Happy"].asBool();
                for(int j=0;j<25;j++)
                {
                    k2_client::JointOrientationAndType JOAT;
                    k2_client::JointPositionAndState JPAS;
                    std::string fieldName;
                    switch (j)
                    {
                        case 0: fieldName = "SpineBase";break;
                        case 1: fieldName = "SpineMid";break;
                        case 2: fieldName = "Neck";break;
                        case 3: fieldName = "Head";break;
                        case 4: fieldName = "ShoulderLeft";break;
                        case 5: fieldName = "ElbowLeft";break;
                        case 6: fieldName = "WristLeft";break;
                        case 7: fieldName = "HandLeft";break;
                        case 8: fieldName = "ShoulderRight";break;
                        case 9: fieldName = "ElbowRight";break;
                        case 10: fieldName = "WristRight";break;
                        case 11: fieldName = "HandRight";break;
                        case 12: fieldName = "HipLeft";break;
                        case 13: fieldName = "KneeLeft";break;
                        case 14: fieldName = "AnkleLeft";break;
                        case 15: fieldName = "SpineBase";break;
                        case 16: fieldName = "HipRight";break;
                        case 17: fieldName = "KneeRight";break;
                        case 18: fieldName = "AnkleRight";break;
                        case 19: fieldName = "FootRight";break;
                        case 20: fieldName = "SpineShoulder";break;
                        case 21: fieldName = "HandTipLeft";break;
                        case 22: fieldName = "ThumbLeft";break;
                        case 23: fieldName = "HandTipRight";break;
                        case 24: fieldName = "ThumbRight";break;
                    }
					
                    JOAT.orientation.x = jsonObject[i]["JointOrientations"][fieldName]["Orientation"]["X"].asDouble();
                    JOAT.orientation.y = jsonObject[i]["JointOrientations"][fieldName]["Orientation"]["Y"].asDouble();
                    JOAT.orientation.z = jsonObject[i]["JointOrientations"][fieldName]["Orientation"]["Z"].asDouble();
                    JOAT.orientation.w = jsonObject[i]["JointOrientations"][fieldName]["Orientation"]["W"].asDouble();
                    JOAT.jointType = jsonObject[i]["JointOrientations"][fieldName]["JointType"].asInt();

                    JPAS.trackingState = jsonObject[i]["Joints"][fieldName]["TrackingState"].asBool();
                    JPAS.position.x = jsonObject[i]["Joints"][fieldName]["Position"]["X"].asDouble();
                    JPAS.position.y = jsonObject[i]["Joints"][fieldName]["Position"]["Y"].asDouble();
                    JPAS.position.z = jsonObject[i]["Joints"][fieldName]["Position"]["Z"].asDouble();
                    JPAS.jointType = jsonObject[i]["Joints"][fieldName]["JointType"].asInt();
					
                    body.jointOrientations.push_back(JOAT);
                    body.jointPositions.push_back(JPAS);
                }
                bodyArray.bodies.push_back(body);
            }
        }
        catch (...)
        {
            ROS_ERROR("An exception occured");
            continue;
        }
        bodyPub.publish(bodyArray);
        ros::spinOnce();
        r.sleep();
	}
	return 0;
}
コード例 #8
0
static ::LRESULT CALLBACK WindowProc(::HWND hWnd, ::UINT Msg, ::WPARAM wParam, ::LPARAM lParam)
{
    Application *lpApplication = Application::GetInstance();
    switch (Msg)
    {
    case WM_CREATE:
        {
            Renderer *renderer = new Renderer(hWnd);
            renderer->Initialize();
            lpApplication->SetRenderer(renderer);

            int li32SocketVersion = MAKEWORD(1, 1);
            Networking::Socket::Initialize(li32SocketVersion);
        }
        break;

    case WM_DESTROY:
        {
            Networking::Socket::Release();

            Renderer *lpRenderer = lpApplication->GetRenderer();
            if (lpRenderer != 0)
            {
                lpRenderer->Release();
                delete lpRenderer;
                lpApplication->SetRenderer(0);
            }
        }
        break;

    case WM_CLOSE:
        {
            Renderer *lpRenderer = lpApplication->GetRenderer();
            if (0 != lpRenderer)
            {
                lpRenderer->Exit();
            }

            REPORTERROR("Sending quit message");
            ::PostQuitMessage(0);
        }
        return 0;

    case WM_DROPFILES:
        {
            ::HDROP hDrop = reinterpret_cast< ::HDROP>(wParam);
            ::UINT luFilesDropped = ::DragQueryFile(hDrop, 0xFFFFFFFF, 0, 0);
            REPORTERROR1("There were %d files dropped", luFilesDropped);

            if (1 == luFilesDropped)
            {
                ::UINT luBufferSize = ::DragQueryFile(hDrop, 0, 0, 0);
                luBufferSize += 1;
                char *lpBuffer = new char[luBufferSize];

                ::UINT luResult = ::DragQueryFile(hDrop, 0, lpBuffer, luBufferSize);
                if (0 == luResult)
                {
                    ::DWORD luErrorCode = ::GetLastError();
                    REPORTWIN32ERROR("DragQueryFile failed", luErrorCode);
                }

                struct addrinfo hints;
                ::ZeroMemory(&hints, sizeof(hints));

                hints.ai_family = AF_UNSPEC;
                hints.ai_socktype = SOCK_STREAM;
                hints.ai_protocol = IPPROTO_TCP;

#if 1
                Networking::Socket mySocket(AF_INET, SOCK_STREAM, IPPROTO_TCP);

                struct hostent *host = ::gethostbyname("localhost");

                ::SOCKADDR_IN SockAddr;
                SockAddr.sin_port = ::htons(8888);
                SockAddr.sin_family = AF_INET;
                SockAddr.sin_addr.s_addr = *((unsigned long *)host->h_addr);

                bool lbResult = mySocket.Connect(&SockAddr, sizeof(SockAddr));
#else
                // Resolve the server address and port
                struct addrinfo *result;
                int iResult = ::getaddrinfo("127.0.0.1", DEFAULT_PORT, &hints, &result);
                if ( iResult != 0 )
                {
                    REPORTWIN32MODULEERROR(::GetModuleHandle("WS2_32.dll"), "Failed to getaddrinfo, error %d, '%s'", iResult);
                }

                //::hostent *lpLocalHost = ::gethostbyname("localhost");

                Networking::Socket mySocket(result->ai_family, result->ai_socktype, result->ai_protocol);
                bool lbResult = mySocket.Connect(result->ai_addr, result->ai_addrlen);

                ::freeaddrinfo(result);
#endif

                if (lbResult)
                {
                    lbResult = mySocket.Send(lpBuffer, luBufferSize);
                }
                if (lbResult)
                {
                    TextureHeader textureHeader;
                    int liSize;
                    lbResult = mySocket.Receive(&textureHeader, liSize);
                    if (lbResult)
                    {
                        REPORTERROR3("Texture %dx%d of size %d", textureHeader.mu32Width, textureHeader.mu32Height, textureHeader.mu32TotalTextureDataSize);

                        unsigned char *lpTextureData = new unsigned char[textureHeader.mu32TotalTextureDataSize];
                        lbResult = mySocket.Receive(lpTextureData, liSize);

                        textureHeader.mpData = lpTextureData;

                        Renderer *lpRenderer = lpApplication->GetRenderer();
                        if (0 != lpRenderer)
                        {
                            lpRenderer->UploadTexture(textureHeader);
                        }

                        delete [] lpTextureData;
                    }
                }

                delete [] lpBuffer;
            }
        }
        break;
    }

    return ::DefWindowProc(hWnd, Msg, wParam, lParam);
}