Beispiel #1
0
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;
	}
}
Beispiel #2
0
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
	}
}
Beispiel #3
0
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
	}
}
Beispiel #4
0
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);
		}
	}

}