Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
14 changes: 12 additions & 2 deletions rviz_common/include/rviz_common/display.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -151,11 +151,21 @@ class RVIZ_COMMON_PUBLIC Display : public rviz_common::properties::BoolProperty

/// Called periodically by the visualization manager.
/**
* \param wall_dt Wall-clock time, in seconds, since the last time the update list was run through.
* \param ros_dt ROS time, in seconds, since the last time the update list was run through.
* \param wall_dt Wall-clock time since the last time the update list was run through.
* \param ros_dt ROS time since the last time the update list was run through.
*/
virtual
void
update(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds ros_dt);

/// Called periodically by the visualization manager.
/**
* \param wall_dt Wall-clock time, in nanoseconds, since the last time the update list was run through.
* \param ros_dt ROS time, in nanoseconds, since the last time the update list was run through.
*/
[[deprecated("Use update(std::chrono::nanoseconds, std::chrono::nanoseconds)")]]
virtual
void
update(float wall_dt, float ros_dt);

/// Called to tell the display to clear its state.
Expand Down
4 changes: 4 additions & 0 deletions rviz_common/include/rviz_common/display_group.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -146,6 +146,10 @@ class RVIZ_COMMON_PUBLIC DisplayGroup : public Display
virtual DisplayGroup * getGroupAt(int index) const;

/// Call update() on all child Displays.
void update(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds ros_dt) override;

/// Call update() on all child Displays.
[[deprecated("Use update(std::chrono::nanoseconds, std::chrono::nanoseconds)")]]
void update(float wall_dt, float ros_dt) override;

/// Reset this and all child Displays.
Expand Down
12 changes: 12 additions & 0 deletions rviz_common/include/rviz_common/tool.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -102,6 +102,18 @@ class RVIZ_COMMON_PUBLIC Tool : public QObject
virtual void deactivate() = 0;

/// Called periodically, typically at 30Hz.
/**
* \param wall_dt Wall-clock time since the last time the update list was run through.
* \param ros_dt ROS time since the last time the update list was run through.
*/
virtual void update(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds ros_dt);

/// Called periodically, typically at 30Hz.
/**
* \param wall_dt Wall-clock time, in nanoseconds, since the last time the update list was run through.
* \param ros_dt ROS time, in nanoseconds, since the last time the update list was run through.
*/
[[deprecated("Use update(std::chrono::nanoseconds, std::chrono::nanoseconds)")]]
virtual void update(float wall_dt, float ros_dt);

enum
Expand Down
7 changes: 7 additions & 0 deletions rviz_common/include/rviz_common/view_controller.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -127,6 +127,13 @@ class RVIZ_COMMON_PUBLIC ViewController : public properties::Property
/**
* Override with code that needs to run repeatedly.
*/
virtual void update(std::chrono::nanoseconds dt, std::chrono::nanoseconds ros_dt);

/// Called at 30Hz by ViewManager::update() while this view is active.
/**
* Override with code that needs to run repeatedly.
*/
[[deprecated("Use update(std::chrono::nanoseconds, std::chrono::nanoseconds)")]]
virtual void update(float dt, float ros_dt);

/// Called when mouse events are fired.
Expand Down
3 changes: 3 additions & 0 deletions rviz_common/include/rviz_common/view_manager.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -71,6 +71,9 @@ class RVIZ_COMMON_PUBLIC ViewManager : public QObject

void initialize();

void update(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds ros_dt);

[[deprecated("Use update(std::chrono::nanoseconds, std::chrono::nanoseconds)")]]
void update(float wall_dt, float ros_dt);

/// Return the current ViewController in use for the main RenderWindow.
Expand Down
5 changes: 2 additions & 3 deletions rviz_common/include/rviz_common/visualization_manager.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,6 @@
#ifndef RVIZ_COMMON__VISUALIZATION_MANAGER_HPP_
#define RVIZ_COMMON__VISUALIZATION_MANAGER_HPP_

#include <chrono>
#include <deque>
#include <memory>

Expand Down Expand Up @@ -372,8 +371,8 @@ protected Q_SLOTS:

rviz_common::properties::ColorProperty * background_color_property_;

float time_update_timer_;
float frame_update_timer_;
std::chrono::nanoseconds time_update_timer_;
std::chrono::nanoseconds frame_update_timer_;

std::shared_ptr<rviz_common::interaction::HandlerManagerIface> handler_manager_;
std::shared_ptr<rviz_common::interaction::SelectionManagerIface> selection_manager_;
Expand Down
5 changes: 5 additions & 0 deletions rviz_common/src/rviz_common/display.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -183,6 +183,11 @@ void Display::setTopic(const QString & topic, const QString & datatype)
(void) datatype;
}

void Display::update(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds ros_dt)
{
update(wall_dt.count(), ros_dt.count());
}

void Display::update(float wall_dt, float ros_dt)
{
(void) wall_dt;
Expand Down
8 changes: 7 additions & 1 deletion rviz_common/src/rviz_common/display_group.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -224,7 +224,7 @@ void DisplayGroup::fixedFrameChanged()
}
}

void DisplayGroup::update(float wall_dt, float ros_dt)
void DisplayGroup::update(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds ros_dt)
{
int num_children = displays_.size();
for (int i = 0; i < num_children; i++) {
Expand All @@ -235,6 +235,12 @@ void DisplayGroup::update(float wall_dt, float ros_dt)
}
}

