From e7b561f349697bbb0120044a6f88ee7b7e594672 Mon Sep 17 00:00:00 2001 From: Kevin Welsh Date: Mon, 13 May 2024 17:37:34 -0400 Subject: [PATCH 1/4] Fix trajectory display. Handle change from s to ns in clock time. Display the last trajectory position for a fixed time before looping back around. --- .../src/trajectory_visualization.cpp | 65 +++++++++++-------- 1 file changed, 39 insertions(+), 26 deletions(-) diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp index a717256d68..82659c817a 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp @@ -56,7 +56,7 @@ #include #include -#include +#include using namespace std::placeholders; @@ -465,15 +465,6 @@ void TrajectoryVisualization::update(float wall_dt, float sim_dt) { int previous_state = current_state_; int waypoint_count = displaying_trajectory_message_->getWayPointCount(); - if (use_sim_time_property_->getBool()) - { - current_state_time_ += sim_dt; - } - else - { - current_state_time_ += wall_dt; - } - float tm = getStateDisplayTime(); if (trajectory_slider_panel_ && trajectory_slider_panel_->isVisible() && trajectory_slider_panel_->isPaused()) { @@ -485,26 +476,48 @@ void TrajectoryVisualization::update(float wall_dt, float sim_dt) current_state_ = 0; current_state_time_ = 0.0; } - else if (tm < 0.0) - { - // using realtime factors: skip to next waypoint based on elapsed display time - const float rt_factor = -tm; // negative tm is the intended realtime factor - while (current_state_ < waypoint_count && - (tm = displaying_trajectory_message_->getWayPointDurationFromPrevious(current_state_ + 1) / rt_factor) < - current_state_time_) + else + { // Handle the update based on the time elapsed + float dt; + float tm = getStateDisplayTime(); + + if (use_sim_time_property_->getBool()) { - current_state_time_ -= tm; - // if we are stuck in the while loop we should move the robot along the path to keep up - if (tm < current_state_time_) - display_path_robot_->update(displaying_trajectory_message_->getWayPointPtr(current_state_)); + dt = sim_dt; + } + else + { + dt = wall_dt; + } + // Convert from ns to s + dt /= 1e9; + // Scale dt by user entered factor (e.g. "3x") + // The negative sign on tm indicates that dt should be scaled. + if (tm < 0.0) + dt *= -tm; + current_state_time_ += dt; + if (tm < 0.0) + { + for (current_state_ = current_state_; current_state_ < waypoint_count; ++current_state_) + { + float state_duration = displaying_trajectory_message_->getWayPointDurationFromPrevious(current_state_+1); + // If we are on the last state, show it for a fixed amount of time before looping. + // getWayPointDurationFromPrevious returns 0 for the last state, so we need to handle it separately. + // Its our choice to specify either the start state display duration or the end state display duration. + if (current_state_ == waypoint_count - 1) + state_duration = 1.0; + if (current_state_time_ < state_duration) + break; + current_state_time_ -= state_duration; + } + } + else if (current_state_time_ > tm) + { // fixed display time per state + + current_state_time_ = 0.0; ++current_state_; } } - else if (current_state_time_ > tm) - { // fixed display time per state - current_state_time_ = 0.0; - ++current_state_; - } if (current_state_ == previous_state) return; From ac29c6f4849c7954e513773cd4078d6c6f7513d9 Mon Sep 17 00:00:00 2001 From: Kevin Welsh Date: Mon, 13 May 2024 17:44:17 -0400 Subject: [PATCH 2/4] clang-format --- .../src/trajectory_visualization.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp index 82659c817a..24705b9752 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp @@ -56,7 +56,7 @@ #include #include -#include +#include using namespace std::placeholders; @@ -476,11 +476,11 @@ void TrajectoryVisualization::update(float wall_dt, float sim_dt) current_state_ = 0; current_state_time_ = 0.0; } - else + else { // Handle the update based on the time elapsed float dt; float tm = getStateDisplayTime(); - + if (use_sim_time_property_->getBool()) { dt = sim_dt; @@ -500,13 +500,13 @@ void TrajectoryVisualization::update(float wall_dt, float sim_dt) { for (current_state_ = current_state_; current_state_ < waypoint_count; ++current_state_) { - float state_duration = displaying_trajectory_message_->getWayPointDurationFromPrevious(current_state_+1); + float state_duration = displaying_trajectory_message_->getWayPointDurationFromPrevious(current_state_ + 1); // If we are on the last state, show it for a fixed amount of time before looping. // getWayPointDurationFromPrevious returns 0 for the last state, so we need to handle it separately. // Its our choice to specify either the start state display duration or the end state display duration. if (current_state_ == waypoint_count - 1) state_duration = 1.0; - if (current_state_time_ < state_duration) + if (current_state_time_ < state_duration) break; current_state_time_ -= state_duration; } From 1f22702953491da8ec740edde91a51cc3dbb66bd Mon Sep 17 00:00:00 2001 From: Kevin Welsh Date: Tue, 14 May 2024 10:30:07 -0400 Subject: [PATCH 3/4] Don't assign things to themselves :) --- .../rviz_plugin_render_tools/src/trajectory_visualization.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp index 24705b9752..675d56450b 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp @@ -498,7 +498,7 @@ void TrajectoryVisualization::update(float wall_dt, float sim_dt) current_state_time_ += dt; if (tm < 0.0) { - for (current_state_ = current_state_; current_state_ < waypoint_count; ++current_state_) + for (; current_state_ < waypoint_count; ++current_state_) { float state_duration = displaying_trajectory_message_->getWayPointDurationFromPrevious(current_state_ + 1); // If we are on the last state, show it for a fixed amount of time before looping. From f9671a988ba2124468a7247ed17771ce1f2f30ae Mon Sep 17 00:00:00 2001 From: Kevin Welsh Date: Thu, 8 Aug 2024 12:11:15 -0400 Subject: [PATCH 4/4] actually use the alpha value... --- .../planning_scene_rviz_plugin/src/planning_scene_display.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp index a9d6419730..6c51f6c3c5 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp @@ -347,7 +347,7 @@ void PlanningSceneDisplay::changedSceneName() void PlanningSceneDisplay::renderPlanningScene() { QColor color = scene_color_property_->getColor(); - Ogre::ColourValue env_color(color.redF(), color.greenF(), color.blueF()); + Ogre::ColourValue env_color(color.redF(), color.greenF(), color.blueF(), scene_alpha_property_->getFloat()); if (attached_body_color_property_) color = attached_body_color_property_->getColor(); Ogre::ColourValue attached_color(color.redF(), color.greenF(), color.blueF());