Beispiel #1
0
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);
}
Beispiel #2
0
// 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;
}
Beispiel #3
0
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);
    }
}
Beispiel #4
0
/**
 * 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;
}
Beispiel #5
0
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);
		}
	}
}
Beispiel #6
0
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);
		}
	}
}
Beispiel #7
0
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);
	}
Beispiel #9
0
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; 
}
Beispiel #11
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) ;
}
Beispiel #13
0
void init(trie_node_adt *n) {
	n->pointer = initialize_node();
}
Beispiel #14
0
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;
    
}
Beispiel #15
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 ;
}