// This resets the flags of streamToReset and sets its string to be
 // newString.
 inline void
 ParsingUtilities::ResetStringstream( std::istringstream& streamToReset,
                                      std::string const& newString )
 {
   streamToReset.clear();
   streamToReset.str( newString );
 }
 inline bool
 AsciiXmlParser::loadString( std::string const stringToParse )
 /* this loads stringToParse into the internal stringstream for parsing. if
   * there was a problem loading the file, false is returned. if there was a
   * file open, it is now closed.
   */
 {
   resetContent();
   stringParsingStream.clear();
   stringParsingStream.str( stringToParse );
   textStream = &stringParsingStream;
   return stringParsingStream.good();
 }
int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam<DOF>& wam) {

	BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);

	// These vectors are fixed sized, stack allocated, and zero-initialized.
	jp_type jp;  // jp is a DOFx1 column vector of joint positions
	cp_type cp;  // cp is a 3x1 vector representing a Cartesian position

	wam.gravityCompensate();
	printMenu();
	Hand* hand = pm.getHand();

	std::string line;
	bool going = true;
	while (going) {
		printf(">>> ");
		std::getline(std::cin, line);

		switch (line[0]) {

		case 'p':
			moveToStr(wam, &cp, "tool position", line.substr(1));
			break;

		case 'j':
					moveToStr(wam, &jp, "joint positions", line.substr(1));
					break;

		case 'h':
			std::cout << "Moving to home position: "
					<< wam.getHomePosition() << std::endl;
			wam.moveHome();
			break;

		case 'i':
			printf("WAM idled.\n");
			wam.idle();
			break;

		case 'g':
			hand->trapezoidalMove(Hand::jp_type(M_PI/3.0), Hand::SPREAD);
			break;


		case 'o':
			hand->initialize();
//			hand->close(Hand::GRASP);
//			hand->open(Hand::GRASP);
//		   	hand->close(Hand::SPREAD);
//		   	hand->trapezoidalMove(Hand::jp_type(M_PI/2.0),1, Hand::GRASP);
//		   	hand->trapezoidalMove(Hand::jp_type(M_PI/2.0), Hand::GRASP);
		   	break;

		case 'a':
//			hand->initialize();
			hand->close(Hand::SPREAD);
//			hand->close(Hand::GRASP);
			break;

		case 'c':
//			hand->initialize();
			hand->close(Hand::GRASP);
			break;

//		case 'f':
//			while (std::getline(file_jp,line_jp))
//			{
//			moveToStr(wam, &jp, "joint positions", line_jp);
//			waitForEnter();
//			}
//			break;

		case 'd':
//					hand->initialize();
					hand->open(Hand::SPREAD);
					break;

		case 'e':
//							hand->initialize();
							hand->open(Hand::GRASP);
							break;

		case 'b':
			hand->trapezoidalMove(Hand::jp_type(M_PI/2.0), Hand::GRASP);
			break;

		case 't':
			hand->trapezoidalMove(Hand::jp_type((M_PI)/6.0), Hand::GRASP);
//			std::cout << "sending back a message..." << std::endl;
			break;

		case 'm':
//			temp_str = line.substr(2);
			iss2.clear();
			iss2.str("");
			iss2.str(line.substr(2)) ;
		    iss2 >> angle_degree;
//			std::cin >> angle_degree;
			angle_radian = angle_degree*(M_PI/180);
			hand->trapezoidalMove(Hand::jp_type(angle_radian), Hand::GRASP);
			std::cout << angle_degree << std::endl;
			break;


		case 'q':
		case 'x':
			printf("Quitting.\n");
			going = false;
			break;

		default:
			if (line.size() != 0) {
				printf("Unrecognized option.\n");
				printMenu();
			}
			break;
		}
	}


	wam.idle();
	pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);
	return 0;
}
Ejemplo n.º 4
0
int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam<DOF>& wam) {

    BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);

    // These vectors are fixed sized, stack allocated, and zero-initialized.
    jp_type jp;  // jp is a DOFx1 column vector of joint positions
    cp_type cp;  // cp is a 3x1 vector representing a Cartesian position

    wam.gravityCompensate();
    printMenu();
    Hand* hand = pm.getHand();


