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 subject_publish_callback(const ViconDriver::Subject &subject)
{
static std::set<std::string> defined_msgs;
std::string msgname = "vicon_" + subject.name;
if(defined_msgs.find(msgname) == defined_msgs.end())
{
if(pthread_mutex_trylock(&ipc_mutex) == 0)
{
IPC_defineMsg(msgname.c_str(), IPC_VARIABLE_LENGTH, ViconSubject::getIPCFormat());
pthread_mutex_unlock(&ipc_mutex);
defined_msgs.insert(msgname);
}
else
return;
}
if(loadCalib(subject.name))
{
Eigen::Affine3d current_pose = Eigen::Affine3d::Identity();
current_pose.translate(Eigen::Vector3d(subject.translation));
current_pose.rotate(Eigen::Quaterniond(subject.rotation));
current_pose = current_pose*calib_pose[subject.name];
const Eigen::Vector3d position(current_pose.translation());
const Eigen::Quaterniond rotation(current_pose.rotation());
ViconSubject subject_ipc;
subject_ipc.time_sec = subject.time_usec/1000000; // Integer division
subject_ipc.time_usec = subject.time_usec - subject_ipc.time_sec*1000000;
subject_ipc.frame_number = subject.frame_number;
subject_ipc.name = const_cast<char*>(subject.name.c_str());
subject_ipc.occluded = subject.occluded;
subject_ipc.position[0] = position.x();
subject_ipc.position[1] = position.y();
subject_ipc.position[2] = position.z();
subject_ipc.orientation[0] = rotation.x();
subject_ipc.orientation[1] = rotation.y();
subject_ipc.orientation[2] = rotation.z();
subject_ipc.orientation[3] = rotation.w();
subject_ipc.num_markers = subject.markers.size();
subject_ipc.markers = new ViconMarker[subject_ipc.num_markers];
for(int i = 0; i < subject_ipc.num_markers; i++)
{
subject_ipc.markers[i].name =
const_cast<char*>(subject.markers[i].name.c_str());
subject_ipc.markers[i].subject_name =
const_cast<char*>(subject.markers[i].subject_name.c_str());
subject_ipc.markers[i].position[0] = subject.markers[i].translation[0];
subject_ipc.markers[i].position[1] = subject.markers[i].translation[1];
subject_ipc.markers[i].position[2] = subject.markers[i].translation[2];
subject_ipc.markers[i].occluded = subject.markers[i].occluded;
}
if(pthread_mutex_trylock(&ipc_mutex) == 0)
{
IPC_publishData(msgname.c_str(), &subject_ipc);
pthread_mutex_unlock(&ipc_mutex);
}
delete[] subject_ipc.markers;
}
}
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;
}
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_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);
}
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);
}
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);
}
/* Function to create msg2 and publish a message called "ListeningToC"
*/
static void sendZoeResponse(drive_arc_command_msg_type msg1) {
// Creating the response message
drive_arc_response_msg_type msg2;
msg2.command.radius = msg1.radius;
msg2.command.time = msg1.time;
msg2.command.speed = msg1.speed;
strcpy(msg2.command.sender.data, msg1.sender.data);
msg2.command.timestamp= msg1.timestamp;
msg2.returnData.returnValue=1;
if( (msg1.radius > 0 && msg1.radius <= 1000) && (msg1.speed >= (-1) && msg1.speed <= 1)) {
msg2.returnData.isValid=1;
}
strcpy(msg2.responseSender.data, id);
gettimeofday(&msg2.timestamp, NULL);
//Writing to log file
fp=fopen("CommandLog.txt","a");
fprintf(fp,"\nResponse Received Command Radius: %f , Speed: %f , Time: %f",msg2.command.radius,msg2.command.speed,msg2.command.time);
fprintf(fp,"\tSender: %s",msg2.command.sender.data);
time_t commandTime,responseTime;
commandTime=msg2.command.timestamp.tv_sec;
fprintf(fp,"\tTimestamp: %s", ctime(&commandTime));
fprintf(fp,"Response Sender: %s",msg2.responseSender.data);
responseTime=msg2.timestamp.tv_sec;
fprintf(fp,"\tResponse Timestamp: %s\n", ctime(&commandTime));
fclose(fp);
IPC_publishData(MESSAGE_NAME_DRIVE_ARC_RESPONSE, &msg2); /*send the message*/
}
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 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
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 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;
}
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
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);
}
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 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_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_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 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);
}
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);
}
void
publish_fused_odometry(void)
{
IPC_RETURN_TYPE err;
carmen_fused_odometry_message fused_odometry_message;
err = IPC_publishData(CARMEN_FUSED_ODOMETRY_NAME, &fused_odometry_message);
carmen_test_ipc_exit(err, "Could not publish fused odometry", CARMEN_FUSED_ODOMETRY_NAME);
}
int read_message(int message_num, int publish, int no_wait)
{
// char *line[MAX_LINE_LENGTH];
char *line;
char *current_pos;
int i, j;
char command[100];
static double last_update = 0;
double current_time;
line = (char *) malloc(MAX_LINE_LENGTH * sizeof(char));
if (line == NULL)
carmen_die("Could not alloc memory in playback.c:read_message()\n");
carmen_logfile_read_line(logfile_index, logfile, message_num,
MAX_LINE_LENGTH, line);
current_pos = carmen_next_word(line);
for(i = 0; i < (int)(sizeof(logger_callbacks) /
sizeof(logger_callback_t)); i++) {
/* copy the command over */
j = 0;
while(line[j] != ' ') {
command[j] = line[j];
j++;
}
command[j] = '\0';
if(strncmp(command, logger_callbacks[i].logger_message_name, j) == 0) {
if(!basic_messages || !logger_callbacks[i].interpreted) {
current_pos =
logger_callbacks[i].conv_func(current_pos,
logger_callbacks[i].message_data);
playback_timestamp = atof(current_pos);
//printf("command = %s, playback_timestamp = %lf\n", command, playback_timestamp);
if(publish) {
current_time = carmen_get_time();
if(current_time - last_update > 0.2) {
print_playback_status();
last_update = current_time;
}
if (!no_wait)
wait_for_timestamp(playback_timestamp);
IPC_publishData(logger_callbacks[i].ipc_message_name,
logger_callbacks[i].message_data);
}
/* return 1 if it is a front laser message */
free(line);
return (strcmp(command, "FLASER") == 0);
}
}
}
free(line);
return 0;
}
IPC_RETURN_TYPE
carmen_minoru_publish_message(
const carmen_minoru_stereoimage_message *message)
{
IPC_RETURN_TYPE err;
err = IPC_publishData(CARMEN_MINORU_STEREOIMAGE_NAME, (carmen_minoru_stereoimage_message *)message);
carmen_test_ipc_exit(err, "Could not publish", CARMEN_MINORU_STEREOIMAGE_NAME);
return err;
}
IPC_RETURN_TYPE
carmen_minoru_publish_bumblebee_fake_message(
const carmen_minoru_stereoimage_message *message)
{
IPC_RETURN_TYPE err;
err = IPC_publishData(CARMEN_BUMBLEBEE_BASIC_STEREOIMAGE10_NAME, (carmen_minoru_stereoimage_message *)message);
carmen_test_ipc_exit(err, "Could not publish", CARMEN_BUMBLEBEE_BASIC_STEREOIMAGE10_NAME);
return err;
}
void
localize_ackerman_velodyne_publish_frontlaser(double timestamp, double *laser_ranges)
{
IPC_RETURN_TYPE err = IPC_OK;
localize_ackerman_velodyne_create_frontlaser_message(timestamp, laser_ranges);
err = IPC_publishData(CARMEN_LASER_FRONTLASER_NAME, &flaser_message);
carmen_test_ipc(err, "Could not publish laser_frontlaser_message",
CARMEN_LASER_FRONTLASER_NAME);
}
void periodic_publish_goal()
{
static carmen_behavior_selector_goal_list_message message;
static carmen_ackerman_traj_point_t goal;
goal.x = goalpos.x;
goal.y = goalpos.y;
goal.theta = goalpos.theta;
message.goal_list = &goal;
message.size = 1;
IPC_publishData(CARMEN_BEHAVIOR_SELECTOR_GOAL_LIST_NAME, &message);
}
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);
}
IPC_RETURN_TYPE
carmen_mapping_traffic_light_publish_message(carmen_mapping_traffic_light_message *message)
{
IPC_RETURN_TYPE err;
char *message_name = carmen_mapping_traffic_light_message_name();
err = IPC_publishData(message_name, message);
carmen_test_ipc_exit(err, "Could not publish", message_name);
free(message_name);
return err;
}
IPC_RETURN_TYPE
carmen_bumblebee_basic_publish_message(int camera,
const carmen_bumblebee_basic_stereoimage_message *message)
{
IPC_RETURN_TYPE err;
char *message_name = carmen_bumblebee_basic_message_name(camera);
err = IPC_publishData(message_name, (carmen_bumblebee_basic_stereoimage_message *)message);
carmen_test_ipc_exit(err, "Could not publish", message_name);
free(message_name);
return err;
}
