mmo-ref/og-libsurvive
1
0
Fork 0
mirror of https://github.com/collabora/libsurvive.git synced 2026-08-04 00:51:21 -04:00
Code Issues Projects Releases Packages Wiki Activity Actions

Fix imu timestamp in ros node

Browse source
This commit is contained in:
Justin Berger 2023-03-11 01:12:55 -07:00
parent 3abf12e054
commit d89876a1af
4 changed files with 9 additions and 7 deletions
Download patch file Download diff file Expand all files Collapse all files

1
include/libsurvive/survive.h
View file

@ -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);

1
src/survive.c
View file

@ -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) {

2
src/survive_sensor_activations.c
View file

@ -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;

12
tools/ros_publisher/main.cc
View file

@ -13,7 +13,6 @@
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
double ros_offset = 0;
SurviveSimpleContext *actx = 0;
std::unique_ptr<ros::NodeHandle> 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;