// Write a socket program here. It is a server socket that would close only when the program closes
// This client socket would connect to a server socket on a external machine a recieve data from it
//---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------

    int status;
    struct addrinfo host_info;
    struct addrinfo *host_info_list;
    memset(&host_info, 0, sizeof host_info);
    host_info.ai_family = AF_UNSPEC;
    host_info.ai_socktype = SOCK_STREAM;
    host_info.ai_flags = AI_PASSIVE;
    status = getaddrinfo("192.168.0.110", "5555", &host_info, &host_info_list); //192.168.0.110 this is the wam(server) address
    int socketfd ;
    socketfd = socket(host_info_list->ai_family, host_info_list->ai_socktype, host_info_list->ai_protocol);
    int yes = 1;
    status = setsockopt(socketfd, SOL_SOCKET, SO_REUSEADDR, &yes, sizeof(int));
    status = bind(socketfd, host_info_list->ai_addr, host_info_list->ai_addrlen);
    status =  listen(socketfd, 5);
    int new_sd;
    struct sockaddr_storage their_addr;
    socklen_t addr_size = sizeof(their_addr);
    new_sd = accept(socketfd, (struct sockaddr *)&their_addr, &addr_size);
    std::string line;

    bool going = true;


    while (going) {
        std::cout << "Whiel loop starts" << std::endl;
//-----------------------------------------------------------------------------------
        ssize_t bytes_recieved;
        char incomming_data_buffer[1000];
        bytes_recieved = recv(new_sd, incomming_data_buffer,1000, 0);
        std::cout << bytes_recieved << std::endl;
//    if (bytes_recieved == 0) std::cout << "host shut down." << std::endl ;
//    if (bytes_recieved == -1)std::cout << "recieve error!" << std::endl ;
        incomming_data_buffer[bytes_recieved] = '\0';
        std::cout << incomming_data_buffer << std::endl;

        const char *msg = "Niladri Das I am the Server";
        const char *msg1 = "Press f exactly four times";
        int len1;
        int len;
        ssize_t bytes_sent;
        ssize_t bytes_sent1;
        len = strlen(msg);
        len1 = strlen(msg1);
//-----------------------------------------------------------------------------------
//		printf(">>> ");

//		std::cin.getline(incomming_data_buffer, line);
        line = std::string(incomming_data_buffer);
        std::cout << incomming_data_buffer << std::endl;
        switch (line[0]) {
        case 'j':
            moveToStr(wam, &jp, "joint positions", line.substr(1));
            std::cout << "sending back a message..." << std::endl;
            bytes_sent = send(new_sd, msg, len, 0);
            break;
        case 'f':
            std::getline(file_jp,line_jp);
            {
                moveToStr(wam, &jp, "joint positions", line_jp);

            }
            std::cout << "sending back a message..." << std::endl;
            bytes_sent1 = send(new_sd, msg1, len1, 0);
            break;

        case 'p':
            moveToStr(wam, &cp, "tool position", line.substr(1));
            std::cout << "sending back a message..." << std::endl;
            bytes_sent = send(new_sd, msg, len, 0);
            break;

        case 'M':
            std::cout << "send()ing back a message..."  << std::endl;
            bytes_sent = send(new_sd, msg, len, 0);
            break;

        case 'h':
            std::cout << "Moving to home position: "
                      << wam.getHomePosition() << std::endl;
            wam.moveHome();
            std::cout << "sending back a message" << std::endl;
            bytes_sent = send(new_sd, msg, len,0);
            break;

        case 'i':
            printf("WAM idled.\n");
            wam.idle();
            std::cout << "sending back a message" << std::endl;
            bytes_sent = send(new_sd, msg, len,0);
            break;
        case 'o':
            hand->initialize();
            //			hand->close(Hand::GRASP);
            //			hand->open(Hand::GRASP);
            //		   	hand->close(Hand::SPREAD);
            //		   	hand->trapezoidalMove(Hand::jp_type(M_PI/2.0),1, Hand::GRASP);
            //		   	hand->trapezoidalMove(Hand::jp_type(M_PI/2.0), Hand::GRASP);
            std::cout << "sending back a message..." << std::endl;
            bytes_sent = send(new_sd, msg, len, 0);

            break;

        case 'a':
            //			hand->initialize();
            hand->close(Hand::SPREAD);
            //			hand->close(Hand::GRASP);
            std::cout << "sending back a message..." << std::endl;
            bytes_sent = send(new_sd, msg, len, 0);

            break;

        case 'c':
            //			hand->initialize();
            hand->close(Hand::GRASP);
            std::cout << "sending back a message..." << std::endl;
            bytes_sent = send(new_sd, msg, len, 0);

            break;

        case 'd':
            //					hand->initialize();
            hand->open(Hand::SPREAD);
            std::cout << "sending back a message..." << std::endl;
            bytes_sent = send(new_sd, msg, len, 0);

            break;

        case 'e':
            //							hand->initialize();
            hand->open(Hand::GRASP);
            std::cout << "sending back a message..." << std::endl;
            bytes_sent = send(new_sd, msg, len, 0);

            break;

        case 'b':
            hand->trapezoidalMove(Hand::jp_type(M_PI/2.0), Hand::GRASP);
            std::cout << "sending back a message..." << std::endl;
            bytes_sent = send(new_sd, msg, len, 0);

            break;


        case 't':
            hand->trapezoidalMove(Hand::jp_type((M_PI)/6.0), Hand::GRASP);
            std::cout << "sending back a message..." << std::endl;
            bytes_sent = send(new_sd, msg, len, 0);

            break;

        case 'm':
            //			temp_str = line.substr(2);
            iss2.clear();
            iss2.str("");
            iss2.str(line.substr(2)) ;
            iss2 >> angle_degree;
            //			std::cin >> angle_degree;
            angle_radian = angle_degree*(M_PI/180);
            hand->trapezoidalMove(Hand::jp_type(angle_radian), Hand::GRASP);
            std::cout << angle_degree << std::endl;
            bytes_sent = send(new_sd, msg, len, 0);
            break;


        case 'q':
        case 'x':
            printf("Quitting.\n");
            going = false;
            std::cout << "sending back a message..." << std::endl;
            bytes_sent = send(new_sd, msg, len, 0);

            break;

        default:
            if (line.size() != 0) {
                printf("Unrecognized option.\n");
                std::cout << "sending back a message..." << std::endl;
                bytes_sent = send(new_sd, msg, len, 0);

                printMenu();
            }
            break;
        }
        std::cout << "While loop complete" << std::endl;
    }


    wam.idle();
    pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);

//-----------------------------------------------------------------------------
    freeaddrinfo(host_info_list);
    close(new_sd);
    close(socketfd);

//-----------------------------------------------------------------------------
    return 0;
}