void DisplayGroup::update(float wall_dt, float ros_dt)
{
this->update(std::chrono::nanoseconds(std::lround(wall_dt)),
std::chrono::nanoseconds(std::lround(ros_dt)));
Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I am quite rusty on the rviz API but wouldn't it be better to do something like std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<float>(seconds)) here to preserve times like 0.1f?

Copy link
Copy Markdown
Contributor Author

@riv-mjohnson riv-mjohnson Sep 9, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Unfortunately, float wall_dt = 0.1f is 0.1 nanoseconds, not 0.1 seconds. See #1322. As such, rounding and losing fractions of a nanosecond shouldn't be a problem.

I made this PR to try and remove the ambiguity.

I don't know what we want to do about preserving existing behaviour vs changing behaviour to match the ros1 / documented behaviour.

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ah, I did not spend enough time reading your issue. I thought the sec -> ns change just happened in rolling.

}

void DisplayGroup::reset()
{
Display::reset();
Expand Down
5 changes: 5 additions & 0 deletions rviz_common/src/rviz_common/tool.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -79,6 +79,11 @@ bool Tool::accessAllKeys() const
return access_all_keys_;
}

void Tool::update(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds ros_dt)
{
update(wall_dt.count(), ros_dt.count());
}

void Tool::update(float wall_dt, float ros_dt)
{
(void) wall_dt;
Expand Down
5 changes: 5 additions & 0 deletions rviz_common/src/rviz_common/view_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -192,6 +192,11 @@ void ViewController::activate()
onActivate();
}

void ViewController::update(std::chrono::nanoseconds dt, std::chrono::nanoseconds ros_dt)
{
update(dt.count(), ros_dt.count());
}

void ViewController::update(float dt, float ros_dt)
{
(void) dt;
Expand Down
8 changes: 7 additions & 1 deletion rviz_common/src/rviz_common/view_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -87,13 +87,19 @@ void ViewManager::initialize()
setCurrent(create("rviz_default_plugins/Orbit"), false);
}

void ViewManager::update(float wall_dt, float ros_dt)
void ViewManager::update(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds ros_dt)
{
if (getCurrent()) {
getCurrent()->update(wall_dt, ros_dt);
}
}

void ViewManager::update(float wall_dt, float ros_dt)
{
this->update(std::chrono::nanoseconds(std::lround(wall_dt)),
std::chrono::nanoseconds(std::lround(ros_dt)));
}

ViewController * ViewManager::create(const QString & class_id)
{
QString error;
Expand Down
26 changes: 14 additions & 12 deletions rviz_common/src/rviz_common/visualization_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -140,8 +140,8 @@ VisualizationManager::VisualizationManager(
render_panel_(render_panel),
wall_clock_elapsed_(0),
ros_time_elapsed_(0),
time_update_timer_(0.0f),
frame_update_timer_(0.0f),
time_update_timer_(0),
frame_update_timer_(0),
render_requested_(1),
frame_count_(0),
window_manager_(wm),
Expand Down Expand Up @@ -336,15 +336,17 @@ BitAllocator * VisualizationManager::visibilityBits()

void VisualizationManager::onUpdate()
{
const auto wall_now = std::chrono::system_clock::now();
const auto wall_diff = wall_now - last_update_wall_time_;
const uint64_t wall_dt = std::chrono::duration_cast<std::chrono::nanoseconds>(wall_diff).count();
const std::chrono::time_point<std::chrono::system_clock> wall_now =
std::chrono::system_clock::now();
const std::chrono::nanoseconds wall_dt =
std::chrono::duration_cast<std::chrono::nanoseconds>(wall_now - last_update_wall_time_);
const auto ros_now = clock_->now();
const uint64_t ros_dt = ros_now.nanoseconds() - last_update_ros_time_.nanoseconds();
const auto ros_dt = std::chrono::nanoseconds(
std::lround(ros_now.nanoseconds() - last_update_ros_time_.nanoseconds()));
last_update_ros_time_ = ros_now;
last_update_wall_time_ = wall_now;

if (ros_dt < 0.0) {
if (ros_dt < std::chrono::nanoseconds(0)) {
resetTime();
}

Expand All @@ -362,15 +364,15 @@ void VisualizationManager::onUpdate()

time_update_timer_ += wall_dt;

if (time_update_timer_ > 0.1f) {
time_update_timer_ = 0.0f;
if (time_update_timer_ > std::chrono::nanoseconds(0)) {
time_update_timer_ = std::chrono::nanoseconds(0);
updateTime();
}

frame_update_timer_ += wall_dt;

if (frame_update_timer_ > 1.0f) {
frame_update_timer_ = 0.0f;
if (frame_update_timer_ > std::chrono::nanoseconds(1)) {
frame_update_timer_ = std::chrono::nanoseconds(0);
updateFrames();
}

Expand All @@ -390,7 +392,7 @@ void VisualizationManager::onUpdate()
}

frame_count_++;
if (render_requested_ || wall_diff > std::chrono::milliseconds(10)) {
if (render_requested_ || wall_dt > std::chrono::milliseconds(10)) {
render_requested_ = 0;
std::lock_guard<std::mutex> lock(private_->render_mutex_);
ogre_root_->renderOneFrame();
Expand Down