#include <sys/time.h>
#include "Logger.h"
#include "Aria.h"
#include <cstdlib>
#include <iostream>
#include <boost/asio.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/config.hpp>
#include <boost/program_options/detail/config_file.hpp>
#include <boost/program_options/parsers.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/ini_parser.hpp>
#include <boost/bind.hpp>
#include <boost/smart_ptr.hpp>
#include <boost/thread.hpp>
#include <string>
#include <sstream>
#include <set>
#include <sstream>
#include <exception>
#include <fstream>
#include <limits>
#include <stdarg.h>
#include <fcntl.h>
#include <string.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/time.h>
#include <time.h>
#include <unistd.h>
#include <stdarg.h>
#include <signal.h>
#define MAX_INTERVAL 1
using boost::asio::ip::udp;
using namespace std;
float temp_heading, temp_distance;
struct DataLine {
float nNumber;
std::string sName;
};
DataLine output;
clock_t prev = 0;
clock_t now;
float distanza_minima = 1500;
float offset = 900;
std::string comando;
float valore;
float heading_prova = 0;
float distance_prova = 0;
const int max_length = 100;
int integer_value;
short int porta;
float last_distance = 0.0;
bool discovery = true;
bool right_disc = false;
bool left_disc = false;
bool start = false;
std::string file;
FILE *file_open;
bool call_fprintf = true;
double interval;
timeval t1, t2, tempo;
double elapsedTime;
bool is_equal(float a, float b) {
return (fabs(a - b) < std::numeric_limits<float>::epsilon());
}
ArRobot robot;
ArSick sick;
bool newGoal=false;
ArPose m_goal;
void alarm_handler_to_check_timeout(int);
void load_from_file(void){
boost::property_tree::ptree pt;
boost::property_tree::ini_parser::read_ini("parametri.ini", pt);
porta = pt.get<short int>("Connessione.porta");
distanza_minima = pt.get<float>("Parametri.distanza_minima");
offset = pt.get<float>("Parametri.offset");
file = pt.get<std::string>("File.file");
}
int wrap_fprintf(FILE* str, const char* format, ...) {
int result = 1;
if (call_fprintf) {
va_list args;
va_start(args, format);
result = vfprintf(str, format, args);
va_end(args);
}
else {
/* do nothing */
}
return result;
}
/* A subclass of ArASyncTask, to contain a method that runs in a new thread */
class CControl : public ArASyncTask
{
ArMutex myMutex;
int myCounter;
float m_heading, m_distance;
bool first;
bool flag_heading, flag_distance;
private:
public:
CControl()
{
first=false;
flag_heading=true;
flag_distance=true;
}
void setHeading(float heading)
{
myMutex.lock();
m_heading=heading;
//first=false;
flag_heading=false;
myMutex.unlock();
}
void setDistance(float distance)
{
myMutex.lock();
m_distance=distance;
//first=false;
flag_distance=false;
myMutex.unlock();
}
void* runThread(void*)
{
printf("\nSpostamento X: %f, Y: %f\n", robot.getX(), robot.getY());
wrap_fprintf(file_open," %f, %f, \n",robot.getX(),robot.getY());
while(Aria::getRunning())
{
if(discovery){
// printf("\nRoutine Discovery Avviata\a\n");
robot.setRotVel(4.0);
}
printf("\nInterval: %f Now: %f Prev: %f\n", interval, (double)now/CLOCKS_PER_SEC, (double)prev/CLOCKS_PER_SEC);
elapsedTime = (t2.tv_sec - t1.tv_sec) * 1000.0; // sec to ms
elapsedTime += (t2.tv_usec - t1.tv_usec) / 1000.0; // us to ms
cout << "\n" << elapsedTime << " ms.\n";
if( !flag_heading && !flag_distance)
{
myMutex.lock();
temp_heading=m_heading;
temp_distance=m_distance;
myMutex.unlock();
gettimeofday(&t2, NULL);
if (is_equal(temp_heading, 0.0)){
robot.setRotVel(0.0);
}
else robot.setRotVel(-ArMath::radToDeg(temp_heading));
if( temp_distance <= (distanza_minima + offset) && temp_distance > distanza_minima){
if (last_distance == 0) last_distance = temp_distance;
float delta = temp_distance - last_distance;
if (is_equal(delta, 0.0)) continue; // skip sending
else {
if ( delta > 0.0){
robot.setVel(float(delta/10));
// printf("\n\n**********\n\nAvanzo di %f\n******\n\n", delta);
}
}
last_distance = temp_distance;
}
else if(temp_distance <= distanza_minima || is_equal(temp_distance, 0.0))
robot.setVel(0.0);
else if(temp_distance > (distanza_minima + offset) )
robot.setVel(float(temp_distance/20));
// printf("\nrunThread:: getX=%f getY=%f heading= %f distance = %f rob_vel = %f rob_rot_vel = %f\n",robot.getX(), robot.getY(), ArMath::radToDeg(temp_heading),temp_distance, robot.getVel(),robot.getRotVel());
flag_heading = true;
flag_distance = true;
}
ArUtil::sleep(100);
}
}
/* Lock the mutex object. */
void lockMutex() { myMutex.lock(); }
/* Unlock the mutex object. */
void unlockMutex() { myMutex.unlock(); }
};
CControl control;
DataLine GetValueFromLine(const std::string& sData) {
std::string sName, sInteger;
std::stringstream ss;
DataLine Result;
size_t sz = sData.find('@');
sName = sData.substr(0,sz); // Just in case you need it later
Result.sName = sName;
sInteger = sData.substr(sz + 1,sData.length() - sz);
ss.str(sInteger);
ss >> Result.nNumber;
if (ss.fail()) {
// something went wrong, probably not an integer
}
return Result;
}
void server(boost::asio::io_service& io_service, short port)
{
udp::socket sock(io_service, udp::endpoint(udp::v4(), port));
std::cout << "\n**Connessione Accettata**\n";
std::cout << "\nOffset: " << offset << "\nDistanza Minima: " << distanza_minima << "\n";
signal(SIGALRM,alarm_handler_to_check_timeout);
alarm(1);
for (;;)
{
char data[max_length];
udp::endpoint sender_endpoint;
prev = clock();
size_t length = sock.receive_from(
boost::asio::buffer(data, max_length), sender_endpoint);
if(length == 0) printf("\n\nNONRICEVONULLA\n\n");
data[length] = 0;
output = GetValueFromLine(data);
comando = output.sName;
valore = output.nNumber;
if (output.sName == "right"){
robot.setRotVel(5.0);
robot.setVel(0.0);
// std::cout << "\nSto ricevendo: " << output.sName;
// printf("\nValore di discovery %d\n", discovery);
// printf("\nLEFT\n");
}
else if(output.sName == "left"){
robot.setRotVel(-5.0);
robot.setVel(0.0);
//printf("\nRIGHT\n");
}
else if (output.sName == "heading"){
start = false;
control.setHeading(output.nNumber);
// std::cout << "\nSto ricevendo: " << output.sName << "e heading: " << output.nNumber;
if(output.nNumber > 0) {
discovery = false;
right_disc = false;
left_disc = true;
}
else if(output.nNumber < 0) {
discovery = false;
left_disc = false;
right_disc = true;
}
}
else if (output.sName == "distance"){
start = false;
control.setDistance(output.nNumber);
// std::cout << "\nSto ricevendo: " << output.sName << "e distance: " << output.nNumber;
gettimeofday(&t1, NULL);
}
else printf("\nReceived code error\n");
gettimeofday(&tempo, NULL);
wrap_fprintf(file_open," %f, %f, %f %f %ld\n",robot.getX(),robot.getY(), temp_distance, ArMath::radToDeg(temp_heading), tempo.tv_sec);
}
}
void alarm_handler_to_check_timeout(int) {
now = clock();
printf("\nHndler_Interval: %f Now: %f Prev: %f\n", interval, (double)now, (double)prev);
interval = (double)(now - prev) / CLOCKS_PER_SEC;
if (interval > MAX_INTERVAL) { // define MAX_INTERVAL according to your needs
printf("\nDentro STOP condition\n"); // timeout, stop.
}
}
int main(int argc, char **argv)
{
load_from_file();
file_open = fopen(file.c_str(), "w");
if(file_open == NULL){
perror("Error opening the reports file");
}
wrap_fprintf(file_open," X[mm], Y[mm] Distance[mm]\n");
Aria::init();
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArSimpleConnector simpleConnector(&parser);
ArActionGoto gotoPoseAction("goto");
gotoPoseAction.setCloseDist(50);
ArSonarDevice sonar;
ArAnalogGyro gyro(&robot);
robot.addRangeDevice(&sick);
robot.addRangeDevice(&sonar);
// Make a key handler, so that escape will shut down the program cleanly
ArKeyHandler keyHandler;
Aria::setKeyHandler(&keyHandler);
robot.attachKeyHandler(&keyHandler);
printf("main:: You may press escape to exit\n");
// Collision avoidance actions at higher priority
ArActionLimiterForwards limiterAction("speed limiter near", 300, 600, 250);
ArActionLimiterForwards limiterFarAction("speed limiter far", 300, 1100, 400);
ArActionLimiterTableSensor tableLimiterAction;
robot.addAction(&tableLimiterAction, 100);
robot.addAction(&limiterAction, 95);
robot.addAction(&limiterFarAction, 90);
// Goto action at lower priority
robot.addAction(&gotoPoseAction, 50);
// Stop action at lower priority, so the robot stops if it has no goal
ArActionStop stopAction("stop");
robot.addAction(&stopAction, 40);
// Parse all command line arguments
if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
exit(1);
}
// Connect to the robot
if (!simpleConnector.connectRobot(&robot))
{
printf("main:: Could not connect to robot... exiting\n");
Aria::exit(1);
}
robot.runAsync(true);
// turn on the motors, turn off amigobot sounds
robot.enableMotors();
robot.comInt(ArCommands::SOUNDTOG, 0);
simpleConnector.setupLaser(&sick);
sick.runAsync();
if (!sick.blockingConnect())
{
ArLog::log(ArLog::Terse,
"main: Could not connect to SICK laser... exiting\n");
Aria::shutdown();
Aria::exit(1);
}
CLogger logger(&robot, &sick, 10, 1, "laserOdo.log");
control.runAsync();
try{
boost::asio::io_service io_service;
server(io_service, porta);}
catch (std::exception& e)
{
std::cerr << "Exception: " << e.what() << "\n";
}
return 0;
}