int process_args( char* buffer, arg_t segments, char delim1, char delim2 ) {
// Divide the string by the delimiting character to get individual segments
int n_segments = get_instances( buffer, segments, delim1 );
// Dividne the substsrings by the secondary delimiter to get pairs
for ( int i = 0; i < n_segments; ++i ) {
get_pairs( segments[i], delim2 );
}
return n_segments;
}
void
AirspaceRoute::add_nearby_airspace(const RouteAirspaceIntersection &inx,
const RouteLink &e)
{
const SearchPointVector& fat = inx.first->get_clearance();
const ClearingPair p = get_pairs(fat, e.first, e.second);
const ClearingPair pb = get_backup_pairs(fat, e.first, inx.second);
// process all options
add_candidate(RouteLinkBase(e.first, p.first));
add_candidate(RouteLinkBase(e.first, p.second));
add_candidate(RouteLinkBase(e.first, pb.first));
add_candidate(RouteLinkBase(e.first, pb.second));
}
int main(int argc, char **argv) {
std::vector<color_keyframe::Ptr> frames;
util U;
U.load("http://localhost/corridor_map2", frames);
timestamp_t t0 = get_timestamp();
std::vector<std::pair<int, int> > overlapping_keyframes;
int size;
int workers = argc - 1;
ros::init(argc, argv, "panorama");
std::vector<
actionlib::SimpleActionClient<rm_multi_mapper::WorkerSlamAction>*> ac_list;
for (int i = 0; i < workers; i++) {
actionlib::SimpleActionClient<rm_multi_mapper::WorkerSlamAction>* ac =
new actionlib::SimpleActionClient<
rm_multi_mapper::WorkerSlamAction>(
std::string(argv[i + 1]), true);
ac_list.push_back(ac);
}
sql::ResultSet *res;
size = frames.size();
get_pairs(overlapping_keyframes);
std::vector<rm_multi_mapper::WorkerSlamGoal> goals;
int keyframes_size = (int) overlapping_keyframes.size();
for (int k = 0; k < workers; k++) {
rm_multi_mapper::WorkerSlamGoal goal;
int last_elem = (keyframes_size / workers) * (k + 1);
if (k == workers - 1)
last_elem = keyframes_size;
for (int i = (keyframes_size / workers) * k; i < last_elem; i++) {
rm_multi_mapper::KeyframePair keyframe;
keyframe.first = overlapping_keyframes[i].first;
keyframe.second = overlapping_keyframes[i].second;
goal.Overlap.push_back(keyframe);
}
goals.push_back(goal);
}
ROS_INFO("Waiting for action server to start.");
for (int i = 0; i < workers; i++) {
ac_list[i]->waitForServer();
}
ROS_INFO("Action server started, sending goal.");
// send a goal to the action
for (int i = 0; i < workers; i++) {
ac_list[i]->sendGoal(goals[i]);
}
//wait for the action to return
std::vector<bool> finished;
for (int i = 0; i < workers; i++) {
bool finished_before_timeout = ac_list[i]->waitForResult(
ros::Duration(30.0));
finished.push_back(finished_before_timeout);
}
bool success = true;
for (int i = 0; i < workers; i++) {
success = finished[i] && success;
}
Eigen::MatrixXf acc_JtJ;
acc_JtJ.setZero(size * 6, size * 6);
Eigen::VectorXf acc_Jte;
acc_Jte.setZero(size * 6);
if (success) {
for (int i = 0; i < workers; i++) {
Eigen::MatrixXf JtJ;
JtJ.setZero(size * 6, size * 6);
Eigen::VectorXf Jte;
Jte.setZero(size * 6);
rm_multi_mapper::Vector rosJte = ac_list[i]->getResult()->Jte;
rm_multi_mapper::Matrix rosJtJ = ac_list[i]->getResult()->JtJ;
vector2eigen(rosJte, Jte);
matrix2eigen(rosJtJ, JtJ);
acc_JtJ += JtJ;
acc_Jte += Jte;
}
} else {
ROS_INFO("Action did not finish before the time out.");
std::exit(0);
}
Eigen::VectorXf update = -acc_JtJ.ldlt().solve(acc_Jte);
float iteration_max_update = std::max(std::abs(update.maxCoeff()),
std::abs(update.minCoeff()));
ROS_INFO("Max update %f", iteration_max_update);
/*for (int i = 0; i < (int)frames.size(); i++) {
frames[i]->get_pos() = Sophus::SE3f::exp(update.segment<6>(i))
* frames[i]->get_pos();
std::string query = "UPDATE `positions` SET `q0` = " +
boost::lexical_cast<std::string>(frames[i]->get_pos().so3().data()[0]) +
", `q1` = " +
boost::lexical_cast<std::string>(frames[i]->get_pos().so3().data()[1]) +
", `q2` = " +
boost::lexical_cast<std::string>(frames[i]->get_pos().so3().data()[2]) +
", `q3` = " +
boost::lexical_cast<std::string>(frames[i]->get_pos().so3().data()[3]) +
", `t0` = " +
boost::lexical_cast<std::string>(frames[i]->get_pos().translation()[0]) +
", `t1` = " +
boost::lexical_cast<std::string>(frames[i]->get_pos().translation()[1]) +
", `t2` = " +
boost::lexical_cast<std::string>(frames[i]->get_pos().translation()[2]) +
", `int0` = " +
boost::lexical_cast<std::string>(frames[i]->get_intrinsics().array()[0]) +
", `int1` = " +
boost::lexical_cast<std::string>(frames[i]->get_intrinsics().array()[1]) +
", `int2` = " +
boost::lexical_cast<std::string>(frames[i]->get_intrinsics().array()[2]) +
" WHERE `id` = " +
boost::lexical_cast<std::string>(i) +
";";
res = U.sql_query(query);
delete res;
}*/
timestamp_t t1 = get_timestamp();
double secs = (t1 - t0) / 1000000.0L;
std::cout << secs << std::endl;
return 0;
}
int main(int argc, char *argv[]) {
int i;
int get_host_name = getHostVMNo();
getVMsIP(host_ip);
inet_pton(AF_INET,host_ip,&host_ip_struct);
///define this function
get_pairs(ip_hw_map);
memcpy((void *)hostmac,(void *)ip_hw_map[0].hw,IF_HADDR);
struct tour_packet packet;
int proto = htons(ETH_P_IP);
//create 4 sockets
rt_fd = socket(AF_INET,SOCK_RAW,PROTO_ID);
udp_fd = socket(AF_INET,SOCK_DGRAM,0);
pf_fd = socket(PF_PACKET,SOCK_RAW,htons(ETH_P_IP));
pg_fd = socket(AF_INET,SOCK_RAW,IPPROTO_ICMP);
int flag = 1;
setsockopt(rt_fd,IPPROTO_IP,IP_HDRINCL,&flag,sizeof(flag));
setsockopt(udp_fd,SOL_SOCKET,SO_REUSEADDR,&flag,sizeof(flag));
//struct in_addr ip_multicast;
inet_pton(AF_INET,IP_MULTICAST,&ip_multicast);
int port_multicast = PORT_MULTICAST;
struct tour_packet *packet_tour;
char inputVM[20];
char *Vmno;
int j,n, input_error=0, vmno;
packet_tour = (void *)malloc(sizeof(struct tour_packet));
if (argc <=1) {
printf("\nTour is running..\n");
} else {
inet_pton(AF_INET,host_ip,&vm_ip_list[0]);
for (i =1;i < argc ;i ++)
{
strcpy(inputVM,argv[i]);
if(inputVM[0]>= '1' && inputVM[0]<= '9')
{
n=atoi(inputVM);
if(n<1 || n>10)
{
input_error=1;
break;
}
}
else
{
for(j=0;j<3;j++)
inputVM[j]=tolower(inputVM[j]);
if(inputVM[0]=='v')
{
if(inputVM[1]=='m')
{
Vmno=inputVM+2;
n=atoi(Vmno);
if(n<1 || n>10)
{
input_error=1;
break;
}
}
else
{
input_error=1;
break;
}
}
}
//int vmno = atoi(argv[i]);
vmno=n;
getVMIPstruct(&vm_ip_list[i],vmno);
count++;
//vm_ip_list[i] = get_ip(vmno)
//make a ip list
}
if(input_error==1)
{
printf("\nImproper VM no provided.Program terminated\n");
exit(0);
}
//get multicast ip address and porti
enable_multicating(ip_multicast,port_multicast);
//genrate tour payload
packet_tour->ip = ip_multicast;
packet_tour->port = port_multicast;
packet_tour->index = 0;
packet_tour->end_index = count;
//start your tour
memcpy(packet_tour->vm_list,vm_ip_list,sizeof(vm_ip_list));
if (count != 0)
send_to_next_node(packet_tour,count);
}
monitor_sockets();
return 1;
}
