예제 #1
0
void ipc_gps_gprmc_handler( carmen_gps_gprmc_message *gps_data)
{
  fprintf(stderr, "g");
  carmen_logwrite_write_gps_gprmc(gps_data, outfile, carmen_get_time() - logger_starttime);
}
예제 #2
0
void laser_laser5_handler(carmen_laser_laser_message *laser)
{
  fprintf(stderr, "5");
  carmen_logwrite_write_laser_laser(laser, 5, outfile,
				    carmen_get_time() - logger_starttime);
}
예제 #3
0
void localize_handler(carmen_localize_globalpos_message *msg)
{
  fprintf(stderr, "L");
  carmen_logwrite_write_localize(msg, outfile, carmen_get_time() - 
				 logger_starttime);
}
예제 #4
0
void base_odometry_handler(carmen_base_odometry_message *odometry)
{
  fprintf(stderr, "O");
  carmen_logwrite_write_odometry(odometry, outfile, 
				 carmen_get_time() - logger_starttime);
}
예제 #5
0
void publish_xsens_global(xsens_global data, /*CmtOutputMode& mode,*/ CmtOutputSettings& settings)
{

    IPC_RETURN_TYPE err;

    switch (settings & CMT_OUTPUTSETTINGS_ORIENTMODE_MASK) {

		case CMT_OUTPUTSETTINGS_ORIENTMODE_QUATERNION:

            carmen_xsens_global_quat_message xsens_quat_message;

            //Acceleration
            xsens_quat_message.m_acc.x = data.acc.x;
            xsens_quat_message.m_acc.y = data.acc.y;
            xsens_quat_message.m_acc.z = data.acc.z;

            //Gyro
            xsens_quat_message.m_gyr.x = data.gyr.x;
            xsens_quat_message.m_gyr.y = data.gyr.y;
            xsens_quat_message.m_gyr.z = data.gyr.z;

            //Magnetism
            xsens_quat_message.m_mag.x = data.mag.x;
            xsens_quat_message.m_mag.y = data.mag.y;
            xsens_quat_message.m_mag.z = data.mag.z;

            xsens_quat_message.quat_data.m_data[0] = data.quatData.data[0];
            xsens_quat_message.quat_data.m_data[1] = data.quatData.data[1];
            xsens_quat_message.quat_data.m_data[2] = data.quatData.data[2];
            xsens_quat_message.quat_data.m_data[3] = data.quatData.data[3];

            //Temperature and Message Count
            xsens_quat_message.m_temp = data.temp;
            xsens_quat_message.m_count = data.count;

            //Timestamp
            xsens_quat_message.timestamp = carmen_get_time();

            //Host
            xsens_quat_message.host = carmen_get_host();

            err = IPC_publishData(CARMEN_XSENS_GLOBAL_QUAT_NAME, &xsens_quat_message);
            carmen_test_ipc_exit(err, "Could not publish", CARMEN_XSENS_GLOBAL_QUAT_NAME);

			break;

		case CMT_OUTPUTSETTINGS_ORIENTMODE_EULER:

            carmen_xsens_global_euler_message xsens_euler_message;

            //Acceleration
            xsens_euler_message.m_acc.x = data.acc.x;
            xsens_euler_message.m_acc.y = data.acc.y;
            xsens_euler_message.m_acc.z = data.acc.z;

            //Gyro
            xsens_euler_message.m_gyr.x = data.gyr.x;
            xsens_euler_message.m_gyr.y = data.gyr.y;
            xsens_euler_message.m_gyr.z = data.gyr.z;

            //Magnetism
            xsens_euler_message.m_mag.x = data.mag.x;
            xsens_euler_message.m_mag.y = data.mag.y;
            xsens_euler_message.m_mag.z = data.mag.z;

            xsens_euler_message.euler_data.m_pitch = data.eulerData.pitch;
            xsens_euler_message.euler_data.m_roll = data.eulerData.roll;
            xsens_euler_message.euler_data.m_yaw = data.eulerData.yaw;


            //Temperature and Message Count
            xsens_euler_message.m_temp = data.temp;
            xsens_euler_message.m_count = data.count;

            //Timestamp
            xsens_euler_message.timestamp = carmen_get_time();

            //Host
            xsens_euler_message.host = carmen_get_host();

            err = IPC_publishData(CARMEN_XSENS_GLOBAL_EULER_NAME, &xsens_euler_message);
            carmen_test_ipc_exit(err, "Could not publish", CARMEN_XSENS_GLOBAL_EULER_NAME);

            break;

		case CMT_OUTPUTSETTINGS_ORIENTMODE_MATRIX:

			carmen_xsens_global_matrix_message xsens_matrix_message;

			//Acceleration
            xsens_matrix_message.m_acc.x = data.acc.x;
            xsens_matrix_message.m_acc.y = data.acc.y;
            xsens_matrix_message.m_acc.z = data.acc.z;

            //Gyro
            xsens_matrix_message.m_gyr.x = data.gyr.x;
            xsens_matrix_message.m_gyr.y = data.gyr.y;
            xsens_matrix_message.m_gyr.z = data.gyr.z;

            //Magnetism
            xsens_matrix_message.m_mag.x = data.mag.x;
            xsens_matrix_message.m_mag.y = data.mag.y;
            xsens_matrix_message.m_mag.z = data.mag.z;


            xsens_matrix_message.matrix_data.m_data[0][0] = data.matrixData.data[0][0];
            xsens_matrix_message.matrix_data.m_data[0][1] = data.matrixData.data[0][1];
            xsens_matrix_message.matrix_data.m_data[0][2] = data.matrixData.data[0][2];

            xsens_matrix_message.matrix_data.m_data[1][0] = data.matrixData.data[1][0];
            xsens_matrix_message.matrix_data.m_data[1][1] = data.matrixData.data[1][1];
            xsens_matrix_message.matrix_data.m_data[1][2] = data.matrixData.data[1][2];

            xsens_matrix_message.matrix_data.m_data[2][0] = data.matrixData.data[2][0];
            xsens_matrix_message.matrix_data.m_data[2][1] = data.matrixData.data[2][1];
            xsens_matrix_message.matrix_data.m_data[2][2] = data.matrixData.data[2][2];

            //Temperature and Message Count
            xsens_matrix_message.m_temp = data.temp;
            xsens_matrix_message.m_count = data.count;

            //Timestamp
            xsens_matrix_message.timestamp = carmen_get_time();
	    xsens_matrix_message.timestamp2 = data.timestamp;

            //Host
            xsens_matrix_message.host = carmen_get_host();

            err = IPC_publishData(CARMEN_XSENS_GLOBAL_MATRIX_NAME, &xsens_matrix_message);
            carmen_test_ipc_exit(err, "Could not publish", CARMEN_XSENS_GLOBAL_MATRIX_NAME);

			break;
      
		default:
            carmen_die("Unknown settings in publish_xsens_global()\n");
      break;
	}
}
예제 #6
0
int main(int argc, char **argv)
{
  char filename[1024];
  char key;

  /* initialize connection to IPC network */
  carmen_ipc_initialize(argc, argv);
  carmen_param_check_version(argv[0]);	

  /* open logfile, check if file overwrites something */
  if(argc < 2) 
    carmen_die("usage: %s <logfile>\n", argv[0]);
  strcpy(filename, argv[1]);

  outfile = carmen_fopen(filename, "r");
  if (outfile != NULL) {
    fprintf(stderr, "Overwrite %s? ", filename);
    scanf("%c", &key);
    if (toupper(key) != 'Y')
      exit(-1);
    carmen_fclose(outfile);
  }
  outfile = carmen_fopen(filename, "w");
  if(outfile == NULL)
    carmen_die("Error: Could not open file %s for writing.\n", filename);
  carmen_logwrite_write_header(outfile);


  get_logger_params(argc, argv);

  if  ( !(log_odometry && log_laser && log_robot_laser ) )
    carmen_warn("\nWARNING: You are neither logging laser nor odometry messages!\n");
  


  if (log_params)
    get_all_params();

  register_ipc_messages();


  if (log_odometry) 
    carmen_base_subscribe_odometry_message(NULL, (carmen_handler_t)
					   base_odometry_handler, 
					   CARMEN_SUBSCRIBE_ALL);

  if (log_robot_laser) {
    carmen_robot_subscribe_frontlaser_message(NULL, (carmen_handler_t)
					      robot_frontlaser_handler, 
					      CARMEN_SUBSCRIBE_ALL);
    carmen_robot_subscribe_rearlaser_message(NULL, (carmen_handler_t)
					     robot_rearlaser_handler, 
					     CARMEN_SUBSCRIBE_ALL);
  }

  if (log_laser) {
    carmen_laser_subscribe_laser1_message(NULL, (carmen_handler_t)
					  laser_laser1_handler, 
					  CARMEN_SUBSCRIBE_ALL);
    carmen_laser_subscribe_laser2_message(NULL, (carmen_handler_t)
					  laser_laser2_handler, 
					  CARMEN_SUBSCRIBE_ALL);
    carmen_laser_subscribe_laser3_message(NULL, (carmen_handler_t)
					  laser_laser3_handler, 
					  CARMEN_SUBSCRIBE_ALL);
    carmen_laser_subscribe_laser4_message(NULL, (carmen_handler_t)
					  laser_laser4_handler, 
					  CARMEN_SUBSCRIBE_ALL);
    carmen_laser_subscribe_laser5_message(NULL, (carmen_handler_t)
					  laser_laser5_handler, 
					  CARMEN_SUBSCRIBE_ALL);
  }

  if (log_localize) {	
    carmen_localize_subscribe_globalpos_message(NULL, (carmen_handler_t)
						localize_handler,
						CARMEN_SUBSCRIBE_ALL);
  }

  if (log_simulator) {

    carmen_simulator_subscribe_truepos_message(NULL, (carmen_handler_t)  
					       carmen_simulator_truepos_handler,
					       CARMEN_SUBSCRIBE_ALL);
  }
  
  
  if (log_gps) {
    carmen_gps_subscribe_nmea_message( NULL,
				       (carmen_handler_t) ipc_gps_gpgga_handler,
				       CARMEN_SUBSCRIBE_ALL );
    
    carmen_gps_subscribe_nmea_rmc_message( NULL,
					   (carmen_handler_t) ipc_gps_gprmc_handler,
					   CARMEN_SUBSCRIBE_ALL );
  }

  if (log_smart) {
    smart_subscribe_status_message( NULL,
               (carmen_handler_t) smart_status_handler,
               CARMEN_SUBSCRIBE_ALL );
  }

  if (log_gyro) {
    gyro_subscribe_integrated_message( NULL,
               (carmen_handler_t) gyro_integrated_handler,
               CARMEN_SUBSCRIBE_ALL );
  }

  signal(SIGINT, shutdown_module);
  logger_starttime = carmen_get_time();
  carmen_ipc_dispatch();
  return 0;
}
예제 #7
0
void 
carmen_conventional_dynamic_program(int goal_x, int goal_y) 
{
  double *utility_ptr;
  int index;
  double max_val, min_val;
  int num_expanded;
  int done;

  struct timeval start_time, end_time;
  int delta_sec, delta_usec;
  static double last_time, cur_time, last_print;
  
  queue state_queue;
  state_ptr current_state;

  if (costs == NULL)
    return;

  gettimeofday(&start_time, NULL);

  cur_time = carmen_get_time();
  if ((int)(cur_time - last_print) > 10) {
    carmen_verbose("Time since last DP: %d secs, %d usecs\n", (int)(cur_time - last_time),
		   ((int)((cur_time-last_time)*1e6)) % 1000000);
    last_print = cur_time;
  }
  last_time = cur_time;

  if (utility == NULL) {
    utility = (double *)calloc(x_size*y_size, sizeof(double));
    carmen_test_alloc(utility);
  }
  
  utility_ptr = utility;
  for (index = 0; index < x_size * y_size; index++) 
    *(utility_ptr++) = -1;

  if (is_out_of_map(goal_x, goal_y))
    return;

  max_val = -MAXDOUBLE;
  min_val = MAXDOUBLE; 
  done = 0;

  state_queue = make_queue();

  current_state = carmen_conventional_create_state(goal_x, goal_y, 0);
  max_val = MAX_UTILITY;
  *(utility_value(goal_x, goal_y)) = max_val;
  add_neighbours_to_queue(goal_x, goal_y, state_queue);    
  num_expanded = 1;
  
  while ((current_state = pop_queue(state_queue)) != NULL) {
    num_expanded++;
    if (current_state->cost <= *(utility_value(current_state->x, current_state->y)))
      add_neighbours_to_queue(current_state->x, current_state->y, state_queue);
    if (current_state->cost < min_val)
      min_val = current_state->cost;
    free(current_state);
  }

  delete_queue(&state_queue);

  gettimeofday(&end_time, NULL);

  delta_usec = end_time.tv_usec - start_time.tv_usec;
  delta_sec = end_time.tv_sec - start_time.tv_sec;
  if (delta_usec < 0) {
    delta_sec--;
    delta_usec += 1000000;
  }

  //  carmen_warn("Elasped time for dp: %d secs, %d usecs\n", delta_sec, delta_usec);
}
예제 #8
0
void video_event_handler(int kinect_id, uint8_t* data, uint32_t timestamp __attribute__ ((unused)), int size){
	copy_video(kinect_id, data, carmen_get_time(), size);
}
예제 #9
0
void depth_event_handler(int kinect_id, uint16_t* data, uint32_t timestamp __attribute__ ((unused)), int size){
	copy_depth(kinect_id, data, carmen_get_time(), size);
}
예제 #10
0
static void base_odometry_handler(void)
{
  int i;

  if (strcmp(robot_host, carmen_robot_latest_odometry.host) == 0)
    odometry_count = CARMEN_ROBOT_ESTIMATES_CONVERGE;
  else
    odometry_count++;

  carmen_warn("o");

  for(i = 0; i < CARMEN_ROBOT_MAX_READINGS - 1; i++) {
    carmen_robot_odometry[i] = carmen_robot_odometry[i + 1];
    odometry_local_timestamp[i] = odometry_local_timestamp[i + 1];
  }
  carmen_robot_odometry[CARMEN_ROBOT_MAX_READINGS - 1] = 
    carmen_robot_latest_odometry;
  odometry_local_timestamp[CARMEN_ROBOT_MAX_READINGS - 1] = carmen_get_time();

  carmen_running_average_add(&odometry_average, 
			     odometry_local_timestamp[CARMEN_ROBOT_MAX_READINGS - 1]- 
			     carmen_robot_latest_odometry.timestamp);

  if (collision_avoidance)    {

    if (carmen_robot_bumper_on()) {
      fprintf(stderr, "S");
      carmen_robot_stop_robot(CARMEN_ROBOT_ALL_STOP);
      command_tv = 0;
      command_rv = 0;
    }
    
#ifndef COMPILE_WITHOUT_LASER_SUPPORT
    if (carmen_robot_latest_odometry.tv > 0 &&
	carmen_robot_laser_max_front_velocity() < carmen_robot_latest_odometry.tv &&
	command_tv > carmen_robot_laser_max_front_velocity())  {

      if (carmen_robot_laser_max_front_velocity() <= 0.0)   {
	command_tv = 0;
	fprintf(stderr, "S");
	carmen_robot_stop_robot(CARMEN_ROBOT_ALLOW_ROTATE);
      }
      else {
	command_tv = carmen_robot_laser_max_front_velocity();
	carmen_robot_send_base_velocity_command();
      }
    }
    else if (carmen_robot_latest_odometry.tv < 0 &&
	     carmen_robot_laser_min_rear_velocity() > carmen_robot_latest_odometry.tv &&
	     command_tv < carmen_robot_laser_min_rear_velocity())  {
      if (carmen_robot_laser_min_rear_velocity() >= 0.0) {
	fprintf(stderr, "S");
	command_tv = 0;
	carmen_robot_stop_robot(CARMEN_ROBOT_ALLOW_ROTATE);
      }
      else  {
	command_tv = carmen_robot_laser_min_rear_velocity();
	carmen_robot_send_base_velocity_command();
      }
    }
#endif
  } // End of if (collision_avoidance)

  if (use_sonar) 
    carmen_robot_correct_sonar_and_publish();
  if (use_bumper)
    carmen_robot_correct_bumper_and_publish();
#ifndef COMPILE_WITHOUT_LASER_SUPPORT
  if (use_laser)
    carmen_robot_correct_laser_and_publish();
#endif
}
예제 #11
0
int carmen_laser_run(void)
{
  static int first = 1;
  static double last_update;
  static double last_alive = 0;
  double current_time;
  int print_stats;
  static int laser1_stalled = 0, laser2_stalled = 0, laser3_stalled = 0,
    laser4_stalled = 0,    laser5_stalled = 0;;

  if(first) {
    last_update = carmen_get_time();
    first = 0;
  }
  current_time = carmen_get_time();
  print_stats = (current_time - last_update > 1.0);
  if(use_laser1) {
    sick_handle_laser(&laser1);
    if(laser1.new_reading)
      publish_laser_message(&laser1, &laser1_config);   
    laser1_stalled = (current_time - laser1.timestamp > 1.0);
    //**
	//fprintf(stderr, "time: %.1f",current_time - laser1.timestamp);
	//**
    if(print_stats) 
      fprintf(stderr, "L1: %s(%.1f%% full) ", laser1_stalled ?
	      "STALLED " : " ", (laser1.buffer_position - 
				 laser1.processed_mark) / 
	      (float)LASER_BUFFER_SIZE * 100.0);
  }
  if(use_laser2) {
    sick_handle_laser(&laser2);
    if(laser2.new_reading)
      publish_laser_message(&laser2, &laser2_config);
    laser2_stalled = (current_time - laser2.timestamp > 1.0);
    if(print_stats) 
      fprintf(stderr, "L2: %s(%.1f%% full) ", laser2_stalled ?
	      "STALLED " : " ", (laser2.buffer_position - 
				 laser2.processed_mark) / 
	      (float)LASER_BUFFER_SIZE * 100.0);
  }
  if(use_laser3) {
    sick_handle_laser(&laser3);
    if(laser3.new_reading)
      publish_laser_message(&laser3, &laser3_config);   
    laser3_stalled = (current_time - laser3.timestamp > 1.0);
    if(print_stats) 
      fprintf(stderr, "L3: %s(%.1f%% full) ", laser3_stalled ?
	      "STALLED " : " ", laser3.buffer_position / 
	      (float)LASER_BUFFER_SIZE * 100.0);
  }
  if(use_laser4) {
    sick_handle_laser(&laser4);
    if(laser4.new_reading)
      publish_laser_message(&laser4, &laser4_config);   
    laser4_stalled = (current_time - laser4.timestamp > 1.0);
    if(print_stats) 
      fprintf(stderr, "L4: %s(%.1f%% full) ", laser4_stalled ?
	      "STALLED " : " ", laser4.buffer_position / 
	      (float)LASER_BUFFER_SIZE * 100.0);
  }
  if(use_laser5) {
    sick_handle_laser(&laser5);
    if(laser5.new_reading)
      publish_laser_message(&laser5, &laser5_config);   
    laser5_stalled = (current_time - laser5.timestamp > 1.0);
    if(print_stats) 
      fprintf(stderr, "L5: %s(%.1f%% full) ", laser5_stalled ?
	      "STALLED " : " ", laser5.buffer_position / 
	      (float)LASER_BUFFER_SIZE * 100.0);
  }
  if(print_stats) {
    fprintf(stderr, "\n");
    last_update = current_time;
  }
  if(current_time - last_alive > 1.0) {
    publish_laser_alive(laser1_stalled, laser2_stalled, laser3_stalled,
			laser4_stalled, laser5_stalled);
    last_alive = current_time;
  }

  carmen_publish_heartbeat("laser");

  return 0;
}
예제 #12
0
static void velocity_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData,
			     void *clientData __attribute__ ((unused)))
{
  carmen_robot_velocity_message v;
  FORMATTER_PTR formatter;
  IPC_RETURN_TYPE err;

  formatter = IPC_msgInstanceFormatter(msgRef);
  err = IPC_unmarshallData(formatter, callData, &v,
			   sizeof(carmen_robot_velocity_message));  
  IPC_freeByteArray(callData);

  carmen_test_ipc_return(err, "Could not unmarshall", 
			 IPC_msgInstanceName(msgRef));

  time_of_last_command = carmen_get_time();

  command_rv = v.rv;
  command_tv = v.tv;

  following_vector = following_trajectory = 0;
  carmen_robot_send_base_velocity_command();
  publish_vector_status(0, 0);
  IPC_freeDataElements(formatter, &v);
}

