diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 403a412..6c5ef82 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -656,6 +656,7 @@ SURVIVE_EXPORT void survive_default_gen_detected_process(SurviveObject *so, int SURVIVE_EXPORT void survive_default_new_object_process(SurviveObject *so); SURVIVE_EXPORT double survive_run_time(const SurviveContext *ctx); SURVIVE_EXPORT double survive_run_time_since_epoch(const SurviveContext *ctx); +SURVIVE_EXPORT double survive_time_to_time_since_epoch(const SurviveContext *ctx, double t); SURVIVE_EXPORT const SurvivePose* survive_external_to_world(const SurviveContext *ctx); SURVIVE_EXPORT size_t survive_input_event_count(const SurviveContext *ctx); diff --git a/src/survive.c b/src/survive.c index 76f661b..76e2afa 100644 --- a/src/survive.c +++ b/src/survive.c @@ -1268,6 +1268,7 @@ double survive_run_time(const SurviveContext *ctx) { return pctx->lastRunTime = timestamp_in_s(); } +double survive_time_to_time_since_epoch(const SurviveContext *ctx, double t) { return t + OGStartTimeS(); } double survive_run_time_since_epoch(const SurviveContext *ctx) { return survive_run_time(ctx) + OGStartTimeS(); } double static_time(const SurviveContext *ctx, void *user) { diff --git a/src/survive_sensor_activations.c b/src/survive_sensor_activations.c index 524b0d8..0416485 100644 --- a/src/survive_sensor_activations.c +++ b/src/survive_sensor_activations.c @@ -94,7 +94,7 @@ survive_us SurviveSensorActivations_runtime(SurviveSensorActivations *self, surv void SurviveSensorActivations_add_imu(SurviveSensorActivations *self, struct PoserDataIMU *imuData) { self->last_imu = imuData->hdr.timecode; - // fprintf(stderr, "imu tc: %f\n", self->last_imu/ 48000000.); + //fprintf(stderr, "imu tc: %f\n", self->last_imu/ 48000000.); if (self->imu_init_cnt > 0) { self->imu_init_cnt--; return; diff --git a/tools/ros_publisher/main.cc b/tools/ros_publisher/main.cc index fc36d83..821522b 100644 --- a/tools/ros_publisher/main.cc +++ b/tools/ros_publisher/main.cc @@ -13,7 +13,6 @@ #include #include -double ros_offset = 0; SurviveSimpleContext *actx = 0; std::unique_ptr n; @@ -26,7 +25,7 @@ static ros::Publisher& rootJoyPublisher() { } static ros::Time rostime_from_survivetime(FLT timecode) { - return ros::Time().fromSec(timecode + ros_offset); + return ros::Time().fromSec(timecode); } static std::string sanitize(const std::string& serial) { @@ -82,7 +81,10 @@ struct ObjectPublishers { sensor_msgs::Imu imu; imu.header.frame_id = std::string(serial_number()) + "_imu"; imu.header.seq = imu_seq++; - imu.header.stamp = rostime_from_survivetime(1e-6 * SurviveSensorActivations_runtime(&so->activations, so->activations.last_imu)); + //printf("!!!! %u\n", timecode); + auto long_timecode = SurviveSensorActivations_long_timecode_imu(&so->activations, timecode); + imu.header.stamp = rostime_from_survivetime(SurviveSensorActivations_runtime(&so->activations, long_timecode)*1e-6); + //printf("!!!! %ld %f %f\n", long_timecode, SurviveSensorActivations_runtime(&so->activations, long_timecode)*1e-6, so->activations.runtime_offset); imu.angular_velocity.x = accelgyromag[3]; imu.angular_velocity.y = accelgyromag[4]; @@ -211,8 +213,6 @@ int main(int argc, char **argv) { survive_install_imu_fn(ctx, imu_func); auto now = survive_simple_run_time_since_epoch(actx); - auto ros_now = ros::Time::now().toSec(); - ros_offset = ros_now - now; survive_simple_start_thread(actx); @@ -248,7 +248,7 @@ int main(int argc, char **argv) { break; } - ros_now = ros::Time::now().toSec(); + auto ros_now = ros::Time::now().toSec(); if (ros_now > last_chirp + .25) { last_chirp = now; for (const SurviveSimpleObject *it = survive_simple_get_first_object(actx); it != 0;