[ros-users] Hybridizing ROS with Blocking TCP/IP Stack

Brian Gerkey gerkey at willowgarage.com
Wed Apr 20 15:22:02 UTC 2011


hi Matt,

I don't know whether you figured this out, but here's a hint: you
don't need to call spin() or spinOnce() if you're only publishing.
Outgoing messages are handled by a separate thread that roscpp
manages.  For example, see the code below, adapted from
roscpp_tutorials/talker.cpp, which blocks on keyboard input between
each publish() call, and never spins.

Now, if you subscribe to a topic, you do need to spin in order to
yield control so that roscpp can fire your subscription callback(s).
But you should first get things working in the publish-only case.

	brian.

#include "ros/ros.h"
#include "std_msgs/String.h"

#include <sstream>

int main(int argc, char **argv)
{
  ros::init(argc, argv, "talker");
  ros::NodeHandle n;
  ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
  ros::Rate loop_rate(10);

  int count = 0;
  while (ros::ok())
  {
    std_msgs::String msg;
    std::stringstream ss;
    ss << "hello world " << count;
    msg.data = ss.str();

    ROS_INFO("%s", msg.data.c_str());

    chatter_pub.publish(msg);

    // Block on keyboard input
    printf("Hit enter\n");
    getchar();

    loop_rate.sleep();
    ++count;
  }


  return 0;
}


