static void
ipc_kinect_depth_handler(carmen_kinect_depth_message *message)
{
if (is_new_msg(message->timestamp, timestamp_depth))
{
message_laser.timestamp = carmen_get_time();
convert_depthmap_to_beams(message_laser.range, fov_in_degrees, message_laser.num_readings, message_laser.config.maximum_range,
message->depth, message->width-DEPTH_WIDTH_PADDING, message->height, message->size);
carmen_laser_publish_laser_message(message_laser.id, &message_laser);
timestamp_depth = message->timestamp;
}
}
void publish_depth(int kinect_id){
IPC_RETURN_TYPE err = IPC_OK;
char* msg_name=carmen_kinect_get_depth_messagename(kinect_id);
if (msg_depth[kinect_id].size > 0 && is_new_msg(msg_depth[kinect_id].timestamp, timestamp_depth)) {
timestamp_depth = msg_depth[kinect_id].timestamp;
err = IPC_publishData(msg_name, &(msg_depth[kinect_id]));
carmen_test_ipc_exit(err, "Could not publish", msg_name);
#ifdef DEBUG
printf("publishing depth timestamp: %f\n", timestamp_depth);
fflush(stdout);
#endif
}
}
void copy_video(int kinect_id, uint8_t* data, double timestamp, int size){
if (size > 0 && is_new_msg(timestamp, timestamp_video)) {
msg_video[kinect_id].id = kinect_id;
msg_video[kinect_id].width = kinect_video_width;
msg_video[kinect_id].height = kinect_video_height;
msg_video[kinect_id].size = size;
msg_video[kinect_id].timestamp = timestamp;
msg_video[kinect_id].host = carmen_get_host();
memcpy(msg_video[kinect_id].video, data, size*sizeof(unsigned char));
#ifdef DEBUG
printf("copying video timestamp: %f\n", timestamp);
fflush(stdout);
#endif
}
}
void copy_depth(int kinect_id, uint16_t* data, double timestamp, int size){
int i;
if (size > 0 && is_new_msg(timestamp, timestamp_depth)) {
msg_depth[kinect_id].id = kinect_id;
msg_depth[kinect_id].width = kinect_depth_width;
msg_depth[kinect_id].height = kinect_depth_height;
msg_depth[kinect_id].size = size;
msg_depth[kinect_id].timestamp = timestamp;
msg_depth[kinect_id].host = carmen_get_host();
for(i=0; i<size; i++)
msg_depth[kinect_id].depth[i] = convert_kinect_depth_raw_to_meters(data[i]);
#ifdef DEBUG
printf("copying depth timestamp: %f\n", timestamp);
fflush(stdout);
#endif
}
}
void process_received_message(message_header *mh, char msg[], int i) {
if (mh->type == PRIVATE) {
puts("\n\n\t receive private message ... \n");
process_private_msg(msg, i);
} else if (mh->type == BROADCAST) {
puts("\n\n\t Receive BROADCAST message ... \n");
if (is_new_msg(mh->id)) {
add_msg_to_container(mh->id);
process_broadcast_msg(mh, msg, i);
// check if broadcast bag have n citizen's peer token
if (is_salute == 0) {
int curr_peer_token = count_peer_token();
int curr_init_token = count_init_token();
if (curr_peer_token >= (max_citizen_number*2) && curr_init_token == max_citizen_number) {
is_salute = 1;
find_leader();
printf("\t leader(%s) is from %s:%d\n", leader.peer_token, leader.remote_ip, ntohs(leader.udp_port));
send_salute_message();
}
}
} else {
// check if broadcast bag have n citizen's peer token
if (is_salute == 0) {
int curr_peer_token = count_peer_token();
int curr_init_token = count_init_token();
if (curr_peer_token >= (max_citizen_number*2) && curr_init_token == max_citizen_number) {
is_salute = 1;
find_leader();
printf("\t leader(%s) is from %s:%d\n", leader.peer_token, leader.remote_ip, ntohs(leader.udp_port));
send_salute_message();
}
}
throw_exception(NOTE, "\t drop duplicate message");
return;
}
} else {
throw_exception(NOTE, "\t wrong type: message ignored");
}
}
void send_udp_message() {
// double check for network variables
inet_pton(AF_INET, local_ip, &network_ip);
self_peer_msg.ip = network_ip;
printf("sender ip: %s", local_ip);
self_peer_msg.udp_port = network_udp_port;
add_peer_token_to_container(self_peer_msg.peer_token, self_peer_msg.udp_port, self_peer_msg.ip, tcp_port);
//message body
char package[TOKEN_LENTH + 7] = {'\0'};memcpy
(package, self_peer_msg.peer_token, TOKEN_LENTH);
memcpy(package + TOKEN_LENTH, &network_ip, 4);
memcpy(package + TOKEN_LENTH + 4, &self_peer_msg.udp_port, 2);
/* begin broadcasting */
int i, sock_fd = -1;
for (i = 0; i < MAX_CONNECTIONS; i++) {
sock_fd = get_conn_fd(i);
if (sock_fd != -1) {
/* construct message header */
message_header mh;
init_header(&mh);
generate_message_id(mh.id);
mh.type = BROADCAST;
mh.payload_length = htons(strlen(package) + 1);
if (sizeof(mh) != 12) {
printf("mh length is %d\n", sizeof(mh));
throw_exception(ERROR, "sizeof(mh) != 12\n");
}
send_message(sock_fd, &mh, package);
/* add msg_id to msg_bag */
if (is_new_msg(mh.id))
add_msg_to_container(mh.id);
}
}
}