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; }