[ros-users] ROS node publishing not responsive in C++ code

[ibwood]

Hi everyone,

I have some C++ code that compiles without any errors. The code is designed
to communicate with the usb ports and send values to motor drivers for
operation with a joystick. I'm having trouble deciphering why it is not
outputting "e" and continuing on to initialize and publish to the joyChatter
node. Please tell me if I have any syntax incorrect. Here is the code:

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

 #include <sstream>
 #include <cmath>
 #include <iostream>

 #include <sys/ioctl.h>
 #include <sys/types.h>
 #include <sys/stat.h>
 #include <stdio.h>
 #include <limits.h>
 #include <string.h>
 #include <fcntl.h>
 #include <errno.h>
 #include <termios.h>
 #include <unistd.h>

 #define PI 3.14159265358979323846264338327950288419716

 using namespace std;

 const double CIRC=0.7853981633974483, R=0.55, ratio=127;	// initialize
constants

 int main(int argc, char **argv)
 {
	int ms1, ms2, ms3, ms4;			//motor speeds of omnimaxbot	
	int i, j, wr, rd, x=0, y=0, z=0, fd, fd1, fd2, numSent=0;
 	char parseChar[1], stringIn[50], mc_char;

 	// unsigned long bytes_read	= 0;		//Number of bytes read from port
 	// unsigned long bytes_written	= 0;		//Number of bytes written to the port




 	// attempt to open serial port connected to the microprocessor

 	fd = open("/dev/HCS12",O_RDWR | O_NOCTTY | O_NDELAY);
 

 	system("stty -F /dev/HCS12 115200 cs8 -cstopb -parity -icanon hupcl
-crtscts min 1 time 1");		// set settings for HCS12 microcontroller


 	// check for errors opening port	

 	if (fd == -1 )

 	{

		printf("open_port: Unable to open /dev/HCS12\n");

 	}

 
 	else // if no error

 	{

 		fcntl(fd,F_SETFL,0);

		printf("Test Port HCS12 has been successfully opened and %d is the file
description\n",fd);

 	}

	//open serial port connected to motor driver for motors 1 and 2
	fd1 = open("/dev/Driver12",O_RDWR | O_NOCTTY | O_NDELAY);
	
	system("stty -F /dev/Driver12 9600 cs8 -cstopb -parity -icanon hupcl
-crtscts min 1 time 1");		//settings for motor drivers	
	
	if (fd1 == -1 )

	{

		perror("open_port: Unable to open /dev/Driver12\n");

	}		

	else // if no error

 	{

 		fcntl(fd1,F_SETFL,0);

		printf("Test Port Driver12 has been successfully opened and %d is the file
description\n",fd1);

 	}
	
	//open serial port connected to motor driver for motors 3 and 4
	fd2 = open("/dev/Driver34",O_RDWR | O_NOCTTY | O_NDELAY);
	
	system("stty -F /dev/Driver34 9600 cs8 -cstopb -parity -icanon hupcl
-crtscts min 1 time 1");		//settings for motor drivers

	if (fd2 == -1 )

	{

		perror("open_port: Unable to open /dev/Driver34\n");

	}

	else // if no error

 	{

 		fcntl(fd2,F_SETFL,0);

		printf("Test Port Driver34 has been successfully opened and %d is the file
description\n",fd2);

 	}

printf("e");
	// initalize node
	ros::init(argc, argv, "joyTalker");
	ros::NodeHandle joyTalker;
printf("f");
	// Publish to topic joyChatter
	ros::Publisher pub = joyTalker.advertise<std_msgs::String>("joyChatter",
1000);
printf("0");
	ros::Rate r(10);
printf("1");
	while (joyTalker.ok())
	{
		parseChar[0]=0;	  

		for(j=0; j<50; j++)
		{
			stringIn[j]=0;	//initialize the string
		}

		i=0;
printf("2");
		while(parseChar[0] != 10)
		{
			if(i==50)	// exceeds the allowable size
			{
				break;
			}

			rd = read(fd,parseChar,1);
			// printf("%s ",parseChar);
			if ((parseChar[0] > 43) && ((parseChar[0] < 58) || (parseChar[0]==32)))
                        {
				stringIn[i]=parseChar[0];
				i++;
			}				
			
			// kinematics of the omnimaxbot
			ms1 = ((x/sqrt(2))+y+(R*tan((z/ratio)*(2*PI))))/CIRC;
			ms2 = (-(x/sqrt(2))+y+(R*tan((z/ratio)*(2*PI))))/CIRC;
			ms3 = ((x/sqrt(2))+y-(R*tan((z/ratio)*(2*PI))))/CIRC;
			ms4 = (-(x/sqrt(2))+y-(R*tan((z/ratio)*(2*PI))))/CIRC;

			if(ms1<0)			//Motor Controller 1, Port A, Motor 1
			{
				ms1=-ms1;
				mc_char = 'a';			
			}
			else
			{
				mc_char = 'A';
			}
			//write data to the specified port
			numSent = sprintf(stringIn, "!%c%d", mc_char, ms1);	
			wr = write(fd1,stringIn,numSent);

			if(ms2<0)			//Motor Controller 1, Port B, Motor 2
			{
				ms2=-ms2;
				mc_char = 'a';			
			}
			else
			{
				mc_char = 'A';
			}
			
			numSent = sprintf(stringIn, "!%c%d", mc_char, ms2);
			wr = write(fd1,stringIn,numSent);

			if(ms3<0)			//Motor Controller 2, Port A, Motor 3
			{
				ms3=-ms3;
				mc_char = 'b';			
			}
			else
			{
				mc_char = 'B';
			}
			numSent = sprintf(stringIn, "!%c%d", mc_char, ms3);
			wr = write(fd2,stringIn,numSent);

			if(ms4<0)			//Motor Controller 2, Port B, Motor 4
			{
				ms4=-ms4;
				mc_char = 'b';			
			}
			else
			{
				mc_char = 'B';
			}			
			numSent = sprintf(stringIn, "!%c%d", mc_char, ms4);
			wr = write(fd2,stringIn,numSent);			       
		}
		printf("got this string %s\n",stringIn);
printf("8");
		// convert message to string for ROS node communication
		std_msgs::String msg;
		std::stringstream ss;
		ss << stringIn;
		ROS_INFO("%s", ss.str().c_str());
		msg.data = ss.str();
		// publish data under topic joyChatter
		pub.publish(msg);

		ros::spinOnce();
	}
 	
 return 0;
 }

-- 
View this message in context: http://ros-users.122217.n3.nabble.com/ROS-node-publishing-not-responsive-in-C-code-tp1091286p1091286.html
Sent from the ROS-Users mailing list archive at Nabble.com.

------------------------------------------------------------------------------
This SF.net email is sponsored by 

Make an app they can't live without
Enter the BlackBerry Developer Challenge
http://p.sf.net/sfu/RIM-dev2dev 
_______________________________________________
ros-users mailing list
ros-users at lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/ros-users