Skip to content

Commit 80d4883

Browse files
nbbrooksnbbrooks
committed
Update moveit_ros_visualization to use rviz update std::chrono::nanoseconds API
1 parent 3f931ed commit 80d4883

File tree

12 files changed

+230
-12
lines changed

12 files changed

+230
-12
lines changed

‎moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.hpp‎

Lines changed: 21 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,9 @@
3636

3737
#pragma once
3838

39+
#include <rclcpp/rclcpp.hpp>
40+
#include <rclcpp/version.h>
41+
3942
#include <rviz_common/display.hpp>
4043
#include <rviz_common/panel_dock_widget.hpp>
4144
#include <moveit/planning_scene_rviz_plugin/planning_scene_display.hpp>
@@ -50,12 +53,11 @@
5053
#include <moveit/kinematics_metrics/kinematics_metrics.hpp>
5154
#include <moveit/dynamics_solver/dynamics_solver.hpp>
5255

53-
#include <rclcpp/rclcpp.hpp>
54-
5556
#include <std_msgs/msg/string.hpp>
5657
#include <moveit_msgs/msg/display_trajectory.hpp>
5758
#endif
5859

60+
#include <chrono>
5961
#include <memory>
6062

6163
namespace Ogre
@@ -106,7 +108,16 @@ class MotionPlanningDisplay : public PlanningSceneDisplay
106108
void load(const rviz_common::Config& config) override;
107109
void save(rviz_common::Config config) const override;
108110

111+
// For Rolling, L-turtle, and newer
112+
#if RCLCPP_VERSION_GTE(30, 0, 0)
113+
void update(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds ros_dt) override;
114+
[[deprecated("Use update(std::chrono::nanoseconds, std::chrono::nanoseconds) instead")]] void
115+
update(float wall_dt, float ros_dt) override;
116+
// For Kilted and older
117+
#else
109118
void update(float wall_dt, float ros_dt) override;
119+
#endif
120+
110121
void reset() override;
111122

112123
moveit::core::RobotStateConstPtr getQueryStartState() const
@@ -213,7 +224,15 @@ private Q_SLOTS:
213224
void onRobotModelLoaded() override;
214225
void onNewPlanningSceneState() override;
215226
void onSceneMonitorReceivedUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type) override;
227+
void updateInternal(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds ros_dt) override;
228+
// For Rolling, L-turtle, and newer
229+
#if RCLCPP_VERSION_GTE(30, 0, 0)
230+
[[deprecated("Use updateInternal(std::chrono::nanoseconds, std::chrono::nanoseconds) instead")]] void
231+
updateInternal(double wall_dt, double ros_dt) override;
232+
// For Kilted and older
233+
#else
216234
void updateInternal(double wall_dt, double ros_dt) override;
235+
#endif
217236

218237
void renderWorkspaceBox();
219238
void updateLinkColors();

‎moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.hpp‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,8 @@
3636

3737
#pragma once
3838

39+
#include <rclcpp/version.h>
40+
3941
#include <QWidget>
4042
#include <QTreeWidgetItem>
4143
#include <QListWidgetItem>
@@ -122,7 +124,15 @@ class MotionPlanningFrame : public QWidget
122124
void initFromMoveGroupNS();
123125
void constructPlanningRequest(moveit_msgs::msg::MotionPlanRequest& mreq);
124126

127+
void updateSceneMarkers(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds ros_dt);
128+
// For Rolling, L-turtle, and newer
129+
#if RCLCPP_VERSION_GTE(30, 0, 0)
130+
[[deprecated("Use updateSceneMarkers(std::chrono::nanoseconds, std::chrono::nanoseconds) instead")]] void
131+
updateSceneMarkers(double wall_dt, double ros_dt);
132+
// For Kilted and older
133+
#else
125134
void updateSceneMarkers(double wall_dt, double ros_dt);
135+
#endif
126136

127137
void updateExternalCommunication();
128138

‎moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp‎