static void follow_vector(void)
{
  double true_angle_difference, angle_difference, displacement;
  double radius;
예제 #13
0
static void
send_trajectory_to_robot()
{
	/*int i; 			//Uncomment the hole block to print the motion command that it is been sent

	for (i = 0; i < NUM_MOTION_COMMANDS_PER_VECTOR; i++)
		printf("v = %lf, phi = %lf, t = %lf\n", motion_commands_vector[i].v, motion_commands_vector[i].phi, motion_commands_vector[i].time);*/
	carmen_robot_ackerman_publish_motion_command(motion_commands_vector, NUM_MOTION_COMMANDS_PER_VECTOR, carmen_get_time());
}
예제 #14
0
static void
localize_globalpos_handler(carmen_localize_ackerman_globalpos_message *msg)
{
//	if (current_algorithm != CARMEN_BEHAVIOR_SELECTOR_A_STAR)
//		return;
	IPC_RETURN_TYPE err = IPC_OK;
	static int roundValue = 2;
	static carmen_ackerman_traj_point_t robot_position;

	robot_position.x = round(msg->globalpos.x * roundValue) / roundValue;
	robot_position.y = round(msg->globalpos.y * roundValue) / roundValue;
	robot_position.theta = round(msg->globalpos.theta * roundValue) / roundValue;
	robot_position.v = msg->v;
	robot_position.phi = msg->phi;


	if (messageControl.carmen_planner_ackerman_update_robot(&robot_position, &robot_config) == 0)
		return;

	//publica caminho
	carmen_planner_status_t status;
	messageControl.carmen_planner_ackerman_get_status(&status);

	//Mensagem que faz andar
	if (status.path.length > 0)
	{
		carmen_motion_planner_publish_path_message(status.path.points, status.path.length, current_algorithm);


		IPC_RETURN_TYPE err;
		static int firsttime = 1;
		static carmen_navigator_ackerman_astar_goal_list_message goal_list_msg;
		if (firsttime)
		{
			err = IPC_defineMsg(CARMEN_ASTAR_GOAL_LIST_NAME, IPC_VARIABLE_LENGTH, CARMEN_ASTAR_GOAL_LIST_FMT);
			carmen_test_ipc_exit(err, "Could not define message", CARMEN_ASTAR_GOAL_LIST_NAME);
			goal_list_msg.host = carmen_get_host();
			firsttime = 0;
		}
		goal_list_msg.goal_list = status.path.points;
		goal_list_msg.size = status.path.length;
		goal_list_msg.timestamp = carmen_get_time();

		err = IPC_publishData(CARMEN_ASTAR_GOAL_LIST_NAME, &goal_list_msg);//todo valgrind encontra problema de valor nao inicializado aqui
		carmen_test_ipc(err, "Could not publish", CARMEN_ASTAR_GOAL_LIST_NAME);
	}



	//Mensagem que exibe o caminho na tela
	if (status.path.length > 0)
	{
		static carmen_navigator_ackerman_plan_tree_message plan_tree_msg;
		static bool firstTime = true;

		if (firstTime == true)
		{
			err = IPC_defineMsg(CARMEN_NAVIGATOR_ACKERMAN_PLAN_TREE_NAME, IPC_VARIABLE_LENGTH,
					CARMEN_NAVIGATOR_ACKERMAN_PLAN_TREE_FMT);
			carmen_test_ipc_exit(err, "Could not define", CARMEN_NAVIGATOR_ACKERMAN_PLAN_TREE_NAME);

			plan_tree_msg.host = carmen_get_host();
			firstTime = false;
		}
		if (status.path.length > 100)
		{	// Ver tipo carmen_navigator_ackerman_plan_tree_message
			printf("Error: status.path.length > 100\n");
			return;
		}
		plan_tree_msg.num_path = 1;
		memcpy(plan_tree_msg.paths[0], status.path.points, sizeof(carmen_ackerman_traj_point_t) * status.path.length);
		plan_tree_msg.path_size[0] = status.path.length;
		err = IPC_publishData(CARMEN_NAVIGATOR_ACKERMAN_PLAN_TREE_NAME, &plan_tree_msg);

		carmen_test_ipc(err, "Could not publish", CARMEN_NAVIGATOR_ACKERMAN_PLAN_TREE_NAME);
	}

}
예제 #15
0
void smart_status_handler(smart_status_message *msg)
{
  fprintf(stderr, "S");
  carmen_logwrite_write_smart_status(msg, outfile, carmen_get_time() - 
         logger_starttime);
}
예제 #16
0
int carmen_localize_get_map(int global, carmen_map_t *map) 
{
  IPC_RETURN_TYPE err;
  carmen_localize_map_query_message msg;
  carmen_localize_map_message *response = NULL;
  int index;

#ifndef NO_ZLIB
  int uncompress_return;
  int uncompress_size;
  uLongf uncompress_size_result;
  unsigned char *uncompressed_data;
#endif

  static int initialized = 0;

  if (!initialized) 
    {
      err = IPC_defineMsg(CARMEN_LOCALIZE_MAP_QUERY_NAME, 
			  IPC_VARIABLE_LENGTH, 
			  CARMEN_LOCALIZE_MAP_QUERY_FMT);
      carmen_test_ipc_exit(err, "Could not define message", 
			   CARMEN_LOCALIZE_MAP_QUERY_NAME);
      initialized = 1;
    }

  msg.map_is_global_likelihood = global;
  msg.timestamp = carmen_get_time();
  msg.host = carmen_get_host();

  err = IPC_queryResponseData(CARMEN_LOCALIZE_MAP_QUERY_NAME, &msg, 
			      (void **)&response, timeout);
  carmen_test_ipc(err, "Could not get map", CARMEN_LOCALIZE_MAP_QUERY_NAME);

#ifndef NO_ZLIB
  if (response && response->compressed) 
    {
      uncompress_size = response->config.x_size*
	response->config.y_size;
      uncompressed_data = (unsigned char *)
	calloc(uncompress_size, sizeof(float));
      carmen_test_alloc(uncompressed_data);
      uncompress_size_result = uncompress_size*sizeof(float);
      uncompress_return = uncompress((void *)uncompressed_data,   
				     &uncompress_size_result,
				     (void *)response->data, 
				     response->size);
      response->data = uncompressed_data;
      response->size = uncompress_size_result;
    }
#else
  if (response && response->compressed) 
    {
      carmen_warn("Received compressed map from server. This program was\n"
		  "compiled without zlib support, so this map cannot be\n"
		  "used. Sorry.\n");
      free(response->data);
      free(response);
      response = NULL;
    }
#endif

  if (response)
    {
      if (map)
	{
	  map->config = response->config;
	  map->complete_map = (float *)response->data;
	  map->map = (float **)calloc(map->config.x_size, sizeof(float));
	  carmen_test_alloc(map->map);
	  
	  for (index = 0; index < map->config.x_size; index++)
	    map->map[index] = map->complete_map+index*map->config.y_size;
	}
      else
	free(response->data);
      free(response);
    } 

  return 0;
}
예제 #17
0
void gyro_integrated_handler(gyro_integrated_message *msg)
{
  fprintf(stderr, "G");
  carmen_logwrite_write_gyro_integrated(msg, outfile, carmen_get_time() - 
         logger_starttime);
}
예제 #18
0
	carmen_test_ipc(err, "Could not publish", CARMEN_FORD_ESCAPE_ERROR_NAME);
}


