void cspec_tree_add_describe(cspec_tree_t * tree, char * key){ if(find_node(tree->describes, key) != NULL){ return; } cspec_node_describe_t * describe = (cspec_node_describe_t *)malloc(sizeof(cspec_node_describe_t)); describe->base = initialize_node(DESCRIBE, key); describe->its = cspec_list_initialize(); cspec_list_add(tree->describes, (void *)describe); }
// search node by use character. // if nothing, create new node static node search_child_or_create(node n, char moji) { node child; child = search_child(n, moji); if(!child) { child = initialize_node(moji); add_child(n, child); } return child; }
void cspec_tree_add_it(cspec_tree_t * tree, char * describe_key, char * it_key, cspec_block_it code_block){ cspec_node_describe_t * describe = find_node(tree->describes, describe_key); if(describe == NULL){ cspec_tree_add_describe(tree, describe_key); describe = find_node(tree->describes, describe_key); } cspec_node_code_block_t * it = find_node(describe->its, it_key); if(it != NULL){ it->code_block_it = code_block; }else{ cspec_node_code_block_t * it = (cspec_node_code_block_t *)malloc(sizeof(cspec_node_code_block_t)); it->base = initialize_node(IT, it_key); it->code_block_it = code_block; cspec_list_add(describe->its, (void *)it); } }
/** * new **/ static VALUE t_new(int argc, VALUE *argv, VALUE klass) { node root; VALUE obj, array, string; root = initialize_node(NULL_CHAR); obj = Data_Make_Struct(klass, struct _node, NULL, destroy_node, root); if (argc == 1) { array = argv[0]; while((string = rb_ary_shift(argv[0])) != Qnil) { t_add(obj, string); } } return obj; }
void cspec_tree_add_after(cspec_tree_t * tree, char * describe_key, char * it_key, cspec_block code_block){ cspec_node_code_block_t * after = (cspec_node_code_block_t *)malloc(sizeof(cspec_node_code_block_t)); after->base = initialize_node(AFTER, NULL); after->code_block = code_block; if(strlen(describe_key) == 0){ // ROOT cspec_list_add(tree->afters, (void *)after); }else{ cspec_tree_add_describe(tree, describe_key); cspec_node_describe_t * describe = find_node(tree->describes, describe_key); if(strlen(it_key) == 0){ // DESCRIBE cspec_list_add(describe->base->afters, (void *)after); }else{ // IT cspec_tree_add_it(tree, describe_key, it_key, NULL); cspec_node_code_block_t * it = find_node(describe->its, it_key); cspec_list_add(it->base->afters, (void *)after); } } }
void cspec_tree_add_before(cspec_tree_t * tree, char * describe_key, char * it_key, cspec_block code_block){ cspec_node_code_block_t * before = (cspec_node_code_block_t *)malloc(sizeof(cspec_node_code_block_t)); before->base = initialize_node(BEFORE, NULL); before->code_block = code_block; if(strlen(describe_key) == 0){ // ROOT cspec_list_add(tree->befores, (void *)before); }else{ cspec_tree_add_describe(tree, describe_key); cspec_node_describe_t * describe = find_node(tree->describes, describe_key); if(strlen(it_key) == 0){ // DESCRIBE cspec_list_add(describe->base->befores, (void *)before); }else{ // IT cspec_tree_add_it(tree, describe_key, it_key, NULL); cspec_node_code_block_t * it = find_node(describe->its, it_key); cspec_list_add(it->base->befores, (void *)before); } } }
void insert (trie_node_adt *root, char *word) { int i, digit; int length = strlen (word); //printf("%d", length); trie_node *node_pointer; node_pointer = root->pointer ; for (i = 0; i < length; i++) { digit = char_to_digit (*(word + i)) - 2; //printf("hello\n"); if (node_pointer->children[digit] == NULL) { //printf("hey\n"); node_pointer->children [digit] = initialize_node(); } node_pointer = node_pointer->children[digit]; //printf("%d\n", digit); } //printf("forloopend"); node_pointer->word = word; // Final node printf("%s\n", node_pointer->word); }
/** * @brief 要素の挿入 * * k番目の要素とk+1番目の要素の間にxを挿入する。 * - 時間計算量: \f$ O(\log{n}) \f$ * * @param[in] k 挿入する位置 * @param[in] x 挿入する値 */ void insert(size_t k, const value_type &x){ const auto p = split(m_root, k); node_type *n = new node_type(); initialize_node(n, x); m_root = merge(merge(p.first, n), p.second); }
int initialize_node(node* my_node,const char* log_path, void* db_ptr,void* arg){ int flag = 1; dare_server_input_t input = { .log = stdout, .peer_pool = my_node->peer_pool, .group_size = my_node->group_size, .server_idx = my_node->node_id }; if (0 != dare_server_init(&input)) { err_log("CONSENSUS MODULE : Cannot init dare\n"); goto initialize_node_exit; } my_node->cur_view.view_id = 1; my_node->cur_view.req_id = 0; my_node->cur_view.leader_id = 9999; zoo_port = my_node->zoo_port; start_zookeeper(&my_node->cur_view, &my_node->zfd, &my_node->lock); int build_log_ret = 0; if(log_path==NULL){ log_path = "."; }else{ if((build_log_ret=mkdir(log_path,S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH))!=0){ if(errno!=EEXIST){ err_log("CONSENSUS MODULE : Log Directory Creation Failed,No Log Will Be Recorded.\n"); }else{ build_log_ret = 0; } } } if(!build_log_ret){ char* sys_log_path = (char*)malloc(sizeof(char)*strlen(log_path)+50); memset(sys_log_path,0,sizeof(char)*strlen(log_path)+50); if(NULL!=sys_log_path){ sprintf(sys_log_path,"%s/node-%u-consensus-sys.log",log_path,my_node->node_id); my_node->sys_log_file = fopen(sys_log_path,"w"); free(sys_log_path); } if(NULL==my_node->sys_log_file && (my_node->sys_log || my_node->stat_log)){ err_log("CONSENSUS MODULE : System Log File Cannot Be Created.\n"); } } my_node->consensus_comp = NULL; my_node->consensus_comp = init_consensus_comp(my_node,my_node->my_address,&my_node->lock, my_node->node_id,my_node->sys_log_file,my_node->sys_log, my_node->stat_log,my_node->db_name,db_ptr,my_node->group_size, &my_node->cur_view,&my_node->highest_to_commit,&my_node->highest_committed, &my_node->highest_seen,arg); if(NULL==my_node->consensus_comp){ goto initialize_node_exit; } flag = 0; initialize_node_exit: return flag; } node* system_initialize(node_id_t node_id,const char* config_path, const char* log_path, void* db_ptr,void* arg){ node* my_node = (node*)malloc(sizeof(node)); memset(my_node,0,sizeof(node)); if(NULL==my_node){ goto exit_error; } my_node->node_id = node_id; myid = node_id; my_node->db_ptr = db_ptr; if(pthread_mutex_init(&my_node->lock,NULL)){ err_log("CONSENSUS MODULE : Cannot Init The Lock.\n"); goto exit_error; } if(consensus_read_config(my_node,config_path)){ err_log("CONSENSUS MODULE : Configuration File Reading Failed.\n"); goto exit_error; } if(initialize_node(my_node,log_path,db_ptr,arg)){ err_log("CONSENSUS MODULE : Network Layer Initialization Failed.\n"); goto exit_error; } return my_node; exit_error: if(NULL!=my_node){ } return NULL; }
int main(int argc, char** argv){ //pass the .graph file to open /* argc=3 argv[0]=/.../patrolling_sim/bin/Conscientious_Reactive argv[1]=__name:=XXXXXX argv[2]=maps/1r-5-map.graph argv[3]=ID_ROBOT */ //More than One robot (ID between 0 and 99) if ( atoi(argv[3]) >= NUM_MAX_ROBOTS || atoi(argv[3])<-1 ){ ROS_INFO("The Robot's ID must be an integer number between 0 and %d", NUM_MAX_ROBOTS-1); return 0; }else{ ID_ROBOT = atoi(argv[3]); } uint i = strlen( argv[2] ); char graph_file[i]; strcpy (graph_file,argv[2]); //Check Graph Dimension: uint dimension = GetGraphDimension(graph_file); //Create Structure to save the Graph Info; vertex vertex_web[dimension]; //Get the Graph info from the Graph File GetGraphInfo(vertex_web, dimension, graph_file); uint j; /* Output Graph Data */ for (i=0;i<dimension;i++){ printf ("ID= %u\n", vertex_web[i].id); printf ("X= %f, Y= %f\n", vertex_web[i].x, vertex_web[i].y); printf ("#Neigh= %u\n", vertex_web[i].num_neigh); for (j=0;j<vertex_web[i].num_neigh; j++){ printf("\tID = %u, DIR = %s, COST = %u\n", vertex_web[i].id_neigh[j], vertex_web[i].dir[j], vertex_web[i].cost[j]); } printf("\n"); } /* Define Starting Vertex/Position (Launch File Parameters) */ ros::init(argc, argv, "c_cognitive"); ros::NodeHandle nh; double initial_x, initial_y; XmlRpc::XmlRpcValue list; nh.getParam("initial_pos", list); ROS_ASSERT(list.getType() == XmlRpc::XmlRpcValue::TypeArray); int value = ID_ROBOT; if (value == -1){value = 0;} ROS_ASSERT(list[2*value].getType() == XmlRpc::XmlRpcValue::TypeDouble); initial_x = static_cast<double>(list[2*value]); ROS_ASSERT(list[2*value+1].getType() == XmlRpc::XmlRpcValue::TypeDouble); initial_y = static_cast<double>(list[2*value+1]); // printf("x=%f, y=%f\n", initial_x, initial_y); uint current_vertex = IdentifyVertex(vertex_web, dimension, initial_x, initial_y); // printf("v=%d\n",current_vertex); //Publicar dados de "odom" para nó de posições odom_pub = nh.advertise<nav_msgs::Odometry>("positions", 1); //only concerned about the most recent //Subscrever posições de outros robots odom_sub = nh.subscribe("positions", 10, positionsCB); char string[20]; char string2[20]; if(ID_ROBOT==-1){ strcpy (string,"odom"); //string = "odom" strcpy (string2,"cmd_vel"); //string = "cmd_vel" TEAMSIZE = 1; }else{ strcpy (string,"robot_"); strcpy (string2,"robot_"); char id[3]; itoa(ID_ROBOT, id, 10); strcat(string,id); strcat(string2,id); strcat(string,"/odom"); //string = "robot_X/odom" strcat(string2,"/cmd_vel"); //string = "robot_X/cmd_vel" TEAMSIZE = ID_ROBOT + 1; } // printf("string de publicação da odometria: %s\n",string); //Cmd_vel to backup: cmd_vel_pub = nh.advertise<geometry_msgs::Twist>(string2, 1); //Subscrever para obter dados da propria "odom" do robot corrente ros::Subscriber sub; sub = nh.subscribe(string, 1, odomCB); //size of the buffer = 1 (?) ros::spinOnce(); // Define Goal if(ID_ROBOT==-1){ strcpy (string,"move_base"); //string = "move_base" }else{ strcpy (string,"robot_"); char id[3]; itoa(ID_ROBOT, id, 10); strcat(string,id); strcat(string,"/move_base"); //string = "robot_X/move_base" } //printf("string do move base = %s\n",string); MoveBaseClient ac(string, true); //wait for the action server to come up while(!ac.waitForServer(ros::Duration(5.0))){ ROS_INFO("Waiting for the move_base action server to come up"); } //Define Goal: move_base_msgs::MoveBaseGoal goal; //Publicar dados para "results" results_pub = nh.advertise<geometry_msgs::PointStamped>("results", 1); //only concerned about the most recent results_sub = nh.subscribe("results", 10, resultsCB); //Subscrever "results" vindo dos robots initialize_node(); //dizer q está vivo ros::Rate loop_rate(1); //1 segundo /* Wait until all nodes are ready.. */ while(initialize){ ros::spinOnce(); loop_rate.sleep(); } /* Run Algorithm */ //instantaneous idleness and last visit initialized with zeros: double instantaneous_idleness [dimension]; double last_visit [dimension]; for(i=0;i<dimension;i++){ instantaneous_idleness[i]= 0.0; last_visit[i]= 0.0; if(i==current_vertex){ last_visit[i]= 0.1; //Avoids getting back at the initial vertex right away } } bool inpath = false; uint path [dimension]; uint elem_s_path=0, i_path=0; interference = false; ResendGoal = false; goal_complete = true; // printf("ID_ROBOT [2] = %d\n",ID_ROBOT); //-1 in the case there is only 1 robot. while(1){ if (goal_complete){ if (i_path>0){ //nao faz update no inicio //Update Idleness Table: double now = ros::Time::now().toSec(); for(i=0; i<dimension; i++){ if (i == next_vertex){ last_visit[i] = now; } instantaneous_idleness[i]= now - last_visit[i]; } current_vertex = next_vertex; //Show Idleness Table: /* for (i=0; i<dimension; i++){ printf("idleness[%u] = %f\n",i,instantaneous_idleness[i]); } */ } if(inpath){ //The robot is on its way to a global objective -> get NEXT_VERTEX from its path: i_path++; //desde que nao passe o tamanho do path if (i_path<elem_s_path){ next_vertex=path[i_path]; }else{ inpath = false; } } if (!inpath){ elem_s_path = heuristic_pathfinder_conscientious_cognitive(current_vertex, vertex_web, instantaneous_idleness, dimension, path); /* printf("Path: "); for (i=0;i<elem_s_path;i++){ if (i==elem_s_path-1){ printf("%u.\n",path[i]); }else{ printf("%u, ",path[i]); } } */ //we have the path and the number of elements in the path i_path=1; next_vertex = path[i_path]; inpath = true; // printf("Move Robot to Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y); } if (inpath){ geometry_msgs::Quaternion angle_quat = tf::createQuaternionMsgFromYaw(0.0); //Send the goal to the robot (Global Map) goal.target_pose.header.frame_id = "map"; goal.target_pose.header.stamp = ros::Time::now(); goal.target_pose.pose.position.x = vertex_web[next_vertex].x; goal.target_pose.pose.position.y = vertex_web[next_vertex].y; goal.target_pose.pose.orientation = angle_quat; //alpha -> orientação (queria optimizar este parametro -> através da direcção do vizinho!) ROS_INFO("Sending goal - Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y); ac.sendGoal(goal, boost::bind(&goalDoneCallback, _1, _2), boost::bind(&goalActiveCallback), boost::bind(&goalFeedbackCallback, _1)); //ac.sendGoal(goal); //ac.waitForResult(); } goal_complete = false; //garantir q n volta a entrar a seguir aqui // printf("ID_ROBOT [3] = %d\n",ID_ROBOT); //-1 in the case there is only 1 robot. }else{ //goal not complete (active) if (interference){ // Stop the robot.. ROS_INFO("Interference detected!\n"); send_interference(); //get own "odom" positions... ros::spinOnce(); //Waiting until conflict is solved... while(interference){ interference = check_interference(); if (goal_complete || ResendGoal){ interference = false; } } // se saiu é pq interference = false } if(ResendGoal){ geometry_msgs::Quaternion angle_quat = tf::createQuaternionMsgFromYaw(0.0); //Send the goal to the robot (Global Map) goal.target_pose.header.frame_id = "map"; goal.target_pose.header.stamp = ros::Time::now(); goal.target_pose.pose.position.x = vertex_web[next_vertex].x; goal.target_pose.pose.position.y = vertex_web[next_vertex].y; goal.target_pose.pose.orientation = angle_quat; //alpha -> orientação (queria optimizar este parametro -> através da direcção do vizinho!) ROS_INFO("Sending goal - Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y); // printf("ID_ROBOT = %d\n", ID_ROBOT); ac.sendGoal(goal, boost::bind(&goalDoneCallback, _1, _2), boost::bind(&goalActiveCallback), boost::bind(&goalFeedbackCallback, _1)); ResendGoal = false; //para nao voltar a entrar (envia goal so uma vez) } if(end_simulation){ return 0; } // printf("ID_ROBOT [4] = %d\n",ID_ROBOT); //-1 in the case there is only 1 robot. } } // return 0; }
int main(int argc, char** argv){ //pass the .graph file to open /* argc=3 argv[0]=/.../patrolling_sim/bin/GBS argv[1]=__name:=XXXXXX argv[2]=maps/1r-5-map.graph argv[3]=ID_ROBOT argv[4]=NUMBER_OF_ROBOTS //this is only necessary to automatically define G2 */ //More than One robot (ID between 0 and 99) if ( atoi(argv[3])>NUM_MAX_ROBOTS || atoi(argv[3])<-1 ){ ROS_INFO("The Robot's ID must be an integer number between 0 an 99"); //max 100 robots return 0; }else{ ID_ROBOT = atoi(argv[3]); //printf("ID_ROBOT = %d\n",ID_ROBOT); //-1 in the case there is only 1 robot. } uint i = strlen( argv[2] ); char graph_file[i]; strcpy (graph_file,argv[2]); //Check Graph Dimension: uint dimension = GetGraphDimension(graph_file); //Create Structure to save the Graph Info; vertex vertex_web[dimension]; //Get the Graph info from the Graph File GetGraphInfo(vertex_web, dimension, graph_file); /** Define G1 and G2 **/ double G1 = 0.1; int NUMBER_OF_ROBOTS = atoi(argv[4]); //default: double G2 = 100.0; double edge_min = 1.0; if ( strcmp (graph_file,"maps/grid/grid.graph") == 0 ){ if (NUMBER_OF_ROBOTS == 1){G2 = 20.54;} if (NUMBER_OF_ROBOTS == 2){G2 = 17.70;} if (NUMBER_OF_ROBOTS == 4){G2 = 11.15;} if (NUMBER_OF_ROBOTS == 6){G2 = 10.71;} if (NUMBER_OF_ROBOTS == 8){G2 = 10.29;} if (NUMBER_OF_ROBOTS == 12){G2 = 9.13;} }else if (strcmp (graph_file,"maps/example/example.graph") == 0 ) { if (NUMBER_OF_ROBOTS == 1){G2 = 220.0;} if (NUMBER_OF_ROBOTS == 2){G2 = 180.5;} if (NUMBER_OF_ROBOTS == 4){G2 = 159.3;} if (NUMBER_OF_ROBOTS == 6){G2 = 137.15;} if (NUMBER_OF_ROBOTS == 8 || NUMBER_OF_ROBOTS == 12){G2 = 126.1;} edge_min = 20.0; }else if (strcmp (graph_file,"maps/cumberland/cumberland.graph") == 0) { if (NUMBER_OF_ROBOTS == 1){G2 = 152.0;} if (NUMBER_OF_ROBOTS == 2){G2 = 100.4;} if (NUMBER_OF_ROBOTS == 4){G2 = 80.74;} if (NUMBER_OF_ROBOTS == 6){G2 = 77.0;} if (NUMBER_OF_ROBOTS == 8 || NUMBER_OF_ROBOTS == 12){G2 = 63.5;} edge_min = 50.0; } printf("G1 = %f, G2 = %f\n", G1, G2); /* Define Starting Vertex/Position (Launch File Parameters) */ ros::init(argc, argv, "c_reactive"); ros::NodeHandle nh; double initial_x, initial_y; XmlRpc::XmlRpcValue list; nh.getParam("initial_pos", list); ROS_ASSERT(list.getType() == XmlRpc::XmlRpcValue::TypeArray); int value = ID_ROBOT; if (value == -1){value = 0;} ROS_ASSERT(list[2*value].getType() == XmlRpc::XmlRpcValue::TypeDouble); initial_x = static_cast<double>(list[2*value]); ROS_ASSERT(list[2*value+1].getType() == XmlRpc::XmlRpcValue::TypeDouble); initial_y = static_cast<double>(list[2*value+1]); // printf("initial position: x = %f, y = %f\n", initial_x, initial_y); uint current_vertex = IdentifyVertex(vertex_web, dimension, initial_x, initial_y); // printf("initial vertex = %d\n\n",current_vertex); //INITIALIZE tab_intention: int tab_intention[NUMBER_OF_ROBOTS]; for (i=0; i<NUMBER_OF_ROBOTS; i++){ tab_intention[i] = -1; } //Publicar dados de "odom" para nó de posições odom_pub = nh.advertise<nav_msgs::Odometry>("positions", 1); //only concerned about the most recent //Subscrever posições de outros robots odom_sub = nh.subscribe("positions", 10, positionsCB); char string[20]; char string2[20]; if(ID_ROBOT==-1){ strcpy (string,"odom"); //string = "odom" strcpy (string2,"cmd_vel"); //string = "cmd_vel" TEAMSIZE = 1; }else{ strcpy (string,"robot_"); strcpy (string2,"robot_"); char id[3]; itoa(ID_ROBOT, id, 10); strcat(string,id); strcat(string2,id); strcat(string,"/odom"); //string = "robot_X/odom" strcat(string2,"/cmd_vel"); //string = "robot_X/cmd_vel" TEAMSIZE = ID_ROBOT + 1; } // printf("string de publicação da odometria: %s\n",string); //Cmd_vel to backup: cmd_vel_pub = nh.advertise<geometry_msgs::Twist>(string2, 1); //Subscrever para obter dados de "odom" do robot corrente ros::Subscriber sub; sub = nh.subscribe(string, 1, odomCB); //size of the buffer = 1 (?) ros::spinOnce(); /* Define Goal */ if(ID_ROBOT==-1){ strcpy (string,"move_base"); //string = "move_base }else{ strcpy (string,"robot_"); char id[3]; itoa(ID_ROBOT, id, 10); strcat(string,id); strcat(string,"/move_base"); //string = "robot_X/move_base" } //printf("string = %s\n",string); MoveBaseClient ac(string, true); //wait for the action server to come up while(!ac.waitForServer(ros::Duration(5.0))){ ROS_INFO("Waiting for the move_base action server to come up"); } //Define Goal: move_base_msgs::MoveBaseGoal goal; //Publicar dados para "results" results_pub = nh.advertise<geometry_msgs::PointStamped>("results", 1); //only concerned about the most recent results_sub = nh.subscribe("results", 10, resultsCB); //Subscrever "results" vindo dos robots initialize_node(); //dizer q está vivo ros::Rate loop_rate(1); //1 segundo /* Wait until all nodes are ready.. */ while(initialize){ ros::spinOnce(); loop_rate.sleep(); } /* Run Algorithm */ double instantaneous_idleness [dimension]; double last_visit [dimension]; for(i=0;i<dimension;i++){ instantaneous_idleness[i]= 0.0; last_visit[i]= 0.0; if(i==current_vertex){ last_visit[i]= 0.1; //Avoids getting back at the initial vertex } } interference = false; ResendGoal = false; goal_complete = true; double now; while(1){ if(goal_complete){ if(next_vertex>-1){ //Update Idleness Table: now = ros::Time::now().toSec(); for(i=0; i<dimension; i++){ if (i == next_vertex){ last_visit[i] = now; } instantaneous_idleness[i]= now - last_visit[i]; } current_vertex = next_vertex; //Show Idleness Table: /* for (i=0; i<dimension; i++){ printf("idleness[%u] = %f\n",i,instantaneous_idleness[i]); } */ } //devolver proximo vertex tendo em conta apenas as idlenesses; next_vertex = (int) state_exchange_bayesian_strategy(current_vertex, vertex_web, instantaneous_idleness, tab_intention, NUMBER_OF_ROBOTS, G1, G2, edge_min); //printf("Move Robot to Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y); /** SEND INTENTION **/ send_intention(next_vertex); //Send the goal to the robot (Global Map) geometry_msgs::Quaternion angle_quat = tf::createQuaternionMsgFromYaw(0.0); goal.target_pose.header.frame_id = "map"; goal.target_pose.header.stamp = ros::Time::now(); goal.target_pose.pose.position.x = vertex_web[next_vertex].x; goal.target_pose.pose.position.y = vertex_web[next_vertex].y; goal.target_pose.pose.orientation = angle_quat; ROS_INFO("Sending goal - Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y); ac.sendGoal(goal, boost::bind(&goalDoneCallback, _1, _2), boost::bind(&goalActiveCallback), boost::bind(&goalFeedbackCallback, _1)); //ac.waitForResult(); goal_complete = false; }else{ if (interference){ // Stop the robot.. ROS_INFO("Interference detected!\n"); send_interference(); //get own "odom" positions... ros::spinOnce(); //Waiting until conflict is solved... while(interference){ interference = check_interference(); if (goal_complete || ResendGoal){ interference = false; } } // se saiu é pq interference = false } if(ResendGoal){ //Send the goal to the robot (Global Map) geometry_msgs::Quaternion angle_quat = tf::createQuaternionMsgFromYaw(0.0); goal.target_pose.header.frame_id = "map"; goal.target_pose.header.stamp = ros::Time::now(); goal.target_pose.pose.position.x = vertex_web[next_vertex].x; goal.target_pose.pose.position.y = vertex_web[next_vertex].y; goal.target_pose.pose.orientation = angle_quat; //alpha -> orientação (queria optimizar este parametro -> através da direcção do vizinho!) ROS_INFO("Sending goal - Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y); // printf("ID_ROBOT = %d\n", ID_ROBOT); ac.sendGoal(goal, boost::bind(&goalDoneCallback, _1, _2), boost::bind(&goalActiveCallback), boost::bind(&goalFeedbackCallback, _1)); ResendGoal = false; //para nao voltar a entrar (envia goal so uma vez) } if (arrived && NUMBER_OF_ROBOTS>1){ //a different robot arrived at a vertex: update idleness table and keep track of last vertices positions of other robots. //printf("Robot %d reached Goal %d.\n", robot_arrived, vertex_arrived); //Update Idleness Table: now = ros::Time::now().toSec(); for(i=0; i<dimension; i++){ if (i == vertex_arrived){ //actualizar last_visit[dimension] last_visit[vertex_arrived] = now; } //actualizar instantaneous_idleness[dimension] instantaneous_idleness[i]= now - last_visit[i]; } //Show Idleness Table: // for (i=0; i<dimension; i++){ // printf("idleness[%u] = %f\n",i,instantaneous_idleness[i]); // } arrived = false; } if (intention && NUMBER_OF_ROBOTS>1){ tab_intention[robot_intention] = vertex_intention; //printf("tab_intention[ID=%d]=%d\n",robot_intention,tab_intention[robot_intention]); intention = false; } if(end_simulation){ return 0; } } } return 0; }
int CHOLMOD(rowcolcounts) ( /* ---- input ---- */ cholmod_sparse *A, /* matrix to analyze */ Int *fset, /* subset of 0:(A->ncol)-1 */ size_t fsize, /* size of fset */ Int *Parent, /* size nrow. Parent [i] = p if p is the parent of i */ Int *Post, /* size nrow. Post [k] = i if i is the kth node in * the postordered etree. */ /* ---- output --- */ Int *RowCount, /* size nrow. RowCount [i] = # entries in the ith row of * L, including the diagonal. */ Int *ColCount, /* size nrow. ColCount [i] = # entries in the ith * column of L, including the diagonal. */ Int *First, /* size nrow. First [i] = k is the least postordering * of any descendant of i. */ Int *Level, /* size nrow. Level [i] is the length of the path from * i to the root, with Level [root] = 0. */ /* --------------- */ cholmod_common *Common ) { double fl, ff ; Int *Ap, *Ai, *Anz, *PrevNbr, *SetParent, *Head, *PrevLeaf, *Anext, *Ipost, *Iwork ; Int i, j, r, k, len, s, p, pend, inew, stype, nf, anz, inode, parent, nrow, ncol, packed, use_fset, jj ; size_t w ; int ok = TRUE ; /* ---------------------------------------------------------------------- */ /* check inputs */ /* ---------------------------------------------------------------------- */ RETURN_IF_NULL_COMMON (FALSE) ; RETURN_IF_NULL (A, FALSE) ; RETURN_IF_NULL (Parent, FALSE) ; RETURN_IF_NULL (Post, FALSE) ; RETURN_IF_NULL (ColCount, FALSE) ; RETURN_IF_NULL (First, FALSE) ; RETURN_IF_NULL (Level, FALSE) ; RETURN_IF_XTYPE_INVALID (A, CHOLMOD_PATTERN, CHOLMOD_ZOMPLEX, FALSE) ; stype = A->stype ; if (stype > 0) { /* symmetric with upper triangular part not supported */ ERROR (CHOLMOD_INVALID, "symmetric upper not supported") ; return (FALSE) ; } Common->status = CHOLMOD_OK ; /* ---------------------------------------------------------------------- */ /* allocate workspace */ /* ---------------------------------------------------------------------- */ nrow = A->nrow ; /* the number of rows of A */ ncol = A->ncol ; /* the number of columns of A */ /* w = 2*nrow + (stype ? 0 : ncol) */ w = CHOLMOD(mult_size_t) (nrow, 2, &ok) ; w = CHOLMOD(add_size_t) (w, (stype ? 0 : ncol), &ok) ; if (!ok) { ERROR (CHOLMOD_TOO_LARGE, "problem too large") ; return (FALSE) ; } CHOLMOD(allocate_work) (nrow, w, 0, Common) ; if (Common->status < CHOLMOD_OK) { return (FALSE) ; } ASSERT (CHOLMOD(dump_perm) (Post, nrow, nrow, "Post", Common)) ; ASSERT (CHOLMOD(dump_parent) (Parent, nrow, "Parent", Common)) ; /* ---------------------------------------------------------------------- */ /* get inputs */ /* ---------------------------------------------------------------------- */ Ap = A->p ; /* size ncol+1, column pointers for A */ Ai = A->i ; /* the row indices of A, of size nz=Ap[ncol+1] */ Anz = A->nz ; packed = A->packed ; ASSERT (IMPLIES (!packed, Anz != NULL)) ; /* ---------------------------------------------------------------------- */ /* get workspace */ /* ---------------------------------------------------------------------- */ Iwork = Common->Iwork ; SetParent = Iwork ; /* size nrow (i/i/l) */ PrevNbr = Iwork + nrow ; /* size nrow (i/i/l) */ Anext = Iwork + 2*((size_t) nrow) ; /* size ncol (i/i/l) (unsym only) */ PrevLeaf = Common->Flag ; /* size nrow */ Head = Common->Head ; /* size nrow+1 (unsym only)*/ /* ---------------------------------------------------------------------- */ /* find the first descendant and level of each node in the tree */ /* ---------------------------------------------------------------------- */ /* First [i] = k if the postordering of first descendent of node i is k */ /* Level [i] = length of path from node i to the root (Level [root] = 0) */ for (i = 0 ; i < nrow ; i++) { First [i] = EMPTY ; } /* postorder traversal of the etree */ for (k = 0 ; k < nrow ; k++) { /* node i of the etree is the kth node in the postordered etree */ i = Post [k] ; /* i is a leaf if First [i] is still EMPTY */ /* ColCount [i] starts at 1 if i is a leaf, zero otherwise */ ColCount [i] = (First [i] == EMPTY) ? 1 : 0 ; /* traverse the path from node i to the root, stopping if we find a * node r whose First [r] is already defined. */ len = 0 ; for (r = i ; (r != EMPTY) && (First [r] == EMPTY) ; r = Parent [r]) { First [r] = k ; len++ ; } if (r == EMPTY) { /* we hit a root node, the level of which is zero */ len-- ; } else { /* we stopped at node r, where Level [r] is already defined */ len += Level [r] ; } /* re-traverse the path from node i to r; set the level of each node */ for (s = i ; s != r ; s = Parent [s]) { Level [s] = len-- ; } } /* ---------------------------------------------------------------------- */ /* AA' case: sort columns of A according to first postordered row index */ /* ---------------------------------------------------------------------- */ fl = 0.0 ; if (stype == 0) { /* [ use PrevNbr [0..nrow-1] as workspace for Ipost */ Ipost = PrevNbr ; /* Ipost [i] = k if i is the kth node in the postordered etree. */ for (k = 0 ; k < nrow ; k++) { Ipost [Post [k]] = k ; } use_fset = (fset != NULL) ; if (use_fset) { nf = fsize ; /* clear Anext to check fset */ for (j = 0 ; j < ncol ; j++) { Anext [j] = -2 ; } /* find the first postordered row in each column of A (post,f) * and place the column in the corresponding link list */ for (jj = 0 ; jj < nf ; jj++) { j = fset [jj] ; if (j < 0 || j > ncol || Anext [j] != -2) { /* out-of-range or duplicate entry in fset */ ERROR (CHOLMOD_INVALID, "fset invalid") ; return (FALSE) ; } /* flag column j as having been seen */ Anext [j] = EMPTY ; } /* fset is now valid */ ASSERT (CHOLMOD(dump_perm) (fset, nf, ncol, "fset", Common)) ; } else { nf = ncol ; } for (jj = 0 ; jj < nf ; jj++) { j = (use_fset) ? (fset [jj]) : jj ; /* column j is in the fset; find the smallest row (if any) */ p = Ap [j] ; pend = (packed) ? (Ap [j+1]) : (p + Anz [j]) ; ff = (double) MAX (0, pend - p) ; fl += ff*ff + ff ; if (pend > p) { k = Ipost [Ai [p]] ; for ( ; p < pend ; p++) { inew = Ipost [Ai [p]] ; k = MIN (k, inew) ; } /* place column j in link list k */ ASSERT (k >= 0 && k < nrow) ; Anext [j] = Head [k] ; Head [k] = j ; } } /* Ipost no longer needed for inverse postordering ] * Head [k] contains a link list of all columns whose first * postordered row index is equal to k, for k = 0 to nrow-1. */ } /* ---------------------------------------------------------------------- */ /* compute the row counts and node weights */ /* ---------------------------------------------------------------------- */ if (RowCount != NULL) { for (i = 0 ; i < nrow ; i++) { RowCount [i] = 1 ; } } for (i = 0 ; i < nrow ; i++) { PrevLeaf [i] = EMPTY ; PrevNbr [i] = EMPTY ; SetParent [i] = i ; /* every node is in its own set, by itself */ } if (stype != 0) { /* ------------------------------------------------------------------ */ /* symmetric case: LL' = A */ /* ------------------------------------------------------------------ */ /* also determine the number of entries in triu(A) */ anz = nrow ; for (k = 0 ; k < nrow ; k++) { /* j is the kth node in the postordered etree */ j = initialize_node (k, Post, Parent, ColCount, PrevNbr) ; /* for all nonzeros A(i,j) below the diagonal, in column j of A */ p = Ap [j] ; pend = (packed) ? (Ap [j+1]) : (p + Anz [j]) ; for ( ; p < pend ; p++) { i = Ai [p] ; if (i > j) { /* j is a descendant of i in etree(A) */ anz++ ; process_edge (j, i, k, First, PrevNbr, ColCount, PrevLeaf, RowCount, SetParent, Level) ; } } /* update SetParent: UNION (j, Parent [j]) */ finalize_node (j, Parent, SetParent) ; } Common->anz = anz ; } else { /* ------------------------------------------------------------------ */ /* unsymmetric case: LL' = AA' */ /* ------------------------------------------------------------------ */ for (k = 0 ; k < nrow ; k++) { /* inode is the kth node in the postordered etree */ inode = initialize_node (k, Post, Parent, ColCount, PrevNbr) ; /* for all cols j whose first postordered row is k: */ for (j = Head [k] ; j != EMPTY ; j = Anext [j]) { /* k is the first postordered row in column j of A */ /* for all rows i in column j: */ p = Ap [j] ; pend = (packed) ? (Ap [j+1]) : (p + Anz [j]) ; for ( ; p < pend ; p++) { i = Ai [p] ; /* has i already been considered at this step k */ if (PrevNbr [i] < k) { /* inode is a descendant of i in etree(AA') */ /* process edge (inode,i) and set PrevNbr[i] to k */ process_edge (inode, i, k, First, PrevNbr, ColCount, PrevLeaf, RowCount, SetParent, Level) ; } } } /* clear link list k */ Head [k] = EMPTY ; /* update SetParent: UNION (inode, Parent [inode]) */ finalize_node (inode, Parent, SetParent) ; } } /* ---------------------------------------------------------------------- */ /* finish computing the column counts */ /* ---------------------------------------------------------------------- */ for (j = 0 ; j < nrow ; j++) { parent = Parent [j] ; if (parent != EMPTY) { /* add the ColCount of j to its parent */ ColCount [parent] += ColCount [j] ; } } /* ---------------------------------------------------------------------- */ /* clear workspace */ /* ---------------------------------------------------------------------- */ Common->mark = EMPTY ; /* CHOLMOD(clear_flag) (Common) ; */ CHOLMOD_CLEAR_FLAG (Common) ; ASSERT (CHOLMOD(dump_work) (TRUE, TRUE, 0, Common)) ; /* ---------------------------------------------------------------------- */ /* flop count and nnz(L) for subsequent LL' numerical factorization */ /* ---------------------------------------------------------------------- */ /* use double to avoid integer overflow. lnz cannot be NaN. */ Common->aatfl = fl ; Common->lnz = 0. ; fl = 0 ; for (j = 0 ; j < nrow ; j++) { ff = (double) (ColCount [j]) ; Common->lnz += ff ; fl += ff*ff ; } Common->fl = fl ; PRINT1 (("rowcol fl %g lnz %g\n", Common->fl, Common->lnz)) ; return (TRUE) ; }
void init(trie_node_adt *n) { n->pointer = initialize_node(); }
int main (int argc, const char * argv[]) { FILE* file; char *current, *rest, *type, *ip_pointer; unsigned int a, b, c, d, metric; int prefix, NIC, NIC_num, id; Node* tree = (Node*) (malloc(sizeof(Node))); current = (char*) (malloc(128*sizeof(char))); /* Check arguments */ if (argc != 2) { fprintf(stderr, "Invalid arguments. You should provide a single argument.\n"); return 1; } // Open the file //file = fopen ("/Users/William/Documents/CPSC 317/a2/ip_route/ip_route/p2_medium_input.txt","r"); file = fopen(argv[1], "r"); // Check for errors when opening the file if (!file){ perror("Invalid file"); return 2; } // get the total amount of NIC fgets(current, 35, file); NIC_num = atoi(current); fgets(current, 35, file); type = strtok_r(current, " ", &rest); // assuming the first line after NIC number is of type "U" // used to initial the tree (myTree need to initial the root) if (strcmp(type, "U")==0){ ip_pointer = strtok_r(rest, ".", &rest); a = atoi(ip_pointer); ip_pointer = strtok_r(rest, ".",&rest); b = atoi(ip_pointer); ip_pointer = strtok_r(rest, ".", &rest); c = atoi(ip_pointer); ip_pointer = strtok_r(rest, "/",&rest); d = atoi(ip_pointer); ip_pointer = strtok_r(rest, " ",&rest); prefix = atoi(ip_pointer); ip_pointer = strtok_r(rest, " ",&rest); NIC = atoi(ip_pointer); ip_pointer = strtok_r(rest, " ",&rest); metric = atoi(ip_pointer); ip_pointer = strtok_r(rest, " ",&rest); id = atoi(ip_pointer); initialize_node(tree, a); tree = add_leaf(tree, a, b, c, d, prefix, NIC, metric, NIC_num); fprintf(stdout, "%s %d.%d.%d.%d/%d %d %d\n", "A", a, b, c, d, prefix, metric+1, id); } while (fgets(current, 35, file)!=NULL) { type = strtok_r(current, " ", &rest); // do the table update if is type "U" if (strcmp(type, "U")==0){ ip_pointer = strtok_r(rest, ".", &rest); a = atoi(ip_pointer); ip_pointer = strtok_r(rest, ".",&rest); b = atoi(ip_pointer); ip_pointer = strtok_r(rest, ".", &rest); c = atoi(ip_pointer); ip_pointer = strtok_r(rest, "/",&rest); d = atoi(ip_pointer); ip_pointer = strtok_r(rest, " ",&rest); prefix = atoi(ip_pointer); ip_pointer = strtok_r(rest, " \n",&rest); NIC = atoi(ip_pointer); ip_pointer = strtok_r(rest, " ",&rest); metric = atoi(ip_pointer); ip_pointer = strtok_r(rest, " ",&rest); id = atoi(ip_pointer); Leaf* leaf = find_leaf(tree, a, b, c, d,prefix); // if the address is not found, add that ip to the table if (leaf==NULL){ tree = add_leaf(tree, a, b, c, d, prefix, NIC, metric, NIC_num); fprintf(stdout, "%s %d.%d.%d.%d/%d %d %d\n", "A", a, b, c, d, prefix, metric+1, id); } // else update the table else{ if (update_leaf_nic(leaf, NIC, metric, NIC_num)==1) fprintf(stdout, "%s %d.%d.%d.%d/%d %d %d\n", "A", a, b, c, d, prefix, leaf->metric, id); } } } // check errors after opening the file if (ferror(file)) { perror("Error reading file"); fclose(file); return 3; } fprintf(stdout, "\n"); // printing the tree char* my_printer = (char*) (malloc(20*sizeof(char))); print_tree(tree, my_printer); fclose(file); return 0; }
PerformanceData run( const typename FixtureType::FEMeshType & mesh , const int global_max_x , const int global_max_y , const int global_max_z , const unsigned uq_count , const int steps , const int print_sample ) { typedef Scalar scalar_type ; typedef FixtureType fixture_type ; typedef typename fixture_type::device_type device_type ; enum { ElementNodeCount = fixture_type::element_node_count }; const int total_num_steps = steps ; const Scalar user_dt = 5.0e-6; //const Scalar end_time = 0.0050; // element block parameters const Scalar lin_bulk_visc = 0.0; const Scalar quad_bulk_visc = 0.0; // const Scalar lin_bulk_visc = 0.06; // const Scalar quad_bulk_visc = 1.2; // const Scalar hg_stiffness = 0.0; // const Scalar hg_viscosity = 0.0; // const Scalar hg_stiffness = 0.03; // const Scalar hg_viscosity = 0.001; // material properties const Scalar youngs_modulus=1.0e6; const Scalar poissons_ratio=0.0; const Scalar density = 8.0e-4; const comm::Machine machine = mesh.parallel_data_map.machine ; PerformanceData perf_data ; Kokkos::Impl::Timer wall_clock ; //------------------------------------ // Generate fields typedef Fields< scalar_type , device_type > fields_type ; fields_type mesh_fields( mesh , uq_count , lin_bulk_visc , quad_bulk_visc , youngs_modulus , poissons_ratio , density ); typename fields_type::node_coords_type::HostMirror model_coords_h = Kokkos::create_mirror( mesh_fields.model_coords ); typename fields_type::spatial_precise_view::HostMirror displacement_h = Kokkos::create_mirror( mesh_fields.displacement ); typename fields_type::spatial_precise_view::HostMirror velocity_h = Kokkos::create_mirror( mesh_fields.velocity ); Kokkos::deep_copy( model_coords_h , mesh_fields.model_coords ); //------------------------------------ // Initialization initialize_element( mesh_fields ); initialize_node( mesh_fields ); const Scalar x_bc = global_max_x ; // Initial condition on velocity to initiate a pulse along the X axis { const unsigned X = 0; for ( unsigned inode = 0; inode< mesh_fields.num_nodes; ++inode) { for ( unsigned kq = 0 ; kq < uq_count ; ++kq ) { if ( model_coords_h(inode,X) == 0 ) { velocity_h(inode,kq,X) = 1000 + 100 * kq ; velocity_h(inode,kq,X) = 1000 + 100 * kq ; } } } } Kokkos::deep_copy( mesh_fields.velocity , velocity_h ); Kokkos::deep_copy( mesh_fields.velocity_new , velocity_h ); //-------------------------------------------------------------------------- // We will call a sequence of functions. These functions have been // grouped into several functors to balance the number of global memory // accesses versus requiring too many registers or too much L1 cache. // Global memory accees have read/write cost and memory subsystem contention cost. //-------------------------------------------------------------------------- perf_data.init_time = comm::max( machine , wall_clock.seconds() ); // Parameters required for the internal force computations. perf_data.number_of_steps = total_num_steps ; typedef typename fields_type::spatial_precise_view::scalar_type comm_value_type ; const unsigned comm_value_count = 6 ; Kokkos::AsyncExchange< comm_value_type , device_type , Kokkos::ParallelDataMap > comm_exchange( mesh.parallel_data_map , comm_value_count * uq_count ); for ( int step = 0; step < total_num_steps; ++step ) { //------------------------------------------------------------------------ // rotate the state variable views. swap( mesh_fields.dt , mesh_fields.dt_new ); swap( mesh_fields.displacement , mesh_fields.displacement_new ); swap( mesh_fields.velocity , mesh_fields.velocity_new ); swap( mesh_fields.rotation , mesh_fields.rotation_new ); //------------------------------------------------------------------------ // Communicate "send" nodes' displacement and velocity next_state // to the ghosted nodes. // buffer packages: { { dx , dy , dz , vx , vy , vz }_node } wall_clock.reset(); pack_state( mesh_fields , comm_exchange.buffer(), mesh.parallel_data_map.count_interior , mesh.parallel_data_map.count_send ); comm_exchange.setup(); comm_exchange.send_receive(); unpack_state( mesh_fields , comm_exchange.buffer() , mesh.parallel_data_map.count_owned , mesh.parallel_data_map.count_receive ); device_type::fence(); perf_data.comm_time += comm::max( machine , wall_clock.seconds() ); //------------------------------------------------------------------------ wall_clock.reset(); // First kernel 'grad_hgop' combines two functions: // gradient, velocity gradient gradient( mesh_fields ); // Combine tensor decomposition and rotation functions. decomp_rotate( mesh_fields ); internal_force( mesh_fields , user_dt ); device_type::fence(); perf_data.internal_force_time += comm::max( machine , wall_clock.seconds() ); //------------------------------------------------------------------------ // Assembly of elements' contributions to nodal force into // a nodal force vector. Update the accelerations, velocities, // displacements. // The same pattern can be used for matrix-free residual computations. wall_clock.reset(); nodal_update( mesh_fields , x_bc ); device_type::fence(); perf_data.central_diff += comm::max( machine , wall_clock.seconds() ); if ( print_sample && 0 == step % 100 ) { Kokkos::deep_copy( displacement_h , mesh_fields.displacement_new ); Kokkos::deep_copy( velocity_h , mesh_fields.velocity_new ); if ( 1 == print_sample ) { for ( unsigned kp = 0 ; kp < uq_count ; ++kp ) { std::cout << "step " << step << " : displacement({*,0,0}," << kp << ",0) =" ; for ( unsigned i = 0 ; i < mesh_fields.num_nodes_owned ; ++i ) { if ( model_coords_h(i,1) == 0 && model_coords_h(i,2) == 0 ) { std::cout << " " << displacement_h(i,kp,0); } } std::cout << std::endl ; const float tol = 1.0e-6 ; const int yb = global_max_y ; const int zb = global_max_z ; std::cout << "step " << step << " : displacement({*," << yb << "," << zb << "}," << kp << ",0) =" ; for ( unsigned i = 0 ; i < mesh_fields.num_nodes_owned ; ++i ) { if ( fabs( model_coords_h(i,1) - yb ) < tol && fabs( model_coords_h(i,2) - zb ) < tol ) { std::cout << " " << displacement_h(i,kp,0); } } std::cout << std::endl ; } } else if ( 2 == print_sample ) { const unsigned kp = 0 ; const float tol = 1.0e-6 ; const int xb = global_max_x / 2 ; const int yb = global_max_y / 2 ; const int zb = global_max_z / 2 ; for ( unsigned i = 0 ; i < mesh_fields.num_nodes_owned ; ++i ) { if ( fabs( model_coords_h(i,0) - xb ) < tol && fabs( model_coords_h(i,1) - yb ) < tol && fabs( model_coords_h(i,2) - zb ) < tol ) { std::cout << "step " << step << " : displacement(" << xb << "," << yb << "," << zb << ") = {" << std::setprecision(6) << " " << displacement_h(i,kp,0) << std::setprecision(2) << " " << displacement_h(i,kp,1) << std::setprecision(2) << " " << displacement_h(i,kp,2) << " }" << std::endl ; } } } } } return perf_data ; }