Lines changed: 37 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1337,6 +1337,35 @@ void MotionPlanningDisplay::onDisable()
13371337
// ******************************************************************************************
13381338
// Update
13391339
// ******************************************************************************************
1340+
// For Rolling, L-turtle, and newer
1341+
#if RCLCPP_VERSION_GTE(30, 0, 0)
1342+
void MotionPlanningDisplay::update(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds ros_dt)
1343+
{
1344+
if (int_marker_display_)
1345+
int_marker_display_->update(wall_dt, ros_dt);
1346+
if (frame_)
1347+
frame_->updateSceneMarkers(wall_dt, ros_dt);
1348+
1349+
PlanningSceneDisplay::update(wall_dt, ros_dt);
1350+
}
1351+
1352+
void MotionPlanningDisplay::update(float wall_dt, float ros_dt)
1353+
{
1354+
if (int_marker_display_)
1355+
int_marker_display_->update(
1356+
std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<float>(wall_dt)),
1357+
std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<float>(ros_dt)));
1358+
if (frame_)
1359+
frame_->updateSceneMarkers(
1360+
std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<float>(wall_dt)),
1361+
std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<float>(ros_dt)));
1362+
1363+
PlanningSceneDisplay::update(
1364+
std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<float>(wall_dt)),
1365+
std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<float>(ros_dt)));
1366+
}
1367+
// For Kilted and older
1368+
#else
13401369
void MotionPlanningDisplay::update(float wall_dt, float ros_dt)
13411370
{
13421371
if (int_marker_display_)
@@ -1346,8 +1375,9 @@ void MotionPlanningDisplay::update(float wall_dt, float ros_dt)
13461375

13471376
PlanningSceneDisplay::update(wall_dt, ros_dt);
13481377
}
1378+
#endif
13491379

1350-
void MotionPlanningDisplay::updateInternal(double wall_dt, double ros_dt)
1380+
void MotionPlanningDisplay::updateInternal(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds ros_dt)
13511381
{
13521382
PlanningSceneDisplay::updateInternal(wall_dt, ros_dt);
13531383

@@ -1357,6 +1387,12 @@ void MotionPlanningDisplay::updateInternal(double wall_dt, double ros_dt)
13571387
renderWorkspaceBox();
13581388
}
13591389

1390+
void MotionPlanningDisplay::updateInternal(double wall_dt, double ros_dt)
1391+
{
1392+
updateInternal(std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<float>(wall_dt)),
1393+
std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<float>(ros_dt)));
1394+
}
1395+
13601396
void MotionPlanningDisplay::load(const rviz_common::Config& config)
13611397
{
13621398
PlanningSceneDisplay::load(config);

‎moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp‎

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -686,12 +686,18 @@ void MotionPlanningFrame::tabChanged(int index)
686686
}
687687
}
688688

689-
void MotionPlanningFrame::updateSceneMarkers(double /*wall_dt*/, double /*ros_dt*/)
689+
void MotionPlanningFrame::updateSceneMarkers(std::chrono::nanoseconds /*wall_dt*/, std::chrono::nanoseconds /*ros_dt*/)
690690
{
691691
if (scene_marker_)
692692
scene_marker_->update();
693693
}
694694

