Natively display floorplan on rviz

.github/workflows/build_foxy.yaml

@@ -21,7 +21,7 @@ jobs:
         mkdir -p ws_rmf/src
         cd ws_rmf/src
         git clone https://github.com/osrf/traffic_editor
-        git clone https://github.com/osrf/rmf_core -b foxy
+        git clone https://github.com/osrf/rmf_core
 
     - name: checkout
       uses: actions/checkout@v2

.github/workflows/style.yaml

@@ -4,7 +4,7 @@ jobs:
   build:
     runs-on: ubuntu-18.04
     container:
-      image: docker://ros:eloquent-ros-base-bionic
+      image: docker://ros:foxy-ros-base-focal
     steps:
     - uses: actions/checkout@v1
     - name: deps
@@ -16,7 +16,7 @@ jobs:
       shell: bash
       run: |
         wget https://raw.githubusercontent.com/osrf/rmf_core/master/rmf_utils/test/format/rmf_code_style.cfg
-        source /opt/ros/eloquent/setup.bash
+        source /opt/ros/foxy/setup.bash
         ament_uncrustify -c rmf_code_style.cfg rviz2_plugin/ rmf_schedule_visualizer/src/ rmf_schedule_visualizer/test/
     - name: pycodestyle
       shell: bash

README.md

