std::istream& readText::getLine(std::istream & inStream, std::string & line) {
if (std::getline(inStream, line)) {
while(!goodLine(line)) {
if (!std::getline(inStream, line)) break;
}
}
return inStream;
}
int main(int argc, char **argv)
{
std::string file = "/dev/controller_sensor";
UInt32 baudrate = 115200;
SerialInterface usb("/dev/controller_sensor", baudrate);
if (argc > 1)
{
file = argv[1];
printf("%s info: Opening %s\n", argv[0], file.c_str());
}
if (pthread_mutex_init(&myMutex, NULL) != 0)
{
printf("Failed to get mutex: %s\n", strerror(errno));
exit(-1);
}
name = argv[0];
float temp[3] = {0.0};
float pressure = 0.0;
int motorKilled = 0, waterDetected, dropperLeft= 0, dropperRight= 0;
float curVolt[2] = {0.0};
int numVariables = 10;
float ttemp[3] = {0.0};
float tpressure = 0.0;
int tmotorKilled = 0, twaterDetected= 0, tdropperLeft= 0, tdropperRight= 0;
float tcurVolt[2] = {0.0};
timer tempTimer, pressureTimer, motorTimer, currentTimer, waterTimer, dropperTimer;
tempTimer.start(1, 0);
pressureTimer.start(1, 0);
motorTimer.start(1, 0);
currentTimer.start(1, 0);
waterTimer.start(1, 0);
dropperTimer.start(1, 0);
ros::init(argc, argv, "SubSensorController");
ros::NodeHandle nh;
ros::Publisher temperatuerPub = nh.advertise<std_msgs::Float32MultiArray>("Controller_Box_Temp", 1000);
ros::Publisher pressurePub = nh.advertise<std_msgs::Float32>("Pressure_Data", 1000);
ros::Publisher MotorPub = nh.advertise<std_msgs::UInt8>("Motor_State", 1000);
ros::Publisher computerPub = nh.advertise<std_msgs::Float32MultiArray>("Computer_Cur_Volt", 1000);
ros::Publisher waterPub = nh.advertise<std_msgs::UInt8>("Water_Detected", 1000);
ros::Publisher dropperPub = nh.advertise<std_msgs::UInt8MultiArray>("Dropper_States", 1000);
ros::Subscriber torpedoSub = nh.subscribe("Torpedo_Launch", 1000, torpedoCallback);
ros::Subscriber dropperSub = nh.subscribe("Dropper_Activate", 1000, dropperCallback);
ros::Rate loop_rate(1);
while (ros::ok())
{
if (controllerState == STATE_WAITING_ON_FD)
{
if (checkLock() && getLock())
{
controllerState = STATE_WORKING;
sleep(5);
//fd = open(file.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
// if (fd == -1)
// {
// printf("%s Error: System failed to open %s: %s(%d)\n", argv[0], file.c_str(), strerror(errno), errno);
// sleep(1);
// }
// else
// {
// if (setupTTY(fd) == 0)
// {
// controllerState = STATE_WORKING;
// sleep(5); //wait for sensor to boot and stabilize
// }
// else
// close(fd);
// }
releaseLock();
}
}
else if (controllerState == STATE_WORKING)
{
std::string line = "";
if (checkLock() && getLock())
{
UInt8 aBuf[baudrate];
usb.recv(aBuf,baudrate);
std::string temp ((const char*)aBuf,baudrate);
line = temp;
//line = getTTYLine(fd);
//printf("got line %s\n", line.c_str());
releaseLock();
}
if (line != "")
{
//printf("line: %s\n", line.c_str());
if (line.length() > 0 && goodLine(line, numVariables))
{
int scanfVal;
if ((scanfVal = sscanf(line.c_str(), "S%f,%f,%f,%d,%f,%f,%f,%d,%d,%dE", &ttemp[0], &ttemp[1], &ttemp[2], &tmotorKilled,
&tcurVolt[0], &tcurVolt[1], &tpressure, &twaterDetected, &tdropperLeft, &tdropperRight)) == numVariables)
{
if ((temp[0] != ttemp[0]) || (temp[1] != ttemp[1]) || (temp[2] != ttemp[2]) || tempTimer.isTimeout())
{
//temperature
std_msgs::Float32MultiArray tempMsg;
temp[0] = ttemp[0];
temp[1] = ttemp[1];
temp[2] = ttemp[2];
tempMsg.data.push_back(temp[0]);
tempMsg.data.push_back(temp[1]);
tempMsg.data.push_back(temp[2]);
temperatuerPub.publish(tempMsg);
tempTimer.start(1, 0);
ros::spinOnce();
}
if (motorKilled != tmotorKilled || motorTimer.isTimeout())
{
//motorkilled
std_msgs::UInt8 motorMsg;
motorKilled = tmotorKilled;
motorMsg.data = motorKilled;
MotorPub.publish(motorMsg);
motorTimer.start(1, 0);
ros::spinOnce();
}
if ((curVolt[0] != tcurVolt[0]) || (curVolt[1] != tcurVolt[1] || currentTimer.isTimeout()))
{
//curvolt
std_msgs::Float32MultiArray curMsg;
curVolt[0] = tcurVolt[0];
curVolt[1] = tcurVolt[1];
curMsg.data.push_back(curVolt[0]);
curMsg.data.push_back(curVolt[1]);
computerPub.publish(curMsg);
currentTimer.start(1, 0);
ros::spinOnce();
}
if (pressure != tpressure || pressureTimer.isTimeout())
{
//depth
std_msgs::Float32 pressureMsg;
pressure = tpressure;
pressureMsg.data = pressure;
pressurePub.publish(pressureMsg);
pressureTimer.start(1, 0);
ros::spinOnce();
}
if (waterDetected != twaterDetected || waterTimer.isTimeout())
{
//water detected
std_msgs::UInt8 waterMsg;
waterDetected = twaterDetected;
waterMsg.data = waterDetected;
waterPub.publish(waterMsg);
waterTimer.start(1, 0);
ros::spinOnce();
}
if (tdropperLeft != dropperLeft || tdropperRight != dropperRight || dropperTimer.isTimeout())
{
//water detected
std_msgs::UInt8MultiArray dropperMsg;
dropperLeft = tdropperLeft;
dropperRight = tdropperRight;
dropperMsg.data.push_back(dropperLeft);
dropperMsg.data.push_back(dropperRight);
dropperPub.publish(dropperMsg);
dropperTimer.start(1, 0);
ros::spinOnce();
}
if (VERBOSE)
ROS_INFO("published");
}
else
{
printf("%s info: Throwing away %d:\"%s\"\n", argv[0], scanfVal, line.c_str());
fflush(stdout);
}
}
}
}
}
pthread_mutex_destroy(&myMutex);
close(fd);
return 0;
}