695+
void MotionPlanningFrame::updateSceneMarkers(double wall_dt, double ros_dt)
696+
{
697+
updateSceneMarkers(std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<float>(wall_dt)),
698+
std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<float>(ros_dt)));
699+
}
700+
695701
void MotionPlanningFrame::updateExternalCommunication()
696702
{
697703
if (ui_->allow_external_program->isChecked())

‎moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.hpp‎

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,8 @@
3636

3737
#pragma once
3838

39+
#include <rclcpp/version.h>
40+
3941
#include <rviz_common/display.hpp>
4042
#include <rviz_default_plugins/robot/robot.hpp>
4143
#include <rviz_common/properties/string_property.hpp>
@@ -49,6 +51,8 @@
4951

5052
#include <moveit_planning_scene_rviz_plugin_core_export.h>
5153

54+
#include <chrono>
55+
5256
namespace Ogre
5357
{
5458
class SceneNode;
@@ -79,7 +83,16 @@ class MOVEIT_PLANNING_SCENE_RVIZ_PLUGIN_CORE_EXPORT PlanningSceneDisplay : publi
7983
void load(const rviz_common::Config& config) override;
8084
void save(rviz_common::Config config) const override;
8185

86+
// For Rolling, L-turtle, and newer
87+
#if RCLCPP_VERSION_GTE(30, 0, 0)
88+
void update(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds ros_dt) override;
89+
[[deprecated("Use update(std::chrono::nanoseconds, std::chrono::nanoseconds) instead")]] void
90+
update(float wall_dt, float ros_dt) override;
91+
// For Kilted and older
92+
#else
8293
void update(float wall_dt, float ros_dt) override;
94+
#endif
95+
8396
void reset() override;
8497

8598
void setLinkColor(const std::string& link_name, const QColor& color);
@@ -178,7 +191,15 @@ protected Q_SLOTS:
178191
void fixedFrameChanged() override;
179192

180193
// new virtual functions added by this plugin
194+
virtual void updateInternal(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds ros_dt);
195+
// For Rolling, L-turtle, and newer
196+
#if RCLCPP_VERSION_GTE(30, 0, 0)
197+
[[deprecated("Use updateInternal(std::chrono::nanoseconds, std::chrono::nanoseconds) instead")]] virtual void
198+
updateInternal(double wall_dt, double ros_dt);
199+
// For Kilted and older
200+
#else
181201
virtual void updateInternal(double wall_dt, double ros_dt);
202+
#endif
182203
virtual void onSceneMonitorReceivedUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type);
183204

184205
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;

‎moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp‎

Lines changed: 38 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -35,8 +35,6 @@
3535

3636
/* Author: Ioan Sucan */
3737

38-
#include <rclcpp/version.h>
39-
4038
#include <moveit/common_planning_interface_objects/common_objects.hpp>
4139
#include <moveit/planning_scene_rviz_plugin/planning_scene_display.hpp>
4240
#include <moveit/rviz_plugin_render_tools/robot_state_visualization.hpp>
@@ -669,6 +667,35 @@ void PlanningSceneDisplay::queueRenderSceneGeometry()
669667
planning_scene_needs_render_ = true;
670668
}
671669

670+
// For Rolling, L-turtle, and newer
671+
#if RCLCPP_VERSION_GTE(30, 0, 0)
672+
void PlanningSceneDisplay::update(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds ros_dt)
673+
{
674+
Display::update(wall_dt, ros_dt);
675+
676+
executeMainLoopJobs();
677+
678+
calculateOffsetPosition();
679+
680+
if (planning_scene_monitor_)
681+
updateInternal(wall_dt, ros_dt);
682+
}
683+
684+
void PlanningSceneDisplay::update(float wall_dt, float ros_dt)
685+
{
686+
Display::update(std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<float>(wall_dt)),
687+
std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<float>(ros_dt)));
688+
689+
executeMainLoopJobs();
690+
691+
calculateOffsetPosition();
692+
693+
if (planning_scene_monitor_)
694+
updateInternal(std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<float>(wall_dt)),
695+
std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<float>(ros_dt)));
696+
}
697+
// For Kilted and older
698+
#else
672699
void PlanningSceneDisplay::update(float wall_dt, float ros_dt)
673700
{
674701
Display::update(wall_dt, ros_dt);
@@ -680,10 +707,11 @@ void PlanningSceneDisplay::update(float wall_dt, float ros_dt)
680707
if (planning_scene_monitor_)
681708
updateInternal(wall_dt, ros_dt);
682709
}
710+
#endif
683711

684-
void PlanningSceneDisplay::updateInternal(double wall_dt, double /*ros_dt*/)
712+
void PlanningSceneDisplay::updateInternal(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds /*ros_dt*/)
685713
{
686-
current_scene_time_ += wall_dt;
714+
current_scene_time_ += std::chrono::duration_cast<std::chrono::duration<double>>(wall_dt).count();
687715
if (planning_scene_render_ &&
688716
((current_scene_time_ > scene_display_time_property_->getFloat() && robot_state_needs_render_) ||
689717
planning_scene_needs_render_))
@@ -695,6 +723,12 @@ void PlanningSceneDisplay::updateInternal(double wall_dt, double /*ros_dt*/)
695723
}
696724
}
697725

