187 lines
4.6 KiB
C++
187 lines
4.6 KiB
C++
#include "ros/ros.h"
|
|
#include "geometry_msgs/Twist.h"
|
|
#include <termios.h>
|
|
#include <fcntl.h>
|
|
|
|
#define LOOP_RATE 20 //Hz
|
|
#define QUEUE_SIZE 64 //subscriber buffer size
|
|
|
|
|
|
// -------------- Serial Port -----------------
|
|
int fd;
|
|
bool isConnected = false;
|
|
//const char *port0 = "/dev/ttyACM0"; //red
|
|
const char *port1 = "/dev/ttyACM1"; //yellow
|
|
|
|
int connectit(const char *dev, int baud);
|
|
static void initTermios(int fd, uint32_t baud);
|
|
struct termios sTermios;
|
|
int writecommand(char speed, char dir, bool mode);
|
|
|
|
|
|
// --------------- Fish ID --------------------
|
|
const unsigned char prefix[3] = { 0x21, 0x43, 0x00};
|
|
//const unsigned char fish_ID[9] = { 0x52, 0x71, 0xa7, 0x29, 0xbc, 0xd6, 0xd9, 0x00, 0xe5}; //red
|
|
const unsigned char fish_ID[9] = { 0x52, 0x71, 0x63, 0x4c, 0xb9, 0xd6, 0xda, 0x00, 0xe5}; //yellow
|
|
|
|
|
|
// global vars
|
|
float lin_vel = 0;
|
|
float ang_vel = 0;
|
|
|
|
|
|
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
|
|
*
|
|
* Twist message Callback
|
|
*
|
|
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
|
|
void velCallback (const geometry_msgs::Twist::ConstPtr& vel_msg) {
|
|
|
|
lin_vel = vel_msg->linear.x; //Get linear velocity from Twist message.
|
|
ang_vel = vel_msg->angular.z; //Get the direction from the Twist message. This is not a speed in reality, but steering action.
|
|
}
|
|
|
|
|
|
|
|
|
|
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
|
|
*
|
|
* initTermios. Serial I/O
|
|
*
|
|
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
|
|
static void initTermios(int fd, uint32_t baud)
|
|
{
|
|
tcgetattr(fd, &sTermios); /*Get the attributes set by default*/
|
|
|
|
/* ---------- Serial Port Config ------- */
|
|
sTermios.c_cflag |=(CLOCAL|CREAD);
|
|
sTermios.c_cflag &= ~(CSTOPB|CSIZE);
|
|
sTermios.c_cflag &= ~(PARENB);
|
|
sTermios.c_cflag |= CS8;
|
|
sTermios.c_lflag &= ~(ICANON|ECHO|ECHOE|ISIG);
|
|
sTermios.c_cc[VMIN] = 0;
|
|
sTermios.c_cc[VTIME] = (unsigned int)(100);
|
|
sTermios.c_cc[VSTART] = 0x13;
|
|
sTermios.c_cc[VSTOP] = 0x19;
|
|
|
|
sTermios.c_iflag &= ~(IXON|IXOFF|IXANY|ICRNL|INLCR);
|
|
sTermios.c_oflag &= ~OPOST;
|
|
cfsetispeed(&sTermios, baud);
|
|
cfsetospeed(&sTermios, baud);
|
|
/* -------- Serial Port Config END---- */
|
|
|
|
tcsetattr(fd, TCSANOW, &sTermios);
|
|
}
|
|
|
|
|
|
|
|
|
|
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
|
|
*
|
|
* connectit. Open serial port
|
|
*
|
|
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
|
|
int connectit(const char *dev, int baud) {
|
|
int fd;
|
|
fd = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK);
|
|
if (fd == -1) { perror("serconnect "); return fd; }
|
|
initTermios(fd, baud);
|
|
return fd;
|
|
}
|
|
|
|
|
|
|
|
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
|
|
*
|
|
* writecommand
|
|
*
|
|
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
|
|
int writecommand(char speed, char dir, bool mode) {
|
|
|
|
char msg[15];
|
|
if(!isConnected) return -1;
|
|
|
|
if(dir > 8) dir = 8;
|
|
if(dir < -7) dir = -7;
|
|
|
|
if(speed > 15) speed = 15;
|
|
if(speed < 0) speed = 0;
|
|
|
|
char direction = dir + 7;
|
|
char modeH = mode ? 0x10 : 0x00;
|
|
char dirMode = direction + modeH;
|
|
char checksum = 0xff - speed - direction - modeH;
|
|
for(int i=0;i<3;i++){
|
|
msg[i] = prefix[i];
|
|
}
|
|
msg[3] = speed;
|
|
msg[4] = dirMode;
|
|
msg[5] = checksum;
|
|
for(int i=0;i<9;i++){
|
|
msg[6+i] = fish_ID[i];
|
|
}
|
|
return write(fd, msg, 15);
|
|
//return 0;
|
|
}
|
|
|
|
|
|
|
|
|
|
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
|
|
*
|
|
* Main
|
|
*
|
|
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
|
|
int main(int argc, char **argv)
|
|
{
|
|
bool mode=false; //purpose?
|
|
float x_move=0;
|
|
float y_move=2;
|
|
int direction=0;
|
|
int speed=0;
|
|
int prevDirection=0;
|
|
int prevSpeed=0;
|
|
|
|
fd = connectit(port1,115200);
|
|
if (fd != -1) {
|
|
isConnected = true;
|
|
ROS_INFO("Serial port ttyACM1 connected...");
|
|
sleep(1);
|
|
tcflush(fd,TCIOFLUSH);
|
|
}
|
|
else {
|
|
ROS_ERROR("Error!: Serial port ttyACM1 not found!");
|
|
}
|
|
|
|
ros::init(argc, argv, "yellow_rofish_telemetry");
|
|
ros::NodeHandle nh;
|
|
ros::Subscriber sub = nh.subscribe("yellow/cmd_vel", QUEUE_SIZE, velCallback);
|
|
ros::Rate loop_rate(LOOP_RATE);
|
|
|
|
while(ros::ok()) {
|
|
ros::spinOnce();
|
|
|
|
//do processing here
|
|
if (ang_vel >= 0) {
|
|
direction = (char) 8*ang_vel;
|
|
}
|
|
if (ang_vel < 0) {
|
|
direction = (char) 7*ang_vel;
|
|
}
|
|
if (lin_vel >= 0){
|
|
speed = (char) 15*lin_vel;
|
|
}
|
|
|
|
writecommand(speed, direction, mode);
|
|
|
|
if( (direction!=prevDirection) || (speed!=prevSpeed) ){
|
|
ROS_INFO("direction=%d, speed=%d\n", direction, speed);
|
|
}
|
|
prevDirection = direction;
|
|
prevSpeed = speed;
|
|
|
|
loop_rate.sleep();
|
|
}
|
|
return 0; //never reaches here
|
|
}
|