Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

plugin: add synchronisation to most plugins #186

Closed
wants to merge 2 commits into from
Closed

plugin: add synchronisation to most plugins #186

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
bd1cf42
plugin: add synchronisation to most plugins
mhkabir Jan 10, 2015
9f29e6f
fix build
mhkabir Jan 11, 2015
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
17 changes: 13 additions & 4 deletions mavros/include/mavros/mavros_uas.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -324,14 +324,23 @@ class UAS {
bool cmode_from_str(std::string cmode_str, uint32_t &custom_mode);

/**
* @brief Compute FCU message time from time_boot_ms field
* @brief Compute FCU message time from time_boot_ms or time_usec field
*
* Uses time_offset for calculation
*
* @todo
* @return FCU time if if is known else current wall time.
* @return FCU time if it is known else current wall time.
*/
//ros::Time fcu_time(uint32_t time_boot_ms);
inline ros::Time synchronise_stamp(uint32_t time_boot_ms) {
if(time_offset > 0)
return ros::Time(time_boot_ms + time_offset/1000000);
else return ros::Time::now();
}

inline ros::Time synchronise_stamp(uint64_t time_usec) {
if(time_offset > 0)
return ros::Time(time_usec + time_offset/1000);
else return ros::Time::now();
}

private:
std::recursive_mutex mutex;
Expand Down
2 changes: 1 addition & 1 deletion mavros/src/plugins/global_position.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -122,7 +122,7 @@ class GlobalPositionPlugin : public MavRosPlugin {

std_msgs::Header header;
header.frame_id = frame_id;
header.stamp = ros::Time::now();
header.stamp = uas->synchronise_stamp(gp_pos.time_boot_ms);

sensor_msgs::NavSatFixPtr gps_cord =
boost::make_shared<sensor_msgs::NavSatFix>();
Expand Down
2 changes: 1 addition & 1 deletion mavros/src/plugins/gps.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -183,7 +183,7 @@ class GPSPlugin : public MavRosPlugin {
}

fix->header.frame_id = frame_id;
fix->header.stamp = ros::Time::now();
fix->header.stamp = ros::Time::now(); // TODO: Check type of stamp in message

// store GPS data in UAS
double eph = (raw_gps.eph != UINT16_MAX)? raw_gps.eph / 1E2 : NAN;
Expand Down
13 changes: 6 additions & 7 deletions mavros/src/plugins/imu_pub.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -207,7 +207,7 @@ class IMUPubPlugin : public MavRosPlugin {

// publish data
imu_msg->header.frame_id = frame_id;
imu_msg->header.stamp = ros::Time::now();
imu_msg->header.stamp = uas->synchronise_stamp(att.time_boot_ms);
imu_pub.publish(imu_msg);
}

Expand Down Expand Up @@ -235,7 +235,7 @@ class IMUPubPlugin : public MavRosPlugin {

// publish data
imu_msg->header.frame_id = frame_id;
imu_msg->header.stamp = ros::Time::now();
imu_msg->header.stamp = uas->synchronise_stamp(att_q.time_boot_ms);
imu_pub.publish(imu_msg);
}

Expand All @@ -247,7 +247,7 @@ class IMUPubPlugin : public MavRosPlugin {
has_hr_imu = true;

std_msgs::Header header;
header.stamp = ros::Time::now();
header.stamp = uas->synchronise_stamp(imu_hr.time_usec);
header.frame_id = frame_id;

/* imu/data_raw filled by HR IMU */
Expand Down Expand Up @@ -302,7 +302,7 @@ class IMUPubPlugin : public MavRosPlugin {
mavlink_msg_raw_imu_decode(msg, &imu_raw);

std_msgs::Header header;
header.stamp = ros::Time::now();
header.stamp = uas->synchronise_stamp(imu_raw.time_usec);
header.frame_id = frame_id;

/* NOTE: APM send SCALED_IMU data as RAW_IMU */
Expand Down Expand Up @@ -350,8 +350,7 @@ class IMUPubPlugin : public MavRosPlugin {
mavlink_msg_scaled_imu_decode(msg, &imu_raw);

std_msgs::Header header;
header.stamp = ros::Time::now();
header.frame_id = frame_id;
header.stamp = uas->synchronise_stamp(imu_raw.time_boot_ms);

fill_imu_msg_raw(imu_msg,
imu_raw.xgyro * MILLIRS_TO_RADSEC,
Expand Down Expand Up @@ -386,7 +385,7 @@ class IMUPubPlugin : public MavRosPlugin {
mavlink_msg_scaled_pressure_decode(msg, &press);

std_msgs::Header header;
header.stamp = ros::Time::now();
header.stamp = uas->synchronise_stamp(press.time_boot_ms);
header.frame_id = frame_id;

sensor_msgs::TemperaturePtr temp_msg = boost::make_shared<sensor_msgs::Temperature>();
Expand Down
2 changes: 1 addition & 1 deletion mavros/src/plugins/local_position.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -106,7 +106,7 @@ class LocalPositionPlugin : public MavRosPlugin {

tf::poseTFToMsg(transform, pose->pose);
pose->header.frame_id = frame_id;
pose->header.stamp = ros::Time::now();
pose->header.stamp = uas->synchronise_stamp(pos_ned.time_boot_ms);

if (send_tf)
tf_broadcaster.sendTransform(
Expand Down