void Carmen_Thread::declare_messages() {
    IPC_RETURN_TYPE err;


    err = IPC_defineMsg(CARMEN_BASE_ACKERMAN_VELOCITY_NAME,
                        IPC_VARIABLE_LENGTH,
                        CARMEN_BASE_ACKERMAN_VELOCITY_FMT);
    carmen_test_ipc_exit(err, "Could not define", CARMEN_BASE_ACKERMAN_VELOCITY_NAME);

    err = IPC_defineMsg(CARMEN_LOCALIZE_ACKERMAN_SENSOR_NAME,
                        IPC_VARIABLE_LENGTH,
                        CARMEN_LOCALIZE_ACKERMAN_SENSOR_FMT);
    carmen_test_ipc_exit(err, "Could not define", CARMEN_LOCALIZE_ACKERMAN_SENSOR_NAME);

    err = IPC_defineMsg(CARMEN_NAVIGATOR_ACKERMAN_PLAN_NAME,
                        IPC_VARIABLE_LENGTH,
                        CARMEN_NAVIGATOR_ACKERMAN_PLAN_FMT);
    carmen_test_ipc_exit(err, "Could not define", CARMEN_NAVIGATOR_ACKERMAN_PLAN_NAME);

    carmen_laser_define_laser_message(1);

    carmen_grid_mapping_define_messages();

    carmen_rrt_planner_define_robot_tree_message();

    carmen_rrt_planner_define_status_message();
}
static void
publish_spline_goal_message(carmen_ackerman_traj_point_t poses)
{
	IPC_RETURN_TYPE err;
	carmen_navigator_ackerman_set_goal_message goal_msg;
	static int initialized = 0;

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

	goal_msg.x = poses.x;
	goal_msg.y = poses.y;
	goal_msg.theta = globalpos.theta;
	goal_msg.host = carmen_get_host();
	goal_msg.timestamp = carmen_get_time();

	err = IPC_publishData(CARMEN_NAVIGATOR_ACKERMAN_SET_GOAL_NAME, &goal_msg);
	carmen_test_ipc_exit(err, "Could not publish", CARMEN_NAVIGATOR_ACKERMAN_SET_GOAL_NAME);
}
void
carmen_grid_mapping_publish_distance_map_message(carmen_grid_mapping_distance_map *distance_map, double timestamp)
{
  IPC_RETURN_TYPE err;
  static carmen_grid_mapping_distance_map_message distance_map_message;
  static int first_time = 1;

  if (first_time)
  {
    err = IPC_defineMsg(CARMEN_GRID_MAPPING_DISTANCE_MAP_MESSAGE_NAME, IPC_VARIABLE_LENGTH,
    					CARMEN_GRID_MAPPING_DISTANCE_MAP_MESSAGE_FMT);
	carmen_test_ipc_exit(err, "Could not define", CARMEN_GRID_MAPPING_DISTANCE_MAP_MESSAGE_NAME);

	first_time = 0;
  }

  distance_map_message.config = distance_map->config;
  strcpy(distance_map_message.config.origin, "from_mapping");
  distance_map_message.size = distance_map->config.x_size * distance_map->config.y_size;
  distance_map_message.complete_x_offset = distance_map->complete_x_offset;
  distance_map_message.complete_y_offset = distance_map->complete_y_offset;
  distance_map_message.timestamp = timestamp;
  distance_map_message.host = carmen_get_host();

  err = IPC_publishData(CARMEN_GRID_MAPPING_DISTANCE_MAP_MESSAGE_NAME, &distance_map_message);
  carmen_test_ipc_exit(err, "Could not publish", CARMEN_GRID_MAPPING_DISTANCE_MAP_MESSAGE_NAME);
}
void
carmen_parking_assistant_define_messages()
{
  IPC_RETURN_TYPE err;

  err = IPC_defineMsg(CARMEN_PARKING_ASSISTANT_GOAL_NAME, IPC_VARIABLE_LENGTH,
	CARMEN_PARKING_ASSISTANT_GOAL_FMT);
  carmen_test_ipc_exit(err, "Could not define", CARMEN_PARKING_ASSISTANT_GOAL_NAME);

  err = IPC_defineMsg(CARMEN_PARKING_ASSISTANT_PARKING_SPACE_NAME, IPC_VARIABLE_LENGTH,
	CARMEN_PARKING_ASSISTANT_PARKING_SPACE_FMT);
  carmen_test_ipc_exit(err, "Could not define", CARMEN_PARKING_ASSISTANT_PARKING_SPACE_NAME);
}
void
carmen_mapper_define_messages()
{
  IPC_RETURN_TYPE err;

  err = IPC_defineMsg(CARMEN_MAPPER_MAP_MESSAGE_NAME, IPC_VARIABLE_LENGTH,
  		CARMEN_MAPPER_MAP_MESSAGE_FMT);
  carmen_test_ipc_exit(err, "Could not define", CARMEN_MAPPER_MAP_MESSAGE_NAME);

  err = IPC_defineMsg(CARMEN_MAPPER_MAP_MOVING_OBJECTS_RAW_MAP_MESSAGE_NAME, IPC_VARIABLE_LENGTH,
  		CARMEN_MAPPER_MAP_MESSAGE_FMT);
  carmen_test_ipc_exit(err, "Could not define", CARMEN_MAPPER_MAP_MOVING_OBJECTS_RAW_MAP_MESSAGE_NAME);
}
int
carmen_navigator_query_status(carmen_navigator_status_message **status_msg) 
{
  IPC_RETURN_TYPE err;
  carmen_navigator_status_query_message *msg;
  static int initialized = 0;

  msg = carmen_default_message_create();

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

  err = IPC_queryResponseData(CARMEN_NAVIGATOR_STATUS_QUERY_NAME, msg, 
			      (void **)status_msg, timeout);
  carmen_test_ipc_return_int(err, "Could not query navigator status", 
			     CARMEN_NAVIGATOR_STATUS_QUERY_NAME);

  return 0;
}
int 
carmen_navigator_set_goal_triplet(carmen_point_p goal) 
{
  IPC_RETURN_TYPE err = IPC_OK;
  carmen_navigator_set_goal_triplet_message msg;
  static int initialized = 0;

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


  msg.goal = *goal;
  msg.timestamp = carmen_get_time();
  msg.host = carmen_get_host();

  err = IPC_publishData(CARMEN_NAVIGATOR_SET_GOAL_TRIPLET_NAME, &msg);
  carmen_test_ipc(err, "Could not publish", 
		  CARMEN_NAVIGATOR_SET_GOAL_TRIPLET_NAME);

  return 0;
}
static void
carmen_motion_planner_publish_path_message(
		carmen_ackerman_traj_point_t *path, int size, int algorithm)
{
	IPC_RETURN_TYPE err;
	static int firsttime = 1;
	static carmen_motion_planner_path_message msg;

	if(firsttime)
	{
		IPC_RETURN_TYPE err;

		err = IPC_defineMsg(CARMEN_MOTION_PLANNER_PATH_NAME,
				IPC_VARIABLE_LENGTH,
				CARMEN_MOTION_PLANNER_PATH_FMT);
		carmen_test_ipc_exit(err, "Could not define message",
				CARMEN_MOTION_PLANNER_PATH_NAME);
		msg.host = carmen_get_host();
		firsttime = 0;
	}


	msg.timestamp = carmen_get_time();
	msg.path = path;
	msg.path_size = size;
	msg.algorithm = algorithm;

	err = IPC_publishData(CARMEN_MOTION_PLANNER_PATH_NAME, &msg);
	carmen_test_ipc(err, "Could not publish", CARMEN_MOTION_PLANNER_PATH_NAME);
}
void
publish_navigator_ackerman_plan_message(carmen_ackerman_traj_point_t *points_vector, int size)
{
	carmen_navigator_ackerman_plan_message msg;
	static int		first_time = 1;
	IPC_RETURN_TYPE err;

	msg.path = points_vector;
	msg.path_length = size;
	msg.host = carmen_get_host();
	msg.timestamp = carmen_get_time();

	if (first_time)
	{
		err = IPC_defineMsg(CARMEN_NAVIGATOR_ACKERMAN_PLAN_NAME,
				IPC_VARIABLE_LENGTH,
				CARMEN_NAVIGATOR_ACKERMAN_PLAN_FMT);
		carmen_test_ipc_exit(err, "Could not define message",
				CARMEN_NAVIGATOR_ACKERMAN_PLAN_NAME);
		first_time = 0;
	}

	err = IPC_publishData(CARMEN_NAVIGATOR_ACKERMAN_PLAN_NAME, &msg);
	carmen_test_ipc(err, "Could not publish",
			CARMEN_NAVIGATOR_ACKERMAN_PLAN_NAME);
}
Beispiel #10
0
void carmen_robot_correct_bumper_and_publish(void) 
{  
  double bumper_skew;
  double fraction;
  int low, high;
  
  if(!bumper_ready)
    return;

  check_message_data_chunk_sizes();
  
  bumper_ready = carmen_robot_get_skew(bumper_count, &bumper_skew,
				       &bumper_average, 
				       base_bumper.host);
  if (!bumper_ready) {
    carmen_warn("Waiting for bumper data to accumulate\n");
    return;
  }

  fraction = carmen_robot_get_fraction(base_bumper.timestamp, bumper_skew,
				       &low, &high);

  if (!carmen_robot_config.interpolate_odometry)
    fraction=0;

  construct_bumper_message(&robot_bumper, low, high, fraction);
    
  IPC_RETURN_TYPE err;
  err = IPC_publishData(CARMEN_ROBOT_BUMPER_NAME, &robot_bumper);
  carmen_test_ipc_exit(err, "Could not publish", CARMEN_ROBOT_BUMPER_NAME);

  fprintf(stderr, "b");
  bumper_ready = 0;  
}
void
carmen_velodyne_define_messages()
{
	IPC_RETURN_TYPE err;

	err = IPC_defineMsg(CARMEN_VELODYNE_PARTIAL_SCAN_MESSAGE_NAME, IPC_VARIABLE_LENGTH,
			CARMEN_VELODYNE_PARTIAL_SCAN_MESSAGE_FMT);
	carmen_test_ipc_exit(err, "Could not define", CARMEN_VELODYNE_PARTIAL_SCAN_MESSAGE_NAME);

	err = IPC_defineMsg(CARMEN_VELODYNE_GPS_MESSAGE_NAME, IPC_VARIABLE_LENGTH,
			CARMEN_VELODYNE_GPS_MESSAGE_FMT);
	carmen_test_ipc_exit(err, "Could not define", CARMEN_VELODYNE_GPS_MESSAGE_NAME);

	err = IPC_defineMsg(CARMEN_VELODYNE_VARIABLE_SCAN_MESSAGE_NAME, IPC_VARIABLE_LENGTH, CARMEN_VELODYNE_VARIABLE_SCAN_MESSAGE_FMT);
	carmen_test_ipc_exit(err, "Could not define", CARMEN_VELODYNE_VARIABLE_SCAN_MESSAGE_NAME);
}
void
publish_visual_tracker_output_message()
{
	IPC_RETURN_TYPE err;
	err = IPC_publishData(CARMEN_VISUAL_TRACKER_OUTPUT_MESSAGE_NAME, &message_output);
	carmen_test_ipc_exit(err, "Could not publish", CARMEN_VISUAL_TRACKER_OUTPUT_MESSAGE_NAME);
}
void
publish_globalpos(double x, double y, double theta, double timestamp)
{
	IPC_RETURN_TYPE err;
	carmen_localize_ackerman_globalpos_message globalpos;

	globalpos.timestamp = timestamp;
	globalpos.host = carmen_get_host();
	globalpos.globalpos.x = x;
	globalpos.globalpos.y = y;
	globalpos.globalpos.theta = theta;
	globalpos.globalpos_std.x = 0.001;
	globalpos.globalpos_std.y = 0.001;
	globalpos.globalpos_std.theta = 0.001;
	globalpos.odometrypos.x = x;
	globalpos.odometrypos.y = y;
	globalpos.odometrypos.theta = theta;
	globalpos.globalpos_xy_cov = 0.001;
	globalpos.v = 0.0;
	globalpos.phi = 0.0;
	globalpos.converged = 1;

	globalpos.pose.orientation.pitch = globalpos.pose.orientation.roll = globalpos.pose.position.z = 0.0;
	globalpos.velocity.x = globalpos.velocity.y = globalpos.velocity.z = 0.0;
	globalpos.pose.orientation.yaw = globalpos.globalpos.theta;

	globalpos.pose.position.x = globalpos.globalpos.x;
	globalpos.pose.position.y = globalpos.globalpos.y;
	globalpos.pose.position.z = 0.0;
	globalpos.velocity.x = 0.0;

	err = IPC_publishData(CARMEN_LOCALIZE_ACKERMAN_GLOBALPOS_NAME, &globalpos);
	carmen_test_ipc_exit(err, "Could not publish", CARMEN_LOCALIZE_ACKERMAN_GLOBALPOS_NAME);
}
void carmen_localize_initialize_uniform_command(void)
{
  static carmen_localize_initialize_message init;
  static int first = 1;
  IPC_RETURN_TYPE err;

  if(first) 
    {
      err = IPC_defineMsg(CARMEN_LOCALIZE_INITIALIZE_NAME, 
			  IPC_VARIABLE_LENGTH, 
			  CARMEN_LOCALIZE_INITIALIZE_FMT);
      carmen_test_ipc_exit(err, "Could not define message", 
			   CARMEN_LOCALIZE_INITIALIZE_NAME);

      first = 0;
    }
  init.timestamp = carmen_get_time();
  init.host = carmen_get_host();
    
  init.distribution = CARMEN_INITIALIZE_UNIFORM;
  init.num_modes = 0;
  init.mean = NULL;
  init.std = NULL;
  err = IPC_publishData(CARMEN_LOCALIZE_INITIALIZE_NAME, &init);
  carmen_test_ipc(err, "Could not publish", CARMEN_LOCALIZE_INITIALIZE_NAME);
}
Beispiel #15
0
void
carmen_robot_follow_trajectory(carmen_traj_point_t *trajectory, 
			       int trajectory_length,
			       carmen_traj_point_t *robot)
{
  IPC_RETURN_TYPE err;
  static carmen_robot_follow_trajectory_message msg;

  err = IPC_defineMsg(CARMEN_ROBOT_FOLLOW_TRAJECTORY_NAME, 
		      IPC_VARIABLE_LENGTH, 
		      CARMEN_ROBOT_FOLLOW_TRAJECTORY_FMT);
  carmen_test_ipc_exit(err, "Could not define message", 
		       CARMEN_ROBOT_FOLLOW_TRAJECTORY_NAME);

  msg.trajectory = trajectory;
  msg.trajectory_length = trajectory_length;
  msg.robot_position = *robot;

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

  err = IPC_publishData(CARMEN_ROBOT_FOLLOW_TRAJECTORY_NAME, &msg);
  carmen_test_ipc(err, "Could not publish", 
		  CARMEN_ROBOT_FOLLOW_TRAJECTORY_NAME);
}
void DetectorObserver::test(IplImage *frame)
{
    if (frame)
    {
        m_test.host = carmen_get_host();
        m_test.timestamp = carmen_get_time();

        m_test.channels = frame->nChannels;
        m_test.height = frame->height;
        m_test.width = frame->width;

        m_test.size = frame->height * frame->width * frame->nChannels;

        m_test.image = (unsigned char *) malloc(m_test.size * sizeof(unsigned char *));

        copy_BGR_image_to_RGB_image(frame, m_test.image, frame->nChannels);

        IPC_RETURN_TYPE err;
        err = IPC_publishData(CARMEN_VISUAL_TRACKER_TEST_MESSAGE_NAME, &m_test);
        carmen_test_ipc_exit(err, "Could not publish", CARMEN_VISUAL_TRACKER_TEST_MESSAGE_NAME);

        free(m_test.image);

        emit newFrame(frame);

    }

}
void DetectorObserver::train(IplImage *frame, QRectF selection)
{
    if (frame)
    {
        m_train.host = carmen_get_host();
        m_train.timestamp = carmen_get_time();

        m_train.channels = frame->nChannels;
        m_train.height = frame->height;
        m_train.width = frame->width;

        m_train.size = frame->height * frame->width * frame->nChannels;

        m_train.image = (unsigned char *) malloc(m_train.size * sizeof(unsigned char *));

        copy_BGR_image_to_RGB_image(frame, m_train.image, frame->nChannels);

        m_train.rect.x = (int)selection.x();
        m_train.rect.y = (int)selection.y();
        m_train.rect.width = (int)selection.width();
        m_train.rect.height = (int)selection.height();

        IPC_RETURN_TYPE err;
        err = IPC_publishData(CARMEN_VISUAL_TRACKER_TRAIN_MESSAGE_NAME, &m_train);
        carmen_test_ipc_exit(err, "Could not publish", CARMEN_VISUAL_TRACKER_TRAIN_MESSAGE_NAME);

        free(m_train.image);
    }
}
void 
carmen_localize_initialize_gaussian_command(carmen_point_t mean,
					    carmen_point_t std)
{
  static carmen_localize_initialize_message init;
  static int first = 1;
  IPC_RETURN_TYPE err;
  
  if(first) 
    {
      err = IPC_defineMsg(CARMEN_LOCALIZE_INITIALIZE_NAME, 
			  IPC_VARIABLE_LENGTH, 
			  CARMEN_LOCALIZE_INITIALIZE_FMT);
      carmen_test_ipc_exit(err, "Could not define message", 
			   CARMEN_LOCALIZE_INITIALIZE_NAME);

      first = 0;
    }
  init.timestamp = carmen_get_time();
  init.host = carmen_get_host();

  init.distribution = CARMEN_INITIALIZE_GAUSSIAN;
  init.num_modes = 1;
  init.mean = &mean;
  init.std = &std;
  err = IPC_publishData(CARMEN_LOCALIZE_INITIALIZE_NAME, &init);
  carmen_test_ipc(err, "Could not publish", CARMEN_LOCALIZE_INITIALIZE_NAME);
}
void
carmen_fast_polar_slam_publish_velodyne_projected_message(carmen_fast_polar_slam_velodyne_projected_on_ground_message *message)
{
	IPC_RETURN_TYPE err;

	err = IPC_publishData(CARMEN_FAST_POLAR_SLAM_VELODYNE_PROJECTED_MESSAGE_NAME, message);
	carmen_test_ipc_exit(err, "Could not publish", CARMEN_FAST_POLAR_SLAM_VELODYNE_PROJECTED_MESSAGE_NAME);
}
void
carmen_minoru_define_bumblebee_fake_messages()
{
  IPC_RETURN_TYPE err;

  err = IPC_defineMsg(CARMEN_BUMBLEBEE_BASIC_STEREOIMAGE10_NAME, IPC_VARIABLE_LENGTH, CARMEN_BUMBLEBEE_BASIC_STEREOIMAGE10_FMT);
  carmen_test_ipc_exit(err, "Could not define", CARMEN_BUMBLEBEE_BASIC_STEREOIMAGE10_NAME);
}
void
carmen_minoru_define_messages()
{
  IPC_RETURN_TYPE err;

  err = IPC_defineMsg(CARMEN_MINORU_STEREOIMAGE_NAME, IPC_VARIABLE_LENGTH, CARMEN_MINORU_STEREOIMAGE_FMT);
  carmen_test_ipc_exit(err, "Could not define", CARMEN_MINORU_STEREOIMAGE_NAME);
}
void
carmen_fast_polar_slam_publish_measurement_model_message(carmen_fast_polar_slam_measurement_model_message *message)
{
	IPC_RETURN_TYPE err;

	err = IPC_publishData(CARMEN_FAST_POLAR_SLAM_MEASUREMENT_MODEL_MESSAGE_NAME, message);
	carmen_test_ipc_exit(err, "Could not publish", CARMEN_FAST_POLAR_SLAM_MEASUREMENT_MODEL_MESSAGE_NAME);
}
void
carmen_fast_polar_slam_publish_particles_message(carmen_fast_polar_slam_particles_message *message)
{
	IPC_RETURN_TYPE err;

	err = IPC_publishData(CARMEN_FAST_POLAR_SLAM_PARTICLES_MESSAGE_NAME, message);
	carmen_test_ipc_exit(err, "Could not publish", CARMEN_FAST_POLAR_SLAM_PARTICLES_MESSAGE_NAME);
}
IPC_RETURN_TYPE
carmen_visual_tracker_define_message_train(void)
{
	IPC_RETURN_TYPE err;
	err = IPC_defineMsg(CARMEN_VISUAL_TRACKER_TRAIN_MESSAGE_NAME, IPC_VARIABLE_LENGTH, CARMEN_VISUAL_TRACKER_TRAIN_MESSAGE_FMT);
	carmen_test_ipc_exit(err, "Could not define", CARMEN_VISUAL_TRACKER_TRAIN_MESSAGE_NAME);
	return(err);
}
void
carmen_fast_polar_slam_define_messages()
{
	IPC_RETURN_TYPE err;

	err = IPC_defineMsg(CARMEN_FAST_POLAR_SLAM_BEST_PARTICLE_MESSAGE_NAME, IPC_VARIABLE_LENGTH, CARMEN_FAST_POLAR_SLAM_BEST_PARTICLE_MESSAGE_FMT);
	carmen_test_ipc_exit(err, "Could not define", CARMEN_FAST_POLAR_SLAM_BEST_PARTICLE_MESSAGE_NAME);

	err = IPC_defineMsg(CARMEN_FAST_POLAR_SLAM_VELODYNE_PROJECTED_MESSAGE_NAME, IPC_VARIABLE_LENGTH, CARMEN_FAST_POLAR_SLAM_VELODYNE_PROJECTED_MESSAGE_FMT);
	carmen_test_ipc_exit(err, "Could not define", CARMEN_FAST_POLAR_SLAM_VELODYNE_PROJECTED_MESSAGE_NAME);

	err = IPC_defineMsg(CARMEN_FAST_POLAR_SLAM_PARTICLES_MESSAGE_NAME, IPC_VARIABLE_LENGTH, CARMEN_FAST_POLAR_SLAM_PARTICLES_MESSAGE_FMT);
	carmen_test_ipc_exit(err, "Could not define", CARMEN_FAST_POLAR_SLAM_PARTICLES_MESSAGE_NAME);

	err = IPC_defineMsg(CARMEN_FAST_POLAR_SLAM_MEASUREMENT_MODEL_MESSAGE_NAME, IPC_VARIABLE_LENGTH, CARMEN_FAST_POLAR_SLAM_MEASUREMENT_MODEL_MESSAGE_FMT);
	carmen_test_ipc_exit(err, "Could not define", CARMEN_FAST_POLAR_SLAM_MEASUREMENT_MODEL_MESSAGE_NAME);
}
void
carmen_navigator_spline_define_messages()
{
  IPC_RETURN_TYPE err;

  err = IPC_defineMsg(CARMEN_NAVIGATOR_SPLINE_PATH_NAME, IPC_VARIABLE_LENGTH, CARMEN_NAVIGATOR_SPLINE_PATH_FMT);
  carmen_test_ipc_exit(err, "Could not define", CARMEN_NAVIGATOR_SPLINE_PATH_NAME);
}
void
carmen_monte_carlo_localization_define_messages()
{
  IPC_RETURN_TYPE err;

  err = IPC_defineMsg(CARMEN_MONTE_CARLO_LOCALIZATION_NAME, IPC_VARIABLE_LENGTH,
		  CARMEN_MONTE_CARLO_LOCALIZATION_MESSAGE_FMT);
  carmen_test_ipc_exit(err, "Could not define", CARMEN_MONTE_CARLO_LOCALIZATION_NAME);
}
Beispiel #28
0
void
carmen_saliency_define_saliency_points_message()
{
	IPC_RETURN_TYPE err;

	err = IPC_defineMsg(CARMEN_SALIENCY_SEARCH_SALIENCY_POINTS_MESSAGE_NAME, IPC_VARIABLE_LENGTH,
						CARMEN_SALIENCY_SEARCH_SALIENCY_POINTS_MESSAGE_FMT);
	carmen_test_ipc_exit(err, "Could not define", CARMEN_SALIENCY_SEARCH_SALIENCY_POINTS_MESSAGE_NAME);
}
void
carmen_slam6d_define_messages()
{
  IPC_RETURN_TYPE err;

  err = IPC_defineMsg(CARMEN_SLAM6D_POINTCLOUD_MESSAGE_NAME, IPC_VARIABLE_LENGTH,
		  CARMEN_SLAM6D_POINTCLOUD_MESSAGE_FMT);
  carmen_test_ipc_exit(err, "Could not define", CARMEN_SLAM6D_POINTCLOUD_MESSAGE_NAME);
}
void
carmen_landmark_localization_define_messages()
{
  IPC_RETURN_TYPE err;

  err = IPC_defineMsg(CARMEN_LANDMARK_LOCALIZATION_STATE_MESSAGE_NAME, IPC_VARIABLE_LENGTH,
		  CARMEN_LANDMARK_LOCALIZATION_STATE_MESSAGE_FMT);
  carmen_test_ipc_exit(err, "Could not define", CARMEN_LANDMARK_LOCALIZATION_STATE_MESSAGE_NAME);
}