[ros-users] Hybridizing ROS with Blocking TCP/IP Stack
Matt Bergsma
mjb4 at sfu.ca
Sun Mar 6 04:35:13 UTC 2011
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
More information about the ros-users
mailing list