void init(char *inifilename, int argc, char **argv)
{
// Init config, allows overrides (including the ini file name)
configManager.init(inifilename, argc, argv);
// Init logger (singleton)
Logger *logger = new StdoutLogger();
//logger->SetLogLevel(Logger::LOGLEVEL_INFO);
logger->SetLogLevel(Logger::LOGLEVEL_ERROR);
Logger::getInstance()->Log(Logger::LOGLEVEL_VERBOSE,"CamClient","CamClient started\n");
time_t t = time(0); // get time now
struct tm * now = localtime( & t );
Logger::getInstance()->Log(Logger::LOGLEVEL_INFO,"CamClient","Current time: %d-%d-%d, %d:%d:%d\n",
(now->tm_year + 1900),(now->tm_mon + 1),now->tm_mday,now->tm_hour,now->tm_min,now->tm_sec );
Logger::getInstance()->Log(Logger::LOGLEVEL_VERBOSE,"CamClient","Ini file: %s\n",inifilename);
// Init time measurement
timeMeasurement.init();
CamClient::TimeMeasurementCodeDefs::setnames(&timeMeasurement);
// Initialize camera
if (configManager.usePs3eye)
{
videoInput = VideoInputFactory::CreateVideoInput(VIDEOINPUTTYPE_PS3EYE);
}
else
{
videoInput = VideoInputFactory::CreateVideoInput(VIDEOINPUTTYPE_GENERIC);
}
if (configManager.camID>=0)
{
videoInput->init(configManager.camID);
}
else
{
videoInput->init(configManager.camSourceFilename.data());
}
// Set auto gain, exposure and white balance
videoInput->SetNormalizedGain(-1);
videoInput->SetNormalizedExposure(-1);
videoInput->SetNormalizedWhiteBalance(-1,-1,-1);
if (configManager.showImage)
{
namedWindow(imageWindowName, CV_WINDOW_AUTOSIZE);
}
// Init matrices
frameCaptured = new Mat(480,640,CV_8UC4); // Warning: this assumes the resolution and 4 channel color depth
// Initialize socket communication and server socket
server.InitServer(configManager.serverPort);
}
RRT::RRT(const State &_state, const double _time): time(_time)
{
/// Tree initialization
nodes.reserve(RRT_MAX_ITER);
states.reserve(RRT_MAX_ITER);
states.push_back(_state);
const int dimension = 2;
int *topology = new int[dimension];
MPNN::ANNcoord *scale = new MPNN::ANNcoord[dimension];
for(int i=0;i<dimension;i++) {
scale[i] = 1.0;
topology[i] = 1;
}
kdTree = new MPNN::MultiANN<int>(dimension,1,topology,(MPNN::ANNpoint)scale);
generate_input_seed();
TimeMeasurement tm;
tm.start();
construct_tree();
find_trajectory();
tm.end();
}
int main(int argc, char *argv[])
{
init(inifilename,argc,argv);
if (server.RegisterNode("avalon.aut.bme.hu","~kristof/smeyel/smeyel_reg.php?IP=127.0.0.1:6000")) // TODO: do not hardwire the port!
{
cout << "Error: could not register the node..." << endl;
}
server.ListenServerSocket();
// Enter main loop: wait for commands and execute them.
while(running)
{
// Wait for connection
Logger::getInstance()->Log(Logger::LOGLEVEL_INFO,"CamClient","Waiting for connection\n");
cout << "Waiting for connection." << endl;
struct sockaddr_in addr;
SOCKET sock = accept(server.GetServerSocket(), (struct sockaddr *) &addr, NULL);
if (sock < 0 || sock == INVALID_SOCKET)
{
// WSAGetLastError
Logger::getInstance()->Log(Logger::LOGLEVEL_ERROR,"CamClient","Error on accept(), trying to re-initialize server.\n");
server.DisconnectServer();
server.InitServer(configManager.serverPort);
server.ListenServerSocket();
cout << "Server rebinded, waiting for connection." << endl;
sock = accept(server.GetServerSocket(), (struct sockaddr *) &addr, NULL);
if (sock < 0 || sock == INVALID_SOCKET)
{
Logger::getInstance()->Log(Logger::LOGLEVEL_ERROR,"CamClient","Error on accept(), 2nd try failed, exiting.\n");
server.DisconnectServer();
exit(1);
}
}
// Convert IP address to string
char ipAddressStr[INET_ADDRSTRLEN];
inet_ntop( AF_INET, &addr.sin_addr, ipAddressStr, INET_ADDRSTRLEN );
Logger::getInstance()->Log(Logger::LOGLEVEL_INFO,"CamClient","Connection received from %s\n",ipAddressStr);
cout << "Connected." << endl;
// TODO: while socket is not closed by remote size, repeat waiting for commands and execute them...
bool connectionOpen = true;
server.SetSock(sock);
while(connectionOpen)
{
// Handle connection
// TODO: wait for the whole message
timeMeasurement.start(CamClient::TimeMeasurementCodeDefs::ReceiveCommand);
JsonMessage *msg = server.ReceiveNew();
timeMeasurement.finish(CamClient::TimeMeasurementCodeDefs::ReceiveCommand);
if (!msg)
{
// The connection was closed from the remote side.
// (Or a malformed JSON was received...)
Logger::getInstance()->Log(Logger::LOGLEVEL_ERROR,"CamClient","Error on reading from socket. Closing connection.\n");
connectionOpen=false;
break;
}
// ---------- Message received, now handle it
timeMeasurement.start(CamClient::TimeMeasurementCodeDefs::HandleJson);
handleJSON(msg, &server);
timeMeasurement.finish(CamClient::TimeMeasurementCodeDefs::HandleJson);
}
// Close connection
server.Disconnect();
Logger::getInstance()->Log(Logger::LOGLEVEL_INFO,"CamClient","Connection closed\n");
}
server.DisconnectServer();
return 0;
}
void RRT::optimize_trajectory(){
Trajectory traj;
double length, new_length;
double v_max = world.leader->get_limits().vMax*0.8;
double c_max = world.leader->get_limits().cMax;
double c_min = world.leader->get_limits().cMin;
State st = states[0],
st2;
for (int i = 0; i < out_trajectory.size(); ++i) {
traj.push_back(Desc(st,out_trajectory[i]));
st2 = world.leader->calculate_state(st, out_trajectory[i]);
st = st2;
}
traj.push_back(Desc(st,Control()));
#ifdef DEBUG_RRT
length = 0;
for (int i = 0; i < traj.size(); ++i) {
length += traj[i].control.t * traj[i].control.v;
}
TimeMeasurement tm;
tm.start();
std::cout << "RRT: original trajectory length: " << length << std::endl;
/*
std::cout << std::endl << "LEADER PLANNED POSITIONS:" << std::endl;
for (int i = 0; i < traj.size(); ++i) {
std::cout << " X = [ " << traj[i].state.X(0) << ", " << traj[i].state.X(1) << ", " << traj[i].state.X(2) << " ]" << std::endl;
}
*/
#endif
int loop = 0;
bool isValid;
bool endPoint;
while(loop < 100){
++loop;
int n_first = rand() % traj.size();
int n_second = rand() % traj.size();
while(n_first == n_second){
n_second = rand() % traj.size();
}
if (n_first > n_second){
const int p = n_first;
n_first = n_second;
n_second = p;
}
length = 0;
for (int i = n_first; i < n_second; ++i) {
length += traj[i].control.t * fabs(traj[i].control.v);
}
endPoint = false;
if (n_second != traj.size()-1){
dubins = new geom::Dubins(geom::Position(geom::Point(traj[n_first].state.X(0),traj[n_first].state.X(1)), traj[n_first].state.phi),
geom::Position(geom::Point(traj[n_second].state.X(0),traj[n_second].state.X(1)), traj[n_second].state.phi),
1/world.leader->cMax());
}else{
endPoint = true;
dubins = new geom::Dubins(geom::Position(geom::Point(traj[n_first].state.X(0),traj[n_first].state.X(1)), traj[n_first].state.phi),
geom::Point(goal[0],goal[1]),
1/world.leader->cMax());
length += dist2D(traj.back().state.X,Vector(goal[0],goal[1],0));
}
new_length = dubins->getLength();
if (new_length < length){
#ifdef DEBUG_RRT_DUBINS
if (endPoint){
std::cout << "DUBINS: original part length : " << length << std::endl;
std::cout << " simplified part length : " << new_length << std::endl;
std::cout << " type of Dubins maneuver: " << dubins->getTypeOfManeuver() << " [RSR, LSL, LSR, RSL, RLR, LRL, LS, RS]" << std::endl;
}
#endif
Trajectory new_traj;
for (int i = 0; i < n_first; ++i) {
new_traj.push_back(traj[i]);
}
double t1, t2, t3, c1, c2, c3;
State state_first = traj[n_first].state;
Control cnt;
switch (dubins->getTypeOfManeuver()) {
case geom::RSR:
t1 = dubins->getRadius()*fabs(dubins->getLen1())/v_max;
t2 = dubins->getLen2()/v_max;
t3 = dubins->getRadius()*fabs(dubins->getLen3())/v_max;
c1 = c_min;
c2 = 0;
c3 = c_min;
break;
case geom::LSL:
t1 = dubins->getRadius()*fabs(dubins->getLen1())/v_max;
t2 = dubins->getLen2()/v_max;
t3 = dubins->getRadius()*fabs(dubins->getLen3())/v_max;
c1 = c_max;
c2 = 0;
c3 = c_max;
break;
case geom::LSR:
t1 = dubins->getRadius()*fabs(dubins->getLen1())/v_max;
t2 = dubins->getLen2()/v_max;
t3 = dubins->getRadius()*fabs(dubins->getLen3())/v_max;
c1 = c_max;
c2 = 0;
c3 = c_min;
break;
case geom::RSL:
t1 = dubins->getRadius()*fabs(dubins->getLen1())/v_max;
t2 = dubins->getLen2()/v_max;
t3 = dubins->getRadius()*fabs(dubins->getLen3())/v_max;
c1 = c_min;
c2 = 0;
c3 = c_max;
break;
case geom::RLR:
t1 = dubins->getRadius()*fabs(dubins->getLen1())/v_max;
t2 = dubins->getRadius()*fabs(dubins->getLen2())/v_max;
t3 = dubins->getRadius()*fabs(dubins->getLen3())/v_max;
c1 = c_min;
c2 = c_max;
c3 = c_min;
break;
case geom::LRL:
t1 = dubins->getRadius()*fabs(dubins->getLen1())/v_max;
t2 = dubins->getRadius()*fabs(dubins->getLen2())/v_max;
t3 = dubins->getRadius()*fabs(dubins->getLen3())/v_max;
c1 = c_max;
c2 = c_min;
c3 = c_max;
break;
default:
break;
}
State s_new, s_old = state_first;
int n_added = 0;
if (t1 > eps){
while (t1-RRT_TIME_STEP>eps){
cnt = Control(v_max,0,c1,RRT_TIME_STEP);
new_traj.push_back(Desc(s_old,cnt));
t1 -= RRT_TIME_STEP;
s_new = world.leader->calculate_state(s_old, cnt);
s_old = s_new;
++n_added;
}
cnt = Control(v_max,0,c1,t1);
new_traj.push_back(Desc(s_old,cnt));
s_new = world.leader->calculate_state(s_old, cnt);
s_old = s_new;
++n_added;
}
if (t2 > eps){
while (t2-RRT_TIME_STEP>eps){
cnt = Control(v_max,0,c2,RRT_TIME_STEP);
new_traj.push_back(Desc(s_old,cnt));
t2 -= RRT_TIME_STEP;
s_new = world.leader->calculate_state(s_old, cnt);
s_old = s_new;
++n_added;
}
cnt = Control(v_max,0,c2,t2);
new_traj.push_back(Desc(s_old,cnt));
s_new = world.leader->calculate_state(s_old, cnt);
s_old = s_new;
++n_added;
}
if (t3 > eps){
while (t3-RRT_TIME_STEP>eps){
cnt = Control(v_max,0,c3,RRT_TIME_STEP);
new_traj.push_back(Desc(s_old,cnt));
t3 -= RRT_TIME_STEP;
s_new = world.leader->calculate_state(s_old, cnt);
s_old = s_new;
++n_added;
}
cnt = Control(v_max,0,c3,t3);
new_traj.push_back(Desc(s_old,cnt));
++n_added;
}
isValid = true;
for (int i = 0; i < n_added; ++i) {
const int index = new_traj.size()-n_added+i;
Points pts = world.leader->getSampledTrajectory(new_traj[index].state,&new_traj[index].control,1);
for (int j = 0; j < pts.size(); ++j) {
isValid &= world.map->wall_distance(pts[j]) > world.leader->get_ra();
if (!isValid)
break;
}
if (!isValid)
break;
}
if (!isValid){
continue;
}
loop = 0;
if (!endPoint){
for (int i = n_second; i < traj.size(); ++i) {
new_traj.push_back(traj[i]);
}
}else{
s_new = world.leader->calculate_state(new_traj.back().state, new_traj.back().control);
new_traj.push_back(Desc(State(goal[0],goal[1],0,0),Control()));
}
traj = new_traj;
}
}
out_trajectory.clear();
for (int i = 0; i < traj.size()-1; ++i) {
out_trajectory.push_back(traj[i].control);
}
#ifdef DEBUG_RRT
length = 0;
for (int i = 0; i < traj.size(); ++i) {
length += traj[i].control.t * traj[i].control.v;
}
tm.end();
std::cout << "RRT: optimized trajectory length: " << length << std::endl;
std::cout << " size: " << traj.size()-1 << std::endl;
/*
std::cout << std::endl;
for (int i = 0; i < traj.size(); ++i) {
std::cout << " v = " << traj[i].control.v << ", w = " << traj[i].control.w << ", c = " << traj[i].control.c << ", t = " << traj[i].control.t << std::endl;
}
*/
#endif
}