@@ -1,7 +1,7 @@
-|       | Ubuntu 18.04                                                                               | Ubuntu 20.04                                                                           |
-|-------|--------------------------------------------------------------------------------------------|----------------------------------------------------------------------------------------|
-| Build | ![]( https://github.com/osrf/rmf_schedule_visualizer/workflows/build_eloquent/badge.svg ) | ![]( https://github.com/osrf/rmf_schedule_visualizer/workflows/build_foxy/badge.svg ) |
-| Style | ![]( https://github.com/osrf/rmf_schedule_visualizer/workflows/style/badge.svg )          |                                                                                        |
+|       | Ubuntu 20.04                                                                              |
+|-------|-------------------------------------------------------------------------------------------|
+| Build | ![]( https://github.com/osrf/rmf_schedule_visualizer/workflows/build_foxy/badge.svg )     |
+| Style | ![]( https://github.com/osrf/rmf_schedule_visualizer/workflows/style/badge.svg )          |
 
 # rmf_schedule_visualizer
 
@@ -13,7 +13,7 @@ A visualizer for robot trajectories in the `rmf schedule database`, live locatio
 
 The visualizer is developed and tested on
 * [Ubuntu 18.04 LTS](http://releases.ubuntu.com/18.04/) 
-* [ROS2 Eloquent](https://index.ros.org/doc/ros2/Installation/#installationguide).
+* [ROS2 Foxy](https://index.ros.org/doc/ros2/Installation/#installationguide).
 
 ## Installation 
 Install RMF dependencies
@@ -37,8 +37,8 @@ git clone https://github.com/osrf/traffic_editor.git
 git clone https://github.com/osrf/rmf_schedule_visualizer.git
 cd ~/ws_rmf
 rosdep update
-rosdep install --from-paths src --ignore-src --rosdistro eloquent -yr
-source /opt/ros/eloquent/setup.bash
+rosdep install --from-paths src --ignore-src --rosdistro foxy -yr
+source /opt/ros/foxy/setup.bash
 CXX=g++-8 colcon build --cmake-args -DCMAKE_BUILD_TYPE=RELEASE
 ```
 

rmf_schedule_visualizer/CMakeLists.txt

@@ -18,9 +18,11 @@ find_package(ament_cmake REQUIRED)
 find_package(rmf_traffic REQUIRED)
 find_package(rmf_traffic_ros2 REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(OpenCV REQUIRED )
 find_package(websocketpp REQUIRED)
 find_package(Boost REQUIRED COMPONENTS system date_time regex random)
 find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
 find_package(rmf_traffic_msgs REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
@@ -68,8 +70,10 @@ target_link_libraries(rviz2
     rmf_traffic_ros2::rmf_traffic_ros2
     rmf_traffic::rmf_traffic
     ${rclcpp_LIBRARIES}
+    ${OpenCV_LIBRARIES}
     ${visualization_msgs_LIBRARIES}
     ${geometry_msgs_LIBRARIES}
+    ${nav_msgs_LIBRARIES}
     Boost::system
     Boost::date_time
     Boost::regex
@@ -84,8 +88,10 @@ target_include_directories(rviz2
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
     $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>
     ${rclcpp_INCLUDE_DIRS}
+    ${OpenCV_INCLUDE_DIRS}
     ${visualization_msgs_INCLUDE_DIRS}
     ${geometry_msgs_INCLUDE_DIRS}
+    ${nav_msgs_INCLUDE_DIRS}
     ${rmf_schedule_visualizer_msgs_INCLUDE_DIRS}
     ${building_map_msgs_INCLUDE_DIRS}
 )

rmf_schedule_visualizer/package.xml

@@ -15,6 +15,7 @@
   <depend>rclcpp</depend>
   <depend>libwebsocketpp-dev</depend>
   <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
   <depend>visualization_msgs</depend>
   <depend>rmf_schedule_visualizer_msgs</depend>
   <depend>building_map_msgs</depend>
@@ -26,6 +27,7 @@
   <depend>rosidl_default_generators</depend>
   <depend>builtin_interfaces</depend>
   <depend>eigen</depend>
+  <depend>libopencv-dev</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

rmf_schedule_visualizer/src/main_rviz.cpp

@@ -36,17 +36,23 @@
 #include <geometry_msgs/msg/point.hpp>
 #include <std_msgs/msg/color_rgba.hpp>
 
+#include <opencv2/core/core.hpp>
+#include <opencv2/imgcodecs.hpp>
+
 #include <mutex>
 #include <unordered_map>
 #include <unordered_set>
 
+#include "nav_msgs/msg/occupancy_grid.hpp"
+
 using namespace std::chrono_literals;
 
 class RvizNode : public rclcpp::Node
 {
 public:
   using Marker = visualization_msgs::msg::Marker;
   using MarkerArray = visualization_msgs::msg::MarkerArray;
+  using OccupancyGrid = nav_msgs::msg::OccupancyGrid;
   using Point = geometry_msgs::msg::Point;
   using RequestParam = rmf_schedule_visualizer::RequestParam;
   using Element = rmf_traffic::schedule::Viewer::View::Element;
@@ -95,6 +101,11 @@ class RvizNode : public rclcpp::Node
       "map_markers",
       transient_qos_profile);
 
+    transient_qos_profile.transient_local();
+    _floorplan_pub = this->create_publisher<OccupancyGrid>(
+      "floorplan",
+      transient_qos_profile);
+
     // Create subscriber for rviz_param in separate thread
     _cb_group_param_sub = this->create_callback_group(
       rclcpp::callback_group::CallbackGroupType::MutuallyExclusive);
@@ -701,6 +712,7 @@ class RvizNode : public rclcpp::Node
           _level = level;
           RCLCPP_INFO(this->get_logger(), "Level cache updated");
           publish_map_markers();
+          publish_floorplan();
           break;
         }
       }
@@ -712,6 +724,46 @@ class RvizNode : public rclcpp::Node
     }
   }
 
+  void publish_floorplan()
+  {
+    if (_level.images.size() == 0)
+      return;
+
+    auto floorplan_img = _level.images[0]; // only use the first img
+    RCLCPP_INFO(this->get_logger(),
+      "Loading floorplan Image: " + floorplan_img.name +
+      floorplan_img.encoding);
+    cv::Mat img =
+      cv::imdecode(cv::Mat(floorplan_img.data), cv::IMREAD_GRAYSCALE);
+
+    OccupancyGrid floorplan_msg;
+    floorplan_msg.info.resolution = floorplan_img.scale;
+    floorplan_msg.header.frame_id = "map";
+    floorplan_msg.info.width = img.cols;
+    floorplan_msg.info.height = img.rows;
+    floorplan_msg.info.origin.position.x = floorplan_img.x_offset;
+    floorplan_msg.info.origin.position.y = floorplan_img.y_offset;
+    floorplan_msg.info.origin.position.z = -0.01;
+
+    Eigen::Quaternionf q = Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitX())
+      * Eigen::AngleAxisf(floorplan_img.yaw, Eigen::Vector3f::UnitZ());
+    floorplan_msg.info.origin.orientation.x = q.x();
+    floorplan_msg.info.origin.orientation.y = q.y();
+    floorplan_msg.info.origin.orientation.z = q.z();
+    floorplan_msg.info.origin.orientation.w = q.w();
+
+    std::vector<uint8_t> u_data(img.data, img.data + img.rows*img.cols);
+    std::vector<int8_t> occupancy_data;
+    for (auto pix : u_data)
+    {
+      auto pix_val = round((int)(0xFF - pix)/255.0*100);
+      occupancy_data.push_back(pix_val);
+    }
+
+    floorplan_msg.data = occupancy_data;
+    _floorplan_pub->publish(floorplan_msg);
+  }
+
   struct RvizParam
   {
     std::string map_name;
@@ -738,6 +790,7 @@ class RvizNode : public rclcpp::Node
   rclcpp::TimerBase::SharedPtr _timer;
   rclcpp::Publisher<MarkerArray>::SharedPtr _schedule_markers_pub;
   rclcpp::Publisher<MarkerArray>::SharedPtr _map_markers_pub;
+  rclcpp::Publisher<OccupancyGrid>::SharedPtr _floorplan_pub;
   rclcpp::Subscription<RvizParamMsg>::SharedPtr _param_sub;
   rclcpp::callback_group::CallbackGroup::SharedPtr _cb_group_param_sub;
   rclcpp::Subscription<BuildingMap>::SharedPtr _map_sub;