diff --git a/include/rviz_2d_plot_plugin/plot_2d_config.hpp b/include/rviz_2d_plot_plugin/plot_2d_config.hpp index 2a84775..cf3f3b3 100644 --- a/include/rviz_2d_plot_plugin/plot_2d_config.hpp +++ b/include/rviz_2d_plot_plugin/plot_2d_config.hpp @@ -46,6 +46,7 @@ enum class XAxisMode enum class PlotMode { TimeSeries, + Delay, XY, }; diff --git a/src/plot_2d_controller.cpp b/src/plot_2d_controller.cpp index eb724f2..9c21e26 100644 --- a/src/plot_2d_controller.cpp +++ b/src/plot_2d_controller.cpp @@ -454,16 +454,32 @@ bool Plot2DController::appendSerializedMessage( x_value = x_result.value.value(); } - const double transformed_value = - result.value.value() * config_.series[i].value_scale + config_.series[i].value_offset; - series.status = PlotControllerStatus::Ok; - series.message.clear(); - series.samples.append(sample_time, x_value, transformed_value); - if (!xy_mode || config_.time.xy_history_mode == XYHistoryMode::RollingTimeWindow) { - series.samples.pruneToWindow(sample_time, config_.time.window_seconds); + const bool delay_mode = config_.plot_mode == PlotMode::Delay; + if (delay_mode) + { + const double transformed_value = receive_time - x_value; + series.status = PlotControllerStatus::Ok; + series.message.clear(); + series.samples.append(sample_time, x_value, transformed_value); + if (!xy_mode || config_.time.xy_history_mode == XYHistoryMode::RollingTimeWindow) { + series.samples.pruneToWindow(sample_time, config_.time.window_seconds); + } + series.latest_value = transformed_value; + appended = true; + } + else + { + const double transformed_value = + result.value.value() * config_.series[i].value_scale + config_.series[i].value_offset; + series.status = PlotControllerStatus::Ok; + series.message.clear(); + series.samples.append(sample_time, x_value, transformed_value); + if (!xy_mode || config_.time.xy_history_mode == XYHistoryMode::RollingTimeWindow) { + series.samples.pruneToWindow(sample_time, config_.time.window_seconds); + } + series.latest_value = transformed_value; + appended = true; } - series.latest_value = transformed_value; - appended = true; } updateAggregateStatus(); diff --git a/src/plot_2d_display_options.cpp b/src/plot_2d_display_options.cpp index 94ab18b..697e501 100644 --- a/src/plot_2d_display_options.cpp +++ b/src/plot_2d_display_options.cpp @@ -212,6 +212,8 @@ std::string plotModeName(const PlotMode mode) switch (mode) { case PlotMode::TimeSeries: return "Time Series"; + case PlotMode::Delay: + return "Delay"; case PlotMode::XY: return "XY"; } @@ -223,6 +225,9 @@ PlotMode plotModeFromName(const std::string & name) if (name == "XY") { return PlotMode::XY; } + if (name == "Delay") { + return PlotMode::Delay; + } return PlotMode::TimeSeries; } @@ -232,6 +237,7 @@ void addPlotModeOptions(rviz_common::properties::EnumProperty * property) return; } property->addOptionStd(plotModeName(PlotMode::TimeSeries)); + property->addOptionStd(plotModeName(PlotMode::Delay)); property->addOptionStd(plotModeName(PlotMode::XY)); }