diff --git a/dependencies.repos b/dependencies.repos index 7d3da7d0..f755523a 100644 --- a/dependencies.repos +++ b/dependencies.repos @@ -6,7 +6,7 @@ repositories: tesseract: type: git url: https://github.com/tesseract-robotics/tesseract.git - version: 0.19.2 + version: 0.20.0 trajopt: type: git url: https://github.com/tesseract-robotics/trajopt.git @@ -14,11 +14,11 @@ repositories: tesseract_planning: type: git url: https://github.com/tesseract-robotics/tesseract_planning.git - version: 0.19.1 + version: 0.20.0 tesseract_qt: type: git url: https://github.com/tesseract-robotics/tesseract_qt.git - version: 0.19.0 + version: 0.20.0 descartes_light: type: git url: https://github.com/swri-robotics/descartes_light.git diff --git a/tesseract_rviz/include/tesseract_rviz/environment_display.h b/tesseract_rviz/include/tesseract_rviz/environment_display.h index fd708b70..0711e64f 100644 --- a/tesseract_rviz/include/tesseract_rviz/environment_display.h +++ b/tesseract_rviz/include/tesseract_rviz/environment_display.h @@ -16,9 +16,6 @@ class EnvironmentDisplay : public rviz_common::Display void update(float wall_dt, float ros_dt) override; void reset() override; - void load(const rviz_common::Config& config) override; - void save(rviz_common::Config config) const override; - public Q_SLOTS: void onEnableChanged() override; void onComponentInfoChanged(std::shared_ptr component_info); diff --git a/tesseract_rviz/include/tesseract_rviz/environment_monitor_properties.h b/tesseract_rviz/include/tesseract_rviz/environment_monitor_properties.h index dec6945b..ccdb1455 100644 --- a/tesseract_rviz/include/tesseract_rviz/environment_monitor_properties.h +++ b/tesseract_rviz/include/tesseract_rviz/environment_monitor_properties.h @@ -50,8 +50,7 @@ class EnvironmentMonitorProperties : public QObject */ std::shared_ptr getComponentInfo() const; - void load(const rviz_common::Config& config); - void save(rviz_common::Config config) const; + void resetMonitor(); Q_SIGNALS: void componentInfoChanged(std::shared_ptr component_info); diff --git a/tesseract_rviz/include/tesseract_rviz/workbench_display.h b/tesseract_rviz/include/tesseract_rviz/workbench_display.h index e1d73988..a9297ce9 100644 --- a/tesseract_rviz/include/tesseract_rviz/workbench_display.h +++ b/tesseract_rviz/include/tesseract_rviz/workbench_display.h @@ -16,9 +16,6 @@ class WorkbenchDisplay : public rviz_common::Display void update(float wall_dt, float ros_dt) override; void reset() override; - void load(const rviz_common::Config& config) override; - void save(rviz_common::Config config) const override; - public Q_SLOTS: void onEnableChanged() override; void onComponentInfoChanged(std::shared_ptr component_info); diff --git a/tesseract_rviz/src/environment_display.cpp b/tesseract_rviz/src/environment_display.cpp index bcd7a36d..796f39e2 100644 --- a/tesseract_rviz/src/environment_display.cpp +++ b/tesseract_rviz/src/environment_display.cpp @@ -101,18 +101,6 @@ void EnvironmentDisplay::update(float wall_dt, float ros_dt) new tesseract_gui::events::PreRender(data_->widget->getComponentInfo()->getSceneName())); } -void EnvironmentDisplay::load(const rviz_common::Config& config) -{ - rviz_common::Display::load(config); - data_->monitor_properties->load(config); -} - -void EnvironmentDisplay::save(rviz_common::Config config) const -{ - data_->monitor_properties->save(config); - rviz_common::Display::save(config); -} - void EnvironmentDisplay::onEnable() { Display::onEnable(); diff --git a/tesseract_rviz/src/environment_monitor_properties.cpp b/tesseract_rviz/src/environment_monitor_properties.cpp index aca944be..ede85408 100644 --- a/tesseract_rviz/src/environment_monitor_properties.cpp +++ b/tesseract_rviz/src/environment_monitor_properties.cpp @@ -26,6 +26,10 @@ namespace tesseract_rviz { +static constexpr char MODE_URDF[] = "URDF"; +static constexpr char MODE_MONITOR[] = "Monitor"; +static constexpr char MODE_SNAPSHOT[] = "Snapshot"; + struct EnvironmentMonitorProperties::Implementation { std::shared_ptr rviz_node; @@ -65,16 +69,16 @@ EnvironmentMonitorProperties::EnvironmentMonitorProperties(rviz_common::Display* data_->main_property = data_->parent; data_->display_mode_property = new rviz_common::properties::EnumProperty("Display Mode", - "URDF", + MODE_URDF, "Leverage URDF or connect to monitor " "namespace", data_->main_property, SLOT(onDisplayModeChanged()), this); - data_->display_mode_property->addOptionStd("URDF", 0); - data_->display_mode_property->addOptionStd("Monitor", 1); - data_->display_mode_property->addOptionStd("Snapshot", 2); + data_->display_mode_property->addOptionStd(MODE_URDF); + data_->display_mode_property->addOptionStd(MODE_MONITOR); + data_->display_mode_property->addOptionStd(MODE_SNAPSHOT); data_->urdf_description_string_property = new rviz_common::properties::StringProperty("URDF Parameter", @@ -115,7 +119,7 @@ EnvironmentMonitorProperties::EnvironmentMonitorProperties(rviz_common::Display* EnvironmentMonitorProperties::~EnvironmentMonitorProperties() { - tesseract_gui::EnvironmentManager::remove(data_->component_info); + resetMonitor(); data_->internal_node_executor->cancel(); if (data_->internal_node_spinner->joinable()) data_->internal_node_spinner->join(); @@ -141,6 +145,7 @@ void EnvironmentMonitorProperties::onInitialize(Ogre::SceneManager* scene_manage data_->scene_manager = scene_manager; data_->scene_node = scene_node; + // Call at least once to initialize visibility of properties onDisplayModeChanged(); } @@ -149,76 +154,54 @@ std::shared_ptr EnvironmentMonitorProperties return data_->component_info; } -void EnvironmentMonitorProperties::load(const rviz_common::Config& config) +void EnvironmentMonitorProperties::resetMonitor() { - QString mode; - if (config.mapGetString("tesseract::EnvMonitorMode", &mode)) - data_->display_mode_property->setString(mode); - - QString urdf_description; - if (config.mapGetString("tesseract::EnvMonitorURDFDescription", &urdf_description)) - data_->urdf_description_string_property->setString(urdf_description); - - QString topic; - if (config.mapGetString("tesseract::EnvMonitorTopic", &topic)) - data_->environment_topic_property->setString(topic); + // Shutdown the callback + data_->snapshot.reset(); - if (config.mapGetString("tesseract::EnvMonitorSnapshotTopic", &topic)) - data_->environment_snapshot_topic_property->setString(topic); + if (data_->monitor != nullptr) + data_->monitor->shutdown(); - if (config.mapGetString("tesseract::EnvMonitorJointStateTopic", &topic)) - data_->joint_state_topic_property->setString(topic); -} + tesseract_gui::EnvironmentManager::remove(data_->component_info); + data_->render_manager = nullptr; + data_->contact_results_render_manager = nullptr; -void EnvironmentMonitorProperties::save(rviz_common::Config config) const -{ - config.mapSetValue("tesseract::EnvMonitorMode", data_->display_mode_property->getString()); - config.mapSetValue("tesseract::EnvMonitorURDFDescription", data_->urdf_description_string_property->getString()); - config.mapSetValue("tesseract::EnvMonitorTopic", data_->environment_topic_property->getString()); - config.mapSetValue("tesseract::EnvMonitorSnapshotTopic", data_->environment_snapshot_topic_property->getString()); - config.mapSetValue("tesseract::EnvMonitorJointStateTopic", data_->joint_state_topic_property->getString()); + data_->parent->deleteStatus("Tesseract"); } void EnvironmentMonitorProperties::onDisplayModeChanged() { - if (data_->display_mode_property->getOptionInt() == 0) + if (data_->display_mode_property->getStdString() == MODE_URDF) { data_->environment_topic_property->setHidden(true); data_->environment_snapshot_topic_property->setHidden(true); - data_->snapshot.reset(); - data_->urdf_description_string_property->setHidden(false); onURDFDescriptionChanged(); } - else if (data_->display_mode_property->getOptionInt() == 1) + else if (data_->display_mode_property->getStdString() == MODE_MONITOR) { data_->urdf_description_string_property->setHidden(true); data_->environment_snapshot_topic_property->setHidden(true); - data_->snapshot.reset(); - data_->environment_topic_property->setHidden(false); onEnvironmentTopicChanged(); } - else if (data_->display_mode_property->getOptionInt() == 2) + else if (data_->display_mode_property->getStdString() == MODE_SNAPSHOT) { data_->urdf_description_string_property->setHidden(true); data_->environment_topic_property->setHidden(true); data_->environment_snapshot_topic_property->setHidden(false); onEnvironmentSnapshotTopicChanged(); } - onJointStateTopicChanged(); } void EnvironmentMonitorProperties::onURDFDescriptionChanged() { + if (data_->display_mode_property->getStdString() != MODE_URDF) + return; if (data_->scene_manager == nullptr || data_->scene_node == nullptr) return; - data_->parent->deleteStatus("Tesseract"); - - tesseract_gui::EnvironmentManager::remove(data_->component_info); - data_->render_manager = nullptr; - data_->contact_results_render_manager = nullptr; + resetMonitor(); std::string urdf_xml_string, srdf_xml_string; if (!data_->rviz_node->get_parameter(data_->urdf_description_string_property->getStdString(), urdf_xml_string)) @@ -232,9 +215,6 @@ void EnvironmentMonitorProperties::onURDFDescriptionChanged() auto locator = std::make_shared(); if (env->init(urdf_xml_string, srdf_xml_string, locator)) { - if (data_->monitor != nullptr) - data_->monitor->shutdown(); - data_->monitor = std::make_unique( data_->node, env, data_->urdf_description_string_property->getStdString()); if (data_->monitor != nullptr) @@ -261,17 +241,12 @@ void EnvironmentMonitorProperties::onURDFDescriptionChanged() void EnvironmentMonitorProperties::onEnvironmentTopicChanged() { + if (data_->display_mode_property->getStdString() != MODE_MONITOR) + return; if (data_->scene_manager == nullptr || data_->scene_node == nullptr) return; - data_->parent->deleteStatus("Tesseract"); - - if (data_->monitor != nullptr) - data_->monitor->shutdown(); - - tesseract_gui::EnvironmentManager::remove(data_->component_info); - data_->render_manager = nullptr; - data_->contact_results_render_manager = nullptr; + resetMonitor(); auto env = std::make_shared(); data_->monitor = @@ -306,14 +281,7 @@ void EnvironmentMonitorProperties::snapshotCallback(const tesseract_msgs::msg::E if (data_->scene_manager == nullptr || data_->scene_node == nullptr) return; - if (data_->monitor != nullptr) - data_->monitor->shutdown(); - - data_->parent->deleteStatus("Tesseract"); - - tesseract_gui::EnvironmentManager::remove(data_->component_info); - data_->render_manager = nullptr; - data_->contact_results_render_manager = nullptr; + resetMonitor(); tesseract_environment::Commands commands = tesseract_rosutils::fromMsg(msg->command_history); std::unordered_map jv; @@ -325,9 +293,6 @@ void EnvironmentMonitorProperties::snapshotCallback(const tesseract_msgs::msg::E { env->setState(jv); - if (data_->monitor != nullptr) - data_->monitor->shutdown(); - data_->monitor = std::make_unique( data_->node, env, data_->urdf_description_string_property->getStdString()); if (data_->monitor != nullptr) @@ -355,18 +320,12 @@ void EnvironmentMonitorProperties::snapshotCallback(const tesseract_msgs::msg::E void EnvironmentMonitorProperties::onEnvironmentSnapshotTopicChanged() { + if (data_->display_mode_property->getStdString() != MODE_SNAPSHOT) + return; if (data_->scene_manager == nullptr || data_->scene_node == nullptr) return; - if (data_->monitor != nullptr) - data_->monitor->shutdown(); - - // Shutdown the callback - data_->snapshot.reset(); - - tesseract_gui::EnvironmentManager::remove(data_->component_info); - data_->render_manager = nullptr; - data_->contact_results_render_manager = nullptr; + resetMonitor(); // Connect to new topic data_->snapshot = data_->node->create_subscription( diff --git a/tesseract_rviz/src/workbench_display.cpp b/tesseract_rviz/src/workbench_display.cpp index 156d1a6f..063ea637 100644 --- a/tesseract_rviz/src/workbench_display.cpp +++ b/tesseract_rviz/src/workbench_display.cpp @@ -121,18 +121,6 @@ void WorkbenchDisplay::update(float wall_dt, float ros_dt) new tesseract_gui::events::PreRender(data_->widget->getComponentInfo()->getSceneName())); } -void WorkbenchDisplay::load(const rviz_common::Config& config) -{ - rviz_common::Display::load(config); - data_->monitor_properties->load(config); -} - -void WorkbenchDisplay::save(rviz_common::Config config) const -{ - data_->monitor_properties->save(config); - rviz_common::Display::save(config); -} - void WorkbenchDisplay::onEnableChanged() { QApplication::setOverrideCursor(QCursor(Qt::WaitCursor));