726+
void PlanningSceneDisplay::updateInternal(double wall_dt, double ros_dt)
727+
{
728+
updateInternal(std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<float>(wall_dt)),
729+
std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<float>(ros_dt)));
730+
}
731+
698732
void PlanningSceneDisplay::load(const rviz_common::Config& config)
699733
{
700734
Display::load(config);

‎moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.hpp‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,8 @@
3636

3737
#pragma once
3838

39+
#include <rclcpp/version.h>
40+
3941
#include <rviz_common/display.hpp>
4042

4143
#ifndef Q_MOC_RUN
@@ -45,6 +47,8 @@
4547
#include <rclcpp/rclcpp.hpp>
4648
#endif
4749

50+
#include <chrono>
51+
4852
namespace rviz_common
4953
{
5054
namespace properties
@@ -71,7 +75,17 @@ class RobotStateDisplay : public rviz_common::Display
7175
~RobotStateDisplay() override;
7276

7377
void load(const rviz_common::Config& config) override;
78+
79+
// For Rolling, L-turtle, and newer
80+
#if RCLCPP_VERSION_GTE(30, 0, 0)
81+
void update(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds ros_dt) override;
82+
[[deprecated("Use update(std::chrono::nanoseconds, std::chrono::nanoseconds) instead")]] void
83+
update(float wall_dt, float ros_dt) override;
84+
// For Kilted and older
85+
#else
7486
void update(float wall_dt, float ros_dt) override;
87+
#endif
88+
7589
void reset() override;
7690

7791
const moveit::core::RobotModelConstPtr& getRobotModel() const

‎moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp‎

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -459,6 +459,34 @@ void RobotStateDisplay::onDisable()
459459
Display::onDisable();
460460
}
461461

462+
// For Rolling, L-turtle, and newer
463+
#if RCLCPP_VERSION_GTE(30, 0, 0)
464+
void RobotStateDisplay::update(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds ros_dt)
465+
{
466+
Display::update(wall_dt, ros_dt);
467+
calculateOffsetPosition();
468+
if (robot_ && update_state_ && robot_state_)
469+
{
470+
update_state_ = false;
471+
robot_state_->update();
472+
robot_->update(robot_state_);
473+
}
474+
}
475+
476+
void RobotStateDisplay::update(float wall_dt, float ros_dt)
477+
{
478+
Display::update(std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<float>(wall_dt)),
479+
std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<float>(ros_dt)));
480+
calculateOffsetPosition();
481+
if (robot_ && update_state_ && robot_state_)
482+
{
483+
update_state_ = false;
484+
robot_state_->update();
485+
robot_->update(robot_state_);
486+
}
487+
}
488+
// For Kilted and older
489+
#else
462490
void RobotStateDisplay::update(float wall_dt, float ros_dt)
463491
{
464492
Display::update(wall_dt, ros_dt);
@@ -470,6 +498,7 @@ void RobotStateDisplay::update(float wall_dt, float ros_dt)
470498
robot_->update(robot_state_);
471499
}
472500
}
501+
#endif
473502

474503
// ******************************************************************************************
475504
// Calculate Offset Position

‎moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.hpp‎

Lines changed: 11 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,8 @@
3636

3737
#pragma once
3838

39+
#include <rclcpp/version.h>
40+
3941
#include <moveit/macros/class_forward.hpp>
4042
#include <rviz_common/display.hpp>
4143
#include <rviz_common/panel_dock_widget.hpp>
@@ -88,7 +90,15 @@ class TrajectoryVisualization : public QObject
8890

8991
~TrajectoryVisualization() override;
9092

91-
virtual void update(double wall_dt, double sim_dt);
93+
virtual void update(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds sim_dt);
94+
// For Rolling, L-turtle, and newer
95+
#if RCLCPP_VERSION_GTE(30, 0, 0)
96+
[[deprecated("Use update(std::chrono::nanoseconds, std::chrono::nanoseconds) instead")]] virtual void
97+
update(double wall_dt, double ros_dt);
98+
// For Kilted and older
99+
#else
100+
virtual void update(double wall_dt, double ros_dt);
101+
#endif
92102
virtual void reset();
93103

94104
void onInitialize(const rclcpp::Node::SharedPtr& node, Ogre::SceneNode* scene_node,

0 commit comments

Comments
 (0)