Implement model visibility toggling (#226)

* Implement model visibility toggling

* Update building_sim_plugins/building_gazebo_plugins/src/toggle_floors.cpp

Co-authored-by: Yadu <yadunund@openrobotics.org>

* Change loop structure in toggle_floors.cpp

* toggle door visibility

* toggle lift shaft door visibility

* code style

* visibility toggling of robots

* tested and works well! minor changes on readability

Co-authored-by: Yadu <yadunund@openrobotics.org>
Co-authored-by: youliang <tan_you_liang@hotmail.com>

building_map_tools/building_map/building.py

@@ -243,6 +243,29 @@ def generate_sdf_world(self, options):
                         'name': level_name,
                         'model_name': f'{self.name}_{level_name}'})
 
+                for model in level.models:
+                    if model.static:
+                        model_ele = SubElement(
+                            floor_ele,
+                            'model',
+                            {'name': model.name})
+
+                for door in level.doors:
+                    model_ele = SubElement(
+                        floor_ele,
+                        'model',
+                        {'name': door.params['name'].value})
+
+                for lift_name, lift in self.lifts.items():
+                    if level_name in lift.level_doors:
+                        for door in lift.doors:
+                            if door.name in lift.level_doors[level_name]:
+                                model_ele = SubElement(
+                                    floor_ele,
+                                    'model',
+                                    {'name': (f'ShaftDoor_{lift_name}_' +
+                                              f'{level_name}_{door.name}')})
+
         elif 'ignition' in options:
             plugin_ele = gui_ele.find('.//plugin[@filename="GzScene3D"]')
             camera_pose_ele = plugin_ele.find('camera_pose')

building_map_tools/building_map/level.py

@@ -157,12 +157,9 @@ def generate_walls(self, model_ele, model_name, model_path):
                 self.transformed_vertices)
 
     def generate_sdf_models(self, world_ele):
-        model_cnt = 0
         for model in self.models:
-            model_cnt += 1
             model.generate(
                 world_ele,
-                model_cnt,
                 self.transform,
                 self.elevation)
 

building_map_tools/building_map/model.py

@@ -30,7 +30,7 @@ def __init__(self, name, yaml_node):
 
         self.yaw = yaml_node['yaw']
 
-    def generate(self, world_ele, model_cnt, transform, elevation):
+    def generate(self, world_ele, transform, elevation):
         include_ele = SubElement(world_ele, 'include')
         name_ele = SubElement(include_ele, 'name')
         name_ele.text = self.name

building_sim_plugins/building_gazebo_plugins/CMakeLists.txt

@@ -114,18 +114,20 @@ target_include_directories(lift
 
 add_library(toggle_floors SHARED src/toggle_floors.cpp)
 
+ament_target_dependencies(toggle_floors
+    Qt5
+    gazebo_ros
+    rmf_fleet_msgs
+    rclcpp
+)
+
 target_include_directories(toggle_floors
   PUBLIC
     ${GAZEBO_INCLUDE_DIRS}
     #${building_sim_common_INCLUDE_DIRS}
     ${Qt5Core_INCLUDE_DIRS}
 )
 
-target_link_libraries(toggle_floors
-  PUBLIC
-    Qt5::Widgets
-)
-
 ###############################
 # thumbnail generation stuff
 ###############################

building_sim_plugins/building_gazebo_plugins/src/toggle_floors.cpp

@@ -6,15 +6,26 @@
 #include <gazebo/msgs/msgs.hh>
 #include <gazebo/rendering/rendering.hh>
 
+#include <rclcpp/rclcpp.hpp>
+#include <gazebo_ros/node.hpp>
+#include <rmf_fleet_msgs/msg/fleet_state.hpp>
+#include <rmf_fleet_msgs/msg/robot_state.hpp>
+
 #include <string>
-using std::string;
+#include <unordered_map>
 
+using std::string;
+using FleetState = rmf_fleet_msgs::msg::FleetState;
+using RobotState = rmf_fleet_msgs::msg::RobotState;
 
 class ToggleFloors : public gazebo::GUIPlugin
 {
   Q_OBJECT
   gazebo::transport::NodePtr node;
   gazebo::transport::PublisherPtr visual_pub;
+  std::unordered_map<string, std::atomic<bool>> floor_visibility;
+  gazebo_ros::Node::SharedPtr ros_node;
+  rclcpp::Subscription<FleetState>::SharedPtr fleet_state_sub;
 
 public:
   ToggleFloors()
@@ -33,6 +44,24 @@ class ToggleFloors : public gazebo::GUIPlugin
   void Load(sdf::ElementPtr sdf)
   {
     printf("ToggleFloors::Load()\n");
+    ros_node = gazebo_ros::Node::Get(sdf);
+
+    // toggle non-static robots
+    fleet_state_sub = ros_node->create_subscription<FleetState>(
+      "/fleet_states", rclcpp::SystemDefaultsQoS(),
+      [&](FleetState::UniquePtr msg)
+      {
+        bool visible;
+        gazebo::msgs::Visual visual_msg;
+        visual_msg.set_parent_name("world");
+        for (const RobotState& robot : msg->robots)
+        {
+          visible = floor_visibility[robot.location.level_name];
+          visual_msg.set_name(robot.name);
+          visual_msg.set_visible(visible);
+          visual_pub->Publish(visual_msg);
+        }
+      });
 
     QHBoxLayout* hbox = new QHBoxLayout;
 
@@ -45,6 +74,17 @@ class ToggleFloors : public gazebo::GUIPlugin
       string floor_name = floor_ele->GetAttribute("name")->GetAsString();
       string model_name =
         floor_ele->GetAttribute("model_name")->GetAsString();
+
+      std::vector<string> models;
+      auto model_ele = floor_ele->GetElement("model");
+      while (model_ele)
+      {
+        if (model_ele->HasAttribute("name"))
+          models.push_back(model_ele->GetAttribute("name")->GetAsString());
+        model_ele = model_ele->GetNextElement("model");
+      }
+      floor_visibility[floor_name] = true;
+
       printf(
         "ToggleFloors::Load found a floor element: [%s]->[%s]\n",
         floor_name.c_str(),
@@ -57,18 +97,23 @@ class ToggleFloors : public gazebo::GUIPlugin
       connect(
         button,
         &QAbstractButton::clicked,
-        [this, button, model_name]()
+        [this, button, floor_name, model_name, models]()
         {
-          this->button_clicked(button, model_name);
+          this->button_clicked(button, floor_name, model_name, models);
         });
       hbox->addWidget(button);
     }
     setLayout(hbox);
   }
 
-  void button_clicked(QPushButton* button, string model_name)
+  void button_clicked(
+    QPushButton* button,
+    string floor_name,
+    string model_name,
+    std::vector<string> models)
   {
     bool visible = button->isChecked();
+    floor_visibility[floor_name] = visible;
     printf(
       "clicked: [%s] %s\n",
       model_name.c_str(),
@@ -78,6 +123,11 @@ class ToggleFloors : public gazebo::GUIPlugin
     visual_msg.set_name(model_name);
     visual_msg.set_visible(visible);
     visual_pub->Publish(visual_msg);
+    for (const string& model : models)
+    {
+      visual_msg.set_name(model);
+      visual_pub->Publish(visual_msg);
+    }
   }
 };