int
main(int argc, char *argv[])
{
int gps_nr = 0;
SerialDevice dev;
carmen_gps_gpgga_message gpgga;
carmen_gps_gprmc_message gprmc;
carmen_erase_structure(&gpgga, sizeof(carmen_gps_gpgga_message) );
carmen_erase_structure(&gprmc, sizeof(carmen_gps_gprmc_message) );
gpgga.host = carmen_get_host();
gprmc.host = carmen_get_host();
DEVICE_init_params( &dev );
carmen_ipc_initialize( argc, argv );
ipc_initialize_messages();
read_parameters( &dev, argc, argv );
carmen_extern_gpgga_ptr = &gpgga;
carmen_extern_gpgga_ptr->nr = gps_nr;
carmen_extern_gprmc_ptr = &gprmc;
carmen_extern_gprmc_ptr->nr = gps_nr;
fprintf( stderr, "INFO: ************************\n" );
fprintf( stderr, "INFO: ********* GPS ********\n" );
fprintf( stderr, "INFO: ************************\n" );
fprintf( stderr, "INFO: open device: %s\n", dev.ttyport );
if (DEVICE_connect_port( &dev )<0) {
fprintf( stderr, "ERROR: can't open device !!!\n\n" );
exit(1);
} else {
fprintf( stderr, "INFO: done\n" );
}
while(TRUE) {
if ( DEVICE_bytes_waiting( dev.fd )>10 ) {
if (DEVICE_read_data( dev )) {
gpgga.timestamp = carmen_get_time();
gprmc.timestamp = carmen_get_time();
ipc_publish_position();
}
usleep(100000);
} else {
carmen_ipc_sleep(0.25);
//IPC_listen(0);
//usleep(250000);
}
}
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;
}
carmen_navigator_ackerman_plan_message
build_navigator_ackerman_plan_message(carmen_ackerman_motion_command_p motion_commands_vector, int num_motion_commands)
{
int i, trajectory_vector_of_points_size;
carmen_navigator_ackerman_plan_message predicted_trajectory_message;
trajectory_vector_of_points_size = build_predicted_trajectory(motion_commands_vector, num_motion_commands, g_robot_position);
predicted_trajectory_message.path_length = trajectory_vector_of_points_size;
predicted_trajectory_message.path = (carmen_ackerman_traj_point_t *) malloc(sizeof(carmen_ackerman_traj_point_t) * (trajectory_vector_of_points_size));
for (i = 0; i < trajectory_vector_of_points_size; i++)
{
predicted_trajectory_message.path[i].x = trajectory_vector_of_points[i].x;
predicted_trajectory_message.path[i].y = trajectory_vector_of_points[i].y;
predicted_trajectory_message.path[i].theta = trajectory_vector_of_points[i].theta;
predicted_trajectory_message.path[i].v = trajectory_vector_of_points[i].v;
predicted_trajectory_message.path[i].phi = trajectory_vector_of_points[i].phi;
}
predicted_trajectory_message.timestamp = carmen_get_time();
predicted_trajectory_message.host = carmen_get_host();
return predicted_trajectory_message;
}
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);
}
static void initialize_arm_message(carmen_arm_state_message * arm)
{
arm->num_joints = arm_model.num_joints;
if( arm_model.num_joints > 0 ) {
arm->joint_angles = (double*)calloc( arm_model.num_joints, sizeof( double ) );
carmen_test_alloc( arm->joint_angles );
if( USE_ARM_CURRENT_STATES ) {
arm->flags |= USING_CURRENTS_MASK;
arm->num_currents = arm_model.num_joints;
arm->joint_currents = (double*)calloc( arm->num_currents, sizeof( double ) );
carmen_test_alloc( arm->joint_currents );
}
else
arm->joint_currents = NULL;
if( USE_ARM_ANGULAR_VEL_STATES ) {
arm->flags |= USING_ANGULAR_VEL_MASK;
arm->num_vels = arm_model.num_joints;
arm->joint_angular_vels = (double*)calloc( arm->num_vels, sizeof( double ) );
carmen_test_alloc( arm->joint_angular_vels );
}
else
arm->joint_angular_vels = NULL;
}
arm->host = carmen_get_host();
}
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);
}
static void
publish_carmen_base_ackerman_odometry_message(double timestamp)
{
IPC_RETURN_TYPE err = IPC_OK;
static carmen_base_ackerman_odometry_message odometry;
static int first = 1;
if (first)
{
odometry.host = carmen_get_host();
odometry.x = 0;
odometry.y = 0;
odometry.theta = 0;
odometry.v = odometry.phi = 0;
first = 0;
}
odometry.x = car_config->odom_pose.x;
odometry.y = car_config->odom_pose.y;
odometry.theta = car_config->odom_pose.theta;
odometry.v = car_config->v;
odometry.phi = car_config->phi;
odometry.timestamp = timestamp;
err = IPC_publishData(CARMEN_BASE_ACKERMAN_ODOMETRY_NAME, &odometry);
carmen_test_ipc(err, "Could not publish base_odometry_message", CARMEN_BASE_ACKERMAN_ODOMETRY_NAME);
}
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_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
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);
}
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 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 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);
}
}
static void
xsens_mtig_handler(carmen_xsens_mtig_message *message)
{
carmen_xsens_xyz_message xsens_xyz_message;
xsens_xyz_message.quat = message->quat;
xsens_xyz_message.acc = message->acc;
xsens_xyz_message.gyr = message->gyr;
xsens_xyz_message.mag = message->mag;
xsens_xyz_message.velocity = message->velocity;
Gdc_Coord_3d gdc = Gdc_Coord_3d(message->latitude, message->longitude, message->height);
Utm_Coord_3d utm;
Gdc_To_Utm_Converter::Init();
Gdc_To_Utm_Converter::Convert(gdc,utm);
// The axis are changed to match the carmen frame of reference
xsens_xyz_message.position.x = utm.y;
xsens_xyz_message.position.y = -utm.x;
xsens_xyz_message.position.z = utm.z;
xsens_xyz_message.gps_fix = message->gps_fix;
xsens_xyz_message.xkf_valid = message->xkf_valid;
xsens_xyz_message.sensor_ID = message->sensor_ID;
xsens_xyz_message.timestamp = message->timestamp;
xsens_xyz_message.host = carmen_get_host();
publish_xsens_xyz_message(xsens_xyz_message);
}
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 send_command(double v, double phi) {
velocity_message.v = v;
velocity_message.phi = phi;
velocity_message.host = carmen_get_host();
velocity_message.timestamp = carmen_get_time();
IPC_publishData(CARMEN_BASE_ACKERMAN_VELOCITY_NAME, &velocity_message);
}
carmen_default_message *carmen_default_message_create(void)
{
if (static_message.host == NULL)
static_message.host = carmen_get_host();
static_message.timestamp = carmen_get_time();
return &static_message;
}
void
copy_disparity_to_saliency_points_message(carmen_simple_stereo_disparity_message* message)
{
saliency_points.timestamp = message->timestamp;
saliency_points.host = carmen_get_host();
memcpy(saliency_points.reference_image, message->reference_image, image_height * image_width * 3);
}
void
carmen_laslam_initialize_landmark_message(int width, int height)
{
landmarks_message.host = carmen_get_host();
landmarks_message.image_height = height;
landmarks_message.image_width = width;
landmarks_message.image_size = 3 * width * height;
landmarks_message.reference_image = (uchar*)malloc(landmarks_message.image_size * sizeof(uchar));
}
static void
initialize_sonar_message(carmen_base_sonar_message *sonar)
{
double sensor_angle;
carmen_param_set_module("robot");
carmen_param_get_double("sensor_angle", &sensor_angle, NULL);
sonar->cone_angle = sensor_angle;
sonar->timestamp = 0.0;
sonar->host = carmen_get_host();
}
void
publish_fused(double x, double y, double theta, double timestamp)
{
static int first = 1;
static carmen_fused_odometry_particle_message odometry_message;
IPC_RETURN_TYPE err;
if (first)
{
odometry_message.particle_pos = (carmen_vector_3D_t *) malloc (1 * sizeof(carmen_vector_3D_t));
odometry_message.weights = (double *) malloc (1 * sizeof(double));
first = 0;
}
odometry_message.angular_velocity.pitch = 0;
odometry_message.angular_velocity.roll = 0;
odometry_message.angular_velocity.yaw = 0;
odometry_message.gps_position_at_turn_on.x = x;
odometry_message.gps_position_at_turn_on.y = y;
odometry_message.gps_position_at_turn_on.z = 0;
odometry_message.host = carmen_get_host();
odometry_message.num_particles = 1;
odometry_message.particle_pos[0].x = x;
odometry_message.particle_pos[0].y = y;
odometry_message.particle_pos[0].z = 0;
odometry_message.phi = 0;
odometry_message.pose.position.x = x;
odometry_message.pose.position.y = y;
odometry_message.pose.position.z = 0;
odometry_message.pose.orientation.yaw = theta;
odometry_message.pose.orientation.roll = 0;
odometry_message.pose.orientation.pitch = 0;
odometry_message.timestamp = timestamp;
odometry_message.velocity.x = 0;
odometry_message.velocity.y = 0;
odometry_message.velocity.z = 0;
odometry_message.weight_type = 0;
odometry_message.weights[0] = 1.0;
odometry_message.xsens_yaw_bias = 0;
err = IPC_publishData(CARMEN_FUSED_ODOMETRY_PARTICLE_NAME, &odometry_message);
carmen_test_ipc_exit(err, "Could not publish", CARMEN_FUSED_ODOMETRY_PARTICLE_NAME);
}
static void publish_vector_status(double distance, double angle)
{
static carmen_robot_vector_status_message msg;
int err;
msg.host = carmen_get_host();
msg.timestamp = carmen_get_time();
msg.vector_distance = distance;
msg.vector_angle = angle;
err = IPC_publishData(CARMEN_ROBOT_VECTOR_STATUS_NAME, &msg);
carmen_test_ipc(err, "Could not publish", CARMEN_ROBOT_VECTOR_STATUS_NAME);
}
void proccontrol_publish_output(int pid, char *output)
{
static carmen_proccontrol_output_message msg;
static int first = 1;
if(first) {
msg.host = carmen_get_host();
first = 0;
}
msg.pid = pid;
msg.output = output;
carmen_ipc_publish_exit(CARMEN_PROCCONTROL_OUTPUT_NAME, msg);
}
static void
publish_spline_path_message(carmen_ackerman_traj_point_t *poses, int number_of_poses)
{
IPC_RETURN_TYPE err;
carmen_navigator_spline_path_message spline_path_message;
spline_path_message.goal_list = poses;
spline_path_message.size = number_of_poses;
spline_path_message.host = carmen_get_host();
spline_path_message.timestamp = carmen_get_time();
err = IPC_publishData(CARMEN_NAVIGATOR_SPLINE_PATH_NAME, &spline_path_message);
carmen_test_ipc_exit(err, "Could not publish", CARMEN_NAVIGATOR_SPLINE_PATH_NAME);
}
static void
publish_spline_goal_list_message(carmen_ackerman_traj_point_t *poses, int number_of_poses)
{
IPC_RETURN_TYPE err;
carmen_behavior_selector_goal_list_message goal_list_msg;
poses->theta = globalpos.theta;
goal_list_msg.goal_list = poses;
goal_list_msg.size = number_of_poses;
goal_list_msg.host = carmen_get_host();
goal_list_msg.timestamp = carmen_get_time();
err = IPC_publishData(CARMEN_BEHAVIOR_SELECTOR_GOAL_LIST_NAME, &goal_list_msg);
carmen_test_ipc_exit(err, "Could not publish", CARMEN_BEHAVIOR_SELECTOR_GOAL_LIST_NAME);
}
void
carmen_mapper_moving_objects_raw_map_publish_message(carmen_map_t *carmen_map, double timestamp)
{
IPC_RETURN_TYPE err;
static carmen_mapper_map_message mapper_message;
mapper_message.config = carmen_map->config;
strcpy(mapper_message.config.origin, "from_mapping");
mapper_message.complete_map = carmen_map->complete_map;
mapper_message.size = carmen_map->config.x_size * carmen_map->config.y_size;
mapper_message.host = carmen_get_host();
mapper_message.timestamp = timestamp;
err = IPC_publishData(CARMEN_MAPPER_MAP_MOVING_OBJECTS_RAW_MAP_MESSAGE_NAME, &mapper_message);
carmen_test_ipc_exit(err, "Could not publish", CARMEN_MAPPER_MAP_MOVING_OBJECTS_RAW_MAP_MESSAGE_NAME);
}
carmen_fused_odometry_message
assemble_fused_odometry_message(carmen_fused_odometry_particle_message particle_message)
{
carmen_fused_odometry_message message;
message.pose = particle_message.pose;
message.xsens_yaw_bias = particle_message.xsens_yaw_bias;
message.velocity = particle_message.velocity;
message.angular_velocity = particle_message.angular_velocity;
message.phi = particle_message.phi;
message.gps_position_at_turn_on = particle_message.gps_position_at_turn_on;
message.timestamp = particle_message.timestamp;
message.host = carmen_get_host();
return (message);
}
static void
publish_car_status()
{
IPC_RETURN_TYPE err = IPC_OK;
carmen_ford_escape_status_message msg;
msg.g_XGV_velocity = g_XGV_velocity;
msg.g_XGV_atan_curvature = g_XGV_atan_curvature;
msg.g_XGV_brakes = g_XGV_brakes;
msg.g_XGV_gear = g_XGV_gear;
msg.g_XGV_headlights_status = g_XGV_headlights_status;
msg.g_XGV_horn_status = g_XGV_horn_status;
msg.g_XGV_right_front_wheel_speed = g_XGV_right_front_wheel_speed;
msg.g_XGV_left_front_wheel_speed = g_XGV_left_front_wheel_speed;
msg.g_XGV_right_rear_wheel_speed = g_XGV_right_rear_wheel_speed;
msg.g_XGV_left_rear_wheel_speed = g_XGV_left_rear_wheel_speed;
msg.g_XGV_main_fuel_supply = g_XGV_main_fuel_supply;
msg.g_XGV_main_propulsion = g_XGV_main_propulsion;
msg.g_XGV_parking_brake = g_XGV_parking_brake;
msg.g_XGV_steering = g_XGV_steering;
msg.g_XGV_throttle = g_XGV_throttle;
msg.g_XGV_turn_signal = g_XGV_turn_signal;
// g_XGV_component_status bit Interpretation F: disengaged, T: engaged (see page 62 of ByWire XGV User Manual, Version 1.5)
// 0 (16) Manual override
// 1 (17) SafeStop pause relay F: run, T: pause
// 2 (18) SafeStop stop relay F: run, T: stop
// 3-4 (19-20) SafeStop link status 7 (23) Partial mode override: speed 0: no link, 1: bypass, 2: link good
// 5 (21) External E-Stop button F: run, T: stop
// 6 (22) Partial mode override: F: computer control, T: manual
// steering control
// F: computer control, T: manual
// control
// 8 (24) Door/Liftgate pause F: run, T: pause
// 9 (25) Error-caused pause* F: run, T: pause
// 10 (26) Emergency manual override* F: disengaged, T: engaged
// 11 (27) Steering needs initialization* F: initialized, T: uninitialized
// Steering initialization waiting F: initialized, T: user must init
// on user input* steering manually or press OK
msg.g_XGV_component_status = g_XGV_component_status;
// printf("g_XGV_component_status = 0x%x\n", g_XGV_component_status);
msg.timestamp = carmen_get_time();
msg.host = carmen_get_host();
err = IPC_publishData(CARMEN_FORD_ESCAPE_STATUS_NAME, &msg);
carmen_test_ipc(err, "Could not publish", CARMEN_FORD_ESCAPE_STATUS_NAME);
}
void
init_message_laser_params(int laser_id, double accuracy, double fov, double resolution, double maxrange)
{
fov_in_degrees = fov;
message_laser.id = laser_id;
message_laser.num_remissions=0;
message_laser.config.remission_mode = REMISSION_NONE;
message_laser.config.laser_type = LASER_EMULATED_USING_KINECT;
message_laser.config.fov = carmen_degrees_to_radians(fov);
message_laser.config.start_angle = -0.5 * message_laser.config.fov;
message_laser.config.angular_resolution = carmen_degrees_to_radians(resolution);
message_laser.num_readings=1 + carmen_round(message_laser.config.fov / message_laser.config.angular_resolution);
message_laser.config.maximum_range = maxrange;
message_laser.config.accuracy = accuracy;
message_laser.host = carmen_get_host();
message_laser.range = laser_ranges;
}