static void
publish_velocity_message(void *clientData __attribute__ ((unused)), unsigned long currentTime __attribute__ ((unused)), unsigned long scheduledTime __attribute__ ((unused)))
{
	// This function publish the low level velocity and steering angle from the car

	IPC_RETURN_TYPE err = IPC_OK;
	carmen_robot_ackerman_velocity_message robot_ackerman_velocity_message;
	static int heartbeat = 0;

	robot_ackerman_velocity_message.v = g_XGV_velocity;
	robot_ackerman_velocity_message.phi = get_phi_from_curvature(-tan(g_XGV_atan_curvature), ford_escape_hybrid_config);
	robot_ackerman_velocity_message.timestamp = carmen_get_time(); // @@ Alberto: era igual a = ford_escape_hybrid_config->XGV_v_and_phi_timestamp;
	robot_ackerman_velocity_message.host = carmen_get_host();

	err = IPC_publishData(CARMEN_ROBOT_ACKERMAN_VELOCITY_NAME, &robot_ackerman_velocity_message);
	carmen_test_ipc(err, "Could not publish ford_escape_hybrid message named carmen_robot_ackerman_velocity_message", CARMEN_ROBOT_ACKERMAN_VELOCITY_NAME);

	if ((heartbeat % 10) == 0)
	{
		publish_car_status();
		publish_car_error();
	}

	heartbeat++;
}
//////////////////////////////////////////////////////////////////////////////////////////////////
예제 #19
0
int main(int argc, char *argv[]) {
  UDPConnectionServer connection(2368);
  AcquisitionThread<DataPacket> thread(connection);
  Calibration calib;
  std::shared_ptr<DataPacket> packet;
  IPC_RETURN_TYPE err;
  char dump_filename[4096];

  carmen_velodyne_ipc_initialize(argc, argv);

  carmen_velodyne_read_parameters(argc, argv);

  carmen_velodyne_set_spin_rate(spin_rate);
  
  calib_file.open(calib_path);
  if (!calib_file.is_open())
    carmen_die("\nError: Could not open %s for reading\n", calib_filename);
  calib_file >> calib;
  calib_file.close();
  
  signal(SIGINT, carmen_velodyne_sigint_handler);
  thread.start();

  double time;
  double start_time = carmen_get_time();
  int num_packets = 0;
  int start_num_packets = 0;
  int num_lost_packets = 0;
  int start_num_lost_packets = 0;
  
  while (!quit) {
    try {
      while (dump_enabled || points_publish) {
        packet = thread.getBuffer().dequeue();
        ++num_packets;

        if (dump_enabled) {
          if (!dump_file.is_open()) {
            char dump_time[1024];
            time_t local = packet->getTimestamp();
            struct tm* time = localtime(&local);

            strftime(dump_time, sizeof(dump_time), "%Y-%m-%d-%H%M%S", time);
            sprintf(dump_filename, "%s/%s.bin", dump_path, dump_time);
            dump_file.open(dump_filename, std::ios::out | std::ios::binary);
          }

          if (dump_file.is_open()) {
            long dump_filepos = dump_file.tellp();
            dump_file << *packet;
            carmen_velodyne_publish_packet(id, dump_filename,
              dump_filepos, packet->getTimestamp());
          }
          else {
            carmen_warn("\nWarning: Could not open %s for writing\n",
              dump_filename);
          }
        }

        if (points_publish) {
          VdynePointCloud pointCloud;
          Converter::toPointCloud(*packet, calib, pointCloud);
          int num_points = pointCloud.getSize();
          float x[num_points], y[num_points], z[num_points];
          std::vector<VdynePointCloud::Point3D>::const_iterator it;
          int i = 0;

          for (it = pointCloud.getPointBegin();
              it != pointCloud.getPointEnd(); ++it, ++i) {
            x[i] = it->mX;
            y[i] = it->mY;
            z[i] = it->mZ;
          }
          carmen_velodyne_publish_pointcloud(id, num_points, x, y, z,
            packet->getTimestamp());
        }

        double time = carmen_get_time();
        if (time-start_time >= 1.0) {
          num_lost_packets = thread.getBuffer().getNumDroppedElements();
          float period_num_packets = num_packets-start_num_packets;
          float period_num_lost_packets = num_lost_packets-
            start_num_lost_packets;
          
          double packet_loss = period_num_lost_packets/
            (period_num_packets+period_num_lost_packets);
          fprintf(stdout, "\rPacket loss: %6.2f%%", packet_loss*100.0);
          fflush(stdout);

          start_time = time;
          start_num_packets = num_packets;
          start_num_lost_packets = num_lost_packets;          
        }
      }
    }
    catch (IOException& exception) {
      // buffer underrun
    }

    carmen_ipc_sleep(0.0);
  }

  if (dump_file.is_open())
    dump_file.close();

  fprintf(stdout, "\n");

  return 0;
}
예제 #20
0
// updates the internal state variables
int carmen_base_direct_update_status(double* packet_timestamp) //
{
    // fill in the time stamp and update internal time
    double curr_time = carmen_get_time();

    //s_delta_time = curr_time - s_time;
    s_time = curr_time;
    *packet_timestamp = curr_time;

    // sonar is not implemented in orc 5 so we don't deal with this
    s_left_range = 0.0;
    s_right_range = 0.0;

    // ir deleted since not being used

    // bumpers
    s_bumpers[0] = orc_digital_read( s_orc, ORC_BUMPER_PORT0 );
    s_bumpers[1] = orc_digital_read( s_orc, ORC_BUMPER_PORT1 );
    s_bumpers[2] = orc_digital_read( s_orc, ORC_BUMPER_PORT2 );
    s_bumpers[3] = orc_digital_read( s_orc, ORC_BUMPER_PORT3 );

    // read encoders
    int curr_left_tick = orc_quadphase_read( s_orc, ORC_LEFT_ENCODER_PORT );
    int curr_right_tick = orc_quadphase_read( s_orc, ORC_RIGHT_ENCODER_PORT );

    int left_delta = compute_delta_ticks_mod( curr_left_tick, s_left_tick ) * ORC_LEFT_REVERSE_ENCODER ;

    int right_delta = compute_delta_ticks_mod( curr_right_tick, s_right_tick ) * ORC_RIGHT_REVERSE_ENCODER ;

    s_left_displacement = delta_tick_to_metres( left_delta );
    s_right_displacement = delta_tick_to_metres( right_delta );
    s_left_tick = curr_left_tick;
    s_right_tick = curr_right_tick;

    // update velocity information

    int left_delta_velocity = orc_quadphase_read_velocity_signed( s_orc, ORC_LEFT_ENCODER_PORT );
    int right_delta_velocity = orc_quadphase_read_velocity_signed( s_orc, ORC_RIGHT_ENCODER_PORT );

    // looking for underflow or overflow
    /*

    if( left_delta_velocity > SHORT_MAX/2 )
      left_delta_velocity = SHORT_MAX - left_delta_velocity;
    if( right_delta_velocity  > SHORT_MAX/2 )
      right_delta_velocity = SHORT_MAX - right_delta_velocity;

    if( left_delta_velocity < -1.0 * SHORT_MAX/2 )
      left_delta_velocity = SHORT_MAX + left_delta_velocity;
    if( right_delta_velocity  < -1.0 * SHORT_MAX/2 )
      right_delta_velocity = SHORT_MAX + right_delta_velocity;

    */

    s_left_velocity = -1.0 * delta_tick_to_metres( left_delta_velocity ) * 60.0 ;  //s_left_displacement / s_delta_time;
    s_right_velocity = delta_tick_to_metres( right_delta_velocity ) * 60.0 ;  //s_right_displacement / s_delta_time;



    //printf( "  *** ticks: left %d, right %d \n", left_delta, right_delta );
    //printf( "  *** ticks: left %d, right %d \n", left_delta_velocity , right_delta_velocity );
    //printf( "  *** vels: left %f, right %f \n", s_left_velocity, s_right_velocity );

    // update position information
    double displacement = ( s_left_displacement + s_right_displacement )/2;
    double rotation = atan2( s_right_displacement - s_left_displacement,
                             ORC_WHEEL_BASE );
    s_x = s_x + displacement*cos( s_theta );
    s_y = s_y + displacement*sin( s_theta );
    s_theta = carmen_normalize_theta( s_theta + rotation );

    return 0;
}
예제 #21
0
void param_change_handler(carmen_param_variable_change_message *msg)
{
  carmen_logwrite_write_param(msg->module_name, msg->variable_name, 
			      msg->value, msg->timestamp, 
			      msg->host, outfile, carmen_get_time());
}
예제 #22
0
void robot_rearlaser_handler(carmen_robot_laser_message *laser)
{
  fprintf(stderr, "R");
  carmen_logwrite_write_robot_laser(laser, 2, outfile, 
				    carmen_get_time() - logger_starttime);
}
예제 #23
0
void base_sonar_handler(carmen_base_sonar_message *sonar)
{
  fprintf(stderr, "S");
  carmen_logwrite_write_base_sonar(sonar, outfile, 
				 carmen_get_time() - logger_starttime);
}
예제 #24
0
{
  rear_laser_count++;
}

void shutdown_module(int x __attribute__ ((unused)))
{
  carmen_ipc_disconnect();
  printf("\nDisconnected from laser.\n");
  exit(1);  
}

void print_statistics(void *clientdata __attribute__ ((unused)), 
		      unsigned long t1 __attribute__ ((unused)), 
		      unsigned long t2 __attribute__ ((unused)))
{
  double current_timestamp = carmen_get_time();

  fprintf(stderr, "L1 - %.2f Hz      L2 - %.2f Hz\n", 
	  front_laser_count / (current_timestamp - first_timestamp),
	  rear_laser_count / (current_timestamp - first_timestamp));
}

int main(int argc, char **argv)
{
  carmen_ipc_initialize(argc, argv);
  carmen_param_check_version(argv[0]);
  signal(SIGINT, shutdown_module);
 
  carmen_laser_subscribe_frontlaser_message(&frontlaser, (carmen_handler_t)
					    laser_frontlaser_handler, 
					    CARMEN_SUBSCRIBE_LATEST);