Skip to content

Commit

Permalink
Merge pull request #73 from rjoomen/master
Browse files Browse the repository at this point in the history 
EnvironmentMonitor settings cleanup
  • Loading branch information
@rjoomen
rjoomen authored Oct 6, 2023
2 parents c2423a7 + 49837e5 commit 7397e48
Show file tree
Hide file tree
Showing 7 changed files with 36 additions and 108 deletions.
6 changes: 3 additions & 3 deletions dependencies.repos
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,19 +6,19 @@ 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
version: 0.6.1
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
Expand Down
3 changes: 0 additions & 3 deletions tesseract_rviz/include/tesseract_rviz/environment_display.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<const tesseract_gui::ComponentInfo> component_info);
Expand Down
3 changes: 1 addition & 2 deletions tesseract_rviz/include/tesseract_rviz/environment_monitor_properties.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,8 +50,7 @@ class EnvironmentMonitorProperties : public QObject
*/
std::shared_ptr<const tesseract_gui::ComponentInfo> 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<const tesseract_gui::ComponentInfo> component_info);
Expand Down
3 changes: 0 additions & 3 deletions tesseract_rviz/include/tesseract_rviz/workbench_display.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<const tesseract_gui::ComponentInfo> component_info);
Expand Down
12 changes: 0 additions & 12 deletions tesseract_rviz/src/environment_display.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down
105 changes: 32 additions & 73 deletions tesseract_rviz/src/environment_monitor_properties.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<rclcpp::Node> rviz_node;
Expand Down Expand Up @@ -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",
Expand Down Expand Up @@ -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();
Expand All @@ -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();
}

Expand All @@ -149,76 +154,54 @@ std::shared_ptr<const tesseract_gui::ComponentInfo> 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))
Expand All @@ -232,9 +215,6 @@ void EnvironmentMonitorProperties::onURDFDescriptionChanged()
auto locator = std::make_shared<tesseract_rosutils::ROSResourceLocator>();
if (env->init(urdf_xml_string, srdf_xml_string, locator))
{
if (data_->monitor != nullptr)
data_->monitor->shutdown();

data_->monitor = std::make_unique<tesseract_monitoring::ROSEnvironmentMonitor>(
data_->node, env, data_->urdf_description_string_property->getStdString());
if (data_->monitor != nullptr)
Expand All @@ -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<tesseract_environment::Environment>();
data_->monitor =
Expand Down Expand Up @@ -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<std::string, double> jv;
Expand All @@ -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<tesseract_monitoring::ROSEnvironmentMonitor>(
data_->node, env, data_->urdf_description_string_property->getStdString());
if (data_->monitor != nullptr)
Expand Down Expand Up @@ -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<tesseract_msgs::msg::Environment>(
Expand Down
12 changes: 0 additions & 12 deletions tesseract_rviz/src/workbench_display.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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));
Expand Down

0 comments on commit 7397e48

Please sign in to comment.