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@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/ros-users
(text/plain)