[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