Yeah I figured that out just yesterday. Anyways I have modified my code to look like linux programming:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184
|
#define PI = 3.14159265358979323846264338327950288419716
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>
#include <cmath>
#include <iostream>
#include <stdio.h>
#include <string.h>
#include <fcntl.h>
#include <errno.h>
#include <termios.h>
#include <unistd.h>
using namespace std;
int fd;
int wr,rd;
int i,j;
char parseChar[1];
char stringIn[50];
// unsigned long bytes_read = 0; //Number of bytes read from port
// unsigned long bytes_written = 0; //Number of bytes written to the port
int numSent;
int bStatus;
struct termios options; //Contains various port settings
// set settings for serial port
system("stty -F /dev/ttyUSB0 115200 cs8 -cstopb -parity -icanon hupcl -crtscts min 1 time 1");
// attempt to open serial port
fd = open("/dev/ttyUSB0",O_RDWR | O_NOCTTY | O_NDELAY);
//check for errors opening port
if (fd == -1 )
{
printf("open_port: Unable to open /dev/ttyUSB0");
}
else // if no error
{
fcntl(fd, F_SETFL,0);
printf("Test Port ttyUSB0 has been successfully opened and %d is the file description\n",fd1);
}
//get current settings of serial ports
tcgetattr(fd1, &options);
tcgetattr(fd2, &options);
tcgetattr(fd3, &options);
//set the read and write speeds
cfsetispeed(&options, B115200);
cfsetospeed(&options, B115200);
//set parity
options.c_cflag &= ~CSIZE;
options.c_cflag |= CS8;
if (bStatus != 0)
{
cout << "byte status error!" << endl;
}
void kinematics (int x, int y, int z)
{
const double r=0.125, R=0.55, ratio=127;
int motorspeed[4], numSent=0;
motorspeed[0] = ((x/sqrt(2))+y+(R*tan((z/ratio)*(2*PI))))/(2*PI*r);
motorspeed[1] = (-(x/sqrt(2))+y+(R*tan((z/ratio)*(2*PI))))/(2*PI*r);
motorspeed[2] = ((x/sqrt(2))+y-(R*tan((z/ratio)*(2*PI))))/(2*PI*r);
motorspeed[3] = (-(x/sqrt(2))+y-(R*tan((z/ratio)*(2*PI))))/(2*PI*r);
fd = open("/dev/Driver1&2",O_RDWR | O_NOCTTY | O_NDELAY);
if (fd == -1 )
{
perror("open_port: Unable to open /dev/Driver1&2");
}
numSent = sprintf(stringIn, "%3d,%3d\n", motorspeed[0], motorspeed[1]);
status[0] = Serial_SendBlock(stringIn, numSent, &numSentS);
fd = open("/dev/Driver3&4",O_RDWR | O_NOCTTY | O_NDELAY);
if (fd == -1 )
{
perror("open_port: Unable to open /dev/Driver3&4");
}
numSent = sprintf(stringIn, "%3d,%3d\n", motorspeed[2], motorspeed[3]);
status[0] = Serial_SendBlock(stringIn, numSent, &numSentS);
}
int main(int argc, char **argv)
{
int x, y, z;
// initalize node
ros::init(argc, argv, "joyTalker");
ros::NodeHandle joyTalker;
// Publish to topic joyChatter
ros::Publisher pub = joyTalker.advertise<std_msgs::String>("joyChatter", 1000);
ros::Rate r(10);
while (joyTalker.ok())
{
parseChar[0]=0;
for(j=0;j<50;j++)
{
stringIn[j]=0;
}
i=0;
while(parseChar[0] != 10)
{
if(i==50)
{
break;
}
rd = read(fd,parseChar,1);
//printf("%s ",parseChar);
if ((parseChar[0] > 43) && ((parseChar[0] < 58) || (parseChar[0]==32)))
{
stringIn[i]=parseChar[0];
i++;
}
if (wr != 0)
{
cout << "byte status error!!!" << endl;
}
kinematics(x, y, z);
}
printf("got this string %s\n",stringIn);
// 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;
}
|
These are the errors that come up. I am unsure how to fix them. I thought tcgetattr, cfsetispeed, and cfsetospeed were all predefined in linux. Unless I'm simply missing the library for them.
/home/mars/ros/omnibot/src/joyTalker.cpp:61: error: expected constructor, destructor, or type conversion before ‘(’ token
/home/mars/ros/omnibot/src/joyTalker.cpp:64: error: expected constructor, destructor, or type conversion before ‘=’ token
/home/mars/ros/omnibot/src/joyTalker.cpp:67: error: expected unqualified-id before ‘if’
/home/mars/ros/omnibot/src/joyTalker.cpp:72: error: expected unqualified-id before ‘else’
/home/mars/ros/omnibot/src/joyTalker.cpp:79: error: expected constructor, destructor, or type conversion before ‘(’ token
/home/mars/ros/omnibot/src/joyTalker.cpp:80: error: expected constructor, destructor, or type conversion before ‘(’ token
/home/mars/ros/omnibot/src/joyTalker.cpp:81: error: expected constructor, destructor, or type conversion before ‘(’ token
/home/mars/ros/omnibot/src/joyTalker.cpp:84: error: expected constructor, destructor, or type conversion before ‘(’ token
/home/mars/ros/omnibot/src/joyTalker.cpp:85: error: expected constructor, destructor, or type conversion before ‘(’ token
/home/mars/ros/omnibot/src/joyTalker.cpp:88: error: expected constructor, destructor, or type conversion before ‘.’ token
/home/mars/ros/omnibot/src/joyTalker.cpp:89: error: expected constructor, destructor, or type conversion before ‘.’ token
/home/mars/ros/omnibot/src/joyTalker.cpp:91: error: expected unqualified-id before ‘if’
/home/mars/ros/omnibot/src/joyTalker.cpp: In function ‘void kinematics(int, int, int)’:
/home/mars/ros/omnibot/src/joyTalker.cpp:101: error: expected primary-expression before ‘=’ token
/home/mars/ros/omnibot/src/joyTalker.cpp:101: error: expected primary-expression before ‘=’ token
/home/mars/ros/omnibot/src/joyTalker.cpp:102: error: expected primary-expression before ‘=’ token
/home/mars/ros/omnibot/src/joyTalker.cpp:102: error: expected primary-expression before ‘=’ token
/home/mars/ros/omnibot/src/joyTalker.cpp:103: error: expected primary-expression before ‘=’ token
/home/mars/ros/omnibot/src/joyTalker.cpp:103: error: expected primary-expression before ‘=’ token
/home/mars/ros/omnibot/src/joyTalker.cpp:104: error: expected primary-expression before ‘=’ token
/home/mars/ros/omnibot/src/joyTalker.cpp:104: error: expected primary-expression before ‘=’ token
/home/mars/ros/omnibot/src/joyTalker.cpp:114: error: ‘status’ was not declared in this scope
/home/mars/ros/omnibot/src/joyTalker.cpp:114: error: ‘numSentS’ was not declared in this scope
/home/mars/ros/omnibot/src/joyTalker.cpp:114: error: ‘Serial_SendBlock’ was not declared in this scope