On Sat, Mar 5, 2011 at 8:35 PM, Matt Bergsma <mjb4 at sfu.ca> wrote:
> Below is the actual node code as currently implemented, it's currently very
> stripped down to make trouble shooting easier.
>
>
>
> #include <ros/ros.h>
> #include <geometry_msgs/PoseStamped.h>
>
> #include "signal.h"
> #include <stdio.h>
> #include <stdlib.h>
> #include <string.h>
> #include <unistd.h>
> #include <sys/types.h>
> #include <sys/socket.h>
> #include <netinet/in.h>
> #include <string>
> #include <math.h>
>
>
> int portno = 2012, ROSsocket, newROSsocket;
> float xCurrent = 0, yCurrent = 0, thetaCurrent = 0;
> float xGoal, yGoal, thetaGoal, zUnit, wUnit, qVector;
> char xString[8], yString[8], thetaString[8], buffer[51], currentPose[100];
>
> geometry_msgs::PoseStamped setgoal;
> ros::Publisher setgoal_pub;
>
> //templates for functions before main function
> void quit(int sig);
> void error(const char *msg);
> void getgoalbase();
> void setgoalbase(int n);
> void readData();
>
> /***************************************************************************
> **
>  * void quit(int sig)
>  * Quits the program when Ctrl + C is pressed and restores console to
> original state
>  * */
> void quit(int sig)
> {
>    close(ROSsocket);
>    close(newROSsocket);
>    ros::shutdown();
>    exit(0);
> }
>
> /***************************************************************************
> **
>  * void error()
>  * Error function for handling all errors, takes error message as input
>  * */
> void error(const char *msg)
> {
>    perror(msg);
>    exit(1);
> }
>
>
> /***************************************************************************
> **
>  * void getgoalbase()
>  * Subscribes to appopriate topic to determine current position of base
>  * */
> void getgoalbase()
> {
>    //subscribe to topic here and write to current x,y,theta
>    sprintf(currentPose, "Current position(x,y,theta) =
> (%f,%f,%f)",xCurrent, yCurrent, thetaCurrent);
>
> }
>
> /***************************************************************************
> **
>  * void setgoalbase(int n)
>  * Checks if input text is formatted properly
>  * Publishes coordinates if input is formatted properly
>  * */
> void setgoalbase(int n)
> {
>    int count = 0;
>    int i = 12;
>
>    //ascii value for "," = 44
>    //ascii value for "(" = 40
>    //ascii value for ")" = 41
>    //ascii value for SPACE = 32
>
>
>    //store x coordinate into a string
>    for(i=i; i<51; i++)
>    {
>        if(buffer[i] == 32) //skip character it is a SPACE
>            continue;
>
>        if(buffer[i] == 44) //break loop if "," is found
>        {
>            i++;
>            break;
>        }
>
>        if((buffer[i] == 47) || (buffer[i] < 45) || (buffer[i] > 57))
> //generate error if non numeric character is found
>        {
>            printf("Input was not formatted properly, could not
> publish.\n");
>            n = write(newROSsocket,"Input was not formatted properly, could
> not publish. Format must follow \"setgoalbase(x,y,theta)\"",100);
>            return;
>        }
>
>        xString[count] = buffer[i];
>        count++;
>    }
>
>    count = 0;
>
>    //store y coordinate into a string
>    for(i=i; i<51; i++)
>    {
>        if(buffer[i] == 32) //skip character if it is a SPACE
>            continue;
>
>        if(buffer[i] == 44) //break loop is "," is found
>        {
>            i++;
>            break;
>        }
>
>        if((buffer[i] == 47) || (buffer[i] < 45) || (buffer[i] > 57))
> //generate error if non numeric character is found
>        {
>            printf("Input was not formatted properly, could not
> publish.\n");
>            n = write(newROSsocket,"Input was not formatted properly, could
> not publish. Format must follow \"setgoalbase(x,y,theta)\"",100);
>            return;
>        }
>
>        yString[count] = buffer[i];
>        count++;
>    }
>
>    count = 0;
>
>    //store theta into a string
>    for(i=i; i<51; i++)
>    {
>        if(buffer[i] == 32) //skip character it is a SPACE
>            continue;
>
>        if(buffer[i] == 41) //break loop if ")" is found
>            break;
>
>        if((buffer[i] == 47) || (buffer[i] < 45) || (buffer[i] > 57))
> //generate error if non numeric character is found
>        {
>            printf("Input was not formatted properly, could not
> publish.\n");
>            n = write(newROSsocket,"Input was not formatted properly, could
> not publish. Format must follow \"setgoalbase(x,y,theta)\"",100);
>            return;
>        }
>
>        thetaString[count] = buffer[i];
>        count++;
>    }
>
>    //convert strings to doubles
>    xGoal = atof(xString);
>    yGoal = atof(yString);
>    thetaGoal = atof(thetaString);
>
>
>    printf("x goal set to: %f\n",xGoal);
>    printf("y goal set to: %f\n",yGoal);
>    printf("theta goal set to: %f\n",thetaGoal);
>    sprintf(currentPose, "Goal set successfully to (%f,%f,%f)",xGoal, yGoal,
> thetaGoal);
>
>
>    setgoal.header.stamp = ros::Time::now();
>    setgoal.pose.position.x = xGoal;
>    setgoal.pose.position.y = yGoal;
>    setgoal.pose.position.z = 0;
>
>    printf("yo\n");
>    //z^2 + w^2 = 1
>    thetaGoal = thetaGoal*0.0174532925199433; //convert from deg to rad
>    qVector = sqrt(1 + pow(thetaGoal,2)); //calculate unit vector, x and y
> equal 0
>
>    //normalize vectors
>    zUnit = 1/qVector;
>    wUnit = (thetaGoal/qVector);
>
>    setgoal.pose.orientation.x = 0;
>    setgoal.pose.orientation.y = 0;
>    setgoal.pose.orientation.w = wUnit;
>    setgoal.pose.orientation.z = zUnit;
>
>    printf("what\n");
>
>    setgoal_pub.publish(setgoal);
>
>    printf("up\n");
>
> }
>
> /***************************************************************************
> **
>  * void readData()
>  * Reads input from data buffer and checks if any of the above commands are
> found
>  * */
> void readData()
> {
>    char * pch = 0;
>    int n;
>    char *sendbuf;
>
>    //create a buffer and read the input
>    bzero(buffer,51);
>    n = read(newROSsocket,buffer,50);
>
>    if (n < 0)
>        error("ERROR reading from socket");
>
>    //return the pointer to the first occurence of "getgoalbase" in buffer
>    //if command is not found, value of 0 is returned
>    pch = strstr(buffer,"getgoalbase");
>
>    if (pch != 0)
>    {
>        printf("\nMessage recieved: %s\n",buffer);
>        getgoalbase();
>        printf("Current x position: %f\n",xCurrent);
>        printf("Current y position: %f\n",yCurrent);
>        printf("Current theta position: %f\n",thetaCurrent);
>
>        sendbuf = currentPose;
>        n = send(newROSsocket, sendbuf, (int)strlen(sendbuf), 0 );
>
>        if (n < 0)
>            error("ERROR writing to socket");
>
>        return;
>    }
>
>    //return the pointer to the first occurence of "setgoalbase" in buffer
>    //if command is not found, value of 0 is returned
>    pch = strstr(buffer,"setgoalbase(");
>
>    if (pch != 0)
>    {
>        printf("\nMessage recieved: %s\n",buffer);
>        setgoalbase(n);
>        printf("dawg\n");
>        sendbuf = currentPose;
>        printf("keep\n");
>        n = send(newROSsocket, sendbuf, (int)strlen(sendbuf), 0 );
>        printf("it\n");
>
>        if (n < 0)
>            error("ERROR writing to socket");
>
>        printf("real\n");
>
>        return;
>    }
>
>
>    if (pch == 0)
>    {
>        printf("\nMessage recieved: %s\n",buffer);
>        n = write(newROSsocket,"No command found.\n",17);
>
>        if (n < 0)
>            error("ERROR writing to socket");
>    }
>
>    printf("We spun motherfucker\n");
> }
>
> int main(int argc, char** argv)
> {
>    int pid;
>    socklen_t clilen;
>    struct sockaddr_in serv_addr, cli_addr;
>    char szHostName[255];
>
>
>    // Standard ros initialization.
>    ros::init(argc, argv, "canal");
>    ros::NodeHandle node; // Global node handle
>    setgoal_pub =
> node.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal", 50);
>
>    ros::spinOnce();
>
>    signal(SIGINT,quit);   // Quits program if ctrl + c is pressed
>    signal(SIGCHLD,SIG_IGN);
>
>    //create socket
>    ROSsocket = socket(AF_INET, SOCK_STREAM, 0);
>
>    //set all points in the buffer to zero
>    bzero((char *) &serv_addr, sizeof(serv_addr));
>
>    //standard server structure initialization
>    serv_addr.sin_family = AF_INET;
>    serv_addr.sin_port = htons(portno);
>    serv_addr.sin_addr.s_addr = INADDR_ANY;
>
>    //bind socket to address, error if unsuccessful
>    if (bind(ROSsocket, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) <
> 0)
>        error("ERROR on binding");
>
>    //listen on the socket for connections
>    listen(ROSsocket,5);
>    clilen = sizeof(cli_addr);
>
>    //get host name
>    gethostname(szHostName, 255);
>
>    printf("\nWaiting for client on %s, port %u\n", szHostName, portno);
>
>    while (1) //infinite while loop
>    {
>        printf("123 dance with me \n");
>
>        newROSsocket = accept(ROSsocket, (struct sockaddr *) &cli_addr,
> &clilen);
>        printf("After rossocket command\n");
>        if (newROSsocket < 0)
>            error("ERROR on accept");
>
>         pid = fork();
>
>        if (pid < 0)
>            error("ERROR on fork");
>
>        if (pid == 0)
>        {
>            close(ROSsocket);
>            readData();
>            ros::spinOnce();
>            exit(0);
>            printf("again\n");
>        }
>        else close(newROSsocket);
>    }
>
>    close(ROSsocket);
>    return 0; /* we never get here */
>
> }
>
>
>
>
> Is it possible for you to post an example (or the actual ROS node code) for
> us to see how you are doing it, that might help us understand the issue,
> also you might consider posting this on http://answers.ros.org/.
>
> Thanks,
>
> ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
> William Woodall
> Graduate Software Engineering
> Auburn University
> w at auburn.edu
> wjwwood at gmail.com
> ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
>
> On Fri, Mar 4, 2011 at 9:14 PM, Matt Bergsma <mjb4 at sfu.ca> wrote:
> Perhaps a bit of a weird question for you all. I have a pre-existing large
> windows code base that communicates via TCP/IP sockets. And looking to bring
> it into ros by writing a a ROS node that accepts the TCP/IP socket message
> and converts it to the appropriate ROS topic. The problem I'm currently
> running into is that ros::spinOnce doesn't seem to like to behave and
> publish messages as requested. Currently We have a simple client TCP/IP that
> accepts the message, does some stuff to it (which works) than publishes to
> the move_base_simple/goal topic, well in theory. The publishing isn't
> happening. Since we are using accept() to get a message from the socket
> (which is a a blocking call),  the translator node, announces to the ros
> network where its talking, spins once, than hits the blocking call to wait
> for a message, than handles it, and publishes to the appropriate topic.
> After the publishing command, it runs ros::spinOnce() again, then returns to
> waiting for a socket message. Using printf messages I can see the
> appropriate messages showing its getting stuck at the blocking call as
> expected, but no messages are being sent on the ros topic. Any idea's of a
> work around for this? Current thought is to spawn a thread for the TCP/IP
> stack, and then using some combination of flag, and semaphore to control the
> ROS publisher side of things. This code will need to be expanded to include
> a couple ROS subscribers in the future as well.
>
> Matt
>
> _______________________________________________
> ros-users mailing list
> ros-users at code.ros.org
> https://code.ros.org/mailman/listinfo/ros-users
>
>
> _______________________________________________
> ros-users mailing list
> ros-users at code.ros.org
> https://code.ros.org/mailman/listinfo/ros-users
>



More information about the ros-users mailing list