gboolean on_transmit_goals(gpointer _user) { RendererGoal *self = (RendererGoal*) _user; pthread_mutex_lock(&self->mutex); lcmtypes_goal_list_t glist; glist.num_goals = self->my_goals->len; glist.goals = (lcmtypes_goal_t *) self->my_goals->data; glist.utime = timestamp_now(); glist.sender_id = self->sender_id; lcmtypes_goal_list_t_publish(self->lc, "GOALS", &glist); pthread_mutex_unlock(&self->mutex); if (glist.num_goals == 0) self->empty_count++; else self->empty_count = 0; /* If goal list is empty, stop transmitting shortly afterwards */ if (self->empty_count > 20) { self->transmitter = 0; return FALSE; } return TRUE; }
overlay::overlay(client_service_sptr clientService, int topologyID) : basic_engine_object(clientService->get_io_service()) , basic_client_object(clientService->get_client_param_sptr()) , client_service_(clientService) , topology_id_(topologyID) , neighbors_conn_((topologyID == STREAM_TOPOLOGY) ? STREAM_NEIGHTBOR_PEER_CNT : HUB_NEIGHTBOR_PEER_CNT) { set_obj_desc("overlay"); timestamp_t now = timestamp_now(); last_exchange_neighbor_time_ = now; last_member_request_time_ = now; if (topology_id_ == STREAM_TOPOLOGY) { max_neighbor_cnt_ = get_client_param_sptr()->stream_neighbor_peer_cnt; neighbor_ping_interval_ = STREAM_PEER_PEER_PING_INTERVAL; } else if (topology_id_ == HUB_TOPOLOGY) { max_neighbor_cnt_ = get_client_param_sptr()->hub_neighbor_peer_cnt; neighbor_ping_interval_ = HUB_PEER_PEER_PING_INTERVAL; } else if (topology_id_ == CACHE_TOPOLOGY) { max_neighbor_cnt_ = get_client_param_sptr()->stream_neighbor_peer_cnt; neighbor_ping_interval_ = STREAM_PEER_PEER_PING_INTERVAL; } else { BOOST_ASSERT(0 && "必须明确指定拓扑的max_neighbor_cnt_、neighbor_ping_interval_等"); } }
int main(void){ cc3d_init(); printf("SystemCoreClock: %d\n", (int)SystemCoreClock); // test config read/write (to eeprom) const char str[] = "Hello World!"; uint8_t buf[13] = {0}; printf("Writing string to config: %s\n", str); cc3d_write_config((const uint8_t*)str, sizeof(str)); printf("Reading string from config: "); cc3d_read_config(buf, sizeof(str)); printf("%s\n", buf); uint8_t led_state = 0; timestamp_t ts = timestamp_from_now_us(500000); serial_dev_t flexiport = cc3d_get_flexiport_serial_interface(); unsigned int loop = 0; while(1){ printf("RC1: %d ", (int)cc3d_read_pwm(CC3D_IN_PWM1)); printf("RC2: %d ", (int)cc3d_read_pwm(CC3D_IN_PWM2)); printf("RC3: %d ", (int)cc3d_read_pwm(CC3D_IN_PWM3)); printf("RC4: %d ", (int)cc3d_read_pwm(CC3D_IN_PWM4)); printf("RC5: %d ", (int)cc3d_read_pwm(CC3D_IN_PWM5)); printf("RC6: %d ", (int)cc3d_read_pwm(CC3D_IN_PWM6)); cc3d_write_pwm(CC3D_OUT_PWM1, 1000 + loop % 1000); cc3d_write_pwm(CC3D_OUT_PWM2, 1000 + loop % 1000); cc3d_write_pwm(CC3D_OUT_PWM3, 1000 + loop % 1000); cc3d_write_pwm(CC3D_OUT_PWM4, 1000 + loop % 1000); cc3d_write_pwm(CC3D_OUT_PWM5, 1000 + loop % 1000); cc3d_write_pwm(CC3D_OUT_PWM6, 1000 + loop % 1000); float ax, ay, az, gx, gy, gz; cc3d_read_acceleration_g(&ax, &ay, &az); cc3d_read_angular_velocity_dps(&gx, &gy, &gz); printf("Time: %ld ", (long int)timestamp_now()); printf("Gyro: %5d %5d %5d, Acc: %5d %5d %5d\n", (int)(ax * 1000), (int)(ay * 1000), (int)(az * 1000), (int)(gx * 1000), (int)(gy * 1000), (int)(gz * 1000)); serial_printf(flexiport, "Hello World!\n"); if(timestamp_expired(ts)){ led_state = ~led_state; if(led_state) cc3d_led_on(); else cc3d_led_off(); ts = timestamp_from_now_us(500000); loop += 100; } } }
static int lcm_tcpq_handle(lcm_tcpq_t * self) { if(self->socket < 0 && 0 != _connect_to_server(self)) { return -1; } // read, ignore message type uint32_t msg_type; if(_recv_uint32(self->socket, &msg_type)) goto disconnected; // read channel length, channel uint32_t channel_len; if(_recv_uint32(self->socket, &channel_len)) goto disconnected; if(_ensure_buf_capacity((void**)&self->recv_channel_buf, &self->recv_channel_buf_len, channel_len+1)) { fprintf(stderr, "Memory allocation error\n"); return -1; } if(channel_len != _recv_fully(self->socket, self->recv_channel_buf, channel_len)) goto disconnected; self->recv_channel_buf[channel_len] = 0; // read payload size, payload uint32_t data_len; if(_recv_uint32(self->socket, &data_len)) goto disconnected; if(_ensure_buf_capacity(&self->data_buf, &self->data_buf_len, data_len)) { fprintf(stderr, "Memory allocation error\n"); return -1; } if(data_len != _recv_fully(self->socket, self->data_buf, data_len)) goto disconnected; lcm_recv_buf_t rbuf; rbuf.data = self->data_buf; rbuf.data_size = data_len; rbuf.recv_utime = timestamp_now(); rbuf.lcm = self->lcm; if(lcm_try_enqueue_message(self->lcm, self->recv_channel_buf)) lcm_dispatch_handlers(self->lcm, &rbuf, self->recv_channel_buf); return 0; disconnected: _close_socket(self->socket); self->socket = -1; return -1; }
static gboolean on_timer(void *user_data) { state_t *app = (state_t*) user_data; int64_t now = timestamp_now(); if(now > app->next_report_utime) { // int nhosts = g_hash_table_size(app->hosts); // printf("%d host%s transmitting\n", nhosts, nhosts == 1 ? "" : "s"); app->next_report_utime = now + app->report_interval_usec; } return TRUE; }
void *gps_read_proc(void *v) { gps_t *g = (gps_t*) v; uint8_t buf[GPS_MESSAGE_MAXLEN + 1]; buf[GPS_MESSAGE_MAXLEN] = 0; while (1) { int res = gps_readline(g, buf, GPS_MESSAGE_MAXLEN); if (!res && g->readline_callback != NULL) { int64_t now = timestamp_now(); g->readline_callback(g->readline_callback_context, now, (const char*) buf); } } return NULL; }
int main(int argc, char **argv) { state_t *app = calloc(1, sizeof(state_t)); app->lcmurl = strdup(get_default_lcm_url()); if (0 != lcm_parse_url(app->lcmurl, &app->mc_addr, &app->mc_port)) { fprintf (stderr, "invalid URL [%s]\n", app->lcmurl); return 1; } struct in_addr ia; ia = app->mc_addr; printf("MC group: %s port: %d\n", inet_ntoa(ia), app->mc_port); if(0 != setup_socket(app)) { return 1; } app->hosts = g_hash_table_new_full(g_int_hash, g_int_equal, NULL, (GDestroyNotify)host_destroy); app->mainloop = g_main_loop_new(NULL, FALSE); app->ioc = g_io_channel_unix_new(app->recvfd); app->sid = g_io_add_watch(app->ioc, G_IO_IN, (GIOFunc)on_message_ready, app); g_timeout_add(100, on_timer, app); app->report_interval_usec = DEFAULT_REPORT_INTERVAL_SECONDS * 1000000; app->next_report_utime = timestamp_now() + 1000000; bot_signal_pipe_glib_quit_on_kill(app->mainloop); g_main_loop_run(app->mainloop); g_io_channel_unref(app->ioc); g_source_remove(app->sid); shutdown(app->recvfd, SHUT_RDWR); g_hash_table_destroy(app->hosts); free(app->lcmurl); free(app); return 0; }
void writeDataFromPipe(int in, int out) { /* reads data from `in`, formats it, writes it to `out`. * mode 0 - read from pipe */ unsigned char buffer[INBUFFSIZE]; char outbuf[OUTBUFFSIZE]; int n; char tstamp[30]; int tstamp_len; if ((n = read(in, buffer, INBUFFSIZE)) < 0) { /* read nothing */ perror(RPIPEFAIL); } else { /* successfully read something from somewhere */ if (n >=8) { /* need at least 8 bytes for a meter packet */ /* read from pipe */ /* print timestamp if necessary */ if (tty_data.tstamp) { timestamp_now( timeStamp ); tstamp_len = timestamp_sprint( timeStamp, tstamp, 30 ); write(out, tstamp, tstamp_len); write(out, "\t", 1); } /* format data */ fmtData(buffer, outbuf, n); /* print data */ write(out, outbuf, strlen(outbuf)); /* count (decoded packet) writes */ tty_data.numSamples++; /* print total number of bytes if necessary */ if (tty_data.dspbytes) { buffer[0] = 0; sprintf(buffer, "\t%s %i\n", TOTALBYTES, n); write (out, buffer, strlen(buffer)); } } /* end if n >= 8 */ } /* end else there were characters to read */ } /* end writeData() */
static int on_message_ready(GIOChannel *source, GIOCondition cond, void *user_data) { state_t *app = (state_t*) user_data; char buf[65536]; struct sockaddr_in from; socklen_t fromlen = sizeof(struct sockaddr); int sz = recvfrom(app->recvfd, buf, 65536, 0, (struct sockaddr*) &from, &fromlen); if (sz < 0) { perror("receiving message"); return TRUE; } int64_t recv_utime = timestamp_now(); time_t recv_t = recv_utime / 1000000; struct tm *recv_tm = localtime(&recv_t); char recv_tm_buf[200]; strftime(recv_tm_buf, sizeof(recv_tm_buf), "%b %d %H:%M:%S", recv_tm); int *key = (int*) &from.sin_addr.s_addr; host_t * host = g_hash_table_lookup(app->hosts, key); if(!host) { host = host_new(from.sin_addr); g_hash_table_insert(app->hosts, &host->addr.s_addr, host); printf("%s - new host detected! %s\n", recv_tm_buf, host->addr_str); } sender_t *sender = host_get_sender(host, ntohs(from.sin_port)); if(!sender) { sender = host_add_new_sender(host, ntohs(from.sin_port)); printf("%s - new sender detected! %s\n", recv_tm_buf, sender->id_str); } sender->last_recvtime = recv_utime; host->last_recvtime = recv_utime; return TRUE; }
static int lcm_memq_publish(lcm_memq_t *self, const char *channel, const void *data, unsigned int datalen) { if (!lcm_has_handlers(self->lcm, channel)) { dbg(DBG_LCM, "Publishing [%s] size [%d] - dropping (no subscribers)\n", channel, datalen); return 0; } dbg(DBG_LCM, "Publishing to [%s] message size [%d]\n", channel, datalen); memq_msg_t *msg = memq_msg_new(self->lcm, channel, data, datalen, timestamp_now()); g_mutex_lock(self->mutex); int was_empty = g_queue_is_empty(self->queue); g_queue_push_tail(self->queue, msg); if (was_empty) { if (lcm_internal_pipe_write(self->notify_pipe[1], "+", 1) < 0) { perror(__FILE__ " - write to notify pipe (lcm_memq_publish)"); } } g_mutex_unlock(self->mutex); return 0; }
void absent_packet_list::erase(seqno_t seqno) { BOOST_AUTO(itr, not_requesting_packets_.find(seqno)); if (itr != not_requesting_packets_.end()) { erase(itr, false); } else if ((itr = requesting_packets_.find(seqno)) != requesting_packets_.end()) { erase(itr, true); } else { BOOST_AUTO(&pktInfoPtr, get_slot(media_packet_info_vector_, seqno)); if (pktInfoPtr&&pktInfoPtr->is_this(seqno, timestamp_now())) { __delete(seqno); } } BOOST_ASSERT(!find(seqno)); }
static void * timer_thread (void * user) { lcm_logprov_t * lr = (lcm_logprov_t *) user; int64_t abstime; struct timeval sleep_tv; while (lcm_internal_pipe_read(lr->timer_pipe[0], &abstime, 8) == 8) { if (abstime < 0) return NULL; int64_t now = timestamp_now(); if (abstime > now) { int64_t sleep_utime = abstime - now; sleep_tv.tv_sec = sleep_utime / 1000000; sleep_tv.tv_usec = sleep_utime % 1000000; // sleep until the next timed message, or until an abort message fd_set fds; FD_ZERO (&fds); FD_SET (lr->timer_pipe[0], &fds); int status = select (lr->timer_pipe[0] + 1, &fds, NULL, NULL, &sleep_tv); if (0 == status) { // select timed out if(lcm_internal_pipe_write(lr->notify_pipe[1], "+", 1) < 0) { perror(__FILE__ " - write (timer select)"); } } } else { if(lcm_internal_pipe_write(lr->notify_pipe[1], "+", 1) < 0) { perror(__FILE__ " - write (timer)"); } } } perror ("timer_thread read failed"); return NULL; }
/* output to telemetry * whatever 'tag' means is up to the user, and can just be set to 0, but * was designed to be used for tracking specific objects as they head around * the place */ void telemetry_vprintf(uint32_t tag, const char *format, va_list vargs) { char telem_str[TELEMETRY_MAX_LINE_LENGTH]; char annotated_telem_str[TELEMETRY_MAX_LINE_LENGTH]; if (! telemetry_running) return; /* Build the string for the supplied stuff */ vsnprintf(telem_str, TELEMETRY_MAX_LINE_LENGTH-1, format, vargs); telem_str[TELEMETRY_MAX_LINE_LENGTH-1] = 0; /* Add our own stuff to give context */ snprintf(annotated_telem_str, TELEMETRY_MAX_LINE_LENGTH-1, "%s %lu %08x %s", telemetry_client_name, timestamp_now(), tag, telem_str); annotated_telem_str[TELEMETRY_MAX_LINE_LENGTH-1] = 0; /* Output */ if (telemetry_output_file != NULL) fprintf((FILE *)telemetry_output_file, "%s\n", annotated_telem_str); if (telemetry_context != NULL) telemetry_zmq_send(annotated_telem_str, strlen(annotated_telem_str)); }
static void* write_thread(void *user_data) { logger_t *logger = (logger_t*) user_data; GTimeVal start_time; g_get_current_time(&start_time); int num_splits = 0; int64_t next_split_sec = start_time.tv_sec + logger->auto_split_hours * SECONDS_PER_HOUR; while(1) { void *msg = g_async_queue_pop(logger->write_queue); // Is it time to start a new logfile? int split_log = 0; if(logger->auto_split_hours) { GTimeVal now; g_get_current_time(&now); split_log = (now.tv_sec > next_split_sec); } else if(logger->auto_split_mb) { double logsize_mb = (double)logger->logsize / (1 << 20); split_log = (logsize_mb > logger->auto_split_mb); } if(_reset_logfile) { split_log = 1; _reset_logfile = 0; } if(split_log) { // Yes. open up a new log file lcm_eventlog_destroy(logger->log); if(0 != open_logfile(logger)) exit(1); num_splits++; int64_t log_duration_sec = logger->auto_split_hours * SECONDS_PER_HOUR; next_split_sec = start_time.tv_sec + (num_splits + 1) * log_duration_sec; logger->logsize = 0; logger->last_report_logsize = 0; } // Should the write thread exit? g_mutex_lock(logger->mutex); if(logger->write_thread_exit_flag) { g_mutex_unlock(logger->mutex); return NULL; } // nope. write the event to disk lcm_eventlog_event_t *le = (lcm_eventlog_event_t*) msg; int64_t sz = sizeof(lcm_eventlog_event_t) + le->channellen + 1 + le->datalen; logger->write_queue_size -= sz; g_mutex_unlock(logger->mutex); if(0 != lcm_eventlog_write_event(logger->log, le)) { static int64_t last_spew_utime = 0; char *reason = strdup(strerror(errno)); int64_t now = timestamp_now(); if(now - last_spew_utime > 500000) { fprintf(stderr, "lcm_eventlog_write_event: %s\n", reason); last_spew_utime = now; } free(reason); free(le); if(errno == ENOSPC) { exit(1); } else { continue; } } if (logger->fflush_interval_ms >= 0 && (le->timestamp - logger->last_fflush_time) > logger->fflush_interval_ms*1000) { fflush(logger->log->f); logger->last_fflush_time = le->timestamp; } // bookkeeping, cleanup int64_t offset_utime = le->timestamp - logger->time0; logger->nevents++; logger->events_since_last_report ++; logger->logsize += 4 + 8 + 8 + 4 + le->channellen + 4 + le->datalen; free(le); if (offset_utime - logger->last_report_time > 1000000) { double dt = (offset_utime - logger->last_report_time)/1000000.0; double tps = logger->events_since_last_report / dt; double kbps = (logger->logsize - logger->last_report_logsize) / dt / 1024.0; printf("Summary: %s ti:%4"PRIi64"sec Events: %-9"PRIi64" ( %4"PRIi64" MB ) TPS: %8.2f KB/s: %8.2f\n", logger->fname, timestamp_seconds(offset_utime), logger->nevents, logger->logsize/1048576, tps, kbps); logger->last_report_time = offset_utime; logger->events_since_last_report = 0; logger->last_report_logsize = logger->logsize; } } }
int main(int argc, char *argv[]) { auto options = std::unique_ptr<getopt_t, void(*)(getopt_t*)> (getopt_create(), getopt_destroy); // getopt_t *options = getopt_create(); getopt_add_bool(options.get(), 'h', "help", 0, "Show this help"); getopt_add_bool(options.get(), 'd', "debug", 0, "Enable debugging output (slow)"); getopt_add_bool(options.get(), 'w', "window", 1, "Show the detected tags in a window"); getopt_add_bool(options.get(), 'q', "quiet", 0, "Reduce output"); getopt_add_int(options.get(), '\0', "border", "1", "Set tag family border size"); getopt_add_int(options.get(), 't', "threads", "4", "Use this many CPU threads"); getopt_add_double(options.get(), 'x', "decimate", "1.0", "Decimate input image by this factor"); getopt_add_double(options.get(), 'b', "blur", "0.0", "Apply low-pass blur to input"); getopt_add_bool(options.get(), '0', "refine-edges", 1, "Spend more time trying to align edges of tags"); getopt_add_bool(options.get(), '1', "refine-decode", 0, "Spend more time trying to decode tags"); getopt_add_bool(options.get(), '2', "refine-pose", 0, "Spend more time trying to precisely localize tags"); getopt_add_double(options.get(), 's', "size", "0.04047", "Physical side-length of the tag (meters)"); getopt_add_int(options.get(), 'c', "camera", "0", "Camera ID"); getopt_add_int(options.get(), 'i', "tag_id", "-1", "Tag ID (-1 for all tags in family)"); if (!getopt_parse(options.get(), argc, argv, 1) || getopt_get_bool(options.get(), "help")) { printf("Usage: %s [options]\n", argv[0]); getopt_do_usage(options.get()); exit(0); } AprilTagDetector tag_detector(options); auto lcm = std::make_shared<lcm::LCM>(); Eigen::Matrix3d camera_matrix = Eigen::Matrix3d::Identity(); // camera_matrix(0,0) = bot_camtrans_get_focal_length_x(mCamTransLeft); // camera_matrix(1,1) = bot_camtrans_get_focal_length_y(mCamTransLeft); // camera_matrix(0,2) = bot_camtrans_get_principal_x(mCamTransLeft); // camera_matrix(1,2) = bot_camtrans_get_principal_y(mCamTransLeft); camera_matrix(0,0) = 535.04778754; camera_matrix(1,1) = 533.37100256; camera_matrix(0,2) = 302.83654976; camera_matrix(1,2) = 237.69023961; Eigen::Vector4d distortion_coefficients(-7.74010810e-02, -1.97835565e-01, -4.47956948e-03, -5.42361499e-04); // camera matrix: // [[ 535.04778754 0. 302.83654976] // [ 0. 533.37100256 237.69023961] // [ 0. 0. 1. ]] // distortion coefficients: [ -7.74010810e-02 -1.97835565e-01 -4.47956948e-03 -5.42361499e-04 // 9.30985112e-01] cv::VideoCapture capture(getopt_get_int(options.get(), "camera")); if (!capture.isOpened()) { std::cout << "Cannot open the video cam" << std::endl; return -1; } cv::Mat frame; Eigen::Isometry3d tag_to_camera = Eigen::Isometry3d::Identity(); crazyflie_t::webcam_pos_t tag_to_camera_msg; while (capture.read(frame)) { std::vector<TagMatch> tags = tag_detector.detectTags(frame); if (tags.size() > 0) { tag_to_camera = getRelativeTransform(tags[0], camera_matrix, distortion_coefficients, tag_detector.getTagSize()); tag_to_camera_msg = encodeWebcamPos(tag_to_camera); tag_to_camera_msg.frame_id = 1; } else { tag_to_camera_msg = encodeWebcamPos(tag_to_camera); tag_to_camera_msg.frame_id = -1; } tag_to_camera_msg.timestamp = timestamp_now(); lcm->publish("WEBCAM_POS", &tag_to_camera_msg); } return 0; }
/*! \brief Thread routine I/O * * @param ptr pointer to Ports type with input and output ip address and port. */ void *datap_io_thread (void* ptr) { Ports* port_info; SensorID sensor_listen_id; port_info = (Ports*) ptr; int i = 0; int status; int retval; int clientsocket_fd; int numbytes; socklen_t addr_len; char timestring[TIMEBUFLEN]; char ipstr[INET6_ADDRSTRLEN]; char recvbuf[MAX_RECV_BUFLEN]; char s[INET6_ADDRSTRLEN]; pthread_t my_id; FILE *fp_mpl, *fp_mpu, *fp_adis; struct addrinfo hints, *res, *p, *ai_client; struct sockaddr_storage client_addr; socklen_t client_addr_len; client_addr_len = sizeof(struct sockaddr_storage); memset(&hints, 0, sizeof hints); hints.ai_family = AF_UNSPEC; // AF_INET or AF_INET6 to force version hints.ai_socktype = SOCK_DGRAM; // UDP hints.ai_flags = AI_PASSIVE; // use local host address. fprintf(stderr, "%s: listen port %s\n", __func__, port_info->host_listen_port); if(ports_equal(port_info->client_port, IMU_A_TX_PORT_ADIS)) { sensor_listen_id = ADIS_LISTENER; } else if (ports_equal(port_info->client_port, IMU_A_TX_PORT_MPU)) { sensor_listen_id = MPU_LISTENER; } else if (ports_equal(port_info->client_port, IMU_A_TX_PORT_MPL)) { sensor_listen_id = MPL_LISTENER; } else { sensor_listen_id = UNKNOWN_SENSOR; } if(sensor_listen_id == ADIS_LISTENER) { fp_adis = fopen("adis16405_log.txt", "w"); get_current_time(timestring) ; fprintf(fp_adis, "# adis16405 IMU data started at: %s\n", timestring); fprintf(fp_adis, "# adis16405 IMU raw data\n"); fprintf(fp_adis, "# timestamp,ax,ay,az,gx,gy,gz,mx,my,mz,C\n"); } else if (sensor_listen_id == MPU_LISTENER) { fp_mpu = fopen("mpu9150_log.txt", "w"); get_current_time(timestring) ; fprintf(fp_mpu, "# mpu9150 IMU data started at: %s\n", timestring); fprintf(fp_mpu, "# mpu9150 IMU raw data\n"); fprintf(fp_mpu, "# timestamp,ax,ay,az,gx,gy,gz,C\n"); } else if (sensor_listen_id == MPL_LISTENER) { fp_mpl = fopen("mpl3115a2_log.txt", "w"); get_current_time(timestring) ; fprintf(fp_mpl, "# mpl3115a2 Pressure Sensor data started at: %s\n", timestring); fprintf(fp_mpl, "# mpl3115a2 Pressure sensor raw data\n"); fprintf(fp_mpl, "# timestamp,P,T\n"); } else { ; } memset(&hints, 0, sizeof hints); hints.ai_family = AF_UNSPEC; hints.ai_socktype = SOCK_DGRAM; if ((retval = getaddrinfo(port_info->client_addr, port_info->client_port, &hints, &res)) != 0) { fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(retval)); die_nice("client get address"); } /* Create a socket for the client */ for(ai_client = res; ai_client != NULL; ai_client = ai_client->ai_next) { if ((clientsocket_fd = socket(ai_client->ai_family, ai_client->ai_socktype, ai_client->ai_protocol)) == -1) { perror("clientsocket"); continue; } break; } if (ai_client == NULL) { die_nice("failed to bind client socket\n"); } //for(i=0; i<NPACK; ++i) { uint32_t datacount = 0; while(!user_exit_requested) { char countmsg[MAX_USER_STRBUF]; struct sockaddr *sa; socklen_t len; char hbuf[NI_MAXHOST], sbuf[NI_MAXSERV]; addr_len = sizeof client_addr; if ((numbytes = recvfrom(hostsocket_fd, recvbuf, MAX_RECV_BUFLEN-1 , 0, (struct sockaddr *)&client_addr, &addr_len)) == -1) { die_nice("recvfrom"); } if (getnameinfo((struct sockaddr *)&client_addr, client_addr_len, hbuf, sizeof(hbuf), sbuf, sizeof(sbuf), NI_NUMERICHOST | NI_NUMERICSERV) == 0) { if(ports_equal(sbuf, IMU_A_TX_PORT_MPU) && (sensor_listen_id == MPU_LISTENER)) { if(numbytes != sizeof(MPU_packet) ){ die_nice("wrong numbytes mpu"); } memcpy (&mpu9150_udp_data, recvbuf, sizeof(MPU_packet)); mpu9150_imu_data = (MPU9150_read_data) mpu9150_udp_data.data; if(enable_logging) { fprintf(fp_mpu, "%c%c%c%c,%f,%d,%d,%d,%d,%d,%d,%d\n", mpu9150_udp_data.ID[0], mpu9150_udp_data.ID[1],mpu9150_udp_data.ID[2],mpu9150_udp_data.ID[3], timestamp_now(), mpu9150_imu_data.accel_xyz.x, mpu9150_imu_data.accel_xyz.y, mpu9150_imu_data.accel_xyz.z, mpu9150_imu_data.gyro_xyz.x, mpu9150_imu_data.gyro_xyz.y, mpu9150_imu_data.gyro_xyz.z, mpu9150_imu_data.celsius); ++datacount; snprintf(countmsg, MAX_USER_STRBUF , " %d MPU entries.", datacount ); if(datacount %COUNT_INTERVAL == 0) { log_msg(countmsg); } } fflush(fp_mpu); #if DEBUG_MPU_NET printf("\r\nraw_temp: %3.2f C\r\n", mpu9150_temp_to_dC(mpu9150_udp_data.celsius)); printf("ACCL: x: %d\ty: %d\tz: %d\r\n", mpu9150_udp_data.accel_xyz.x, mpu9150_udp_data.accel_xyz.y, mpu9150_udp_data.accel_xyz.z); printf("GRYO: x: 0x%x\ty: 0x%x\tz: 0x%x\r\n", mpu9150_udp_data.gyro_xyz.x, mpu9150_udp_data.gyro_xyz.y, mpu9150_udp_data.gyro_xyz.z); if ((numbytes = sendto(clientsocket_fd, recvbuf, strlen(recvbuf), 0, ai_client->ai_addr, ai_client->ai_addrlen)) == -1) { die_nice("client sendto"); } #endif } else if (ports_equal(sbuf, IMU_A_TX_PORT_MPL) && (sensor_listen_id == MPL_LISTENER)) { if(numbytes != sizeof(MPL_packet) ){ die_nice("wrong numbytes mpl"); } memcpy (&mpl3115a2_udp_data, recvbuf, sizeof(MPL_packet)); mpl3115a2_pt_data = mpl3115a2_udp_data.data; if(enable_logging) { fprintf(fp_mpl, "%c%c%c%c,%f,%d,%d\n", mpl3115a2_udp_data.ID[0], mpl3115a2_udp_data.ID[1],mpl3115a2_udp_data.ID[2],mpl3115a2_udp_data.ID[3], timestamp_now(), mpl3115a2_pt_data.mpu_pressure, mpl3115a2_pt_data.mpu_temperature ); ++datacount; snprintf(countmsg, MAX_USER_STRBUF , " %d MPL entries.", datacount ); if(datacount % COUNT_INTERVAL == 0) { log_msg(countmsg); } } fflush(fp_mpl); } else if (ports_equal(sbuf, IMU_A_TX_PORT_ADIS) && (sensor_listen_id == ADIS_LISTENER)) { double adis_temp_C = 0.0; bool adis_temp_neg = false; if(numbytes != sizeof(ADIS_packet) ){ die_nice("wrong numbytes adis"); } memcpy (&adis16405_udp_data, recvbuf, sizeof(ADIS_packet)); adis16405_imu_data = adis16405_udp_data.data; if(enable_logging) { adis_temp_neg = adis16405_temp_to_dC(&adis_temp_C, &adis16405_imu_data.adis_temp_out); fprintf(fp_adis, "%c%c%c%c,%f,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n", // fprintf(fp_adis, "%f,%d,%d,%d,%d,%d,%d,%0x%x\n", adis16405_udp_data.ID[0], adis16405_udp_data.ID[1],adis16405_udp_data.ID[2],adis16405_udp_data.ID[3], timestamp_now(), adis16405_imu_data.adis_xaccl_out, adis16405_imu_data.adis_yaccl_out, adis16405_imu_data.adis_zaccl_out, adis16405_imu_data.adis_xgyro_out, adis16405_imu_data.adis_ygyro_out, adis16405_imu_data.adis_zgyro_out, adis16405_imu_data.adis_xmagn_out, adis16405_imu_data.adis_ymagn_out, adis16405_imu_data.adis_zmagn_out, adis16405_imu_data.adis_temp_out ); ++datacount; snprintf(countmsg, MAX_USER_STRBUF , " %d ADIS entries.", datacount ); if(datacount %COUNT_INTERVAL == 0) { log_msg(countmsg); } } fflush(fp_adis); } else { // printf("Unrecognized Packet %s:%s\tListen: %s\tThreadID: %d\n", hbuf, sbuf, listentostr(sensor_listen_id), port_info->thread_id); // if(ports_equal(sbuf, IMU_A_TX_PORT_ADIS)) printf("ADIS port\n\n"); // if(ports_equal(sbuf, IMU_A_TX_PORT_MPU)) printf("MPU port\n\n"); } } } get_current_time(timestring) ; if(sensor_listen_id == MPU_LISTENER ) { fprintf(fp_mpu, "# mpu9150 IMU data closed at: %s\n", timestring); fclose(fp_mpu); } else if (sensor_listen_id == MPL_LISTENER) { fprintf(fp_mpl, "# mpu3115a2 P T data closed at: %s\n", timestring); fclose(fp_mpl); } else if (sensor_listen_id == ADIS_LISTENER) { fprintf(fp_adis, "# adis16405 IMU data closed at: %s\n", timestring); fclose(fp_adis); } close(clientsocket_fd); freeaddrinfo(res); // free the linked list fprintf(stderr, "Leaving thread %d\n", port_info->thread_id); return 0; }
int main(int argc, char *argv[]) { #ifndef WIN32 setlinebuf (stdout); #endif char logpath[PATH_MAX]; memset (logpath, 0, sizeof (logpath)); logger_t logger; memset (&logger, 0, sizeof (logger)); // set some defaults logger.force_overwrite = 0; logger.auto_increment = 0; logger.use_strftime = 0; char *chan_regex = strdup(".*"); double max_write_queue_size_mb = DEFAULT_MAX_WRITE_QUEUE_SIZE_MB; logger.invert_channels = 0; logger.fflush_interval_ms = 100; char *lcmurl = NULL; char *optstring = "a:fic:shm:vu:"; int c; struct option long_opts[] = { { "auto-split-hours", required_argument, 0, 'a' }, { "auto-split-mb", required_argument, 0, 'b' }, { "channel", required_argument, 0, 'c' }, { "force", no_argument, 0, 'f' }, { "increment", required_argument, 0, 'i' }, { "lcm-url", required_argument, 0, 'l' }, { "max-unwritten-mb", required_argument, 0, 'm' }, { "strftime", required_argument, 0, 's' }, { "invert-channels", no_argument, 0, 'v' }, { "flush-interval", required_argument, 0,'u'}, { 0, 0, 0, 0 } }; while ((c = getopt_long (argc, argv, optstring, long_opts, 0)) >= 0) { switch (c) { case 'a': logger.auto_split_hours = strtod(optarg, NULL); logger.auto_increment = 1; if(logger.auto_split_hours <= 0) { usage(); return 1; } break; case 'b': logger.auto_split_mb = strtod(optarg, NULL); logger.auto_increment = 1; if(logger.auto_split_mb <= 0) { usage(); return 1; } break; case 'f': logger.force_overwrite = 1; break; case 'c': free(chan_regex); chan_regex = strdup(optarg); break; case 'i': logger.auto_increment = 1; break; case 's': logger.use_strftime = 1; break; case 'l': free(lcmurl); lcmurl = strdup(optarg); break; case 'v': logger.invert_channels = 1; break; case 'm': max_write_queue_size_mb = strtod(optarg, NULL); if(max_write_queue_size_mb <= 0) { usage(); return 1; } break; case 'u': logger.fflush_interval_ms = atol(optarg); if(logger.fflush_interval_ms <= 0) { usage(); return 1; } break; case 'h': default: usage(); return 1; }; } if (optind == argc) { strcpy (logger.input_fname, "lcmlog-%Y-%m-%d"); logger.auto_increment = 1; logger.use_strftime = 1; } else if (optind == argc - 1) { strncpy (logger.input_fname, argv[optind], sizeof (logger.input_fname)); } else if (optind < argc-1) { usage (); return 1; } // initialize GLib threading g_thread_init(NULL); logger.time0 = timestamp_now(); logger.max_write_queue_size = (int64_t)(max_write_queue_size_mb * (1 << 20)); if(0 != open_logfile(&logger)) return 1; // create write thread logger.write_thread_exit_flag = 0; logger.mutex = g_mutex_new(); logger.write_queue_size = 0; logger.write_queue = g_async_queue_new(); logger.write_thread = g_thread_create(write_thread, &logger, TRUE, NULL); // begin logging logger.lcm = lcm_create (lcmurl); free(lcmurl); if (!logger.lcm) { fprintf (stderr, "Couldn't initialize LCM!"); return 1; } if(logger.invert_channels) { // if inverting the channels, subscribe to everything and invert on the // callback lcm_subscribe(logger.lcm, ".*", message_handler, &logger); char *regexbuf = g_strdup_printf("^%s$", chan_regex); #ifdef USE_GREGEX GError *rerr = NULL; logger.regex = g_regex_new(regexbuf, (GRegexCompileFlags) 0, (GRegexMatchFlags) 0, &rerr); if(rerr) { fprintf(stderr, "%s\n", rerr->message); g_free(regexbuf); return 1; } #else if (0 != regcomp (&logger.preg, regexbuf, REG_NOSUB | REG_EXTENDED)) { fprintf(stderr, "bad regex!\n"); g_free(regexbuf); return 1; } #endif g_free(regexbuf); } else { // otherwise, let LCM handle the regex lcm_subscribe(logger.lcm, chan_regex, message_handler, &logger); } free(chan_regex); _mainloop = g_main_loop_new (NULL, FALSE); signal_pipe_glib_quit_on_kill (); glib_mainloop_attach_lcm (logger.lcm); #ifdef USE_SIGHUP signal(SIGHUP, sighup_handler); #endif // main loop g_main_loop_run (_mainloop); fprintf(stderr, "Logger exiting\n"); // stop the write thread g_mutex_lock(logger.mutex); logger.write_thread_exit_flag = 1; g_mutex_unlock(logger.mutex); g_async_queue_push(logger.write_queue, &logger.write_thread_exit_flag); g_thread_join(logger.write_thread); g_mutex_free(logger.mutex); // cleanup. This isn't strictly necessary, do it to be pedantic and so that // leak checkers don't complain glib_mainloop_detach_lcm (logger.lcm); lcm_destroy (logger.lcm); lcm_eventlog_destroy (logger.log); for(void *msg = g_async_queue_try_pop(logger.write_queue); msg; msg=g_async_queue_try_pop(logger.write_queue)) { if(msg == &logger.write_thread_exit_flag) continue; free(msg); } g_async_queue_unref(logger.write_queue); if(logger.invert_channels) { #ifdef USE_GREGEX g_regex_unref(logger.regex); #else regfree(&logger.preg); #endif } return 0; }
static gboolean on_gl_expose (GtkWidget *widget, GdkEventExpose *event, void *user_data) { Viewer * self = (Viewer *) user_data; // if not enough time has elapsed since our last redraw, we // schedule a redraw in the future (if one isn't already pending). int64_t now = timestamp_now(); double dt = (now - self->last_draw_utime) / 1000000.0; if (dt < (1.0/MAX_REDRAW_HZ)) { if (!self->redraw_timer_pending) { int delay_ms = (now - self->last_draw_utime)/1000 + 1; g_timeout_add(delay_ms, on_redraw_timer, self); self->redraw_timer_pending = 1; } return TRUE; } self->last_draw_utime = now; // If we're making a movie, don't draw any faster than the // requested movie FPS rate. if (self->movie_gzf && !self->movie_draw_pending) return TRUE; // set this to 1 in order to cause viewer to exit cleanly after a // few hundred frames: useful for generating gprof output. g_draws++; if (0) { int thresh = 300; // for profiling PROFILE if (g_draws%50 == 0) printf("draws: %5i / %i\n", g_draws, thresh); if (g_draws == thresh) { printf("Profiling is enabled: exiting now\n"); exit(0); } } // we're going to actually draw. gtku_gl_drawing_area_set_context (self->gl_area); render_scene (self); gtku_gl_drawing_area_swap_buffers (self->gl_area); // write a movie frame? if (self->movie_draw_pending) { assert(self->movie_gzf); glReadPixels (0, 0, self->movie_width, self->movie_height, GL_RGB, GL_UNSIGNED_BYTE, self->movie_buffer); gzprintf(self->movie_gzf, "P6 %d %d %d\n", self->movie_width, self->movie_height, 255); for (int h = self->movie_height - 1; h >= 0; h--) { int offset = self->movie_stride * h; gzwrite(self->movie_gzf, &self->movie_buffer[offset], self->movie_stride); } self->movie_draw_pending = 0; int64_t now = timestamp_now(); double dt; if (self->movie_frame_last_utime == 0) dt = 1.0 / self->movie_desired_fps; else dt = (now - self->movie_frame_last_utime)/1000000.0; double fps = 1.0 / dt; self->movie_frame_last_utime = now; double alpha = 0.8; // higher = lower-pass self->movie_actual_fps = alpha * self->movie_actual_fps + (1 - alpha) * fps; self->movie_frames++; printf("%20s %6d (%5.2f fps)\r", self->movie_path, self->movie_frames, self->movie_actual_fps); fflush(NULL); } return TRUE; }
static int lcm_logprov_handle (lcm_logprov_t * lr) { lcm_recv_buf_t rbuf; if (!lr->event) return -1; char ch; int status = lcm_internal_pipe_read(lr->notify_pipe[0], &ch, 1); if (status == 0) { fprintf (stderr, "Error: lcm_handle read 0 bytes from notify_pipe\n"); return -1; } else if (status < 0) { fprintf (stderr, "Error: lcm_handle read: %s\n", strerror (errno)); return -1; } int64_t now = timestamp_now (); /* Initialize the wall clock if this is the first time through */ if (lr->next_clock_time < 0) lr->next_clock_time = now; // rbuf.channel = lr->event->channel, rbuf.data = (uint8_t*) lr->event->data; rbuf.data_size = lr->event->datalen; rbuf.recv_utime = lr->next_clock_time; rbuf.lcm = lr->lcm; if(lcm_try_enqueue_message(lr->lcm, lr->event->channel)) lcm_dispatch_handlers (lr->lcm, &rbuf, lr->event->channel); int64_t prev_log_time = lr->event->timestamp; if (load_next_event (lr) < 0) { /* end-of-file reached. This call succeeds, but next call to * _handle will fail */ lr->event = NULL; if(lcm_internal_pipe_write(lr->notify_pipe[1], "+", 1) < 0) { perror(__FILE__ " - write(notify)"); } return 0; } /* Compute the wall time for the next event */ if (lr->speed > 0) lr->next_clock_time += (lr->event->timestamp - prev_log_time) / lr->speed; else lr->next_clock_time = now; if (lr->next_clock_time > now) { int wstatus = lcm_internal_pipe_write(lr->timer_pipe[1], &lr->next_clock_time, 8); if(wstatus < 0) { perror(__FILE__ " - write(timer_pipe)"); } } else { int wstatus = lcm_internal_pipe_write(lr->notify_pipe[1], "+", 1); if(wstatus < 0) { perror(__FILE__ " - write(notify_pipe)"); } } return 0; }
static void message_handler (const lcm_recv_buf_t *rbuf, const char *channel, void *u) { logger_t *logger = (logger_t*) u; if(logger->invert_channels) { #ifdef USE_GREGEX if(g_regex_match(logger->regex, channel, (GRegexMatchFlags) 0, NULL)) return; #else if(0 == regexec(&logger->preg, channel, 0, NULL, 0)) return; #endif } int channellen = strlen(channel); // check if the backlog of unwritten messages is too big. If so, then // ignore this event int64_t mem_sz = sizeof(lcm_eventlog_event_t) + channellen + 1 + rbuf->data_size; g_mutex_lock(logger->mutex); int64_t mem_required = mem_sz + logger->write_queue_size; if(mem_required > logger->max_write_queue_size) { // can't write to logfile fast enough. drop packet. g_mutex_unlock(logger->mutex); // maybe print an informational message to stdout int64_t now = timestamp_now(); logger->dropped_packets_count ++; int rc = logger->dropped_packets_count - logger->last_drop_report_count; if(now - logger->last_drop_report_utime > 1000000 && rc > 0) { printf("Can't write to log fast enough. Dropped %d packet%s\n", rc, rc==1?"":"s"); logger->last_drop_report_utime = now; logger->last_drop_report_count = logger->dropped_packets_count; } return; } else { logger->write_queue_size = mem_required; g_mutex_unlock(logger->mutex); } // queue up the message for writing to disk by the write thread lcm_eventlog_event_t *le = (lcm_eventlog_event_t*) malloc(mem_sz); memset(le, 0, mem_sz); le->timestamp = rbuf->recv_utime; le->channellen = channellen; le->datalen = rbuf->data_size; // log_write_event will handle le.eventnum. le->channel = ((char*)le) + sizeof(lcm_eventlog_event_t); strcpy(le->channel, channel); le->data = le->channel + channellen + 1; assert((char*)le->data + rbuf->data_size == (char*)le + mem_sz); memcpy(le->data, rbuf->data, rbuf->data_size); g_async_queue_push(logger->write_queue, le); }
//等待其它节点连接请求 void overlay::start() { //先停止 stop(); //启动URDP监听器,监听连接请求 OVERLAY_DBG( std::cout << topology_id_ << "***********overlay start********************:" << (timestamp_t)timestamp_now() << std::endl; std::cout << "udp_local_endpoint_:" << udp_local_endpoint_ << std::endl; std::cout << "tcp_local_endpoint_:" << tcp_local_endpoint_ << std::endl; );