diff --git a/.github/workflows/build_eloquent.yaml b/.github/workflows/build_eloquent.yaml deleted file mode 100644 index 47a28a2..0000000 --- a/.github/workflows/build_eloquent.yaml +++ /dev/null @@ -1,42 +0,0 @@ -name: build_eloquent -on: [push, pull_request] -jobs: - build: - runs-on: ubuntu-18.04 - container: - image: docker://ros:eloquent-ros-base-bionic - - steps: - - name: deps - run: | - sudo apt update - sudo apt install -y wget - echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable bionic main" > /etc/apt/sources.list.d/gazebo-stable.list - wget https://packages.osrfoundation.org/gazebo.key -O - | apt-key add - - sudo apt update - sudo apt install python3-shapely python3-yaml python3-requests \ - libignition-common3-dev libignition-plugin-dev \ - g++-8 -y - - name: setup - run: | - 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 - - - name: checkout - uses: actions/checkout@v2 - with: - path: ws_rmf/src/rmf_schedule_visualizer - - - name: ros-deps - run: | - cd ws_rmf - rosdep update - rosdep install --from-paths src --ignore-src --rosdistro eloquent -yr - - name: build - shell: bash - run: | - cd ws_rmf - source /opt/ros/eloquent/setup.bash - CXX=g++-8 colcon build --cmake-args -DCMAKE_BUILD_TYPE=RELEASE --packages-up-to visualizer diff --git a/.github/workflows/build_foxy.yaml b/.github/workflows/build_foxy.yaml index 1923766..d54bbf6 100644 --- a/.github/workflows/build_foxy.yaml +++ b/.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 diff --git a/.github/workflows/style.yaml b/.github/workflows/style.yaml index f1c6c09..672d8ff 100644 --- a/.github/workflows/style.yaml +++ b/.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 diff --git a/README.md b/README.md index d526b18..d4bfc88 100644 --- a/README.md +++ b/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 ``` diff --git a/rmf_schedule_visualizer/CMakeLists.txt b/rmf_schedule_visualizer/CMakeLists.txt index c21479c..8a0c6c6 100644 --- a/rmf_schedule_visualizer/CMakeLists.txt +++ b/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 $ $ ${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} ) diff --git a/rmf_schedule_visualizer/package.xml b/rmf_schedule_visualizer/package.xml index 1b62665..6907d1f 100644 --- a/rmf_schedule_visualizer/package.xml +++ b/rmf_schedule_visualizer/package.xml @@ -15,6 +15,7 @@ rclcpp libwebsocketpp-dev geometry_msgs + nav_msgs visualization_msgs rmf_schedule_visualizer_msgs building_map_msgs @@ -26,6 +27,7 @@ rosidl_default_generators builtin_interfaces eigen + libopencv-dev ament_lint_auto ament_lint_common diff --git a/rmf_schedule_visualizer/src/main_rviz.cpp b/rmf_schedule_visualizer/src/main_rviz.cpp index 7e8278d..b136cbd 100644 --- a/rmf_schedule_visualizer/src/main_rviz.cpp +++ b/rmf_schedule_visualizer/src/main_rviz.cpp @@ -36,10 +36,15 @@ #include #include +#include +#include + #include #include #include +#include "nav_msgs/msg/occupancy_grid.hpp" + using namespace std::chrono_literals; class RvizNode : public rclcpp::Node @@ -47,6 +52,7 @@ 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( + "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 u_data(img.data, img.data + img.rows*img.cols); + std::vector 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::SharedPtr _schedule_markers_pub; rclcpp::Publisher::SharedPtr _map_markers_pub; + rclcpp::Publisher::SharedPtr _floorplan_pub; rclcpp::Subscription::SharedPtr _param_sub; rclcpp::callback_group::CallbackGroup::SharedPtr _cb_group_param_sub; rclcpp::Subscription::SharedPtr _map_sub;