diff --git a/.editorconfig b/.editorconfig new file mode 100644 index 000000000..c98ee52bb --- /dev/null +++ b/.editorconfig @@ -0,0 +1,17 @@ +# http://editorconfig.org +root = true + +[*] +indent_style = space +indent_size = 2 +charset = utf-8 +trim_trailing_whitespace = true +insert_final_newline = true +max_line_length = 100 + +# Use 4 spaces for the Python files +[*.py] +indent_size = 4 + +[*.md] +trim_trailing_whitespace = false diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml new file mode 100755 index 000000000..e25864c0b --- /dev/null +++ b/.github/workflows/main.yml @@ -0,0 +1,20 @@ +name: CI + +on: [push, pull_request] + +jobs: + industrial_ci: + strategy: + matrix: + env: + - {ROS_DISTRO: humble, ROS_REPO: testing} + - {ROS_DISTRO: humble, ROS_REPO: main} + - {ROS_DISTRO: iron, ROS_REPO: testing} + - {ROS_DISTRO: iron, ROS_REPO: main} + - {ROS_DISTRO: rolling, ROS_REPO: testing} + - {ROS_DISTRO: rolling, ROS_REPO: main} + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v1 + - uses: 'ros-industrial/industrial_ci@master' + env: ${{matrix.env}} diff --git a/.ros2_build.sh b/.ros2_build.sh new file mode 100755 index 000000000..3ba41d07b --- /dev/null +++ b/.ros2_build.sh @@ -0,0 +1,15 @@ +#!/usr/bin/env bash + +# Enable the ros2-testing repository +sed 's#ros2#ros2-testing#' /etc/apt/sources.list.d/ros2-latest.list | tee >> /etc/apt/sources.list.d/ros2-latest.list + +# Install colcon, we need it and it's not installed by default +apt-get update +rosdep update +apt-get install -y build-essential python3-colcon-ros + +cd /ws + +# Use rosdep to install the rest of the package's dependencies, then build it +rosdep install src --from-paths -i -y +colcon build diff --git a/.travis.yml b/.travis.yml new file mode 100644 index 000000000..3131dcd3f --- /dev/null +++ b/.travis.yml @@ -0,0 +1,15 @@ +language: generic + +services: + - docker + +jobs: + include: + - stage: compile + name: 'ROS Dashing' + script: + - exec docker run -a STDOUT -a STDERR --rm -v $(pwd):/ws/src/mapviz ros:dashing /ws/src/mapviz/.ros2_build.sh + - stage: compile + name: 'ROS Eloquent' + script: + - exec docker run -a STDOUT -a STDERR --rm -v $(pwd):/ws/src/mapviz ros:eloquent /ws/src/mapviz/.ros2_build.sh diff --git a/LICENSE b/LICENSE new file mode 100644 index 000000000..602b49878 --- /dev/null +++ b/LICENSE @@ -0,0 +1,24 @@ +Copyright (c) 2014, Southwest Research Institute® (SwRI®) +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of Southwest Research Institute® (SwRI®) nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/README.md b/README.md new file mode 100644 index 000000000..ca297968f --- /dev/null +++ b/README.md @@ -0,0 +1,11 @@ +Mapviz [![Build Status](https://travis-ci.org/swri-robotics/mapviz.svg?branch=master)](https://travis-ci.org/swri-robotics/mapviz) +====== + +Mapviz is a [ROS](http://www.ros.org/) based visualization tool with a plug-in system similar to [RVIZ](http://wiki.ros.org/rviz) focused on visualizing 2D data. + +![](https://github.com/swri-robotics/mapviz/wiki/mapviz.png) + +Usage +----- + +[View the documentation](https://swri-robotics.github.io/mapviz/) for usage information. diff --git a/docs/Gemfile b/docs/Gemfile new file mode 100644 index 000000000..709bb019b --- /dev/null +++ b/docs/Gemfile @@ -0,0 +1,3 @@ +source 'https://rubygems.org' +gem 'github-pages', group: :jekyll_plugins +gem 'just-the-docs' diff --git a/docs/_config.yml b/docs/_config.yml new file mode 100644 index 000000000..a20d390a7 --- /dev/null +++ b/docs/_config.yml @@ -0,0 +1,60 @@ +title: "Mapviz" +description: Modular ROS visualization tool for 2D data +baseurl: "/mapviz" # the subpath of your site, e.g. /blog +url: "https://swri-robotics.github.io" # the base hostname & protocol for your site, e.g. http://example.com +remote_theme: pmarsceill/just-the-docs + +permalink: /:title/ +exclude: ["node_modules/", "*.gemspec", "*.gem", "Gemfile", "Gemfile.lock", "package.json", "package-lock.json", "script/", "LICENSE.txt", "lib/", "bin/", "README.md", "Rakefile"] + +# Set a path/url to a logo that will be displayed instead of the title +#logo: "/assets/images/just-the-docs.png" + +# Enable or disable the site search +search_enabled: true + +# Set the search token separator for hyphenated-word search: +search_tokenizer_separator: /[\s/]+/ + +# Enable or disable heading anchors +heading_anchors: true + +# Aux links for the upper right navigation +aux_links: + "Mapviz on GitHub": + - "//github.com/swri-robotics/mapviz" + +# Footer content appears at the bottom of every page's main content +footer_content: "Copyright © 2020 Southwest Research Institute. Distributed under the BSD 3-Clause license." + +# Color scheme currently only supports "dark" or nil (default) +color_scheme: nil + +# Google Analytics Tracking (optional) +# e.g, UA-1234567-89 +# ga_tracking: + +plugins: + - jekyll-seo-tag + +collections: + plugins: + output: true + +defaults: + - scope: + path: '' + type: 'plugins' + values: + layout: 'plugin' + parent: 'Plugins' + +compress_html: + clippings: all + comments: all + endings: all + startings: [] + blanklines: false + profile: false + +theme: jekyll-theme-cayman \ No newline at end of file diff --git a/docs/_layouts/plugin.md b/docs/_layouts/plugin.md new file mode 100644 index 000000000..8ab648287 --- /dev/null +++ b/docs/_layouts/plugin.md @@ -0,0 +1,27 @@ +--- +layout: default +--- + +{% capture content %} + +# {{ page.title }} + +{{ page.description }} + +{% if page.image and page.image != "" %} +![]({{ site.baseurl | append: '/assets/images/' }}{{ page.image }}) +{% endif %} + +## Parameters + +{% if page.parameters %} + {% for param in page.parameters %} +| {{ param.name }} | {{ param.description | replace: "|","/"}} | + {%- endfor -%} +{% else %} + No parameters. +{% endif %} + +{% endcapture %} + +{{ content | markdownify }} diff --git a/docs/_plugins/attitude_indicator.md b/docs/_plugins/attitude_indicator.md new file mode 100644 index 000000000..f692780fa --- /dev/null +++ b/docs/_plugins/attitude_indicator.md @@ -0,0 +1,6 @@ +--- +title: "Attitude Indicator" +description: "" +image: "" +parameters: +--- diff --git a/docs/_plugins/coordinate_picker.md b/docs/_plugins/coordinate_picker.md new file mode 100644 index 000000000..62972fa2c --- /dev/null +++ b/docs/_plugins/coordinate_picker.md @@ -0,0 +1,8 @@ +--- +title: "Coordinate Picker" +description: "Transforms coordinates of clicked points on the map to a specified frame. The most recent coordinate is placed on the clipboard, and a list of coordinates is displayed in the GUI." +image: "screenshot_coordinate_picker.png" +parameters: + - name: frame + description: "Coordinate frame into which to transform the clicked point" +--- diff --git a/docs/_plugins/disparity.md b/docs/_plugins/disparity.md new file mode 100644 index 000000000..bdbfda3bc --- /dev/null +++ b/docs/_plugins/disparity.md @@ -0,0 +1,20 @@ +--- +title: "Disparity" +description: "Overlays a [sensor_msgs::DisparityImage](http://docs.ros.org/api/stereo_msgs/html/msg/DisparityImage.html) onto the display using the ''jet'' color map." +image: "disparity.png" +parameters: + - name: Topic + description: The disparity topic name + - name: Anchor + description: (top left | top center | top right | center left | center | center right | bottom left | bottom center | bottom right) + - name: Offset X + description: Display offset from the left + - name: Offset Y + description: Display offset from the top + - name: Width + description: Display width + - name: Height + description: Display height + - name: Units + description: (pixels | percent of window) +--- diff --git a/docs/_plugins/draw_polygon.md b/docs/_plugins/draw_polygon.md new file mode 100644 index 000000000..e5e9ba18c --- /dev/null +++ b/docs/_plugins/draw_polygon.md @@ -0,0 +1,6 @@ +--- +title: "Draw Polygon" +description: "Draw a polygon on the canvas and publish to a topic." +image: "" +parameters: +--- diff --git a/docs/_plugins/float.md b/docs/_plugins/float.md new file mode 100644 index 000000000..29e0b483d --- /dev/null +++ b/docs/_plugins/float.md @@ -0,0 +1,22 @@ +--- +title: "Float" +description: "Displays the most recent value from a `std_msgs::Float32/64, marti_common_msgs/Float32/64Stamped` or a `marti_sensor_msgs/Velocity` message at a fixed location on the scene." +image: "" +parameters: + - name: "Topic" + description: "The float topic" + - name: "Font" + description: "The font for rendering the float" + - name: "Color" + description: "The color for drawing the float" + - name: "Anchor" + description: "(top left | top center | top right | center left | center | center right | bottom left | bottom center | bottom right)" + - name: "Offset X" + description: "Horizontal offset from the anchor" + - name: "Offset Y" + description: "Vertical offset from the anchor" + - name: "Units" + description: "(pixels | percent of window)" + - name: "Postfix" + description: "Text to append to the displayed value (ex. to show units)" +--- diff --git a/docs/_plugins/gps.md b/docs/_plugins/gps.md new file mode 100644 index 000000000..108557352 --- /dev/null +++ b/docs/_plugins/gps.md @@ -0,0 +1,20 @@ +--- +title: "GPS" +description: "Projects [gps_common::GPSFix](http://docs.ros.org/kinetic/api/gps_common/html/msg/GPSFix.html) message data into the scene." +image: "" +parameters: + - name: Topic + description: The GPS topic + - name: Color + description: The color of the GPS data + - name: Draw Style + description: (lines | points | arrows) + - name: Static Arrow Sizes + description: If checked, draw arrows the same size regardless of zoom level; slider adjusts size + - name: Position Tolerance + description: Distance threshold for adding new GPS points to visualization + - name: Buffer Size + description: Size of circular buffer of GPS points + - name: Show Laps + description: If checked, multiple loops of GPS coordinates will have different colors +--- diff --git a/docs/_plugins/grid.md b/docs/_plugins/grid.md new file mode 100644 index 000000000..c77a0f042 --- /dev/null +++ b/docs/_plugins/grid.md @@ -0,0 +1,22 @@ +--- +title: "Grid" +description: "Projects a 2D grid into the scene." +image: "" +parameters: + - name: Frame + description: Coordinate frame of the grid + - name: Color + description: Color of the grid + - name: Alpha + description: Alpha transparency of the grid + - name: X + description: X offset of the grid from the specified coordinate frame origin + - name: Y + description: Y offset of the grid from the specified coordinate frame origin + - name: Size + description: Size of each grid cell + - name: Rows + description: Number of grid rows + - name: Columns + description: Number of grid columns +--- diff --git a/docs/_plugins/image.md b/docs/_plugins/image.md new file mode 100644 index 000000000..9d13b841b --- /dev/null +++ b/docs/_plugins/image.md @@ -0,0 +1,20 @@ +--- +title: "Image" +description: "Overlays a [sensor_msgs::Image](http://docs.ros.org/api/sensor_msgs/html/msg/Image.html) onto the display." +image: "" +parameters: + - name: Topic + description: The image topic name + - name: Anchor + description: (top left | top center | top right | center left | center | center right | bottom left | bottom center | bottom right) + - name: Offset X + description: Display offset from the left + - name: Offset Y + description: Display offset from the top + - name: Width + description: Display width + - name: Height + description: Display height + - name: Units + description: (pixels | percent of window) +--- diff --git a/docs/_plugins/laserscan.md b/docs/_plugins/laserscan.md new file mode 100644 index 000000000..ce6f448c1 --- /dev/null +++ b/docs/_plugins/laserscan.md @@ -0,0 +1,20 @@ +--- +title: "LaserScan" +description: "Projects a [sensor_msgs::LaserScan](http://docs.ros.org/api/sensor_msgs/html/msg/LaserScan.html) message into the scene." +image: "" +parameters: + - name: "Topic" + description: "The laser scan topic name" + - name: "Min Color" + description: "The color associated with minimum return intensity" + - name: "Max Color" + description: "The color associated with maximum return intensity" + - name: "Min Intesity" + description: "Minimum intensity value" + - name: "Max Intensity" + description: "Maximum intensity value" + - name: "Point Size" + description: "Display size of laser scan points in pixels" + - name: "Buffer Size" + description: "Size of circular buffer of laser scan messages points" +--- diff --git a/docs/_plugins/marker.md b/docs/_plugins/marker.md new file mode 100644 index 000000000..d680b3530 --- /dev/null +++ b/docs/_plugins/marker.md @@ -0,0 +1,10 @@ +--- +title: "Marker" +description: "Projects a [visualization_msgs::Marker](http://docs.ros.org/api/visualization_msgs/html/msg/Marker.html) or [visualization_msgs::MarkerArray](http://docs.ros.org/api/visualization_msgs/html/msg/MarkerArray.html) into the scene. + +[Markers](http://wiki.ros.org/rviz/DisplayTypes/Marker) are the most flexible display type and more or less mirror the [OpenGL primitives](https://www.opengl.org/wiki/Primitive)." +image: "" +parameters: + - name: "Topic" + description: "The marker topic" +--- diff --git a/docs/_plugins/measuring.md b/docs/_plugins/measuring.md new file mode 100644 index 000000000..56714f464 --- /dev/null +++ b/docs/_plugins/measuring.md @@ -0,0 +1,6 @@ +--- +title: "Measuring" +description: "Measure distance on the canvas with the mouse." +image: "" +parameters: +--- diff --git a/docs/_plugins/move_base.md b/docs/_plugins/move_base.md new file mode 100644 index 000000000..1ee62a8ec --- /dev/null +++ b/docs/_plugins/move_base.md @@ -0,0 +1,6 @@ +--- +title: "Move Base" +description: "Allows the user to send goals to [move_base](wiki.ros.org/move_base)." +image: "" +parameters: +--- diff --git a/docs/_plugins/multires_image.md b/docs/_plugins/multires_image.md new file mode 100644 index 000000000..daba699fe --- /dev/null +++ b/docs/_plugins/multires_image.md @@ -0,0 +1,8 @@ +--- +title: "Multi-Res Image" +description: "Projects a geo-referenced multi-resolution image tile map into the scene. The concept is the same as the Google Maps style pan/zoom satellite imagery." +image: "multires2.png" +parameters: + - name: "Geo File" + description: "Path to the geo-referenced map tiles." +--- diff --git a/docs/_plugins/navsat.md b/docs/_plugins/navsat.md new file mode 100644 index 000000000..0d5eb3f99 --- /dev/null +++ b/docs/_plugins/navsat.md @@ -0,0 +1,16 @@ +--- +title: "NavSat" +description: "Projects [sensor_msgs::NavSatFix](https://docs.ros.org/jade/api/sensor_msgs/html/msg/NavSatFix.html) message data into the scene." +image: "" +parameters: + - name: "Topic" + description: "The GPS topic" + - name: "Color" + description: "The color of the GPS data" + - name: "Draw Style" + description: "(lines | points)" + - name: "Position Tolerance" + description: "Distance threshold for adding new GPS points to visualization" + - name: "Buffer Size" + description: "Size of circular buffer of GPS points" +--- diff --git a/docs/_plugins/occupancy_grid.md b/docs/_plugins/occupancy_grid.md new file mode 100644 index 000000000..9a2b0b3ba --- /dev/null +++ b/docs/_plugins/occupancy_grid.md @@ -0,0 +1,6 @@ +--- +title: "Occupancy Grid" +description: "" +image: "" +parameters: +--- diff --git a/docs/_plugins/odometry.md b/docs/_plugins/odometry.md new file mode 100644 index 000000000..fd2937e75 --- /dev/null +++ b/docs/_plugins/odometry.md @@ -0,0 +1,18 @@ +--- +title: "Odometry" +description: "Projects [nav_msgs::Odometry](http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html) message data into the scene." +image: "" +parameters: + - name: "Topic" + description: "The odometry topic" + - name: "Color" + description: "The color of the odometry data" + - name: "Draw Style" + description: "(lines | points | arrows)" + - name: "Show Covariance" + description: "Draw covariance ellipse around latest data" + - name: "Position Tolerance" + description: "Distance threshold for adding new odometry points to visualization" + - name: "Buffer Size" + description: "Size of circular buffer of odometry points" +--- diff --git a/docs/_plugins/path.md b/docs/_plugins/path.md new file mode 100644 index 000000000..2667def29 --- /dev/null +++ b/docs/_plugins/path.md @@ -0,0 +1,8 @@ +--- +title: "Path" +description: "Projects [nav_msgs::Path](http://docs.ros.org/api/nav_msgs/html/msg/Path.html) message data into the scene." +image: "" +parameters: + - name: "Topic" + description: "The path topic" +--- diff --git a/docs/_plugins/plan_route.md b/docs/_plugins/plan_route.md new file mode 100644 index 000000000..58b929d45 --- /dev/null +++ b/docs/_plugins/plan_route.md @@ -0,0 +1,6 @@ +--- +title: "Plan Route" +description: "" +image: "" +parameters: +--- diff --git a/docs/_plugins/point_click_publisher.md b/docs/_plugins/point_click_publisher.md new file mode 100644 index 000000000..9398787d1 --- /dev/null +++ b/docs/_plugins/point_click_publisher.md @@ -0,0 +1,10 @@ +--- +title: "Point Click Publisher" +description: "Publishes a [geometry_msgs::PointStamped](http://docs.ros.org/api/geometry_msgs/html/msg/PointStamped.html) message every time a user clicks on the map frame that corresponds to the clicked location." +image: "" +parameters: + - name: "Topic" + description: "The topic to publish the point to" + - name: "Frame" + description: "The target frame to transform the point to before publishing it" +--- diff --git a/docs/_plugins/point_drawing.md b/docs/_plugins/point_drawing.md new file mode 100644 index 000000000..9d49410ce --- /dev/null +++ b/docs/_plugins/point_drawing.md @@ -0,0 +1,6 @@ +--- +title: "Point Drawing" +description: "" +image: "" +parameters: +--- diff --git a/docs/_plugins/pointcloud2.md b/docs/_plugins/pointcloud2.md new file mode 100644 index 000000000..1e3e431db --- /dev/null +++ b/docs/_plugins/pointcloud2.md @@ -0,0 +1,6 @@ +--- +title: "Pointcloud2" +description: "" +image: "" +parameters: +--- diff --git a/docs/_plugins/pose.md b/docs/_plugins/pose.md new file mode 100644 index 000000000..fe51cc22d --- /dev/null +++ b/docs/_plugins/pose.md @@ -0,0 +1,6 @@ +--- +title: "Pose" +description: "" +image: "" +parameters: +--- diff --git a/docs/_plugins/robot_image.md b/docs/_plugins/robot_image.md new file mode 100644 index 000000000..705ca6172 --- /dev/null +++ b/docs/_plugins/robot_image.md @@ -0,0 +1,14 @@ +--- +title: "Robot Image" +description: "Projects an image loaded from file into the scene to represent the robot platform." +image: "" +parameters: + - name: "Image File" + description: " Path to the image file" + - name: "Frame" + description: "Frame to tie the image to" + - name: "Width" + description: "The physical width represented by the image" + - name: "Height" + description: "The physical height represented by the image" +--- diff --git a/docs/_plugins/route.md b/docs/_plugins/route.md new file mode 100644 index 000000000..d90101a7b --- /dev/null +++ b/docs/_plugins/route.md @@ -0,0 +1,6 @@ +--- +title: "Route" +description: "" +image: "" +parameters: +--- diff --git a/docs/_plugins/string.md b/docs/_plugins/string.md new file mode 100644 index 000000000..da91b5298 --- /dev/null +++ b/docs/_plugins/string.md @@ -0,0 +1,20 @@ +--- +title: "String" +description: "Displays the most recent string from a `std_msgs::String` message at a fixed location on the scene." +image: "" +parameters: + - name: "Topic" + description: "The string topic" + - name: "Font" + description: "The font for rendering the string" + - name: "Color" + description: "The color for drawing the string" + - name: "Anchor" + description: "(top left | top center | top right | center left | center | center right | bottom left | bottom center | bottom right)" + - name: "Offset X" + description: "Horizontal offset from the anchor" + - name: "Offset Y" + description: "Vertical offset from the anchor" + - name: "Units" + description: "(pixels | percent of window)" +--- diff --git a/docs/_plugins/textured_marker.md b/docs/_plugins/textured_marker.md new file mode 100644 index 000000000..d25a76caf --- /dev/null +++ b/docs/_plugins/textured_marker.md @@ -0,0 +1,10 @@ +--- +title: "Textured Marker" +description: "Projects `marti_visualization_msgs::TexturedMarker` and `marti_visualization_msgs::TexturedMarkerArray` message data into the scene. + +Textured markers follow the same general approach as traditional markers, but can be used to texture dense image data onto a quad which is projected into the scene." +image: "" +parameters: + - name: "Topic" + description: "The textured marker topic" +--- diff --git a/docs/_plugins/tf_frame.md b/docs/_plugins/tf_frame.md new file mode 100644 index 000000000..6a6c989aa --- /dev/null +++ b/docs/_plugins/tf_frame.md @@ -0,0 +1,16 @@ +--- +title: "TF Frame" +description: "Projects [Tf](http://wiki.ros.org/tf) data into the scene similar to the Odometry plug-in." +image: "" +parameters: + - name: "Frame" + description: "The Tf frame" + - name: "Color" + description: "The color of the Tf data" + - name: "Draw Style" + description: "(lines | points | arrows)" + - name: "Position Tolerance" + description: "Distance threshold for adding new Tf points to visualization" + - name: "Buffer Size" + description: "Size of circular buffer of Tf points" +--- diff --git a/docs/_plugins/tile_map.md b/docs/_plugins/tile_map.md new file mode 100644 index 000000000..ff2394848 --- /dev/null +++ b/docs/_plugins/tile_map.md @@ -0,0 +1,14 @@ +--- +title: "Tile Map" +description: "Projects a geo-referenced multi-resolution image tile map into the scene. Map tiles can be obtained from [Bing Maps](https://www.bing.com/mapspreview) or any [WMTS Tile Service](http://www.opengeospatial.org/standards/wmts). Pre-defined services that access [Stamen Design](http://maps.stamen.com/) (terrain, watercolor, and toner) are provided. Custom or local WMTS map servers can also be specified. Map data is cached to disk which enables some limited use completely offline." +image: "satellite.png" +parameters: + - name: "Source" + description: "The name of source of the tile data." + - name: "Base URL" + description: "A template URL used to obtain map tiles. When obtaining map tiles, parameters labeled `{level}`, `{x}`, and `{y}` in the URL will be replaced with appropriate values. For example, `http://tile.stamen.com/terrain/{level}/{x}/{y}.png` is appropriate for retrieving terrain tiles from Stamen Design." + - name: "API Key" + description: "When the `Bing Maps (terrain)` source is selected, you must enter a Bing Maps access key here and click the `Save` button in order for tiles to be available. You can get a Bing Maps Key from the [Microsoft Developer Network](https://msdn.microsoft.com/en-us/library/ff428642.aspx)." + - name: "Max Zoom" + description: "The maximum zoom level that will be used when requesting tiles." +--- diff --git a/docs/assets/images/disparity.png b/docs/assets/images/disparity.png new file mode 100644 index 000000000..c526b89fc Binary files /dev/null and b/docs/assets/images/disparity.png differ diff --git a/docs/assets/images/mapviz.png b/docs/assets/images/mapviz.png new file mode 100644 index 000000000..b2a1c897b Binary files /dev/null and b/docs/assets/images/mapviz.png differ diff --git a/docs/assets/images/mapviz_features.png b/docs/assets/images/mapviz_features.png new file mode 100644 index 000000000..78a7ac60d Binary files /dev/null and b/docs/assets/images/mapviz_features.png differ diff --git a/docs/assets/images/multires.png b/docs/assets/images/multires.png new file mode 100644 index 000000000..2afae9f6b Binary files /dev/null and b/docs/assets/images/multires.png differ diff --git a/docs/assets/images/multires2.png b/docs/assets/images/multires2.png new file mode 100644 index 000000000..31f3c8531 Binary files /dev/null and b/docs/assets/images/multires2.png differ diff --git a/docs/assets/images/roads.png b/docs/assets/images/roads.png new file mode 100644 index 000000000..af99a1b28 Binary files /dev/null and b/docs/assets/images/roads.png differ diff --git a/docs/assets/images/satellite.png b/docs/assets/images/satellite.png new file mode 100644 index 000000000..47ce663b9 Binary files /dev/null and b/docs/assets/images/satellite.png differ diff --git a/docs/assets/images/screenshot_coordinate_picker.png b/docs/assets/images/screenshot_coordinate_picker.png new file mode 100644 index 000000000..fa3d37b5c Binary files /dev/null and b/docs/assets/images/screenshot_coordinate_picker.png differ diff --git a/docs/assets/images/terrain.png b/docs/assets/images/terrain.png new file mode 100644 index 000000000..f4dddab2f Binary files /dev/null and b/docs/assets/images/terrain.png differ diff --git a/docs/assets/images/toner.png b/docs/assets/images/toner.png new file mode 100644 index 000000000..41c30f726 Binary files /dev/null and b/docs/assets/images/toner.png differ diff --git a/docs/assets/images/watercolor.png b/docs/assets/images/watercolor.png new file mode 100644 index 000000000..5cf747d0f Binary files /dev/null and b/docs/assets/images/watercolor.png differ diff --git a/docs/assets/js/search-data.json b/docs/assets/js/search-data.json new file mode 100644 index 000000000..8e1b98639 --- /dev/null +++ b/docs/assets/js/search-data.json @@ -0,0 +1,12 @@ +--- +--- +{ + {% assign comma = false %} + {% for page in site.html_pages %}{% if page.search_exclude != true %}{% if comma == true%},{% endif %}"{{ forloop.index0 }}": { + "title": "{{ page.title | replace: '&', '&' }}", + "content": "{{ page.content | markdownify | replace: ' + +Plugin Name +Description + +{% for plugin in site.plugins %} + +{{ plugin.title | markdownify | remove: '

' | remove: '

' }} +{{ plugin.description | markdownify | remove: '

' | remove: '

' }} + +{%- comment -%} +{%- if plugin.image and plugin.image != "" -%} +![]({{ site.baseurl | append: '/assets/images/' }}{{ plugin.image }}) +{%- endif -%} +{%- endcomment -%} +{%- endfor -%} + diff --git a/mapviz/CHANGELOG.rst b/mapviz/CHANGELOG.rst new file mode 100644 index 000000000..df62cd39c --- /dev/null +++ b/mapviz/CHANGELOG.rst @@ -0,0 +1,306 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package mapviz +^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.3.0 (2023-08-24) +------------------ +* Updates for rolling and removing EOL distro support (`#785 `_) +* Contributors: David Anthony + +2.2.2 (2023-06-07) +------------------ +* Add ros_environment as dependency +* Iron Compatibility (`#779 `_) +* Contributors: David Anthony + +2.2.1 (2023-05-30) +------------------ +* Updating maintainers list (`#778 `_) +* Contributors: David Anthony + +2.1.0 (2020-10-22) +------------------ +* ROS Foxy support (`#695 `_) +* Contributors: P. J. Reed + +2.0.0 (2020-05-13) +------------------ +* Port mapviz to ROS 2 (`#672 `_) +* Remove OpenGL warning (`#667 `_) +* Resolves segfault issue on ClearHistory() function call (`#651 `_) +* Fix plug-in config panel to scroll per-pixel instead of per-item, allowing configuration of plug-ins that have config settings that are too large to fit in the panel all at once. (`#646 `_) +* Contributors: Daniel D'Souza, Marc Alban, Matthew Bries, Matthew Grogan, P. J. Reed, Jason Gassaway, John Reyes, Jacob Hassold, Kevin Nickels, Roger Strain + +1.2.0 (2019-09-04) +------------------ + +1.1.1 (2019-05-17) +------------------ +* Set main window as in focus on start `#630 `_ +* Specify default configuration extension on save +* Contributors: Daniel D'Souza, mattrich37, mrichardson + +1.1.0 (2019-02-20) +------------------ + +1.0.1 (2019-01-25) +------------------ + +1.0.0 (2019-01-23) +------------------ +* Sharing tf_manager\_ between main app and plugins (`#555 `_) +* Contributors: Davide Faconti + +0.3.0 (2018-11-16) +------------------ +* Merge all -devel branches into a single master branch +* Add function to lock canvas movement (`#596 `_) +* Contributors: P. J. Reed + +0.2.6 (2018-07-31) +------------------ + +0.2.5 (2018-04-12) +------------------ +* Add clear history functionality. +* New plugin to send commands to move_base +* improve text contrast (`#550 `_) +* Glew warning fixed (`#539 `_) +* remove copy and paste of Print... +* add context menu to config_item (`#526 `_) +* Merge pull request `#523 `_ from matt-attack/add-keyboard-input-support-kinetic +* Add keyboard input support for plugins +* update to use non deprecated pluginlib macro +* Fix the "File" menu actions (`#513 `_) +* Merge pull request `#481 `_ from pjreed/threaded-video-recording-kinetic +* Move video recording into its own thread +* Contributors: Davide Faconti, Marc Alban, Matthew Bries, Mikael Arguedas, P. J. Reed + +0.2.4 (2017-08-11) +------------------ +* Add basic profiling to mapviz. +* Handle GL canvas transforms with an invalid target frame +* Add parameter to disable auto-saving functionality. +* Contributors: Elliot Johnson, Marc Alban, P. J. Reed + +0.2.3 (2016-12-10) +------------------ +* Fix mapviz kinetic build. (`#456 `_) + Add a missing rosdep dependency on libxi-dev. +* Contributors: Edward Venator + +0.2.2 (2016-12-07) +------------------ +* Migrated OpenCV to 3.1 (default in Kinetic) +* Contributors: Brian Holt + +0.2.1 (2016-10-23) +------------------ +* Add a GUI for controlling the Image Transport (`#432 `_) + This will add a sub-menu under the "View" menu that will: + - List all available image transports + - Indicate which one is currently the default + - Allow the user to choose which one will be used for new ImageTransport subscriptions + - Save and restore this setting to Mapviz's config file + - Cause any `image` plugins using the default transport to resubscribe + In addition, the image plugin now has a menu that can be used to change the + transport for that specific plugin so that it is different from the default. + Fixes `#430 `_ + Conflicts: + mapviz/package.xml +* Fix icon colors for point drawing plugins (`#433 `_) + This was probably broken back when all of these were refactored to have a + single base class. It looks like the member variable that holds the color + used to draw the icon was never actually being updated. + Fixes `#426 `_ +* Remove unnecessary include +* Fix warnings in mapviz. + Fix several reorder and signed comparison warnings in the mapviz + package. +* Giving `tile_map` an interface overhaul + MapQuest has turned off their public API for map tiles, so this plugin needed some work. I have: + - Removed the MapQuest sources + - Made the interface for adding new sources more powerful + - Overhauled how sources are saved and loaded under the hood + - Added a button to reset the current tile cache + Resolves `#402 `_ + Conflicts: + tile_map/CMakeLists.txt +* Adding a dialog for selecting services by type + This dialog is similar to the ones for listing topics or TF frames, but it is + a little different under the hood. Notably: + - It relies on the rosapi node in order to be able to search for services + - Since searching is done via a service call, ROS communication is handled + on a separate thread that will not block the GUI + - Unlike topics, only searching for a single service type is supported + Conflicts: + mapviz/package.xml +* Adding a way for plugin config widgets to resize + - Adding an event plugins can emit to indicate their geometry has changed + - Modifying the PCL2 plugin to use it as an example + Fixes `#393 `_ +* Adding a button to reset the location and zoom level + This adds an icon on the right side of Mapviz's status bar tthat will reset + the view to the default zoom level and center it on the origin of the target + frame. + Resolves `#371 `_ +* Contributors: Ed Venator, Marc Alban, P. J. Reed + +0.2.0 (2016-06-23) +------------------ +* Update mapviz to qt5 +* Adding a Q_OBJECT declaration to MapvizPlugin +* Adding signals for various plugin events + The MapvizPlugin class will now emit signals when any of the following settings change: + - Draw Order + - Target Frame + - Use Latest Transforms + - Visibility + Note that the signals will only be emitted if the setting actually *changes*, not + if it is somehow set to the same value that it was previously. +* Contributors: Ed Venator, P. J. Reed + +0.1.3 (2016-05-20) +------------------ +* Implement mapviz plug-in for calling the marti_nav_msgs::PlanRoute service. +* Adding an explicit dependency on pkg-config to package.xml (`#355 `_) +* Add _gencpp dependency to mapviz targets. + This commit adds the _gencpp target to mapviz targets to ensure that + the AddMapvizDisplay service is built before the targets. +* Make compiler flags specific to each target. +* Implement service for adding and modifying mapviz displays. +* Fix for `#339 `_; explicitly depending on OpenCV 2 +* Fix for `#336 `_; Qt event handler exceptions shouldn't crash Mapviz +* Fixing blending for GL drawing + The call to QGLWidget::beginNativePainting has a side effect of clearing + GL settings related to blending and depth testing, and that was causing + alpha transparency to not work right for plugins. I fixed it by manually + re-enabling those settings every time beginNativePainting is called. +* Fix for `#319 `_ + Previously, the MapCanvas::MapGlCoordToFixedFrame function relied on + the transform\_ member variable being set, but it is not set if the + target frame is . Instead it will now use the qtransform\_ + variable, which is always initialized for the purpose of QPainters. +* Saving & restoring all matrices and attribs +* Moving QPainter drawing back to being after GL + I had switched the order while debugging things and forgot to set it + back to the way it originally was. +* Removing a leftover debug print +* Fixing `#317 `_ + First, the model view matrix needs to be saved and restored around + QPainter operations because Qt clears several GL variables. Also, the + image plugin needed to explicitly call glMatrixMode(GL_PROJECTION); + it does a few operations on the projection matrix and was just assuming + that was the current matrix mode. Also, I added a function that plugins + need to override if they want to do QPainter operations; this will + eliminate unnecessary overhead for plugins that do not. +* Removing extraneous calls to MapCanvas::update() + Now that update() is being called automatically at a rate of 50 Hz, + the explicit calls in many locations are unnecessary. It was also + possible for it to be called in some of these locations from a + non-main thread, which is invalid and could cause crashes. +* Adding the ability to toggle anti-aliasing + Now there's a checkbox under the "View" menu that will toggle whether + anti-aliasing is applied to the canvas. In some situations this will + make the display look much prettier at a slight performance cost. +* Cleaning up documentation. +* Merging QPainter/anti-aliasing fixes into jade-devel + This is the same as the old version of this change, except updated + to the most recent version of Mapviz. +* Fixing a compile error +* Fix for `#298 `_; right-click + drag will now zoom +* Update map canvas at a fixed rate. + This update adds a timer to the map canvas to repaint at a fixed rate. + The default rate is 50 Hz, but there is a method to change it (not + exposed to the UI at the moment). 50Hz was chosen because it is fast + enough to give smooth animations and we almost always are running + mapviz with at least one plugin triggering updates from a 50Hz topic. +* Update mapviz.launch file to also launch anonymously. +* Initialize mapviz as an anonymous node. +* This commit adds a class called SelectFrameDialog that plugins can use + to present the user with a dialog to choose a TF frame. The dialog + sorts the frames by name and provides an edit box that the user can + use to filter the frames to a specific substring. +* Fixing an issue that could cause the click publisher plugin's publisher to not be initialized after it's first added. +* Adding a plugin that, when a user clicks on a point, will publish that point's coordinates to a topic. +* Adding color button widget and updating plugins. + This commit adds a subclass of QPushButton called ColorButton that + implements a widget for displaying and selecting colors. We've been + doing this manually everywhere with duplicated code. This is a simple + abstraction but allows us to elminate a lot of duplication, especially + in plugins that have multiple color selections. +* Remove debugging messages from SelectFrameDialog. + These were accidentally left in during initial development. +* Add documentation for the SelectTopicDialog. +* Adds SelecTopicDialog to mapviz. + This commit adds the SelectTopicDialog that can be used in plugins to + provide the user with a dialog to select topics. Typically we have + done this with a lot of duplicated code across all the plugins. This + commit also updates the plugins in mapviz_plugins to use the new + dialog. + The new dialog provides several benefits: + - Reduced code duplication + - Simplifies writing new plugins + - Common behavior between all plugins + - Topics sorted by name + - User can filter topics by substring + - Continuously checks the master for new topics while the dialog is open. +* Contributors: Elliot Johnson, Marc Alban, P. J. Reed + +0.1.2 (2016-01-06) +------------------ +* Show full path when recording screenshots/movies. +* Fixes a bug in plug-in sorting. +* Sorts topic, plug-in, and frame lists in selection dialogs. +* Contributors: Elliot Johnson, Marc Alban + +0.1.1 (2015-11-17) +------------------ +* Fixes mapviz launch file frame param +* Marks single argument constructors explicit. +* Contributors: Edward Venator, Marc Alban, Vincent Rousseau + +0.1.0 (2015-09-29) +------------------ + +0.0.3 (2015-09-28) +------------------ +* Fixing casting issues that prevented compilation on 32-bit systems. +* Contributors: P. J. Reed + +0.0.2 (2015-09-27) +------------------ +* Adds missing qt4_opengl dependency + +0.0.1 (2015-09-27) +------------------ +* Renames all marti_common packages that were renamed. + (See http://github.com/swri-robotics/marti_common/issues/231) +* Adds missing dependencies in mapviz package.xml. +* Fixes catkin_lint problems that could prevent installation. +* Formats package files +* Cleans up dependencies +* Adds required rosdeps +* Saving/loading config files to the ROS_WORKSPACE directory. +* fixes lint issues +* Makes mapviz show a warning dialog instead of crash when it fails to load a plugin. +* includes yaml_util header in mapviz plug-in base class +* Handles loading old config files that still reference "mutlires_image". +* Adds an RQT plugin version of Mapviz. +* updates cmake version to squash the CMP0003 warning +* uses correct operator when combining quaternions +* adds option for rotating camera 90 degrees +* throttles log msgs +* removes dependencies on build_tools +* uses format 2 package definition +* allows plug-in selection with double-clicks +* displays file name in window title +* displays preview icon next to plug-in names +* fixes issue with coordinates displayed on status bar +* fixes missing organization in license text +* Adds tooltips describing the various mapviz widgets +* fixes GLEW/GL include order +* catkinizes mapviz +* changes license to BSD +* adds license and readme files +* Contributors: Ed Venator, Edward Venator, Jerry Towler, Marc Alban, P. J. Reed diff --git a/mapviz/CMakeLists.txt b/mapviz/CMakeLists.txt new file mode 100644 index 000000000..5fc2c1460 --- /dev/null +++ b/mapviz/CMakeLists.txt @@ -0,0 +1,210 @@ +### SET CMAKE POLICIES ### +cmake_minimum_required(VERSION 3.10...3.17) + +if(${CMAKE_VERSION} VERSION_LESS 3.12) + cmake_policy(VERSION ${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION}) +endif() + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(${CMAKE_VERSION} VERSION_EQUAL "3.11.0" OR ${CMAKE_VERSION} VERSION_GREATER "3.11.0") + cmake_policy(SET CMP0072 NEW) +endif() + +if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() +### END CMAKE POLICIES ### + +project(mapviz + DESCRIPTION "Mapviz is a ROS based visualization tool with a plug-in system similar to RVIZ focused on visualizing 2D data." + LANGUAGES CXX) + +### ROS Packages ### +find_package(ament_cmake REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(image_transport REQUIRED) +find_package(mapviz_interfaces REQUIRED) +find_package(marti_common_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rqt_gui_cpp REQUIRED) +find_package(std_srvs REQUIRED) +find_package(swri_math_util REQUIRED) +find_package(swri_transform_util REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) + +find_package(tf2_geometry_msgs REQUIRED) + +### PkgConfig ### +find_package(PkgConfig REQUIRED) +pkg_check_modules(YamlCpp yaml-cpp) + +### Boost ### +find_package(Boost REQUIRED date_time system filesystem) + +### OpenCV 3/4 on ROS2 ### +find_package(OpenCV COMPONENTS core imgproc imgcodecs videoio REQUIRED) + +# ### Qt5 on ROS2 ### +find_package(Qt5Concurrent REQUIRED) +find_package(Qt5Core REQUIRED) +find_package(Qt5Gui REQUIRED) +find_package(Qt5OpenGL REQUIRED) +find_package(Qt5Widgets REQUIRED) +add_definitions(-DWFlags=WindowFlags) +set(QT_USE_QTOPENGL TRUE) + +### OpenGL ### +find_package(OpenGL REQUIRED) +find_package(GLUT REQUIRED) + +### GLEW ### +find_package(GLEW REQUIRED) + +# Fix conflict between Boost signals used by tf and QT signals used by mapviz +add_definitions(-DQT_NO_KEYWORDS) + +# Need to include the current dir to include the results of building Qt UI files +set(CMAKE_INCLUDE_CURRENT_DIR ON) + +# Qt UI files +set(UI_FILES + src/configitem.ui + src/${PROJECT_NAME}.ui + src/pluginselect.ui +) +# Headers containing QObject definitions +set(QT_HEADERS + include/${PROJECT_NAME}/color_button.h + include/${PROJECT_NAME}/config_item.h + include/${PROJECT_NAME}/map_canvas.h + include/${PROJECT_NAME}/${PROJECT_NAME}.hpp + include/${PROJECT_NAME}/${PROJECT_NAME}_plugin.h + include/${PROJECT_NAME}/rqt_${PROJECT_NAME}.h + include/${PROJECT_NAME}/select_frame_dialog.h + include/${PROJECT_NAME}/select_service_dialog.h + include/${PROJECT_NAME}/select_topic_dialog.h + include/${PROJECT_NAME}/video_writer.h + include/${PROJECT_NAME}/widgets.h +) +# Source files for mapviz +set(SRC_FILES + src/${PROJECT_NAME}.cpp + src/color_button.cpp + src/config_item.cpp + src/${PROJECT_NAME}_application.cpp + src/map_canvas.cpp + src/rqt_${PROJECT_NAME}.cpp + src/select_frame_dialog.cpp + src/select_service_dialog.cpp + src/select_topic_dialog.cpp + src/video_writer.cpp +) + +### Qt5 on ROS2 ### +qt5_add_resources(RCC_SRCS src/resources/icons.qrc) +qt5_wrap_ui(SRC_FILES ${UI_FILES}) +qt5_wrap_cpp(SRC_FILES ${QT_HEADERS}) + +### Build mapviz as an rqt plugin ### +add_library(rqt_${PROJECT_NAME} + ${SRC_FILES} + ${RCC_SRCS} +) +target_include_directories(rqt_${PROJECT_NAME} + PUBLIC + include + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR} + ${CMAKE_CURRENT_BINARY_DIR} +) + +target_link_libraries(rqt_${PROJECT_NAME} + OpenGL::GL + ${GLUT_LIBRARIES} + GLEW::GLEW + opencv_core + opencv_highgui + opencv_imgproc + opencv_imgcodecs + opencv_videoio + ${YamlCpp_LIBRARIES} + Qt5::Concurrent + Qt5::Core + Qt5::Gui + Qt5::OpenGL + Qt5::Widgets +) +ament_target_dependencies(rqt_${PROJECT_NAME} + ament_cmake + cv_bridge + geometry_msgs + image_transport + mapviz_interfaces + marti_common_msgs + pluginlib + rclcpp + rqt_gui_cpp + std_srvs + swri_math_util + swri_transform_util + tf2 + tf2_geometry_msgs + tf2_ros +) +set_target_properties(rqt_${PROJECT_NAME} PROPERTIES + COMPILE_FLAGS "-D__STDC_FORMAT_MACROS" +) + +### Build mapviz as a standalone executable ### +add_executable(${PROJECT_NAME} + src/${PROJECT_NAME}_main.cpp +) +target_link_libraries(${PROJECT_NAME} + rqt_${PROJECT_NAME} + ${Boost_LIBRARIES} + ${GLU_LIBRARY} +) +ament_target_dependencies(${PROJECT_NAME} + swri_math_util + swri_transform_util +) +set_target_properties(${PROJECT_NAME} PROPERTIES + COMPILE_FLAGS "-D__STDC_FORMAT_MACROS" +) + +### Install mapviz ### +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} + FILES_MATCHING PATTERN "*.h" +) + +install(TARGETS ${PROJECT_NAME} rqt_${PROJECT_NAME} + RUNTIME DESTINATION lib/${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib +) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + +install(FILES plugin.xml + DESTINATION share/${PROJECT_NAME} +) + +ament_export_dependencies(${RUNTIME_DEPS} + Qt5Concurrent + Qt5Core + Qt5Gui + Qt5OpenGL + Qt5Widgets +) +ament_export_include_directories(include) +ament_export_libraries(rqt_${PROJECT_NAME} ${YamlCpp_LIBRARIES}) +ament_package() diff --git a/mapviz/include/mapviz/color_button.h b/mapviz/include/mapviz/color_button.h new file mode 100644 index 000000000..af034240f --- /dev/null +++ b/mapviz/include/mapviz/color_button.h @@ -0,0 +1,73 @@ +// ***************************************************************************** +// +// Copyright (c) 2015, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL Southwest Research Institute® BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY +// OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +// DAMAGE. +// +// ***************************************************************************** +#ifndef MAPVIZ__COLOR_BUTTON_H_ +#define MAPVIZ__COLOR_BUTTON_H_ + +#include +#include + +namespace mapviz +{ +/** + * The ColorButton widget provides a color display that the user can + * click on to select a new color. You can use this widget in Qt + * Designer by placing a QPushButton and promoting it to a custom + * widget. You have to setup the promoted widget once in each .ui file: + * + * Base class name: QPushButton + * Promoted class name: mapviz::ColorButton + * Include file name: mapviz/color_button.h + * Global Include: True + */ +class ColorButton : public QPushButton +{ + Q_OBJECT; + + QColor color_; + + public: + explicit ColorButton(QWidget *parent = 0); + + const QColor& color() const { return color_; } + + Q_SIGNALS: + // Emitted when the color is changed by user interaction. + void colorEdited(const QColor &color); + // Emitted when the color is changed by user interaction or programatically. + void colorChanged(const QColor &color); + + public Q_SLOTS: + void setColor(const QColor &color); + + private Q_SLOTS: + void handleClicked(); +}; // class ColorButton +} // namespace mapviz +#endif // MAPVIZ__COLOR_BUTTON_H_ diff --git a/mapviz/include/mapviz/config_item.h b/mapviz/include/mapviz/config_item.h new file mode 100644 index 000000000..48de5cc1f --- /dev/null +++ b/mapviz/include/mapviz/config_item.h @@ -0,0 +1,90 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MAPVIZ__CONFIG_ITEM_H_ +#define MAPVIZ__CONFIG_ITEM_H_ + +// QT libraries +#include +#include +#include +#include + +// C++ standard libraries +#include +#include + +// Auto-generated UI files +#include "ui_configitem.h" + +namespace mapviz +{ +class ConfigItem : public QWidget +{ + Q_OBJECT + +public: + explicit ConfigItem(QWidget *parent = nullptr, Qt::WindowFlags flags = Qt::WindowFlags()); + ~ConfigItem() override = default; + + void SetName(QString name); + void SetType(QString type); + void SetWidget(QWidget* widget); + + void SetListItem(QListWidgetItem* item) { item_ = item; } + bool Collapsed() const { return ui_.content->isHidden(); } + QString Name() const { return name_; } + + Ui::configitem ui_; + +Q_SIGNALS: + void UpdateSizeHint(); + void ToggledDraw(QListWidgetItem* plugin, bool visible); + void RemoveRequest(QListWidgetItem* plugin); + +public Q_SLOTS: + void Hide(); + void EditName(); + void Remove(); + void ToggleDraw(bool toggled); + +private: + void contextMenuEvent(QContextMenuEvent *event) override; + +protected: + QListWidgetItem* item_; + QString name_; + QString type_; + QAction* edit_name_action_; + QAction* remove_item_action_; + bool visible_; +}; +} // namespace mapviz + +#endif // MAPVIZ__CONFIG_ITEM_H_ diff --git a/mapviz/include/mapviz/map_canvas.h b/mapviz/include/mapviz/map_canvas.h new file mode 100644 index 000000000..faf9c4c6f --- /dev/null +++ b/mapviz/include/mapviz/map_canvas.h @@ -0,0 +1,254 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MAPVIZ__MAP_CANVAS_H_ +#define MAPVIZ__MAP_CANVAS_H_ + +// QT libraries +#include +#include +#include +#include +#include + +// ROS libraries +#include +#include +#include +#include + +#include + +// C++ standard libraries +#include +#include +#include +#include +#include + +namespace mapviz +{ +class MapCanvas : public QGLWidget +{ + Q_OBJECT + +public: + explicit MapCanvas(QWidget *parent = nullptr); + ~MapCanvas() override; + + void InitializeTf(std::shared_ptr tf); + + void AddPlugin(MapvizPluginPtr plugin, int order); + void RemovePlugin(MapvizPluginPtr plugin); + void SetFixedFrame(const std::string& frame); + void SetTargetFrame(const std::string& frame); + void ToggleFixOrientation(bool on); + void ToggleRotate90(bool on); + void ToggleEnableAntialiasing(bool on); + void ToggleUseLatestTransforms(bool on); + void UpdateView(); + void ReorderDisplays(); + void ResetLocation(); + QPointF MapGlCoordToFixedFrame(const QPointF& point); + QPointF FixedFrameToMapGlCoord(const QPointF& point); + + double frameRate() const; + + float ViewScale() const { return view_scale_; } + float OffsetX() const { return offset_x_; } + float OffsetY() const { return offset_y_; } + + + void setCanvasAbleToMove(bool assigning) + { + canvas_able_to_move_ = assigning; + } + + void leaveEvent(QEvent* e) override; + + void SetViewScale(float scale) + { + view_scale_ = scale; + UpdateView(); + } + + void SetOffsetX(float x) + { + offset_x_ = x; + UpdateView(); + } + + void SetOffsetY(float y) + { + offset_y_ = y; + UpdateView(); + } + + void SetBackground(const QColor& color) + { + bg_color_ = color; + update(); + } + + void CaptureFrames(bool enabled) + { + capture_frames_ = enabled; + update(); + } + + /** + * Copies the current capture buffer into the target buffer. The target + * buffer must already be initialized to a size of: + * height * width * 4 + * @param buffer An initialize buffer to copy data into + * @return false if the current capture buffer is empty + */ + bool CopyCaptureBuffer(uchar* buffer) + { + if (!capture_buffer_.empty()) { + memcpy(&buffer[0], &capture_buffer_[0], capture_buffer_.size()); + return true; + } + + return false; + } + + /** + * Resizes a vector to be large enough to hold the current capture buffer + * and then copies the capture buffer into it. + * @param buffer A vector to copy the capture buffer into. + * @return false if the current capture buffer is empty + */ + bool CopyCaptureBuffer(std::vector& buffer) + { + buffer.clear(); + if (!capture_buffer_.empty()) { + buffer.resize(capture_buffer_.size()); + memcpy(&buffer[0], &capture_buffer_[0], buffer.size()); + + return true; + } + + return false; + } + + void CaptureFrame(bool force = false); + +Q_SIGNALS: + void Hover(double x, double y, double scale); + +public Q_SLOTS: + void setFrameRate(const double fps); + +protected: + void initializeGL() override; + void initGlBlending(); + void pushGlMatrices(); + void popGlMatrices(); + void resizeGL(int w, int h) override; + void paintEvent(QPaintEvent* event) override; + void wheelEvent(QWheelEvent* e) override; + void mousePressEvent(QMouseEvent* e) override; + void mouseReleaseEvent(QMouseEvent* e) override; + void mouseMoveEvent(QMouseEvent* e) override; + void keyPressEvent(QKeyEvent* e) override; + + void Recenter(); + void TransformTarget(QPainter* painter); + void Zoom(float factor); + + void InitializePixelBuffers(); + + bool canvas_able_to_move_ = true; + bool has_pixel_buffers_; + int32_t pixel_buffer_size_; + GLuint pixel_buffer_ids_[2]; + int32_t pixel_buffer_index_; + bool capture_frames_; + + bool initialized_; + bool fix_orientation_; + bool rotate_90_; + bool enable_antialiasing_; + + QTimer frame_rate_timer_; + + QColor bg_color_; + + Qt::MouseButton mouse_button_; + bool mouse_pressed_; + int mouse_x_; + int mouse_y_; + // Used when right-click dragging to zoom + int mouse_previous_y_; + + bool mouse_hovering_; + int mouse_hover_x_; + int mouse_hover_y_; + + // Offset based on previous mouse drags + double offset_x_; + double offset_y_; + + // Offset based on current mouse drag + double drag_x_; + double drag_y_; + + // The center of the view + float view_center_x_; + float view_center_y_; + + // View scale in meters per pixel + float view_scale_; + + // The bounds of the view + float view_left_; + float view_right_; + float view_top_; + float view_bottom_; + + // The bounds of the scene + float scene_left_; + float scene_right_; + float scene_top_; + float scene_bottom_; + + std::string fixed_frame_; + std::string target_frame_; + + std::shared_ptr tf_buf_; + tf2::Stamped transform_; + QTransform qtransform_; + std::list plugins_; + + std::vector capture_buffer_; +}; +} // namespace mapviz + +#endif // MAPVIZ__MAP_CANVAS_H_ diff --git a/mapviz/include/mapviz/mapviz.hpp b/mapviz/include/mapviz/mapviz.hpp new file mode 100644 index 000000000..e737a5442 --- /dev/null +++ b/mapviz/include/mapviz/mapviz.hpp @@ -0,0 +1,219 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MAPVIZ__MAPVIZ_HPP_ +#define MAPVIZ__MAPVIZ_HPP_ + +// QT libraries +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include // Service +#include +#include +#include + +// ROS libraries +#include +#include +#include +#include +#include +#include + +// C++ standard libraries +#include +#include +#include +#include + +// Auto-generated UI files +#include "ui_mapviz.h" +#include "ui_pluginselect.h" + + +#include "mapviz/stopwatch.h" + +namespace mapviz +{ +class Mapviz : public QMainWindow +{ + Q_OBJECT + +public: + Mapviz(bool is_standalone, + int argc, + char** argv, + QWidget *parent = 0, + Qt::WindowFlags flags = Qt::WindowFlags()); + ~Mapviz(); + + rclcpp::Node::SharedPtr GetNode(); + + void Initialize(); + +public Q_SLOTS: + void AutoSave(); + void OpenConfig(); + void SaveConfig(); + void ClearConfig(); + void SelectNewDisplay(); + void RemoveDisplay(); + void RemoveDisplay(QListWidgetItem* item); + void ReorderDisplays(); + void FixedFrameSelected(const QString& text); + void TargetFrameSelected(const QString& text); + void ToggleUseLatestTransforms(bool on); + void UpdateFrames(); + void SpinOnce(); + void UpdateSizeHints(); + void ToggleConfigPanel(bool on); + void ToggleStatusBar(bool on); + void ToggleCaptureTools(bool on); + void ToggleFixOrientation(bool on); + void ToggleRotate90(bool on); + void ToggleEnableAntialiasing(bool on); + void ToggleShowPlugin(QListWidgetItem* item, bool visible); + void ToggleRecord(bool on); + void SetImageTransport(QAction* transport_action); + void UpdateImageTransportMenu(); + void CaptureVideoFrame(); + void StopRecord(); + void Screenshot(); + void Force720p(bool on); + void Force480p(bool on); + void SetResizable(bool on); + void SelectBackgroundColor(const QColor &color); + void SetCaptureDirectory(); + void Hover(double x, double y, double scale); + void Recenter(); + void HandleProfileTimer(); + void ClearHistory(); + +Q_SIGNALS: + /** + * Emitted every time a frame is grabbed when Mapviz is in video recording + * mode, typically at a rate of 30 FPS. + * Note that the QImage emitted says its format is ARGB, but its pixel + * order is actually BGRA. + */ + void FrameGrabbed(QImage); + void ImageTransportChanged(); + +protected: + void Open(const std::string& filename); + void Save(const std::string& filename); + + MapvizPluginPtr CreateNewDisplay( + const std::string& name, + const std::string& type, + bool visible, + bool collapsed, + int draw_order = 0); + + void AddDisplay( + const mapviz_interfaces::srv::AddMapvizDisplay::Request::SharedPtr req, + mapviz_interfaces::srv::AddMapvizDisplay::Response::SharedPtr resp); + + void ClearDisplays(); + void AdjustWindowSize(); + + QString GetDefaultConfigPath(); + + virtual void showEvent(QShowEvent* event); + virtual void closeEvent(QCloseEvent* event); + + static const QString ROS_WORKSPACE_VAR; + static const QString MAPVIZ_CONFIG_FILE; + static const char IMAGE_TRANSPORT_PARAM[]; + + Ui::mapviz ui_; + + QMenu* image_transport_menu_; + + QTimer frame_timer_; + QTimer spin_timer_; + QTimer save_timer_; + QTimer record_timer_; + QTimer profile_timer_; + + QLabel* xy_pos_label_; + QLabel* lat_lon_pos_label_; + + QWidget* spacer1_; + QWidget* spacer2_; + QWidget* spacer3_; + QPushButton* recenter_button_; + QPushButton* rec_button_; + QPushButton* stop_button_; + QPushButton* screenshot_button_; + + int argc_; + char** argv_; + + bool is_standalone_; + bool initialized_; + bool force_720p_; + bool force_480p_; + bool resizable_; + QColor background_; + + std::string capture_directory_; + QThread video_thread_; + VideoWriter* vid_writer_; + + bool updating_frames_; + + std::shared_ptr node_; + rclcpp::Service::SharedPtr add_display_srv_; + std::shared_ptr tf_buf_; + std::shared_ptr tf_; + swri_transform_util::TransformManagerPtr tf_manager_; + + pluginlib::ClassLoader* loader_; + MapCanvas* canvas_; + std::map plugins_; + + Stopwatch meas_spin_; +}; +} // namespace mapviz + +#endif // MAPVIZ__MAPVIZ_HPP_ diff --git a/mapviz/include/mapviz/mapviz_application.h b/mapviz/include/mapviz/mapviz_application.h new file mode 100644 index 000000000..4a93eecd1 --- /dev/null +++ b/mapviz/include/mapviz/mapviz_application.h @@ -0,0 +1,59 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MAPVIZ__MAPVIZ_APPLICATION_H_ +#define MAPVIZ__MAPVIZ_APPLICATION_H_ + +#include +#include + +#include + +namespace mapviz +{ +/** + * This class exists solely so that we can override QApplication::notify and + * log exceptions in the event loop as errors rather than letting them + * crash the entire program. + */ +class MapvizApplication : public QApplication +{ +public: + MapvizApplication(int &argc, char** argv, + rclcpp::Logger logger = rclcpp::get_logger("mapviz::MapvizApplication")); + + void setLogger(const rclcpp::Logger& logger); +private: + bool notify(QObject* receiver, QEvent* event) override; + + rclcpp::Logger logger_; +}; +} // namespace mapviz + +#endif // MAPVIZ__MAPVIZ_APPLICATION_H_ diff --git a/mapviz/include/mapviz/mapviz_plugin.h b/mapviz/include/mapviz/mapviz_plugin.h new file mode 100644 index 000000000..083397944 --- /dev/null +++ b/mapviz/include/mapviz/mapviz_plugin.h @@ -0,0 +1,392 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MAPVIZ__MAPVIZ_PLUGIN_H_ +#define MAPVIZ__MAPVIZ_PLUGIN_H_ + +// ROS libraries +#include +#include +#include +#include +#include +#include + +#include +#include + +// QT libraries +#include +#include +#include + +// C++ standard libraries +#include +#include + + +#include "mapviz/stopwatch.h" + +namespace mapviz +{ +class MapvizPlugin : public QObject +{ + Q_OBJECT +public: + ~MapvizPlugin() override = default; + + virtual bool Initialize( + std::shared_ptr tf_buffer, + std::shared_ptr tf_listener, + swri_transform_util::TransformManagerPtr tf_manager, + QGLWidget* canvas) + { + tf_buf_ = tf_buffer; + tf_ = tf_listener; + tf_manager_ = tf_manager; + return Initialize(canvas); + } + + virtual void Shutdown() = 0; + + virtual void ClearHistory() {} + + /** + * Draws on the Mapviz canvas using OpenGL commands; this will be called + * before Paint(); + */ + virtual void Draw(double x, double y, double scale) = 0; + + /** + * Draws on the Mapviz canvas using a QPainter; this is called after Draw(). + * You only need to implement this if you're actually using a QPainter. + */ + virtual void Paint(QPainter* /* painter */, double /* x */, + double /* y */, double /* scale */) {} + + void SetUseLatestTransforms(bool value) + { + if (value != use_latest_transforms_) { + use_latest_transforms_ = value; + Q_EMIT UseLatestTransformsChanged(use_latest_transforms_); + } + } + + void SetName(const std::string& name) { name_ = name; } + + std::string Name() const { return name_; } + + void SetType(const std::string& type) { type_ = type; } + + std::string Type() const { return type_; } + + int DrawOrder() const { return draw_order_; } + + void SetDrawOrder(int order) + { + if (draw_order_ != order) { + draw_order_ = order; + Q_EMIT DrawOrderChanged(draw_order_); + } + } + + virtual void SetNode(rclcpp::Node& node) + { + // node_ = node; + node_ = node.shared_from_this(); + } + + void DrawPlugin(double x, double y, double scale) + { + if (visible_ && initialized_) { + meas_transform_.start(); + Transform(); + meas_transform_.stop(); + + meas_draw_.start(); + Draw(x, y, scale); + meas_draw_.stop(); + } + } + + void PaintPlugin(QPainter* painter, double x, double y, double scale) + { + if (visible_ && initialized_) { + meas_transform_.start(); + Transform(); + meas_transform_.stop(); + + meas_paint_.start(); + Paint(painter, x, y, scale); + meas_paint_.start(); + } + } + + void SetTargetFrame(const std::string& frame_id) + { + if (frame_id != target_frame_) { + target_frame_ = frame_id; + + meas_transform_.start(); + Transform(); + meas_transform_.stop(); + + Q_EMIT TargetFrameChanged(target_frame_); + } + } + + bool Visible() const { return visible_; } + + void SetVisible(bool visible) + { + if (visible_ != visible) { + visible_ = visible; + Q_EMIT VisibleChanged(visible_); + } + } + + bool GetTransform( + const rclcpp::Time& stamp, + swri_transform_util::Transform& transform, + bool use_latest_transforms = true) + { + return GetTransform(source_frame_, stamp, transform, use_latest_transforms); + } + + bool GetTransform(const std::string& source, + const rclcpp::Time& stamp, + swri_transform_util::Transform& transform, + bool use_latest_transforms = true) + { + if (!initialized_) { + return false; + } + + tf2::TimePoint time; + rclcpp::Time now = node_->now(); + + if (use_latest_transforms_ && use_latest_transforms) { + time = tf2::TimePointZero; + } + else + { + time = tf2::timeFromSec(stamp.seconds()); + } + + if (tf_manager_->GetTransform( + target_frame_, + source, + time, + transform)) + { + return true; + } else if (time != tf2::TimePointZero) { + rclcpp::Duration elapsed = now - stamp; + + if (elapsed.seconds() < 0.1) + { + // If the stamped transform failed because it is too recent, find the + // most recent transform in the cache instead. + if (tf_manager_->GetTransform( + target_frame_, + source, + tf2::TimePointZero, + transform)) + { + return true; + } + } + } + + return false; + } + + virtual void Transform() = 0; + + virtual void LoadConfig(const YAML::Node& load, const std::string& path) = 0; + virtual void SaveConfig(YAML::Emitter& emitter, const std::string& path) = 0; + + virtual QWidget* GetConfigWidget(QWidget* /* parent */) { return nullptr; } + + virtual void PrintError(const std::string& message) = 0; + virtual void PrintInfo(const std::string& message) = 0; + virtual void PrintWarning(const std::string& message) = 0; + + void SetIcon(IconWidget* icon) { icon_ = icon; } + + void PrintMeasurements() + { + std::string header = type_ + " (" + name_ + ")"; + meas_transform_.printInfo(node_->get_logger(), header + " Transform()"); + meas_paint_.printInfo(node_->get_logger(), header + " Paint()"); + meas_draw_.printInfo(node_->get_logger(), header + " Draw()"); + } + + void PrintErrorHelper( + QLabel *status_label, + const std::string& message, + double throttle = 0.0); + void PrintInfoHelper( + QLabel *status_label, + const std::string& message, + double throttle = 0.0); + void PrintWarningHelper( + QLabel *status_label, + const std::string& message, + double throttle = 0.0); + +public Q_SLOTS: + virtual void DrawIcon() {} + + /** + * Override this to return "true" if you want QPainter support for your + * plugin. + */ + virtual bool SupportsPainting() + { + return false; + } + +Q_SIGNALS: + void DrawOrderChanged(int draw_order); + void SizeChanged(); + void TargetFrameChanged(const std::string& target_frame); + void UseLatestTransformsChanged(bool use_latest_transforms); + void VisibleChanged(bool visible); + + +protected: + bool initialized_; + bool visible_; + + QGLWidget* canvas_; + IconWidget* icon_; + + std::shared_ptr node_; + + std::shared_ptr tf_buf_; + std::shared_ptr tf_; + swri_transform_util::TransformManagerPtr tf_manager_; + + std::string target_frame_; + std::string source_frame_; + std::string type_; + std::string name_; + + bool use_latest_transforms_; + + int draw_order_; + + virtual bool Initialize(QGLWidget* canvas) = 0; + + MapvizPlugin() : + initialized_(false), + visible_(true), + canvas_(nullptr), + icon_(nullptr), + node_(nullptr), + tf_(), + target_frame_(""), + source_frame_(""), + use_latest_transforms_(false), + draw_order_(0) + {} + + private: + // Collect basic profiling info to know how much time each plugin + // spends in Transform(), Paint(), and Draw(). + Stopwatch meas_transform_; + Stopwatch meas_paint_; + Stopwatch meas_draw_; +}; +typedef std::shared_ptr MapvizPluginPtr; + +// Implementation + +inline void MapvizPlugin::PrintErrorHelper(QLabel *status_label, const std::string &message, + double throttle) +{ + if (message == status_label->text().toStdString()) { + return; + } + + auto logger = node_ ? node_->get_logger() : rclcpp::get_logger("mapviz"); + if (throttle > 0.0) { + RCLCPP_ERROR(logger, "Error: %s", message.c_str()); + } else { + RCLCPP_ERROR(logger, "%s", message.c_str()); + } + QPalette p(status_label->palette()); + p.setColor(QPalette::Text, Qt::red); + status_label->setPalette(p); + status_label->setText(message.c_str()); +} + +inline void MapvizPlugin::PrintInfoHelper(QLabel *status_label, const std::string &message, + double throttle) +{ + if (message == status_label->text().toStdString()) { + return; + } + + auto logger = node_ ? node_->get_logger() : rclcpp::get_logger("mapviz"); + if (throttle > 0.0) { + RCLCPP_INFO(logger, "%s", message.c_str()); + } else { + RCLCPP_INFO(logger, "%s", message.c_str()); + } + QPalette p(status_label->palette()); + p.setColor(QPalette::Text, Qt::darkGreen); + status_label->setPalette(p); + status_label->setText(message.c_str()); +} + +inline void MapvizPlugin::PrintWarningHelper(QLabel *status_label, const std::string &message, + double throttle) +{ + if (message == status_label->text().toStdString()) { + return; + } + + auto logger = node_ ? node_->get_logger() : rclcpp::get_logger("mapviz"); + if (throttle > 0.0) { + RCLCPP_WARN(logger, "%s", message.c_str()); + } else { + RCLCPP_WARN(logger, "%s", message.c_str()); + } + QPalette p(status_label->palette()); + p.setColor(QPalette::Text, Qt::darkYellow); + status_label->setPalette(p); + status_label->setText(message.c_str()); +} + +} // namespace mapviz + +#endif // MAPVIZ__MAPVIZ_PLUGIN_H_ + diff --git a/mapviz/include/mapviz/rqt_mapviz.h b/mapviz/include/mapviz/rqt_mapviz.h new file mode 100644 index 000000000..9f83bff0c --- /dev/null +++ b/mapviz/include/mapviz/rqt_mapviz.h @@ -0,0 +1,68 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MAPVIZ__RQT_MAPVIZ_H_ +#define MAPVIZ__RQT_MAPVIZ_H_ + +/* + * The RQT GUI CPP files use the Qt macros "slots" and "signals". These conflict + * with Boost macros of the same name; normally we fix this by adding "-DQT_NO_KEYWORDS" + * in our CMakeLists file, then using Q_SLOTS and Q_SIGNALS in our source code instead. + * Since we can't edit the ROS source code, though, we need to define those macros before + * we include the ROS headers and then undefine them afterwards. + */ +#define slots +#define signals +#include +#undef slots +#undef signals + +#include "mapviz.hpp" + +namespace mapviz +{ +class RqtMapviz : public rqt_gui_cpp::Plugin +{ +Q_OBJECT +public: + RqtMapviz(); + virtual void initPlugin(qt_gui_cpp::PluginContext& context); + virtual void shutdownPlugin(); + virtual void saveSettings( + qt_gui_cpp::Settings& plugin_settings, + qt_gui_cpp::Settings& instance_settings) const; + virtual void restoreSettings( + const qt_gui_cpp::Settings& plugin_settings, + const qt_gui_cpp::Settings& instance_settings); +private: + Mapviz* widget_; +}; +} // namespace mapviz + +#endif // MAPVIZ__RQT_MAPVIZ_H_ diff --git a/mapviz/include/mapviz/select_frame_dialog.h b/mapviz/include/mapviz/select_frame_dialog.h new file mode 100644 index 000000000..50e1676ec --- /dev/null +++ b/mapviz/include/mapviz/select_frame_dialog.h @@ -0,0 +1,127 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** +#ifndef MAPVIZ__SELECT_FRAME_DIALOG_H_ +#define MAPVIZ__SELECT_FRAME_DIALOG_H_ + +#include + +#include + +#include +#include +#include + + +QT_BEGIN_NAMESPACE +class QLineEdit; +class QListWidget; +class QPushButton; +QT_END_NAMESPACE + +namespace mapviz +{ +/** + * Provides a dialog for the user to select one or more TF frames. + * Several static functions are provided that can be used instead of + * instantiating the class directly. + */ +class SelectFrameDialog : public QDialog +{ + Q_OBJECT; + + public: + /** + * Present the user with a dialog to select a single frame. + * + * If the user cancels the selection or doesn't make a valid + * selection, the returned string be empty. + */ + static std::string selectFrame( + std::shared_ptr tf_buffer, + QWidget *parent = nullptr); + + /** + * Present the user with a dialog to select a multiple TF frames. + * + * If the user cancels the selection or doesn't make a valid + * selection, the returned vector will be empty. + */ + static std::vector selectFrames( + std::shared_ptr tf_buffer, + QWidget *parent = nullptr); + + /** + * Constructor for the SelectFrameDialog. + */ + explicit SelectFrameDialog(std::shared_ptr tf_buffer, + QWidget *parent = nullptr); + + /** + * Choose whether the user can select one (allow=false) or multiple + * (allow=true) frames. The default is false. + */ + void allowMultipleFrames(bool allow); + + /** + * Returns the currently selected frame. If multiple frames are + * allowed, this will only return the first selected element. If + * there is no selection, the returned info will have an empty frame + * name and datatype. + */ + std::string selectedFrame() const; + /** + * Returns the currently selected frames. If there is no selection, + * the returned vector will be empty. + */ + std::vector selectedFrames() const; + + private: + void timerEvent(QTimerEvent *) override; + void closeEvent(QCloseEvent *) override; + + std::vector filterFrames( + const std::vector &) const; + + private Q_SLOTS: + void fetchFrames(); + void updateDisplayedFrames(); + + private: + std::shared_ptr tf_buf_; + std::vector known_frames_; + std::vector displayed_frames_; + int fetch_frames_timer_id_; + + QPushButton *ok_button_; + QPushButton *cancel_button_; + QListWidget *list_widget_; + QLineEdit *name_filter_; +}; // class SelectFrameDialog +} // namespace mapviz +#endif // MAPVIZ__SELECT_FRAME_DIALOG_H_ diff --git a/mapviz/include/mapviz/select_service_dialog.h b/mapviz/include/mapviz/select_service_dialog.h new file mode 100644 index 000000000..efa6071ff --- /dev/null +++ b/mapviz/include/mapviz/select_service_dialog.h @@ -0,0 +1,174 @@ +// ***************************************************************************** +// +// Copyright (c) 2015, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL Southwest Research Institute® BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY +// OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +// DAMAGE. +// +// ***************************************************************************** + +#ifndef MAPVIZ__SELECT_SERVICE_DIALOG_H_ +#define MAPVIZ__SELECT_SERVICE_DIALOG_H_ + +#include + +#include +#include +#include + +#include +#include +#include +#include + +QT_BEGIN_NAMESPACE +class QLineEdit; +class QListWidget; +class QPushButton; +QT_END_NAMESPACE + +// This is ugly, but necessary in order to be able to send a std::vector +// via a queued signal/slot connection. +typedef std::vector ServiceStringVector; +Q_DECLARE_METATYPE(ServiceStringVector) + +namespace mapviz +{ +/** + * Enumerating services requires making a remote service call; doing this in the GUI thread + * could cause Mapviz to block and become unresponsive, so it is offloaded to another thread. + */ +class ServiceUpdaterThread : public QThread +{ + Q_OBJECT +public: + ServiceUpdaterThread( + const std::shared_ptr& nh, + const std::string& allowed_datatype, + QObject* parent) : + QThread(parent), + nh_(nh), + allowed_datatype_(allowed_datatype) + { + } + void run() override; + +Q_SIGNALS: + void servicesFetched(ServiceStringVector services); + void fetchingFailed(const QString error_msg); + +private: + std::shared_ptr nh_; + const std::string& allowed_datatype_; +}; + +/** + * Provides a dialog that the user can use to either list all known ROS services + * or all ROS services that handle a particular type. + */ +class SelectServiceDialog : public QDialog +{ + Q_OBJECT +public: + /** + * Convenience function for creating a dialog that will prompt the user to select + * a service and then return the value. If no service was selected, the returned + * value will be empty. + * @param[in] datatype The type of service to search for; if empty, it will show + * the user a list of all services. + * @param[in] parent The dialog's parent widget. + * @return The name of the selected service, or an empty string if there was none. + */ + static std::string selectService(rclcpp::Node::SharedPtr node, const std::string& datatype, QWidget* parent = 0); + + /** + * Constructs a new SelectServiceDialog and automatically starts a timer that + * will refresh the list of services every 5 seconds. + * @param[in] datatype The type of service to search for; if empty, it will show + * the user a list of all services. + * @param[in] parent The dialog's parent widget. + */ + explicit SelectServiceDialog(const rclcpp::Node::SharedPtr& node, + const std::string& datatype = "", + QWidget* parent = nullptr); + ~SelectServiceDialog() override; + + /** + * Set a datatype filter to limit displayed topics based on their + * types. If the vector is empty (default), the dialog will display + * all available topics. + * @param[in] datatype The type of service to search for. + */ + void setDatatypeFilter(const std::string& datatype); + + /** + * Gets the service the user had selected, or an empty string if there was + * none. + * @return The selected service. + */ + std::string selectedService() const; + +private Q_SLOTS: + /** + * If no worker thread is currently active, this will start a worker thread + * that will fetch all of the services matching the known data type. + */ + void fetchServices(); + /** + * Updates the list of services displayed to the user based on the list + * of known services and the current filter value. + */ + void updateDisplayedServices(); + /** + * Sets our list of known services. + */ + void updateKnownServices(ServiceStringVector services); + /** + * Displays a message box indicating that there was an error and stops our + * update timer. + */ + void displayUpdateError(const QString&); + +private: + std::vector filterServices(); + void timerEvent(QTimerEvent *) override; + void closeEvent(QCloseEvent *) override; + + std::shared_ptr nh_; + + std::string allowed_datatype_; + std::vector displayed_services_; + std::vector known_services_; + + int fetch_services_timer_id_; + + QPushButton *cancel_button_; + QListWidget *list_widget_; + QLineEdit *name_filter_; + QPushButton *ok_button_; + std::shared_ptr worker_thread_; +}; +} // namespace mapviz + +#endif // MAPVIZ__SELECT_SERVICE_DIALOG_H_ diff --git a/mapviz/include/mapviz/select_topic_dialog.h b/mapviz/include/mapviz/select_topic_dialog.h new file mode 100644 index 000000000..dd7a80f40 --- /dev/null +++ b/mapviz/include/mapviz/select_topic_dialog.h @@ -0,0 +1,197 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** +#ifndef MAPVIZ__SELECT_TOPIC_DIALOG_H_ +#define MAPVIZ__SELECT_TOPIC_DIALOG_H_ + +#include + +#include + +#include +#include +#include +#include +#include + +QT_BEGIN_NAMESPACE +class QLineEdit; +class QListWidget; +class QPushButton; +QT_END_NAMESPACE + +namespace mapviz +{ +/** + * Provides a dialog for the user to select one or more topics. + * Several static functions are provided that can be used instead of + * instantiating the class directly. + */ +class SelectTopicDialog : public QDialog +{ + Q_OBJECT; + + public: + /** + * Present the user with a dialog to select a single topic. This is + * convenience wrapper for the common case where only one datatype + * is allowed. + * + * If the user cancels the selection or doesn't make a valid + * selection, the topic and datatype fields of the returned topic + * info will be empty. + */ + static std::string selectTopic( + const rclcpp::Node::SharedPtr& node, + const std::string &datatype, + QWidget *parent = nullptr); + + /** + * Present the user with a dialog to select a single topic This is a + * convenience wrapper for the common case where two datatypes are allowed. + * + * If the user cancels the selection or doesn't make a valid + * selection, the topic and datatype fields of the returned topic + * info will be empty. + */ + static std::string selectTopic( + const rclcpp::Node::SharedPtr& node, + const std::string &datatype1, + const std::string &datatype2, + QWidget *parent = nullptr); + + /** + * Present the user with a dialog to select a single topic. + * + * If the user cancels the selection or doesn't make a valid + * selection, the topic and datatype fields of the returned topic + * info will be empty. + */ + static std::string selectTopic( + const rclcpp::Node::SharedPtr& node, + const std::vector &datatypes, + QWidget *parent = nullptr); + + /** + * Present the user with a dialog to select a multiple topics. This + * is a convenience wrapper for the common case where only one + * datatype is allowed. + * + * If the user cancels the selection or doesn't make a valid + * selection, the returned vector will be empty. + */ + static std::vector selectTopics( + const rclcpp::Node::SharedPtr& node, + const std::string &datatype, + QWidget *parent = nullptr); + + /** + * Present the user with a dialog to select a multiple topics. This + * is a convenience wrapper for the common case where two datatypes + * are allowed. + * + * If the user cancels the selection or doesn't make a valid + * selection, the returned vector will be empty. + */ + static std::vector selectTopics( + const rclcpp::Node::SharedPtr& node, + const std::string &datatype1, + const std::string &datatype2, + QWidget *parent = nullptr); + + /** + * Present the user with a dialog to select a multiple topics. + * + * If the user cancels the selection or doesn't make a valid + * selection, the returned vector will be empty. + */ + static std::vector selectTopics( + const rclcpp::Node::SharedPtr& node, + const std::vector &datatypes, + QWidget *parent = nullptr); + + /** + * Constructor for the SelectTopicDialog. + */ + explicit SelectTopicDialog(const rclcpp::Node::SharedPtr& node, + QWidget *parent = nullptr); + + /** + * Choose whether the user can select one (allow=false) or multiple + * (allow=true) topics. The default is false. + */ + void allowMultipleTopics(bool allow); + + /** + * Set a datatype filter to limit displayed topics based on their + * types. If the vector is empty (default), the dialog will display + * all available topics. + */ + void setDatatypeFilter(const std::vector &datatypes); + + /** + * Returns the currently selected topic. If multiple topics are + * allowed, this will only return the first selected element. If + * there is no selection, the returned info will have an empty topic + * name and datatype. + */ + std::string selectedTopic() const; + /** + * Returns the currently selected topics. If there is no selection, + * the returned vector will be empty. + */ + std::vector selectedTopics() const; + + private: + void timerEvent(QTimerEvent *) override; + void closeEvent(QCloseEvent *) override; + + std::vector filterTopics( + const std::map> &) const; + + private Q_SLOTS: + void fetchTopics(); + void updateDisplayedTopics(); + + private: + std::set allowed_datatypes_; + std::map> known_topics_; + + std::vector displayed_topics_; + int fetch_topics_timer_id_; + + std::shared_ptr nh_; // This may need to be a shared instance of Mapviz's node + + QPushButton *ok_button_; + QPushButton *cancel_button_; + QListWidget *list_widget_; + QLineEdit *name_filter_; +}; // class SelectTopicDialog +} // namespace mapviz + +#endif // MAPVIZ__SELECT_TOPIC_DIALOG_H_ diff --git a/mapviz/include/mapviz/stopwatch.h b/mapviz/include/mapviz/stopwatch.h new file mode 100644 index 000000000..b6959b893 --- /dev/null +++ b/mapviz/include/mapviz/stopwatch.h @@ -0,0 +1,119 @@ +// ***************************************************************************** +// +// Copyright (c) 2017, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** +#ifndef MAPVIZ__STOPWATCH_H_ +#define MAPVIZ__STOPWATCH_H_ + +#include + +#include +#include + + +namespace mapviz +{ +/* This class measures the wall time of an interval and keeps track of + * the number of intervals, the average duration, and the maximum + * duration. This is used to provide some simple measurements to keep + * an eye on performance. + */ +class Stopwatch +{ + public: + Stopwatch() + : + count_(0), + clock(), + total_time_(0, 0), + max_time_(0, 0), + start_(0, 0) + { + } + + /* Start measuring a new time interval. */ + void start() + { + start_ = clock.now(); + } + + /* End the current time interval and update the measurements. + * Behavior is undefined if start() was not called prior to this. + */ + void stop() + { + rclcpp::Duration dt = clock.now() - start_; + count_ += 1; + total_time_ = total_time_ + dt; + max_time_ = std::max(max_time_, dt); + } + + /* Return the number of intervals measured. */ + int count() const { return count_; } + + /* Returns the longest observed duration. */ + rclcpp::Duration maxTime() const {return max_time_;} + + /* Returns the average duration spent in the interval. */ + rclcpp::Duration avgTime() const + { + if (count_) { + return total_time_*(1.0/count_); + } else { + return rclcpp::Duration(0, 0); + } + } + + /* Print measurement info to the ROS console. */ + void printInfo(rclcpp::Logger logger, const std::string &name) const + { + if (count_) { + RCLCPP_INFO(logger, + "%s -- calls: %d, avg time: %.2fms, max time: %.2fms", + name.c_str(), + count_, + avgTime().seconds()*1000.0, + maxTime().seconds()*1000.0); + } else { + RCLCPP_INFO(logger, + "%s -- calls: %d, avg time: --ms, max time: --ms", + name.c_str(), + count_); + } + } + + private: + int count_; + rclcpp::Clock clock; + rclcpp::Duration total_time_; + rclcpp::Duration max_time_; + + rclcpp::Time start_; +}; // class PluginInstrumentation +} // namespace mapviz + +#endif // MAPVIZ__STOPWATCH_H_ diff --git a/mapviz/include/mapviz/video_writer.h b/mapviz/include/mapviz/video_writer.h new file mode 100644 index 000000000..aa122b2c5 --- /dev/null +++ b/mapviz/include/mapviz/video_writer.h @@ -0,0 +1,70 @@ +// ***************************************************************************** +// +// Copyright (c) 2017, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MAPVIZ__VIDEO_WRITER_H_ +#define MAPVIZ__VIDEO_WRITER_H_ + +#include +#include +#include + +#include +#include + +#ifndef Q_MOC_RUN +#include +#endif + +namespace mapviz +{ +class VideoWriter : public QObject +{ + Q_OBJECT + +public: + VideoWriter() : + video_mutex_(QMutex::Recursive) + {} + + bool initializeWriter(const std::string& directory, int width, int height); + bool isRecording(); + void stop(); + +public Q_SLOTS: + void processFrame(QImage frame); + +private: + int height_; + int width_; + QMutex video_mutex_; + std::shared_ptr video_writer_; +}; +} // namespace mapviz + +#endif // MAPVIZ__VIDEO_WRITER_H_ diff --git a/mapviz/include/mapviz/widgets.h b/mapviz/include/mapviz/widgets.h new file mode 100644 index 000000000..fb75ac544 --- /dev/null +++ b/mapviz/include/mapviz/widgets.h @@ -0,0 +1,171 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MAPVIZ_WIDGETS_H_ +#define MAPVIZ_WIDGETS_H_ + +// QT libraries +#include +#include +#include +#include +#include +#include +#include +#include + +namespace mapviz +{ +class PluginConfigList : public QListWidget +{ + Q_OBJECT + +public: + explicit PluginConfigList(QWidget *parent = nullptr) : QListWidget(parent) {} + PluginConfigList() = default; + + void UpdateIndices() + { + for (int i = 0; i < count(); i++) { + item(i)->setData(Qt::UserRole, QVariant((static_cast(i)))); + } + } + +Q_SIGNALS: + void ItemsMoved(); + +protected: + void dropEvent(QDropEvent* event) override + { + QListWidget::dropEvent(event); + + UpdateIndices(); + + Q_EMIT ItemsMoved(); + } +}; + +class PluginConfigListItem : public QListWidgetItem +{ +public: + explicit PluginConfigListItem(QListWidget *parent = nullptr) : QListWidgetItem(parent) {} + + bool operator< (const QListWidgetItem & other) const override + { + return data(Qt::UserRole).toFloat() < other.data(Qt::UserRole).toFloat(); + } +}; + +class SingleClickLabel : public QLabel +{ + Q_OBJECT + +public: + explicit SingleClickLabel(QWidget *parent = 0, Qt::WindowFlags flags = Qt::WindowFlags()) : + QLabel(parent, flags) {} + + ~SingleClickLabel() override = default; + +Q_SIGNALS: + void Clicked(); + +protected: + void mousePressEvent(QMouseEvent*) override + { + Q_EMIT Clicked(); + } +}; + +class DoubleClickWidget : public QWidget +{ + Q_OBJECT + +public: + explicit DoubleClickWidget(QWidget *parent = 0, Qt::WindowFlags flags = Qt::WindowFlags()) : + QWidget(parent, flags) {} + + ~DoubleClickWidget() override = default; + +Q_SIGNALS: + void DoubleClicked(); + void RightClicked(); + +protected: + void mouseDoubleClickEvent(QMouseEvent* event) override + { + if (event->button() == Qt::LeftButton) { + Q_EMIT DoubleClicked(); + } + } + + void mouseReleaseEvent(QMouseEvent* event) override + { + if (event->button() == Qt::RightButton) { + Q_EMIT RightClicked(); + } + } +}; + +class IconWidget : public QWidget +{ + Q_OBJECT + +public: + explicit IconWidget(QWidget *parent = nullptr, Qt::WindowFlags flags = Qt::WindowFlags()) : + QWidget(parent, flags) + { + pixmap_ = QPixmap(16, 16); + pixmap_.fill(Qt::transparent); + } + + ~IconWidget() override = default; + + void SetPixmap(QPixmap pixmap) + { + pixmap_ = pixmap; + update(); + } + +protected: + void paintEvent(QPaintEvent*) override + { + QPainter painter(this); + painter.fillRect(0, 0, width(), height(), palette().color(QPalette::Button)); + + int x_offset = (width() - pixmap_.width()) / 2; + int y_offset = (height() - pixmap_.height()) / 2; + + painter.drawPixmap(x_offset, y_offset, pixmap_); + } + + QPixmap pixmap_; +}; +} // namespace mapviz + +#endif // MAPVIZ__WIDGETS_H_ diff --git a/mapviz/launch/mapviz.launch.py b/mapviz/launch/mapviz.launch.py new file mode 100644 index 000000000..2f9d98512 --- /dev/null +++ b/mapviz/launch/mapviz.launch.py @@ -0,0 +1,41 @@ +import launch +import launch.actions +import launch.substitutions +import launch_ros.actions + + +def generate_launch_description(): + return launch.LaunchDescription([ + launch_ros.actions.Node( + package="mapviz", + executable="mapviz", + name="mapviz", + ), + launch_ros.actions.Node( + package="swri_transform_util", + executable="initialize_origin.py", + name="initialize_origin", + parameters=[ + {"name": "local_xy_frame", "value": "map"}, + {"name": "local_xy_origin", "value": "swri"}, + {"name": "local_xy_origins", "value": """[ + {"name": "swri", + "latitude": 29.45196669, + "longitude": -98.61370577, + "altitude": 233.719, + "heading": 0.0}, + {"name": "back_40", + "latitude": 29.447507, + "longitude": -98.629367, + "altitude": 200.0, + "heading": 0.0} + ]"""} + ] + ), + launch_ros.actions.Node( + package="tf2_ros", + executable="static_transform_publisher", + name="swri_transform", + arguments=["0", "0", "0", "0", "0", "0", "map", "origin"] + ) + ]) diff --git a/mapviz/launch/mapviz.launch.xml b/mapviz/launch/mapviz.launch.xml new file mode 100644 index 000000000..e02dc6301 --- /dev/null +++ b/mapviz/launch/mapviz.launch.xml @@ -0,0 +1,27 @@ + + + + + + + + + + + + + diff --git a/mapviz/mainpage.dox b/mapviz/mainpage.dox new file mode 100644 index 000000000..b06ce90e5 --- /dev/null +++ b/mapviz/mainpage.dox @@ -0,0 +1,19 @@ +/** +\mainpage +\section mapviz + +\b mapviz is a set of API's for ... + +This package... (see sumet_perception/material_classification/mainpage.dox for example description) + + +\subsection codeapi Code API + +The C++ API consists of the following main classes: + +- \b ConfigItem - \copybrief ConfigItem +- \b MapCanvas - \copybrief MapCanvas +- \b mapviz::MapvizPlugin - \copybrief mapviz::MapvizPlugin +- \b Mapviz - \copybrief Mapviz + +**/ diff --git a/mapviz/package.xml b/mapviz/package.xml new file mode 100644 index 000000000..f72b3b345 --- /dev/null +++ b/mapviz/package.xml @@ -0,0 +1,50 @@ + + mapviz + 2.3.0 + + + mapviz + + + Marc Alban + P. J. Reed + Southwest Research Institute + BSD + https://github.com/swri-robotics/mapviz + + ament_cmake + pkg-config + qt5-qmake + + libqt5-core + libqt5-opengl-dev + ros_environment + + cv_bridge + geometry_msgs + glut + image_transport + libglew-dev + libxi-dev + libxmu-dev + mapviz_interfaces + marti_common_msgs + pluginlib + rclcpp + rqt_gui_cpp + rqt_gui + std_srvs + swri_math_util + swri_transform_util + tf2 + tf2_geometry_msgs + tf2_ros + yaml-cpp + + libqt5-opengl + + + ament_cmake + + + diff --git a/mapviz/plugin.xml b/mapviz/plugin.xml new file mode 100644 index 000000000..3e9e934d7 --- /dev/null +++ b/mapviz/plugin.xml @@ -0,0 +1,17 @@ + + + + Mapviz is a ROS based visualization tool with a plug-in system similar to RVIZ focused on visualizing 2D data. + + + + + folder + Plugins related to visualization. + + + image-x-generic + 2D visualization tool + + + diff --git a/mapviz/src/color_button.cpp b/mapviz/src/color_button.cpp new file mode 100644 index 000000000..5042f1d6e --- /dev/null +++ b/mapviz/src/color_button.cpp @@ -0,0 +1,75 @@ +// ***************************************************************************** +// +// Copyright (c) 2015, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL Southwest Research Institute® BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY +// OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +// DAMAGE. +// +// ***************************************************************************** +#include + +#include + +namespace mapviz +{ +ColorButton::ColorButton(QWidget *parent) + : + QPushButton(parent) +{ + setColor(Qt::black); + QObject::connect(this, SIGNAL(clicked(bool)), + this, SLOT(handleClicked())); +} + +void ColorButton::setColor(const QColor &color) +{ + if (!color.isValid() || color == color_) { + return; + } + + color_ = color; + // This was a very strange bug. On initialization, the constructor + // would set the stylesheet to black, then the external owner would + // call setColor to change the color to something else. We would + // properly set the internal color_ and stylesheet, but it would + // continue to display black. If the user changed the color, it + // would change properly. Calling setStylesheet() twice fixes the + // behavior. + setStyleSheet("background: " + color_.name()); + setStyleSheet("background: " + color_.name()); + Q_EMIT colorChanged(color_); +} + +void ColorButton::handleClicked() +{ + // Note: We do not pass ourself as the parent or else the dialog + // will inherit our color as the background! + QColor new_color = QColorDialog::getColor(color_); + if (!new_color.isValid() || new_color == color_) { + return; + } + setColor(new_color); + Q_EMIT colorEdited(new_color); +} +} // namespace mapviz diff --git a/mapviz/src/config_item.cpp b/mapviz/src/config_item.cpp new file mode 100644 index 000000000..e17eeb04d --- /dev/null +++ b/mapviz/src/config_item.cpp @@ -0,0 +1,122 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include +#include +#include +#include + +namespace mapviz +{ + ConfigItem::ConfigItem(QWidget *parent, Qt::WindowFlags flags) : + QWidget(parent, flags), + item_(nullptr), + visible_(true) + { + ui_.setupUi(this); + + edit_name_action_ = new QAction("Edit Name", this); + remove_item_action_ = new QAction("Remove", this); + remove_item_action_->setIcon(QIcon(":/images/remove-icon-th.png")); + + connect(edit_name_action_, SIGNAL(triggered()), this, SLOT(EditName())); + connect(remove_item_action_, SIGNAL(triggered()), this, SLOT(Remove())); + } + + void ConfigItem::ToggleDraw(bool toggled) + { + if (visible_ != toggled) { + visible_ = toggled; + if (ui_.show->isChecked() != toggled) { + ui_.show->setChecked(toggled); + } + + Q_EMIT ToggledDraw(item_, toggled); + } + } + + void ConfigItem::contextMenuEvent(QContextMenuEvent* event) + { + QMenu menu(this); + menu.addAction(edit_name_action_); + menu.addAction(remove_item_action_); + menu.exec(event->globalPos()); + } + + void ConfigItem::SetName(QString name) + { + name_ = name; + ui_.namelabel->setText(type_ + " (" + name_ + ")"); + } + + void ConfigItem::SetType(QString type) + { + type_ = type; + ui_.namelabel->setText(type_ + " (" + name_ + ")"); + } + + void ConfigItem::SetWidget(QWidget* widget) + { + ui_.label->hide(); + ui_.content_layout->addWidget(widget); + } + + void ConfigItem::EditName() + { + bool ok; + QString text = QInputDialog::getText( + this, + tr("Set Display name"), + tr(""), + QLineEdit::Normal, + name_, &ok); + + if (ok && !text.isEmpty()) { + SetName(text); + } + } + + void ConfigItem::Remove() + { + Q_EMIT RemoveRequest(item_); + } + + void ConfigItem::Hide() + { + if (!ui_.content->isHidden()) { + ui_.content->hide(); + ui_.signlabel->setText(" + "); + } else { + ui_.content->show(); + ui_.signlabel->setText(" - "); + } + + Q_EMIT UpdateSizeHint(); + } +} // namespace mapviz diff --git a/mapviz/src/configitem.ui b/mapviz/src/configitem.ui new file mode 100644 index 000000000..dfd41b857 --- /dev/null +++ b/mapviz/src/configitem.ui @@ -0,0 +1,797 @@ + + + configitem + + + + 0 + 0 + 400 + 300 + + + + + 0 + 0 + + + + Form + + + + 0 + + + 2 + + + 0 + + + 0 + + + + + + 0 + 0 + + + + + + + + + 0 + 0 + 0 + + + + + + + 169 + 169 + 169 + + + + + + + 234 + 230 + 222 + + + + + + + 195 + 191 + 185 + + + + + + + 78 + 76 + 74 + + + + + + + 104 + 102 + 99 + + + + + + + 0 + 0 + 0 + + + + + + + 255 + 255 + 255 + + + + + + + 0 + 0 + 0 + + + + + + + 169 + 169 + 169 + + + + + + + 169 + 169 + 169 + + + + + + + 0 + 0 + 0 + + + + + + + 205 + 204 + 201 + + + + + + + 255 + 255 + 220 + + + + + + + 0 + 0 + 0 + + + + + + + + + 0 + 0 + 0 + + + + + + + 169 + 169 + 169 + + + + + + + 234 + 230 + 222 + + + + + + + 195 + 191 + 185 + + + + + + + 78 + 76 + 74 + + + + + + + 104 + 102 + 99 + + + + + + + 0 + 0 + 0 + + + + + + + 255 + 255 + 255 + + + + + + + 0 + 0 + 0 + + + + + + + 169 + 169 + 169 + + + + + + + 169 + 169 + 169 + + + + + + + 0 + 0 + 0 + + + + + + + 205 + 204 + 201 + + + + + + + 255 + 255 + 220 + + + + + + + 0 + 0 + 0 + + + + + + + + + 78 + 76 + 74 + + + + + + + 169 + 169 + 169 + + + + + + + 234 + 230 + 222 + + + + + + + 195 + 191 + 185 + + + + + + + 78 + 76 + 74 + + + + + + + 104 + 102 + 99 + + + + + + + 78 + 76 + 74 + + + + + + + 255 + 255 + 255 + + + + + + + 78 + 76 + 74 + + + + + + + 169 + 169 + 169 + + + + + + + 169 + 169 + 169 + + + + + + + 0 + 0 + 0 + + + + + + + 156 + 153 + 148 + + + + + + + 255 + 255 + 220 + + + + + + + 0 + 0 + 0 + + + + + + + + QWidget { background-color : DarkGray ;} + + + + 0 + + + QLayout::SetDefaultConstraint + + + 0 + + + 0 + + + 0 + + + 0 + + + + + + + + + + 211 + 211 + 211 + + + + + + + 162 + 159 + 154 + + + + + + + 211 + 211 + 211 + + + + + + + 211 + 211 + 211 + + + + + + + + + 211 + 211 + 211 + + + + + + + 162 + 159 + 154 + + + + + + + 211 + 211 + 211 + + + + + + + 211 + 211 + 211 + + + + + + + + + 211 + 211 + 211 + + + + + + + 162 + 159 + 154 + + + + + + + 211 + 211 + 211 + + + + + + + 211 + 211 + 211 + + + + + + + + + DejaVu Sans Mono + 9 + + + + QLabel { background-color : LightGray ; } + + + - + + + + + + + + 0 + 0 + + + + + Ubuntu + 9 + 75 + true + + + + Name + + + + + + + + 0 + 0 + + + + + 20 + 20 + + + + + + + + + 0 + 0 + + + + + 20 + 16777215 + + + + + + + true + + + false + + + + + + + + + + + 0 + 0 + + + + + 0 + + + 0 + + + 0 + + + 0 + + + + + + 0 + 0 + + + + Config goes here... + + + + + + + + + + + mapviz::DoubleClickWidget + QWidget +
mapviz/widgets.h
+ 1 + + DoubleClicked() + RightClicked() + + + + mapviz::SingleClickLabel + QLabel +
mapviz/widgets.h
+ + + mapviz::IconWidget + QWidget +
mapviz/widgets.h
+ 1 + + + + + + header + DoubleClicked() + configitem + Hide() + + + 169 + 22 + + + 2 + 123 + + + + + signlabel + Clicked() + configitem + Hide() + + + 20 + 20 + + + 1 + 82 + + + + + show + toggled(bool) + configitem + ToggleDraw(bool) + + + 385 + 11 + + + 401 + 210 + + + + + + Hide() + ToggleDraw(bool) + EditName() + + diff --git a/mapviz/src/map_canvas.cpp b/mapviz/src/map_canvas.cpp new file mode 100644 index 000000000..e618bf075 --- /dev/null +++ b/mapviz/src/map_canvas.cpp @@ -0,0 +1,640 @@ + // ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + + +#include +#include +#include + +#include + +#include +#include + + +// C++ standard libraries +#include +#include +#include +#include + + +namespace mapviz +{ + + +bool compare_plugins(MapvizPluginPtr a, MapvizPluginPtr b) +{ + return a->DrawOrder() < b->DrawOrder(); +} + +/** + * Convenience method for generating a geometry_msgs::msg::PointStamped in one line, + * since we do this a few times. + * @param x + * @param y + * @param z + * @return + */ +geometry_msgs::msg::PointStamped make_point_stamped(double x, double y, double z) +{ + geometry_msgs::msg::PointStamped point; + point.point.x = x; + point.point.y = y; + point.point.z = z; + return point; +} + +/** + * Convenience method for converting a tf2::Stamped object into + * the equivalent ROS message. + * @param transform The source object + * @return That tf as a ROS message + */ +auto tf2_to_msg(const tf2::Stamped& transform) +{ + return tf2::toMsg(transform); +} + +MapCanvas::MapCanvas(QWidget* parent) : + QGLWidget(QGLFormat(QGL::SampleBuffers), parent), + has_pixel_buffers_(false), + pixel_buffer_size_(0), + pixel_buffer_ids_(), + pixel_buffer_index_(0), + capture_frames_(false), + initialized_(false), + fix_orientation_(false), + rotate_90_(false), + enable_antialiasing_(true), + mouse_button_(Qt::NoButton), + mouse_pressed_(false), + mouse_x_(0), + mouse_y_(0), + mouse_previous_y_(0), + mouse_hovering_(false), + mouse_hover_x_(0), + mouse_hover_y_(0), + offset_x_(0), + offset_y_(0), + drag_x_(0), + drag_y_(0), + view_center_x_(0), + view_center_y_(0), + view_scale_(1), + view_left_(-25), + view_right_(25), + view_top_(10), + view_bottom_(-10), + scene_left_(-10), + scene_right_(10), + scene_top_(10), + scene_bottom_(-10) +{ + RCLCPP_INFO(rclcpp::get_logger("mapviz"), "View scale: %f meters/pixel", view_scale_); + setMouseTracking(true); + + + QObject::connect(&frame_rate_timer_, SIGNAL(timeout()), this, SLOT(update())); + setFrameRate(50.0); + frame_rate_timer_.start(); + setFocusPolicy(Qt::StrongFocus); +} + +MapCanvas::~MapCanvas() +{ + if (pixel_buffer_size_ != 0) { + glDeleteBuffersARB(2, pixel_buffer_ids_); + } +} + +void MapCanvas::InitializeTf(std::shared_ptr tf) +{ + tf_buf_ = tf; +} + +void MapCanvas::InitializePixelBuffers() +{ + if (has_pixel_buffers_) { + int32_t buffer_size = width() * height() * 4; + + if (pixel_buffer_size_ != buffer_size) { + if (pixel_buffer_size_ != 0) { + glDeleteBuffersARB(2, pixel_buffer_ids_); + } + + glGenBuffersARB(2, pixel_buffer_ids_); + glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, pixel_buffer_ids_[0]); + glBufferDataARB(GL_PIXEL_PACK_BUFFER_ARB, buffer_size, 0, GL_STREAM_READ_ARB); + glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, pixel_buffer_ids_[1]); + glBufferDataARB(GL_PIXEL_PACK_BUFFER_ARB, buffer_size, 0, GL_STREAM_READ_ARB); + glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, 0); + + pixel_buffer_size_ = buffer_size; + } + } +} + +void MapCanvas::initializeGL() +{ + GLenum err = glewInit(); + if (GLEW_OK != err) { + RCLCPP_ERROR(rclcpp::get_logger("mapviz"), "Error: %s\n", glewGetErrorString(err)); + } else { + // Check if pixel buffers are available for asynchronous capturing + std::string extensions = (const char*)glGetString(GL_EXTENSIONS); + has_pixel_buffers_ = extensions.find("GL_ARB_pixel_buffer_object") != std::string::npos; + } + + glClearColor(0.58f, 0.56f, 0.5f, 1); + if (enable_antialiasing_) { + glEnable(GL_MULTISAMPLE); + glEnable(GL_POINT_SMOOTH); + glEnable(GL_LINE_SMOOTH); + glEnable(GL_POLYGON_SMOOTH); + glHint(GL_LINE_SMOOTH_HINT, GL_NICEST); + glHint(GL_POINT_SMOOTH_HINT, GL_NICEST); + glHint(GL_POLYGON_SMOOTH_HINT, GL_NICEST); + } else { + glDisable(GL_MULTISAMPLE); + glDisable(GL_POINT_SMOOTH); + glDisable(GL_LINE_SMOOTH); + glDisable(GL_POLYGON_SMOOTH); + } + initGlBlending(); + + initialized_ = true; +} + +void MapCanvas::initGlBlending() +{ + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + glDepthFunc(GL_NEVER); + glDisable(GL_DEPTH_TEST); +} + +void MapCanvas::resizeGL(int w, int h) +{ + UpdateView(); +} + +void MapCanvas::CaptureFrame(bool force) +{ + // Ensure the pixel size is actually 4 + glPixelStorei(GL_PACK_ALIGNMENT, 4); + + if (has_pixel_buffers_ && !force) { + InitializePixelBuffers(); + + pixel_buffer_index_ = (pixel_buffer_index_ + 1) % 2; + int32_t next_index = (pixel_buffer_index_ + 1) % 2; + + glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, pixel_buffer_ids_[pixel_buffer_index_]); + glReadPixels(0, 0, width(), height(), GL_BGRA, GL_UNSIGNED_BYTE, 0); + glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, pixel_buffer_ids_[next_index]); + GLubyte* data = reinterpret_cast( + glMapBufferARB(GL_PIXEL_PACK_BUFFER_ARB, GL_READ_ONLY_ARB)); + if (data) { + capture_buffer_.resize(pixel_buffer_size_); + + memcpy(&capture_buffer_[0], data, pixel_buffer_size_); + + glUnmapBufferARB(GL_PIXEL_PACK_BUFFER_ARB); + } + glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, 0); + } else { + int32_t buffer_size = width() * height() * 4; + capture_buffer_.clear(); + capture_buffer_.resize(buffer_size); + + glReadPixels(0, 0, width(), height(), GL_BGRA, GL_UNSIGNED_BYTE, &capture_buffer_[0]); + } +} + +void MapCanvas::paintEvent(QPaintEvent* event) +{ + if (capture_frames_) { + CaptureFrame(); + } + + QPainter p(this); + p.setRenderHints(QPainter::Antialiasing | + QPainter::TextAntialiasing | + QPainter::SmoothPixmapTransform, + enable_antialiasing_); + p.beginNativePainting(); + // .beginNativePainting() disables blending and clears a handful of other + // values that we need to manually reset. + initGlBlending(); + glMatrixMode(GL_MODELVIEW); + glPushMatrix(); + + glClearColor(bg_color_.redF(), bg_color_.greenF(), bg_color_.blueF(), 1.0f); + UpdateView(); + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + TransformTarget(&p); + + // Draw test pattern + glLineWidth(3); + glBegin(GL_LINES); + // Red line to the right + glColor3f(1, 0, 0); + glVertex2f(0, 0); + glVertex2f(20, 0); + + // Green line to the top + glColor3f(0, 1, 0); + glVertex2f(0, 0); + glVertex2f(0, 20); + glEnd(); + + std::list::iterator it; + for (it = plugins_.begin(); it != plugins_.end(); ++it) { + // Before we let a plugin do any drawing, push all matrices and attributes. + // This helps to ensure that plugins can't accidentally mess something up + // for the next plugin. + pushGlMatrices(); + + (*it)->DrawPlugin(view_center_x_, view_center_y_, view_scale_); + + if ((*it)->SupportsPainting()) { + p.endNativePainting(); + (*it)->PaintPlugin(&p, view_center_x_, view_center_y_, view_scale_); + p.beginNativePainting(); + initGlBlending(); + } + + popGlMatrices(); + } + + glMatrixMode(GL_MODELVIEW); + glPopMatrix(); + p.endNativePainting(); +} + +void MapCanvas::pushGlMatrices() +{ + glMatrixMode(GL_TEXTURE); + glPushMatrix(); + glMatrixMode(GL_PROJECTION); + glPushMatrix(); + glMatrixMode(GL_MODELVIEW); + glPushMatrix(); + glPushAttrib(GL_ALL_ATTRIB_BITS); +} + +void MapCanvas::popGlMatrices() +{ + glPopAttrib(); + glMatrixMode(GL_MODELVIEW); + glPopMatrix(); + glMatrixMode(GL_PROJECTION); + glPopMatrix(); + glMatrixMode(GL_TEXTURE); + glPopMatrix(); +} + +void MapCanvas::wheelEvent(QWheelEvent* e) +{ + float numDegrees = e->angleDelta().y() / -8; + + Zoom(numDegrees / 10.0); +} + +void MapCanvas::Zoom(float factor) +{ + view_scale_ *= std::pow(1.1, factor); + UpdateView(); +} + +void MapCanvas::mousePressEvent(QMouseEvent* e) +{ + mouse_x_ = e->x(); + mouse_y_ = e->y(); + mouse_previous_y_ = mouse_y_; + drag_x_ = 0; + drag_y_ = 0; + mouse_pressed_ = true; + mouse_button_ = e->button(); +} + +void MapCanvas::keyPressEvent(QKeyEvent* event) +{ + std::list::iterator it; + for (it = plugins_.begin(); it != plugins_.end(); ++it) { + (*it)->event(event); + } +} + +QPointF MapCanvas::MapGlCoordToFixedFrame(const QPointF& point) +{ + bool invertible = true; + return qtransform_.inverted(&invertible).map(point); +} + +QPointF MapCanvas::FixedFrameToMapGlCoord(const QPointF& point) +{ + return qtransform_.map(point); +} + +void MapCanvas::mouseReleaseEvent(QMouseEvent* e) +{ + mouse_button_ = Qt::NoButton; + mouse_pressed_ = false; + offset_x_ += drag_x_; + offset_y_ += drag_y_; + drag_x_ = 0; + drag_y_ = 0; +} + +void MapCanvas::mouseMoveEvent(QMouseEvent* e) +{ + if (mouse_pressed_ && canvas_able_to_move_) { + int diff; + switch (mouse_button_) { + case Qt::LeftButton: + case Qt::MiddleButton: + if (((mouse_x_ - e->x()) != 0 || (mouse_y_ - e->y()) != 0)) { + drag_x_ = -((mouse_x_ - e->x()) * view_scale_); + drag_y_ = ((mouse_y_ - e->y()) * view_scale_); + } + break; + case Qt::RightButton: + diff = e->y() - mouse_previous_y_; + if (diff != 0) { + Zoom((static_cast(diff)) / 10.0f); + } + mouse_previous_y_ = e->y(); + break; + default: + // Unexpected mouse button + break; + } + } + + double center_x = -offset_x_ - drag_x_; + double center_y = -offset_y_ - drag_y_; + double x = center_x + (e->x() - width() / 2.0) * view_scale_; + double y = center_y + (height() / 2.0 - e->y()) * view_scale_; + + geometry_msgs::msg::PointStamped point_in = make_point_stamped(x, y, 0.0); + geometry_msgs::msg::PointStamped point_out; + + auto tfm_temp = tf2_to_msg(transform_); + tf2::doTransform(point_in, point_out, tfm_temp); + + mouse_hovering_ = true; + mouse_hover_x_ = e->x(); + mouse_hover_y_ = e->y(); + + Q_EMIT Hover(point_out.point.x, point_out.point.y, view_scale_); +} + +void MapCanvas::leaveEvent(QEvent* e) +{ + mouse_hovering_ = false; + Q_EMIT Hover(0, 0, 0); +} + +void MapCanvas::SetFixedFrame(const std::string& frame) +{ + fixed_frame_ = frame; + std::list::iterator it; + for (it = plugins_.begin(); it != plugins_.end(); ++it) { + (*it)->SetTargetFrame(frame); + } +} + +void MapCanvas::SetTargetFrame(const std::string& frame) +{ + offset_x_ = 0; + offset_y_ = 0; + drag_x_ = 0; + drag_y_ = 0; + + target_frame_ = frame; +} + +void MapCanvas::ToggleFixOrientation(bool on) +{ + fix_orientation_ = on; +} + +void MapCanvas::ToggleRotate90(bool on) +{ + rotate_90_ = on; +} + +void MapCanvas::ToggleEnableAntialiasing(bool on) +{ + enable_antialiasing_ = on; + QGLFormat format; + format.setSwapInterval(1); + format.setSampleBuffers(enable_antialiasing_); + // After setting the format, initializeGL will automatically be called again, then paintGL. + this->setFormat(format); +} + +void MapCanvas::ToggleUseLatestTransforms(bool on) +{ + std::list::iterator it; + for (it = plugins_.begin(); it != plugins_.end(); ++it) { + (*it)->SetUseLatestTransforms(on); + } +} + +void MapCanvas::AddPlugin(MapvizPluginPtr plugin, int) +{ + plugins_.push_back(plugin); +} + +void MapCanvas::RemovePlugin(MapvizPluginPtr plugin) +{ + plugin->Shutdown(); + plugins_.remove(plugin); +} + +void MapCanvas::TransformTarget(QPainter* painter) +{ + glTranslatef(offset_x_ + drag_x_, offset_y_ + drag_y_, 0); + // In order for plugins drawing with a QPainter to be able to use the same coordinates + // as plugins using drawing using native GL commands, we have to replicate the + // GL transforms using a QTransform. Note that a QPainter's coordinate system is + // flipped on the Y axis relative to OpenGL's. + qtransform_ = qtransform_.translate(offset_x_ + drag_x_, -(offset_y_ + drag_y_)); + + view_center_x_ = -offset_x_ - drag_x_; + view_center_y_ = -offset_y_ - drag_y_; + + if (!tf_buf_ || fixed_frame_.empty() || target_frame_.empty() || target_frame_ == "") { + qtransform_ = qtransform_.scale(1, -1); + painter->setWorldTransform(qtransform_, false); + + return; + } + + bool success = false; + + try + { + auto tfrm = tf_buf_->lookupTransform( + fixed_frame_, + target_frame_, + tf2::TimePointZero, + std::chrono::seconds(1)); + + tf2::fromMsg(tfrm, transform_); + + // If the viewer orientation is fixed don't rotate the center point. + if (fix_orientation_) { + transform_.setRotation(tf2::Transform::getIdentity().getRotation()); + } + + if (rotate_90_) { + tf2::Quaternion yaw90; + yaw90.setRPY(0, 0, -swri_math_util::_half_pi); + transform_.setRotation(yaw90 * transform_.getRotation()); + } + + double roll, pitch, yaw; + transform_.getBasis().getRPY(roll, pitch, yaw); + + glRotatef(-yaw * 57.2957795, 0, 0, 1); + qtransform_ = qtransform_.rotateRadians(yaw); + + glTranslatef(-transform_.getOrigin().getX(), -transform_.getOrigin().getY(), 0); + qtransform_ = qtransform_.translate( + -transform_.getOrigin().getX(), + transform_.getOrigin().getY()); + + geometry_msgs::msg::PointStamped point = make_point_stamped(view_center_x_, view_center_y_, 0.0); + geometry_msgs::msg::PointStamped center; + + auto tfm_temp = tf2_to_msg(transform_); + tf2::doTransform(point, center, tfm_temp); + + view_center_x_ = center.point.x; + view_center_y_ = center.point.y; + + qtransform_ = qtransform_.scale(1, -1); + painter->setWorldTransform(qtransform_, false); + + if (mouse_hovering_) { + double center_x = -offset_x_ - drag_x_; + double center_y = -offset_y_ - drag_y_; + double x = center_x + (mouse_hover_x_ - width() / 2.0) * view_scale_; + double y = center_y + (height() / 2.0 - mouse_hover_y_) * view_scale_; + + geometry_msgs::msg::PointStamped hover_in = make_point_stamped(x, y, 0.0); + geometry_msgs::msg::PointStamped hover_out; + + tfm_temp = tf2_to_msg(transform_); + tf2::doTransform(hover_in, hover_out, tfm_temp); + + Q_EMIT Hover(hover_out.point.x, hover_out.point.y, view_scale_); + } + + success = true; + } + catch (const tf2::LookupException& e) + { + RCLCPP_ERROR(rclcpp::get_logger("mapviz"), "%s", e.what()); + } + catch (const tf2::ConnectivityException& e) + { + RCLCPP_ERROR(rclcpp::get_logger("mapviz"), "%s", e.what()); + } + catch (const tf2::ExtrapolationException& e) + { + RCLCPP_ERROR(rclcpp::get_logger("mapviz"), "%s", e.what()); + } + catch (...) + { + RCLCPP_ERROR(rclcpp::get_logger("mapviz"), "Error looking up transform"); + } + + if (!success) + { + qtransform_ = qtransform_.scale(1, -1); + painter->setWorldTransform(qtransform_, false); + } +} + +void MapCanvas::UpdateView() +{ + if (initialized_) { + Recenter(); + + glViewport(0, 0, width(), height()); + glMatrixMode(GL_PROJECTION); + glLoadIdentity(); + glOrtho(view_left_, view_right_, view_top_, view_bottom_, -0.5f, 0.5f); + + qtransform_ = QTransform::fromTranslate(width() / 2.0, height() / 2.0). + scale(1.0 / view_scale_, 1.0 / view_scale_); + } +} + +void MapCanvas::ResetLocation() +{ + SetTargetFrame(target_frame_); + SetViewScale(1.0); +} + +void MapCanvas::ReorderDisplays() +{ + plugins_.sort(compare_plugins); +} + +void MapCanvas::Recenter() +{ + // Recalculate the bounds of the view + view_left_ = -(width() * view_scale_ * 0.5); + view_top_ = -(height() * view_scale_ * 0.5); + view_right_ = (width() * view_scale_ * 0.5); + view_bottom_ = (height() * view_scale_ * 0.5); +} + +void MapCanvas::setFrameRate(const double fps) +{ + if (fps <= 0.0) { + RCLCPP_ERROR(rclcpp::get_logger("mapviz"), "Invalid frame rate: %f", fps); + return; + } + + frame_rate_timer_.setInterval(1000.0/fps); +} + +double MapCanvas::frameRate() const +{ + return 1000.0 / frame_rate_timer_.interval(); +} +} // namespace mapviz diff --git a/mapviz/src/mapviz.cpp b/mapviz/src/mapviz.cpp new file mode 100644 index 000000000..17f4d3dc5 --- /dev/null +++ b/mapviz/src/mapviz.cpp @@ -0,0 +1,1494 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include + +// C++ standard libraries +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if CV_MAJOR_VERSION > 2 +#include +#include +#endif + +// QT libraries +#if QT_VERSION >= 0x050000 +#include +#else +#include +#endif +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Other Project libraries +#include +#include + +#include +#include + +#include + +// YAML libraries +#include + +// Boost libraries +#include +#include +#include +#include +#include + +// OpenCV libraries +#include +#include + +namespace mapviz +{ +const QString Mapviz::ROS_WORKSPACE_VAR = "ROS_WORKSPACE"; +const QString Mapviz::MAPVIZ_CONFIG_FILE = "/.mapviz_config"; +const char Mapviz::IMAGE_TRANSPORT_PARAM[] = "image_transport"; + +Mapviz::Mapviz(bool is_standalone, int argc, char** argv, QWidget *parent, Qt::WindowFlags flags) : + QMainWindow(parent, flags), + xy_pos_label_(new QLabel("fixed: 0.0,0.0")), + lat_lon_pos_label_(new QLabel("lat/lon: 0.0,0.0")), + argc_(argc), + argv_(argv), + is_standalone_(is_standalone), + initialized_(false), + force_720p_(false), + force_480p_(false), + resizable_(true), + background_(Qt::gray), + capture_directory_("~"), + vid_writer_(nullptr), + updating_frames_(false), + node_(nullptr), + canvas_(nullptr) +{ + // Multiple users could be using mapviz, so its name needs to be anonymous, + // but ROS 2 Dashing doesn't have a way to set that through node options; + // we manually do it the same way that ros::init did in ROS 1 + std::stringstream name; + name << "mapviz"; + char buf[200]; + std::snprintf(buf, sizeof(buf), "_%llu", (unsigned long long)rclcpp::Clock().now().nanoseconds()); + name << buf; + node_ = std::make_shared(name.str()); + + QString default_path = GetDefaultConfigPath(); + node_->declare_parameter("config", default_path.toStdString()); + node_->declare_parameter("auto_save_backup", true); + node_->declare_parameter("print_profile_data", false); + node_->declare_parameter(IMAGE_TRANSPORT_PARAM, "raw"); + + ui_.setupUi(this); + + xy_pos_label_->setVisible(false); + lat_lon_pos_label_->setVisible(false); + + ui_.statusbar->addPermanentWidget(xy_pos_label_); + ui_.statusbar->addPermanentWidget(lat_lon_pos_label_); + + spacer1_ = new QWidget(ui_.statusbar); + spacer1_-> setMaximumSize(22, 22); + spacer1_-> setMinimumSize(22, 22); + ui_.statusbar->addPermanentWidget(spacer1_); + + screenshot_button_ = new QPushButton(); + screenshot_button_->setMinimumSize(22, 22); + screenshot_button_->setMaximumSize(22, 22); + screenshot_button_->setIcon(QIcon(":/images/image-x-generic.png")); + screenshot_button_->setFlat(true); + screenshot_button_->setToolTip("Capture screenshot of display canvas"); + ui_.statusbar->addPermanentWidget(screenshot_button_); + + spacer2_ = new QWidget(ui_.statusbar); + spacer2_->setMaximumSize(22, 22); + spacer2_->setMinimumSize(22, 22); + ui_.statusbar->addPermanentWidget(spacer2_); + + rec_button_ = new QPushButton(); + rec_button_->setMinimumSize(22, 22); + rec_button_->setMaximumSize(22, 22); + rec_button_->setIcon(QIcon(":/images/media-record.png")); + rec_button_->setCheckable(true); + rec_button_->setFlat(true); + rec_button_->setToolTip("Start recording video of display canvas"); + ui_.statusbar->addPermanentWidget(rec_button_); + + stop_button_ = new QPushButton(); + stop_button_->setMinimumSize(22, 22); + stop_button_->setMaximumSize(22, 22); + stop_button_->setIcon(QIcon(":/images/media-playback-stop.png")); + stop_button_->setToolTip("Stop recording video of display canvas"); + stop_button_->setEnabled(false); + stop_button_->setFlat(true); + ui_.statusbar->addPermanentWidget(stop_button_); + + spacer3_ = new QWidget(ui_.statusbar); + spacer3_->setMaximumSize(22, 22); + spacer3_->setMinimumSize(22, 22); + ui_.statusbar->addPermanentWidget(spacer3_); + + recenter_button_ = new QPushButton(); + recenter_button_->setMinimumSize(22, 22); + recenter_button_->setMaximumSize(22, 22); + recenter_button_->setIcon(QIcon(":/images/arrow_in.png")); + recenter_button_->setToolTip("Reset the viewport to the default location and zoom level"); + recenter_button_->setFlat(true); + ui_.statusbar->addPermanentWidget(recenter_button_); + + ui_.statusbar->setVisible(true); + + QActionGroup* group = new QActionGroup(this); + + ui_.actionForce_720p->setActionGroup(group); + ui_.actionForce_480p->setActionGroup(group); + ui_.actionResizable->setActionGroup(group); + + ui_.targetframe->addItem(""); + + canvas_ = new MapCanvas(this); + setCentralWidget(canvas_); + + connect( + canvas_, + SIGNAL(Hover(double, double, double)), + this, + SLOT(Hover(double, double, double))); + connect(ui_.configs, SIGNAL(ItemsMoved()), this, SLOT(ReorderDisplays())); + connect(ui_.actionExit, SIGNAL(triggered()), this, SLOT(close())); + connect(ui_.actionClear, SIGNAL(triggered()), this, SLOT(ClearConfig())); + connect( + ui_.bg_color, + SIGNAL(colorEdited(const QColor &)), + this, + SLOT(SelectBackgroundColor(const QColor &))); + + connect(recenter_button_, SIGNAL(clicked()), this, SLOT(Recenter())); + connect(rec_button_, SIGNAL(toggled(bool)), this, SLOT(ToggleRecord(bool))); + connect(stop_button_, SIGNAL(clicked()), this, SLOT(StopRecord())); + connect(screenshot_button_, SIGNAL(clicked()), this, SLOT(Screenshot())); + connect(ui_.actionClear_History, SIGNAL(triggered()), this, SLOT(ClearHistory())); + + // Use a separate thread for writing video files so that it won't cause + // lag on the main thread. + // It's ok for the video writer to be a pointer that we instantiate here and + // then forget about; the worker thread will delete it when the thread exits. + vid_writer_ = new VideoWriter(); + vid_writer_->moveToThread(&video_thread_); + connect(&video_thread_, SIGNAL(finished()), vid_writer_, SLOT(deleteLater())); + connect(this, SIGNAL(FrameGrabbed(QImage)), vid_writer_, SLOT(processFrame(QImage))); + video_thread_.start(); + + image_transport_menu_ = new QMenu("Default Image Transport", ui_.menu_View); + ui_.menu_View->addMenu(image_transport_menu_); + + connect(image_transport_menu_, SIGNAL(aboutToShow()), this, SLOT(UpdateImageTransportMenu())); + + ui_.bg_color->setColor(background_); + canvas_->SetBackground(background_); +} + +Mapviz::~Mapviz() +{ + video_thread_.quit(); + video_thread_.wait(); +} + +rclcpp::Node::SharedPtr Mapviz::GetNode() +{ + return node_; +} + +void Mapviz::showEvent(QShowEvent* event) +{ + Initialize(); +} + +void Mapviz::closeEvent(QCloseEvent* event) +{ + AutoSave(); + + for (auto& display : plugins_) { + MapvizPluginPtr plugin = display.second; + canvas_->RemovePlugin(plugin); + } + + plugins_.clear(); +} + +void Mapviz::Initialize() +{ + if (!initialized_) { + if (is_standalone_) { + spin_timer_.start(30); + connect(&spin_timer_, SIGNAL(timeout()), this, SLOT(SpinOnce())); + } + + // Create a sub-menu that lists all available Image Transports + image_transport::ImageTransport it(node_); + std::vector transports = it.getLoadableTransports(); + QActionGroup* group = new QActionGroup(image_transport_menu_); + for (const auto& iter : transports) + { + QString transport = QString::fromStdString(iter).replace( + QString::fromStdString(IMAGE_TRANSPORT_PARAM) + "/", ""); + QAction* action = image_transport_menu_->addAction(transport); + action->setCheckable(true); + group->addAction(action); + } + + connect(group, SIGNAL(triggered(QAction*)), this, SLOT(SetImageTransport(QAction*))); + + tf_buf_ = std::make_shared(node_->get_clock()); + tf_buf_->setUsingDedicatedThread(true); + tf_ = std::make_shared(*tf_buf_, node_, false); + tf_manager_ = std::make_shared(node_); + try + { + tf_manager_->Initialize(tf_buf_); + } + catch (...) + { + RCLCPP_ERROR(node_->get_logger(), "Error initializing tf_manager"); + } + + loader_ = new pluginlib::ClassLoader( + "mapviz", "mapviz::MapvizPlugin"); + + std::vector plugins = loader_->getDeclaredClasses(); + for (const auto& plugin : plugins) { + RCLCPP_INFO(node_->get_logger(), "Found mapviz plugin: %s", plugin.c_str()); + } + + canvas_->InitializeTf(tf_buf_); + canvas_->SetFixedFrame(ui_.fixedframe->currentText().toStdString()); + canvas_->SetTargetFrame(ui_.targetframe->currentText().toStdString()); + + add_display_srv_ = node_->create_service( + "add_mapviz_display", + std::bind(&Mapviz::AddDisplay, + this, + std::placeholders::_1, + std::placeholders::_2)); + + QString default_path = GetDefaultConfigPath(); + + std::string config; + node_->get_parameter_or("config", config, default_path.toStdString()); + + bool auto_save; + node_->get_parameter_or("auto_save_backup", auto_save, true); + + Open(config); + + UpdateFrames(); + frame_timer_.start(1000); + connect(&frame_timer_, SIGNAL(timeout()), this, SLOT(UpdateFrames())); + + if (auto_save) { + save_timer_.start(10000); + connect(&save_timer_, SIGNAL(timeout()), this, SLOT(AutoSave())); + } + + connect(&record_timer_, SIGNAL(timeout()), this, SLOT(CaptureVideoFrame())); + + bool print_profile_data; + node_->get_parameter_or("print_profile_data", print_profile_data, false); + if (print_profile_data) { + profile_timer_.start(2000); + connect(&profile_timer_, SIGNAL(timeout()), this, SLOT(HandleProfileTimer())); + } + + setFocus(); // Set the main window as focused object, + // prevent other fields from obtaining focus at startup + + initialized_ = true; + } +} + +QString Mapviz::GetDefaultConfigPath() +{ + QProcessEnvironment env = QProcessEnvironment::systemEnvironment(); + QString default_path = QDir::homePath(); + if (env.contains(ROS_WORKSPACE_VAR)) { + // If the ROS_WORKSPACE environment variable is defined, try to read our + // config file out of that. If we can't read it, fall back to trying to + // read one from the user's home directory. + QString ws_path = env.value(ROS_WORKSPACE_VAR, default_path); + if (QFileInfo(ws_path + MAPVIZ_CONFIG_FILE).isReadable()) { + default_path = ws_path; + } else { + RCLCPP_WARN( + node_->get_logger(), + "Could not load config file from ROS_WORKSPACE at %s; trying home directory...", + ws_path.toStdString().c_str()); + } + } + default_path += MAPVIZ_CONFIG_FILE; + + return default_path; +} + +void Mapviz::SpinOnce() +{ + if (rclcpp::ok()) { + meas_spin_.start(); + rclcpp::spin_some(node_); + meas_spin_.stop(); + } else { + QApplication::exit(); + } +} + +void Mapviz::UpdateFrames() +{ + std::vector frames; + tf_buf_->_getFrameStrings(frames); + std::sort(frames.begin(), frames.end()); + + if ( + ui_.fixedframe->count() >= 0 && + static_cast(ui_.fixedframe->count()) == frames.size()) + { + bool changed = false; + for (size_t i = 0; i < frames.size(); i++) { + if (frames[i] != ui_.fixedframe->itemText(i).toStdString()) { + changed = true; + } + } + + if (!changed) { + return; + } + } + + updating_frames_ = true; + + std::string current_fixed = ui_.fixedframe->currentText().toStdString(); + + ui_.fixedframe->clear(); + for (const auto& frame : frames) { + ui_.fixedframe->addItem(frame.c_str()); + } + + if (!current_fixed.empty()) { + int index = ui_.fixedframe->findText(current_fixed.c_str()); + if (index < 0) { + ui_.fixedframe->addItem(current_fixed.c_str()); + } + + index = ui_.fixedframe->findText(current_fixed.c_str()); + ui_.fixedframe->setCurrentIndex(index); + } + + std::string current_target = ui_.targetframe->currentText().toStdString(); + + ui_.targetframe->clear(); + ui_.targetframe->addItem(""); + for (const auto& frame : frames) { + ui_.targetframe->addItem(frame.c_str()); + } + + if (!current_target.empty()) { + int index = ui_.targetframe->findText(current_target.c_str()); + if (index < 0) { + ui_.targetframe->addItem(current_target.c_str()); + } + + index = ui_.targetframe->findText(current_target.c_str()); + ui_.targetframe->setCurrentIndex(index); + } + + updating_frames_ = false; + + if (current_target != ui_.targetframe->currentText().toStdString()) { + TargetFrameSelected(ui_.targetframe->currentText()); + } + + if (current_fixed != ui_.fixedframe->currentText().toStdString()) { + FixedFrameSelected(ui_.fixedframe->currentText()); + } +} + +void Mapviz::Force720p(bool on) +{ + if (force_720p_ != on) { + force_720p_ = on; + + if (force_720p_) { + force_480p_ = false; + resizable_ = false; + } + + AdjustWindowSize(); + } +} + +void Mapviz::Force480p(bool on) +{ + if (force_480p_ != on) { + force_480p_ = on; + + if (force_480p_) { + force_720p_ = false; + resizable_ = false; + } + + AdjustWindowSize(); + } +} + +void Mapviz::SetResizable(bool on) +{ + if (resizable_ != on) { + resizable_ = on; + + if (resizable_) { + force_720p_ = false; + force_480p_ = false; + } + + AdjustWindowSize(); + } +} + +void Mapviz::AdjustWindowSize() +{ + canvas_->setSizePolicy(QSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred)); + setSizePolicy(QSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred)); + + this->setMinimumSize(QSize(100, 100)); + this->setMaximumSize(QSize(10000, 10000)); + + if (force_720p_) { + canvas_->setMinimumSize(1280, 720); + canvas_->setMaximumSize(1280, 720); + canvas_->setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed)); + adjustSize(); + this->setMaximumSize(this->sizeHint()); + this->setMinimumSize(this->sizeHint()); + setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed)); + } else if (force_480p_) { + canvas_->setMinimumSize(640, 480); + canvas_->setMaximumSize(640, 480); + canvas_->setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed)); + adjustSize(); + this->setMaximumSize(this->sizeHint()); + this->setMinimumSize(this->sizeHint()); + setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed)); + } else if (stop_button_->isEnabled()) { + canvas_->setMinimumSize(canvas_->width(), canvas_->height()); + canvas_->setMaximumSize(canvas_->width(), canvas_->height()); + canvas_->setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed)); + adjustSize(); + this->setMaximumSize(this->sizeHint()); + this->setMinimumSize(this->sizeHint()); + setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed)); + } else { + canvas_->setMinimumSize(100, 100); + canvas_->setMaximumSize(10000, 10000); + } +} + +void Mapviz::Open(const std::string& filename) +{ + RCLCPP_INFO(node_->get_logger(), "Loading configuration from %s", filename.c_str()); + + std::string title; + size_t last_slash = filename.find_last_of('/'); + if (last_slash != std::string::npos && last_slash != filename.size() - 1) { + title = filename.substr(last_slash + 1) + " (" + + filename.substr(0, last_slash + 1) + ")"; + } else { + title = filename; + } + + title += " - mapviz"; + setWindowTitle(QString::fromStdString(title)); + + YAML::Node doc = YAML::LoadFile(filename); + if (!doc) { + RCLCPP_ERROR(node_->get_logger(), "Failed to load file: %s", filename.c_str()); + return; + } + + std::vector failed_plugins; + + try + { + boost::filesystem::path filepath(filename); + std::string config_path = filepath.parent_path().string(); + + ClearDisplays(); + + if (doc["capture_directory"]) { + capture_directory_ = doc["capture_directory"].as(); + } + + if (doc["fixed_frame"]) { + std::string fixed_frame = doc["fixed_frame"].as(); + ui_.fixedframe->setEditText(fixed_frame.c_str()); + } + + if (doc["target_frame"]) { + std::string target_frame = doc["target_frame"].as(); + ui_.targetframe->setEditText(target_frame.c_str()); + } + + if (doc["fix_orientation"]) { + bool fix_orientation = doc["fix_orientation"].as(); + ui_.actionFix_Orientation->setChecked(fix_orientation); + } + + if (doc["rotate_90"]) { + bool rotate_90 = doc["rotate_90"].as(); + ui_.actionRotate_90->setChecked(rotate_90); + } + + if (doc["enable_antialiasing"]) { + bool enable_antialiasing = doc["enable_antialiasing"].as(); + ui_.actionEnable_Antialiasing->setChecked(enable_antialiasing); + } + + if (doc["show_displays"]) { + bool show_displays = doc["show_displays"].as(); + ui_.actionConfig_Dock->setChecked(show_displays); + } + + if (doc["show_capture_tools"]) { + bool show_capture_tools = doc["show_capture_tools"].as(); + ui_.actionShow_Capture_Tools->setChecked(show_capture_tools); + } + + if (doc["show_status_bar"]) { + bool show_status_bar = doc["show_status_bar"].as(); + ui_.actionShow_Status_Bar->setChecked(show_status_bar); + } + + if (doc["show_capture_tools"]) { + bool show_capture_tools = doc["show_capture_tools"].as(); + ui_.actionShow_Capture_Tools->setChecked(show_capture_tools); + } + + if (doc["window_width"]) { + int window_width = doc["window_width"].as(); + resize(window_width, height()); + } + + if (doc["window_height"]) { + int window_height = doc["window_height"].as(); + resize(width(), window_height); + } + + if (doc["view_scale"]) { + float scale = doc["view_scale"].as(); + canvas_->SetViewScale(scale); + } + + if (doc["offset_x"]) { + float x = doc["offset_x"].as(); + canvas_->SetOffsetX(x); + } + + if (doc["offset_y"]) { + float y = doc["offset_y"].as(); + canvas_->SetOffsetY(y); + } + + if (doc["force_720p"]) { + bool force_720p = doc["force_720p"].as(); + + if (force_720p) { + ui_.actionForce_720p->setChecked(true); + } + } + + if (doc["force_480p"]) { + bool force_480p = doc["force_480p"].as(); + + if (force_480p) { + ui_.actionForce_480p->setChecked(true); + } + } + + if (doc[IMAGE_TRANSPORT_PARAM]) { + std::string image_transport = doc[IMAGE_TRANSPORT_PARAM].as(); + + node_->set_parameter({IMAGE_TRANSPORT_PARAM, image_transport}); + } + + bool use_latest_transforms = true; + if (doc["use_latest_transforms"]) { + use_latest_transforms = doc["use_latest_transforms"].as(); + } + ui_.uselatesttransforms->setChecked(use_latest_transforms); + canvas_->ToggleUseLatestTransforms(use_latest_transforms); + + if (doc["background"]) { + std::string color = doc["background"].as(); + background_ = QColor(color.c_str()); + ui_.bg_color->setColor(background_); + canvas_->SetBackground(background_); + } + + if (doc["displays"]) { + const YAML::Node& displays = doc["displays"]; + for (const auto& display : displays) { + std::string type = display["type"].as(); + std::string name = display["name"].as(); + + const YAML::Node& config = display["config"]; + + bool visible = config["visible"].as(); + + bool collapsed = config["collapsed"].as(); + + try + { + MapvizPluginPtr plugin = + CreateNewDisplay(name, type, visible, collapsed); + plugin->LoadConfig(config, config_path); + plugin->DrawIcon(); + } + catch (const pluginlib::LibraryLoadException& e) + { + failed_plugins.push_back(type); + RCLCPP_ERROR(node_->get_logger(), "%s", e.what()); + } + } + } + } + catch (const YAML::ParserException& e) + { + RCLCPP_ERROR(node_->get_logger(), "%s", e.what()); + return; + } + catch (const YAML::Exception& e) + { + RCLCPP_ERROR(node_->get_logger(), "%s", e.what()); + return; + } + + if (!failed_plugins.empty()) { + std::stringstream message; + message << "The following plugin(s) failed to load:" << std::endl; + std::string failures = boost::algorithm::join(failed_plugins, "\n"); + message << failures << std::endl << std::endl << "Check the ROS log for more details."; + + QMessageBox::warning(this, "Failed to load plugins", QString::fromStdString(message.str())); + } +} + +void Mapviz::Save(const std::string& filename) +{ + std::ofstream fout(filename.c_str()); + if (fout.fail()) { + RCLCPP_ERROR(node_->get_logger(), "Failed to open file: %s", filename.c_str()); + return; + } + + boost::filesystem::path filepath(filename); + std::string config_path = filepath.parent_path().string(); + + YAML::Emitter out; + + out << YAML::BeginMap; + out << YAML::Key << "capture_directory" << YAML::Value << capture_directory_; + out << YAML::Key << "fixed_frame" << YAML::Value << ui_.fixedframe->currentText().toStdString(); + out << YAML::Key << "target_frame" << YAML::Value << ui_.targetframe->currentText().toStdString(); + out << YAML::Key << "fix_orientation" << YAML::Value << ui_.actionFix_Orientation->isChecked(); + out << YAML::Key << "rotate_90" << YAML::Value << ui_.actionRotate_90->isChecked(); + out << YAML::Key + << "enable_antialiasing" + << YAML::Value + << ui_.actionEnable_Antialiasing->isChecked(); + out << YAML::Key + << "show_displays" + << YAML::Value + << ui_.actionConfig_Dock->isChecked(); + out << YAML::Key << "show_status_bar" << YAML::Value << ui_.actionShow_Status_Bar->isChecked(); + out << YAML::Key + << "show_capture_tools" + << YAML::Value + << ui_.actionShow_Capture_Tools->isChecked(); + out << YAML::Key << "window_width" << YAML::Value << width(); + out << YAML::Key << "window_height" << YAML::Value << height(); + out << YAML::Key << "view_scale" << YAML::Value << canvas_->ViewScale(); + out << YAML::Key << "offset_x" << YAML::Value << canvas_->OffsetX(); + out << YAML::Key << "offset_y" << YAML::Value << canvas_->OffsetY(); + out << YAML::Key + << "use_latest_transforms" + << YAML::Value + << ui_.uselatesttransforms->isChecked(); + out << YAML::Key << "background" << YAML::Value << background_.name().toStdString(); + std::string image_transport; + if (node_->get_parameter(IMAGE_TRANSPORT_PARAM, image_transport)) { + out << YAML::Key << IMAGE_TRANSPORT_PARAM << YAML::Value << image_transport; + } + + if (force_720p_) { + out << YAML::Key << "force_720p" << YAML::Value << force_720p_; + } + + if (force_480p_) { + out << YAML::Key << "force_480p" << YAML::Value << force_480p_; + } + + if (ui_.configs->count() > 0) { + out << YAML::Key << "displays"<< YAML::Value << YAML::BeginSeq; + + for (int i = 0; i < ui_.configs->count(); i++) { + out << YAML::BeginMap; + out << YAML::Key << "type" << YAML::Value << plugins_[ui_.configs->item(i)]->Type(); + out << YAML::Key + << "name" + << YAML::Value + << (dynamic_cast(ui_.configs->itemWidget(ui_.configs->item(i)))) + ->Name().toStdString(); + out << YAML::Key << "config" << YAML::Value; + out << YAML::BeginMap; + + out << YAML::Key << "visible" << YAML::Value << plugins_[ui_.configs->item(i)]->Visible(); + out << YAML::Key + << "collapsed" + << YAML::Value + << (dynamic_cast(ui_.configs->itemWidget(ui_.configs->item(i))))->Collapsed(); + + plugins_[ui_.configs->item(i)]->SaveConfig(out, config_path); + + out << YAML::EndMap; + out << YAML::EndMap; + } + + out << YAML::EndSeq; + } + + out << YAML::EndMap; + + fout << out.c_str(); + fout.close(); +} + +void Mapviz::AutoSave() +{ + QProcessEnvironment env = QProcessEnvironment::systemEnvironment(); + QString default_path = QDir::homePath(); + + if (env.contains(ROS_WORKSPACE_VAR)) { + // Try to save our config in the ROS_WORKSPACE directory, but if we can't write + // to that -- probably because it is read-only -- try to use the home directory + // instead. + QString ws_path = env.value(ROS_WORKSPACE_VAR, default_path); + QString ws_file = ws_path + MAPVIZ_CONFIG_FILE; + QFileInfo file_info(ws_file); + QFileInfo dir_info(ws_path); + if ( + (!file_info.exists() && dir_info.isWritable()) || + file_info.isWritable()) + { + // Note that FileInfo::isWritable will return false if a file does not exist, so + // we need to check both if the target file is writable and if the target dir is + // writable if the file doesn't exist. + default_path = ws_path; + } else { + RCLCPP_WARN(node_->get_logger(), + "Could not write config file to %s. Trying home directory.", + (ws_path + MAPVIZ_CONFIG_FILE).toStdString().c_str()); + } + } + default_path += MAPVIZ_CONFIG_FILE; + + + Save(default_path.toStdString()); +} + +void Mapviz::OpenConfig() +{ + QFileDialog dialog(this, "Select Config File"); + dialog.setFileMode(QFileDialog::ExistingFile); + dialog.setNameFilter(tr("Mapviz Config Files (*.mvc)")); + + dialog.exec(); + + if (dialog.result() == QDialog::Accepted && dialog.selectedFiles().count() == 1) { + std::string path = dialog.selectedFiles().first().toStdString(); + Open(path); + } +} + +void Mapviz::ClearConfig() +{ + ClearDisplays(); +} + +void Mapviz::SaveConfig() +{ + QFileDialog dialog(this, "Save Config File"); + dialog.setFileMode(QFileDialog::AnyFile); + dialog.setAcceptMode(QFileDialog::AcceptSave); + dialog.setNameFilter(tr("Mapviz Config Files (*.mvc)")); + dialog.setDefaultSuffix("mvc"); + + dialog.exec(); + + if (dialog.result() == QDialog::Accepted && dialog.selectedFiles().count() == 1) { + std::string path = dialog.selectedFiles().first().toStdString(); + + std::string title; + size_t last_slash = path.find_last_of('/'); + if (last_slash != std::string::npos && last_slash != path.size() - 1) { + title = path.substr(last_slash + 1) + " (" + + path.substr(0, last_slash + 1) + ")"; + } else { + title = path; + } + title += " - mapviz"; + setWindowTitle(QString::fromStdString(title)); + Save(path); + } +} + +void Mapviz::ClearHistory() +{ + RCLCPP_DEBUG(node_->get_logger(), "Mapviz::ClearHistory()"); + for (auto& plugin : plugins_) { + plugin.second->ClearHistory(); + } +} + +void Mapviz::SelectNewDisplay() +{ + RCLCPP_INFO(rclcpp::get_logger("mapviz"), "Select new display ..."); + QDialog dialog; + Ui::pluginselect ui; + ui.setupUi(&dialog); + + std::vector plugins = loader_->getDeclaredClasses(); + std::map plugin_types; + for (const auto& plugin : plugins) { + QString type(plugin.c_str()); + type = type.split('/').last(); + ui.displaylist->addItem(type); + plugin_types[type.toStdString()] = plugin; + } + ui.displaylist->setCurrentRow(0); + + dialog.exec(); + + if (dialog.result() == QDialog::Accepted) { + std::string type_name = ui.displaylist->selectedItems().first()->text().toStdString(); + std::string type = plugin_types[type_name]; + std::string name = "new display"; + try + { + CreateNewDisplay(name, type, true, false); + } + catch (const pluginlib::LibraryLoadException& e) + { + std::stringstream message; + message << "Unable to load " << type << "." << std::endl + << "Check the ROS log for more details."; + QMessageBox::warning(this, "Plugin failed to load", QString::fromStdString(message.str())); + RCLCPP_ERROR(node_->get_logger(), "%s", e.what()); + } + } +} + +void Mapviz::AddDisplay( + const mapviz_interfaces::srv::AddMapvizDisplay::Request::SharedPtr req, + mapviz_interfaces::srv::AddMapvizDisplay::Response::SharedPtr resp) +{ + std::map properties; + for (auto& property : req->properties) { + properties[property.key] = property.value; + } + + YAML::Node config; + for (auto& property_pair : properties) { + config[property_pair.first] = property_pair.second; + } + + if (!config) { + // ROS_ERROR("Failed to parse properties into YAML."); + RCLCPP_ERROR(node_->get_logger(), "Failed to parse properties into YAML."); + resp->success = false; + throw std::runtime_error("Failed to parse properties into YAML."); + } + + for (auto& display : plugins_) { + MapvizPluginPtr plugin = display.second; + if (!plugin) { + RCLCPP_ERROR(node_->get_logger(), "Invalid plugin ptr."); + continue; + } + + if (plugin->Name() == req->name && plugin->Type() == req->type) { + plugin->LoadConfig(config, ""); + plugin->SetVisible(req->visible); + + if (req->draw_order > 0) { + display.first->setData(Qt::UserRole, QVariant(req->draw_order - 1.1)); + ui_.configs->sortItems(); + + ReorderDisplays(); + } else if (req->draw_order < 0) { + display.first->setData( + Qt::UserRole, QVariant(ui_.configs->count() + req->draw_order + 0.1)); + ui_.configs->sortItems(); + + ReorderDisplays(); + } + + resp->success = true; + + return; + } + } + + try + { + MapvizPluginPtr plugin = + CreateNewDisplay(req->name, req->type, req->visible, false, req->draw_order); + plugin->LoadConfig(config, ""); + plugin->DrawIcon(); + resp->success = true; + } + catch (const pluginlib::LibraryLoadException& e) + { + RCLCPP_ERROR(node_->get_logger(), "%s", e.what()); + resp->success = false; + resp->message = "Failed to load display plug-in."; + } +} + +void Mapviz::Hover(double x, double y, double scale) +{ + if (ui_.statusbar->isVisible()) { + if (scale == 0) { + xy_pos_label_->setVisible(false); + lat_lon_pos_label_->setVisible(false); + return; + } + + int32_t precision = static_cast(std::ceil(std::max(0.0, std::log10(1.0 / scale)))); + + QString text = ui_.fixedframe->currentText(); + if (text.isEmpty() || text == "/") { + text = "fixed"; + } + text += ": "; + + std::ostringstream x_ss; + x_ss << std::fixed << std::setprecision(precision); + x_ss << x; + text += x_ss.str().c_str(); + + text += ", "; + + std::ostringstream y_ss; + y_ss << std::fixed << std::setprecision(precision); + y_ss << y; + text += y_ss.str().c_str(); + + xy_pos_label_->setText(text); + xy_pos_label_->setVisible(true); + xy_pos_label_->update(); + + swri_transform_util::Transform transform; + if + ( + tf_manager_->SupportsTransform( + swri_transform_util::_wgs84_frame, + ui_.fixedframe->currentText().toStdString()) && + tf_manager_->GetTransform( + swri_transform_util::_wgs84_frame, + ui_.fixedframe->currentText().toStdString(), + transform)) + { + tf2::Vector3 point(x, y, 0); + point = transform * point; + + QString lat_lon_text = "lat/lon: "; + + double lat_scale = (1.0 / 111111.0) * scale; + int32_t lat_precision = static_cast( + std::ceil(std::max(0.0, std::log10(1.0 / lat_scale)))); + + std::ostringstream lat_ss; + lat_ss << std::fixed << std::setprecision(lat_precision); + lat_ss << point.y(); + lat_lon_text += lat_ss.str().c_str(); + + lat_lon_text += ", "; + + double lon_scale = (1.0 + / (111111.0 * std::cos(point.y() * swri_math_util::_deg_2_rad))) * scale; + int32_t lon_precision = static_cast( + std::ceil(std::max(0.0, std::log10(1.0 / lon_scale)))); + + std::ostringstream lon_ss; + lon_ss << std::fixed << std::setprecision(lon_precision); + lon_ss << point.x(); + lat_lon_text += lon_ss.str().c_str(); + + lat_lon_pos_label_->setText(lat_lon_text); + lat_lon_pos_label_->setVisible(true); + lat_lon_pos_label_->update(); + } else if (lat_lon_pos_label_->isVisible()) { + lat_lon_pos_label_->setVisible(false); + } + } +} + +MapvizPluginPtr Mapviz::CreateNewDisplay( + const std::string& name, + const std::string& type, + bool visible, + bool collapsed, + int draw_order) +{ + auto* config_item = new ConfigItem(); + + config_item->SetName(name.c_str()); + + std::string real_type = type; + if (real_type == "mapviz_plugins/mutlires_image") { + // The "multires_image" plugin was originally accidentally named "mutlires_image". + // Loading a mapviz config file that still has the old name would normally cause it + // to crash, so this will check for and correct it. + real_type = "mapviz_plugins/multires_image"; + } + + RCLCPP_INFO(node_->get_logger(), "creating: %s", real_type.c_str()); + MapvizPluginPtr plugin = loader_->createSharedInstance(real_type); + + // Setup configure widget + config_item->SetWidget(plugin->GetConfigWidget(this)); + plugin->SetIcon(config_item->ui_.icon); + plugin->SetType(real_type); + plugin->SetName(name); + plugin->SetNode(*node_); + plugin->Initialize(tf_buf_, tf_, tf_manager_, canvas_); + plugin->SetVisible(visible); + + if (draw_order == 0) { + plugin->SetDrawOrder(ui_.configs->count()); + } else if (draw_order > 0) { + plugin->SetDrawOrder(std::min(ui_.configs->count(), draw_order - 1)); + } else if (draw_order < 0) { + plugin->SetDrawOrder(std::max(0, ui_.configs->count() + draw_order + 1)); + } + + QString pretty_type(real_type.c_str()); + pretty_type = pretty_type.split('/').last(); + config_item->SetType(pretty_type); + QListWidgetItem* item = new PluginConfigListItem(); + config_item->SetListItem(item); + item->setSizeHint(config_item->sizeHint()); + connect(config_item, SIGNAL(UpdateSizeHint()), this, SLOT(UpdateSizeHints())); + connect( + config_item, + SIGNAL(ToggledDraw(QListWidgetItem*, bool)), + this, + SLOT(ToggleShowPlugin(QListWidgetItem*, bool))); + connect( + config_item, + SIGNAL(RemoveRequest(QListWidgetItem*)), + this, + SLOT(RemoveDisplay(QListWidgetItem*))); + connect(plugin.get(), SIGNAL(VisibleChanged(bool)), config_item, SLOT(ToggleDraw(bool))); + connect(plugin.get(), SIGNAL(SizeChanged()), this, SLOT(UpdateSizeHints())); + + if (real_type == "mapviz_plugins/image") { + // This is a little kludgey because we're relying on hard-coding a + // plugin type here... feel free to suggest a better way. + // If the default image transport has changed, we want to notify all of our + // image plugins of it so that they will resubscribe appropriately. + connect(this, SIGNAL(ImageTransportChanged()), + plugin.get(), SLOT(Resubscribe())); + } + + if (draw_order == 0) { + ui_.configs->addItem(item); + } else { + ui_.configs->insertItem(plugin->DrawOrder(), item); + } + + ui_.configs->setItemWidget(item, config_item); + ui_.configs->UpdateIndices(); + + // Add plugin to canvas + plugin->SetTargetFrame(ui_.fixedframe->currentText().toStdString()); + plugin->SetUseLatestTransforms(ui_.uselatesttransforms->isChecked()); + plugins_[item] = plugin; + canvas_->AddPlugin(plugin, -1); + + config_item->ToggleDraw(visible); + + if (collapsed) { + config_item->Hide(); + } + + ReorderDisplays(); + + return plugin; +} + +void Mapviz::ToggleShowPlugin(QListWidgetItem* item, bool visible) +{ + RCLCPP_INFO(node_->get_logger(), "Toggle show plugin"); + + if (plugins_.count(item) == 1) { + plugins_[item]->SetVisible(visible); + } + canvas_->UpdateView(); +} + +void Mapviz::FixedFrameSelected(const QString& text) +{ + if (!updating_frames_) { + RCLCPP_INFO( + node_->get_logger(), + "fixed frame selected: %s", + text.toStdString().c_str()); + if (canvas_ != nullptr) { + canvas_->SetFixedFrame(text.toStdString()); + } + } +} + +void Mapviz::TargetFrameSelected(const QString& text) +{ + if (!updating_frames_) { + RCLCPP_INFO( + node_->get_logger(), + "Target frame selected: %s", + text.toStdString().c_str()); + + if (canvas_ != nullptr) { + canvas_->SetTargetFrame(text.toStdString()); + } + } +} + +void Mapviz::ToggleUseLatestTransforms(bool on) +{ + canvas_->ToggleUseLatestTransforms(on); +} + +void Mapviz::ToggleFixOrientation(bool on) +{ + canvas_->ToggleFixOrientation(on); +} + +void Mapviz::ToggleRotate90(bool on) +{ + canvas_->ToggleRotate90(on); +} + +void Mapviz::ToggleEnableAntialiasing(bool on) +{ + canvas_->ToggleEnableAntialiasing(on); +} + +void Mapviz::ToggleConfigPanel(bool on) +{ + if (on) { + ui_.configdock->show(); + } else { + ui_.configdock->hide(); + } + + AdjustWindowSize(); +} + +void Mapviz::ToggleStatusBar(bool on) +{ + ui_.statusbar->setVisible(on); + + AdjustWindowSize(); +} + +void Mapviz::ToggleCaptureTools(bool on) +{ + if (on) { + ui_.actionShow_Status_Bar->setChecked(true); + } + + screenshot_button_->setVisible(on); + rec_button_->setVisible(on); + stop_button_->setVisible(on); + spacer1_->setVisible(on); + spacer2_->setVisible(on); + spacer3_->setVisible(on); +} + +void Mapviz::ToggleRecord(bool on) +{ + stop_button_->setEnabled(true); + + if (on) { + rec_button_->setIcon(QIcon(":/images/media-playback-pause.png")); + rec_button_->setToolTip("Pause recording video of display canvas"); + if (!vid_writer_->isRecording()) { + // Lock the window size. + AdjustWindowSize(); + + canvas_->CaptureFrames(true); + + std::string posix_time = boost::posix_time::to_iso_string( + boost::posix_time::second_clock::local_time()); + boost::replace_all(posix_time, ".", "_"); + std::string filename = capture_directory_ + "/mapviz_" + posix_time + ".avi"; + boost::replace_all(filename, "~", getenv("HOME")); + + + if (!vid_writer_->initializeWriter(filename, canvas_->width(), canvas_->height())) { + RCLCPP_ERROR(node_->get_logger(), "Failed to open video file for writing"); + StopRecord(); + return; + } + + RCLCPP_INFO(node_->get_logger(), "Writing video to: %s", filename.c_str()); + ui_.statusbar->showMessage("Recording video to " + QString::fromStdString(filename)); + + canvas_->updateGL(); + } + + record_timer_.start(1000.0 / 30.0); + } else { + rec_button_->setIcon(QIcon(":/images/media-record.png")); + rec_button_->setToolTip("Continue recording video of display canvas"); + record_timer_.stop(); + } +} + +void Mapviz::SetImageTransport(QAction* transport_action) +{ + std::string transport = transport_action->text().toStdString(); + RCLCPP_INFO( + node_->get_logger(), + "Setting %s to %s", + IMAGE_TRANSPORT_PARAM, + transport.c_str()); + node_->set_parameter({IMAGE_TRANSPORT_PARAM, transport}); + + Q_EMIT(ImageTransportChanged()); +} + +void Mapviz::UpdateImageTransportMenu() +{ + QList actions = image_transport_menu_->actions(); + + std::string current_transport; + node_->get_parameter_or(IMAGE_TRANSPORT_PARAM, current_transport, std::string("raw")); + for(const auto action : actions) + { + if (action->text() == QString::fromStdString(current_transport)) { + action->setChecked(true); + return; + } + } + + RCLCPP_WARN( + node_->get_logger(), + "%s param was set to an unrecognized value: %s", + IMAGE_TRANSPORT_PARAM, + current_transport.c_str()); +} + +void Mapviz::CaptureVideoFrame() +{ + // We need to store the data inside a QImage in order to emit it as a + // signal. + // Note that the QImage here is set to "ARGB32", but it is actually BGRA. + // Qt doesn't have a comparable BGR format, and the cv::VideoWriter this + // is going to expects BGR format, but it'd be a waste for us to convert + // to RGB and then back to BGR. + QImage frame(canvas_->width(), canvas_->height(), QImage::Format_ARGB32); + if (canvas_->CopyCaptureBuffer(frame.bits())) { + Q_EMIT(FrameGrabbed(frame)); + } else { + RCLCPP_ERROR(rclcpp::get_logger("mapviz"), "Failed to get capture buffer"); + } +} + +void Mapviz::Recenter() +{ + canvas_->ResetLocation(); +} + +void Mapviz::StopRecord() +{ + rec_button_->setChecked(false); + stop_button_->setEnabled(false); + + record_timer_.stop(); + if (vid_writer_) { + vid_writer_->stop(); + } + canvas_->CaptureFrames(false); + + ui_.statusbar->showMessage(QString("")); + rec_button_->setToolTip("Start recording video of display canvas"); + + AdjustWindowSize(); +} + +void Mapviz::Screenshot() +{ + canvas_->CaptureFrame(true); + + std::vector frame; + if (canvas_->CopyCaptureBuffer(frame)) { + cv::Mat image(canvas_->height(), canvas_->width(), CV_8UC4, &frame[0]); + cv::Mat screenshot; + cvtColor(image, screenshot, cv::COLOR_BGRA2BGR); + + cv::flip(screenshot, screenshot, 0); + + std::string posix_time = boost::posix_time::to_iso_string( + boost::posix_time::second_clock::local_time()); + boost::replace_all(posix_time, ".", "_"); + std::string filename = capture_directory_ + "/mapviz_" + posix_time + ".png"; + boost::replace_all(filename, "~", getenv("HOME")); + + RCLCPP_INFO(rclcpp::get_logger("mapviz"), "Writing screenshot to: %s", filename.c_str()); + ui_.statusbar->showMessage("Saved image to " + QString::fromStdString(filename)); + + cv::imwrite(filename, screenshot); + } else { + RCLCPP_ERROR(rclcpp::get_logger("mapviz"), "Failed to take screenshot."); + } +} + +void Mapviz::UpdateSizeHints() +{ + for (int i = 0; i < ui_.configs->count(); i++) { + QListWidgetItem* item = ui_.configs->item(i); + auto* widget = dynamic_cast(ui_.configs->itemWidget(item)); + if (widget) { + // Make sure the ConfigItem in the QListWidgetItem we're getting really + // exists; if this method is called before it's been initialized, it would + // cause a crash. + item->setSizeHint(widget->sizeHint()); + } + } +} + +void Mapviz::RemoveDisplay() +{ + QListWidgetItem* item = ui_.configs->takeItem(ui_.configs->currentRow()); + RemoveDisplay(item); +} + +void Mapviz::RemoveDisplay(QListWidgetItem* item) +{ + RCLCPP_INFO(rclcpp::get_logger("mapviz"), "Remove display ..."); + + if (item) { + canvas_->RemovePlugin(plugins_[item]); + plugins_.erase(item); + + delete item; + } +} + +void Mapviz::ClearDisplays() +{ + while (ui_.configs->count() > 0) { + RCLCPP_INFO(node_->get_logger(), "Remove display ..."); + + QListWidgetItem* item = ui_.configs->takeItem(0); + + canvas_->RemovePlugin(plugins_[item]); + plugins_.erase(item); + + delete item; + } +} + +void Mapviz::ReorderDisplays() +{ + RCLCPP_INFO(rclcpp::get_logger("mapviz"), "Reorder displays"); + for (int i = 0; i < ui_.configs->count(); i++) { + plugins_[ui_.configs->item(i)]->SetDrawOrder(i); + } + canvas_->ReorderDisplays(); +} + +void Mapviz::SelectBackgroundColor(const QColor &color) +{ + background_ = color; + canvas_->SetBackground(background_); +} + +void Mapviz::SetCaptureDirectory() +{ + QFileDialog dialog(this, "Select Capture Directory"); + dialog.setOption(QFileDialog::ShowDirsOnly, true); + + dialog.exec(); + + if (dialog.result() == QDialog::Accepted && dialog.selectedFiles().count() == 1) { + capture_directory_ = dialog.selectedFiles().first().toStdString(); + } +} + +void Mapviz::HandleProfileTimer() +{ + RCLCPP_INFO(node_->get_logger(), "Mapviz Profiling Data"); + meas_spin_.printInfo(node_->get_logger(), "ROS SpinOnce()"); + for (auto& display : plugins_) { + MapvizPluginPtr plugin = display.second; + if (plugin) { + plugin->PrintMeasurements(); + } + } +} +} // namespace mapviz diff --git a/mapviz/src/mapviz.ui b/mapviz/src/mapviz.ui new file mode 100644 index 000000000..f0b89fa14 --- /dev/null +++ b/mapviz/src/mapviz.ui @@ -0,0 +1,814 @@ + + + mapviz + + + + 0 + 0 + 600 + 600 + + + + + 0 + 0 + + + + + 600 + 400 + + + + mapviz + + + + + 0 + 0 + 600 + 27 + + + + + &File + + + + + + + + + + + + &View + + + + + + + + + + + + + + + + + Data + + + + + + + + + + true + + + + + + 332 + 301 + + + + QDockWidget::DockWidgetMovable + + + Qt::LeftDockWidgetArea|Qt::RightDockWidgetArea + + + Config + + + 1 + + + + + 0 + + + 2 + + + 0 + + + 0 + + + 0 + + + + + + + + + 85 + 16777215 + + + + + Sans Serif + 9 + + + + Fixed Frame: + + + + + + + + 85 + 16777215 + + + + + Sans Serif + 9 + + + + Target Frame: + + + + + + + + 85 + 16777215 + + + + + Sans Serif + 9 + + + + Background: + + + + + + + + 24 + 24 + + + + Set the background color + + + false + + + + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 25 + + + + + Sans Serif + 9 + + + + The reference frame for the camera view + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 25 + + + + + Sans Serif + 9 + + + + The reference frame used to denote the "world" frame + + + + true + + + + + + + Use the current time when transforming data instead of using the + timestamps associated with the data + + + + Qt::LeftToRight + + + Use Latest Transforms + + + true + + + + + + + + + + Qt::ScrollBarAsNeeded + + + true + + + QAbstractItemView::InternalMove + + + Qt::MoveAction + + + QAbstractItemView::ScrollPerPixel + + + + + + + + 4 + + + 4 + + + + + + 80 + 16777215 + + + + Add a new display + + + Add + + + + + + + + 80 + 16777215 + + + + Remove the selected display + + + Remove + + + + + + + + + + + + Exit + + + + + Open Config + + + + + Save Config + + + + + true + + + true + + + Show Config Panel + + + Show the display configuration panel + + + + + true + + + Fix Orientation + + + Fix the orientation of the camera + + + + + true + + + Force 720p + + + Lock the display canvas to 720p + + + + + true + + + Force 480p + + + Lock the display canvas to 480p + + + + + true + + + true + + + Resizable + + + Make the window resizable + + + + + Set Capture Directory + + + Set the capture directory for screeshots and videos + + + + + true + + + true + + + Show Status Bar + + + Show the status bar + + + + + true + + + true + + + Show Capture Tools + + + Show the capture tools on the status bar + + + + + true + + + Rotate 90° + + + Rotate the camera by 90 degrees + + + + + true + + + true + + + Enable Antialiasing + + + Enable antialiasing on the GL surface + + + + + Image Transport + + + + + Clear History + + + + + Clear Config + + + + + + mapviz::PluginConfigList + QListWidget +
mapviz/widgets.h
+ + + mapviz::ColorButton + QPushButton +
mapviz/color_button.h
+ + + + fixedframe + targetframe + uselatesttransforms + bg_color + configs + addbutton + removebutton + + + + + + + addbutton + clicked() + mapviz + SelectNewDisplay() + + + 107 + 573 + + + 327 + 471 + + + + + removebutton + clicked() + mapviz + RemoveDisplay() + + + 224 + 573 + + + 328 + 407 + + + + + actionConfig_Dock + toggled(bool) + mapviz + ToggleConfigPanel(bool) + + + -1 + -1 + + + 399 + 299 + + + + + fixedframe + editTextChanged(QString) + mapviz + FixedFrameSelected(QString) + + + 185 + 62 + + + 428 + 192 + + + + + targetframe + editTextChanged(QString) + mapviz + TargetFrameSelected(QString) + + + 206 + 94 + + + 428 + 330 + + + + + actionFix_Orientation + toggled(bool) + mapviz + ToggleFixOrientation(bool) + + + -1 + -1 + + + 399 + 299 + + + + + actionOpen_config + triggered() + mapviz + OpenConfig() + + + -1 + -1 + + + 399 + 299 + + + + + actionSave_config + triggered() + mapviz + SaveConfig() + + + -1 + -1 + + + 399 + 299 + + + + + actionForce_720p + toggled(bool) + mapviz + Force720p(bool) + + + -1 + -1 + + + 399 + 299 + + + + + actionForce_480p + toggled(bool) + mapviz + Force480p(bool) + + + -1 + -1 + + + 399 + 299 + + + + + actionResizable + toggled(bool) + mapviz + SetResizable(bool) + + + -1 + -1 + + + 399 + 299 + + + + + uselatesttransforms + toggled(bool) + mapviz + ToggleUseLatestTransforms(bool) + + + 58 + 130 + + + 460 + 242 + + + + + actionSet_Capture_Directory + triggered() + mapviz + SetCaptureDirectory() + + + -1 + -1 + + + 354 + 299 + + + + + actionShow_Status_Bar + toggled(bool) + mapviz + ToggleStatusBar(bool) + + + -1 + -1 + + + 354 + 299 + + + + + actionShow_Capture_Tools + triggered(bool) + mapviz + ToggleCaptureTools(bool) + + + -1 + -1 + + + 354 + 299 + + + + + actionRotate_90 + toggled(bool) + mapviz + ToggleRotate90(bool) + + + -1 + -1 + + + 354 + 299 + + + + + actionEnable_Antialiasing + toggled(bool) + mapviz + ToggleEnableAntialiasing(bool) + + + -1 + -1 + + + 399 + 299 + + + + + + SelectNewDisplay() + RemoveDisplay() + ToggleConfigPanel(bool) + FixedFrameSelected(QString) + TargetFrameSelected(QString) + ToggleFixOrientation(bool) + ToggleEnableAntialiasing(bool) + MoveDisplay(QModelIndexList) + OpenConfig() + SaveConfig() + Force720p(bool) + Force480p(bool) + SetResizable(bool) + ToggleUseLatestTransforms(bool) + SetCaptureDirectory() + ToggleStatusBar(bool) + ToggleCaptureTools(bool) + ToggleRotate90(bool) + + diff --git a/mapviz/src/mapviz_application.cpp b/mapviz/src/mapviz_application.cpp new file mode 100644 index 000000000..9c278184d --- /dev/null +++ b/mapviz/src/mapviz_application.cpp @@ -0,0 +1,64 @@ +// ***************************************************************************** +// +// Copyright (c) 2015, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL Southwest Research Institute® BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY +// OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +// DAMAGE. +// +// ***************************************************************************** + +#include "mapviz/mapviz_application.h" + +#include "rclcpp/rclcpp.hpp" + +namespace mapviz +{ + MapvizApplication::MapvizApplication(int& argc, char** argv, rclcpp::Logger logger) : + QApplication(argc, argv), + logger_(logger) + { + } + + bool MapvizApplication::notify(QObject* receiver, QEvent* event) + { + try { + return QApplication::notify(receiver, event); + } + catch (const rclcpp::exceptions::RCLError& e) { + RCLCPP_ERROR(logger_, + "Unhandled RCLError in Qt event loop: %s", e.what()); + } + catch (const std::exception& e) { + RCLCPP_ERROR(logger_, + "Unhandled std::exception in Qt event loop: %s", e.what()); + } + + return false; + } + + void MapvizApplication::setLogger(const rclcpp::Logger &logger) + { + logger_ = logger; + } +} // namespace mapviz diff --git a/mapviz/src/mapviz_main.cpp b/mapviz/src/mapviz_main.cpp new file mode 100644 index 000000000..c5c81deee --- /dev/null +++ b/mapviz/src/mapviz_main.cpp @@ -0,0 +1,54 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include "mapviz/mapviz.hpp" +#include "mapviz/mapviz_application.h" +#include + +int main(int argc, char **argv) +{ + // Initialize ROS; spinning on the Node is handled in mapviz.cpp + rclcpp::init(argc, argv); + + // Initialize Qt resources + Q_INIT_RESOURCE(icons); + + // Initialize QT + mapviz::MapvizApplication app(argc, argv); + + // Initialize glut (for displaying text) + glutInit(&argc, argv); + + // Start mapviz + mapviz::Mapviz mapviz(true, argc, argv); + app.setLogger(mapviz.GetNode()->get_logger()); + mapviz.show(); + + return mapviz::MapvizApplication::exec(); +} diff --git a/mapviz/src/pluginselect.ui b/mapviz/src/pluginselect.ui new file mode 100644 index 000000000..0086d7fa1 --- /dev/null +++ b/mapviz/src/pluginselect.ui @@ -0,0 +1,90 @@ + + + pluginselect + + + + 0 + 0 + 400 + 300 + + + + Select New Display + + + true + + + + + + true + + + + + + + Qt::Vertical + + + QDialogButtonBox::Cancel|QDialogButtonBox::Ok + + + + + + + + + buttonBox + accepted() + pluginselect + accept() + + + 306 + 243 + + + 157 + 274 + + + + + buttonBox + rejected() + pluginselect + reject() + + + 316 + 260 + + + 286 + 274 + + + + + displaylist + doubleClicked(QModelIndex) + pluginselect + accept() + + + 162 + 114 + + + 209 + 294 + + + + + diff --git a/mapviz/src/resources/LICENSE b/mapviz/src/resources/LICENSE new file mode 100644 index 000000000..7989b1224 --- /dev/null +++ b/mapviz/src/resources/LICENSE @@ -0,0 +1,9 @@ +The Tango base icon theme is released to the Public Domain. + +http://tango.freedesktop.org/ + +--- + +The Silk icon theme is licensed under the Creative Commons Attribution 2.4 License. + +http://www.famfamfam.com/lab/icons/silk/ diff --git a/mapviz/src/resources/arrow_in.png b/mapviz/src/resources/arrow_in.png new file mode 100644 index 000000000..745c65134 Binary files /dev/null and b/mapviz/src/resources/arrow_in.png differ diff --git a/mapviz/src/resources/green-arrow.png b/mapviz/src/resources/green-arrow.png new file mode 100644 index 000000000..c0ab1fabe Binary files /dev/null and b/mapviz/src/resources/green-arrow.png differ diff --git a/mapviz/src/resources/icons.qrc b/mapviz/src/resources/icons.qrc new file mode 100644 index 000000000..bef973684 --- /dev/null +++ b/mapviz/src/resources/icons.qrc @@ -0,0 +1,11 @@ + + + arrow_in.png + image-x-generic.png + media-playback-pause.png + media-playback-stop.png + media-record.png + remove-icon-th.png + green-arrow.png + + diff --git a/mapviz/src/resources/image-x-generic.png b/mapviz/src/resources/image-x-generic.png new file mode 100644 index 000000000..68da5027c Binary files /dev/null and b/mapviz/src/resources/image-x-generic.png differ diff --git a/mapviz/src/resources/media-playback-pause.png b/mapviz/src/resources/media-playback-pause.png new file mode 100644 index 000000000..c8b4fe225 Binary files /dev/null and b/mapviz/src/resources/media-playback-pause.png differ diff --git a/mapviz/src/resources/media-playback-stop.png b/mapviz/src/resources/media-playback-stop.png new file mode 100644 index 000000000..ede2815e5 Binary files /dev/null and b/mapviz/src/resources/media-playback-stop.png differ diff --git a/mapviz/src/resources/media-record.png b/mapviz/src/resources/media-record.png new file mode 100644 index 000000000..2f66cdebb Binary files /dev/null and b/mapviz/src/resources/media-record.png differ diff --git a/mapviz/src/resources/remove-icon-th.png b/mapviz/src/resources/remove-icon-th.png new file mode 100644 index 000000000..d58c33f7b Binary files /dev/null and b/mapviz/src/resources/remove-icon-th.png differ diff --git a/mapviz/src/rqt_mapviz.cpp b/mapviz/src/rqt_mapviz.cpp new file mode 100644 index 000000000..67fd75f3d --- /dev/null +++ b/mapviz/src/rqt_mapviz.cpp @@ -0,0 +1,66 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include "mapviz/rqt_mapviz.h" +#include + +namespace mapviz +{ + + RqtMapviz::RqtMapviz() : + widget_(nullptr) + { + setObjectName("RqtMapviz"); + } + + void RqtMapviz::initPlugin(qt_gui_cpp::PluginContext& context) + { + // The plugin class doesn't really do very much -- just start Mapviz + // and add it to the context. + widget_ = new Mapviz(false, 0, nullptr); + widget_->setWindowFlags(Qt::Widget); + context.addWidget(widget_); + } + + void RqtMapviz::shutdownPlugin() + { + } + + void RqtMapviz::saveSettings(qt_gui_cpp::Settings& plugin_settings, + qt_gui_cpp::Settings& instance_settings) const + { + } + + void RqtMapviz::restoreSettings(const qt_gui_cpp::Settings& plugin_settings, + const qt_gui_cpp::Settings& instance_settings) + { + } +} // namespace mapviz + +PLUGINLIB_EXPORT_CLASS(mapviz::RqtMapviz, rqt_gui_cpp::Plugin) diff --git a/mapviz/src/select_frame_dialog.cpp b/mapviz/src/select_frame_dialog.cpp new file mode 100644 index 000000000..e41c2402d --- /dev/null +++ b/mapviz/src/select_frame_dialog.cpp @@ -0,0 +1,257 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + + +namespace mapviz +{ +std::string SelectFrameDialog::selectFrame( + std::shared_ptr tf_buffer, + QWidget *parent) +{ + SelectFrameDialog dialog(tf_buffer, parent); + dialog.allowMultipleFrames(false); + if (dialog.exec() == QDialog::Accepted) { + return dialog.selectedFrame(); + } else { + return ""; + } +} + +std::vector SelectFrameDialog::selectFrames( + std::shared_ptr tf_buffer, + QWidget *parent) +{ + SelectFrameDialog dialog(tf_buffer, parent); + dialog.allowMultipleFrames(true); + if (dialog.exec() == QDialog::Accepted) { + return dialog.selectedFrames(); + } else { + return std::vector(); + } +} + +SelectFrameDialog::SelectFrameDialog( + std::shared_ptr tf_buffer, + QWidget *parent) + : QDialog(parent) + , tf_buf_(tf_buffer) + , ok_button_(new QPushButton("&Ok")) + , cancel_button_(new QPushButton("&Cancel")) + , list_widget_(new QListWidget()) + , name_filter_(new QLineEdit()) +{ + QHBoxLayout *filter_box = new QHBoxLayout(); + filter_box->addWidget(new QLabel("Filter:")); + filter_box->addWidget(name_filter_); + + QHBoxLayout *button_box = new QHBoxLayout(); + button_box->addStretch(1); + button_box->addWidget(cancel_button_); + button_box->addWidget(ok_button_); + + QVBoxLayout *vbox = new QVBoxLayout(); + vbox->addWidget(list_widget_); + vbox->addLayout(filter_box); + vbox->addLayout(button_box); + setLayout(vbox); + + connect(ok_button_, SIGNAL(clicked(bool)), + this, SLOT(accept())); + connect(cancel_button_, SIGNAL(clicked(bool)), + this, SLOT(reject())); + connect(name_filter_, SIGNAL(textChanged(const QString &)), + this, SLOT(updateDisplayedFrames())); + + ok_button_->setDefault(true); + + allowMultipleFrames(false); + setWindowTitle("Select frames..."); + + resize(600, 600); + + fetch_frames_timer_id_ = startTimer(1000); + fetchFrames(); +} + +void SelectFrameDialog::timerEvent(QTimerEvent *event) +{ + if (event->timerId() == fetch_frames_timer_id_) { + fetchFrames(); + } +} + +void SelectFrameDialog::closeEvent(QCloseEvent *event) +{ + // We don't need to keep making requests from the ROS master. + killTimer(fetch_frames_timer_id_); + QDialog::closeEvent(event); +} + +void SelectFrameDialog::allowMultipleFrames( + bool allow) +{ + if (allow) { + list_widget_->setSelectionMode(QAbstractItemView::MultiSelection); + } else { + list_widget_->setSelectionMode(QAbstractItemView::SingleSelection); + } +} + +std::string SelectFrameDialog::selectedFrame() const +{ + std::vector selection = selectedFrames(); + if (selection.empty()) { + return ""; + } else { + return selection.front(); + } +} + +std::vector SelectFrameDialog::selectedFrames() const +{ + QModelIndexList qt_selection = list_widget_->selectionModel()->selectedIndexes(); + + std::vector selection; + selection.resize(qt_selection.size()); + for (int i = 0; i < qt_selection.size(); i++) { + if (!qt_selection[i].isValid()) { + continue; + } + + int row = qt_selection[i].row(); + if (row < 0 || static_cast(row) >= displayed_frames_.size()) { + continue; + } + + selection[i] = displayed_frames_[row]; + } + + return selection; +} + +void SelectFrameDialog::fetchFrames() +{ + if (tf_buf_ == nullptr) { + return; + } + + known_frames_.clear(); + tf_buf_->_getFrameStrings(known_frames_); + std::sort(known_frames_.begin(), known_frames_.end()); + updateDisplayedFrames(); +} + +std::vector SelectFrameDialog::filterFrames( + const std::vector &frames) const +{ + QString frame_filter = name_filter_->text(); + std::vector filtered; + + for (const auto & frame : frames) { + QString frame_name = QString::fromStdString(frame); + if (!frame_filter.isEmpty() && + !frame_name.contains(frame_filter, Qt::CaseInsensitive)) { + continue; + } + + filtered.push_back(frame); + } + + return filtered; +} + +void SelectFrameDialog::updateDisplayedFrames() +{ + std::vector next_displayed_frames = filterFrames(known_frames_); + + // It's a lot more work to keep track of the additions/removals like + // this compared to resetting the QListWidget's items each time, but + // it allows Qt to properly track the selection and current items + // across updates, which results in much less frustration for the user. + + std::set prev_names; + prev_names.insert(displayed_frames_.begin(), displayed_frames_.end()); + + std::set next_names; + next_names.insert(next_displayed_frames.begin(), next_displayed_frames.end()); + + std::set added_names; + std::set_difference(next_names.begin(), next_names.end(), + prev_names.begin(), prev_names.end(), + std::inserter(added_names, added_names.end())); + + std::set removed_names; + std::set_difference(prev_names.begin(), prev_names.end(), + next_names.begin(), next_names.end(), + std::inserter(removed_names, removed_names.end())); + + // Remove all the removed names + size_t removed = 0; + for (size_t i = 0; i < displayed_frames_.size(); i++) { + if (removed_names.count(displayed_frames_[i]) == 0) { + continue; + } + + QListWidgetItem *item = list_widget_->takeItem(static_cast(i - removed)); + delete item; + removed++; + } + + // Now we can add the new items. + for (size_t i = 0; i < next_displayed_frames.size(); i++) { + if (added_names.count(next_displayed_frames[i]) == 0) { + continue; + } + + list_widget_->insertItem(i, QString::fromStdString(next_displayed_frames[i])); + if (list_widget_->count() == 1) { + list_widget_->setCurrentRow(0); + } + } + + displayed_frames_.swap(next_displayed_frames); +} +} // namespace mapviz diff --git a/mapviz/src/select_service_dialog.cpp b/mapviz/src/select_service_dialog.cpp new file mode 100644 index 000000000..37c3b5bb6 --- /dev/null +++ b/mapviz/src/select_service_dialog.cpp @@ -0,0 +1,292 @@ +// ***************************************************************************** +// +// Copyright (c) 2015, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL Southwest Research Institute® BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY +// OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +// DAMAGE. +// +// ***************************************************************************** + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace mapviz +{ + void ServiceUpdaterThread::run() + { + std::map> service_map = + nh_->get_service_names_and_types(); + + if (allowed_datatype_.empty()) + { + std::vector service_list; + for (auto const& service : service_map) { + service_list.push_back(service.first); + } + Q_EMIT servicesFetched(service_list); + } else { + std::vector service_list; + for (auto const& service : service_map) { + if (std::find(service.second.begin(), + service.second.end(), + allowed_datatype_) != service.second.end()) { + service_list.push_back(service.first); + } + } + Q_EMIT servicesFetched(service_list); + } + } + + std::string SelectServiceDialog::selectService(rclcpp::Node::SharedPtr node, + const std::string& datatype, + QWidget* parent) + { + SelectServiceDialog dialog(node, datatype, parent); + dialog.setDatatypeFilter(datatype); + if (dialog.exec() == QDialog::Accepted) { + return dialog.selectedService(); + } else { + return ""; + } + } + + SelectServiceDialog::SelectServiceDialog(const rclcpp::Node::SharedPtr& node, + const std::string& datatype, + QWidget* parent) + : QDialog(parent) + , nh_(node) + , allowed_datatype_(datatype) + , cancel_button_(new QPushButton("&Cancel")) + , list_widget_(new QListWidget()) + , name_filter_(new QLineEdit()) + , ok_button_(new QPushButton("&Ok")) + { + QHBoxLayout *filter_box = new QHBoxLayout(); + filter_box->addWidget(new QLabel("Filter:")); + filter_box->addWidget(name_filter_); + + QHBoxLayout *button_box = new QHBoxLayout(); + button_box->addStretch(1); + button_box->addWidget(cancel_button_); + button_box->addWidget(ok_button_); + + QVBoxLayout *vbox = new QVBoxLayout(); + vbox->addWidget(list_widget_); + vbox->addLayout(filter_box); + vbox->addLayout(button_box); + setLayout(vbox); + + // This is ugly, but necessary in order to be able to send a std::vector + // via a queued signal/slot connection. + qRegisterMetaType("ServiceStringVector"); + + connect(ok_button_, SIGNAL(clicked(bool)), + this, SLOT(accept())); + connect(cancel_button_, SIGNAL(clicked(bool)), + this, SLOT(reject())); + connect(name_filter_, SIGNAL(textChanged(const QString &)), + this, SLOT(updateDisplayedServices())); + + ok_button_->setDefault(true); + + setWindowTitle("Select service..."); + + fetch_services_timer_id_ = startTimer(5000); + fetchServices(); + } + + SelectServiceDialog::~SelectServiceDialog() + { + if (worker_thread_) + { + // If the thread's parent is destroyed before the thread has finished, + // it will cause a segmentation fault. We'll wait a few seconds for + // it to finish cleanly, and if that doesn't work, try to force it to + // die and wait a few more. + worker_thread_->wait(5000); + if (worker_thread_->isRunning()) + { + worker_thread_->terminate(); + worker_thread_->wait(2000); + } + } + } + + void SelectServiceDialog::fetchServices() + { + // If we don't currently have a worker thread or the previous one has + // finished, start a new one. + if (!worker_thread_ || worker_thread_->isFinished()) + { + worker_thread_.reset(new ServiceUpdaterThread(nh_, allowed_datatype_, this)); + QObject::connect(worker_thread_.get(), + SIGNAL(servicesFetched(ServiceStringVector)), + this, + SLOT(updateKnownServices(ServiceStringVector))); + QObject::connect(worker_thread_.get(), + SIGNAL(fetchingFailed(const QString)), + this, + SLOT(displayUpdateError(const QString))); + worker_thread_->start(); + } + } + + void SelectServiceDialog::updateKnownServices(ServiceStringVector services) + { + known_services_ = services; + updateDisplayedServices(); + } + + void SelectServiceDialog::displayUpdateError(const QString& error_msg) + { + killTimer(fetch_services_timer_id_); + QMessageBox mbox(this->parentWidget()); + mbox.setIcon(QMessageBox::Warning); + mbox.setText(error_msg); + mbox.exec(); + } + + std::vector SelectServiceDialog::filterServices() + { + std::vector filtered_services; + + QString filter_text = name_filter_->text(); + + for(const std::string& service : known_services_) + { + if (QString::fromStdString(service).contains(filter_text, Qt::CaseInsensitive)) + { + filtered_services.push_back(service); + } + } + + return filtered_services; + } + + void SelectServiceDialog::updateDisplayedServices() + { + std::vector next_displayed_services = filterServices(); + + // It's a lot more work to keep track of the additions/removals like + // this compared to resetting the QListWidget's items each time, but + // it allows Qt to properly track the selection and current items + // across updates, which results in much less frustration for the user. + + std::set prev_names; + for (const auto & displayed_service : displayed_services_) { + prev_names.insert(displayed_service); + } + + std::set next_names; + for (const auto & next_displayed_service : next_displayed_services) { + next_names.insert(next_displayed_service); + } + + std::set added_names; + std::set_difference(next_names.begin(), next_names.end(), + prev_names.begin(), prev_names.end(), + std::inserter(added_names, added_names.end())); + + std::set removed_names; + std::set_difference(prev_names.begin(), prev_names.end(), + next_names.begin(), next_names.end(), + std::inserter(removed_names, removed_names.end())); + + // Remove all the removed names + size_t removed = 0; + for (size_t i = 0; i < displayed_services_.size(); i++) { + if (removed_names.count(displayed_services_[i]) == 0) { + continue; + } + + QListWidgetItem *item = list_widget_->takeItem(i - removed); + delete item; + removed++; + } + + // Now we can add the new items. + for (size_t i = 0; i < next_displayed_services.size(); i++) { + if (added_names.count(next_displayed_services[i]) == 0) { + continue; + } + + list_widget_->insertItem(i, QString::fromStdString(next_displayed_services[i])); + if (list_widget_->count() == 1) { + list_widget_->setCurrentRow(0); + } + } + + displayed_services_.swap(next_displayed_services); + } + + void SelectServiceDialog::setDatatypeFilter(const std::string& datatype) + { + allowed_datatype_ = datatype; + updateDisplayedServices(); + } + + std::string SelectServiceDialog::selectedService() const + { + QModelIndex qt_selection = list_widget_->selectionModel()->currentIndex(); + + if (qt_selection.isValid()) { + int row = qt_selection.row(); + if (row < static_cast(displayed_services_.size())) { + return displayed_services_[row]; + } + } + + return ""; + } + + void SelectServiceDialog::timerEvent(QTimerEvent* event) + { + if (event->timerId() == fetch_services_timer_id_) { + fetchServices(); + } + } + + void SelectServiceDialog::closeEvent(QCloseEvent* event) + { + // We don't need to keep making requests from the ROS master. + killTimer(fetch_services_timer_id_); + QDialog::closeEvent(event); + } +} // namespace mapviz diff --git a/mapviz/src/select_topic_dialog.cpp b/mapviz/src/select_topic_dialog.cpp new file mode 100644 index 000000000..0c9e41091 --- /dev/null +++ b/mapviz/src/select_topic_dialog.cpp @@ -0,0 +1,335 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + + +namespace mapviz +{ +std::string SelectTopicDialog::selectTopic( + const rclcpp::Node::SharedPtr& node, + const std::string &datatype, + QWidget *parent) +{ + std::vector datatypes; + datatypes.push_back(datatype); + return selectTopic(node, datatypes, parent); +} + +std::string SelectTopicDialog::selectTopic( + const rclcpp::Node::SharedPtr& node, + const std::string &datatype1, + const std::string &datatype2, + QWidget *parent) +{ + std::vector datatypes; + datatypes.push_back(datatype1); + datatypes.push_back(datatype2); + return selectTopic(node, datatypes, parent); +} + +std::string SelectTopicDialog::selectTopic( + const rclcpp::Node::SharedPtr& node, + const std::vector &datatypes, + QWidget *parent) +{ + SelectTopicDialog dialog(node, parent); + dialog.allowMultipleTopics(false); + dialog.setDatatypeFilter(datatypes); + if (dialog.exec() == QDialog::Accepted) { + return dialog.selectedTopic(); + } else { + return std::string(); + } +} + +std::vector SelectTopicDialog::selectTopics( + const rclcpp::Node::SharedPtr& node, + const std::string &datatype, + QWidget *parent) +{ + std::vector datatypes; + datatypes.push_back(datatype); + return selectTopics(node, datatypes, parent); +} + +std::vector SelectTopicDialog::selectTopics( + const rclcpp::Node::SharedPtr& node, + const std::string &datatype1, + const std::string &datatype2, + QWidget *parent) +{ + std::vector datatypes; + datatypes.push_back(datatype1); + datatypes.push_back(datatype2); + return selectTopics(node, datatypes, parent); +} + +std::vector SelectTopicDialog::selectTopics( + const rclcpp::Node::SharedPtr& node, + const std::vector &datatypes, + QWidget *parent) +{ + SelectTopicDialog dialog(node, parent); + dialog.allowMultipleTopics(true); + dialog.setDatatypeFilter(datatypes); + if (dialog.exec() == QDialog::Accepted) { + return dialog.selectedTopics(); + } else { + return std::vector(); + } +} + +SelectTopicDialog::SelectTopicDialog(const rclcpp::Node::SharedPtr& node, QWidget *parent) + : + nh_(node), + ok_button_(new QPushButton("&Ok")), + cancel_button_(new QPushButton("&Cancel")), + list_widget_(new QListWidget()), + name_filter_(new QLineEdit()) +{ + QHBoxLayout *filter_box = new QHBoxLayout(); + filter_box->addWidget(new QLabel("Filter:")); + filter_box->addWidget(name_filter_); + + QHBoxLayout *button_box = new QHBoxLayout(); + button_box->addStretch(1); + button_box->addWidget(cancel_button_); + button_box->addWidget(ok_button_); + + QVBoxLayout *vbox = new QVBoxLayout(); + vbox->addWidget(list_widget_); + vbox->addLayout(filter_box); + vbox->addLayout(button_box); + setLayout(vbox); + + connect(ok_button_, SIGNAL(clicked(bool)), + this, SLOT(accept())); + connect(cancel_button_, SIGNAL(clicked(bool)), + this, SLOT(reject())); + connect(name_filter_, SIGNAL(textChanged(const QString &)), + this, SLOT(updateDisplayedTopics())); + + ok_button_->setDefault(true); + + allowMultipleTopics(false); + setWindowTitle("Select topics..."); + + fetch_topics_timer_id_ = startTimer(1000); + fetchTopics(); +} + +void SelectTopicDialog::timerEvent(QTimerEvent *event) +{ + if (event->timerId() == fetch_topics_timer_id_) { + fetchTopics(); + } +} + +void SelectTopicDialog::closeEvent(QCloseEvent *event) +{ + // We don't need to keep making requests from the ROS master. + killTimer(fetch_topics_timer_id_); + QDialog::closeEvent(event); +} + +void SelectTopicDialog::allowMultipleTopics( + bool allow) +{ + if (allow) { + list_widget_->setSelectionMode(QAbstractItemView::MultiSelection); + } else { + list_widget_->setSelectionMode(QAbstractItemView::SingleSelection); + } +} + +void SelectTopicDialog::setDatatypeFilter( + const std::vector &datatypes) +{ + allowed_datatypes_.clear(); + for (const auto & datatype : datatypes) { + allowed_datatypes_.insert(datatype); + } + updateDisplayedTopics(); +} + +std::string SelectTopicDialog::selectedTopic() const +{ + std::vector selection = selectedTopics(); + if (selection.empty()) { + return std::string(); + } else { + return selection.front(); + } +} + +std::vector SelectTopicDialog::selectedTopics() const +{ + QModelIndexList qt_selection = list_widget_->selectionModel()->selectedIndexes(); + + std::vector selection; + selection.resize(qt_selection.size()); + for (int i = 0; i < qt_selection.size(); i++) { + if (!qt_selection[i].isValid()) { + continue; + } + + int row = qt_selection[i].row(); + if (row < 0 || static_cast(row) >= displayed_topics_.size()) { + continue; + } + + selection[i] = displayed_topics_[row]; + } + + return selection; +} + +static bool topicSort(const std::string &info1, + const std::string &info2) +{ + return info1 < info2; +} + +void SelectTopicDialog::fetchTopics() +{ + known_topics_ = nh_->get_topic_names_and_types(); + std::vector map_keys; + for (auto const& element : known_topics_) + { + map_keys.push_back(element.first); + } + std::sort(map_keys.begin(), map_keys.end(), topicSort); + updateDisplayedTopics(); +} + +std::vector SelectTopicDialog::filterTopics( + const std::map> &topics) const +{ + QString topic_filter = name_filter_->text(); + std::vector filtered; + + for (auto const& topic : topics) { + if (!allowed_datatypes_.empty()) { + // Skip any topic names that don't contain allowed types + bool missing_allowed_type = true; // Assume the worst + for (auto const& datatype : topic.second) { + if (allowed_datatypes_.count(datatype) == 1) { + missing_allowed_type = false; + break; + } + } + if (missing_allowed_type) { + continue; + } + } + + QString topic_name = QString::fromStdString(topic.first); + if (!topic_filter.isEmpty() && + !topic_name.contains(topic_filter, Qt::CaseInsensitive)) { + continue; + } + + filtered.push_back(topic.first); + } + + return filtered; +} + +void SelectTopicDialog::updateDisplayedTopics() +{ + std::vector next_displayed_topics = filterTopics(known_topics_); + + // It's a lot more work to keep track of the additions/removals like + // this compared to resetting the QListWidget's items each time, but + // it allows Qt to properly track the selection and current items + // across updates, which results in much less frustration for the user. + + std::set prev_names; + for (const auto & displayed_topic : displayed_topics_) { + prev_names.insert(displayed_topic); + } + + std::set next_names; + for (const auto & next_displayed_topic : next_displayed_topics) { + next_names.insert(next_displayed_topic); + } + + std::set added_names; + std::set_difference(next_names.begin(), next_names.end(), + prev_names.begin(), prev_names.end(), + std::inserter(added_names, added_names.end())); + + std::set removed_names; + std::set_difference(prev_names.begin(), prev_names.end(), + next_names.begin(), next_names.end(), + std::inserter(removed_names, removed_names.end())); + + // Remove all the removed names + size_t removed = 0; + for (size_t i = 0; i < displayed_topics_.size(); i++) { + if (removed_names.count(displayed_topics_[i]) == 0) { + continue; + } + RCLCPP_DEBUG(nh_->get_logger(), "Removing %s", displayed_topics_[i].c_str()); + + QListWidgetItem *item = list_widget_->takeItem(i - removed); + delete item; + removed++; + } + + // Now we can add the new items. + for (size_t i = 0; i < next_displayed_topics.size(); i++) { + if (added_names.count(next_displayed_topics[i]) == 0) { + continue; + } + + list_widget_->insertItem(i, QString::fromStdString(next_displayed_topics[i])); + RCLCPP_DEBUG(nh_->get_logger(), "Inserting %s", next_displayed_topics[i].c_str()); + if (list_widget_->count() == 1) { + list_widget_->setCurrentRow(0); + } + } + + displayed_topics_.swap(next_displayed_topics); +} +} // namespace mapviz diff --git a/mapviz/src/video_writer.cpp b/mapviz/src/video_writer.cpp new file mode 100644 index 000000000..08a59410d --- /dev/null +++ b/mapviz/src/video_writer.cpp @@ -0,0 +1,128 @@ +// ***************************************************************************** +// +// Copyright (c) 2017, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include + +#include + +#include + +#include +#include + +namespace mapviz +{ + bool VideoWriter::initializeWriter(const std::string& directory, int width, int height) + { + QMutexLocker locker(&video_mutex_); + if (!video_writer_) + { + width_ = width; + height_ = height; + + RCLCPP_INFO(rclcpp::get_logger("mapviz"), + "Initializing recording:\nWidth/Height/Filename: %d / %d / %s", + width_, + height_, + directory.c_str()); + video_writer_ = std::make_shared( + directory, + cv::VideoWriter::fourcc('M', 'J', 'P', 'G'), + 30, + cv::Size(width_, height_)); + + if (!video_writer_->isOpened()) + { + RCLCPP_ERROR(rclcpp::get_logger("mapviz"), "Failed to open video file for writing."); + stop(); + return false; + } + } + + return true; + } + + bool VideoWriter::isRecording() + { + return video_writer_.get() != NULL; + } + + void VideoWriter::processFrame(QImage frame) + { + try + { + RCLCPP_DEBUG(rclcpp::get_logger("mapviz"), "VideoWriter::processFrame():"); + { + QMutexLocker locker(&video_mutex_); + if (!video_writer_) + { + RCLCPP_WARN(rclcpp::get_logger("mapviz"), "Got frame, but video writer wasn't open."); + return; + } + } + + cv::Mat image; + cv::Mat temp_image; + switch (frame.format()) + { + case QImage::Format_ARGB32: + // The image received should have its format set to ARGB32, but it's + // actually BGRA. Need to convert it to BGR and flip it vertically + // before giving it to the cv::VideoWriter. + image = cv::Mat(frame.height(), frame.width(), CV_8UC4, frame.bits()); + cv::cvtColor(image, temp_image, cv::COLOR_BGRA2BGR); + cv::flip(temp_image, image, 0); + break; + default: + RCLCPP_WARN(rclcpp::get_logger("mapviz"), "Unexpected image format: %d", frame.format()); + return; + } + + { + QMutexLocker locker(&video_mutex_); + if (video_writer_) + { + RCLCPP_DEBUG(rclcpp::get_logger("mapviz"), "Writing frame."); + video_writer_->write(image); + } + } + } + catch (const std::exception& e) + { + RCLCPP_ERROR(rclcpp::get_logger("mapviz"), "Error when processing video frame: %s", e.what()); + } + } + + void VideoWriter::stop() + { + RCLCPP_INFO(rclcpp::get_logger("mapviz"), "Stopping video recording."); + QMutexLocker locker(&video_mutex_); + video_writer_.reset(); + } +} // namespace mapviz diff --git a/mapviz_interfaces/CHANGELOG.rst b/mapviz_interfaces/CHANGELOG.rst new file mode 100644 index 000000000..a7bd4dfd5 --- /dev/null +++ b/mapviz_interfaces/CHANGELOG.rst @@ -0,0 +1,97 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package mapviz_interfaces +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.3.0 (2023-08-24) +------------------ + +2.2.2 (2023-06-07) +------------------ + +2.2.1 (2023-05-30) +------------------ +* Updating maintainers list (`#778 `_) +* Contributors: David Anthony + +2.1.0 (2020-10-22) +------------------ + +2.0.0 (2020-05-13) +------------------ +* Port mapviz to ROS 2 (`#672 `_) +* Contributors: P. J. Reed + +1.2.0 (2019-09-04) +------------------ + +1.1.1 (2019-05-17) +------------------ + +1.1.0 (2019-02-20) +------------------ + +1.0.1 (2019-01-25) +------------------ + +1.0.0 (2019-01-23) +------------------ + +0.3.0 (2018-11-16) +------------------ + +0.2.6 (2018-07-31 09:02) +------------------------ + +0.2.5 (2018-04-12 16:26) +------------------------ + +0.2.4 (2017-08-11 09:57) +------------------------ + +0.2.3 (2016-12-10) +------------------ + +0.2.2 (2016-12-07) +------------------ + +0.2.1 (2016-10-23 22:33) +------------------------ + +0.2.0 (2016-06-23) +------------------ + +0.1.3 (2016-05-20 15:12) +------------------------ + +0.1.2 (2016-01-06 17:04) +------------------------ + +0.1.1 (2015-11-17) +------------------ + +0.1.0 (2015-09-29) +------------------ + +0.0.10 (2018-07-31 09:01) +------------------------- + +0.0.9 (2018-04-12 16:23) +------------------------ + +0.0.8 (2017-08-11 09:53) +------------------------ + +0.0.7 (2016-10-23 21:55) +------------------------ + +0.0.6 (2016-08-14) +------------------ + +0.0.5 (2016-05-20 14:40) +------------------------ + +0.0.4 (2016-01-06 17:00) +------------------------ + +0.0.3 (2015-09-28) +------------------ diff --git a/mapviz_interfaces/CMakeLists.txt b/mapviz_interfaces/CMakeLists.txt new file mode 100644 index 000000000..5bcf88748 --- /dev/null +++ b/mapviz_interfaces/CMakeLists.txt @@ -0,0 +1,25 @@ +### SET CMAKE POLICIES ### +cmake_minimum_required(VERSION 3.10...3.17) + +if(${CMAKE_VERSION} VERSION_LESS 3.12) + cmake_policy(VERSION ${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION}) +endif() + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +### END CMAKE POLICIES ### + +project(mapviz_interfaces) + +find_package(ament_cmake REQUIRED) +find_package(marti_common_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + srv/AddMapvizDisplay.srv + DEPENDENCIES marti_common_msgs +) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/mapviz_interfaces/package.xml b/mapviz_interfaces/package.xml new file mode 100644 index 000000000..1ead0660f --- /dev/null +++ b/mapviz_interfaces/package.xml @@ -0,0 +1,24 @@ + + mapviz_interfaces + 2.3.0 + + ROS interfaces used by Mapviz + + P. J. Reed + Southwest Research Institute + BSD + https://github.com/swri-robotics/mapviz + + builtin_interfaces + marti_common_msgs + + rosidl_default_generators + + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + diff --git a/mapviz_interfaces/srv/AddMapvizDisplay.srv b/mapviz_interfaces/srv/AddMapvizDisplay.srv new file mode 100644 index 000000000..60bb41177 --- /dev/null +++ b/mapviz_interfaces/srv/AddMapvizDisplay.srv @@ -0,0 +1,21 @@ +# Add or updates a mapviz display. + +string name # The name of the display. +string type # The plugin type. + +int32 draw_order # The display order. 1 corresponds + # to the first displayed, 2 to the + # second, -1 to last, and -2 to the + # second to last, etc. 0 will keep + # the current display order of an + # existing display and give a new + # display the last display order. + +bool visible # If the display should be visible. + +marti_common_msgs/KeyValue[] properties # Configuration properties. + +--- + +bool success # indicate successful run of triggered service +string message # informational, e.g. for error messages diff --git a/mapviz_plugins/CHANGELOG.rst b/mapviz_plugins/CHANGELOG.rst new file mode 100644 index 000000000..610f40dd6 --- /dev/null +++ b/mapviz_plugins/CHANGELOG.rst @@ -0,0 +1,374 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package mapviz_plugins +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.3.0 (2023-08-24) +------------------ + +2.2.2 (2023-06-07) +------------------ +* Add ros_environment as dependency +* Iron Compatibility (`#779 `_) +* Contributors: David Anthony + +2.2.1 (2023-05-30) +------------------ +* Updating maintainers list (`#778 `_) +* Fix Plan Route plugin in ROS2 Humble (`#765 `_) +* Merge pull request `#759 `_ from agyoungs/fix-marker-plugin-subs +* Check topic for type to determine which subscription callback to trigger +* Contributors: Alex Youngs, David Anthony, P. J. Reed + +2.1.0 (2020-10-22) +------------------ +* Constrain the minimum line and point marker sizes to be 1 pixel wide. (`#704 `_) +* ROS Foxy support (`#695 `_) +* Update the displayed distance continuously while moving a point. +* Use higher precision in the coordinate picker for wgs84 (`#692 `_) +* Clear the namespace list after hitting the clear button. (`#691 `_) +* Contributors: Matt Grogan, Matthew, P. J. Reed, Marc Alban + +2.0.0 (2020-05-13) +------------------ +* Port mapviz to ROS 2 (`#672 `_) +* Contributors: Daniel D'Souza, Matthew Bries, Matthew Grogan, P. J. Reed, Jason Gassaway, John Reyes, Jacob Hassold, Kevin Nickels, Roger Strain + +1.2.0 (2019-09-04) +------------------ +* Add text to measuring plugin (`#640 `_) +* Add mapviz plug-in for PoseStamped messages. (`#641 `_) +* Fix occupancy grid to load color scheme from configuration. (`#642 `_) +* Restore GL_UNPACK_ALIGNMENT to 4 to prevent corruption of Qt font rendering. (`#643 `_) +* Add ability to show and hide markers by namespace (`#636 `_) +* Fixed layout of MeasuringPlugin (`#633 `_) +* Fixed marker plugin to use swri_transform_util to ensure wgs84 markers work properly (`#635 `_) +* Contributors: Arkady Shapkin, Marc Alban, Matthew, Matthew Grogan, agyoungs + +1.1.1 (2019-05-17) +------------------ +* Textured Marker Adjustments (`#611 `_, `#616 `_) (`#625 `_) +* fixed issue `#623 `_ by updating UI field to read "Draw Style:" (`#624 `_) +* Contributors: mattrich37 + +1.1.0 (2019-02-20) +------------------ +* Improve MarkerPlugin (`#603 `_) + * Improved performance of MarkerPlugin::handleMarker() + * Support Text marker alpha channel + * Don't use QColor for glColor4f + * Use marker namespace and id as markers map key +* [mapviz_plugins/attitude_indicator] Minor refactoring and redundant logging removed (`#617 `_) +* 606 sequential measuring (`#607 `_) + * Moved distance calculation to trigger on release of mouse. Added case to prevent distance calculation while moving map. + * Added vertices and lines between measurement points. Left click to add point. Right click to delete. Added color selection for points and clear button to ui. + * Added cumulative distance measurements from multiple points + * Fixed individual and cumulative distance measurements. Changed it to only measure distance between points and not from fixed origin and first point + * Moved distance calculation into separate function which is called when deleting a point, adding a point, or rearranging points. +* Add image size check to textured marker plugin to prevent crashes. (`#613 `_) + * Add image size check to textured marker plugin to prevent crashes. +* Fixed typo in string (`#608 `_) +* 605 add reset button marker (`#609 `_) + * Added Clear all marker buttons, added case for clear all support to markers +* Contributors: Arkady Shapkin, Matthew, jbdaniel18 + +1.0.1 (2019-01-25) +------------------ +* Use shared tf manager in measuring_plugin (`#604 `_) +* Contributors: jgassaway + +1.0.0 (2019-01-23) +------------------ +* Sharing tf_manager\_ between main app and plugins (`#555 `_) +* Fix potential segfault in pointcloud plug-in. (`#602 `_) +* Add Measuring Plugin (`#598 `_) +* Contributors: Davide Faconti, Marc Alban, Matthew + +0.3.0 (2018-11-16) +------------------ +* Merge all -devel branches into a single master branch +* Don't transform laser scans twice (`#544 `_) +* Improving point_drawing plugins and bug fix of tf_plugin (`#557 `_) +* OpenGL rendering of PointClouds (2X speedup) (`#558 `_) +* Occupancy grid (new plugin) (`#568 `_) +* Bug fix in image plugin (`#563 `_) +* Fix Indigo build, clean up warnings (`#597 `_) +* Create Coordinate Picker plugin (`#593 `_) +* Contributors: Davide Faconti, Ed Venator, Edward Venator, Elliot Johnson, Jerry Towler, Marc Alban, Matthew, Matthew Bries, Mikael Arguedas, Neal Seegmiller, Nicholas Alton, P. J. Reed, Vincent Rousseau + +0.2.6 (2018-07-31) +------------------ +* Fix timestamp interval (`#588 `_) +* Update path_plugin.cpp (`#586 `_) +* Replace depcreated plugin macro with newer version +* Contributors: Matthew, P. J. Reed, camjaws + +0.2.5 (2018-04-12) +------------------ +* Add clear history functionality. +* Add support for newlines in text marker plugin (`#572 `_) +* New plugin to send commands to move_base +* Glew warning fixed (`#539 `_) +* Added "keep image ratio" to Image plugin (`#543 `_) +* Remove copy and paste of Print... +* PointCloud2 speed improvement (`#531 `_) +* Dead code removed (`#535 `_) +* Ratio added to robot_image_plugin (`#530 `_) +* Speed up improvement in LaserScan and PointCloud2 (`#525 `_) +* Re-add GPSFix plugin to kinetic-devel (`#519 `_) +* Add support for unpacking rgb8 in pointcloud2s +* Use non-deprecated pluginlib macro +* Add plug-in for drawing and publishing a polygon. +* change the signal that triggers AlphaEdited + minor changes (`#514 `_) +* Added timestamp display to odometry for kinetic +* Contributors: Davide Faconti, Marc Alban, Matthew Bries, Mikael Arguedas, P. J. Reed, jgassaway + +0.2.4 (2017-08-11) +------------------ +* Add /wgs84 frame to point click publisher when available. +* Transform cube and arrow markers properly +* Contributors: Marc Alban, P. J. Reed + +0.2.3 (2016-12-10) +------------------ +* Delete markers that have expired and remove error message. (`#454 `_) +* Fix segfault in pointcloud2 plug-in when pointcloud is empty. (`#450 `_) +* Initialize buffer size variable. (`#447 `_) +* Contributors: Marc Alban + +0.2.2 (2016-12-07) +------------------ +* Migrated OpenCV to 3.1 (default in Kinetic) +* General code cleanup of mapviz_plugins + This doesn't change any functionality; it's just cleaning up code. Notably, this will: + - Fix all warnings (notably lots of ones about type casting) + - Move all .ui files to their own directory + - Remove unused variables + - Remove commented-out code + - Make spacing and indentation consistent + - Make brace style consistent +* Contributors: Brian Holt, Marc Alban, P. J. Reed + +0.2.1 (2016-10-23) +------------------ +* Add a GUI for controlling the Image Transport (`#432 `_) + This will add a sub-menu under the "View" menu that will: + - List all available image transports + - Indicate which one is currently the default + - Allow the user to choose which one will be used for new ImageTransport subscriptions + - Save and restore this setting to Mapviz's config file + - Cause any `image` plugins using the default transport to resubscribe + In addition, the image plugin now has a menu that can be used to change the + transport for that specific plugin so that it is different from the default. + Fixes `#430 `_ + Conflicts: + mapviz/package.xml +* Fix icon colors for point drawing plugins (`#433 `_) + This was probably broken back when all of these were refactored to have a + single base class. It looks like the member variable that holds the color + used to draw the icon was never actually being updated. + Fixes `#426 `_ +* Add option to not scale arrows with zoom level + This adds a checkbox to all of the plugins that can draw a series of + coordinates as arrows; i. e., the NavSatFix, Odometry, and TF Frame + plugins. This checkbox will control whether the arrows are drawn at a fixed + size regardless of zoom level or whether they are scaled with the zoom level. + Resolves `#414 `_ +* Fix signed comparison warnings in mapviz_plugins +* Adding a way for plugin config widgets to resize + - Adding an event plugins can emit to indicate their geometry has changed + - Modifying the PCL2 plugin to use it as an example + Fixes `#393 `_ +* Adding default values for uninitialized variables + Resolves `#372 `_ +* Creates and implements an abstract class for drawing point paths + Updates gps,navsat,odometry,path, and tf_frame plugins to use the + abstract point drawing class. Also adds the draw laps functionality + which will change the color of the path as it passes a base point for + ease of visibility, currently implemented on gps and odometry plugins. + Conflicts: + mapviz_plugins/CMakeLists.txt + mapviz_plugins/include/mapviz_plugins/gps_plugin.h + mapviz_plugins/src/gps_config.ui + mapviz_plugins/src/gps_plugin.cpp +* Ensuring that Mapviz won't subscribe to empty topic names (`#379 `_) + Clean up and made more consistent the code for handling subscriptions for all topics. + The behavior is now: + - All input is trimmed before processing + - If a topic name is empty, the old subscriber will be shut down and will not subscribe to the empty topic + Resolves `#327 `_ +* Fixing some typos in documentation. +* Implementing support for the ARROW marker type + Resolves `#365 `_ +* Contributors: Ed Venator, Marc Alban, P. J. Reed + +0.2.0 (2016-06-23) +------------------ +* Update Qt to version 5 +* Fixing a crash in the PointCloud2 plugin + Also sneaking in a few more changes: + - Caching transformed clouds to improve performance + - Properly saving the value of the "Color Transformer" combo box +* Returning "false" if no other code handles the mouse event + Fixes `#360 `_ +* Contributors: Ed Venator, P. J. Reed + +0.1.3 (2016-05-20) +------------------ +* Implement mapviz plug-in for calling the marti_nav_msgs::PlanRoute service. +* Migrate route plugin to use swri_route_util + This change migrates the mapviz route plugin to use swri_route_util to + get consistent behavior with route transforms and route position + interpolation. As part of this change, the route is now transformed + with each draw so that it will correctly move around if the transform + between the fixed frame and the route frame is not constant. +* Add support for mono8 textured markers. +* Implement service for adding and modifying mapviz displays. +* Adding attitude indicator plugin. +* Changing some "unsigned long"s to "size_t"s. +* Storing source frames individually for plugins w/ buffers +* Fix for `#265 `_; message source frames don't update + Several plugins were storing the source frames of messages received when + they first received a message but never updating them, so subsequent + messages in different frames would be rendered incorrectly. +* Fix for `#339 `_; explicitly depending on OpenCV 2 +* Fix route position search + The route position search would ignore a matching point unless it is + already transformed, which means that only points that have already been + searched and missed would be transformed. + The new logic looks first for the match, then transforms as necessary. + Unmatched points are ignored. +* Guard against repeated transforms + A point should only be transformed once, because the mapviz transforms + are set outside the plugins; `TransformPoint` will now only transform + un-transformed points. +* Remove unused variable + prev_position\_ is set, but never actually used. +* Adds route plugin with routeposition marker attachment. +* Also updating the disparity plugin +* Fixing `#317 `_ + First, the model view matrix needs to be saved and restored around + QPainter operations because Qt clears several GL variables. Also, the + image plugin needed to explicitly call glMatrixMode(GL_PROJECTION); + it does a few operations on the projection matrix and was just assuming + that was the current matrix mode. Also, I added a function that plugins + need to override if they want to do QPainter operations; this will + eliminate unnecessary overhead for plugins that do not. +* Declaring types for Qt signal/slot use properly +* Fixing some typos +* Doing GL drawing on the main thread for `#313 `_ +* GPS plugin snuck back into CMakeLists.txt +* A plugin for displaying std_msgs/Strings +* Marker plugin will use a QPainter to draw text + I modified the Marker plugin so that it will use a QPainter to draw + text labels rather than OpenGL commands. This doesn't really add any + functional benefit; it's meant to serve as an example of how to use + the QPainter. +* Fixing warnings and cleaning up formatting +* updated mapviz_plugins.xml +* add pointcloud2 plugin +* Update map canvas at a fixed rate. + This update adds a timer to the map canvas to repaint at a fixed rate. + The default rate is 50 Hz, but there is a method to change it (not + exposed to the UI at the moment). 50Hz was chosen because it is fast + enough to give smooth animations and we almost always are running + mapviz with at least one plugin triggering updates from a 50Hz topic. +* Making the Image plugin use image_transport. + The image_transport package provides support for transparently + subscribing and publishing to topics using low-bandwidth compressed + formats; if the publisher supports it, this will cause the Image + plugin to consume far less bandwidth than before. +* Handle cases where marker topic changes message types. + This commit makes a better effort to properly support cases where a + marker topic changes between Marker and MarkerArray during runtime. +* Use ROS' shapeshifter to handle marker/marker arrays. +* This commit adds a class called SelectFrameDialog that plugins can use + to present the user with a dialog to choose a TF frame. The dialog + sorts the frames by name and provides an edit box that the user can + use to filter the frames to a specific substring. +* Indigo compatibility. + Fixing swri_transform_util and swri_yaml_util API changes from + Hydro to Indigo. +* Also filtering out clicks that are held for too long. +* Adding a check to prevent the click event from firing if the user is dragging the mouse. +* Fixing an issue that could cause the click publisher plugin's publisher to not be initialized after it's first added. +* Removing some code I had added for debugging. +* Adding a plugin that, when a user clicks on a point, will publish that point's coordinates to a topic. +* Adding color button widget and updating plugins. + This commit adds a subclass of QPushButton called ColorButton that + implements a widget for displaying and selecting colors. We've been + doing this manually everywhere with duplicated code. This is a simple + abstraction but allows us to elminate a lot of duplication, especially + in plugins that have multiple color selections. +* Adds SelectTopicDialog to mapviz. + This commit adds the SelectTopicDialog that can be used in plugins to + provide the user with a dialog to select topics. Typically we have + done this with a lot of duplicated code across all the plugins. This + commit also updates the plugins in mapviz_plugins to use the new + dialog. + The new dialog provides several benefits: + - Reduced code duplication + - Simplifies writing new plugins + - Common behavior between all plugins + - Topics sorted by name + - User can filter topics by substring + - Continuously checks the master for new topics while the dialog is open. +* Contributors: Elliot Johnson, Jerry Towler, Marc Alban, Nicholas Alton, P. J. Reed + +0.1.2 (2016-01-06) +------------------ +* Enables the possibility to load a one-layer tile set +* Sorts topic, plug-in, and frame lists in selection dialogs. +* Fixes tf plug-in update. +* Contributors: Marc Alban, Vincent Rousseau + +0.1.1 (2015-11-17) +------------------ +* Extensions for geo files (PR `#262 `_) +* Adds a plugin to visualize laser scans. + Display features are based on the laserscan plugin for rviz: + * Points can be colored by range, or x/y/z axis + * Points can be colored by interpolation between two colors or rainbow coloring +* Adds a plugin to visualize sensor_msgs/NavSatFix msgs, based on the old GPSFix plugin +* Contributors: Claudio Bandera, Ed Venator, Vincent Rousseau + +0.1.0 (2015-09-29) +------------------ +* Removes gps plugin, since gps_common is not in ROS Jade. See issue + `#238 `_. +* Contributors: Edward Venator + +0.0.3 (2015-09-28) +------------------ + +0.0.2 (2015-09-27) +------------------ +* Adds missing qt4_opengl dependency + +0.0.1 (2015-09-27) +------------------ +* Renames all marti_common packages that were renamed. + (See http://github.com/swri-robotics/marti_common/issues/231) +* Fixes catkin_lint problems that could prevent installation. +* Exports the mapviz_plugins library +* Adds find_package(OpenCV REQUIRED) to cmake config +* adds icon to gps plug-in +* includes yaml_util header in mapviz plug-in base class +* adds gps_common dependency +* Sets the point orientation properly based on the GPSFix track. +* Converts incoming GPSFix points to the local XY frame as they arrive. +* Changes the GPS plugin to always transform from the local XY frame. +* Adds a plugin to display GPSFix data. +* Fixes a few instances where "multires" was typoed as "mutlires". +* updates cmake version to squash the CMP0003 warning +* removes dependencies on build_tools +* switches format 2 package definition +* Updates marker_plugin to correctly handle marker orientation. +* adds color selection for path visualization +* display preview icon next to plug-in names +* sets the z component of path points to 0 before transforming to avoid uninitialized values +* fixes missing organization in license text +* fixes for GLEW/GL include order +* catkinize mapviz +* changes license to BSD +* adds license and readme files +* Contributors: Edward Venator, Elliot Johnson, Marc Alban, P. J. Reed diff --git a/mapviz_plugins/CMakeLists.txt b/mapviz_plugins/CMakeLists.txt new file mode 100644 index 000000000..1f22584bb --- /dev/null +++ b/mapviz_plugins/CMakeLists.txt @@ -0,0 +1,242 @@ +### SET CMAKE POLICIES ### +cmake_minimum_required(VERSION 3.10...3.17) + +if(${CMAKE_VERSION} VERSION_LESS 3.12) + cmake_policy(VERSION ${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION}) +endif() + +if (NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(${CMAKE_VERSION} VERSION_EQUAL "3.11.0" OR ${CMAKE_VERSION} VERSION_GREATER "3.11.0") + cmake_policy(SET CMP0072 NEW) +endif() + +if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() +### END CMAKE POLICIES ### + +project(mapviz_plugins + LANGUAGES CXX) + +# Ros Packages +find_package(ament_cmake REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(gps_msgs REQUIRED) +find_package(image_transport REQUIRED) +find_package(map_msgs REQUIRED) +find_package(mapviz REQUIRED) +find_package(marti_common_msgs REQUIRED) +find_package(marti_nav_msgs REQUIRED) +find_package(marti_sensor_msgs REQUIRED) +find_package(marti_visualization_msgs REQUIRED) +# find_package(move_base_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(stereo_msgs REQUIRED) +find_package(swri_image_util REQUIRED) +find_package(swri_math_util REQUIRED) +find_package(swri_route_util REQUIRED) +find_package(swri_transform_util REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(visualization_msgs REQUIRED) + +### QT ### +find_package(Qt5Core REQUIRED) +find_package(Qt5Gui REQUIRED) +find_package(Qt5OpenGL REQUIRED) +find_package(Qt5Widgets REQUIRED) +add_definitions(-DWFlags=WindowFlags) + +find_package(OpenCV COMPONENTS core imgproc REQUIRED) + +### OpenGL ### +find_package(OpenGL REQUIRED) +find_package(GLUT REQUIRED) + +# Fix conflict between Boost signals used by tf and QT signals used by mapviz +add_definitions(-DQT_NO_KEYWORDS) + +# Need to include the current dir to include the results of building Qt UI files +set(CMAKE_INCLUDE_CURRENT_DIR ON) + +set(UI_FILES + ui/attitude_indicator_config.ui + ui/coordinate_picker_config.ui + ui/disparity_config.ui + ui/draw_polygon_config.ui + ui/float_config.ui + ui/gps_config.ui + ui/grid_config.ui + ui/image_config.ui + ui/laserscan_config.ui + ui/marker_config.ui + ui/measuring_config.ui + # ui/move_base_config.ui + ui/navsat_config.ui + ui/occupancy_grid_config.ui + ui/odometry_config.ui + ui/path_config.ui + ui/plan_route_config.ui + ui/point_click_publisher_config.ui + ui/pointcloud2_config.ui + ui/pose_config.ui + ui/robot_image_config.ui + ui/route_config.ui + ui/string_config.ui + ui/textured_marker_config.ui + ui/tf_frame_config.ui + ui/topic_select.ui +) + +set(SRC_FILES + src/attitude_indicator_plugin.cpp + src/canvas_click_filter.cpp + src/coordinate_picker_plugin.cpp + src/disparity_plugin.cpp + src/draw_polygon_plugin.cpp + src/float_plugin.cpp + src/gps_plugin.cpp + src/grid_plugin.cpp + src/image_plugin.cpp + src/laserscan_plugin.cpp + src/marker_plugin.cpp + src/measuring_plugin.cpp + # src/move_base_plugin.cpp + src/navsat_plugin.cpp + src/occupancy_grid_plugin.cpp + src/odometry_plugin.cpp + src/path_plugin.cpp + src/placeable_window_proxy.cpp + src/plan_route_plugin.cpp + src/point_click_publisher_plugin.cpp + src/pointcloud2_plugin.cpp + src/point_drawing_plugin.cpp + src/pose_plugin.cpp + src/robot_image_plugin.cpp + src/route_plugin.cpp + src/string_plugin.cpp + src/textured_marker_plugin.cpp + src/tf_frame_plugin.cpp +) + +set(HEADER_FILES + include/${PROJECT_NAME}/attitude_indicator_plugin.h + include/${PROJECT_NAME}/canvas_click_filter.h + include/${PROJECT_NAME}/coordinate_picker_plugin.h + include/${PROJECT_NAME}/disparity_plugin.h + include/${PROJECT_NAME}/draw_polygon_plugin.h + include/${PROJECT_NAME}/float_plugin.h + include/${PROJECT_NAME}/gps_plugin.h + include/${PROJECT_NAME}/grid_plugin.h + include/${PROJECT_NAME}/image_plugin.h + include/${PROJECT_NAME}/laserscan_plugin.h + include/${PROJECT_NAME}/marker_plugin.h + include/${PROJECT_NAME}/measuring_plugin.h + # include/${PROJECT_NAME}/move_base_plugin.h + include/${PROJECT_NAME}/navsat_plugin.h + include/${PROJECT_NAME}/occupancy_grid_plugin.h + include/${PROJECT_NAME}/odometry_plugin.h + include/${PROJECT_NAME}/path_plugin.h + include/${PROJECT_NAME}/placeable_window_proxy.h + include/${PROJECT_NAME}/plan_route_plugin.h + include/${PROJECT_NAME}/point_click_publisher_plugin.h + include/${PROJECT_NAME}/pointcloud2_plugin.h + include/${PROJECT_NAME}/point_drawing_plugin.h + include/${PROJECT_NAME}/pose_plugin.h + include/${PROJECT_NAME}/robot_image_plugin.h + include/${PROJECT_NAME}/route_plugin.h + include/${PROJECT_NAME}/string_plugin.h + include/${PROJECT_NAME}/textured_marker_plugin.h + include/${PROJECT_NAME}/tf_frame_plugin.h +) + +qt5_wrap_ui(UI_SRC_FILES ${UI_FILES}) +qt5_wrap_cpp(MOC_FILES ${HEADER_FILES}) + +add_library(${PROJECT_NAME} SHARED + ${MOC_FILES} + ${SRC_FILES} + ${UI_SRC_FILES} +) + +target_include_directories(${PROJECT_NAME} + PUBLIC + include) + +target_compile_definitions(${PROJECT_NAME} + PUBLIC + "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +# Iron and later switched some cv_bridge files to .hpp from .h +if ("$ENV{ROS_DISTRO}" STRLESS "iron") + target_compile_definitions(${PROJECT_NAME} PRIVATE "-DUSE_CVBRIDGE_H_FILES") +endif() + +ament_target_dependencies(${PROJECT_NAME} + ament_cmake + ament_index_cpp + cv_bridge + gps_msgs + image_transport + map_msgs + mapviz + marti_common_msgs + marti_nav_msgs + marti_sensor_msgs + marti_visualization_msgs + # move_base_msgs + nav_msgs + pluginlib + rclcpp + sensor_msgs + std_msgs + stereo_msgs + swri_image_util + swri_math_util + swri_route_util + swri_transform_util + tf2 + tf2_geometry_msgs + tf2_ros + visualization_msgs +) +target_link_libraries(${PROJECT_NAME} + Qt5::Core + Qt5::Gui + Qt5::OpenGL + Qt5::Widgets + ${GLU_LIBRARY} + ${GLUT_LIBRARY} + opencv_core + opencv_imgproc + opencv_highgui +) + +### Install the plugins ### + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} + FILES_MATCHING PATTERN "*.h" +) + +install(TARGETS ${PROJECT_NAME} + RUNTIME DESTINATION lib/${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib +) + +pluginlib_export_plugin_description_file(mapviz mapviz_plugins.xml) + +ament_export_dependencies(${RUNTIME_DEPS} Qt) +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_package() diff --git a/mapviz_plugins/include/mapviz_plugins/attitude_indicator_plugin.h b/mapviz_plugins/include/mapviz_plugins/attitude_indicator_plugin.h new file mode 100644 index 000000000..7a956a6a2 --- /dev/null +++ b/mapviz_plugins/include/mapviz_plugins/attitude_indicator_plugin.h @@ -0,0 +1,116 @@ +// ***************************************************************************** +// +// Copyright (c) 2014-2020, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MAPVIZ_PLUGINS__ATTITUDE_INDICATOR_PLUGIN_H_ +#define MAPVIZ_PLUGINS__ATTITUDE_INDICATOR_PLUGIN_H_ + +// Include mapviz_plugin.h first to ensure GL deps are included in the right order +#include + +// QT libraries +#include +#include +#include +#include + +// ROS libraries +#include +#include +#include +#include +#include + +#include +#include + +// C++ standard libraries +#include +#include +#include + +// QT autogenerated files +#include "ui_attitude_indicator_config.h" + +namespace mapviz_plugins +{ +class AttitudeIndicatorPlugin : public mapviz::MapvizPlugin +{ + Q_OBJECT + + public: + AttitudeIndicatorPlugin(); + ~AttitudeIndicatorPlugin() override = default; + + bool Initialize(QGLWidget* canvas) override; + void Shutdown() override; + + void Draw(double x, double y, double scale) override; + + void Transform() override {} + + void LoadConfig(const YAML::Node& node, const std::string& path) override; + void SaveConfig(YAML::Emitter& emitter, const std::string& path) override; + + QWidget* GetConfigWidget(QWidget* parent) override; + + protected: + void PrintError(const std::string& message) override; + void PrintInfo(const std::string& message) override; + void PrintWarning(const std::string& message) override; + + void drawBackground(); + void drawBall(); + void drawPanel(); + + void timerEvent(QTimerEvent *) override; + + protected Q_SLOTS: + void SelectTopic(); + void TopicEdited(); + + private: + void AttitudeCallbackImu(sensor_msgs::msg::Imu::ConstSharedPtr imu); + void AttitudeCallbackOdom(nav_msgs::msg::Odometry::ConstSharedPtr odometry); + void AttitudeCallbackPose(geometry_msgs::msg::Pose::ConstSharedPtr pose); + void applyAttitudeOrientation(const geometry_msgs::msg::Quaternion &orientation); + + double pitch_; + double roll_; + double yaw_; + PlaceableWindowProxy placer_; + QWidget* config_widget_; + rclcpp::Subscription::SharedPtr imu_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr pose_sub_; + std::string topic_; + std::vector topics_; + Ui::attitude_indicator_config ui_{}; +}; // class AttitudeIndicatorPlugin +} // namespace mapviz_plugins +#endif // MAPVIZ_PLUGINS__ATTITUDE_INDICATOR_PLUGIN_H_ diff --git a/mapviz_plugins/include/mapviz_plugins/canvas_click_filter.h b/mapviz_plugins/include/mapviz_plugins/canvas_click_filter.h new file mode 100644 index 000000000..80291d126 --- /dev/null +++ b/mapviz_plugins/include/mapviz_plugins/canvas_click_filter.h @@ -0,0 +1,73 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MAPVIZ_PLUGINS__CANVAS_CLICK_FILTER_H_ +#define MAPVIZ_PLUGINS__CANVAS_CLICK_FILTER_H_ + +#include +#include + +/** + * This is a very simple event filter that listens for mouseReleased events; + * when it sees one, it emits a signal with the given point. + * + * Click events are filtered by how long the mouse was held down and how far the + * cursor moved in order to prevent the user holding and dragging the map + * from firing a click event. By default, "clicks" that take longer than 500ms + * or move longer than 2 pixels are ignored. + */ +namespace mapviz_plugins +{ +class CanvasClickFilter : public QObject +{ + Q_OBJECT + +public: + CanvasClickFilter(); + + void setMaxClickTime(qint64 max_ms); + void setMaxClickMovement(qreal max_distance); + +Q_SIGNALS: + void pointClicked(const QPointF&); + +protected: + bool eventFilter(QObject *object, QEvent* event) override; + +private: + bool is_mouse_down_; + QPointF mouse_down_pos_; + qint64 mouse_down_time_; + + qint64 max_ms_; + qreal max_distance_; +}; +} // namespace mapviz_plugins + +#endif // MAPVIZ_PLUGINS__CANVAS_CLICK_FILTER_H_ diff --git a/mapviz_plugins/include/mapviz_plugins/coordinate_picker_plugin.h b/mapviz_plugins/include/mapviz_plugins/coordinate_picker_plugin.h new file mode 100644 index 000000000..28c4262f2 --- /dev/null +++ b/mapviz_plugins/include/mapviz_plugins/coordinate_picker_plugin.h @@ -0,0 +1,94 @@ +// ***************************************************************************** +// +// Copyright (c) 2018, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MAPVIZ_PLUGINS__COORDINATE_PICKER_PLUGIN_H_ +#define MAPVIZ_PLUGINS__COORDINATE_PICKER_PLUGIN_H_ + +#include + +// ROS Libraries +#include + +// Mapviz libraries +#include + +// C++ Standard Libraries +#include + +// QT autogenerated files +#include "ui_coordinate_picker_config.h" + +namespace mapviz_plugins +{ +class CoordinatePickerPlugin : public mapviz::MapvizPlugin +{ + Q_OBJECT + + public: + CoordinatePickerPlugin(); + ~CoordinatePickerPlugin() override; + + bool Initialize(QGLWidget* canvas) override; + void Shutdown() override { } + + void Draw(double x, double y, double scale) override; + void Transform() override { } + + void LoadConfig(const YAML::Node& node, const std::string& path) override; + void SaveConfig(YAML::Emitter& emitter, const std::string& path) override; + + QWidget* GetConfigWidget(QWidget* parent) override; + + void PrintError(const std::string& message) override; + void PrintInfo(const std::string& message) override; + void PrintWarning(const std::string& message) override; + + protected: + bool eventFilter(QObject* object, QEvent* event) override; + bool handleMousePress(QMouseEvent*); + bool handleMouseRelease(QMouseEvent*); + bool handleMouseMove(QMouseEvent*); + + protected Q_SLOTS: + void SelectFrame(); + void FrameEdited(); + void ToggleCopyOnClick(int state); + void ClearCoordList(); + + private: + Ui::coordinate_picker_config ui_; + QWidget* config_widget_; + mapviz::MapCanvas* map_canvas_; + + bool copy_on_click_; +}; +} // namespace mapviz_plugins + +#endif // MAPVIZ_PLUGINS__COORDINATE_PICKER_PLUGIN_H_ + diff --git a/mapviz_plugins/include/mapviz_plugins/disparity_plugin.h b/mapviz_plugins/include/mapviz_plugins/disparity_plugin.h new file mode 100644 index 000000000..8a82f342f --- /dev/null +++ b/mapviz_plugins/include/mapviz_plugins/disparity_plugin.h @@ -0,0 +1,150 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MAPVIZ_PLUGINS__DISPARITY_PLUGIN_H_ +#define MAPVIZ_PLUGINS__DISPARITY_PLUGIN_H_ + +// Include mapviz_plugin.h first to ensure GL deps are included in the right order +#include + +// QT libraries +#include +#include +#include +#include + +// ROS libraries +#ifdef USE_CVBRIDGE_H_FILES +#include +#else +#include +#endif +#include +#include +#include +#include + +#include + +// C++ standard libraries +#include +#include + +// QT autogenerated files +#include "ui_disparity_config.h" + +namespace mapviz_plugins +{ +class DisparityPlugin : public mapviz::MapvizPlugin +{ + Q_OBJECT + +public: + enum Anchor { + TOP_LEFT, + TOP_CENTER, + TOP_RIGHT, + CENTER_LEFT, + CENTER, + CENTER_RIGHT, + BOTTOM_LEFT, + BOTTOM_CENTER, + BOTTOM_RIGHT}; + + enum Units {PIXELS, PERCENT}; + + DisparityPlugin(); + ~DisparityPlugin() override = default; + + bool Initialize(QGLWidget* canvas) override; + void Shutdown() override {} + + void Draw(double x, double y, double scale) override; + + void Transform() override {} + + void LoadConfig(const YAML::Node& node, const std::string& path) override; + void SaveConfig(YAML::Emitter& emitter, const std::string& path) override; + + QWidget* GetConfigWidget(QWidget* parent) override; + +protected: + void PrintError(const std::string& message) override; + void PrintInfo(const std::string& message) override; + void PrintWarning(const std::string& message) override; + +protected Q_SLOTS: + void SelectTopic(); + void TopicEdited(); + void SetAnchor(QString anchor); + void SetUnits(QString units); + void SetOffsetX(int offset); + void SetOffsetY(int offset); + void SetWidth(int width); + void SetHeight(int height); + void SetSubscription(bool visible); + +private: + Ui::disparity_config ui_; + QWidget* config_widget_; + + std::string topic_; + Anchor anchor_; + Units units_; + double offset_x_; + double offset_y_; + double width_; + double height_; + + bool has_image_; + + double last_width_; + double last_height_; + + rclcpp::Subscription::SharedPtr disparity_sub_; + bool has_message_; + + stereo_msgs::msg::DisparityImage disparity_; + + cv::Mat_ disparity_color_; + cv::Mat scaled_image_; + + void disparityCallback(const stereo_msgs::msg::DisparityImage::SharedPtr image); + + void ScaleImage(double width, double height); + void DrawIplImage(cv::Mat *image); + + std::string AnchorToString(Anchor anchor); + std::string UnitsToString(Units units); + + static const unsigned char COLOR_MAP[]; +}; +} // namespace mapviz_plugins + +#endif // MAPVIZ_PLUGINS__DISPARITY_PLUGIN_H_ diff --git a/mapviz_plugins/include/mapviz_plugins/draw_polygon_plugin.h b/mapviz_plugins/include/mapviz_plugins/draw_polygon_plugin.h new file mode 100644 index 000000000..d60fd49e7 --- /dev/null +++ b/mapviz_plugins/include/mapviz_plugins/draw_polygon_plugin.h @@ -0,0 +1,115 @@ +// ***************************************************************************** +// +// Copyright (c) 2016, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MAPVIZ_PLUGINS__DRAW_POLYGON_PLUGIN_H_ +#define MAPVIZ_PLUGINS__DRAW_POLYGON_PLUGIN_H_ + +#include + +// QT libraries +#include +#include +#include + +// ROS libraries +#include +#include + +// Mapviz libraries +#include + +// Messages +#include + +// C++ standard libraries +#include +#include + +// QT autogenerated files +#include "ui_draw_polygon_config.h" + +namespace mapviz_plugins +{ +class DrawPolygonPlugin : public mapviz::MapvizPlugin +{ + Q_OBJECT + + public: + DrawPolygonPlugin(); + ~DrawPolygonPlugin() override; + + bool Initialize(QGLWidget* canvas) override; + void Shutdown() override {} + + void Draw(double x, double y, double scale) override; + + void Transform() override {}; + + void LoadConfig(const YAML::Node& node, const std::string& path) override; + void SaveConfig(YAML::Emitter& emitter, const std::string& path) override; + + QWidget* GetConfigWidget(QWidget* parent) override; + + protected: + void PrintError(const std::string& message) override; + void PrintInfo(const std::string& message) override; + void PrintWarning(const std::string& message) override; + bool eventFilter(QObject *object, QEvent* event) override; + bool handleMousePress(QMouseEvent *); + bool handleMouseRelease(QMouseEvent *); + bool handleMouseMove(QMouseEvent *); + + protected Q_SLOTS: + void PublishPolygon(); + void Clear(); + void SelectFrame(); + void FrameEdited(); + + private: + Ui::draw_polygon_config ui_; + QWidget* config_widget_; + mapviz::MapCanvas* map_canvas_; + + std::string polygon_topic_; + rclcpp::Publisher::SharedPtr polygon_pub_; + + std::vector vertices_; + std::vector transformed_vertices_; + + int selected_point_; + bool is_mouse_down_; + QPointF mouse_down_pos_; + qint64 mouse_down_time_; + + qint64 max_ms_; + qreal max_distance_; +}; +} // namespace mapviz_plugins + +#endif // MAPVIZ_PLUGINS__DRAW_POLYGON_PLUGIN_H_ diff --git a/mapviz_plugins/include/mapviz_plugins/float_plugin.h b/mapviz_plugins/include/mapviz_plugins/float_plugin.h new file mode 100644 index 000000000..2abee56a7 --- /dev/null +++ b/mapviz_plugins/include/mapviz_plugins/float_plugin.h @@ -0,0 +1,160 @@ +// ***************************************************************************** +// +// Copyright (c) 2019, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL Southwest Research Institute® BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY +// OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +// DAMAGE. +// +// ***************************************************************************** + +#ifndef MAPVIZ_PLUGINS_FLOAT_PLUGIN_H +#define MAPVIZ_PLUGINS_FLOAT_PLUGIN_H + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +// QT autogenerated files +#include "ui_float_config.h" + +namespace mapviz_plugins +{ + class FloatPlugin : public mapviz::MapvizPlugin + { + Q_OBJECT + + public: + enum Anchor { + TOP_LEFT, + TOP_CENTER, + TOP_RIGHT, + CENTER_LEFT, + CENTER, + CENTER_RIGHT, + BOTTOM_LEFT, + BOTTOM_CENTER, + BOTTOM_RIGHT + }; + + enum Units { + PIXELS, + PERCENT + }; + + FloatPlugin(); + ~FloatPlugin() override = default; + + bool Initialize(QGLWidget* canvas) override; + void Shutdown() override {} + + void Draw(double x, double y, double scale) override; + void Paint(QPainter* painter, double x, double y, double scale) override; + + void Transform() override {} + + void LoadConfig(const YAML::Node& node, const std::string& path) override; + void SaveConfig(YAML::Emitter& emitter, const std::string& path) override; + + QWidget* GetConfigWidget(QWidget* parent) override; + + bool SupportsPainting() override + { + return true; + } + + protected: + void PaintText(QPainter* painter); + void PrintError(const std::string& message) override; + void PrintInfo(const std::string& message) override; + void PrintWarning(const std::string& message) override; + + protected Q_SLOTS: + void SelectColor(); + void SelectFont(); + void SelectTopic(); + void TopicEdited(); + void SetAnchor(QString anchor); + void SetUnits(QString units); + void SetOffsetX(int offset); + void SetOffsetY(int offset); + void PostfixEdited(); + + private: + Ui::float_config ui_; + QWidget* config_widget_; + + std::string topic_; + std::string postfix_; + Anchor anchor_; + Units units_; + int offset_x_; + int offset_y_; + + rclcpp::Subscription::SharedPtr float32_sub_; + rclcpp::Subscription::SharedPtr float64_sub_; + rclcpp::Subscription::SharedPtr float32_stamped_sub_; + rclcpp::Subscription::SharedPtr float64_stamped_sub_; + rclcpp::Subscription::SharedPtr velocity_sub_; + bool has_message_; + bool has_painted_; + + QColor color_; + QFont font_; + QStaticText message_; + + void floatCallback(double value); + + std::string AnchorToString(Anchor anchor); + std::string UnitsToString(Units units); + + static const char* ANCHOR_KEY; + static const char* COLOR_KEY; + static const char* FONT_KEY; + static const char* OFFSET_X_KEY; + static const char* OFFSET_Y_KEY; + static const char* TOPIC_KEY; + static const char* UNITS_KEY; + static const char* POSTFIX_KEY; + }; +} + + +#endif //MAPVIZ_PLUGINS_FLOAT_PLUGIN_H diff --git a/mapviz_plugins/include/mapviz_plugins/gps_plugin.h b/mapviz_plugins/include/mapviz_plugins/gps_plugin.h new file mode 100644 index 000000000..97d18a298 --- /dev/null +++ b/mapviz_plugins/include/mapviz_plugins/gps_plugin.h @@ -0,0 +1,91 @@ +// ***************************************************************************** +// +// Copyright (C) 2013 All Right Reserved, Southwest Research Institute® (SwRI®) +// +// Contract No. 10-58058A +// Contractor Southwest Research Institute® (SwRI®) +// Address 6220 Culebra Road, San Antonio, Texas 78228-0510 +// Contact Steve Dellenback (210) 522-3914 +// +// This code was developed as part of an internal research project fully funded +// by Southwest Research Institute®. +// +// THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY +// KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A +// PARTICULAR PURPOSE. +// +// ***************************************************************************** + +#ifndef MAPVIZ_PLUGINS__GPS_PLUGIN_H_ +#define MAPVIZ_PLUGINS__GPS_PLUGIN_H_ + +// Include mapviz_plugin.h first to ensure GL deps are included in the right order +#include + +#include +#include + +// QT libraries +#include +#include +#include + +// ROS libraries +#include +#include +#include +#include + +// C++ standard libraries +#include +#include +#include + +// QT autogenerated files +#include "ui_gps_config.h" + +namespace mapviz_plugins +{ +class GpsPlugin : public mapviz_plugins::PointDrawingPlugin +{ + Q_OBJECT + + public: + GpsPlugin(); + ~GpsPlugin() override = default; + + bool Initialize(QGLWidget* canvas) override; + void Shutdown() override {} + + void Draw(double x, double y, double scale) override; + + void LoadConfig(const YAML::Node& node, const std::string& path) override; + void SaveConfig(YAML::Emitter& emitter, const std::string& path) override; + + QWidget* GetConfigWidget(QWidget* parent) override; + + protected: + void PrintError(const std::string& message) override; + void PrintInfo(const std::string& message) override; + void PrintWarning(const std::string& message) override; + + protected Q_SLOTS: + void SelectTopic(); + void TopicEdited(); + + private: + Ui::gps_config ui_; + QWidget* config_widget_; + + std::string topic_; + + // ros::Subscriber gps_sub_; + rclcpp::Subscription::SharedPtr gps_sub_; + bool has_message_; + + void GPSFixCallback(const gps_msgs::msg::GPSFix::SharedPtr gps); +}; +} // namespace mapviz_plugins + +#endif // MAPVIZ_PLUGINS__GPS_PLUGIN_H_ diff --git a/mapviz_plugins/include/mapviz_plugins/grid_plugin.h b/mapviz_plugins/include/mapviz_plugins/grid_plugin.h new file mode 100644 index 000000000..e4043197f --- /dev/null +++ b/mapviz_plugins/include/mapviz_plugins/grid_plugin.h @@ -0,0 +1,123 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MAPVIZ_PLUGINS__GRID_PLUGIN_H_ +#define MAPVIZ_PLUGINS__GRID_PLUGIN_H_ + +#include + +// QT libraries +#include +#include +#include +#include + +// ROS libraries +#include +#include + +#include + +// C++ standard libraries +#include +#include + +// QT autogenerated files +#include "ui_grid_config.h" + +namespace mapviz_plugins +{ +class GridPlugin : public mapviz::MapvizPlugin +{ + Q_OBJECT + +public: + GridPlugin(); + ~GridPlugin() override = default; + + bool Initialize(QGLWidget* canvas) override; + void Shutdown() override {} + + void Draw(double x, double y, double scale) override; + + void Transform() override; + + void LoadConfig(const YAML::Node& node, const std::string& path) override; + void SaveConfig(YAML::Emitter& emitter, const std::string& path) override; + + QWidget* GetConfigWidget(QWidget* parent) override; + +protected: + void PrintError(const std::string& message) override; + void PrintInfo(const std::string& message) override; + void PrintWarning(const std::string& message) override; + +protected Q_SLOTS: + void SelectFrame(); + void FrameEdited(); + void SetAlpha(double alpha); + void SetX(double x); + void SetY(double y); + void SetSize(double size); + void SetRows(int rows); + void SetColumns(int columns); + void DrawIcon() override; + +private: + Ui::grid_config ui_; + QWidget* config_widget_; + + double alpha_; + + tf2::Vector3 top_left_; + + double size_; + int rows_; + int columns_; + + bool transformed_; + + std::list top_points_; + std::list bottom_points_; + std::list left_points_; + std::list right_points_; + + std::list transformed_top_points_; + std::list transformed_bottom_points_; + std::list transformed_left_points_; + std::list transformed_right_points_; + + swri_transform_util::Transform transform_; + + void RecalculateGrid(); + void Transform(std::list& src, std::list& dst); +}; +} // namespace mapviz_plugins + +#endif // MAPVIZ_PLUGINS__GRID_PLUGIN_H_ diff --git a/mapviz_plugins/include/mapviz_plugins/image_plugin.h b/mapviz_plugins/include/mapviz_plugins/image_plugin.h new file mode 100644 index 000000000..2e65d93de --- /dev/null +++ b/mapviz_plugins/include/mapviz_plugins/image_plugin.h @@ -0,0 +1,157 @@ +// ***************************************************************************** +// +// Copyright (c) 2014-2020, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MAPVIZ_PLUGINS__IMAGE_PLUGIN_H_ +#define MAPVIZ_PLUGINS__IMAGE_PLUGIN_H_ + +#include + +// QT libraries +#include +#include +#include +#include + +// ROS libraries +#include +#include +#include +#include +#ifdef USE_CVBRIDGE_H_FILES +#include +#else +#include +#endif +#include + +#include + +// C++ standard libraries +#include +#include + +// QT autogenerated files +#include "ui_image_config.h" + +namespace mapviz_plugins +{ +class ImagePlugin : public mapviz::MapvizPlugin +{ + Q_OBJECT + +public: + enum Anchor { + TOP_LEFT, + TOP_CENTER, + TOP_RIGHT, + CENTER_LEFT, + CENTER, + CENTER_RIGHT, + BOTTOM_LEFT, + BOTTOM_CENTER, + BOTTOM_RIGHT + }; + + enum Units {PIXELS, PERCENT}; + + ImagePlugin(); + ~ImagePlugin() override = default; + + bool Initialize(QGLWidget* canvas) override; + void Shutdown() override {} + + void Draw(double x, double y, double scale) override; + + void SetNode(rclcpp::Node& node) override; + + void Transform() override {} + + void LoadConfig(const YAML::Node& node, const std::string& path) override; + void SaveConfig(YAML::Emitter& emitter, const std::string& path) override; + + QWidget* GetConfigWidget(QWidget* parent) override; + +protected: + void PrintError(const std::string& message) override; + void PrintInfo(const std::string& message) override; + void PrintWarning(const std::string& message) override; + +public Q_SLOTS: + void Resubscribe(); + +protected Q_SLOTS: + void SelectTopic(); + void TopicEdited(); + void SetAnchor(QString anchor); + void SetUnits(QString units); + void SetOffsetX(int offset); + void SetOffsetY(int offset); + void SetWidth(double width); + void SetHeight(double height); + void SetSubscription(bool visible); + void SetTransport(const QString& transport); + void KeepRatioChanged(bool checked); + +private: + Ui::image_config ui_; + QWidget* config_widget_; + + std::string topic_; + Anchor anchor_; + Units units_; + int offset_x_; + int offset_y_; + double width_; + double height_; + std::string transport_; + + bool force_resubscribe_; + bool has_image_; + + double last_width_; + double last_height_; + double original_aspect_ratio_; + + image_transport::Subscriber image_sub_; + bool has_message_; + + cv_bridge::CvImagePtr cv_image_; + cv::Mat scaled_image_; + + void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& image); + + void ScaleImage(double width, double height); + void DrawIplImage(cv::Mat *image); + + std::string AnchorToString(Anchor anchor); + std::string UnitsToString(Units units); +}; +} // namespace mapviz_plugins + +#endif // MAPVIZ_PLUGINS__IMAGE_PLUGIN_H_ diff --git a/mapviz_plugins/include/mapviz_plugins/laserscan_plugin.h b/mapviz_plugins/include/mapviz_plugins/laserscan_plugin.h new file mode 100644 index 000000000..a7a1405f6 --- /dev/null +++ b/mapviz_plugins/include/mapviz_plugins/laserscan_plugin.h @@ -0,0 +1,153 @@ +// ***************************************************************************** +// +// Copyright (c) 2015, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MAPVIZ_PLUGINS__LASERSCAN_PLUGIN_H_ +#define MAPVIZ_PLUGINS__LASERSCAN_PLUGIN_H_ + +#include + +// QT libraries +#include +#include + +// ROS libraries +#include +#include + +// C++ standard libraries +#include +#include +#include + +// QT autogenerated files +#include "ui_laserscan_config.h" + +namespace mapviz_plugins +{ +class LaserScanPlugin : public mapviz::MapvizPlugin +{ + Q_OBJECT + + public: + enum + { + COLOR_FLAT = 0, + COLOR_INTENSITY = 1, + COLOR_RANGE = 2, + COLOR_X = 3, + COLOR_Y = 4, + COLOR_Z = 5 + }; + LaserScanPlugin(); + ~LaserScanPlugin() override = default; + + bool Initialize(QGLWidget* canvas) override; + void Shutdown() override {} + + void ClearHistory() override; + + void Draw(double x, double y, double scale) override; + + void Transform() override; + + void LoadConfig(const YAML::Node& node, const std::string& path) override; + void SaveConfig(YAML::Emitter& emitter, const std::string& path) override; + + QWidget* GetConfigWidget(QWidget* parent) override; + + protected: + void PrintError(const std::string& message) override; + void PrintInfo(const std::string& message) override; + void PrintWarning(const std::string& message) override; + + protected Q_SLOTS: + void SelectTopic(); + void TopicEdited(); + void AlphaEdited(double val); + void ColorTransformerChanged(int index); + void MinValueChanged(double value); + void MaxValueChanged(double value); + void PointSizeChanged(int value); + void BufferSizeChanged(int value); + void UseRainbowChanged(int check_state); + void UpdateColors(); + void DrawIcon() override; + void ResetTransformedScans(); + + private: + struct StampedPoint + { + tf2::Vector3 point; + tf2::Vector3 transformed_point; + QColor color; + float range; + float intensity; + }; + + struct Scan + { + rclcpp::Time stamp; + QColor color; + std::vector points; + std::string source_frame_; + bool transformed; + bool has_intensity; + }; + + void laserScanCallback(const sensor_msgs::msg::LaserScan::SharedPtr scan); + QColor CalculateColor(const StampedPoint& point, bool has_intensity); + void updatePreComputedTriginometic(const sensor_msgs::msg::LaserScan::SharedPtr msg); + + Ui::laserscan_config ui_; + QWidget* config_widget_; + + std::string topic_; + double alpha_; + double min_value_; + double max_value_; + size_t point_size_; + size_t buffer_size_; + + bool has_message_; + + // Use a list instead of a deque for scans to facilitate removing + // timed-out scans in the middle of the list in case I ever re-implement + // decay time (evenator) + std::deque scans_; + rclcpp::Subscription::SharedPtr laserscan_sub_; + std::vector precomputed_cos_; + std::vector precomputed_sin_; + size_t prev_ranges_size_; + float prev_angle_min_; + float prev_increment_; + bool GetScanTransform(const Scan &scan, swri_transform_util::Transform& transform); +}; +} // namespace mapviz_plugins + +#endif // MAPVIZ_PLUGINS__LASERSCAN_PLUGIN_H_ diff --git a/mapviz_plugins/include/mapviz_plugins/marker_plugin.h b/mapviz_plugins/include/mapviz_plugins/marker_plugin.h new file mode 100644 index 000000000..198d31eda --- /dev/null +++ b/mapviz_plugins/include/mapviz_plugins/marker_plugin.h @@ -0,0 +1,179 @@ +// ***************************************************************************** +// +// Copyright (c) 2014-2020, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MAPVIZ_PLUGINS__MARKER_PLUGIN_H_ +#define MAPVIZ_PLUGINS__MARKER_PLUGIN_H_ + +#include + +// QT libraries +#include +#include + +// ROS libraries +#include +#include +#include + +#include + +#include + +// C++ standard libraries +#include +#include +#include +#include + +// QT autogenerated files +#include "ui_marker_config.h" + +namespace mapviz_plugins +{ +using MarkerId = std::pair; + +struct MarkerIdHash { + std::size_t operator () (const MarkerId &p) const { + std::size_t seed = 0; + boost::hash_combine(seed, p.first); + boost::hash_combine(seed, p.second); + return seed; + } +}; + +struct MarkerNsHash { + std::size_t operator () (const std::string &p) const { + std::size_t seed = 0; + boost::hash_combine(seed, p); + return seed; + } +}; + +class MarkerPlugin : public mapviz::MapvizPlugin +{ + Q_OBJECT + +public: + MarkerPlugin(); + ~MarkerPlugin() override = default; + + bool Initialize(QGLWidget* canvas) override; + void Shutdown() override {} + + void Draw(double x, double y, double scale) override; + void Paint(QPainter* painter, double x, double y, double scale) override; + + void Transform() override; + + void LoadConfig(const YAML::Node& node, const std::string& path) override; + void SaveConfig(YAML::Emitter& emitter, const std::string& path) override; + + QWidget* GetConfigWidget(QWidget* parent) override; + + bool SupportsPainting() override + { + return true; + } + +protected: + void PrintError(const std::string& message) override; + void PrintInfo(const std::string& message) override; + void PrintWarning(const std::string& message) override; + void timerEvent(QTimerEvent *) override; + +protected Q_SLOTS: + void SelectTopic(); + void TopicEdited(); + void ClearHistory() override; + +private: + struct Color + { + float r, g, b, a; + }; + + struct StampedPoint + { + tf2::Vector3 point; + tf2::Quaternion orientation; + + tf2::Vector3 transformed_point; + + tf2::Vector3 arrow_point; + tf2::Vector3 transformed_arrow_point; + tf2::Vector3 transformed_arrow_left; + tf2::Vector3 transformed_arrow_right; + + Color color; + }; + + struct MarkerData + { + rclcpp::Time stamp; + rclcpp::Time expire_time; + + int display_type; + Color color; + + std::vector points; + std::string text; + + float scale_x; + float scale_y; + float scale_z; + + std::string source_frame; + swri_transform_util::Transform local_transform; + + bool transformed; + }; + + Ui::marker_config ui_{}; + QWidget* config_widget_; + + std::string topic_; + + rclcpp::Subscription::SharedPtr marker_sub_; + rclcpp::Subscription::SharedPtr marker_array_sub_; + bool connected_; + bool has_message_{}; + + std::unordered_map markers_; + std::unordered_map marker_visible_; + + void handleMarker(visualization_msgs::msg::Marker::ConstSharedPtr marker); + void handleMarkerArray(visualization_msgs::msg::MarkerArray::ConstSharedPtr markers); + void processMarker(const visualization_msgs::msg::Marker& marker); + void subscribe(); + void transformArrow(MarkerData& markerData, + const swri_transform_util::Transform& transform); +}; +} // namespace mapviz_plugins + +#endif // MAPVIZ_PLUGINS__MARKER_PLUGIN_H_ diff --git a/mapviz_plugins/include/mapviz_plugins/measuring_plugin.h b/mapviz_plugins/include/mapviz_plugins/measuring_plugin.h new file mode 100644 index 000000000..9402fbbc3 --- /dev/null +++ b/mapviz_plugins/include/mapviz_plugins/measuring_plugin.h @@ -0,0 +1,123 @@ +// ***************************************************************************** +// +// Copyright (c) 2018, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MAPVIZ_PLUGINS__MEASURING_PLUGIN_H_ +#define MAPVIZ_PLUGINS__MEASURING_PLUGIN_H_ + +#include + +// ROS Libraries +#include + +// Mapviz libraries +#include + +// C++ libraries +#include +#include + +// QT autogenerated files +#include "ui_measuring_config.h" + +namespace mapviz_plugins +{ +class MeasuringPlugin : public mapviz::MapvizPlugin +{ + Q_OBJECT + + public: + MeasuringPlugin(); + ~MeasuringPlugin() override; + + bool Initialize(QGLWidget* canvas) override; + void Shutdown() override { } + + void Paint(QPainter* painter, double x, double y, double scale) override; + void Draw(double x, double y, double scale) override; + void Transform() override { } + + void LoadConfig(const YAML::Node& node, const std::string& path) override; + void SaveConfig(YAML::Emitter& emitter, const std::string& path) override; + + void DistanceCalculation(); + + QWidget* GetConfigWidget(QWidget* parent) override; + + void PrintError(const std::string& message) override; + void PrintInfo(const std::string& message) override; + void PrintWarning(const std::string& message) override; + + bool SupportsPainting() override + { + return true; + } + + protected: + bool eventFilter(QObject* object, QEvent* event) override; + bool handleMousePress(QMouseEvent*); + bool handleMouseRelease(QMouseEvent*); + bool handleMouseMove(QMouseEvent*); + + protected Q_SLOTS: + void Clear(); + void BkgndColorToggled(bool) { } + void MeasurementsToggled(bool) { } + void FontSizeChanged(int) { } + void AlphaChanged(double) { } + + private: + Ui::measuring_config ui_; + QWidget* config_widget_; + + mapviz::MapCanvas* map_canvas_; + tf2::Vector3 last_position_; + + std::vector vertices_; + std::vector transformed_vertices_; + + int selected_point_; + bool is_mouse_down_; + QPointF mouse_down_pos_; + qint64 mouse_down_time_; + + qint64 max_ms_; + qreal max_distance_; + std::vector measurements_; +}; + +struct MeasurementBox +{ + QRectF rect; + QString string; +}; + +} // namespace mapviz_plugins + +#endif // MAPVIZ_PLUGINS__MEASURING_PLUGIN_H_ + diff --git a/mapviz_plugins/include/mapviz_plugins/move_base_plugin.h b/mapviz_plugins/include/mapviz_plugins/move_base_plugin.h new file mode 100644 index 000000000..cfe3059df --- /dev/null +++ b/mapviz_plugins/include/mapviz_plugins/move_base_plugin.h @@ -0,0 +1,118 @@ +// ***************************************************************************** +// +// Copyright (c) 2017, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MAPVIZ_PLUGINS_PLAN_ROUTE_PLUGIN_H_ +#define MAPVIZ_PLUGINS_PLAN_ROUTE_PLUGIN_H_ + +// C++ standard libraries +#include +#include +#include + +// QT libraries +#include +#include +#include + +// ROS libraries +#include +#include + +// Mapviz libraries +#include + +#include +#include +#include + +// QT autogenerated files +#include "ui_move_base_config.h" + +namespace mapviz_plugins +{ + class MoveBasePlugin : public mapviz::MapvizPlugin + { + Q_OBJECT + + public: + typedef actionlib::SimpleActionClient MoveBaseClient; + + MoveBasePlugin(); + virtual ~MoveBasePlugin(); + + bool Initialize(QGLWidget* canvas); + void Shutdown() {} + + void Draw(double x, double y, double scale); + + void Paint(QPainter* painter, double x, double y, double scale) {} + void Transform() {} + + void LoadConfig(const YAML::Node& node, const std::string& path); + void SaveConfig(YAML::Emitter& emitter, const std::string& path); + + QWidget* GetConfigWidget(QWidget* parent); + + protected: + virtual void PrintError(const std::string& message) override; + virtual void PrintInfo(const std::string& message) override; + virtual void PrintWarning(const std::string& message) override; + virtual bool eventFilter(QObject *object, QEvent* event) override; + void timerCallback(const ros::TimerEvent& ev = ros::TimerEvent() ); + + bool handleMousePress(QMouseEvent *); + bool handleMouseRelease(QMouseEvent *); + bool handleMouseMove(QMouseEvent *); + + private Q_SLOTS: + void on_pushButtonInitialPose_toggled(bool checked); + void on_pushButtonGoalPose_toggled(bool checked); + void on_pushButtonAbort_clicked(); + + private: + + Ui::move_base_config ui_; + QWidget* config_widget_; + mapviz::MapCanvas* map_canvas_; + + ros::NodeHandle nh_; + ros::Publisher init_pose_pub_; + + bool is_mouse_down_; + QPointF arrow_tail_position_; + float arrow_angle_; + + MoveBaseClient move_base_client_; + move_base_msgs::MoveBaseAction move_base_msg_; + ros::Timer timer_; + bool monitoring_action_state_; + }; +} + +#endif // MAPVIZ_PLUGINS_PLAN_ROUTE_PLUGIN_H_ diff --git a/mapviz_plugins/include/mapviz_plugins/navsat_plugin.h b/mapviz_plugins/include/mapviz_plugins/navsat_plugin.h new file mode 100644 index 000000000..d3baa155a --- /dev/null +++ b/mapviz_plugins/include/mapviz_plugins/navsat_plugin.h @@ -0,0 +1,88 @@ +// ***************************************************************************** +// +// Copyright (C) 2013 All Right Reserved, Southwest Research Institute® (SwRI®) +// +// Contract No. 10-58058A +// Contractor Southwest Research Institute® (SwRI®) +// Address 6220 Culebra Road, San Antonio, Texas 78228-0510 +// Contact Steve Dellenback (210) 522-3914 +// +// This code was developed as part of an internal research project fully funded +// by Southwest Research Institute®. +// +// THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY +// KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A +// PARTICULAR PURPOSE. +// +// ***************************************************************************** + +#ifndef MAPVIZ_PLUGINS__NAVSAT_PLUGIN_H_ +#define MAPVIZ_PLUGINS__NAVSAT_PLUGIN_H_ + +#include +#include +#include + +// QT libraries +#include +#include +#include + +// ROS libraries +#include +#include +#include +#include + +// C++ standard libraries +#include +#include +#include + +// QT autogenerated files +#include "ui_navsat_config.h" + +namespace mapviz_plugins +{ +class NavSatPlugin : public mapviz_plugins::PointDrawingPlugin +{ + Q_OBJECT + + public: + NavSatPlugin(); + ~NavSatPlugin() override = default; + + bool Initialize(QGLWidget* canvas) override; + void Shutdown() override {} + + void Draw(double x, double y, double scale) override; + + void LoadConfig(const YAML::Node& node, const std::string& path) override; + void SaveConfig(YAML::Emitter& emitter, const std::string& path) override; + + QWidget* GetConfigWidget(QWidget* parent) override; + + protected: + void PrintError(const std::string& message) override; + void PrintInfo(const std::string& message) override; + void PrintWarning(const std::string& message) override; + + protected Q_SLOTS: + void SelectTopic(); + void TopicEdited(); + + private: + Ui::navsat_config ui_; + QWidget* config_widget_; + + std::string topic_; + + rclcpp::Subscription::SharedPtr navsat_sub_; + bool has_message_; + + void NavSatFixCallback(const sensor_msgs::msg::NavSatFix::ConstSharedPtr navsat); +}; +} // namespace mapviz_plugins + +#endif // MAPVIZ_PLUGINS__NAVSAT_PLUGIN_H_ diff --git a/mapviz_plugins/include/mapviz_plugins/occupancy_grid_plugin.h b/mapviz_plugins/include/mapviz_plugins/occupancy_grid_plugin.h new file mode 100644 index 000000000..f42ac6496 --- /dev/null +++ b/mapviz_plugins/include/mapviz_plugins/occupancy_grid_plugin.h @@ -0,0 +1,126 @@ +// ***************************************************************************** +// +// Copyright (c) 2018, Eurecat +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Eurecat nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MAPVIZ_PLUGINS__OCCUPANCY_GRID_PLUGIN_H_ +#define MAPVIZ_PLUGINS__OCCUPANCY_GRID_PLUGIN_H_ + +#include + +// QT libraries +#include +#include +#include +#include + +// ROS libraries +#include +#include + +#include +#include +#include + +// C++ standard libraries +#include +#include +#include + +// QT autogenerated files +#include "ui_occupancy_grid_config.h" + +namespace mapviz_plugins +{ +class OccupancyGridPlugin : public mapviz::MapvizPlugin +{ + Q_OBJECT + + typedef std::array Palette; + +public: + OccupancyGridPlugin(); + ~OccupancyGridPlugin() override = default; + + bool Initialize(QGLWidget* canvas) override; + void Shutdown() override {} + + void Draw(double x, double y, double scale) override; + + void Transform() override; + + void LoadConfig(const YAML::Node& node, const std::string& path) override; + void SaveConfig(YAML::Emitter& emitter, const std::string& path) override; + + QWidget* GetConfigWidget(QWidget* parent) override; + +protected: + void PrintError(const std::string& message) override; + void PrintInfo(const std::string& message) override; + void PrintWarning(const std::string& message) override; + +protected Q_SLOTS: + + void SelectTopicGrid(); + void TopicGridEdited(); + void upgradeCheckBoxToggled(bool); + void colorSchemeUpdated(const QString &); + + void DrawIcon() override; + + void FrameChanged(std::string); + +private: + Ui::occupancy_grid_config ui_; + QWidget* config_widget_; + + nav_msgs::msg::OccupancyGrid::SharedPtr grid_; + + rclcpp::Subscription::SharedPtr grid_sub_; + rclcpp::Subscription::SharedPtr update_sub_; + + bool transformed_; + swri_transform_util::Transform transform_; + + GLuint texture_id_; + + QPointF map_origin_; + float texture_x_, texture_y_; + std::vector raw_buffer_; + std::vector color_buffer_; + uint32_t texture_size_; + + Palette map_palette_; + Palette costmap_palette_; + + void Callback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg); + void CallbackUpdate(const map_msgs::msg::OccupancyGridUpdate::SharedPtr msg); + void updateTexture(); +}; +} // namespace mapviz_plugins + +#endif // MAPVIZ_PLUGINS__OCCUPANCY_GRID_PLUGIN_H_ diff --git a/mapviz_plugins/include/mapviz_plugins/odometry_plugin.h b/mapviz_plugins/include/mapviz_plugins/odometry_plugin.h new file mode 100644 index 000000000..80e3ac838 --- /dev/null +++ b/mapviz_plugins/include/mapviz_plugins/odometry_plugin.h @@ -0,0 +1,100 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MAPVIZ_PLUGINS__ODOMETRY_PLUGIN_H_ +#define MAPVIZ_PLUGINS__ODOMETRY_PLUGIN_H_ + +#include +#include +// QT libraries +#include +#include +#include + +// ROS libraries +#include +#include +#include + +#include + +// C++ standard libraries +#include +#include +#include + +// QT autogenerated files +#include "ui_odometry_config.h" + +namespace mapviz_plugins +{ +class OdometryPlugin : public mapviz_plugins::PointDrawingPlugin +{ + Q_OBJECT + + public: + OdometryPlugin(); + ~OdometryPlugin() override = default; + + bool Initialize(QGLWidget* canvas) override; + void Shutdown() override {} + + void Paint(QPainter* painter, double x, double y, double scale) override; + void Draw(double x, double y, double scale) override; + void LoadConfig(const YAML::Node& node, const std::string& path) override; + void SaveConfig(YAML::Emitter& emitter, const std::string& path) override; + + QWidget* GetConfigWidget(QWidget* parent) override; + + bool SupportsPainting() override + { + return true; + } + + protected: + void PrintError(const std::string& message) override; + void PrintInfo(const std::string& message) override; + void PrintWarning(const std::string& message) override; + + protected Q_SLOTS: + void SelectTopic(); + void TopicEdited(); + + private: + Ui::odometry_config ui_; + QWidget* config_widget_; + std::string topic_; + rclcpp::Subscription::SharedPtr odometry_sub_; + bool has_message_; + void odometryCallback(const nav_msgs::msg::Odometry::SharedPtr odometry); +}; +} // namespace mapviz_plugins + +#endif // MAPVIZ_PLUGINS__ODOMETRY_PLUGIN_H_ + diff --git a/mapviz_plugins/include/mapviz_plugins/path_plugin.h b/mapviz_plugins/include/mapviz_plugins/path_plugin.h new file mode 100644 index 000000000..b47c7150d --- /dev/null +++ b/mapviz_plugins/include/mapviz_plugins/path_plugin.h @@ -0,0 +1,97 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MAPVIZ_PLUGINS__PATH_PLUGIN_H_ +#define MAPVIZ_PLUGINS__PATH_PLUGIN_H_ + +#include + +// QT libraries +#include +#include +#include + +// ROS libraries +#include +#include +#include + +#include +#include + +// C++ standard libraries +#include +#include + +// QT autogenerated files +#include "ui_path_config.h" + +namespace mapviz_plugins +{ +class PathPlugin : public mapviz_plugins::PointDrawingPlugin +{ + Q_OBJECT + + public: + PathPlugin(); + ~PathPlugin() override = default; + + bool Initialize(QGLWidget* canvas) override; + void Shutdown() override {} + + void Draw(double x, double y, double scale) override; + + void LoadConfig(const YAML::Node& node, const std::string& path) override; + void SaveConfig(YAML::Emitter& emitter, const std::string& path) override; + + QWidget* GetConfigWidget(QWidget* parent) override; + + protected: + void PrintError(const std::string& message) override; + void PrintInfo(const std::string& message) override; + void PrintWarning(const std::string& message) override; + + protected Q_SLOTS: + void SelectTopic(); + void TopicEdited(); + + private: + Ui::path_config ui_; + QWidget* config_widget_; + + std::string topic_; + + rclcpp::Subscription::SharedPtr path_sub_; + bool has_message_; + + void pathCallback(const nav_msgs::msg::Path::SharedPtr path); +}; +} // namespace mapviz_plugins + +#endif // MAPVIZ_PLUGINS__PATH_PLUGIN_H_ diff --git a/mapviz_plugins/include/mapviz_plugins/placeable_window_proxy.h b/mapviz_plugins/include/mapviz_plugins/placeable_window_proxy.h new file mode 100644 index 000000000..18e9e6866 --- /dev/null +++ b/mapviz_plugins/include/mapviz_plugins/placeable_window_proxy.h @@ -0,0 +1,122 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** +#ifndef MAPVIZ_PLUGINS__PLACEABLE_WINDOW_PROXY_H_ +#define MAPVIZ_PLUGINS__PLACEABLE_WINDOW_PROXY_H_ + +#include +#include +#include + +QT_BEGIN_NAMESPACE; +class QMouseEvent; +class QResizeEvent; +QT_END_NAMESPACE; + +/** + * This object supports moving/resizing a "window" on the canvas. It + * should be installed as an event filter on the map canvas and be + * given an initial rectangle. It will watch for mouse events that + * occur within the rectangle and move/resize the rectangle + * accordingly. You can listen to signals to determine when the + * window was moved, or just use the rectangle at the start of every + * draw. + * + * Since this class is designed for moving windows around the canvas, + * it does everything in terms of pixels. + * + * To do: Need to deactivate the proxy when the plugin is marked + * invisible, or is not drawing data. + */ +namespace mapviz_plugins +{ +class PlaceableWindowProxy : public QObject +{ +Q_OBJECT + +public: + PlaceableWindowProxy(); + ~PlaceableWindowProxy() override; + + void setContainer(QWidget *); + + QRect rect() const; + +Q_SIGNALS: + void rectChanged(const QRect &); + +public Q_SLOTS: + void setRect(const QRect &); + void setVisible(bool visible); + +protected: + bool eventFilter(QObject *object, QEvent *event) override; + + bool handleMousePress(QMouseEvent *); + bool handleMouseRelease(QMouseEvent *); + bool handleMouseMove(QMouseEvent *); + bool handleResize(QResizeEvent *); + + void timerEvent(QTimerEvent *) override; + + void rectResize(int dx, int dy); + void winResize(const QSize &); + + QRectF resizeHelper(const QRectF &rect, + const QPointF &p1, + const QPointF &p2, + const QPointF &p3) const; + + + +private: + enum State { + INACTIVE = 0, + MOVE_ALL, + MOVE_TOP_LEFT, + MOVE_BOTTOM_LEFT, + MOVE_BOTTOM_RIGHT, + MOVE_TOP_RIGHT + }; + + QWidget *target_; + bool visible_; + + bool has_cursor_; + State state_; + QRectF rect_; + + QRectF start_rect_; + QPoint start_point_; + + int win_resize_timer_; + + State getNextState(const QPointF &pt) const; +}; // class PlaceableWindowProxy +} // namespace mapviz_plugins +#endif // MAPVIZ_PLUGINS__PLACEABLE_WINDOW_PROXY_H_ diff --git a/mapviz_plugins/include/mapviz_plugins/plan_route_plugin.h b/mapviz_plugins/include/mapviz_plugins/plan_route_plugin.h new file mode 100644 index 000000000..f2329184f --- /dev/null +++ b/mapviz_plugins/include/mapviz_plugins/plan_route_plugin.h @@ -0,0 +1,133 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MAPVIZ_PLUGINS__PLAN_ROUTE_PLUGIN_H_ +#define MAPVIZ_PLUGINS__PLAN_ROUTE_PLUGIN_H_ + +#include + +// QT libraries +#include +#include +#include + +// ROS libraries +#include +#include + +// Mapviz libraries +#include +#include + +// Messages +#include +#include +#include + +// C++ standard libraries +#include +#include + +// QT autogenerated files +#include "ui_plan_route_config.h" + +namespace mapviz_plugins +{ +class PlanRoutePlugin : public mapviz::MapvizPlugin +{ + Q_OBJECT + + public: + PlanRoutePlugin(); + ~PlanRoutePlugin() override; + + bool Initialize(QGLWidget* canvas) override; + void Shutdown() override {} + + void Draw(double x, double y, double scale) override; + void Paint(QPainter* painter, double x, double y, double scale) override; + + void Transform() override {}; + + void LoadConfig(const YAML::Node& node, const std::string& path) override; + void SaveConfig(YAML::Emitter& emitter, const std::string& path) override; + + QWidget* GetConfigWidget(QWidget* parent) override; + + bool SupportsPainting() override + { + return true; + } + + protected: + void PrintError(const std::string& message) override; + void PrintInfo(const std::string& message) override; + void PrintWarning(const std::string& message) override; + bool eventFilter(QObject *object, QEvent* event) override; + bool handleMousePress(QMouseEvent *); + bool handleMouseRelease(QMouseEvent *); + bool handleMouseMove(QMouseEvent *); + + protected Q_SLOTS: + void PublishRoute(); + void PlanRoute(); + void Clear(); + void VisibilityChanged(bool); + + private: + // void Retry(const ros::TimerEvent& e); + void Retry(); + + void ClientCallback(rclcpp::Client::SharedFuture future); + + Ui::plan_route_config ui_{}; + QWidget* config_widget_; + mapviz::MapCanvas* map_canvas_; + + std::string route_topic_; + + rclcpp::Publisher::SharedPtr route_pub_; + rclcpp::TimerBase::SharedPtr retry_timer_; + + bool failed_service_; + swri_route_util::RoutePtr route_preview_; + + std::vector waypoints_; + + int selected_point_; + bool is_mouse_down_; + QPointF mouse_down_pos_; + qint64 mouse_down_time_; + + qint64 max_ms_; + qreal max_distance_; +}; +} // namespace mapviz_plugins + +#endif // MAPVIZ_PLUGINS__PLAN_ROUTE_PLUGIN_H_ diff --git a/mapviz_plugins/include/mapviz_plugins/point_click_publisher_plugin.h b/mapviz_plugins/include/mapviz_plugins/point_click_publisher_plugin.h new file mode 100644 index 000000000..ef1f0117a --- /dev/null +++ b/mapviz_plugins/include/mapviz_plugins/point_click_publisher_plugin.h @@ -0,0 +1,101 @@ +// ***************************************************************************** +// +// Copyright (c) 2014-2020, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MAPVIZ_PLUGINS__POINT_CLICK_PUBLISHER_PLUGIN_H_ +#define MAPVIZ_PLUGINS__POINT_CLICK_PUBLISHER_PLUGIN_H_ + +// Include mapviz_plugin.h first to ensure GL deps are included in the right order +#include + +#include +#include +#include + +#include + +#include + +#include + +// C++ Standard Libraries +#include + +// QT autogenerated files +#include "ui_point_click_publisher_config.h" +#include "ui_topic_select.h" + +/** + * This is a pretty straightforward plugin. It watches for user clicks on the + * canvas, then converts the coordinates into a specified frame and publishes + * them as PointStamped messages on a specified topic. + */ +namespace mapviz_plugins +{ +class PointClickPublisherPlugin : public mapviz::MapvizPlugin +{ + Q_OBJECT; +public: + PointClickPublisherPlugin(); + ~PointClickPublisherPlugin() override; + + bool Initialize(QGLWidget* canvas) override; + void Shutdown() override {} + + void SetNode(rclcpp::Node& node) override; + void PrintError(const std::string& message) override; + void PrintInfo(const std::string& message) override; + void PrintWarning(const std::string& message) override; + + void Draw(double x, double y, double scale) override; + + void Transform() override {} + + void LoadConfig(const YAML::Node& node, const std::string& path) override; + void SaveConfig(YAML::Emitter& emitter, const std::string& path) override; + + QWidget* GetConfigWidget(QWidget* parent) override; + +protected Q_SLOTS: + void pointClicked(const QPointF& point); + void topicChanged(const QString& topic); + void updateFrames(); + +private: + Ui::point_click_publisher_config ui_{}; + QWidget* config_widget_; + + CanvasClickFilter click_filter_; + mapviz::MapCanvas* canvas_; + + QTimer frame_timer_; + rclcpp::Publisher::SharedPtr point_publisher_; +}; +} // namespace mapviz_plugins + +#endif // MAPVIZ_PLUGINS__POINT_CLICK_PUBLISHER_PLUGIN_H_ diff --git a/mapviz_plugins/include/mapviz_plugins/point_drawing_plugin.h b/mapviz_plugins/include/mapviz_plugins/point_drawing_plugin.h new file mode 100644 index 000000000..4d1b3af04 --- /dev/null +++ b/mapviz_plugins/include/mapviz_plugins/point_drawing_plugin.h @@ -0,0 +1,143 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MAPVIZ_PLUGINS__POINT_DRAWING_PLUGIN_H_ +#define MAPVIZ_PLUGINS__POINT_DRAWING_PLUGIN_H_ + +#include +#include + +// QT libraries +#include +#include +#include + +// ROS libraries +#include +#include + +// C++ standard libraries +#include +#include +#include +#include + +namespace mapviz_plugins +{ +class PointDrawingPlugin : public mapviz::MapvizPlugin +{ + Q_OBJECT + + public: + struct StampedPoint + { + StampedPoint(): transformed(false) {} + + tf2::Vector3 point; + tf2::Quaternion orientation; + tf2::Vector3 transformed_point; + tf2::Vector3 transformed_arrow_point; + tf2::Vector3 transformed_arrow_left; + tf2::Vector3 transformed_arrow_right; + std::string source_frame; + bool transformed; + rclcpp::Time stamp; + + std::vector cov_points; + std::vector transformed_cov_points; + }; + + enum DrawStyle + { + LINES = 0, + POINTS, + ARROWS + }; + + PointDrawingPlugin(); + ~PointDrawingPlugin() override = default; + void ClearHistory() override; + + void Transform() override; + virtual bool DrawPoints(double scale); + virtual bool DrawArrows(); + virtual bool DrawArrow(const StampedPoint& point); + virtual bool DrawLaps(); + virtual bool DrawLines(); + virtual void CollectLaps(); + virtual bool DrawLapsArrows(); + virtual bool TransformPoint(StampedPoint& point); + virtual void UpdateColor(QColor base_color, int i); + virtual void DrawCovariance(); + + protected Q_SLOTS: + virtual void BufferSizeChanged(int value); + void DrawIcon() override; + virtual void SetColor(const QColor& color); + virtual void SetDrawStyle(QString style); + virtual void SetDrawStyle(DrawStyle style); + virtual void SetStaticArrowSizes(bool isChecked); + virtual void SetArrowSize(int arrowSize); + virtual void PositionToleranceChanged(double value); + virtual void LapToggled(bool checked); + virtual void CovariancedToggled(bool checked); + virtual void ShowAllCovariancesToggled(bool checked); + void ResetTransformedPoints(); + void ClearPoints(); + + protected: + void pushPoint(StampedPoint point); + double bufferSize() const; + double positionTolerance() const; + const std::deque& points() const; + + private: + int arrow_size_; + DrawStyle draw_style_; + StampedPoint cur_point_; + std::deque points_; + double position_tolerance_; + int buffer_size_; + bool covariance_checked_; + bool show_all_covariances_checked_; + bool new_lap_; + QColor color_; + bool lap_checked_; + int buffer_holder_; + double scale_; + bool static_arrow_sizes_; + + private: + std::vector > laps_; + bool got_begin_; + tf2::Vector3 begin_; +}; +} // namespace mapviz_plugins + +#endif // MAPVIZ_PLUGINS__POINT_DRAWING_PLUGIN_H_ diff --git a/mapviz_plugins/include/mapviz_plugins/pointcloud2_plugin.h b/mapviz_plugins/include/mapviz_plugins/pointcloud2_plugin.h new file mode 100644 index 000000000..d328bf293 --- /dev/null +++ b/mapviz_plugins/include/mapviz_plugins/pointcloud2_plugin.h @@ -0,0 +1,165 @@ +// ***************************************************************************** +// +// Copyright (c) 2015, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MAPVIZ_PLUGINS__POINTCLOUD2_PLUGIN_H_ +#define MAPVIZ_PLUGINS__POINTCLOUD2_PLUGIN_H_ + +#include + +// QT libraries +#include +#include +#include + +// ROS libraries +#include +#include + +// C++ standard libraries +#include +#include +#include +#include + +// QT autogenerated files +#include "ui_pointcloud2_config.h" + +namespace mapviz_plugins +{ +class PointCloud2Plugin : public mapviz::MapvizPlugin +{ +Q_OBJECT + +public: + struct FieldInfo + { + uint8_t datatype; + uint32_t offset; + }; + + enum + { + COLOR_FLAT = 0, + COLOR_Z = 3 + }; + + PointCloud2Plugin(); + ~PointCloud2Plugin() override = default; + + bool Initialize(QGLWidget* canvas) override; + void Shutdown() override {} + + void ClearHistory() override; + + void Draw(double x, double y, double scale) override; + + void Transform() override; + + void LoadConfig(const YAML::Node& node, const std::string& path) override; + void SaveConfig(YAML::Emitter& emitter, const std::string& path) override; + + QWidget* GetConfigWidget(QWidget* parent) override; + +protected: + void PrintError(const std::string& message) override; + void PrintInfo(const std::string& message) override; + void PrintWarning(const std::string& message) override; + +protected Q_SLOTS: + void SelectTopic(); + void TopicEdited(); + void AlphaEdited(double new_value); + void ColorTransformerChanged(int index); + void MinValueChanged(double value); + void MaxValueChanged(double value); + void PointSizeChanged(int value); + void BufferSizeChanged(int value); + void UseRainbowChanged(int check_state); + void UseAutomaxminChanged(int check_state); + void UpdateColors(); + void DrawIcon() override; + void ResetTransformedPointClouds(); + void ClearPointClouds(); + void SetSubscription(bool subscribe); + +private: + struct StampedPoint + { + tf2::Vector3 point; + std::vector features; + }; + + struct Scan + { + rclcpp::Time stamp; + QColor color; + std::vector points; + std::string source_frame; + bool transformed; + std::map new_features; + + std::vector gl_point; + std::vector gl_color; + GLuint point_vbo; + GLuint color_vbo; + }; + + float PointFeature(const uint8_t*, const FieldInfo&); + void PointCloud2Callback(const sensor_msgs::msg::PointCloud2::SharedPtr scan); + QColor CalculateColor(const StampedPoint& point); + void UpdateMinMaxWidgets(); + + Ui::PointCloud2_config ui_{}; + QWidget* config_widget_; + + std::string topic_; + double alpha_; + double max_value_; + double min_value_; + size_t point_size_; + size_t buffer_size_; + bool new_topic_; + bool has_message_; + size_t num_of_feats_; + bool need_new_list_; + std::string saved_color_transformer_; + bool need_minmax_; + std::vector max_; + std::vector min_; + // Use a list instead of a deque for scans to facilitate removing + // timed-out scans in the middle of the list in case I ever re-implement + // decay time (evenator) + std::deque scans_; + rclcpp::Subscription::SharedPtr pc2_sub_; + + QMutex scan_mutex_; +}; +} // namespace mapviz_plugins + +#endif // MAPVIZ_PLUGINS__POINTCLOUD2_PLUGIN_H_ diff --git a/mapviz_plugins/include/mapviz_plugins/pose_plugin.h b/mapviz_plugins/include/mapviz_plugins/pose_plugin.h new file mode 100644 index 000000000..b9163ba4f --- /dev/null +++ b/mapviz_plugins/include/mapviz_plugins/pose_plugin.h @@ -0,0 +1,101 @@ +/** + * Copyright 2019 Hatchbed L.L.C. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + **/ + +#ifndef MAPVIZ_PLUGINS__POSE_PLUGIN_H_ +#define MAPVIZ_PLUGINS__POSE_PLUGIN_H_ + +// Include mapviz_plugin.h first to ensure GL deps are included in the right order +#include + +#include +#include + +// QT libraries +#include +#include +#include + +// ROS libraries +#include +#include +#include +#include + +// C++ standard libraries +#include +#include +#include + +// QT autogenerated files +#include "ui_pose_config.h" + +namespace mapviz_plugins +{ +class PosePlugin : public mapviz_plugins::PointDrawingPlugin +{ + Q_OBJECT + + public: + PosePlugin(); + ~PosePlugin() override = default; + + bool Initialize(QGLWidget* canvas) override; + void Shutdown() override {} + + void Draw(double x, double y, double scale) override; + + void LoadConfig(const YAML::Node& node, const std::string& path) override; + void SaveConfig(YAML::Emitter& emitter, const std::string& path) override; + + QWidget* GetConfigWidget(QWidget* parent) override; + + protected: + void PrintError(const std::string& message) override; + void PrintInfo(const std::string& message) override; + void PrintWarning(const std::string& message) override; + + protected Q_SLOTS: + void SelectTopic(); + void TopicEdited(); + + private: + Ui::pose_config ui_; + QWidget* config_widget_; + + std::string topic_; + + rclcpp::Subscription::SharedPtr pose_sub_; + bool has_message_; + + void PoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr pose); +}; +} // namespace mapviz_plugins + +#endif // MAPVIZ_PLUGINS__POSE_PLUGIN_H_ diff --git a/mapviz_plugins/include/mapviz_plugins/robot_image_plugin.h b/mapviz_plugins/include/mapviz_plugins/robot_image_plugin.h new file mode 100644 index 000000000..71d618e0f --- /dev/null +++ b/mapviz_plugins/include/mapviz_plugins/robot_image_plugin.h @@ -0,0 +1,126 @@ +// ***************************************************************************** +// +// Copyright (c) 2014-2020, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MAPVIZ_PLUGINS__ROBOT_IMAGE_PLUGIN_H_ +#define MAPVIZ_PLUGINS__ROBOT_IMAGE_PLUGIN_H_ + +#include + +// QT libraries +#include +#include +#include + +// ROS libraries +#include +#include + +#include + +// C++ standard libraries +#include + +// QT autogenerated files +#include "ui_robot_image_config.h" +#include "ui_topic_select.h" + +namespace mapviz_plugins +{ +class RobotImagePlugin : public mapviz::MapvizPlugin +{ + Q_OBJECT + +public: + RobotImagePlugin(); + ~RobotImagePlugin() override = default; + + bool Initialize(QGLWidget* canvas) override; + void Shutdown() override {} + + void Draw(double x, double y, double scale) override; + + void Transform() override; + + void LoadConfig(const YAML::Node& node, const std::string& path) override; + void SaveConfig(YAML::Emitter& emitter, const std::string& path) override; + + QWidget* GetConfigWidget(QWidget* parent) override; + +protected: + void PrintError(const std::string& message) override; + void PrintInfo(const std::string& message) override; + void PrintWarning(const std::string& message) override; + +protected Q_SLOTS: + void SelectFile(); + void SelectFrame(); + void FrameEdited(); + void ImageEdited(); + void WidthChanged(double value); + void HeightChanged(double value); + void OffsetXChanged(double value); + void OffsetYChanged(double value); + void RatioEqualToggled(bool toggled); + void RatioCustomToggled(bool toggled); + void RatioOriginalToggled(bool toggled); + +private: + Ui::robot_image_config ui_; + QWidget* config_widget_; + + double width_; // image width, if robot frame is x-forward this corresponds to robot length + double height_; // image height, corresponds to robot width + double offset_x_; // offset of image center from robot frame along x axis + double offset_y_; // offset of image center from robot frame along y axis + double image_ratio_; + + std::string filename_; + QImage image_; + int dimension_; + int texture_id_; + bool texture_loaded_; + + bool transformed_; + + tf2::Vector3 top_left_; + tf2::Vector3 top_right_; + tf2::Vector3 bottom_left_; + tf2::Vector3 bottom_right_; + + tf2::Vector3 top_left_transformed_; + tf2::Vector3 top_right_transformed_; + tf2::Vector3 bottom_left_transformed_; + tf2::Vector3 bottom_right_transformed_; + + void UpdateShape(); + void LoadImage(); +}; +} // namespace mapviz_plugins + +#endif // MAPVIZ_PLUGINS__ROBOT_IMAGE_PLUGIN_H_ diff --git a/mapviz_plugins/include/mapviz_plugins/route_plugin.h b/mapviz_plugins/include/mapviz_plugins/route_plugin.h new file mode 100644 index 000000000..a753123af --- /dev/null +++ b/mapviz_plugins/include/mapviz_plugins/route_plugin.h @@ -0,0 +1,120 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MAPVIZ_PLUGINS__ROUTE_PLUGIN_H_ +#define MAPVIZ_PLUGINS__ROUTE_PLUGIN_H_ + +#include + +// QT libraries +#include +#include +#include + +// ROS libraries +#include +#include +#include +#include +#include + +// C++ standard libraries +#include +#include +#include + +// QT autogenerated files +#include "ui_route_config.h" + +namespace mapviz_plugins +{ +class RoutePlugin : public mapviz::MapvizPlugin +{ + Q_OBJECT + + public: + enum DrawStyle + { + LINES = 0, + POINTS = 1 + }; + + RoutePlugin(); + ~RoutePlugin() override = default; + + bool Initialize(QGLWidget* canvas) override; + void Shutdown() override {} + + void Draw(double x, double y, double scale) override; + + void Transform() override {} + + void LoadConfig(const YAML::Node& node, const std::string& path) override; + void SaveConfig(YAML::Emitter& emitter, const std::string& path) override; + void DrawStopWaypoint(double x, double y); + void DrawRoute(const swri_route_util::Route &route); + void DrawRoutePoint(const swri_route_util::RoutePoint &point); + + QWidget* GetConfigWidget(QWidget* parent) override; + + protected: + void PrintError(const std::string& message) override; + void PrintInfo(const std::string& message) override; + void PrintWarning(const std::string& message) override; + + protected Q_SLOTS: + void SelectTopic(); + void SelectPositionTopic(); + void TopicEdited(); + void PositionTopicEdited(); + void SetDrawStyle(QString style); + void DrawIcon() override; + + private: + Ui::route_config ui_; + QWidget* config_widget_; + + DrawStyle draw_style_; + + std::string topic_; + std::string position_topic_; + + rclcpp::Subscription::SharedPtr route_sub_; + rclcpp::Subscription::SharedPtr position_sub_; + + swri_route_util::Route src_route_; + // marti_nav_msgs::RoutePositionConstPtr src_route_position_; + marti_nav_msgs::msg::RoutePosition::SharedPtr src_route_position_; + + void RouteCallback(const marti_nav_msgs::msg::Route::SharedPtr msg); + void PositionCallback(const marti_nav_msgs::msg::RoutePosition::SharedPtr msg); +}; +} // namespace mapviz_plugins + +#endif // MAPVIZ_PLUGINS__ROUTE_PLUGIN_H_ diff --git a/mapviz_plugins/include/mapviz_plugins/string_plugin.h b/mapviz_plugins/include/mapviz_plugins/string_plugin.h new file mode 100644 index 000000000..c664b7e61 --- /dev/null +++ b/mapviz_plugins/include/mapviz_plugins/string_plugin.h @@ -0,0 +1,152 @@ +// ***************************************************************************** +// +// Copyright (c) 2015, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL Southwest Research Institute® BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY +// OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +// DAMAGE. +// +// ***************************************************************************** + +#ifndef MAPVIZ_PLUGINS__STRING_PLUGIN_H_ +#define MAPVIZ_PLUGINS__STRING_PLUGIN_H_ + + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include + +// QT autogenerated files +#include "ui_string_config.h" + +namespace mapviz_plugins +{ +class StringPlugin : public mapviz::MapvizPlugin +{ + Q_OBJECT + +public: + enum Anchor { + TOP_LEFT, + TOP_CENTER, + TOP_RIGHT, + CENTER_LEFT, + CENTER, + CENTER_RIGHT, + BOTTOM_LEFT, + BOTTOM_CENTER, + BOTTOM_RIGHT + }; + + enum Units { + PIXELS, + PERCENT + }; + + StringPlugin(); + ~StringPlugin() override = default; + + bool Initialize(QGLWidget* canvas) override; + void Shutdown() override {} + + void Draw(double x, double y, double scale) override; + void Paint(QPainter* painter, double x, double y, double scale) override; + + void Transform() override {} + + void LoadConfig(const YAML::Node& node, const std::string& path) override; + void SaveConfig(YAML::Emitter& emitter, const std::string& path) override; + + QWidget* GetConfigWidget(QWidget* parent) override; + + bool SupportsPainting() override + { + return true; + } + + void SetText(const QString& text); + +protected: + void PaintText(QPainter* painter); + void PrintError(const std::string& message) override; + void PrintInfo(const std::string& message) override; + void PrintWarning(const std::string& message) override; + +protected Q_SLOTS: + void SelectColor(); + void SelectFont(); + void SelectTopic(); + void TopicEdited(); + void SetAnchor(QString anchor); + void SetUnits(QString units); + void SetOffsetX(int offset); + void SetOffsetY(int offset); + +private: + Ui::string_config ui_; + QWidget* config_widget_; + + std::string topic_; + Anchor anchor_; + Units units_; + int offset_x_; + int offset_y_; + + rclcpp::Subscription::SharedPtr string_sub_; + rclcpp::Subscription::SharedPtr string_stamped_sub_; + bool has_message_; + bool has_painted_; + + QColor color_; + QFont font_; + QStaticText message_; + + std::string AnchorToString(Anchor anchor); + std::string UnitsToString(Units units); + + static const char* ANCHOR_KEY; + static const char* COLOR_KEY; + static const char* FONT_KEY; + static const char* OFFSET_X_KEY; + static const char* OFFSET_Y_KEY; + static const char* TOPIC_KEY; + static const char* UNITS_KEY; +}; +} // namespace mapviz_plugins + + +#endif // MAPVIZ_PLUGINS__STRING_PLUGIN_H_ diff --git a/mapviz_plugins/include/mapviz_plugins/textured_marker_plugin.h b/mapviz_plugins/include/mapviz_plugins/textured_marker_plugin.h new file mode 100644 index 000000000..8e53ce5c1 --- /dev/null +++ b/mapviz_plugins/include/mapviz_plugins/textured_marker_plugin.h @@ -0,0 +1,145 @@ +// ***************************************************************************** +// +// Copyright (c) 2014-2020, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MAPVIZ_PLUGINS__TEXTURED_MARKER_PLUGIN_H_ +#define MAPVIZ_PLUGINS__TEXTURED_MARKER_PLUGIN_H_ + +#include + +// QT libraries +#include +#include +#include +#include + +#include + +// ROS libraries +#include +#include +#include +#include + +#include + +// C++ standard libraries +#include +#include +#include +#include + +// QT autogenerated files +#include "ui_textured_marker_config.h" + +Q_DECLARE_METATYPE(marti_visualization_msgs::msg::TexturedMarker) + +namespace mapviz_plugins +{ +class TexturedMarkerPlugin : public mapviz::MapvizPlugin +{ + Q_OBJECT + +public: + TexturedMarkerPlugin(); + ~TexturedMarkerPlugin() override = default; + + bool Initialize(QGLWidget * canvas) override; + void Shutdown() override {} + + void Draw(double x, double y, double scale) override; + + void Transform() override; + + void LoadConfig(const YAML::Node & node, const std::string & path) override; + void SaveConfig(YAML::Emitter & emitter, const std::string & path) override; + + QWidget * GetConfigWidget(QWidget * parent) override; + +Q_SIGNALS: + void MarkerReceived(marti_visualization_msgs::msg::TexturedMarker marker); + +protected: + void PrintError(const std::string & message) override; + void PrintInfo(const std::string & message) override; + void PrintWarning(const std::string & message) override; + +protected Q_SLOTS: + void SetAlphaLevel(int alpha); + void SelectTopic(); + void TopicEdited(); + void ClearHistory() override; + void ProcessMarker(marti_visualization_msgs::msg::TexturedMarker marker); + +private: + float alphaVal_; + + struct MarkerData + { + rclcpp::Time stamp; + rclcpp::Time expire_time; + + float alpha_; + + std::vector texture_; + int32_t texture_id_; + int32_t texture_size_; + float texture_x_; + float texture_y_; + + std::string encoding_; + + std::vector quad_; + std::vector transformed_quad_; + + std::string source_frame_; + + bool transformed; + }; + + Ui::textured_marker_config ui_{}; + QWidget * config_widget_; + + std::string topic_; + + rclcpp::Subscription::SharedPtr marker_sub_; + rclcpp::Subscription::SharedPtr + marker_arr_sub_; + + bool has_message_; + + std::map> markers_; + + void MarkerCallback(marti_visualization_msgs::msg::TexturedMarker::ConstSharedPtr marker); + + void MarkerArrayCallback( + marti_visualization_msgs::msg::TexturedMarkerArray::ConstSharedPtr markers); +}; +} // namespace mapviz_plugins + +#endif // MAPVIZ_PLUGINS__TEXTURED_MARKER_PLUGIN_H_ diff --git a/mapviz_plugins/include/mapviz_plugins/tf_frame_plugin.h b/mapviz_plugins/include/mapviz_plugins/tf_frame_plugin.h new file mode 100644 index 000000000..f167edf08 --- /dev/null +++ b/mapviz_plugins/include/mapviz_plugins/tf_frame_plugin.h @@ -0,0 +1,94 @@ +// ***************************************************************************** +// +// Copyright (c) 2014-2020, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MAPVIZ_PLUGINS__TF_FRAME_PLUGIN_H_ +#define MAPVIZ_PLUGINS__TF_FRAME_PLUGIN_H_ + +#include +#include +// QT libraries +#include +#include +#include + +// ROS libraries +#include +#include + +#include + +// C++ standard libraries +#include +#include +#include + +// QT autogenerated files +#include "ui_tf_frame_config.h" +#include "ui_topic_select.h" + +namespace mapviz_plugins +{ +class TfFramePlugin : public mapviz_plugins::PointDrawingPlugin +{ + Q_OBJECT + + public: + TfFramePlugin(); + ~TfFramePlugin() override = default; + + bool Initialize(QGLWidget* canvas) override; + void Shutdown() override {} + + void Draw(double x, double y, double scale) override; + + void LoadConfig(const YAML::Node& node, const std::string& path) override; + void SaveConfig(YAML::Emitter& emitter, const std::string& path) override; + + QWidget* GetConfigWidget(QWidget* parent) override; + + protected: + void PrintError(const std::string& message) override; + void PrintInfo(const std::string& message) override; + void PrintWarning(const std::string& message) override; + + protected Q_SLOTS: + void SelectFrame(); + void FrameEdited(); + + private: + Ui::tf_frame_config ui_{}; + QWidget* config_widget_; + + rclcpp::TimerBase::SharedPtr timer_; + + void TimerCallback(); +}; +} // namespace mapviz_plugins + +#endif // MAPVIZ_PLUGINS__TF_FRAME_PLUGIN_H_ diff --git a/mapviz_plugins/mainpage.dox b/mapviz_plugins/mainpage.dox new file mode 100644 index 000000000..c5b93587f --- /dev/null +++ b/mapviz_plugins/mainpage.dox @@ -0,0 +1,23 @@ +/** +\mainpage +\section mapviz_plugins + +\b mapviz_plugins is a set of API's for ... + +This package... (see sumet_perception/material_classification/mainpage.dox for example description) + + +\subsection codeapi Code API + +The C++ API consists of the following main classes: + +- \b mapviz_plugins::GridPlugin - \copybrief mapviz_plugins::GridPlugin +- \b mapviz_plugins::ImagePlugin - \copybrief mapviz_plugins::ImagePlugin +- \b mapviz_plugins::MarkerPlugin - \copybrief mapviz_plugins::MarkerPlugin +- \b mapviz_plugins::MultiresImagePlugin - \copybrief mapviz_plugins::MultiresImagePlugin +- \b MultiresView - \copybrief MultiresView +- \b mapviz_plugins::OdometryPlugin - \copybrief mapviz_plugins::OdometryPlugin +- \b mapviz_plugins::PathPlugin - \copybrief mapviz_plugins::PathPlugin +- \b mapviz_plugins::RobotImagePlugin - \copybrief mapviz_plugins::RobotImagePlugin + +**/ diff --git a/mapviz_plugins/mapviz_plugins.xml b/mapviz_plugins/mapviz_plugins.xml new file mode 100644 index 000000000..4412f74ec --- /dev/null +++ b/mapviz_plugins/mapviz_plugins.xml @@ -0,0 +1,84 @@ + + + + Plugin to provide an intuitive visualization of orientations + from IMU messages, Odometry messages, and Tf messages. + + + + Prints clicked coordinates in specified frame and copies to clipboard. + + + Measures the distance between points on the map. + + + Odometry mapviz plugin. + + + Pose mapviz plugin. + + + Marker mapviz plugin. + + + Textured marker mapviz plugin. + + + + + Grid mapviz plugin. + + + Image mapviz plugin. + + + Disparity mapviz plugin. + + + Plugin for drawing and publishing a polygon. + + + Path mapviz plugin. + + + TF frame mapviz plugin. + + + Plan route mapviz plugin. + + + Route mapviz plugin. + + + Robot image mapviz plugin. + + + GPS mapviz plugin. + + + NavSat mapviz plugin. + + + LaserScan mapviz plugin. + + + PointCloud2 mapviz plugin. + + + Publishes a StampedPoint when a point on the map canvas is clicked. + + + Displays a std_msgs/String at a fixed point on the canvas. + + + Displays a float and velocity messages at a fixed point on the canvas. + + + Display maps and other OccupancyGrids + + + diff --git a/mapviz_plugins/package.xml b/mapviz_plugins/package.xml new file mode 100644 index 000000000..43c63bc9d --- /dev/null +++ b/mapviz_plugins/package.xml @@ -0,0 +1,57 @@ + + mapviz_plugins + 2.3.0 + + + Common plugins for the Mapviz visualization tool + + + Marc Alban + P. J. Reed + Southwest Research Institute + BSD + https://github.com/swri-robotics/mapviz + + ament_cmake + qt5-qmake + + libqt5-core + libqt5-opengl-dev + ros_environment + + ament_index_cpp + cv_bridge + gps_msgs + image_transport + map_msgs + mapviz + marti_common_msgs + marti_nav_msgs + marti_sensor_msgs + marti_visualization_msgs + + + nav_msgs + pluginlib + rclcpp_action + rclcpp + sensor_msgs + std_msgs + stereo_msgs + swri_image_util + swri_math_util + swri_route_util + swri_transform_util + tf2 + visualization_msgs + + libqt5-opengl + + + ament_cmake + + + + + + diff --git a/mapviz_plugins/src/attitude_indicator_plugin.cpp b/mapviz_plugins/src/attitude_indicator_plugin.cpp new file mode 100644 index 000000000..6ddf60b70 --- /dev/null +++ b/mapviz_plugins/src/attitude_indicator_plugin.cpp @@ -0,0 +1,392 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include +#include + +// QT libraries +#include +#include +#include + +// ROS libraries +#include + +#include +#include + +// Declare plugin +#include + +// C++ standard libraries +#include +#include +#include + +PLUGINLIB_EXPORT_CLASS(mapviz_plugins::AttitudeIndicatorPlugin, mapviz::MapvizPlugin) + +namespace mapviz_plugins +{ + AttitudeIndicatorPlugin::AttitudeIndicatorPlugin() + : MapvizPlugin() + , ui_() + , config_widget_(new QWidget()) + { + ui_.setupUi(config_widget_); + + // Set background white + QPalette p(config_widget_->palette()); + p.setColor(QPalette::Window, Qt::white); + config_widget_->setPalette(p); + roll_ = pitch_ = yaw_ = 0; + topics_.emplace_back("nav_msgs/msg/Odometry"); + topics_.emplace_back("geometry_msgs/msg/Pose"); + topics_.emplace_back("sensor_msgs/msg/Imu"); + // Set status text red + QPalette p3(ui_.status->palette()); + p3.setColor(QPalette::Text, Qt::red); + ui_.status->setPalette(p3); + + placer_.setRect(QRect(0, 0, 100, 100)); + QObject::connect(this, SIGNAL(VisibleChanged(bool)), + &placer_, SLOT(setVisible(bool))); + + QObject::connect(ui_.selecttopic, SIGNAL(clicked()), this, SLOT(SelectTopic())); + QObject::connect(ui_.topic, SIGNAL(editingFinished()), this, SLOT(TopicEdited())); + } + + void AttitudeIndicatorPlugin::SelectTopic() + { + std::string topic = mapviz::SelectTopicDialog::selectTopic( + node_, + topics_); + if (topic.empty()) + { + return; + } + + ui_.topic->setText(QString::fromStdString(topic)); + TopicEdited(); + } + + void AttitudeIndicatorPlugin::TopicEdited() + { + std::string topic = ui_.topic->text().trimmed().toStdString(); + if (topic != topic_) + { + initialized_ = true; + PrintWarning("No messages received."); + + odom_sub_.reset(); + imu_sub_.reset(); + pose_sub_.reset(); + + topic_ = topic; + if (!topic_.empty()) + { + odom_sub_ = node_->create_subscription( + topic_, + rclcpp::QoS(1), + std::bind(&AttitudeIndicatorPlugin::AttitudeCallbackOdom, this, std::placeholders::_1)); + imu_sub_ = node_->create_subscription( + topic_, + rclcpp::QoS(1), + std::bind(&AttitudeIndicatorPlugin::AttitudeCallbackImu, this, std::placeholders::_1)); + pose_sub_ = node_->create_subscription( + topic_, + rclcpp::QoS(1), + std::bind(&AttitudeIndicatorPlugin::AttitudeCallbackPose, this, std::placeholders::_1)); + + RCLCPP_INFO(node_->get_logger(), "Subscribing to %s", topic_.c_str()); + } + } + } + + void AttitudeIndicatorPlugin::AttitudeCallbackOdom( + nav_msgs::msg::Odometry::ConstSharedPtr odometry) + { + applyAttitudeOrientation(odometry->pose.pose.orientation); + } + + void AttitudeIndicatorPlugin::AttitudeCallbackImu(sensor_msgs::msg::Imu::ConstSharedPtr imu) + { + applyAttitudeOrientation(imu->orientation); + } + + void AttitudeIndicatorPlugin::AttitudeCallbackPose(geometry_msgs::msg::Pose::ConstSharedPtr pose) + { + applyAttitudeOrientation(pose->orientation); + } + + void AttitudeIndicatorPlugin::applyAttitudeOrientation( + const geometry_msgs::msg::Quaternion &orientation) + { + tf2::Quaternion attitude_orientation( + orientation.x, + orientation.y, + orientation.z, + orientation.w); + + tf2::Matrix3x3 m(attitude_orientation); + m.getRPY(roll_, pitch_, yaw_); + roll_ = roll_ * (180.0 / M_PI); + pitch_ = pitch_ * (180.0 / M_PI); + yaw_ = yaw_ * (180.0 / M_PI); + + canvas_->update(); + } + + void AttitudeIndicatorPlugin::PrintError(const std::string& message) + { + PrintErrorHelper(ui_.status, message); + } + + void AttitudeIndicatorPlugin::PrintInfo(const std::string& message) + { + PrintInfoHelper(ui_.status, message); + } + + void AttitudeIndicatorPlugin::PrintWarning(const std::string& message) + { + PrintWarningHelper(ui_.status, message); + } + + QWidget* AttitudeIndicatorPlugin::GetConfigWidget(QWidget* parent) + { + config_widget_->setParent(parent); + return config_widget_; + } + + bool AttitudeIndicatorPlugin::Initialize(QGLWidget* canvas) + { + initialized_ = true; + canvas_ = canvas; + placer_.setContainer(canvas_); + startTimer(50); + return true; + } + + void AttitudeIndicatorPlugin::Shutdown() + { + placer_.setContainer(nullptr); + } + + void AttitudeIndicatorPlugin::timerEvent(QTimerEvent*) + { + canvas_->update(); + } + + void AttitudeIndicatorPlugin::drawBall() + { + GLdouble eqn[4] = {0.0, 0.0, 1.0, 0.0}; + GLdouble eqn2[4] = {0.0, 0.0, -1.0, 0.0}; + GLdouble eqn4[4] = {0.0, 0.0, 1.0, 0.05}; + GLdouble eqn3[4] = {0.0, 0.0, -1.0, 0.05}; + + glEnable(GL_DEPTH_TEST); + glDepthFunc(GL_LESS); + + glPushMatrix(); + + glColor3f(0.392156863f, 0.584313725f, 0.929411765f); + glRotated(90.0 + pitch_, 1.0, 0.0, 0.0); + + glRotated(roll_, 0.0, 1.0, 0.0); + glRotated(yaw_, 0.0, 0.0, 1.0); + glClipPlane(GL_CLIP_PLANE1, eqn2); + glEnable(GL_CLIP_PLANE1); + glutSolidSphere(.8, 20, 16); + glDisable(GL_CLIP_PLANE1); + glPopMatrix(); + + glPushMatrix(); + + glLineWidth(2); + glColor3f(1.0f, 1.0f, 1.0f); + glRotated(90.0 + pitch_, 1.0, 0.0, 0.0); + glRotated(roll_, 0.0, 1.0, 0.0); + glRotated(yaw_, 0.0, 0.0, 1.0); + glClipPlane(GL_CLIP_PLANE3, eqn4); + glClipPlane(GL_CLIP_PLANE2, eqn3); + glEnable(GL_CLIP_PLANE2); + glEnable(GL_CLIP_PLANE3); + glutWireSphere(.801, 10, 16); + glDisable(GL_CLIP_PLANE2); + glDisable(GL_CLIP_PLANE3); + glPopMatrix(); + + glPushMatrix(); + glColor3f(0.62745098f, 0.321568627f, 0.176470588f); + glRotated(90.0 + pitch_, 1.0, 0.0, 0.0); // x + glRotated(roll_, 0.0, 1.0, 0.0); // y + glRotated(yaw_, 0.0, 0.0, 1.0); // z + glClipPlane(GL_CLIP_PLANE0, eqn); + glEnable(GL_CLIP_PLANE0); + glutSolidSphere(.8, 20, 16); + glDisable(GL_CLIP_PLANE0); + glPopMatrix(); + glDisable(GL_DEPTH_TEST); + } + + void AttitudeIndicatorPlugin::Draw(double x, double y, double scale) + { + glPushAttrib(GL_ALL_ATTRIB_BITS); + glMatrixMode(GL_PROJECTION); + glPushMatrix(); + glLoadIdentity(); + glOrtho(0, canvas_->width(), canvas_->height(), 0, -1.0f, 1.0f); + glMatrixMode(GL_MODELVIEW); + glPushMatrix(); + glLoadIdentity(); + // Setup coordinate system so that we have a [-1,1]x[1,1] cube on + // the screen. + QRect rect = placer_.rect(); + double s_x = rect.width() / 2.0; + double s_y = -rect.height() / 2.0; + double t_x = rect.right() - s_x; + double t_y = rect.top() - s_y; + + double m[16] = { + s_x, 0, 0, 0, + 0, s_y, 0, 0, + 0, 0, 1.0, 0, + t_x, t_y, 0, 1.0}; + glMultMatrixd(m); + + // Placed in a separate function so that we don't forget to pop the + // GL state back. + + drawBackground(); + drawBall(); + + drawPanel(); + + glPopMatrix(); + glMatrixMode(GL_PROJECTION); + glPopMatrix(); + glMatrixMode(GL_MODELVIEW); + glPopAttrib(); + PrintInfo("OK!"); + } + + void AttitudeIndicatorPlugin::drawBackground() + { + glBegin(GL_TRIANGLES); + glColor4f(0.0f, 0.0f, 0.0f, 1.0f); + + glVertex2d(-1.0, -1.0); + glVertex2d(-1.0, 1.0); + glVertex2d(1.0, 1.0); + + glVertex2d(-1.0, -1.0); + glVertex2d(1.0, 1.0); + glVertex2d(1.0, -1.0); + + glEnd(); + } + + void AttitudeIndicatorPlugin::drawPanel() + { + glLineWidth(2); + + glBegin(GL_LINE_STRIP); + glColor4f(1.0f, 1.0f, 1.0f, 1.0f); + + glVertex2d(-0.9, 0.0); + glVertex2d(-0.2, 0.0); + + int divisions = 20; + for (int i = 1; i < divisions; i++) + { + glVertex2d(-0.2 * std::cos(M_PI * i / divisions), + -0.2 * std::sin(M_PI * i / divisions)); + } + + glVertex2f(0.2, 0.0); + glVertex2f(0.9, 0.0); + glEnd(); + + glBegin(GL_LINES); + glVertex2f(0.0, -0.2f); + glVertex2f(0.0, -0.9f); + glEnd(); + } + + void AttitudeIndicatorPlugin::LoadConfig(const YAML::Node& node, const std::string& path) + { + if (node["topic"]) + { + std::string topic = node["topic"].as(); + ui_.topic->setText(topic.c_str()); + } + + QRect current = placer_.rect(); + int x = current.x(); + int y = current.y(); + int width = current.width(); + int height = current.height(); + + if (node["x"]) + { + x = node["x"].as(); + } + + if (node["y"]) + { + y = node["y"].as(); + } + + if (node["width"]) + { + width = node["width"].as(); + } + + if (node["height"]) + { + height = node["height"].as(); + } + + QRect position(x, y, width, height); + placer_.setRect(position); + + TopicEdited(); + } + + void AttitudeIndicatorPlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path) + { + emitter << YAML::Key << "topic" << YAML::Value << ui_.topic->text().toStdString(); + + QRect position = placer_.rect(); + + emitter << YAML::Key << "x" << YAML::Value << position.x(); + emitter << YAML::Key << "y" << YAML::Value << position.y(); + emitter << YAML::Key << "width" << YAML::Value << position.width(); + emitter << YAML::Key << "height" << YAML::Value << position.height(); + } +} // namespace mapviz_plugins diff --git a/mapviz_plugins/src/canvas_click_filter.cpp b/mapviz_plugins/src/canvas_click_filter.cpp new file mode 100644 index 000000000..13ca433df --- /dev/null +++ b/mapviz_plugins/src/canvas_click_filter.cpp @@ -0,0 +1,83 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include +#include +#include +#include "mapviz_plugins/canvas_click_filter.h" + +namespace mapviz_plugins +{ + CanvasClickFilter::CanvasClickFilter() + : QObject() + , is_mouse_down_(false) + , max_ms_(Q_INT64_C(500)) + , max_distance_(2.0) + { } + + void CanvasClickFilter::setMaxClickTime(qint64 max_ms) + { + max_ms_ = max_ms; + } + + void CanvasClickFilter::setMaxClickMovement(qreal max_distance) + { + max_distance_ = max_distance; + } + + bool CanvasClickFilter::eventFilter(QObject* object, QEvent* event) + { + if (event->type() == QEvent::MouseButtonPress) + { + is_mouse_down_ = true; + QMouseEvent* me = dynamic_cast(event); + mouse_down_pos_ = me->localPos(); + mouse_down_time_ = QDateTime::currentMSecsSinceEpoch(); + } else if (event->type() == QEvent::MouseButtonRelease) { + if (is_mouse_down_) + { + QMouseEvent* me = dynamic_cast(event); + + qreal distance = QLineF(mouse_down_pos_, me->localPos()).length(); + qint64 msecsDiff = QDateTime::currentMSecsSinceEpoch() - mouse_down_time_; + + // Only fire the event if the mouse has moved less than the maximum distance + // and was held for shorter than the maximum time.. This prevents click + // events from being fired if the user is dragging the mouse across the map + // or just holding the cursor in place. + if (msecsDiff < max_ms_ && distance <= max_distance_) + { + Q_EMIT pointClicked(me->localPos()); + } + } + is_mouse_down_ = false; + } + return false; + } +} // namespace mapviz_plugins diff --git a/mapviz_plugins/src/coordinate_picker_plugin.cpp b/mapviz_plugins/src/coordinate_picker_plugin.cpp new file mode 100644 index 000000000..fcee6ca8b --- /dev/null +++ b/mapviz_plugins/src/coordinate_picker_plugin.cpp @@ -0,0 +1,306 @@ +// ***************************************************************************** +// +// Copyright (c) 2018, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include +#include + +#include +#include +#include + +#if QT_VERSION >= 0x050000 +#include +#else +#include +#endif + +// ROS Libraries +#include + +// Mapviz Libraries +#include + +// +#include +#include +#include + +#include + +#include + +PLUGINLIB_EXPORT_CLASS(mapviz_plugins::CoordinatePickerPlugin, mapviz::MapvizPlugin) + +namespace mapviz_plugins +{ + +CoordinatePickerPlugin::CoordinatePickerPlugin() + : MapvizPlugin() + , config_widget_(new QWidget()) + , map_canvas_(nullptr) + , copy_on_click_(false) +{ + ui_.setupUi(config_widget_); + + QObject::connect(ui_.selectframe, SIGNAL(clicked()), + this, SLOT(SelectFrame())); + QObject::connect(ui_.frame, SIGNAL(editingFinished()), + this, SLOT(FrameEdited())); + QObject::connect(ui_.copyCheckBox, SIGNAL(stateChanged(int)), + this, SLOT(ToggleCopyOnClick(int))); + QObject::connect(ui_.clearListButton, SIGNAL(clicked()), + this, SLOT(ClearCoordList())); + + ui_.coordTextEdit->setPlaceholderText(tr("Click on the map; coordinates appear here")); +} + +CoordinatePickerPlugin::~CoordinatePickerPlugin() +{ + if (map_canvas_) + { + map_canvas_->removeEventFilter(this); + } +} + +QWidget* CoordinatePickerPlugin::GetConfigWidget(QWidget* parent) +{ + config_widget_->setParent(parent); + + return config_widget_; +} + +bool CoordinatePickerPlugin::Initialize(QGLWidget* canvas) +{ + map_canvas_ = dynamic_cast< mapviz::MapCanvas* >(canvas); + map_canvas_->installEventFilter(this); + + initialized_ = true; + PrintInfo("OK"); + + return true; +} + +bool CoordinatePickerPlugin::eventFilter(QObject* object, QEvent* event) +{ + if(!this->Visible()) + { + RCLCPP_DEBUG(node_->get_logger(), "Ignoring mouse event, since coordinate picker plugin is hidden"); + return false; + } + + switch (event->type()) + { + case QEvent::MouseButtonPress: + return handleMousePress(dynamic_cast< QMouseEvent* >(event)); + case QEvent::MouseButtonRelease: + return handleMouseRelease(dynamic_cast< QMouseEvent* >(event)); + case QEvent::MouseMove: + return handleMouseMove(dynamic_cast< QMouseEvent* >(event)); + default: + return false; + } +} + +bool CoordinatePickerPlugin::handleMousePress(QMouseEvent* event) +{ + QPointF point = event->localPos(); + RCLCPP_DEBUG(node_->get_logger(), "Map point: %f %f", point.x(), point.y()); + + swri_transform_util::Transform transform; + std::string frame = ui_.frame->text().toStdString(); + if (frame.empty()) + { + frame = target_frame_; + } + + // Frames get confusing. The `target_frame_` member is set by the "Fixed + // Frame" combobox in the GUI. When we transform the map coordinate to the + // fixed frame, we get it in the `target_frame_` frame. + // + // Then we translate from that frame into *our* target frame, `frame`. + if (tf_manager_->GetTransform(frame, target_frame_, transform)) + { + RCLCPP_DEBUG(node_->get_logger(), + "Transforming from fixed frame '%s' to (plugin) target frame '%s'", + target_frame_.c_str(), + frame.c_str()); + QPointF transformed = map_canvas_->MapGlCoordToFixedFrame(point); + RCLCPP_DEBUG(node_->get_logger(), + "Point in fixed frame: %f %f", + transformed.x(), + transformed.y()); + tf2::Vector3 position(transformed.x(), transformed.y(), 0.0); + position = transform * position; + point.setX(position.x()); + point.setY(position.y()); + + PrintInfo("OK"); + } else { + QString warning; + QTextStream(&warning) << "No available transform from '" + << QString::fromStdString(target_frame_) + << "' to '" + << QString::fromStdString(frame) + << "'"; + PrintWarning(warning.toStdString()); + return false; + } + + + RCLCPP_DEBUG(node_->get_logger(), + "Transformed point in frame '%s': %f %f", + frame.c_str(), + point.x(), + point.y()); + QString new_point; + QTextStream stream(&new_point); + if (swri_transform_util::FrameIdsEqual(frame, swri_transform_util::_wgs84_frame)) + { + stream.setRealNumberPrecision(9); + } + else + { + stream.setRealNumberPrecision(4); + } + stream << point.x() << ", " << point.y(); + + if (copy_on_click_) + { + QClipboard* clipboard = QGuiApplication::clipboard(); + clipboard->setText(new_point); + } + + stream << " (" << QString::fromStdString(frame) << ")\n"; + + ui_.coordTextEdit->setPlainText(ui_.coordTextEdit->toPlainText().prepend(new_point)); + + // Let other plugins process this event too + return false; +} + +bool CoordinatePickerPlugin::handleMouseRelease(QMouseEvent* event) +{ + // Let other plugins process this event too + return false; +} + +bool CoordinatePickerPlugin::handleMouseMove(QMouseEvent* event) +{ + // Let other plugins process this event too + return false; +} + +void CoordinatePickerPlugin::SelectFrame() +{ + std::string frame = mapviz::SelectFrameDialog::selectFrame(tf_buf_); + if (!frame.empty()) + { + ui_.frame->setText(QString::fromStdString(frame)); + FrameEdited(); + } +} + +void CoordinatePickerPlugin::FrameEdited() +{ + RCLCPP_INFO(node_->get_logger(), + "Setting target frame to %s", + ui_.frame->text().toStdString().c_str()); +} + +void CoordinatePickerPlugin::ToggleCopyOnClick(int state) +{ + switch (state) + { + case Qt::Checked: + copy_on_click_ = true; + break; + case Qt::PartiallyChecked: + case Qt::Unchecked: + default: + copy_on_click_ = false; + break; + } +} + +void CoordinatePickerPlugin::ClearCoordList() +{ + ui_.coordTextEdit->setPlainText(QString()); +} + +void CoordinatePickerPlugin::Draw(double x, double y, double scale) +{ +} + +void CoordinatePickerPlugin::LoadConfig(const YAML::Node& node, const std::string& path) +{ + if (node["frame"]) + { + std::string frame; + frame = node["frame"].as(); + ui_.frame->setText(QString::fromStdString(frame)); + } + + if (node["copy"]) + { + bool copy; + copy = node["copy"].as(); + if (copy) + { + ui_.copyCheckBox->setCheckState(Qt::Checked); + } else { + ui_.copyCheckBox->setCheckState(Qt::Unchecked); + } + } +} + +void CoordinatePickerPlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path) +{ + std::string frame = ui_.frame->text().toStdString(); + emitter << YAML::Key << "frame" << YAML::Value << frame; + + bool copy_on_click = ui_.copyCheckBox->isChecked(); + emitter << YAML::Key << "copy" << YAML::Value << copy_on_click; +} + +void CoordinatePickerPlugin::PrintError(const std::string& message) +{ + PrintErrorHelper(ui_.status, message, 1.0); +} + +void CoordinatePickerPlugin::PrintInfo(const std::string& message) +{ + PrintInfoHelper(ui_.status, message, 1.0); +} + +void CoordinatePickerPlugin::PrintWarning(const std::string& message) +{ + PrintWarningHelper(ui_.status, message, 1.0); +} + +} // namespace mapviz_plugins diff --git a/mapviz_plugins/src/disparity_plugin.cpp b/mapviz_plugins/src/disparity_plugin.cpp new file mode 100644 index 000000000..6229829e2 --- /dev/null +++ b/mapviz_plugins/src/disparity_plugin.cpp @@ -0,0 +1,781 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include + +// QT libraries +#include +#include + +// ROS libraries +#include +#include +#include + +#ifdef USE_CVBRIDGE_H_FILES +#include +#else +#include +#endif + +#include + +// Declare plugin +#include + +// C++ standard libraries +#include +#include +#include +#include + +PLUGINLIB_EXPORT_CLASS(mapviz_plugins::DisparityPlugin, mapviz::MapvizPlugin) + +namespace mapviz_plugins +{ + DisparityPlugin::DisparityPlugin() + : MapvizPlugin() + , ui_() + , config_widget_(new QWidget()) + , anchor_(TOP_LEFT) + , units_(PIXELS) + , offset_x_(0) + , offset_y_(0) + , width_(320) + , height_(240) + , has_image_(false) + , last_width_(0) + , last_height_(0) + , has_message_(false) + { + ui_.setupUi(config_widget_); + + // Set background white + QPalette p(config_widget_->palette()); + p.setColor(QPalette::Window, Qt::white); + config_widget_->setPalette(p); + + // Set status text red + QPalette p3(ui_.status->palette()); + p3.setColor(QPalette::Text, Qt::red); + ui_.status->setPalette(p3); + + QObject::connect(ui_.selecttopic, SIGNAL(clicked()), this, SLOT(SelectTopic())); + QObject::connect(ui_.topic, SIGNAL(editingFinished()), this, SLOT(TopicEdited())); + QObject::connect(ui_.anchor, SIGNAL(activated(QString)), this, SLOT(SetAnchor(QString))); + QObject::connect(ui_.units, SIGNAL(activated(QString)), this, SLOT(SetUnits(QString))); + QObject::connect(ui_.offsetx, SIGNAL(valueChanged(int)), this, SLOT(SetOffsetX(int))); + QObject::connect(ui_.offsety, SIGNAL(valueChanged(int)), this, SLOT(SetOffsetY(int))); + QObject::connect(ui_.width, SIGNAL(valueChanged(int)), this, SLOT(SetWidth(int))); + QObject::connect(ui_.height, SIGNAL(valueChanged(int)), this, SLOT(SetHeight(int))); + QObject::connect(this, SIGNAL(VisibleChanged(bool)), this, SLOT(SetSubscription(bool))); + } + + void DisparityPlugin::SetOffsetX(int offset) + { + offset_x_ = offset; + } + + void DisparityPlugin::SetOffsetY(int offset) + { + offset_y_ = offset; + } + + void DisparityPlugin::SetWidth(int width) + { + width_ = width; + } + + void DisparityPlugin::SetHeight(int height) + { + height_ = height; + } + + void DisparityPlugin::SetAnchor(QString anchor) + { + if (anchor == "top left") + { + anchor_ = TOP_LEFT; + } else if (anchor == "top center") { + anchor_ = TOP_CENTER; + } else if (anchor == "top right") { + anchor_ = TOP_RIGHT; + } else if (anchor == "center left") { + anchor_ = CENTER_LEFT; + } else if (anchor == "center") { + anchor_ = CENTER; + } else if (anchor == "center right") { + anchor_ = CENTER_RIGHT; + } else if (anchor == "bottom left") { + anchor_ = BOTTOM_LEFT; + } else if (anchor == "bottom center") { + anchor_ = BOTTOM_CENTER; + } else if (anchor == "bottom right") { + anchor_ = BOTTOM_RIGHT; + } + } + + void DisparityPlugin::SetUnits(QString units) + { + if (units == "pixels") + { + units_ = PIXELS; + } else if (units == "percent") { + units_ = PERCENT; + } + } + void DisparityPlugin::SetSubscription(bool visible) + { + if(topic_.empty()) + { + return; + } else if (!visible) { + disparity_sub_.reset(); + RCLCPP_INFO(node_->get_logger(), "Dropped subscription to %s", topic_.c_str()); + } else { + disparity_sub_ = node_->create_subscription( + topic_, + rclcpp::QoS(1), + std::bind(&DisparityPlugin::disparityCallback, this, std::placeholders::_1) + ); + + RCLCPP_INFO(node_->get_logger(), "Subscribing to %s", topic_.c_str()); + } + } + void DisparityPlugin::SelectTopic() + { + std::string topic = mapviz::SelectTopicDialog::selectTopic( + node_, + "stereo_msgs/msg/DisparityImage" + ); + + if (!topic.empty()) + { + ui_.topic->setText(QString::fromStdString(topic)); + TopicEdited(); + } + } + + void DisparityPlugin::TopicEdited() + { + std::string topic = ui_.topic->text().trimmed().toStdString(); + if (!this->Visible()) + { + PrintWarning("Topic is Hidden"); + initialized_ = false; + has_message_ = false; + if (!topic.empty()) + { + topic_ = topic; + } + disparity_sub_.reset(); + return; + } + if (topic != topic_) + { + PrintWarning("Topic is Hidden"); + initialized_ = false; + has_message_ = false; + topic_ = topic; + PrintWarning("No messages received."); + + disparity_sub_.reset(); + + if (!topic.empty()) + { + disparity_sub_ = node_->create_subscription( + topic_, + rclcpp::QoS(1), + std::bind(&DisparityPlugin::disparityCallback, this, std::placeholders::_1) + ); + + RCLCPP_INFO(node_->get_logger(), "Subscribing to %s", topic_.c_str()); + } + } + } + + void DisparityPlugin::disparityCallback( + const stereo_msgs::msg::DisparityImage::SharedPtr disparity) + { + if (!has_message_) + { + initialized_ = true; + has_message_ = true; + } + + if (disparity->min_disparity == 0.0 && disparity->max_disparity == 0.0) + { + PrintError("Min and max disparity not set."); + has_image_ = false; + return; + } + + if (disparity->image.encoding != sensor_msgs::image_encodings::TYPE_32FC1) + { + PrintError("Invalid encoding."); + has_image_ = false; + return; + } + + disparity_ = *disparity; + + // Colormap and display the disparity image + float min_disparity = disparity->min_disparity; + float max_disparity = disparity->max_disparity; + float multiplier = 255.0f / (max_disparity - min_disparity); + + cv_bridge::CvImageConstPtr cv_disparity = + cv_bridge::toCvShare(disparity->image, disparity); + + disparity_color_.create(disparity->image.height, disparity->image.width); + + for (int row = 0; row < disparity_color_.rows; row++) + { + const float* d = cv_disparity->image.ptr(row); + for (int col = 0; col < disparity_color_.cols; col++) + { + int index = static_cast((d[col] - min_disparity) * multiplier + 0.5); + index = std::min(255, std::max(0, index)); + // Fill as BGR + disparity_color_(row, col)[2] = COLOR_MAP[3*index + 0]; + disparity_color_(row, col)[1] = COLOR_MAP[3*index + 1]; + disparity_color_(row, col)[0] = COLOR_MAP[3*index + 2]; + } + } + + last_width_ = 0; + last_height_ = 0; + + has_image_ = true; + } + + void DisparityPlugin::PrintError(const std::string& message) + { + PrintErrorHelper(ui_.status, message); + } + + void DisparityPlugin::PrintInfo(const std::string& message) + { + PrintInfoHelper(ui_.status, message); + } + + void DisparityPlugin::PrintWarning(const std::string& message) + { + PrintWarningHelper(ui_.status, message); + } + + QWidget* DisparityPlugin::GetConfigWidget(QWidget* parent) + { + config_widget_->setParent(parent); + + return config_widget_; + } + + bool DisparityPlugin::Initialize(QGLWidget* canvas) + { + canvas_ = canvas; + + return true; + } + + void DisparityPlugin::ScaleImage(double width, double height) + { + if (!has_image_) + { + return; + } + + cv::resize(disparity_color_, scaled_image_, cvSize2D32f(width, height), 0, 0, CV_INTER_AREA); + } + + void DisparityPlugin::DrawIplImage(cv::Mat *image) + { + // TODO(malban): glTexture2D may be more efficient than glDrawPixels + + if (!has_image_) + return; + + if (image == NULL) + return; + + if (image->cols == 0 || image->rows == 0) + return; + + GLenum format; + switch (image->channels()) + { + case 1: + format = GL_LUMINANCE; + break; + case 2: + format = GL_LUMINANCE_ALPHA; + break; + case 3: + format = GL_BGR; + break; + default: + return; + } + + glPixelZoom(1.0, -1.0f); + glDrawPixels(image->cols, image->rows, format, GL_UNSIGNED_BYTE, image->ptr()); + + PrintInfo("OK"); + } + + void DisparityPlugin::Draw(double x, double y, double scale) + { + // Calculate the correct offsets and dimensions + double x_offset = offset_x_; + double y_offset = offset_y_; + double width = width_; + double height = height_; + if (units_ == PERCENT) + { + x_offset = offset_x_ * canvas_->width() / 100.0; + y_offset = offset_y_ * canvas_->height() / 100.0; + width = width_ * canvas_->width() / 100.0; + height = height_ * canvas_->height() / 100.0; + } + + // Scale the source image if necessary + if (width != last_width_ || height != last_height_) + { + ScaleImage(width, height); + } + + // Calculate the correct render position + double x_pos = 0; + double y_pos = 0; + if (anchor_ == TOP_LEFT) + { + x_pos = x_offset; + y_pos = y_offset; + } else if (anchor_ == TOP_CENTER) { + x_pos = (canvas_->width() - width) / 2.0 + x_offset; + y_pos = y_offset; + } else if (anchor_ == TOP_RIGHT) { + x_pos = canvas_->width() - width - x_offset; + y_pos = y_offset; + } else if (anchor_ == CENTER_LEFT) { + x_pos = x_offset; + y_pos = (canvas_->height() - height) / 2.0 + y_offset; + } else if (anchor_ == CENTER) { + x_pos = (canvas_->width() - width) / 2.0 + x_offset; + y_pos = (canvas_->height() - height) / 2.0 + y_offset; + } else if (anchor_ == CENTER_RIGHT) { + x_pos = canvas_->width() - width - x_offset; + y_pos = (canvas_->height() - height) / 2.0 + y_offset; + } else if (anchor_ == BOTTOM_LEFT) { + x_pos = x_offset; + y_pos = canvas_->height() - height - y_offset; + } else if (anchor_ == BOTTOM_CENTER) { + x_pos = (canvas_->width() - width) / 2.0 + x_offset; + y_pos = canvas_->height() - height - y_offset; + } else if (anchor_ == BOTTOM_RIGHT) { + x_pos = canvas_->width() - width - x_offset; + y_pos = canvas_->height() - height - y_offset; + } + + glMatrixMode(GL_PROJECTION); + glPushMatrix(); + glLoadIdentity(); + glOrtho(0, canvas_->width(), canvas_->height(), 0, -0.5f, 0.5f); + + glRasterPos2d(x_pos, y_pos); + + DrawIplImage(&scaled_image_); + + glPopMatrix(); + + last_width_ = width; + last_height_ = height; + } + + void DisparityPlugin::LoadConfig(const YAML::Node& node, const std::string& path) + { + if (node["topic"]) + { + std::string topic = node["topic"].as(); + ui_.topic->setText(topic.c_str()); + TopicEdited(); + } + + if (node["anchor"]) + { + std::string anchor = node["anchor"].as(); + ui_.anchor->setCurrentIndex(ui_.anchor->findText(anchor.c_str())); + SetAnchor(anchor.c_str()); + } + + if (node["units"]) + { + std::string units = node["units"].as(); + ui_.units->setCurrentIndex(ui_.units->findText(units.c_str())); + SetUnits(units.c_str()); + } + + if (node["offset_x"]) + { + offset_x_ = node["offset_x"].as(); + ui_.offsetx->setValue(static_cast(offset_x_)); + } + + if (node["offset_y"]) + { + offset_y_ = node["offset_y"].as(); + ui_.offsety->setValue(static_cast(offset_y_)); + } + + if (node["width"]) + { + width_ = node["width"].as(); + ui_.width->setValue(static_cast(width_)); + } + + if (node["height"]) + { + height_ = node["height"].as(); + ui_.height->setValue(static_cast(height_)); + } + } + + void DisparityPlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path) + { + emitter << YAML::Key << "topic" << YAML::Value << ui_.topic->text().toStdString(); + emitter << YAML::Key << "anchor" << YAML::Value << AnchorToString(anchor_); + emitter << YAML::Key << "units" << YAML::Value << UnitsToString(units_); + emitter << YAML::Key << "offset_x" << YAML::Value << offset_x_; + emitter << YAML::Key << "offset_y" << YAML::Value << offset_y_; + emitter << YAML::Key << "width" << YAML::Value << width_; + emitter << YAML::Key << "height" << YAML::Value << height_; + } + + std::string DisparityPlugin::AnchorToString(Anchor anchor) + { + std::string anchor_string = "top left"; + + if (anchor == TOP_LEFT) + { + anchor_string = "top left"; + } else if (anchor == TOP_CENTER) { + anchor_string = "top center"; + } else if (anchor == TOP_RIGHT) { + anchor_string = "top right"; + } else if (anchor == CENTER_LEFT) { + anchor_string = "center left"; + } else if (anchor == CENTER) { + anchor_string = "center"; + } else if (anchor == CENTER_RIGHT) { + anchor_string = "center right"; + } else if (anchor == BOTTOM_LEFT) { + anchor_string = "bottom left"; + } else if (anchor == BOTTOM_CENTER) { + anchor_string = "bottom center"; + } else if (anchor == BOTTOM_RIGHT) { + anchor_string = "bottom right"; + } + + return anchor_string; + } + + std::string DisparityPlugin::UnitsToString(Units units) + { + std::string units_string = "pixels"; + + if (units == PIXELS) + { + units_string = "pixels"; + } else if (units == PERCENT) { + units_string = "percent"; + } + + return units_string; + } + + const unsigned char DisparityPlugin::COLOR_MAP[768] = + { 150, 150, 150, + 107, 0, 12, + 106, 0, 18, + 105, 0, 24, + 103, 0, 30, + 102, 0, 36, + 101, 0, 42, + 99, 0, 48, + 98, 0, 54, + 97, 0, 60, + 96, 0, 66, + 94, 0, 72, + 93, 0, 78, + 92, 0, 84, + 91, 0, 90, + 89, 0, 96, + 88, 0, 102, + 87, 0, 108, + 85, 0, 114, + 84, 0, 120, + 83, 0, 126, + 82, 0, 131, + 80, 0, 137, + 79, 0, 143, + 78, 0, 149, + 77, 0, 155, + 75, 0, 161, + 74, 0, 167, + 73, 0, 173, + 71, 0, 179, + 70, 0, 185, + 69, 0, 191, + 68, 0, 197, + 66, 0, 203, + 65, 0, 209, + 64, 0, 215, + 62, 0, 221, + 61, 0, 227, + 60, 0, 233, + 59, 0, 239, + 57, 0, 245, + 56, 0, 251, + 55, 0, 255, + 54, 0, 255, + 52, 0, 255, + 51, 0, 255, + 50, 0, 255, + 48, 0, 255, + 47, 0, 255, + 46, 0, 255, + 45, 0, 255, + 43, 0, 255, + 42, 0, 255, + 41, 0, 255, + 40, 0, 255, + 38, 0, 255, + 37, 0, 255, + 36, 0, 255, + 34, 0, 255, + 33, 0, 255, + 32, 0, 255, + 31, 0, 255, + 29, 0, 255, + 28, 0, 255, + 27, 0, 255, + 26, 0, 255, + 24, 0, 255, + 23, 0, 255, + 22, 0, 255, + 20, 0, 255, + 19, 0, 255, + 18, 0, 255, + 17, 0, 255, + 15, 0, 255, + 14, 0, 255, + 13, 0, 255, + 11, 0, 255, + 10, 0, 255, + 9, 0, 255, + 8, 0, 255, + 6, 0, 255, + 5, 0, 255, + 4, 0, 255, + 3, 0, 255, + 1, 0, 255, + 0, 4, 255, + 0, 10, 255, + 0, 16, 255, + 0, 22, 255, + 0, 28, 255, + 0, 34, 255, + 0, 40, 255, + 0, 46, 255, + 0, 52, 255, + 0, 58, 255, + 0, 64, 255, + 0, 70, 255, + 0, 76, 255, + 0, 82, 255, + 0, 88, 255, + 0, 94, 255, + 0, 100, 255, + 0, 106, 255, + 0, 112, 255, + 0, 118, 255, + 0, 124, 255, + 0, 129, 255, + 0, 135, 255, + 0, 141, 255, + 0, 147, 255, + 0, 153, 255, + 0, 159, 255, + 0, 165, 255, + 0, 171, 255, + 0, 177, 255, + 0, 183, 255, + 0, 189, 255, + 0, 195, 255, + 0, 201, 255, + 0, 207, 255, + 0, 213, 255, + 0, 219, 255, + 0, 225, 255, + 0, 231, 255, + 0, 237, 255, + 0, 243, 255, + 0, 249, 255, + 0, 255, 255, + 0, 255, 249, + 0, 255, 243, + 0, 255, 237, + 0, 255, 231, + 0, 255, 225, + 0, 255, 219, + 0, 255, 213, + 0, 255, 207, + 0, 255, 201, + 0, 255, 195, + 0, 255, 189, + 0, 255, 183, + 0, 255, 177, + 0, 255, 171, + 0, 255, 165, + 0, 255, 159, + 0, 255, 153, + 0, 255, 147, + 0, 255, 141, + 0, 255, 135, + 0, 255, 129, + 0, 255, 124, + 0, 255, 118, + 0, 255, 112, + 0, 255, 106, + 0, 255, 100, + 0, 255, 94, + 0, 255, 88, + 0, 255, 82, + 0, 255, 76, + 0, 255, 70, + 0, 255, 64, + 0, 255, 58, + 0, 255, 52, + 0, 255, 46, + 0, 255, 40, + 0, 255, 34, + 0, 255, 28, + 0, 255, 22, + 0, 255, 16, + 0, 255, 10, + 0, 255, 4, + 2, 255, 0, + 8, 255, 0, + 14, 255, 0, + 20, 255, 0, + 26, 255, 0, + 32, 255, 0, + 38, 255, 0, + 44, 255, 0, + 50, 255, 0, + 56, 255, 0, + 62, 255, 0, + 68, 255, 0, + 74, 255, 0, + 80, 255, 0, + 86, 255, 0, + 92, 255, 0, + 98, 255, 0, + 104, 255, 0, + 110, 255, 0, + 116, 255, 0, + 122, 255, 0, + 128, 255, 0, + 133, 255, 0, + 139, 255, 0, + 145, 255, 0, + 151, 255, 0, + 157, 255, 0, + 163, 255, 0, + 169, 255, 0, + 175, 255, 0, + 181, 255, 0, + 187, 255, 0, + 193, 255, 0, + 199, 255, 0, + 205, 255, 0, + 211, 255, 0, + 217, 255, 0, + 223, 255, 0, + 229, 255, 0, + 235, 255, 0, + 241, 255, 0, + 247, 255, 0, + 253, 255, 0, + 255, 251, 0, + 255, 245, 0, + 255, 239, 0, + 255, 233, 0, + 255, 227, 0, + 255, 221, 0, + 255, 215, 0, + 255, 209, 0, + 255, 203, 0, + 255, 197, 0, + 255, 191, 0, + 255, 185, 0, + 255, 179, 0, + 255, 173, 0, + 255, 167, 0, + 255, 161, 0, + 255, 155, 0, + 255, 149, 0, + 255, 143, 0, + 255, 137, 0, + 255, 131, 0, + 255, 126, 0, + 255, 120, 0, + 255, 114, 0, + 255, 108, 0, + 255, 102, 0, + 255, 96, 0, + 255, 90, 0, + 255, 84, 0, + 255, 78, 0, + 255, 72, 0, + 255, 66, 0, + 255, 60, 0, + 255, 54, 0, + 255, 48, 0, + 255, 42, 0, + 255, 36, 0, + 255, 30, 0, + 255, 24, 0, + 255, 18, 0, + 255, 12, 0, + 255, 6, 0, + 255, 0, 0, + }; +} // namespace mapviz_plugins + diff --git a/mapviz_plugins/src/draw_polygon_plugin.cpp b/mapviz_plugins/src/draw_polygon_plugin.cpp new file mode 100644 index 000000000..0204bd7e2 --- /dev/null +++ b/mapviz_plugins/src/draw_polygon_plugin.cpp @@ -0,0 +1,424 @@ +// ***************************************************************************** +// +// Copyright (c) 2016, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include + +// QT libraries +#include +#include +#include +#include +#include + +#include +#include +#include + +// Declare plugin +#include + +// C++ standard libraries +#include +#include +#include +#include +#include + +PLUGINLIB_EXPORT_CLASS(mapviz_plugins::DrawPolygonPlugin, mapviz::MapvizPlugin) + +namespace stu = swri_transform_util; + +namespace mapviz_plugins +{ + DrawPolygonPlugin::DrawPolygonPlugin() + : MapvizPlugin() + , ui_() + , config_widget_(new QWidget()) + , map_canvas_(nullptr) + , selected_point_(-1) + , is_mouse_down_(false) + , mouse_down_time_(0) + , max_ms_(Q_INT64_C(500)) + , max_distance_(2.0) + { + ui_.setupUi(config_widget_); + + ui_.color->setColor(Qt::green); + // Set background white + QPalette p(config_widget_->palette()); + p.setColor(QPalette::Window, Qt::white); + config_widget_->setPalette(p); + // Set status text red + QPalette p3(ui_.status->palette()); + p3.setColor(QPalette::Text, Qt::red); + ui_.status->setPalette(p3); + + QObject::connect(ui_.selectframe, SIGNAL(clicked()), this, + SLOT(SelectFrame())); + QObject::connect(ui_.frame, SIGNAL(editingFinished()), this, + SLOT(FrameEdited())); + QObject::connect(ui_.publish, SIGNAL(clicked()), this, + SLOT(PublishPolygon())); + QObject::connect(ui_.clear, SIGNAL(clicked()), this, + SLOT(Clear())); + } + + DrawPolygonPlugin::~DrawPolygonPlugin() + { + if (map_canvas_) + { + map_canvas_->removeEventFilter(this); + } + } + + void DrawPolygonPlugin::SelectFrame() + { + std::string frame = mapviz::SelectFrameDialog::selectFrame(tf_buf_); + if (!frame.empty()) + { + ui_.frame->setText(QString::fromStdString(frame)); + FrameEdited(); + } + } + + void DrawPolygonPlugin::FrameEdited() + { + source_frame_ = ui_.frame->text().toStdString(); + PrintWarning("Waiting for transform."); + + RCLCPP_INFO(node_->get_logger(), "Setting target frame to to %s", source_frame_.c_str()); + + initialized_ = true; + } + + void DrawPolygonPlugin::PublishPolygon() + { + if (polygon_topic_ != ui_.topic->text().toStdString()) + { + polygon_topic_ = ui_.topic->text().toStdString(); + rclcpp::QoS qos = rclcpp::QoS(1).durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + polygon_pub_ = node_->create_publisher( + polygon_topic_, qos); + } + + geometry_msgs::msg::PolygonStamped::UniquePtr polygon = + std::make_unique(); + polygon->header.stamp = node_->get_clock()->now(); + polygon->header.frame_id = ui_.frame->text().toStdString(); + + for (const auto& vertex : vertices_) + { + geometry_msgs::msg::Point32 point; + point.x = vertex.x(); + point.y = vertex.y(); + point.z = 0; + polygon->polygon.points.push_back(point); + } + + polygon_pub_->publish(*polygon); + } + + void DrawPolygonPlugin::Clear() + { + vertices_.clear(); + transformed_vertices_.clear(); + } + + void DrawPolygonPlugin::PrintError(const std::string& message) + { + PrintErrorHelper(ui_.status, message, 1.0); + } + + void DrawPolygonPlugin::PrintInfo(const std::string& message) + { + PrintInfoHelper(ui_.status, message, 1.0); + } + + void DrawPolygonPlugin::PrintWarning(const std::string& message) + { + PrintWarningHelper(ui_.status, message, 1.0); + } + + QWidget* DrawPolygonPlugin::GetConfigWidget(QWidget* parent) + { + config_widget_->setParent(parent); + + return config_widget_; + } + + bool DrawPolygonPlugin::Initialize(QGLWidget* canvas) + { + map_canvas_ = dynamic_cast(canvas); + map_canvas_->installEventFilter(this); + + initialized_ = true; + return true; + } + + bool DrawPolygonPlugin::eventFilter(QObject *object, QEvent* event) + { + switch (event->type()) + { + case QEvent::MouseButtonPress: + return handleMousePress(dynamic_cast(event)); + case QEvent::MouseButtonRelease: + return handleMouseRelease(dynamic_cast(event)); + case QEvent::MouseMove: + return handleMouseMove(dynamic_cast(event)); + default: + return false; + } + } + + bool DrawPolygonPlugin::handleMousePress(QMouseEvent* event) + { + if(!this->Visible()) + { + RCLCPP_DEBUG(node_->get_logger(), "Ignoring mouse press, since draw polygon plugin is hidden"); + return false; + } + + selected_point_ = -1; + int closest_point = 0; + double closest_distance = std::numeric_limits::max(); + + QPointF point = event->localPos(); + stu::Transform transform; + std::string frame = ui_.frame->text().toStdString(); + if (tf_manager_->GetTransform(target_frame_, frame, transform)) + { + for (size_t i = 0; i < vertices_.size(); i++) + { + tf2::Vector3 vertex = vertices_[i]; + vertex = transform * vertex; + + QPointF transformed = map_canvas_->FixedFrameToMapGlCoord(QPointF(vertex.x(), vertex.y())); + + double distance = QLineF(transformed, point).length(); + + if (distance < closest_distance) + { + closest_distance = distance; + closest_point = static_cast(i); + } + } + } + + if (event->button() == Qt::LeftButton) + { + if (closest_distance < 15) + { + selected_point_ = closest_point; + return true; + } else { + is_mouse_down_ = true; + mouse_down_pos_ = event->localPos(); + mouse_down_time_ = QDateTime::currentMSecsSinceEpoch(); + return false; + } + } else if (event->button() == Qt::RightButton) { + if (closest_distance < 15) + { + vertices_.erase(vertices_.begin() + closest_point); + transformed_vertices_.resize(vertices_.size()); + return true; + } + } + + return false; + } + + bool DrawPolygonPlugin::handleMouseRelease(QMouseEvent* event) + { + std::string frame = ui_.frame->text().toStdString(); + if (selected_point_ >= 0 && static_cast(selected_point_) < vertices_.size()) + { + QPointF point = event->localPos(); + stu::Transform transform; + if (tf_manager_->GetTransform(frame, target_frame_, transform)) + { + QPointF transformed = map_canvas_->MapGlCoordToFixedFrame(point); + tf2::Vector3 position(transformed.x(), transformed.y(), 0.0); + position = transform * position; + vertices_[selected_point_].setX(position.x()); + vertices_[selected_point_].setY(position.y()); + } + + selected_point_ = -1; + return true; + } else if (is_mouse_down_) { + qreal distance = QLineF(mouse_down_pos_, event->localPos()).length(); + qint64 msecsDiff = QDateTime::currentMSecsSinceEpoch() - mouse_down_time_; + + // Only fire the event if the mouse has moved less than the maximum distance + // and was held for shorter than the maximum time.. This prevents click + // events from being fired if the user is dragging the mouse across the map + // or just holding the cursor in place. + if (msecsDiff < max_ms_ && distance <= max_distance_) + { + QPointF point = event->localPos(); + + QPointF transformed = map_canvas_->MapGlCoordToFixedFrame(point); + RCLCPP_INFO( + node_->get_logger(), + "mouse point at %f, %f -> %f, %f", + point.x(), + point.y(), + transformed.x(), + transformed.y()); + + stu::Transform transform; + tf2::Vector3 position(transformed.x(), transformed.y(), 0.0); + + if (tf_manager_->GetTransform(frame, target_frame_, transform)) + { + position = transform * position; + vertices_.push_back(position); + transformed_vertices_.resize(vertices_.size()); + RCLCPP_INFO( + node_->get_logger(), + "Adding vertex at %lf, %lf %s", + position.x(), + position.y(), + frame.c_str()); + } + } + } + is_mouse_down_ = false; + + return false; + } + + bool DrawPolygonPlugin::handleMouseMove(QMouseEvent* event) + { + if (selected_point_ >= 0 && static_cast(selected_point_) < vertices_.size()) + { + QPointF point = event->localPos(); + stu::Transform transform; + std::string frame = ui_.frame->text().toStdString(); + if (tf_manager_->GetTransform(frame, target_frame_, transform)) + { + QPointF transformed = map_canvas_->MapGlCoordToFixedFrame(point); + tf2::Vector3 position(transformed.x(), transformed.y(), 0.0); + position = transform * position; + vertices_[selected_point_].setY(position.y()); + vertices_[selected_point_].setX(position.x()); + } + + return true; + } + return false; + } + + void DrawPolygonPlugin::Draw(double x, double y, double scale) + { + stu::Transform transform; + std::string frame = ui_.frame->text().toStdString(); + if (!tf_manager_->GetTransform(target_frame_, frame, transform)) + { + return; + } + + // Transform polygon + for (size_t i = 0; i < vertices_.size(); i++) + { + transformed_vertices_[i] = transform * vertices_[i]; + } + + glLineWidth(1); + const QColor color = ui_.color->color(); + glColor4d(color.redF(), color.greenF(), color.blueF(), 1.0); + glBegin(GL_LINE_STRIP); + + for (const auto& vertex : transformed_vertices_) + { + glVertex2d(vertex.x(), vertex.y()); + } + + glEnd(); + + glBegin(GL_LINES); + + glColor4d(color.redF(), color.greenF(), color.blueF(), 0.25); + + if (transformed_vertices_.size() > 2) + { + glVertex2d(transformed_vertices_.front().x(), transformed_vertices_.front().y()); + glVertex2d(transformed_vertices_.back().x(), transformed_vertices_.back().y()); + } + + glEnd(); + + // Draw vertices + glPointSize(9); + glBegin(GL_POINTS); + + for (const auto& vertex : transformed_vertices_) + { + glVertex2d(vertex.x(), vertex.y()); + } + glEnd(); + + + + PrintInfo("OK"); + } + + void DrawPolygonPlugin::LoadConfig(const YAML::Node& node, const std::string& path) + { + if (node["frame"]) + { + source_frame_ = node["frame"].as(); + ui_.frame->setText(source_frame_.c_str()); + } + + if (node["polygon_topic"]) + { + std::string polygon_topic = node["polygon_topic"].as(); + ui_.topic->setText(polygon_topic.c_str()); + } + if (node["color"]) + { + std::string color = node["color"].as(); + ui_.color->setColor(QColor(color.c_str())); + } + } + + void DrawPolygonPlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path) + { + std::string frame = ui_.frame->text().toStdString(); + emitter << YAML::Key << "frame" << YAML::Value << frame; + + std::string polygon_topic = ui_.topic->text().toStdString(); + emitter << YAML::Key << "polygon_topic" << YAML::Value << polygon_topic; + + std::string color = ui_.color->color().name().toStdString(); + emitter << YAML::Key << "color" << YAML::Value << color; + } +} // namespace mapviz_plugins diff --git a/mapviz_plugins/src/float_plugin.cpp b/mapviz_plugins/src/float_plugin.cpp new file mode 100644 index 000000000..293520b05 --- /dev/null +++ b/mapviz_plugins/src/float_plugin.cpp @@ -0,0 +1,501 @@ +// ***************************************************************************** +// +// Copyright (c) 2019, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL Southwest Research Institute® BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY +// OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +// DAMAGE. +// +// ***************************************************************************** + +#include + +#include +#include + +#include + +PLUGINLIB_EXPORT_CLASS(mapviz_plugins::FloatPlugin, mapviz::MapvizPlugin) + +namespace mapviz_plugins +{ + const char* FloatPlugin::COLOR_KEY = "color"; + const char* FloatPlugin::FONT_KEY = "font"; + const char* FloatPlugin::TOPIC_KEY = "topic"; + const char* FloatPlugin::ANCHOR_KEY = "anchor"; + const char* FloatPlugin::UNITS_KEY = "units"; + const char* FloatPlugin::OFFSET_X_KEY = "offset_x"; + const char* FloatPlugin::OFFSET_Y_KEY = "offset_y"; + const char* FloatPlugin::POSTFIX_KEY = "postfix_text"; + + FloatPlugin::FloatPlugin() + : MapvizPlugin() + , ui_() + , config_widget_(new QWidget()) + , anchor_(TOP_LEFT) + , units_(PIXELS) + , offset_x_(0) + , offset_y_(0) + , has_message_(false) + , has_painted_(false) + , color_(Qt::black) + { + ui_.setupUi(config_widget_); + // Set background white + QPalette p(config_widget_->palette()); + p.setColor(QPalette::Window, Qt::white); + config_widget_->setPalette(p); + + // Set status text red + QPalette p3(ui_.status->palette()); + p3.setColor(QPalette::Text, Qt::red); + ui_.status->setPalette(p3); + + QObject::connect(ui_.selecttopic, SIGNAL(clicked()), this, SLOT(SelectTopic())); + QObject::connect(ui_.topic, SIGNAL(editingFinished()), this, SLOT(TopicEdited())); + QObject::connect(ui_.anchor, SIGNAL(activated(QString)), this, SLOT(SetAnchor(QString))); + QObject::connect(ui_.units, SIGNAL(activated(QString)), this, SLOT(SetUnits(QString))); + QObject::connect(ui_.offsetx, SIGNAL(valueChanged(int)), this, SLOT(SetOffsetX(int))); + QObject::connect(ui_.offsety, SIGNAL(valueChanged(int)), this, SLOT(SetOffsetY(int))); + QObject::connect(ui_.font_button, SIGNAL(clicked()), this, SLOT(SelectFont())); + QObject::connect(ui_.color, SIGNAL(colorEdited(const QColor &)), this, SLOT(SelectColor())); + QObject::connect(ui_.postfix, SIGNAL(editingFinished()), this, SLOT(PostfixEdited())); + + font_.setFamily(tr("Helvetica")); + ui_.font_button->setFont(font_); + ui_.font_button->setText(font_.family()); + + ui_.color->setColor(color_); + } + + bool FloatPlugin::Initialize(QGLWidget* canvas) + { + canvas_ = canvas; + return true; + } + + void FloatPlugin::Draw(double, double, double) + { + // This plugin doesn't do any OpenGL drawing. + } + + void FloatPlugin::Paint(QPainter* painter, double, double, double) + { + if (has_message_) + { + painter->save(); + painter->resetTransform(); + painter->setFont(font_); + + if (!has_painted_) + { + // After the first time we get a new message, we do not know how wide it's + // going to be when rendered, so we can't accurately calculate the top left + // coordinate if it's offset from the right or bottom borders. + // The easiest workaround I've found for this is to draw it once using + // a completely transparent pen, which will cause the QStaticText class to + // know how wide it is; then we can recalculate the offsets and draw it + // again with a visible pen. + QPen invisPen(QBrush(Qt::transparent), 1); + painter->setPen(invisPen); + PaintText(painter); + has_painted_ = true; + } + QPen pen(QBrush(color_), 1); + painter->setPen(pen); + PaintText(painter); + + painter->restore(); + PrintInfo("OK"); + } + else + { + PrintWarning("No messages received."); + } + } + + void FloatPlugin::PaintText(QPainter* painter) + { + // Calculate the correct offsets and dimensions + int x_offset = offset_x_; + int y_offset = offset_y_; + if (units_ == PERCENT) + { + x_offset = static_cast((float)(offset_x_ * canvas_->width()) / 100.0); + y_offset = static_cast((float)(offset_y_ * canvas_->height()) / 100.0); + } + + int right = static_cast((float)canvas_->width() - message_.size().width()) - x_offset; + int bottom = static_cast((float)canvas_->height() - message_.size().height()) - y_offset; + int yCenter = static_cast((float)canvas_->height() / 2.0 - message_.size().height()/2.0); + int xCenter = static_cast((float)canvas_->width() / 2.0 - message_.size().width()/2.0); + + QPoint ulPoint; + + switch (anchor_) + { + case TOP_LEFT: + ulPoint.setX(x_offset); + ulPoint.setY(y_offset); + break; + case TOP_CENTER: + ulPoint.setX(xCenter); + ulPoint.setY(y_offset); + break; + case TOP_RIGHT: + ulPoint.setX(right); + ulPoint.setY(y_offset); + break; + case CENTER_LEFT: + ulPoint.setX(x_offset); + ulPoint.setY(yCenter); + break; + case CENTER: + ulPoint.setX(xCenter); + ulPoint.setY(yCenter); + break; + case CENTER_RIGHT: + ulPoint.setX(right); + ulPoint.setY(yCenter); + break; + case BOTTOM_LEFT: + ulPoint.setX(x_offset); + ulPoint.setY(bottom); + break; + case BOTTOM_CENTER: + ulPoint.setX(xCenter); + ulPoint.setY(bottom); + break; + case BOTTOM_RIGHT: + ulPoint.setX(right); + ulPoint.setY(bottom); + break; + } + painter->drawStaticText(ulPoint, message_); + } + + void FloatPlugin::LoadConfig(const YAML::Node& node, const std::string& path) + { + if (node[TOPIC_KEY]) + { + ui_.topic->setText(QString(node[TOPIC_KEY].as().c_str())); + TopicEdited(); + } + + if (node[FONT_KEY]) + { + font_.fromString(QString(node[FONT_KEY].as().c_str())); + ui_.font_button->setFont(font_); + ui_.font_button->setText(font_.family()); + } + + if (node[COLOR_KEY]) + { + color_ = QColor(node[COLOR_KEY].as().c_str()); + ui_.color->setColor(QColor(color_.name().toStdString().c_str())); + } + + if (node[ANCHOR_KEY]) + { + std::string anchor = node[ANCHOR_KEY].as(); + ui_.anchor->setCurrentIndex(ui_.anchor->findText(anchor.c_str())); + SetAnchor(anchor.c_str()); + } + + if (node[UNITS_KEY]) + { + std::string units = node[UNITS_KEY].as(); + ui_.units->setCurrentIndex(ui_.units->findText(units.c_str())); + SetUnits(units.c_str()); + } + + if (node[OFFSET_X_KEY]) + { + offset_x_ = node[OFFSET_X_KEY].as(); + ui_.offsetx->setValue(offset_x_); + } + + if (node[OFFSET_Y_KEY]) + { + offset_y_ = node[OFFSET_Y_KEY].as(); + ui_.offsety->setValue(offset_y_); + } + + if (node[POSTFIX_KEY]) + { + postfix_ = node[POSTFIX_KEY].as(); + ui_.postfix->setText(QString(postfix_.c_str())); + } + } + + void FloatPlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path) + { + emitter << YAML::Key << FONT_KEY << YAML::Value << font_.toString().toStdString(); + emitter << YAML::Key << COLOR_KEY << YAML::Value << color_.name().toStdString(); + emitter << YAML::Key << TOPIC_KEY << YAML::Value << ui_.topic->text().toStdString(); + emitter << YAML::Key << ANCHOR_KEY << YAML::Value << AnchorToString(anchor_); + emitter << YAML::Key << UNITS_KEY << YAML::Value << UnitsToString(units_); + emitter << YAML::Key << OFFSET_X_KEY << YAML::Value << offset_x_; + emitter << YAML::Key << OFFSET_Y_KEY << YAML::Value << offset_y_; + emitter << YAML::Key << POSTFIX_KEY << YAML::Value << postfix_; + } + + QWidget* FloatPlugin::GetConfigWidget(QWidget* parent) + { + config_widget_->setParent(parent); + return config_widget_; + } + + void FloatPlugin::PrintError(const std::string& message) + { + PrintErrorHelper(ui_.status, message); + } + + void FloatPlugin::PrintInfo(const std::string& message) + { + PrintInfoHelper(ui_.status, message); + } + + void FloatPlugin::PrintWarning(const std::string& message) + { + PrintWarningHelper(ui_.status, message); + } + + void FloatPlugin::SelectColor() + { + color_ = ui_.color->color(); + } + + void FloatPlugin::PostfixEdited() + { + postfix_ = ui_.postfix->text().toStdString(); + } + + void FloatPlugin::SelectFont() + { + bool ok; + QFont font = QFontDialog::getFont(&ok, font_, canvas_); + if (ok) + { + font_ = font; + message_.prepare(QTransform(), font_); + ui_.font_button->setFont(font_); + ui_.font_button->setText(font_.family()); + } + } + + void FloatPlugin::SelectTopic() + { + std::vector topics; + topics.emplace_back("std_msgs/msg/Float32"); + topics.emplace_back("std_msgs/msg/Float64"); + topics.emplace_back("marti_common_msgs/msg/Float32Stamped"); + topics.emplace_back("marti_common_msgs/msg/Float64Stamped"); + topics.emplace_back("marti_sensor_msgs/msg/Velocity"); + std::string topic = mapviz::SelectTopicDialog::selectTopic(node_, topics); + + if (!topic.empty()) + { + ui_.topic->setText(QString::fromStdString(topic)); + TopicEdited(); + } + } + + void FloatPlugin::TopicEdited() + { + std::string topic = ui_.topic->text().trimmed().toStdString(); + if (topic != topic_) + { + initialized_ = false; + has_message_ = false; + PrintWarning("No messages received."); + + float32_sub_.reset(); + float64_sub_.reset(); + float32_stamped_sub_.reset(); + float64_stamped_sub_.reset(); + velocity_sub_.reset(); + + topic_ = topic; + if (!topic.empty()) + { + float32_sub_ = node_->create_subscription(topic_, 1, + [this](const std_msgs::msg::Float32::ConstSharedPtr msg) { + floatCallback(msg->data); + }); + float64_sub_ = node_->create_subscription(topic_, 1, + [this](const std_msgs::msg::Float64::ConstSharedPtr msg) { + floatCallback(msg->data); + }); + float32_stamped_sub_ = node_->create_subscription(topic_, 1, + [this](const marti_common_msgs::msg::Float32Stamped::ConstSharedPtr msg) { + floatCallback(msg->value); + }); + float64_stamped_sub_ = node_->create_subscription(topic_, 1, + [this](const marti_common_msgs::msg::Float64Stamped::ConstSharedPtr msg) { + floatCallback(msg->value); + }); + velocity_sub_ = node_->create_subscription(topic_, 1, + [this](const marti_sensor_msgs::msg::Velocity::ConstSharedPtr msg) { + floatCallback(msg->velocity); + }); + } + } + } + + void FloatPlugin::SetAnchor(QString anchor) + { + if (anchor == "top left") + { + anchor_ = TOP_LEFT; + } + else if (anchor == "top center") + { + anchor_ = TOP_CENTER; + } + else if (anchor == "top right") + { + anchor_ = TOP_RIGHT; + } + else if (anchor == "center left") + { + anchor_ = CENTER_LEFT; + } + else if (anchor == "center") + { + anchor_ = CENTER; + } + else if (anchor == "center right") + { + anchor_ = CENTER_RIGHT; + } + else if (anchor == "bottom left") + { + anchor_ = BOTTOM_LEFT; + } + else if (anchor == "bottom center") + { + anchor_ = BOTTOM_CENTER; + } + else if (anchor == "bottom right") + { + anchor_ = BOTTOM_RIGHT; + } + } + + void FloatPlugin::SetUnits(QString units) + { + if (units == "pixels") + { + units_ = PIXELS; + } + else if (units == "percent") + { + units_ = PERCENT; + } + } + + void FloatPlugin::SetOffsetX(int offset) + { + offset_x_ = offset; + } + + void FloatPlugin::SetOffsetY(int offset) + { + offset_y_ = offset; + } + + void FloatPlugin::floatCallback(double value) + { + + std::string str = std::to_string(value); + str += postfix_; + message_.setText(QString(str.c_str())); + + message_.prepare(QTransform(), font_); + + has_message_ = true; + has_painted_ = false; + initialized_ = true; + } + + std::string FloatPlugin::AnchorToString(FloatPlugin::Anchor anchor) + { + std::string anchor_string = "top left"; + + if (anchor == TOP_LEFT) + { + anchor_string = "top left"; + } + else if (anchor == TOP_CENTER) + { + anchor_string = "top center"; + } + else if (anchor == TOP_RIGHT) + { + anchor_string = "top right"; + } + else if (anchor == CENTER_LEFT) + { + anchor_string = "center left"; + } + else if (anchor == CENTER) + { + anchor_string = "center"; + } + else if (anchor == CENTER_RIGHT) + { + anchor_string = "center right"; + } + else if (anchor == BOTTOM_LEFT) + { + anchor_string = "bottom left"; + } + else if (anchor == BOTTOM_CENTER) + { + anchor_string = "bottom center"; + } + else if (anchor == BOTTOM_RIGHT) + { + anchor_string = "bottom right"; + } + + return anchor_string; + } + + std::string FloatPlugin::UnitsToString(FloatPlugin::Units units) + { + std::string units_string = "pixels"; + + if (units == PIXELS) + { + units_string = "pixels"; + } + else if (units == PERCENT) + { + units_string = "percent"; + } + + return units_string; + } +} diff --git a/mapviz_plugins/src/gps_plugin.cpp b/mapviz_plugins/src/gps_plugin.cpp new file mode 100644 index 000000000..6679af67c --- /dev/null +++ b/mapviz_plugins/src/gps_plugin.cpp @@ -0,0 +1,292 @@ +// ***************************************************************************** +// +// Copyright (C) 2013 All Right Reserved, Southwest Research Institute® (SwRI®) +// +// Contract No. 10-58058A +// Contractor Southwest Research Institute® (SwRI®) +// Address 6220 Culebra Road, San Antonio, Texas 78228-0510 +// Contact Steve Dellenback (210) 522-3914 +// +// This code was developed as part of an internal research project fully funded +// by Southwest Research Institute®. +// +// THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY +// KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A +// PARTICULAR PURPOSE. +// +// ***************************************************************************** + +#include + +// QT libraries +#include +#include +#include + +#include + +// ROS libraries +#include + +#include +#include +#include + +// Declare plugin +#include + +// C++ standard libraries +#include +#include +#include +#include + +PLUGINLIB_EXPORT_CLASS(mapviz_plugins::GpsPlugin, mapviz::MapvizPlugin) + +namespace mapviz_plugins +{ + GpsPlugin::GpsPlugin() + : PointDrawingPlugin() + , ui_() + , config_widget_(new QWidget()) + , has_message_(false) + { + ui_.setupUi(config_widget_); + + ui_.color->setColor(Qt::green); + + // Set background white + QPalette p(config_widget_->palette()); + p.setColor(QPalette::Window, Qt::white); + config_widget_->setPalette(p); + + // Set status text red + QPalette p3(ui_.status->palette()); + p3.setColor(QPalette::Text, Qt::red); + ui_.status->setPalette(p3); + + QObject::connect(ui_.selecttopic, SIGNAL(clicked()), this, + SLOT(SelectTopic())); + QObject::connect(ui_.topic, SIGNAL(editingFinished()), this, + SLOT(TopicEdited())); + QObject::connect(ui_.positiontolerance, SIGNAL(valueChanged(double)), this, + SLOT(PositionToleranceChanged(double))); + QObject::connect(ui_.buffersize, SIGNAL(valueChanged(int)), this, + SLOT(BufferSizeChanged(int))); + QObject::connect(ui_.drawstyle, SIGNAL(activated(QString)), this, + SLOT(SetDrawStyle(QString))); + QObject::connect(ui_.static_arrow_sizes, SIGNAL(clicked(bool)), + this, SLOT(SetStaticArrowSizes(bool))); + QObject::connect(ui_.arrow_size, SIGNAL(valueChanged(int)), + this, SLOT(SetArrowSize(int))); + QObject::connect(ui_.color, SIGNAL(colorEdited(const QColor&)), this, + SLOT(SetColor(const QColor&))); + QObject::connect(ui_.show_laps, SIGNAL(toggled(bool)), this, + SLOT(LapToggled(bool))); + QObject::connect(ui_.buttonResetBuffer, SIGNAL(pressed()), this, + SLOT(ClearPoints())); + } + + void GpsPlugin::SelectTopic() + { + std::string topic = mapviz::SelectTopicDialog::selectTopic(node_, "gps_msgs/msg/GPSFix"); + + if (!topic.empty()) + { + ui_.topic->setText(QString::fromStdString(topic)); + TopicEdited(); + } + } + + void GpsPlugin::TopicEdited() + { + std::string topic = ui_.topic->text().trimmed().toStdString(); + if (topic != topic_) + { + initialized_ = false; + ClearPoints(); + has_message_ = false; + PrintWarning("No messages received."); + + gps_sub_.reset(); + + topic_ = topic; + if (!topic.empty()) + { + gps_sub_ = node_->create_subscription(topic_, rclcpp::QoS(1), + std::bind(&GpsPlugin::GPSFixCallback, this, std::placeholders::_1)); + + RCLCPP_INFO(node_->get_logger(), "Subscribing to %s", topic_.c_str()); + } + } + } + + void GpsPlugin::GPSFixCallback(const gps_msgs::msg::GPSFix::SharedPtr gps) + { + if (!tf_manager_->LocalXyUtil()->Initialized()) + { + return; + } + if (!has_message_) + { + initialized_ = true; + has_message_ = true; + } + + StampedPoint stamped_point; + stamped_point.stamp = gps->header.stamp; + stamped_point.source_frame = tf_manager_->LocalXyUtil()->Frame(); + double x; + double y; + tf_manager_->LocalXyUtil()->ToLocalXy(gps->latitude, gps->longitude, x, y); + + stamped_point.point = tf2::Vector3(x, y, gps->altitude); + + // The GPS "track" is in degrees, but createQuaternionFromYaw expects + // radians. + // Furthermore, the track rotates in the opposite direction and is also + // offset by 90 degrees, so all of that has to be compensated for. + auto temp_quat = tf2::Quaternion(); + temp_quat.setRPY(0, 0, (-gps->track * (M_PI / 180.0)) + M_PI_2); + stamped_point.orientation = temp_quat; + + pushPoint( std::move(stamped_point) ); + } + + void GpsPlugin::PrintError(const std::string& message) + { + PrintErrorHelper(ui_.status, message); + } + + void GpsPlugin::PrintInfo(const std::string& message) + { + PrintInfoHelper(ui_.status, message); + } + + void GpsPlugin::PrintWarning(const std::string& message) + { + PrintWarningHelper(ui_.status, message); + } + + QWidget* GpsPlugin::GetConfigWidget(QWidget* parent) + { + config_widget_->setParent(parent); + + return config_widget_; + } + + bool GpsPlugin::Initialize(QGLWidget* canvas) + { + canvas_ = canvas; + SetColor(ui_.color->color()); + + return true; + } + + void GpsPlugin::Draw(double x, double y, double scale) + { + if (DrawPoints(scale)) + { + PrintInfo("OK"); + } + } + + void GpsPlugin::LoadConfig(const YAML::Node& node, const std::string& path) + { + if (node["topic"]) + { + std::string topic = node["topic"].as(); + ui_.topic->setText(topic.c_str()); + } + + if (node["color"]) + { + std::string color = node["color"].as(); + QColor qcolor(color.c_str()); + SetColor(qcolor); + ui_.color->setColor(qcolor); + } + + if (node["draw_style"]) + { + std::string draw_style = node["draw_style"].as(); + + if (draw_style == "lines") + { + ui_.drawstyle->setCurrentIndex(0); + SetDrawStyle( LINES ); + } else if (draw_style == "points") { + ui_.drawstyle->setCurrentIndex(1); + SetDrawStyle( POINTS ); + } else if (draw_style == "arrows") { + ui_.drawstyle->setCurrentIndex(2); + SetDrawStyle( ARROWS ); + } + } + + if (node["position_tolerance"]) + { + double position_tolerance = node["position_tolerance"].as(); + ui_.positiontolerance->setValue(position_tolerance); + PositionToleranceChanged(position_tolerance); + } + + if (node["buffer_size"]) + { + double buffer_size = node["buffer_size"].as(); + ui_.buffersize->setValue(buffer_size); + BufferSizeChanged(buffer_size); + } + + if (node["show_laps"]) + { + bool show_laps = node["show_laps"].as(); + ui_.show_laps->setChecked(show_laps); + LapToggled(show_laps); + } + + if (node["static_arrow_sizes"]) + { + bool static_arrow_sizes = node["static_arrow_sizes"].as(); + ui_.static_arrow_sizes->setChecked(static_arrow_sizes); + SetStaticArrowSizes(static_arrow_sizes); + } + + if (node["arrow_size"]) + { + int arrow_size = node["arrow_size"].as(); + ui_.arrow_size->setValue(arrow_size); + SetArrowSize(arrow_size); + } + + TopicEdited(); + } + + void GpsPlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path) + { + std::string topic = ui_.topic->text().toStdString(); + emitter << YAML::Key << "topic" << YAML::Value << topic; + + emitter << YAML::Key << "color" << YAML::Value + << ui_.color->color().name().toStdString(); + + std::string draw_style = ui_.drawstyle->currentText().toStdString(); + emitter << YAML::Key << "draw_style" << YAML::Value << draw_style; + + emitter << YAML::Key << "position_tolerance" << + YAML::Value << positionTolerance(); + + emitter << YAML::Key << "buffer_size" << YAML::Value << bufferSize(); + + bool show_laps = ui_.show_laps->isChecked(); + emitter << YAML::Key << "show_laps" << YAML::Value << show_laps; + + emitter << YAML::Key + << "static_arrow_sizes" + << YAML::Value + << ui_.static_arrow_sizes->isChecked(); + + emitter << YAML::Key << "arrow_size" << YAML::Value << ui_.arrow_size->value(); + } +} // namespace mapviz_plugins diff --git a/mapviz_plugins/src/grid_plugin.cpp b/mapviz_plugins/src/grid_plugin.cpp new file mode 100644 index 000000000..5692e3f69 --- /dev/null +++ b/mapviz_plugins/src/grid_plugin.cpp @@ -0,0 +1,376 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include + +#include + +// QT libraries +#include +#include + +#include + +// Declare plugin +#include + +// C++ standard libraries +#include +#include +#include +#include + +PLUGINLIB_EXPORT_CLASS(mapviz_plugins::GridPlugin, mapviz::MapvizPlugin) + +namespace mapviz_plugins +{ + GridPlugin::GridPlugin() + : MapvizPlugin() + , ui_() + , config_widget_(new QWidget()) + , alpha_(1.0) + , top_left_(0, 0, 0) + , size_(1) + , rows_(1) + , columns_(1) + , transformed_(false) + { + ui_.setupUi(config_widget_); + + ui_.color->setColor(Qt::red); + + // Set background white + QPalette p(config_widget_->palette()); + p.setColor(QPalette::Window, Qt::white); + config_widget_->setPalette(p); + + // Set status text red + QPalette p3(ui_.status->palette()); + p3.setColor(QPalette::Text, Qt::red); + ui_.status->setPalette(p3); + + QObject::connect(ui_.select_frame, SIGNAL(clicked()), this, SLOT(SelectFrame())); + QObject::connect(ui_.frame, SIGNAL(textEdited(const QString&)), this, SLOT(FrameEdited())); + QObject::connect(ui_.alpha, SIGNAL(valueChanged(double)), this, SLOT(SetAlpha(double))); + QObject::connect(ui_.x, SIGNAL(valueChanged(double)), this, SLOT(SetX(double))); + QObject::connect(ui_.y, SIGNAL(valueChanged(double)), this, SLOT(SetY(double))); + QObject::connect(ui_.size, SIGNAL(valueChanged(double)), this, SLOT(SetSize(double))); + QObject::connect(ui_.rows, SIGNAL(valueChanged(int)), this, SLOT(SetRows(int))); + QObject::connect(ui_.columns, SIGNAL(valueChanged(int)), this, SLOT(SetColumns(int))); + connect(ui_.color, SIGNAL(colorEdited(const QColor &)), this, SLOT(DrawIcon())); + } + + void GridPlugin::DrawIcon() + { + if (icon_) + { + QPixmap icon(16, 16); + icon.fill(Qt::transparent); + + QPainter painter(&icon); + painter.setRenderHint(QPainter::Antialiasing, true); + + QPen pen(QColor(ui_.color->color())); + + pen.setWidth(2); + pen.setCapStyle(Qt::SquareCap); + painter.setPen(pen); + + painter.drawLine(2, 2, 14, 2); + painter.drawLine(2, 2, 2, 14); + painter.drawLine(14, 2, 14, 14); + painter.drawLine(2, 14, 14, 14); + painter.drawLine(8, 2, 8, 14); + painter.drawLine(2, 8, 14, 8); + + icon_->SetPixmap(icon); + } + } + + void GridPlugin::SetAlpha(double alpha) + { + alpha_ = alpha; + } + + void GridPlugin::SetX(double x) + { + top_left_.setX(x); + + RecalculateGrid(); + } + + void GridPlugin::SetY(double y) + { + top_left_.setY(y); + + RecalculateGrid(); + } + + void GridPlugin::SetSize(double size) + { + size_ = size; + + RecalculateGrid(); + } + + void GridPlugin::SetRows(int rows) + { + rows_ = rows; + + RecalculateGrid(); + } + + void GridPlugin::SetColumns(int columns) + { + columns_ = columns; + + RecalculateGrid(); + } + + void GridPlugin::SelectFrame() + { + std::string frame = mapviz::SelectFrameDialog::selectFrame(tf_buf_); + if (!frame.empty()) + { + ui_.frame->setText(QString::fromStdString(frame)); + FrameEdited(); + } + } + + void GridPlugin::FrameEdited() + { + source_frame_ = ui_.frame->text().toStdString(); + + initialized_ = true; + + RecalculateGrid(); + } + + void GridPlugin::PrintError(const std::string& message) + { + PrintErrorHelper(ui_.status, message); + } + + void GridPlugin::PrintInfo(const std::string& message) + { + PrintInfoHelper(ui_.status, message); + } + + void GridPlugin::PrintWarning(const std::string& message) + { + PrintWarningHelper(ui_.status, message); + } + + QWidget* GridPlugin::GetConfigWidget(QWidget* parent) + { + config_widget_->setParent(parent); + + return config_widget_; + } + + bool GridPlugin::Initialize(QGLWidget* canvas) + { + canvas_ = canvas; + + DrawIcon(); + + return true; + } + + void GridPlugin::Draw(double x, double y, double scale) + { + if (transformed_) { + QColor color = ui_.color->color(); + + glLineWidth(3); + glColor4d(color.redF(), color.greenF(), color.blueF(), alpha_); + glBegin(GL_LINES); + + auto transformed_left_it = transformed_left_points_.begin(); + auto transformed_right_it = transformed_right_points_.begin(); + for (; transformed_left_it != transformed_left_points_.end(); ++transformed_left_it) { + glVertex2d(transformed_left_it->getX(), transformed_left_it->getY()); + glVertex2d(transformed_right_it->getX(), transformed_right_it->getY()); + + ++transformed_right_it; + } + + auto transformed_top_it = transformed_top_points_.begin(); + auto transformed_bottom_it = transformed_bottom_points_.begin(); + for (; transformed_top_it != transformed_top_points_.end(); ++transformed_top_it) { + glVertex2d(transformed_top_it->getX(), transformed_top_it->getY()); + glVertex2d(transformed_bottom_it->getX(), transformed_bottom_it->getY()); + + ++transformed_bottom_it; + } + + glEnd(); + + PrintInfo("OK"); + } + } + + void GridPlugin::RecalculateGrid() + { + transformed_ = false; + + left_points_.clear(); + right_points_.clear(); + top_points_.clear(); + bottom_points_.clear(); + + transformed_left_points_.clear(); + transformed_right_points_.clear(); + transformed_top_points_.clear(); + transformed_bottom_points_.clear(); + + // Set top and bottom + for (int c = 0; c <= columns_; c++) + { + tf2::Vector3 top_point(top_left_.getX() + c * size_, top_left_.getY(), 0); + top_points_.push_back(top_point); + transformed_top_points_.push_back(transform_ * top_point); + + tf2::Vector3 bottom_point(top_left_.getX() + c * size_, top_left_.getY() + size_ * rows_, 0); + bottom_points_.push_back(bottom_point); + transformed_bottom_points_.push_back(transform_ * bottom_point); + } + + // Set left and right + for (int r = 0; r <= rows_; r++) + { + tf2::Vector3 left_point(top_left_.getX(), top_left_.getY() + r * size_, 0); + left_points_.push_back(left_point); + transformed_left_points_.push_back(transform_ * left_point); + + tf2::Vector3 right_point( + top_left_.getX() + size_ * columns_, + top_left_.getY() + r * size_, + 0); + right_points_.push_back(right_point); + transformed_right_points_.push_back(transform_ * right_point); + } + } + + void GridPlugin::Transform() + { + transformed_ = false; + + if (GetTransform(rclcpp::Time(), transform_)) + { + Transform(left_points_, transformed_left_points_); + Transform(right_points_, transformed_right_points_); + Transform(top_points_, transformed_top_points_); + Transform(bottom_points_, transformed_bottom_points_); + + transformed_ = true; + } + } + + void GridPlugin::Transform(std::list& src, std::list& dst) + { + auto points_it = src.begin(); + auto transformed_it = dst.begin(); + for (; points_it != src.end() && transformed_it != dst.end(); ++points_it) + { + (*transformed_it) = transform_ * (*points_it); + + ++transformed_it; + } + } + + void GridPlugin::LoadConfig(const YAML::Node& node, const std::string& path) + { + if (node["color"]) + { + std::string color = node["color"].as(); + ui_.color->setColor(QColor(color.c_str())); + } + + if (node["frame"]) + { + std::string frame = node["frame"].as(); + ui_.frame->setText(QString::fromStdString(frame)); + } + + if (node["x"]) + { + float x = node["x"].as(); + ui_.x->setValue(x); + } + + if (node["y"]) + { + float y = node["y"].as(); + ui_.y->setValue(y); + } + + if (node["alpha"]) + { + alpha_ = node["alpha"].as(); + ui_.alpha->setValue(alpha_); + } + + if (node["size"]) + { + size_ = node["size"].as(); + ui_.size->setValue(size_); + } + + if (node["rows"]) + { + rows_ = node["rows"].as(); + ui_.rows->setValue(rows_); + } + + if (node["columns"]) + { + columns_ = node["columns"].as(); + ui_.columns->setValue(columns_); + } + + FrameEdited(); + } + + void GridPlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path) + { + emitter << YAML::Key << "color" << YAML::Value << ui_.color->color().name().toStdString(); + + emitter << YAML::Key << "alpha" << YAML::Value << alpha_; + + std::string frame = ui_.frame->text().toStdString(); + emitter << YAML::Key << "frame" << YAML::Value << frame; + + emitter << YAML::Key << "x" << YAML::Value << top_left_.getX(); + emitter << YAML::Key << "y" << YAML::Value << top_left_.getY(); + emitter << YAML::Key << "size" << YAML::Value << size_; + emitter << YAML::Key << "rows" << YAML::Value << rows_; + emitter << YAML::Key << "columns" << YAML::Value << columns_; + } +} // namespace mapviz_plugins diff --git a/mapviz_plugins/src/image_plugin.cpp b/mapviz_plugins/src/image_plugin.cpp new file mode 100644 index 000000000..7b401f50d --- /dev/null +++ b/mapviz_plugins/src/image_plugin.cpp @@ -0,0 +1,611 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include + +// QT libraries +#include +#include + +// ROS libraries +#include +#include + +#include + +// Declare plugin +#include + +// C++ standard libraries +#include +#include +#include + +PLUGINLIB_EXPORT_CLASS(mapviz_plugins::ImagePlugin, mapviz::MapvizPlugin) + +namespace mapviz_plugins +{ + ImagePlugin::ImagePlugin() + : MapvizPlugin() + , ui_() + , config_widget_(new QWidget()) + , anchor_(TOP_LEFT) + , units_(PIXELS) + , offset_x_(0) + , offset_y_(0) + , width_(320) + , height_(240) + , transport_("default") + , force_resubscribe_(false) + , has_image_(false) + , last_width_(0) + , last_height_(0) + , original_aspect_ratio_(1.0) + , has_message_(false) + { + ui_.setupUi(config_widget_); + + // Set background white + QPalette p(config_widget_->palette()); + p.setColor(QPalette::Window, Qt::white); + config_widget_->setPalette(p); + + // Set status text red + QPalette p3(ui_.status->palette()); + p3.setColor(QPalette::Text, Qt::red); + ui_.status->setPalette(p3); + + QObject::connect(ui_.selecttopic, SIGNAL(clicked()), this, SLOT(SelectTopic())); + QObject::connect(ui_.topic, SIGNAL(editingFinished()), this, SLOT(TopicEdited())); + QObject::connect(ui_.anchor, SIGNAL(activated(QString)), this, SLOT(SetAnchor(QString))); + QObject::connect(ui_.units, SIGNAL(activated(QString)), this, SLOT(SetUnits(QString))); + QObject::connect(ui_.offsetx, SIGNAL(valueChanged(int)), this, SLOT(SetOffsetX(int))); + QObject::connect(ui_.offsety, SIGNAL(valueChanged(int)), this, SLOT(SetOffsetY(int))); + QObject::connect(ui_.width, SIGNAL(valueChanged(double)), this, SLOT(SetWidth(double))); + QObject::connect(ui_.height, SIGNAL(valueChanged(double)), this, SLOT(SetHeight(double))); + QObject::connect(this, SIGNAL(VisibleChanged(bool)), this, SLOT(SetSubscription(bool))); + QObject::connect(ui_.keep_ratio, SIGNAL(toggled(bool)), this, SLOT(KeepRatioChanged(bool))); + QObject::connect(ui_.transport_combo_box, SIGNAL(activated(const QString&)), + this, SLOT(SetTransport(const QString&))); + + ui_.width->setKeyboardTracking(false); + ui_.height->setKeyboardTracking(false); + } + + void ImagePlugin::SetOffsetX(int offset) + { + offset_x_ = offset; + } + + void ImagePlugin::SetOffsetY(int offset) + { + offset_y_ = offset; + } + + void ImagePlugin::SetWidth(double width) + { + width_ = width; + } + + void ImagePlugin::SetHeight(double height) + { + height_ = height; + } + + void ImagePlugin::SetAnchor(QString anchor) + { + if (anchor == "top left") + { + anchor_ = TOP_LEFT; + } else if (anchor == "top center") { + anchor_ = TOP_CENTER; + } else if (anchor == "top right") { + anchor_ = TOP_RIGHT; + } else if (anchor == "center left") { + anchor_ = CENTER_LEFT; + } else if (anchor == "center") { + anchor_ = CENTER; + } else if (anchor == "center right") { + anchor_ = CENTER_RIGHT; + } else if (anchor == "bottom left") { + anchor_ = BOTTOM_LEFT; + } else if (anchor == "bottom center") { + anchor_ = BOTTOM_CENTER; + } else if (anchor == "bottom right") { + anchor_ = BOTTOM_RIGHT; + } + } + + void ImagePlugin::SetUnits(QString units) + { + // do this in both cases to avoid image clamping + ui_.width->setMaximum(10000); + ui_.height->setMaximum(10000); + + if (units == "pixels") + { + ui_.width->setDecimals(0); + ui_.height->setDecimals(0); + units_ = PIXELS; + width_ = width_ * static_cast(canvas_->width()) / 100.0; + height_ = height_ * static_cast(canvas_->height()) / 100.0; + ui_.width->setSuffix(" px"); + ui_.height->setSuffix(" px"); + } else if (units == "percent") { + ui_.width->setDecimals(1); + ui_.height->setDecimals(1); + units_ = PERCENT; + width_ = width_ * 100.0 / static_cast(canvas_->width()); + height_ = height_ * 100.0 / static_cast(canvas_->height()); + ui_.width->setSuffix(" %"); + ui_.height->setSuffix(" %"); + } + ui_.width->setValue( width_ ); + ui_.height->setValue( height_ ); + + if( units_ == PERCENT) + { + ui_.width->setMaximum(100); + ui_.height->setMaximum(100); + } + } + void ImagePlugin::SetSubscription(bool visible) + { + if(topic_.empty()) + { + return; + } else if (!visible) { + image_sub_.shutdown(); + RCLCPP_INFO(node_->get_logger(), "Dropped subscription to %s", topic_.c_str()); + } else { + Resubscribe(); + } + } + + void ImagePlugin::SetTransport(const QString& transport) + { + transport_ = transport.toStdString(); + RCLCPP_INFO(node_->get_logger(), "Changing image_transport to %s.", transport_.c_str()); + TopicEdited(); + } + + void ImagePlugin::KeepRatioChanged(bool checked) + { + ui_.height->setEnabled( !checked ); + if( checked ) + { + ui_.height->setValue( width_ * original_aspect_ratio_ ); + } + } + + void ImagePlugin::Resubscribe() + { + if (transport_ == "default") + { + force_resubscribe_ = true; + TopicEdited(); + } + } + + void ImagePlugin::SelectTopic() + { + std::string topic = mapviz::SelectTopicDialog::selectTopic( + node_, "sensor_msgs/msg/Image"); + + if (!topic.empty()) + { + ui_.topic->setText(QString::fromStdString(topic)); + TopicEdited(); + } + } + + void ImagePlugin::TopicEdited() + { + std::string topic = ui_.topic->text().trimmed().toStdString(); + if (!this->Visible()) + { + PrintWarning("Topic is Hidden"); + initialized_ = false; + has_message_ = false; + // Force it to resubscribe next time it's made visible + force_resubscribe_ = true; + if (!topic.empty()) + { + topic_ = topic; + } + image_sub_.shutdown(); + return; + } + // Re-subscribe if either the topic or the image transport + // have changed. + if (force_resubscribe_ || + topic != topic_ || + image_sub_.getTransport() != transport_) + { + force_resubscribe_ = false; + initialized_ = false; + has_message_ = false; + topic_ = topic; + PrintWarning("No messages received."); + + image_sub_.shutdown(); + + if (!topic_.empty()) + { + if (transport_ == "default") + { + RCLCPP_DEBUG(node_->get_logger(), "Using default transport."); + image_transport::ImageTransport it(node_); + image_sub_ = it.subscribe(topic_, 1, + std::bind(&ImagePlugin::imageCallback, this, std::placeholders::_1)); + } else { + RCLCPP_DEBUG(node_->get_logger(), "Setting transport to %s on %s.", + transport_.c_str(), node_->get_fully_qualified_name()); + + image_transport::ImageTransport it(node_); + image_sub_ = image_transport::create_subscription(node_.get(), + topic_, + std::bind(&ImagePlugin::imageCallback, this, std::placeholders::_1), + transport_, + rclcpp::QoS(1).get_rmw_qos_profile()); + } + + RCLCPP_INFO(node_->get_logger(), "Subscribing to %s", topic_.c_str()); + } + } + } + + void ImagePlugin::imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& image) + { + if (!has_message_) + { + initialized_ = true; + has_message_ = true; + } + + try + { + cv_image_ = cv_bridge::toCvCopy(image, sensor_msgs::image_encodings::BGR8); + } + catch (const cv_bridge::Exception& e) + { + PrintError(e.what()); + return; + } + + last_width_ = 0; + last_height_ = 0; + original_aspect_ratio_ = static_cast(image->height) / static_cast(image->width); + + if( ui_.keep_ratio->isChecked() ) + { + double height = width_ * original_aspect_ratio_; + if (units_ == PERCENT) + { + height *= static_cast(canvas_->width()) / static_cast(canvas_->height()); + } + ui_.height->setValue(height); + } + + has_image_ = true; + } + + void ImagePlugin::PrintError(const std::string& message) + { + PrintErrorHelper(ui_.status, message); + } + + void ImagePlugin::PrintInfo(const std::string& message) + { + PrintInfoHelper(ui_.status, message); + } + + void ImagePlugin::PrintWarning(const std::string& message) + { + PrintWarningHelper(ui_.status, message); + } + + QWidget* ImagePlugin::GetConfigWidget(QWidget* parent) + { + config_widget_->setParent(parent); + + return config_widget_; + } + + bool ImagePlugin::Initialize(QGLWidget* canvas) + { + canvas_ = canvas; + + return true; + } + + void ImagePlugin::ScaleImage(double width, double height) + { + if (!has_image_) + { + return; + } + + cv::resize(cv_image_->image, scaled_image_, cvSize2D32f(width, height), 0, 0, CV_INTER_AREA); + } + + void ImagePlugin::DrawIplImage(cv::Mat *image) + { + // TODO(malban) glTexture2D may be more efficient than glDrawPixels + + if (image == nullptr || image->cols == 0 || image->rows == 0) + { + return; + } + + GLenum format; + switch (image->channels()) + { + case 1: + format = GL_LUMINANCE; + break; + case 2: + format = GL_LUMINANCE_ALPHA; + break; + case 3: + format = GL_BGR; + break; + default: + return; + } + + glPixelZoom(1.0f, -1.0f); + glPixelStorei(GL_UNPACK_ALIGNMENT, 1); + glDrawPixels(image->cols, image->rows, format, GL_UNSIGNED_BYTE, image->ptr()); + glPixelStorei(GL_UNPACK_ALIGNMENT, 4); + + + PrintInfo("OK"); + } + + void ImagePlugin::Draw(double x, double y, double scale) + { + // Calculate the correct offsets and dimensions + double x_offset = offset_x_; + double y_offset = offset_y_; + double width = width_; + double height = height_; + + if (units_ == PERCENT) + { + x_offset = offset_x_ * canvas_->width() / 100.0; + y_offset = offset_y_ * canvas_->height() / 100.0; + width = width_ * canvas_->width() / 100.0; + height = height_ * canvas_->height() / 100.0; + } + + if( ui_.keep_ratio->isChecked() ) + { + height = original_aspect_ratio_ * width; + } + + // Scale the source image if necessary + if (width != last_width_ || height != last_height_) + { + ScaleImage(width, height); + } + + // Calculate the correct render position + double x_pos = 0; + double y_pos = 0; + if (anchor_ == TOP_LEFT) + { + x_pos = x_offset; + y_pos = y_offset; + } else if (anchor_ == TOP_CENTER) { + x_pos = (canvas_->width() - width) / 2.0 + x_offset; + y_pos = y_offset; + } else if (anchor_ == TOP_RIGHT) { + x_pos = canvas_->width() - width - x_offset; + y_pos = y_offset; + } else if (anchor_ == CENTER_LEFT) { + x_pos = x_offset; + y_pos = (canvas_->height() - height) / 2.0 + y_offset; + } else if (anchor_ == CENTER) { + x_pos = (canvas_->width() - width) / 2.0 + x_offset; + y_pos = (canvas_->height() - height) / 2.0 + y_offset; + } else if (anchor_ == CENTER_RIGHT) { + x_pos = canvas_->width() - width - x_offset; + y_pos = (canvas_->height() - height) / 2.0 + y_offset; + } else if (anchor_ == BOTTOM_LEFT) { + x_pos = x_offset; + y_pos = canvas_->height() - height - y_offset; + } else if (anchor_ == BOTTOM_CENTER) { + x_pos = (canvas_->width() - width) / 2.0 + x_offset; + y_pos = canvas_->height() - height - y_offset; + } else if (anchor_ == BOTTOM_RIGHT) { + x_pos = canvas_->width() - width - x_offset; + y_pos = canvas_->height() - height - y_offset; + } + + glMatrixMode(GL_PROJECTION); + glPushMatrix(); + glLoadIdentity(); + glOrtho(0, canvas_->width(), canvas_->height(), 0, -0.5f, 0.5f); + + glRasterPos2d(x_pos, y_pos); + + DrawIplImage(&scaled_image_); + + glPopMatrix(); + + last_width_ = width; + last_height_ = height; + } + + void ImagePlugin::LoadConfig(const YAML::Node& node, const std::string& path) + { + // Note that image_transport should be loaded before the + // topic to make sure the transport is set appropriately before we + // subscribe. + if (node["image_transport"]) + { + transport_ = node["image_transport"].as(); + int index = ui_.transport_combo_box->findText( QString::fromStdString(transport_) ); + if (index != -1) + { + ui_.transport_combo_box->setCurrentIndex(index); + } else { + RCLCPP_WARN(node_->get_logger(), "Saved image transport %s is unavailable.", + transport_.c_str()); + } + } + + if (node["topic"]) + { + std::string topic; + topic = node["topic"].as(); + ui_.topic->setText(topic.c_str()); + TopicEdited(); + } + + if (node["anchor"]) + { + std::string anchor; + anchor = node["anchor"].as(); + ui_.anchor->setCurrentIndex(ui_.anchor->findText(anchor.c_str())); + SetAnchor(anchor.c_str()); + } + + if (node["units"]) + { + std::string units; + units = node["units"].as(); + ui_.units->setCurrentIndex(ui_.units->findText(units.c_str())); + SetUnits(units.c_str()); + } + + if (node["offset_x"]) + { + offset_x_ = node["offset_x"].as(); + ui_.offsetx->setValue(offset_x_); + } + + if (node["offset_y"]) + { + offset_y_ = node["offset_y"].as(); + ui_.offsety->setValue(offset_y_); + } + + if (node["width"]) + { + width_ = node["width"].as(); + ui_.width->setValue(width_); + } + + if (node["height"]) + { + height_ = node["height"].as(); + ui_.height->setValue(height_); + } + + if (node["keep_ratio"]) + { + bool keep; + keep = node["keep_ratio"].as(); + ui_.keep_ratio->setChecked( keep ); + } + } + + void ImagePlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path) + { + emitter << YAML::Key << "topic" << YAML::Value << ui_.topic->text().toStdString(); + emitter << YAML::Key << "anchor" << YAML::Value << AnchorToString(anchor_); + emitter << YAML::Key << "units" << YAML::Value << UnitsToString(units_); + emitter << YAML::Key << "offset_x" << YAML::Value << offset_x_; + emitter << YAML::Key << "offset_y" << YAML::Value << offset_y_; + emitter << YAML::Key << "width" << YAML::Value << width_; + emitter << YAML::Key << "height" << YAML::Value << height_; + emitter << YAML::Key << "keep_ratio" << YAML::Value << ui_.keep_ratio->isChecked(); + emitter << YAML::Key << "image_transport" << YAML::Value << transport_; + } + + std::string ImagePlugin::AnchorToString(Anchor anchor) + { + std::string anchor_string = "top left"; + + if (anchor == TOP_LEFT) + { + anchor_string = "top left"; + } else if (anchor == TOP_CENTER) { + anchor_string = "top center"; + } else if (anchor == TOP_RIGHT) { + anchor_string = "top right"; + } else if (anchor == CENTER_LEFT) { + anchor_string = "center left"; + } else if (anchor == CENTER) { + anchor_string = "center"; + } else if (anchor == CENTER_RIGHT) { + anchor_string = "center right"; + } else if (anchor == BOTTOM_LEFT) { + anchor_string = "bottom left"; + } else if (anchor == BOTTOM_CENTER) { + anchor_string = "bottom center"; + } else if (anchor == BOTTOM_RIGHT) { + anchor_string = "bottom right"; + } + + return anchor_string; + } + + std::string ImagePlugin::UnitsToString(Units units) + { + std::string units_string = "pixels"; + + if (units == PIXELS) + { + units_string = "pixels"; + } else if (units == PERCENT) { + units_string = "percent"; + } + + return units_string; + } + + void ImagePlugin::SetNode(rclcpp::Node& node) + { + node_ = node.shared_from_this(); + + // As soon as we have a node, we can find the available image transports + // and add them to our combo box. + image_transport::ImageTransport it(node_); + std::vector transports = it.getLoadableTransports(); + for (const std::string& transport : transports) + { + QString qtransport = QString::fromStdString(transport).replace("image_transport/", ""); + ui_.transport_combo_box->addItem(qtransport); + } + } +} // namespace mapviz_plugins + diff --git a/mapviz_plugins/src/laserscan_plugin.cpp b/mapviz_plugins/src/laserscan_plugin.cpp new file mode 100644 index 000000000..ee67e59a6 --- /dev/null +++ b/mapviz_plugins/src/laserscan_plugin.cpp @@ -0,0 +1,687 @@ +// ***************************************************************************** +// +// Copyright (c) 2015, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include + +// Boost libraries +#include + +// QT libraries +#include +#include + +// ROS libraries +#include +#include + +#include + +// Declare plugin +#include + +// C++ standard libraries +#include +#include +#include +#include + +// QT Autogenerated +#include "ui_topic_select.h" + +PLUGINLIB_EXPORT_CLASS(mapviz_plugins::LaserScanPlugin, mapviz::MapvizPlugin) + +namespace mapviz_plugins +{ + LaserScanPlugin::LaserScanPlugin() + : MapvizPlugin() + , ui_() + , config_widget_(new QWidget()) + , topic_("") + , alpha_(1.0) + , min_value_(0.0) + , max_value_(100.0) + , point_size_(3) + , buffer_size_(0) + , has_message_(false) + , prev_ranges_size_(0) + , prev_angle_min_(0.0) + , prev_increment_(0.0) + { + ui_.setupUi(config_widget_); + + // Set background white + QPalette p(config_widget_->palette()); + p.setColor(QPalette::Window, Qt::white); + config_widget_->setPalette(p); + + // Set status text red + QPalette p3(ui_.status->palette()); + p3.setColor(QPalette::Text, Qt::red); + ui_.status->setPalette(p3); + + // Initialize color selector colors + ui_.min_color->setColor(Qt::white); + ui_.max_color->setColor(Qt::black); + + // Set color transformer choices + ui_.color_transformer->addItem(QString("Flat Color"), QVariant(0)); + ui_.color_transformer->addItem(QString("Intensity"), QVariant(1)); + ui_.color_transformer->addItem(QString("Range"), QVariant(2)); + ui_.color_transformer->addItem(QString("X Axis"), QVariant(3)); + ui_.color_transformer->addItem(QString("Y Axis"), QVariant(4)); + ui_.color_transformer->addItem(QString("Z Axis"), QVariant(5)); + + QObject::connect(ui_.selecttopic, + SIGNAL(clicked()), + this, + SLOT(SelectTopic())); + QObject::connect(ui_.topic, + SIGNAL(editingFinished()), + this, + SLOT(TopicEdited())); + QObject::connect(ui_.alpha, + SIGNAL(valueChanged(double)), + this, + SLOT(AlphaEdited(double))); + QObject::connect(ui_.color_transformer, + SIGNAL(currentIndexChanged(int)), + this, + SLOT(ColorTransformerChanged(int))); + QObject::connect(ui_.max_color, + SIGNAL(colorEdited(const QColor &)), + this, + SLOT(UpdateColors())); + QObject::connect(ui_.min_color, + SIGNAL(colorEdited(const QColor &)), + this, + SLOT(UpdateColors())); + QObject::connect(ui_.minValue, + SIGNAL(valueChanged(double)), + this, + SLOT(MinValueChanged(double))); + QObject::connect(ui_.maxValue, + SIGNAL(valueChanged(double)), + this, + SLOT(MaxValueChanged(double))); + QObject::connect(ui_.bufferSize, + SIGNAL(valueChanged(int)), + this, + SLOT(BufferSizeChanged(int))); + QObject::connect(ui_.pointSize, + SIGNAL(valueChanged(int)), + this, + SLOT(PointSizeChanged(int))); + QObject::connect(ui_.use_rainbow, + SIGNAL(stateChanged(int)), + this, + SLOT(UseRainbowChanged(int))); + + QObject::connect(ui_.max_color, + SIGNAL(colorEdited(const QColor &)), + this, + SLOT(DrawIcon())); + QObject::connect(ui_.min_color, + SIGNAL(colorEdited(const QColor &)), + this, + SLOT(DrawIcon())); + QObject::connect(this, + SIGNAL(TargetFrameChanged(const std::string&)), + this, + SLOT(ResetTransformedScans())); + } + + void LaserScanPlugin::ClearHistory() + { + RCLCPP_DEBUG(node_->get_logger(), "LaserScan::ClearHistory()"); + scans_.clear(); + } + + void LaserScanPlugin::DrawIcon() + { + if (icon_) + { + QPixmap icon(16, 16); + icon.fill(Qt::transparent); + + QPainter painter(&icon); + painter.setRenderHint(QPainter::Antialiasing, true); + + QPen pen; + pen.setWidth(4); + pen.setCapStyle(Qt::RoundCap); + + pen.setColor(ui_.min_color->color()); + painter.setPen(pen); + painter.drawPoint(2, 13); + + pen.setColor(ui_.min_color->color()); + painter.setPen(pen); + painter.drawPoint(4, 6); + + pen.setColor(ui_.max_color->color()); + painter.setPen(pen); + painter.drawPoint(12, 9); + + pen.setColor(ui_.max_color->color()); + painter.setPen(pen); + painter.drawPoint(13, 2); + + icon_->SetPixmap(icon); + } + } + + void LaserScanPlugin::ResetTransformedScans() + { + for (Scan& scan : scans_) + { + scan.transformed = false; + } + } + + QColor LaserScanPlugin::CalculateColor(const StampedPoint& point, + bool has_intensity) + { + double val; + int color_transformer = ui_.color_transformer->currentIndex(); + if (color_transformer == COLOR_RANGE) + { + val = point.range; + } else if (color_transformer == COLOR_INTENSITY && has_intensity) { + val = point.intensity; + } else if (color_transformer == COLOR_X) { + val = point.point.x(); + } else if (color_transformer == COLOR_Y) { + val = point.point.y(); + } else if (color_transformer == COLOR_Z) { + val = point.transformed_point.z(); + } else { + // No intensity or (color_transformer == COLOR_FLAT) + return ui_.min_color->color(); + } + if (max_value_ > min_value_) + val = (val - min_value_) / (max_value_ - min_value_); + val = std::max(0.0, std::min(val, 1.0)); + if (ui_.use_rainbow->isChecked()) + { + // Hue Interpolation + int hue = static_cast(val * 255); + return QColor::fromHsl(hue, 255, 127, 255); + } else { + const QColor min_color = ui_.min_color->color(); + const QColor max_color = ui_.max_color->color(); + // RGB Interpolation + int red, green, blue; + red = static_cast(val * max_color.red() + ((1.0 - val) * min_color.red())); + green = static_cast(val * max_color.green() + ((1.0 - val) * min_color.green())); + blue = static_cast(val * max_color.blue() + ((1.0 - val) * min_color.blue())); + return QColor(red, green, blue, 255); + } + } + + void LaserScanPlugin::UpdateColors() + { + for (auto& scan : scans_) + { + for (auto& point : scan.points) + { + point.color = CalculateColor(point, scan.has_intensity); + } + } + } + + void LaserScanPlugin::SelectTopic() + { + std::string topic = mapviz::SelectTopicDialog::selectTopic( + node_, + "sensor_msgs/msg/LaserScan"); + + if (!topic.empty()) + { + ui_.topic->setText(QString::fromStdString(topic)); + TopicEdited(); + } + } + + void LaserScanPlugin::TopicEdited() + { + std::string topic = ui_.topic->text().trimmed().toStdString(); + if (topic != topic_) + { + initialized_ = false; + scans_.clear(); + has_message_ = false; + PrintWarning("No messages received."); + + laserscan_sub_.reset(); + + topic_ = topic; + if (!topic.empty()) + { + laserscan_sub_ = node_->create_subscription( + topic_, + rclcpp::QoS(100), + std::bind(&LaserScanPlugin::laserScanCallback, this, std::placeholders::_1) + ); + + RCLCPP_INFO(node_->get_logger(), "Subscribing to %s", topic_.c_str()); + } + } + } + + void LaserScanPlugin::MinValueChanged(double value) + { + min_value_ = value; + UpdateColors(); + } + + void LaserScanPlugin::MaxValueChanged(double value) + { + max_value_ = value; + UpdateColors(); + } + + void LaserScanPlugin::BufferSizeChanged(int value) + { + buffer_size_ = static_cast(value); + + if (buffer_size_ > 0) + { + while (scans_.size() > buffer_size_) + { + scans_.pop_front(); + } + } + } + + void LaserScanPlugin::PointSizeChanged(int value) + { + point_size_ = static_cast(value); + } + + void LaserScanPlugin::updatePreComputedTriginometic( + const sensor_msgs::msg::LaserScan::SharedPtr msg) + { + if( msg->ranges.size() != prev_ranges_size_ || + msg->angle_min != prev_angle_min_ || + msg->angle_increment != prev_increment_ ) + { + prev_ranges_size_ = msg->ranges.size(); + prev_angle_min_ = msg->angle_min; + prev_increment_ = msg->angle_increment; + + precomputed_cos_.resize( msg->ranges.size() ); + precomputed_sin_.resize( msg->ranges.size() ); + + for (size_t i = 0; i < msg->ranges.size(); i++) + { + double angle = msg->angle_min + msg->angle_increment * i; + precomputed_cos_[i] = cos(angle); + precomputed_sin_[i] = sin(angle); + } + } + } + + bool LaserScanPlugin::GetScanTransform( + const Scan& scan, + swri_transform_util::Transform& transform) + { + bool was_using_latest_transforms = this->use_latest_transforms_; + // Try first with use_latest_transforms_ = false + this->use_latest_transforms_ = false; + bool has_tranform = GetTransform(scan.source_frame_, scan.stamp, transform); + if (!has_tranform && was_using_latest_transforms) + { + // If failed use_latest_transforms_ = true + this->use_latest_transforms_ = true; + has_tranform = GetTransform(scan.source_frame_, scan.stamp, transform); + } + + this->use_latest_transforms_ = was_using_latest_transforms; + return has_tranform; + } + + void LaserScanPlugin::laserScanCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg) + { + if (!has_message_) + { + initialized_ = true; + has_message_ = true; + } + + // Note that unlike some plugins, this one does not store nor rely on the + // source_frame_ member variable. This one can potentially store many + // messages with different source frames, so we need to store and transform + // them individually. + + Scan scan; + scan.stamp = msg->header.stamp; + scan.color = QColor::fromRgbF(1.0f, 0.0f, 0.0f, 1.0f); + scan.source_frame_ = msg->header.frame_id; + scan.has_intensity = !msg->intensities.empty(); + scan.points.reserve( msg->ranges.size() ); + + double x, y; + updatePreComputedTriginometic(msg); + + swri_transform_util::Transform transform; + scan.transformed = GetScanTransform(scan, transform); + + for (size_t i = 0; i < msg->ranges.size(); i++) + { + // Discard the point if it's out of range + if (msg->ranges[i] > msg->range_max || msg->ranges[i] < msg->range_min) + { + continue; + } + StampedPoint point; + x = precomputed_cos_[i] * msg->ranges[i]; + y = precomputed_sin_[i] * msg->ranges[i]; + point.point = tf2::Vector3(x, y, 0.0f); + point.range = msg->ranges[i]; + if (i < msg->intensities.size()) + point.intensity = msg->intensities[i]; + + if (scan.transformed) + { + point.transformed_point = transform * point.point; + } + point.color = CalculateColor(point, scan.has_intensity); + scan.points.push_back(point); + } + scans_.push_back(scan); + + // If there are more items in the scan buffer than buffer_size_, remove them + if (buffer_size_ > 0) + { + while (scans_.size() > buffer_size_) + { + scans_.pop_front(); + } + } + } + + void LaserScanPlugin::PrintError(const std::string& message) + { + PrintErrorHelper(ui_.status, message); + } + + void LaserScanPlugin::PrintInfo(const std::string& message) + { + PrintInfoHelper(ui_.status, message); + } + + void LaserScanPlugin::PrintWarning(const std::string& message) + { + PrintWarningHelper(ui_.status, message); + } + + QWidget* LaserScanPlugin::GetConfigWidget(QWidget* parent) + { + config_widget_->setParent(parent); + + return config_widget_; + } + + bool LaserScanPlugin::Initialize(QGLWidget* canvas) + { + canvas_ = canvas; + + DrawIcon(); + + return true; + } + + void LaserScanPlugin::Draw(double x, double y, double scale) + { + glPointSize(point_size_); + glBegin(GL_POINTS); + + std::deque::const_iterator scan_it = scans_.begin(); + while (scan_it != scans_.end()) + { + if (scan_it->transformed) + { + for (const auto & point : scan_it->points) + { + glColor4d( + point.color.redF(), + point.color.greenF(), + point.color.blueF(), + alpha_); + glVertex2d( + point.transformed_point.getX(), + point.transformed_point.getY()); + } + } + ++scan_it; + } + + glEnd(); + + PrintInfo("OK"); + } + + void LaserScanPlugin::UseRainbowChanged(int check_state) + { + if (check_state == Qt::Checked) + { + ui_.max_color->setVisible(false); + ui_.min_color->setVisible(false); + ui_.maxColorLabel->setVisible(false); + ui_.minColorLabel->setVisible(false); + } else { + ui_.max_color->setVisible(true); + ui_.min_color->setVisible(true); + ui_.maxColorLabel->setVisible(true); + ui_.minColorLabel->setVisible(true); + } + UpdateColors(); + } + + void LaserScanPlugin::Transform() + { + for (auto & scan : scans_) + { + if (!scan.transformed) + { + swri_transform_util::Transform transform; + + if ( GetScanTransform( scan, transform) ) + { + scan.transformed = true; + + for (auto & point : scan.points) + { + point.transformed_point = transform * point.point; + } + } else { + PrintError("No transform between " + scan.source_frame_ + " and " + target_frame_); + } + } + } + // Z color is based on transformed color, so it is dependent on the + // transform + if (ui_.color_transformer->currentIndex() == COLOR_Z) + { + UpdateColors(); + } + } + + void LaserScanPlugin::LoadConfig(const YAML::Node& node, + const std::string& path) + { + if (node["topic"]) + { + std::string topic = node["topic"].as(); + ui_.topic->setText(boost::trim_copy(topic).c_str()); + TopicEdited(); + } + + if (node["size"]) + { + point_size_ = node["size"].as(); + ui_.pointSize->setValue(static_cast(point_size_)); + } + + if (node["buffer_size"]) + { + buffer_size_ = node["buffer_size"].as(); + ui_.bufferSize->setValue(static_cast(buffer_size_)); + } + + if (node["color_transformer"]) + { + std::string color_transformer = node["color_transformer"].as(); + if (color_transformer == "Intensity") { + ui_.color_transformer->setCurrentIndex(COLOR_INTENSITY); + } else if (color_transformer == "Range") { + ui_.color_transformer->setCurrentIndex(COLOR_RANGE); + } else if (color_transformer == "X Axis") { + ui_.color_transformer->setCurrentIndex(COLOR_X); + } else if (color_transformer == "Y Axis") { + ui_.color_transformer->setCurrentIndex(COLOR_Y); + } else if (color_transformer == "Z Axis") { + ui_.color_transformer->setCurrentIndex(COLOR_Z); + } else { + ui_.color_transformer->setCurrentIndex(COLOR_FLAT); + } + } + + if (node["min_color"]) + { + std::string min_color_str = node["min_color"].as(); + ui_.min_color->setColor(QColor(min_color_str.c_str())); + } + + if (node["max_color"]) + { + std::string max_color_str = node["max_color"].as(); + ui_.max_color->setColor(QColor(max_color_str.c_str())); + } + + if (node["value_min"]) + { + min_value_ = node["value_min"].as(); + ui_.minValue->setValue(min_value_); + } + + if (node["max_value"]) + { + max_value_ = node["value_max"].as(); + ui_.maxValue->setValue(max_value_); + } + + if (node["alpha"]) + { + alpha_ = node["alpha"].as(); + ui_.alpha->setValue(alpha_); + } + + if (node["use_rainbow"]) + { + bool use_rainbow = node["use_rainbow"].as(); + ui_.use_rainbow->setChecked(use_rainbow); + } + + // UseRainbowChanged must be called *before* ColorTransformerChanged + UseRainbowChanged(ui_.use_rainbow->checkState()); + // ColorTransformerChanged will also update colors of all points + ColorTransformerChanged(ui_.color_transformer->currentIndex()); + } + + void LaserScanPlugin::ColorTransformerChanged(int index) + { + RCLCPP_DEBUG(node_->get_logger(), "Color transformer changed to %d", index); + switch (index) + { + case COLOR_FLAT: + ui_.min_color->setVisible(true); + ui_.max_color->setVisible(false); + ui_.maxColorLabel->setVisible(false); + ui_.minColorLabel->setVisible(false); + ui_.minValueLabel->setVisible(false); + ui_.maxValueLabel->setVisible(false); + ui_.minValue->setVisible(false); + ui_.maxValue->setVisible(false); + ui_.use_rainbow->setVisible(false); + break; + case COLOR_INTENSITY: // Intensity + case COLOR_RANGE: // Range + case COLOR_X: // X Axis + case COLOR_Y: // Y Axis + case COLOR_Z: // Z axis + default: + ui_.min_color->setVisible(!ui_.use_rainbow->isChecked()); + ui_.max_color->setVisible(!ui_.use_rainbow->isChecked()); + ui_.maxColorLabel->setVisible(!ui_.use_rainbow->isChecked()); + ui_.minColorLabel->setVisible(!ui_.use_rainbow->isChecked()); + ui_.minValueLabel->setVisible(true); + ui_.maxValueLabel->setVisible(true); + ui_.minValue->setVisible(true); + ui_.maxValue->setVisible(true); + ui_.use_rainbow->setVisible(true); + break; + } + UpdateColors(); + } + + /** + * Coerces alpha to [0.0, 1.0] and stores it in alpha_ + */ + void LaserScanPlugin::AlphaEdited(double val) + { + alpha_ = std::max(0.0f, std::min(static_cast(val), 1.0f)); + } + + void LaserScanPlugin::SaveConfig(YAML::Emitter& emitter, + const std::string& path) + { + emitter << YAML::Key << "topic" << + YAML::Value << boost::trim_copy(ui_.topic->text().toStdString()); + emitter << YAML::Key << "size" << + YAML::Value << ui_.pointSize->value(); + emitter << YAML::Key << "buffer_size" << + YAML::Value << ui_.bufferSize->value(); + emitter << YAML::Key << "alpha" << + YAML::Value << alpha_; + emitter << YAML::Key << "color_transformer" << + YAML::Value << ui_.color_transformer->currentText().toStdString(); + emitter << YAML::Key << "min_color" << + YAML::Value << ui_.min_color->color().name().toStdString(); + emitter << YAML::Key << "max_color" << + YAML::Value << ui_.max_color->color().name().toStdString(); + emitter << YAML::Key << "value_min" << + YAML::Value << ui_.minValue->text().toDouble(); + emitter << YAML::Key << "value_max" << + YAML::Value << ui_.maxValue->text().toDouble(); + emitter << YAML::Key << "use_rainbow" << + YAML::Value << ui_.use_rainbow->isChecked(); + } +} // namespace mapviz_plugins + diff --git a/mapviz_plugins/src/marker_plugin.cpp b/mapviz_plugins/src/marker_plugin.cpp new file mode 100644 index 000000000..03dce4c2e --- /dev/null +++ b/mapviz_plugins/src/marker_plugin.cpp @@ -0,0 +1,725 @@ +// ***************************************************************************** +// +// Copyright (c) 2014-2020, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include + +#include + +#include + +#include + +// Declare plugin +#include + +// C++ Standard Libraries +#include +#include + +PLUGINLIB_EXPORT_CLASS(mapviz_plugins::MarkerPlugin, mapviz::MapvizPlugin) + +namespace mapviz_plugins +{ + MarkerPlugin::MarkerPlugin() + : MapvizPlugin() + , ui_() + , config_widget_(new QWidget()) + , connected_(false) + , has_message_(false) + { + ui_.setupUi(config_widget_); + + // Set background white + QPalette p(config_widget_->palette()); + p.setColor(QPalette::Window, Qt::white); + config_widget_->setPalette(p); + + // Set status text red + QPalette p3(ui_.status->palette()); + p3.setColor(QPalette::Text, Qt::red); + ui_.status->setPalette(p3); + + QObject::connect(ui_.selecttopic, SIGNAL(clicked()), this, SLOT(SelectTopic())); + QObject::connect(ui_.topic, SIGNAL(editingFinished()), this, SLOT(TopicEdited())); + QObject::connect(ui_.clear, SIGNAL(clicked()), this, SLOT(ClearHistory())); + + startTimer(1000); + } + + void MarkerPlugin::ClearHistory() + { + RCLCPP_DEBUG(node_->get_logger(), "MarkerPlugin::ClearHistory()"); + markers_.clear(); + marker_visible_.clear(); + ui_.nsList->clear(); + } + + void MarkerPlugin::SelectTopic() + { + std::string topic = mapviz::SelectTopicDialog::selectTopic( + node_, + "visualization_msgs/msg/Marker", + "visualization_msgs/msg/MarkerArray"); + + if (topic.empty()) + { + return; + } + + ui_.topic->setText(QString::fromStdString(topic)); + TopicEdited(); + } + + void MarkerPlugin::TopicEdited() + { + std::string topic = ui_.topic->text().trimmed().toStdString(); + if (topic != topic_) + { + initialized_ = false; + markers_.clear(); + marker_visible_.clear(); + ui_.nsList->clear(); + has_message_ = false; + PrintWarning("No messages received."); + + marker_sub_.reset(); + marker_array_sub_.reset(); + connected_ = false; + + topic_ = topic; + subscribe(); + } + } + + void MarkerPlugin::subscribe() + { + marker_sub_.reset(); + marker_array_sub_.reset(); + if (!topic_.empty()) + { + // ROS 2 does not allow for a simple way to subscribe to a general type of message (i.e. Marker and MarkerArray) + // That would require a way to de-serialize the data for mapviz to consume (based on message type) + // The code below checks for the topic type and subscribes in the appropriate manner + + auto known_topics = node_->get_topic_names_and_types(); + if (known_topics.count(topic_) > 0) + { + std::string topic_type = known_topics[topic_][0]; + if (topic_type == "visualization_msgs/msg/Marker") + { + marker_sub_ = node_->create_subscription( + topic_, + rclcpp::QoS(100), + std::bind(&MarkerPlugin::handleMarker, this, std::placeholders::_1)); + } + else if (topic_type == "visualization_msgs/msg/MarkerArray") + { + marker_array_sub_ = node_->create_subscription( + topic_, + rclcpp::QoS(100), + std::bind(&MarkerPlugin::handleMarkerArray, this, std::placeholders::_1)); + } + else + { + RCLCPP_ERROR(node_->get_logger(), + "Unable to subscribe to topic %s (unsupported type %s).", + topic_.c_str(), topic_type.c_str()); + return; + } + } + else + { + RCLCPP_ERROR(node_->get_logger(), + "Unable to subscribe to topic %s (does not exist).", topic_.c_str()); + return; + } + + RCLCPP_INFO(node_->get_logger(), "Subscribing to %s", topic_.c_str()); + } + } + + void MarkerPlugin::handleMarker(visualization_msgs::msg::Marker::ConstSharedPtr marker) + { + processMarker(*marker); + } + + void MarkerPlugin::processMarker(const visualization_msgs::msg::Marker& marker) + { + connected_ = true; + if (marker.type == visualization_msgs::msg::Marker::ARROW && + marker.points.size() == 1) + { + // Arrow markers must have either 0 or >1 points; exactly one point is + // invalid. If we get one with 1 point, assume it's corrupt and ignore it. + return; + } + if (!has_message_) + { + initialized_ = true; + has_message_ = true; + } + + // Note that unlike some plugins, this one does not store nor rely on the + // source_frame_ member variable. This one can potentially store many + // messages with different source frames, so we need to store and transform + // them individually. + + if (marker.action == visualization_msgs::msg::Marker::ADD) + { + MarkerData& markerData = markers_[std::make_pair(marker.ns, marker.id)]; + markerData.points.clear(); // clear marker points + markerData.text.clear(); // clear marker text + markerData.stamp = marker.header.stamp; + markerData.display_type = marker.type; + markerData.color = {marker.color.r, marker.color.g, marker.color.b, marker.color.a}; + markerData.scale_x = static_cast(marker.scale.x); + markerData.scale_y = static_cast(marker.scale.y); + markerData.scale_z = static_cast(marker.scale.z); + markerData.transformed = true; + markerData.source_frame = marker.header.frame_id; + + if (marker_visible_.emplace(marker.ns, true).second) + { + QString name_string(marker.ns.c_str()); + auto* item = new QListWidgetItem(name_string, ui_.nsList); + item->setFlags(item->flags() | Qt::ItemIsUserCheckable); + item->setCheckState(Qt::Unchecked); + item->setCheckState(Qt::Checked); + } + + + // Since orientation was not implemented, many markers publish + // invalid all-zero orientations, so we need to check for this + // and provide a default identity transform. + tf2::Quaternion orientation(0.0, 0.0, 0.0, 1.0); + if (marker.pose.orientation.x || + marker.pose.orientation.y || + marker.pose.orientation.z || + marker.pose.orientation.w) + { + orientation = tf2::Quaternion(marker.pose.orientation.x, + marker.pose.orientation.y, + marker.pose.orientation.z, + marker.pose.orientation.w); + } + + markerData.local_transform = swri_transform_util::Transform( + tf2::Transform( + orientation, + tf2::Vector3(marker.pose.position.x, + marker.pose.position.y, + marker.pose.position.z))); + + markerData.points.clear(); + markerData.text = std::string(); + + swri_transform_util::Transform transform; + if (!GetTransform(markerData.source_frame, marker.header.stamp, transform)) + { + markerData.transformed = false; + PrintError("No transform between " + markerData.source_frame + " and " + target_frame_); + } + + // Handle lifetime parameter + rclcpp::Duration lifetime = marker.lifetime; + if (lifetime.nanoseconds() == 0) + { + markerData.expire_time = rclcpp::Time(rclcpp::Time::max().nanoseconds(), + node_->get_clock()->get_clock_type()); + } else { + // Temporarily add 5 seconds to fix some existing markers. + markerData.expire_time = node_->now() + lifetime + rclcpp::Duration(5, 0); + } + + if (markerData.display_type == visualization_msgs::msg::Marker::ARROW) + { + StampedPoint point; + point.color = markerData.color; + point.orientation = orientation; + + if (marker.points.empty()) + { + // If the "points" array is empty, we'll use the pose as the base of + // the arrow and scale its size based on the scale_x value. + point.point = markerData.local_transform * tf2::Vector3(0.0, 0.0, 0.0); + point.arrow_point = markerData.local_transform * tf2::Vector3(1.0, 0.0, 0.0); + } else { + // Otherwise the "points" array should have exactly two values, the + // start and end of the arrow. + point.point = markerData.local_transform * tf2::Vector3( + marker.points[0].x, + marker.points[0].y, + marker.points[0].z); + point.arrow_point = markerData.local_transform * tf2::Vector3( + marker.points[1].x, + marker.points[1].y, + marker.points[1].z); + } + + markerData.points.push_back(point); + + if (!marker.points.empty()) + { + // The point we just pushed back has both the start and end of the + // arrow, so the point we're pushing here is useless; we use it later + // only to indicate whether the original message had two points or not. + markerData.points.emplace_back(StampedPoint()); + } + + transformArrow(markerData, transform); + } else if (markerData.display_type == visualization_msgs::msg::Marker::CYLINDER || + markerData.display_type == visualization_msgs::msg::Marker::SPHERE || + markerData.display_type == visualization_msgs::msg::Marker::TEXT_VIEW_FACING) { + StampedPoint point; + point.point = tf2::Vector3(0.0, 0.0, 0.0); + point.transformed_point = transform * (markerData.local_transform * point.point); + point.color = markerData.color; + markerData.points.push_back(point); + markerData.text = marker.text; + } else if (markerData.display_type == visualization_msgs::msg::Marker::CUBE) { + StampedPoint point; + point.color = markerData.color; + + point.point = tf2::Vector3(marker.scale.x / 2, marker.scale.y / 2, 0.0); + point.transformed_point = transform * (markerData.local_transform * point.point); + markerData.points.push_back(point); + + point.point = tf2::Vector3(-marker.scale.x / 2, marker.scale.y / 2, 0.0); + point.transformed_point = transform * (markerData.local_transform * point.point); + markerData.points.push_back(point); + + point.point = tf2::Vector3(-marker.scale.x / 2, -marker.scale.y / 2, 0.0); + point.transformed_point = transform * (markerData.local_transform * point.point); + markerData.points.push_back(point); + + point.point = tf2::Vector3(marker.scale.x / 2, -marker.scale.y / 2, 0.0); + point.transformed_point = transform * (markerData.local_transform * point.point); + markerData.points.push_back(point); + } else if (markerData.display_type == visualization_msgs::msg::Marker::LINE_STRIP || + markerData.display_type == visualization_msgs::msg::Marker::LINE_LIST || + markerData.display_type == visualization_msgs::msg::Marker::CUBE_LIST || + markerData.display_type == visualization_msgs::msg::Marker::SPHERE_LIST || + markerData.display_type == visualization_msgs::msg::Marker::POINTS || + markerData.display_type == visualization_msgs::msg::Marker::TRIANGLE_LIST) { + markerData.points.reserve(marker.points.size()); + StampedPoint point; + for (unsigned int i = 0; i < marker.points.size(); i++) + { + point.point = tf2::Vector3(marker.points[i].x, marker.points[i].y, marker.points[i].z); + point.transformed_point = transform * (markerData.local_transform * point.point); + + if (i < marker.colors.size()) + { + point.color = { + marker.colors[i].r, + marker.colors[i].g, + marker.colors[i].b, + marker.colors[i].a}; + } else { + point.color = markerData.color; + } + + markerData.points.push_back(point); + } + } else { + RCLCPP_WARN_ONCE( + node_->get_logger(), + "Unsupported marker type: %d", + markerData.display_type); + } + } else if (marker.action == visualization_msgs::msg::Marker::DELETE) { + markers_.erase(std::make_pair(marker.ns, marker.id)); + } else if (marker.action == visualization_msgs::msg::Marker::DELETEALL) { + markers_.clear(); + } + } + + /** + * Given a MarkerData that represents an arrow and a transform, this function + * will generate the points involved in drawing the arrow and then transform + * all of them into the target frame. + * @param[inout] markerData A marker that represents an arrow. + * @param[in] transform The tf that should be applied to the arrow's points. + */ + void MarkerPlugin::transformArrow(MarkerData& markerData, + const swri_transform_util::Transform& transform) + { + // The first point in the markerData.points array always represents the + // base of the arrow. + StampedPoint& point = markerData.points.front(); + tf2::Vector3 arrowOffset; + if (markerData.points.size() == 1) + { + // If the markerData only has a single point, that means its "point" is + // the base of the arrow and the arrow's angle and length are determined + // by the orientation and scale_x. + point.transformed_point = transform * (markerData.local_transform * point.point); + tf2::Transform arrow_tf(tf2::Transform( + transform.GetOrientation()) * point.orientation, + tf2::Vector3(0.0, 0.0, 0.0)); + point.transformed_arrow_point = point.transformed_point + + arrow_tf * point.arrow_point * markerData.scale_x; + arrowOffset = tf2::Vector3(0.25, 0.0, 0.0); + } else { + // If the markerData has two points, that means that the start and end points + // of the arrow were explicitly specified in the original message, so the + // length and angle are determined by them. + point.transformed_point = transform * point.point; + point.transformed_arrow_point = transform * point.arrow_point; + // Also, in this mode, scale_y is the diameter of the arrow's head. + arrowOffset = tf2::Vector3(0.25 * markerData.scale_y, 0.0, 0.0); + } + + tf2::Vector3 pointDiff = point.transformed_arrow_point - point.transformed_point; + double angle = std::atan2(pointDiff.getY(), pointDiff.getX()); + + tf2::Quaternion left_q; + left_q.setRPY(0, 0, M_PI*0.75 + angle); + tf2::Transform left_tf(left_q); + + tf2::Quaternion right_q; + right_q.setRPY(0, 0, -M_PI*0.75 + angle); + tf2::Transform right_tf(right_q); + + point.transformed_arrow_left = point.transformed_arrow_point + left_tf * arrowOffset; + point.transformed_arrow_right = point.transformed_arrow_point + right_tf * arrowOffset; + } + + void MarkerPlugin::handleMarkerArray(visualization_msgs::msg::MarkerArray::ConstSharedPtr markers) + { + for (const auto & marker : markers->markers) + { + processMarker(marker); + } + } + + void MarkerPlugin::PrintError(const std::string& message) + { + PrintErrorHelper(ui_.status, message); + } + + void MarkerPlugin::PrintInfo(const std::string& message) + { + PrintInfoHelper(ui_.status, message); + } + + void MarkerPlugin::PrintWarning(const std::string& message) + { + PrintWarningHelper(ui_.status, message); + } + + QWidget* MarkerPlugin::GetConfigWidget(QWidget* parent) + { + config_widget_->setParent(parent); + + return config_widget_; + } + + bool MarkerPlugin::Initialize(QGLWidget* canvas) + { + canvas_ = canvas; + + return true; + } + + void MarkerPlugin::Draw(double x, double y, double scale) + { + for (size_t i = 0; i < ui_.nsList->count(); i++) + { + if (ui_.nsList->item(i)->checkState() == Qt::Checked) + { + marker_visible_[ui_.nsList->item(i)->text().toStdString()] = true; + } else { + marker_visible_[ui_.nsList->item(i)->text().toStdString()] = false; + } + } + + rclcpp::Time now = node_->now(); + + auto markerIter = markers_.begin(); + while (markerIter != markers_.end()) + { + MarkerData& marker = markerIter->second; + + if (!(marker.expire_time > now)) { + PrintInfo("OK"); + markerIter = markers_.erase(markerIter); + continue; + } + + if (!marker.transformed) { + markerIter++; + continue; + } + + if (!marker_visible_[markerIter->first.first]) + { + markerIter++; + continue; + } + + glColor4f(marker.color.r, marker.color.g, marker.color.b, marker.color.a); + + if (marker.display_type == visualization_msgs::msg::Marker::ARROW) { + if (marker.points.size() == 1) { + // If the marker only has one point, use scale_y as the arrow width. + glLineWidth(marker.scale_y); + } else { + // If the marker has both start and end points explicitly specified, use + // scale_x as the shaft diameter. + glLineWidth(marker.scale_x); + } + glBegin(GL_LINES); + + for (const auto &point : marker.points) { + glColor4f(point.color.r, point.color.g, point.color.b, point.color.a); + + glVertex2d( + point.transformed_point.getX(), + point.transformed_point.getY()); + glVertex2d( + point.transformed_arrow_point.getX(), + point.transformed_arrow_point.getY()); + glVertex2d( + point.transformed_arrow_point.getX(), + point.transformed_arrow_point.getY()); + glVertex2d( + point.transformed_arrow_left.getX(), + point.transformed_arrow_left.getY()); + glVertex2d( + point.transformed_arrow_point.getX(), + point.transformed_arrow_point.getY()); + glVertex2d( + point.transformed_arrow_right.getX(), + point.transformed_arrow_right.getY()); + } + + glEnd(); + } else if (marker.display_type == visualization_msgs::msg::Marker::LINE_STRIP) { + glLineWidth(std::max(1.0f, marker.scale_x)); + glBegin(GL_LINE_STRIP); + + for (const auto &point : marker.points) { + glColor4f(point.color.r, point.color.g, point.color.b, point.color.a); + + glVertex2d( + point.transformed_point.getX(), + point.transformed_point.getY()); + } + + glEnd(); + } else if (marker.display_type == visualization_msgs::msg::Marker::LINE_LIST) { + glLineWidth(std::max(1.0f, marker.scale_x)); + glBegin(GL_LINES); + + for (const auto &point : marker.points) { + glColor4f(point.color.r, point.color.g, point.color.b, point.color.a); + + glVertex2d( + point.transformed_point.getX(), + point.transformed_point.getY()); + } + + glEnd(); + } else if (marker.display_type == visualization_msgs::msg::Marker::POINTS) { + glPointSize(std::max(1.0f, marker.scale_x)); + glBegin(GL_POINTS); + + for (const auto &point : marker.points) { + glColor4f(point.color.r, point.color.g, point.color.b, point.color.a); + + glVertex2d( + point.transformed_point.getX(), + point.transformed_point.getY()); + } + + glEnd(); + } else if (marker.display_type == visualization_msgs::msg::Marker::TRIANGLE_LIST) { + glBegin(GL_TRIANGLES); + + for (const auto &point : marker.points) { + glColor4f(point.color.r, point.color.g, point.color.b, point.color.a); + + glVertex2d( + point.transformed_point.getX(), + point.transformed_point.getY()); + } + + glEnd(); + } else if (marker.display_type == visualization_msgs::msg::Marker::CYLINDER || + marker.display_type == visualization_msgs::msg::Marker::SPHERE || + marker.display_type == visualization_msgs::msg::Marker::SPHERE_LIST) { + for (const auto &point : marker.points) { + glColor4f(point.color.r, point.color.g, point.color.b, point.color.a); + + glBegin(GL_TRIANGLE_FAN); + + + double marker_x = point.transformed_point.getX(); + double marker_y = point.transformed_point.getY(); + + glVertex2d(marker_x, marker_y); + + for (int32_t i = 0; i <= 360; i += 10) { + double radians = + static_cast(i) * static_cast(swri_math_util::_deg_2_rad); + // Spheres may be specified w/ only one scale value + if (marker.scale_y == 0.0) { + marker.scale_y = marker.scale_x; + } + glVertex2d( + marker_x + std::sin(radians) * marker.scale_x, + marker_y + std::cos(radians) * marker.scale_y); + } + + glEnd(); + } + } else if (marker.display_type == visualization_msgs::msg::Marker::CUBE || + marker.display_type == visualization_msgs::msg::Marker::CUBE_LIST) { + glBegin(GL_TRIANGLE_FAN); + for (const auto &point : marker.points) { + glColor4f(point.color.r, point.color.g, point.color.b, point.color.a); + + glVertex2d(point.transformed_point.getX(), point.transformed_point.getY()); + } + glEnd(); + } + + markerIter++; + PrintInfo("OK"); + } + } + + void MarkerPlugin::Paint(QPainter* painter, double x, double y, double scale) + { + // Most of the marker drawing is done using OpenGL commands, but text labels + // are rendered using a QPainter. This is intended primarily as an example + // of how the QPainter works. + rclcpp::Time now = node_->now(); + + // We don't want the text to be rotated or scaled, but we do want it to be + // translated appropriately. So, we save off the current world transform + // and reset it; when we actually draw the text, we'll manually translate + // it to the right place. + QTransform tf = painter->worldTransform(); + QFont font("Helvetica", 10); + painter->setFont(font); + painter->save(); + painter->resetTransform(); + + for (auto & markerIter : markers_) + { + MarkerData& marker = markerIter.second; + + if (marker.display_type != visualization_msgs::msg::Marker::TEXT_VIEW_FACING || + marker.expire_time <= now || + !marker.transformed) + { + continue; + } + + QPen pen(QBrush(QColor::fromRgbF(marker.color.r, marker.color.g, + marker.color.b, marker.color.a)), 1); + painter->setPen(pen); + + StampedPoint& rosPoint = marker.points.front(); + QPointF point = tf.map(QPointF(rosPoint.transformed_point.x(), + rosPoint.transformed_point.y())); + + auto text = QString::fromStdString(marker.text); + // Get bounding rectangle + QRectF rect(point, QSizeF(10, 10)); + rect = painter->boundingRect(rect, Qt::AlignLeft | Qt::AlignHCenter, text); + painter->drawText(rect, text); + + PrintInfo("OK"); + } + + painter->restore(); + } + + void MarkerPlugin::Transform() + { + for (auto & markerIter : markers_) + { + MarkerData& marker = markerIter.second; + + swri_transform_util::Transform transform; + if (GetTransform(marker.source_frame, marker.stamp, transform)) + { + marker.transformed = true; + + if (marker.display_type == visualization_msgs::msg::Marker::ARROW) + { + // Points for the ARROW marker type are stored a bit differently + // than other types, so they have their own special transform case. + transformArrow(marker, transform); + } else { + for (auto &point : marker.points) + { + point.transformed_point = transform * (marker.local_transform * point.point); + } + } + } else { + marker.transformed = false; + } + } + } + + void MarkerPlugin::LoadConfig(const YAML::Node& node, const std::string& path) + { + if (node["topic"]) + { + std::string topic = node["topic"].as(); + ui_.topic->setText(boost::trim_copy(topic).c_str()); + + TopicEdited(); + } + } + + void MarkerPlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path) + { + emitter << YAML::Key + << "topic" + << YAML::Value + << boost::trim_copy(ui_.topic->text().toStdString()); + } + + void MarkerPlugin::timerEvent(QTimerEvent *event) + { + bool new_connected = (marker_sub_ && marker_sub_->get_publisher_count() > 0) || + (marker_array_sub_ && marker_array_sub_->get_publisher_count() > 0); + if (connected_ && !new_connected) + { + subscribe(); + } + connected_ = new_connected; + } +} // namespace mapviz_plugins + diff --git a/mapviz_plugins/src/measuring_plugin.cpp b/mapviz_plugins/src/measuring_plugin.cpp new file mode 100644 index 000000000..5c2b5d023 --- /dev/null +++ b/mapviz_plugins/src/measuring_plugin.cpp @@ -0,0 +1,476 @@ +// ***************************************************************************** +// +// Copyright (c) 2018, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include +#include + +// QT libraries +#include +#include +#include +#include + +#include + +// ROS Libraries +#include + +// Mapviz Libraries +#include + +#include + +// C++ Libraries +#include +#include +#include + +PLUGINLIB_EXPORT_CLASS(mapviz_plugins::MeasuringPlugin, mapviz::MapvizPlugin) + +namespace mapviz_plugins +{ + +MeasuringPlugin::MeasuringPlugin() +: MapvizPlugin() +, ui_() +, config_widget_(new QWidget()) +, map_canvas_(nullptr) +, selected_point_(-1) +, is_mouse_down_(false) +, mouse_down_time_(0) +, max_ms_(Q_INT64_C(500)) +, max_distance_(2.0) +{ + ui_.setupUi(config_widget_); + ui_.main_color->setColor(Qt::black); + ui_.bkgnd_color->setColor(Qt::white); + + QObject::connect(ui_.clear, SIGNAL(clicked()), this, + SLOT(Clear())); + QObject::connect(ui_.show_measurements, SIGNAL(toggled(bool)), this, + SLOT(MeasurementsToggled(bool))); + QObject::connect(ui_.show_bkgnd_color, SIGNAL(toggled(bool)), this, + SLOT(BkgndColorToggled(bool))); + QObject::connect(ui_.font_size, SIGNAL(valueChanged(int)), this, + SLOT(FontSizeChanged(int))); + QObject::connect(ui_.alpha, SIGNAL(valueChanged(double)), this, + SLOT(AlphaChanged(double))); + connect(ui_.main_color, SIGNAL(colorEdited(const QColor &)), this, SLOT(DrawIcon())); + connect(ui_.bkgnd_color, SIGNAL(colorEdited(const QColor &)), this, SLOT(DrawIcon())); + + ui_.measurement->setText(tr("Click on the map. Distance between clicks will appear here")); + ui_.totaldistance->setText( + tr("Click on the map. Total distance between clicks will appear here")); +} + +MeasuringPlugin::~MeasuringPlugin() +{ + if (map_canvas_) + { + map_canvas_->removeEventFilter(this); + } +} + +void MeasuringPlugin::Clear() +{ + vertices_.clear(); + measurements_.clear(); + ui_.measurement->setText(tr("Click on the map. Distance between clicks will appear here")); + ui_.totaldistance->setText( + tr("Click on the map. Total distance between clicks will appear here")); +} + +QWidget* MeasuringPlugin::GetConfigWidget(QWidget* parent) +{ + config_widget_->setParent(parent); + + return config_widget_; +} + +bool MeasuringPlugin::Initialize(QGLWidget* canvas) +{ + map_canvas_ = dynamic_cast(canvas); + map_canvas_->installEventFilter(this); + + initialized_ = true; + PrintInfo("OK"); + + return true; +} + +bool MeasuringPlugin::eventFilter(QObject* object, QEvent* event) +{ + if(!this->Visible()) + { + RCLCPP_DEBUG(node_->get_logger(), "Ignoring mouse event, since measuring plugin is hidden"); + return false; + } + + switch (event->type()) + { + case QEvent::MouseButtonPress: + return handleMousePress(dynamic_cast(event)); + case QEvent::MouseButtonRelease: + return handleMouseRelease(dynamic_cast(event)); + case QEvent::MouseMove: + return handleMouseMove(dynamic_cast(event)); + default: + return false; + } +} + +bool MeasuringPlugin::handleMousePress(QMouseEvent* event) +{ + selected_point_ = -1; + int closest_point = 0; + double closest_distance = std::numeric_limits::max(); + QPointF point = event->localPos(); + RCLCPP_DEBUG(node_->get_logger(), "Map point: %f %f", point.x(), point.y()); + for (size_t i = 0; i < vertices_.size(); i++) + { + tf2::Vector3 vertex = vertices_[i]; + QPointF transformed = map_canvas_->FixedFrameToMapGlCoord(QPointF(vertex.x(), vertex.y())); + + double distance = QLineF(transformed, point).length(); + + if (distance < closest_distance) + { + closest_distance = distance; + closest_point = static_cast(i); + } + } + if (event->button() == Qt::LeftButton) + { + if (closest_distance < 15) + { + selected_point_ = closest_point; + return true; + } else { + is_mouse_down_ = true; + mouse_down_pos_ = event->localPos(); + mouse_down_time_ = QDateTime::currentMSecsSinceEpoch(); + return false; + } + } else if (event->button() == Qt::RightButton) { + if (closest_distance < 15) + { + vertices_.erase(vertices_.begin() + closest_point); + DistanceCalculation(); // function to calculate distance + return true; + } + } + + // Let other plugins process this event too + return false; +} + +bool MeasuringPlugin::handleMouseRelease(QMouseEvent* event) +{ + if (selected_point_ >= 0 && static_cast(selected_point_) < vertices_.size()) + { + QPointF point = event->localPos(); + QPointF transformed = map_canvas_->MapGlCoordToFixedFrame(point); + tf2::Vector3 position(transformed.x(), transformed.y(), 0.0); + vertices_[selected_point_].setX(position.x()); + vertices_[selected_point_].setY(position.y()); + + DistanceCalculation(); + + selected_point_ = -1; + + return true; + } else if (is_mouse_down_) { + qreal distance = QLineF(mouse_down_pos_, event->localPos()).length(); + qint64 msecsDiff = QDateTime::currentMSecsSinceEpoch() - mouse_down_time_; + + // Only fire the event if the mouse has moved less than the maximum distance + // and was held for shorter than the maximum time.. This prevents click + // events from being fired if the user is dragging the mouse across the map + // or just holding the cursor in place. + if (msecsDiff < max_ms_ && distance <= max_distance_) + { + QPointF point = event->localPos(); + + QPointF transformed = map_canvas_->MapGlCoordToFixedFrame(point); + tf2::Vector3 position(transformed.x(), transformed.y(), 0.0); + vertices_.push_back(position); + DistanceCalculation(); // call to calculate distance + } + } + is_mouse_down_ = false; + // Let other plugins process this event too + return false; +} + +void MeasuringPlugin::DistanceCalculation() +{ + double distance_instant = -1; // measurement between last two points + double distance_sum = 0; // sum of distance from all points + tf2::Vector3 last_position_(0, 0, 0); + std::string frame = target_frame_; + measurements_.clear(); + for (auto vertex : vertices_) + { + if (last_position_ != tf2::Vector3(0, 0, 0)) + { + distance_instant = last_position_.distance(vertex); + distance_sum = distance_sum + distance_instant; + measurements_.push_back(distance_instant); + } + last_position_ = vertex; + } + measurements_.push_back(distance_sum); + + QString new_point; + QTextStream stream(&new_point); + stream.setRealNumberPrecision(4); + + if (distance_instant > 0.0) + { + stream << distance_instant << " meters"; + } + + ui_.measurement->setText(new_point); + + QString new_point2; + QTextStream stream2(&new_point2); + stream2.setRealNumberPrecision(4); + + if (distance_sum > 0.0) + { + stream2 << distance_sum << " meters"; + } + + ui_.totaldistance->setText(new_point2); +} + +bool MeasuringPlugin::handleMouseMove(QMouseEvent* event) +{ + if (selected_point_ >= 0 && static_cast(selected_point_) < vertices_.size()) + { + QPointF point = event->localPos(); + std::string frame = target_frame_; + QPointF transformed = map_canvas_->MapGlCoordToFixedFrame(point); + tf2::Vector3 position(transformed.x(), transformed.y(), 0.0); + vertices_[selected_point_].setY(position.y()); + vertices_[selected_point_].setX(position.x()); + DistanceCalculation(); //function to calculate distance + return true; + } // Let other plugins process this event too + return false; +} + +void MeasuringPlugin::Draw(double x, double y, double scale) +{ + glLineWidth(1); + const QColor color = ui_.main_color->color(); + glColor4d(color.redF(), color.greenF(), color.blueF(), ui_.alpha->value()/2.0); + glBegin(GL_LINE_STRIP); + + for (const auto& vertex : vertices_) + { + glVertex2d(vertex.x(), vertex.y()); + } + + glEnd(); + + glBegin(GL_LINES); + + glColor4d(color.redF(), color.greenF(), color.blueF(), ui_.alpha->value()/2.0); + + glEnd(); + + // Draw vertices + glPointSize(9); + glBegin(GL_POINTS); + + for (const auto& vertex : vertices_) + { + glVertex2d(vertex.x(), vertex.y()); + } + glEnd(); + + PrintInfo("OK"); +} + +void MeasuringPlugin::Paint(QPainter* painter, double x, double y, double scale) +{ + bool show_measurements = ui_.show_measurements->isChecked(); + if (!show_measurements || vertices_.empty()) + { + return; + } + + QTransform tf = painter->worldTransform(); + QFont font("Helvetica", ui_.font_size->value()); + painter->setFont(font); + painter->save(); + painter->resetTransform(); + + // set the draw color for the text to be the same as the rest + QColor color = ui_.main_color->color(); + double alpha = ui_.alpha->value()*2.0 < 1.0 ? ui_.alpha->value()*2.0 : 1.0; + color.setAlphaF(alpha); + QPen pen(QBrush(color), 1); + painter->setPen(pen); + + const QRectF qrect = QRectF(0, 0, 0, 0); + MeasurementBox mb; + std::vector tags; + + // (midpoint positioned) measurements + for (int i = 0; i < vertices_.size()-1; i++) + { + tf2::Vector3 v1 = vertices_[i]; + tf2::Vector3 v2 = vertices_[i+1]; + + mb.string.setNum(measurements_[i], 'g', 5); + mb.string.prepend(" "); + mb.string.append(" m "); + // drawText used here to get correct mb.rect size + painter->drawText(qrect, 0, mb.string, &mb.rect); + mb.rect.moveTopLeft(tf.map(QPointF((v1.x()+v2.x())/2, (v1.y()+v2.y())/2))); + tags.push_back(mb); + } + // (endpoint positioned) total dist + mb.string.setNum(measurements_.back(), 'g', 5); + mb.string.prepend(" Total: "); + mb.string.append(" m "); + painter->drawText(qrect, 0, mb.string, &mb.rect); + mb.rect.moveTopLeft(tf.map(QPointF(vertices_.back().x(), vertices_.back().y()))); + tags.push_back(mb); + + // prevent text overlapping + for (int i = 0; i < tags.size(); i++) + { + for (int j = 0; j < tags.size(); j++) + { + if (i != j && tags[i].rect.intersects(tags[j].rect)) + { + QRectF overlap = tags[i].rect.intersected(tags[j].rect); + if (tags[i].rect.y() > tags[j].rect.y()) + { + tags[i].rect.moveTop(tags[i].rect.y() + overlap.height()); + } else { + tags[i].rect.moveTop(tags[i].rect.y() - overlap.height()); + } + } + } + } + + // paint tags + for (const auto& tag : tags) + { + if (ui_.show_bkgnd_color->isChecked()) + { + color = ui_.bkgnd_color->color(); + color.setAlphaF(ui_.alpha->value()); + painter->fillRect(tag.rect, color); + painter->drawRect(tag.rect); + } + painter->drawText(tag.rect, tag.string); + } + painter->restore(); +} + +void MeasuringPlugin::LoadConfig(const YAML::Node& node, const std::string& path) +{ + if (node["main_color"]) + { + std::string color = node["main_color"].as(); + ui_.main_color->setColor(QColor(color.c_str())); + } + + if (node["bkgnd_color"]) + { + std::string color = node["bkgnd_color"].as(); + ui_.bkgnd_color->setColor(QColor(color.c_str())); + } + + if (node["show_bkgnd_color"]) + { + bool show_bkgnd_color = node["show_bkgnd_color"].as(); + ui_.show_bkgnd_color->setChecked(show_bkgnd_color); + BkgndColorToggled(show_bkgnd_color); + } + + if (node["show_measurements"]) + { + bool show_measurements = node["show_measurements"].as(); + ui_.show_measurements->setChecked(show_measurements); + MeasurementsToggled(show_measurements); + } + + if (node["font_size"]) + { + int font_size = node["font_size"].as(); + ui_.font_size->setValue(font_size); + FontSizeChanged(font_size); + } + + if (node["alpha"]) + { + double alpha = node["alpha"].as(); + ui_.alpha->setValue(alpha); + AlphaChanged(alpha); + } +} + +void MeasuringPlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path) +{ + emitter << YAML::Key + << "main_color" + << YAML::Value + << ui_.main_color->color().name().toStdString(); + emitter << YAML::Key + << "bkgnd_color" + << YAML::Value + << ui_.bkgnd_color->color().name().toStdString(); + emitter << YAML::Key << "show_bkgnd_color" << YAML::Value << ui_.show_bkgnd_color->isChecked(); + emitter << YAML::Key << "show_measurements" << YAML::Value << ui_.show_measurements->isChecked(); + emitter << YAML::Key << "font_size" << YAML::Value << ui_.font_size->value(); + emitter << YAML::Key << "alpha" << YAML::Value << ui_.alpha->value(); +} + +void MeasuringPlugin::PrintError(const std::string& message) +{ + PrintErrorHelper(ui_.status, message, 1.0); +} + +void MeasuringPlugin::PrintInfo(const std::string& message) +{ + PrintInfoHelper(ui_.status, message, 1.0); +} + +void MeasuringPlugin::PrintWarning(const std::string& message) +{ + PrintWarningHelper(ui_.status, message, 1.0); +} + +} // namespace mapviz_plugins diff --git a/mapviz_plugins/src/move_base_plugin.cpp b/mapviz_plugins/src/move_base_plugin.cpp new file mode 100644 index 000000000..ca5de47cf --- /dev/null +++ b/mapviz_plugins/src/move_base_plugin.cpp @@ -0,0 +1,380 @@ +// ***************************************************************************** +// +// Copyright (c) 2017, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include + +// C++ standard libraries +#include +#include + +// QT libraries +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// ROS libraries +#include +#include + +// Declare plugin +#include + +PLUGINLIB_EXPORT_CLASS(mapviz_plugins::MoveBasePlugin, mapviz::MapvizPlugin) + +namespace mapviz_plugins +{ + +MoveBasePlugin::MoveBasePlugin() : + config_widget_(new QWidget()), + map_canvas_(NULL), + is_mouse_down_(false), + move_base_client_("move_base", true), + monitoring_action_state_(false) +{ + init_pose_pub_ = nh_.advertise("/initialpose", 1, false); + + ui_.setupUi(config_widget_); + + // Set background white + QPalette p(config_widget_->palette()); + p.setColor(QPalette::Background, Qt::white); + config_widget_->setPalette(p); + // Set status text red + + ui_.status->setText("OK"); + QPalette p3(ui_.status->palette()); + p3.setColor(QPalette::Text, Qt::green); + ui_.status->setPalette(p3); + + QObject::connect(ui_.pushButtonInitialPose, SIGNAL(&QPushButton::toggled), + this, SLOT(&MoveBasePlugin::on_pushButtonInitialPose_toggled)); + + QObject::connect(ui_.pushButtonGoalPose, SIGNAL(&QPushButton::toggled), + this, SLOT(&MoveBasePlugin::on_pushButtonGoalPose_toggled)); + + QObject::connect(ui_.pushButtonAbort, SIGNAL(&QPushButton::clicked), + this, SLOT(&MoveBasePlugin::on_pushButtonAbort_clicked)); + + timer_ = nh_.createTimer(ros::Duration(1.0), &MoveBasePlugin::timerCallback, this); + +} + +MoveBasePlugin::~MoveBasePlugin() +{ + if (map_canvas_) + { + map_canvas_->removeEventFilter(this); + } +} + +void MoveBasePlugin::PrintError(const std::string& message) +{ + PrintErrorHelper( ui_.status, message); +} + +void MoveBasePlugin::PrintInfo(const std::string& message) +{ + PrintInfoHelper( ui_.status, message); +} + +void MoveBasePlugin::PrintWarning(const std::string& message) +{ + PrintWarningHelper( ui_.status, message); +} + +QWidget* MoveBasePlugin::GetConfigWidget(QWidget* parent) +{ + config_widget_->setParent(parent); + return config_widget_; +} + +bool MoveBasePlugin::Initialize(QGLWidget* canvas) +{ + map_canvas_ = static_cast(canvas); + map_canvas_->installEventFilter(this); + initialized_ = true; + return true; +} + +bool MoveBasePlugin::eventFilter(QObject *object, QEvent* event) +{ + switch (event->type()) + { + case QEvent::MouseButtonPress: + return handleMousePress(static_cast(event)); + case QEvent::MouseButtonRelease: + return handleMouseRelease(static_cast(event)); + case QEvent::MouseMove: + return handleMouseMove(static_cast(event)); + default: + return false; + } +} + +void MoveBasePlugin::timerCallback(const ros::TimerEvent &) +{ + bool connected = move_base_client_.isServerConnected(); + ui_.pushButtonAbort->setEnabled( connected ); + ui_.pushButtonGoalPose->setEnabled( connected ); + ui_.pushButtonInitialPose->setEnabled( connected ); + + if(!connected) + { + PrintErrorHelper( ui_.status, "[move_base] server not connected"); + } + else if( !monitoring_action_state_ ){ + PrintInfoHelper( ui_.status, "Ready to send command"); + } + else{ + actionlib::SimpleClientGoalState state = move_base_client_.getState(); + switch( state.state_ ) + { + case actionlib::SimpleClientGoalState::PENDING: + PrintWarningHelper( ui_.status, state.toString() ); + break; + + case actionlib::SimpleClientGoalState::PREEMPTED: + PrintWarningHelper( ui_.status, state.toString() ); + monitoring_action_state_ = false; + break; + + case actionlib::SimpleClientGoalState::ACTIVE: + PrintInfoHelper( ui_.status, state.toString() ); + break; + + case actionlib::SimpleClientGoalState::SUCCEEDED: + PrintInfoHelper( ui_.status, state.toString() ); + monitoring_action_state_ = false; + break; + + case actionlib::SimpleClientGoalState::REJECTED: + case actionlib::SimpleClientGoalState::ABORTED: + case actionlib::SimpleClientGoalState::LOST: + case actionlib::SimpleClientGoalState::RECALLED: + PrintErrorHelper( ui_.status, state.toString() ); + monitoring_action_state_ = false; + break; + } + } +} + + +bool MoveBasePlugin::handleMousePress(QMouseEvent* event) +{ + bool init_checked = ui_.pushButtonInitialPose->isChecked(); + bool goal_checked = ui_.pushButtonGoalPose->isChecked(); + if( !init_checked && !goal_checked) + { + return false; + } + + if (event->button() == Qt::LeftButton) + { + is_mouse_down_ = true; + arrow_angle_ = 0; +#if QT_VERSION >= 0x050000 + arrow_tail_position_= map_canvas_->MapGlCoordToFixedFrame( event->localPos() ); +#else + arrow_tail_position_= map_canvas_->MapGlCoordToFixedFrame( event->posF() ); +#endif + return true; + } + return false; +} + +bool MoveBasePlugin::handleMouseMove(QMouseEvent* event) +{ + if (is_mouse_down_) + { +#if QT_VERSION >= 0x050000 + QPointF head_pos = map_canvas_->MapGlCoordToFixedFrame( event->localPos() ); +#else + QPointF head_pos = map_canvas_->MapGlCoordToFixedFrame( event->posF() ); +#endif + arrow_angle_ = atan2( head_pos.y() - arrow_tail_position_.y(), + head_pos.x() - arrow_tail_position_.x() ); + } + return false; +} + +bool MoveBasePlugin::handleMouseRelease(QMouseEvent* event) +{ + if( !is_mouse_down_ ) + { + return false; + } + + is_mouse_down_ = false; + + bool init_checked = ui_.pushButtonInitialPose->isChecked(); + bool goal_checked = ui_.pushButtonGoalPose->isChecked(); + if( !init_checked && !goal_checked) + { + return false; + } + + tf::Quaternion quat = tf::createQuaternionFromYaw(arrow_angle_); + + if( goal_checked ){ + + move_base_msg_.action_goal.header.frame_id = target_frame_; + move_base_msg_.action_goal.header.stamp = ros::Time::now(); + move_base_msg_.action_goal.goal_id.stamp = move_base_msg_.action_goal.header.stamp; + move_base_msg_.action_goal.goal_id.id = "mapviz_goal"; + move_base_msg_.action_goal.goal.target_pose.header = move_base_msg_.action_goal.header; + + geometry_msgs::Pose& pose = move_base_msg_.action_goal.goal.target_pose.pose; + pose.position.x = arrow_tail_position_.x(); + pose.position.y = arrow_tail_position_.y(); + pose.position.z = 0.0; + tf::quaternionTFToMsg( quat, pose.orientation ); + + move_base_client_.sendGoal(move_base_msg_.action_goal.goal); + ui_.pushButtonGoalPose->setChecked(false); + monitoring_action_state_ = true; + } + if( init_checked ){ + geometry_msgs::PoseWithCovarianceStamped initpose; + initpose.header.frame_id = target_frame_; + initpose.header.stamp = ros::Time::now(); + initpose.pose.pose.position.x = arrow_tail_position_.x(); + initpose.pose.pose.position.y = arrow_tail_position_.y(); + initpose.pose.pose.position.z = 0.0; + tf::quaternionTFToMsg( quat, initpose.pose.pose.orientation ); + + init_pose_pub_.publish(initpose); + ui_.pushButtonInitialPose->setChecked(false); + } + return true; +} + + +void MoveBasePlugin::Draw(double x, double y, double scale) +{ + std::array arrow_points; + arrow_points[0] = QPointF(10, 0); + arrow_points[1] = QPointF(6, -2.5); + arrow_points[2] = QPointF(6.5, -1); + arrow_points[3] = QPointF(0, -1); + arrow_points[4] = QPointF(0, 1); + arrow_points[5] = QPointF(6.5, 1); + arrow_points[6] = QPointF(6, 2.5); + + if( is_mouse_down_ ) + { + QPointF transformed_points[7]; + for (size_t i=0; i<7; i++ ) + { + tf::Vector3 point(arrow_points[i].x(), arrow_points[i].y(), 0); + point *= scale*10; + point = tf::Transform( tf::createQuaternionFromYaw(arrow_angle_) ) * point; + transformed_points[i] = QPointF(point.x() + arrow_tail_position_.x(), + point.y() + arrow_tail_position_.y() ); + } + glColor3f(0.1, 0.9, 0.1); + glLineWidth(2); + glBegin(GL_TRIANGLE_FAN); + for (const QPointF& point: transformed_points ) + { + glVertex2d(point.x(), point.y()); + } + glEnd(); + + glColor3f(0.0, 0.6, 0.0); + glBegin(GL_LINE_LOOP); + for (const QPointF& point: transformed_points ) + { + glVertex2d(point.x(), point.y()); + } + glEnd(); + } +} + + +void MoveBasePlugin::LoadConfig(const YAML::Node& node, const std::string& path) +{ + +} + +void MoveBasePlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path) +{ + +} + +void MoveBasePlugin::on_pushButtonInitialPose_toggled(bool checked) +{ + const bool other_checked = ui_.pushButtonGoalPose->isChecked(); + + if(checked){ + if(other_checked){ + ui_.pushButtonGoalPose->setChecked(false); + } + else{ + QPixmap cursor_pixmap = QPixmap(":/images/green-arrow.png"); + QApplication::setOverrideCursor(QCursor(cursor_pixmap)); + } + } + if( !checked && !other_checked ) + { + QApplication::restoreOverrideCursor(); + } +} + +void MoveBasePlugin::on_pushButtonGoalPose_toggled(bool checked) +{ + const bool other_checked = ui_.pushButtonInitialPose->isChecked(); + if(checked){ + if( other_checked){ + ui_.pushButtonInitialPose->setChecked(false); + } + else{ + QPixmap cursor_pixmap = QPixmap(":/images/green-arrow.png"); + QApplication::setOverrideCursor(QCursor(cursor_pixmap)); + } + } + if( !checked && !other_checked ) + { + QApplication::restoreOverrideCursor(); + } + +} + +void MoveBasePlugin::on_pushButtonAbort_clicked() +{ + move_base_client_.cancelGoal(); +} + +} diff --git a/mapviz_plugins/src/navsat_plugin.cpp b/mapviz_plugins/src/navsat_plugin.cpp new file mode 100644 index 000000000..22653ea43 --- /dev/null +++ b/mapviz_plugins/src/navsat_plugin.cpp @@ -0,0 +1,246 @@ +// ***************************************************************************** +// +// Copyright (C) 2013 All Right Reserved, Southwest Research Institute® (SwRI®) +// +// Contract No. 10-58058A +// Contractor Southwest Research Institute® (SwRI®) +// Address 6220 Culebra Road, San Antonio, Texas 78228-0510 +// Contact Steve Dellenback (210) 522-3914 +// +// This code was developed as part of an internal research project fully funded +// by Southwest Research Institute®. +// +// THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY +// KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A +// PARTICULAR PURPOSE. +// +// ***************************************************************************** + +#include + +// QT libraries +#include +#include +#include + +#include + +// ROS libraries +#include + +#include + +// Declare plugin +#include + +// C++ standard libraries +#include +#include +#include +#include + +PLUGINLIB_EXPORT_CLASS(mapviz_plugins::NavSatPlugin, mapviz::MapvizPlugin) + +namespace mapviz_plugins +{ + NavSatPlugin::NavSatPlugin() + : PointDrawingPlugin() + , ui_() + , config_widget_(new QWidget()) + , has_message_(false) + { + ui_.setupUi(config_widget_); + + ui_.color->setColor(Qt::green); + + // Set background white + QPalette p(config_widget_->palette()); + p.setColor(QPalette::Window, Qt::white); + config_widget_->setPalette(p); + + // Set status text red + QPalette p3(ui_.status->palette()); + p3.setColor(QPalette::Text, Qt::red); + ui_.status->setPalette(p3); + + QObject::connect(ui_.selecttopic, SIGNAL(clicked()), this, + SLOT(SelectTopic())); + QObject::connect(ui_.topic, SIGNAL(editingFinished()), this, + SLOT(TopicEdited())); + QObject::connect(ui_.positiontolerance, SIGNAL(valueChanged(double)), this, + SLOT(PositionToleranceChanged(double))); + QObject::connect(ui_.buffersize, SIGNAL(valueChanged(int)), this, + SLOT(BufferSizeChanged(int))); + QObject::connect(ui_.drawstyle, SIGNAL(activated(QString)), this, + SLOT(SetDrawStyle(QString))); + QObject::connect(ui_.color, SIGNAL(colorEdited(const QColor&)), this, + SLOT(SetColor(const QColor&))); + QObject::connect(ui_.buttonResetBuffer, SIGNAL(pressed()), this, + SLOT(ClearPoints())); + } + + void NavSatPlugin::SelectTopic() + { + std::string topic = + mapviz::SelectTopicDialog::selectTopic(node_, "sensor_msgs/msg/NavSatFix"); + + if (!topic.empty()) + { + ui_.topic->setText(QString::fromStdString(topic)); + TopicEdited(); + } + } + + void NavSatPlugin::TopicEdited() + { + std::string topic = ui_.topic->text().trimmed().toStdString(); + if (topic != topic_) + { + initialized_ = false; + ClearPoints(); + has_message_ = false; + PrintWarning("No messages received."); + + navsat_sub_.reset(); + topic_ = topic; + if (!topic.empty()) + { + navsat_sub_ = node_->create_subscription( + topic_, + rclcpp::QoS(1), + std::bind(&NavSatPlugin::NavSatFixCallback, this, std::placeholders::_1)); + + RCLCPP_INFO(node_->get_logger(), "Subscribing to %s", topic_.c_str()); + } + } + } + + void NavSatPlugin::NavSatFixCallback( + const sensor_msgs::msg::NavSatFix::ConstSharedPtr navsat) + { + if (!tf_manager_->LocalXyUtil()->Initialized()) + { + return; + } + if (!has_message_) + { + initialized_ = true; + has_message_ = true; + } + + StampedPoint stamped_point; + stamped_point.stamp = navsat->header.stamp; + + double x; + double y; + tf_manager_->LocalXyUtil()->ToLocalXy(navsat->latitude, navsat->longitude, x, y); + + stamped_point.point = tf2::Vector3(x, y, navsat->altitude); + stamped_point.orientation.setRPY(0, 0, 0); + stamped_point.source_frame = tf_manager_->LocalXyUtil()->Frame(); + + pushPoint( std::move(stamped_point) ); + } + + void NavSatPlugin::PrintError(const std::string& message) + { + PrintErrorHelper(ui_.status, message); + } + + void NavSatPlugin::PrintInfo(const std::string& message) + { + PrintInfoHelper(ui_.status, message); + } + + void NavSatPlugin::PrintWarning(const std::string& message) + { + PrintWarningHelper(ui_.status, message); + } + + QWidget* NavSatPlugin::GetConfigWidget(QWidget* parent) + { + config_widget_->setParent(parent); + + return config_widget_; + } + + bool NavSatPlugin::Initialize(QGLWidget* canvas) + { + canvas_ = canvas; + SetColor(ui_.color->color()); + return true; + } + + void NavSatPlugin::Draw(double x, double y, double scale) + { + if (DrawPoints(scale)) + { + PrintInfo("OK"); + } + } + + void NavSatPlugin::LoadConfig(const YAML::Node& node, const std::string& path) + { + if (node["topic"]) + { + std::string topic = node["topic"].as(); + ui_.topic->setText(topic.c_str()); + } + + if (node["color"]) + { + std::string color = node["color"].as(); + QColor qcolor(color.c_str()); + SetColor(qcolor); + ui_.color->setColor(qcolor); + } + + if (node["draw_style"]) + { + std::string draw_style = node["draw_style"].as(); + + if (draw_style == "lines") + { + ui_.drawstyle->setCurrentIndex(0); + SetDrawStyle( LINES ); + } else if (draw_style == "points") { + ui_.drawstyle->setCurrentIndex(1); + SetDrawStyle( POINTS ); + } + } + + if (node["position_tolerance"]) + { + auto position_tolerance = node["position_tolerance"].as(); + ui_.positiontolerance->setValue(position_tolerance); + PositionToleranceChanged(position_tolerance); + } + + if (node["buffer_size"]) + { + auto buffer_size = node["buffer_size"].as(); + ui_.buffersize->setValue(buffer_size); + BufferSizeChanged(buffer_size); + } + + TopicEdited(); + } + + void NavSatPlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path) + { + std::string topic = ui_.topic->text().toStdString(); + emitter << YAML::Key << "topic" << YAML::Value << topic; + + std::string color = ui_.color->color().name().toStdString(); + emitter << YAML::Key << "color" << YAML::Value << color; + + std::string draw_style = ui_.drawstyle->currentText().toStdString(); + emitter << YAML::Key << "draw_style" << YAML::Value << draw_style; + + emitter << YAML::Key << "position_tolerance" << + YAML::Value << positionTolerance(); + + emitter << YAML::Key << "buffer_size" << YAML::Value << bufferSize(); + } +} // namespace mapviz_plugins diff --git a/mapviz_plugins/src/occupancy_grid_plugin.cpp b/mapviz_plugins/src/occupancy_grid_plugin.cpp new file mode 100644 index 000000000..72d1be4a5 --- /dev/null +++ b/mapviz_plugins/src/occupancy_grid_plugin.cpp @@ -0,0 +1,565 @@ +// ***************************************************************************** +// +// Copyright (c) 2018, Eurecat +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Eurecat nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include + +#include + +// QT libraries +#include +#include + +#include + +// Declare plugin +#include + +// C++ standard libraries +#include +#include +#include + +PLUGINLIB_EXPORT_CLASS(mapviz_plugins::OccupancyGridPlugin, mapviz::MapvizPlugin) + +namespace mapviz_plugins +{ + const int CHANNELS = 4; + + typedef std::array Palette; + + Palette makeMapPalette() + { + Palette palette; + uchar* palette_ptr = palette.data(); + // Standard gray map palette values + for( int i = 0; i <= 100; i++ ) + { + uchar v = 255 - (255 * i) / 100; + *palette_ptr++ = v; // red + *palette_ptr++ = v; // green + *palette_ptr++ = v; // blue + *palette_ptr++ = 255; // alpha + } + // illegal positive values in green + for( int i = 101; i <= 127; i++ ) + { + *palette_ptr++ = 0; // red + *palette_ptr++ = 255; // green + *palette_ptr++ = 0; // blue + *palette_ptr++ = 255; // alpha + } + // illegal negative (char) values in shades of red/yellow + for( int i = 128; i <= 254; i++ ) + { + *palette_ptr++ = 255; // red + *palette_ptr++ = (255*(i-128))/(254-128); // green + *palette_ptr++ = 0; // blue + *palette_ptr++ = 255; // alpha + } + // legal -1 value is tasteful blueish greenish grayish color + *palette_ptr++ = 0x70; // red + *palette_ptr++ = 0x89; // green + *palette_ptr++ = 0x86; // blue + *palette_ptr++ = 160; // alpha + + return palette; + } + + Palette makeCostmapPalette() + { + Palette palette; + uchar* palette_ptr = palette.data(); + + // zero values have alpha=0 + *palette_ptr++ = 0; // red + *palette_ptr++ = 0; // green + *palette_ptr++ = 0; // blue + *palette_ptr++ = 0; // alpha + + // Blue to red spectrum for most normal cost values + for( int i = 1; i <= 98; i++ ) + { + uchar v = (255 * i) / 100; + *palette_ptr++ = v; // red + *palette_ptr++ = 0; // green + *palette_ptr++ = 255 - v; // blue + *palette_ptr++ = 255; // alpha + } + // inscribed obstacle values (99) in cyan + *palette_ptr++ = 0; // red + *palette_ptr++ = 255; // green + *palette_ptr++ = 255; // blue + *palette_ptr++ = 255; // alpha + // lethal obstacle values (100) in purple + *palette_ptr++ = 255; // red + *palette_ptr++ = 0; // green + *palette_ptr++ = 255; // blue + *palette_ptr++ = 255; // alpha + // illegal positive values in green + for( int i = 101; i <= 127; i++ ) + { + *palette_ptr++ = 0; // red + *palette_ptr++ = 255; // green + *palette_ptr++ = 0; // blue + *palette_ptr++ = 255; // alpha + } + // illegal negative (char) values in shades of red/yellow + for( int i = 128; i <= 254; i++ ) + { + *palette_ptr++ = 255; // red + *palette_ptr++ = (255*(i-128))/(254-128); // green + *palette_ptr++ = 0; // blue + *palette_ptr++ = 255; // alpha + } + // legal -1 value is tasteful blueish greenish grayish color + *palette_ptr++ = 0x70; // red + *palette_ptr++ = 0x89; // green + *palette_ptr++ = 0x86; // blue + *palette_ptr++ = 160; // alpha + + return palette; + } + + OccupancyGridPlugin::OccupancyGridPlugin() + : MapvizPlugin() + , ui_() + , config_widget_(new QWidget()) + , transformed_(false) + , texture_id_(0) + , texture_x_(0.0) + , texture_y_(0.0) + , texture_size_(0) + , map_palette_( makeMapPalette() ) + , costmap_palette_( makeCostmapPalette() ) + { + ui_.setupUi(config_widget_); + + // Set background white + QPalette p(config_widget_->palette()); + p.setColor(QPalette::Window, Qt::white); + config_widget_->setPalette(p); + + // Set status text red + QPalette p3(ui_.status->palette()); + p3.setColor(QPalette::Text, Qt::red); + ui_.status->setPalette(p3); + + QObject::connect(ui_.select_grid, SIGNAL(clicked()), this, SLOT(SelectTopicGrid())); + + QObject::connect( + ui_.topic_grid, + SIGNAL(textEdited(const QString&)), + this, + SLOT(TopicGridEdited())); + + QObject::connect( + this, + SIGNAL(TargetFrameChanged(std::string)), + this, + SLOT(FrameChanged(std::string))); + + QObject::connect( + ui_.checkbox_update, + SIGNAL(toggled(bool)), + this, + SLOT(upgradeCheckBoxToggled(bool))); + + QObject::connect( + ui_.color_scheme, + SIGNAL(currentTextChanged(const QString &)), + this, + SLOT(colorSchemeUpdated(const QString &))); + } + + void OccupancyGridPlugin::DrawIcon() + { + if (icon_) + { + QPixmap icon(16, 16); + icon.fill(Qt::transparent); + + QPainter painter(&icon); + painter.setRenderHint(QPainter::Antialiasing, true); + + QPen pen(Qt::black); + + pen.setWidth(2); + pen.setCapStyle(Qt::SquareCap); + painter.setPen(pen); + + painter.drawLine(2, 2, 14, 2); + painter.drawLine(2, 2, 2, 14); + painter.drawLine(14, 2, 14, 14); + painter.drawLine(2, 14, 14, 14); + painter.drawLine(8, 2, 8, 14); + painter.drawLine(2, 8, 14, 8); + + icon_->SetPixmap(icon); + } + } + + void OccupancyGridPlugin::FrameChanged(std::string) + { + transformed_ = false; + } + + void OccupancyGridPlugin::SelectTopicGrid() + { + std::string topic = mapviz::SelectTopicDialog::selectTopic(node_, "nav_msgs/msg/OccupancyGrid"); + if (!topic.empty()) + { + QString str = QString::fromStdString(topic); + ui_.topic_grid->setText( str); + TopicGridEdited(); + } + } + + + void OccupancyGridPlugin::TopicGridEdited() + { + const std::string topic = ui_.topic_grid->text().trimmed().toStdString(); + + initialized_ = false; + grid_.reset(); + raw_buffer_.clear(); + + grid_sub_.reset(); + update_sub_.reset(); + + if (!topic.empty()) + { + grid_sub_ = node_->create_subscription( + topic, + rclcpp::QoS(10), + std::bind(&OccupancyGridPlugin::Callback, this, std::placeholders::_1)); + if(ui_.checkbox_update->isChecked()) + { + update_sub_ = node_->create_subscription( + topic, + rclcpp::QoS(10), + std::bind(&OccupancyGridPlugin::CallbackUpdate, this, std::placeholders::_1)); + } + RCLCPP_INFO(node_->get_logger(), "Subscribing to %s", topic.c_str()); + } + } + + void OccupancyGridPlugin::upgradeCheckBoxToggled(bool) + { + const std::string topic = ui_.topic_grid->text().trimmed().toStdString(); + update_sub_.reset(); + + if (ui_.checkbox_update) + { + update_sub_ = node_->create_subscription( + topic, + rclcpp::QoS(10), + std::bind(&OccupancyGridPlugin::CallbackUpdate, this, std::placeholders::_1)); + } + } + + void OccupancyGridPlugin::colorSchemeUpdated(const QString &) + { + if( grid_ && !raw_buffer_.empty()) + { + const size_t width = grid_->info.width; + const size_t height = grid_->info.height; + const Palette& palette = + (ui_.color_scheme->currentText() == "map") ? map_palette_ : costmap_palette_; + + for (size_t row = 0; row < height; row++) + { + for (size_t col = 0; col < width; col++) + { + size_t index = (col + row * texture_size_); + uchar color = raw_buffer_[index]; + memcpy( &color_buffer_[index*CHANNELS], &palette[color*CHANNELS], CHANNELS); + } + } + updateTexture(); + } + } + + void OccupancyGridPlugin::PrintError(const std::string& message) + { + PrintErrorHelper(ui_.status, message); + } + + void OccupancyGridPlugin::PrintInfo(const std::string& message) + { + PrintInfoHelper(ui_.status, message); + } + + void OccupancyGridPlugin::PrintWarning(const std::string& message) + { + PrintWarningHelper(ui_.status, message); + } + + QWidget* OccupancyGridPlugin::GetConfigWidget(QWidget* parent) + { + config_widget_->setParent(parent); + + return config_widget_; + } + + bool OccupancyGridPlugin::Initialize(QGLWidget* canvas) + { + canvas_ = canvas; + DrawIcon(); + return true; + } + + void OccupancyGridPlugin::updateTexture() + { + if (texture_id_ != -1) + { + glDeleteTextures(1, &texture_id_); + } + + // Get a new texture id. + glGenTextures(1, &texture_id_); + + // Bind the texture with the correct size and null memory. + glBindTexture(GL_TEXTURE_2D, texture_id_); + glPixelStorei(GL_UNPACK_ALIGNMENT, 1); + + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); + + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE); + + glTexEnvf( GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE ); + + glTexImage2D( + GL_TEXTURE_2D, + 0, + GL_RGBA, + texture_size_, + texture_size_, + 0, + GL_RGBA, + GL_UNSIGNED_BYTE, + color_buffer_.data()); + + glBindTexture(GL_TEXTURE_2D, 0); + glPixelStorei(GL_UNPACK_ALIGNMENT, 4); + } + + + void OccupancyGridPlugin::Callback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) + { + grid_ = msg; + const int width = grid_->info.width; + const int height = grid_->info.height; + initialized_ = true; + source_frame_ = grid_->header.frame_id; + transformed_ = GetTransform( source_frame_, grid_->header.stamp, transform_); + if ( !transformed_ ) + { + PrintError("No transform between " + source_frame_ + " and " + target_frame_); + } + + int32_t max_dimension = std::max(height, width); + + texture_size_ = 2; + while (texture_size_ < max_dimension){ + texture_size_ = texture_size_ << 1; + } + + const Palette& palette = + (ui_.color_scheme->currentText() == "map") ? map_palette_ : costmap_palette_; + + raw_buffer_.resize(texture_size_*texture_size_, 0); + color_buffer_.resize(texture_size_*texture_size_*CHANNELS, 0); + + for (size_t row = 0; row < height; row++) + { + for (size_t col = 0; col < width; col++) + { + size_t index_src = (col + row*width); + size_t index_dst = (col + row*texture_size_); + uchar color = static_cast( grid_->data[ index_src ] ); + raw_buffer_[index_dst] = color; + memcpy( &color_buffer_[index_dst*CHANNELS], &palette[color*CHANNELS], CHANNELS); + } + } + + texture_x_ = static_cast(width) / static_cast(texture_size_); + texture_y_ = static_cast(height) / static_cast(texture_size_); + + updateTexture(); + PrintInfo("Map received"); + } + + void OccupancyGridPlugin::CallbackUpdate(const map_msgs::msg::OccupancyGridUpdate::SharedPtr msg) + { + PrintInfo("Update Received"); + + if( initialized_ ) + { + const Palette& palette = + (ui_.color_scheme->currentText() == "map") ? map_palette_ : costmap_palette_; + + for (size_t row = 0; row < msg->height; row++) + { + for (size_t col = 0; col < msg->width; col++) + { + size_t index_src = (col + row * msg->width); + size_t index_dst = ( (col + msg->x) + (row + msg->y)*texture_size_); + uchar color = static_cast( msg->data[ index_src ] ); + raw_buffer_[index_dst] = color; + memcpy( &color_buffer_[index_dst*CHANNELS], &palette[color*CHANNELS], CHANNELS); + } + } + updateTexture(); + } + } + + void OccupancyGridPlugin::Draw(double x, double y, double scale) + { + glPushMatrix(); + + if( grid_ && transformed_) + { + double resolution = grid_->info.resolution; + glTranslatef( transform_.GetOrigin().getX(), + transform_.GetOrigin().getY(), + 0.0); + + const double RAD_TO_DEG = 180.0 / M_PI; + + tf2Scalar yaw, pitch, roll; + tf2::Matrix3x3 mat( transform_.GetOrientation() ); + mat.getEulerYPR(yaw, pitch, roll); + + glRotatef(pitch * RAD_TO_DEG, 0, 1, 0); + glRotatef(roll * RAD_TO_DEG, 1, 0, 0); + glRotatef(yaw * RAD_TO_DEG, 0, 0, 1); + + glTranslatef( grid_->info.origin.position.x, + grid_->info.origin.position.y, + 0.0); + + glScalef( resolution, resolution, 1.0); + + float width = static_cast(grid_->info.width); + float height = static_cast(grid_->info.height); + + glEnable(GL_TEXTURE_2D); + glBindTexture(GL_TEXTURE_2D, texture_id_); + glBegin(GL_TRIANGLES); + + glColor4f(1.0f, 1.0f, 1.0f, ui_.alpha->value() ); + + glTexCoord2d(0, 0); + glVertex2d(0, 0); + glTexCoord2d(texture_x_, 0); + glVertex2d(width, 0); + glTexCoord2d(texture_x_, texture_y_); + glVertex2d(width, height); + + glTexCoord2d(0, 0); + glVertex2d(0, 0); + glTexCoord2d(texture_x_, texture_y_); + glVertex2d(width, height); + glTexCoord2d(0, texture_y_); + glVertex2d(0, height); + + glEnd(); + + glBindTexture(GL_TEXTURE_2D, 0); + glDisable(GL_TEXTURE_2D); + } + glPopMatrix(); + } + + void OccupancyGridPlugin::Transform() + { + if( !initialized_ ) return; + swri_transform_util::Transform transform; + if ( grid_ ) + { + if( GetTransform( source_frame_, rclcpp::Time(0), transform) ) + { + transformed_ = true; + transform_ = transform; + } + } + if ( !transformed_ ) + { + PrintError("No transform between " + source_frame_ + " and " + target_frame_); + } + } + + void OccupancyGridPlugin::LoadConfig(const YAML::Node& node, const std::string& path) + { + if (node["topic"]) + { + std::string topic = node["topic"].as(); + ui_.topic_grid->setText(QString::fromStdString(topic)); + } + + if (node["update"]) + { + bool checked = node["update"].as(); + ui_.checkbox_update->setChecked( checked ); + } + + if (node["alpha"]) + { + double alpha = node["alpha"].as(); + ui_.alpha->setValue(alpha); + } + + if (node["scheme"]) + { + std::string scheme = node["scheme"].as(); + int index = ui_.color_scheme->findText(QString::fromStdString(scheme), Qt::MatchExactly); + if (index >= 0) + { + ui_.color_scheme->setCurrentIndex(index); + } + colorSchemeUpdated(QString::fromStdString(scheme)); + } + + TopicGridEdited(); + } + + void OccupancyGridPlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path) + { + emitter << YAML::Key << "alpha" << YAML::Value << ui_.alpha->value(); + emitter << YAML::Key << "topic" << YAML::Value << ui_.topic_grid->text().toStdString(); + emitter << YAML::Key << "update" << YAML::Value << ui_.checkbox_update->isChecked(); + emitter << YAML::Key + << "scheme" + << YAML::Value + << ui_.color_scheme->currentText().toStdString(); + } +} // namespace mapviz_plugins + diff --git a/mapviz_plugins/src/odometry_plugin.cpp b/mapviz_plugins/src/odometry_plugin.cpp new file mode 100644 index 000000000..2aabf6849 --- /dev/null +++ b/mapviz_plugins/src/odometry_plugin.cpp @@ -0,0 +1,407 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include + +// QT libraries +#include +#include +#include +#include + +#include + +// ROS libraries +#include + +#include +#include +#include + +// Declare plugin +#include + +// C++ standard libraries +#include +#include +#include +#include + +PLUGINLIB_EXPORT_CLASS(mapviz_plugins::OdometryPlugin, mapviz::MapvizPlugin) + +namespace mapviz_plugins +{ + OdometryPlugin::OdometryPlugin() + : PointDrawingPlugin() + , ui_() + , config_widget_(new QWidget()) + , has_message_(false) + { + ui_.setupUi(config_widget_); + ui_.color->setColor(Qt::green); + + // Set background white + QPalette p(config_widget_->palette()); + p.setColor(QPalette::Window, Qt::white); + config_widget_->setPalette(p); + + // Set status text red + QPalette p3(ui_.status->palette()); + p3.setColor(QPalette::Text, Qt::red); + ui_.status->setPalette(p3); + + QObject::connect(ui_.selecttopic, SIGNAL(clicked()), this, + SLOT(SelectTopic())); + QObject::connect(ui_.topic, SIGNAL(editingFinished()), this, + SLOT(TopicEdited())); + QObject::connect(ui_.positiontolerance, SIGNAL(valueChanged(double)), this, + SLOT(PositionToleranceChanged(double))); + QObject::connect(ui_.buffersize, SIGNAL(valueChanged(int)), this, + SLOT(BufferSizeChanged(int))); + QObject::connect(ui_.drawstyle, SIGNAL(activated(QString)), this, + SLOT(SetDrawStyle(QString))); + QObject::connect(ui_.static_arrow_sizes, SIGNAL(clicked(bool)), + this, SLOT(SetStaticArrowSizes(bool))); + QObject::connect(ui_.arrow_size, SIGNAL(valueChanged(int)), + this, SLOT(SetArrowSize(int))); + QObject::connect(ui_.color, SIGNAL(colorEdited(const QColor&)), this, + SLOT(SetColor(const QColor&))); + QObject::connect(ui_.show_laps, SIGNAL(toggled(bool)), this, + SLOT(LapToggled(bool))); + QObject::connect(ui_.show_covariance, SIGNAL(toggled(bool)), this, + SLOT(CovariancedToggled(bool))); + QObject::connect(ui_.show_all_covariances, SIGNAL(toggled(bool)), this, + SLOT(ShowAllCovariancesToggled(bool))); + QObject::connect(ui_.buttonResetBuffer, SIGNAL(pressed()), this, + SLOT(ClearPoints())); + } + + void OdometryPlugin::SelectTopic() + { + std::string topic = mapviz::SelectTopicDialog::selectTopic(node_, "nav_msgs/msg/Odometry"); + + if (!topic.empty()) + { + ui_.topic->setText(QString::fromStdString(topic)); + TopicEdited(); + } + } + + void OdometryPlugin::TopicEdited() + { + std::string topic = ui_.topic->text().trimmed().toStdString(); + if (topic != topic_) + { + initialized_ = false; + ClearPoints(); + has_message_ = false; + PrintWarning("No messages received."); + + odometry_sub_.reset(); + + topic_ = topic; + if (!topic.empty()) + { + odometry_sub_ = node_->create_subscription(topic_, rclcpp::QoS(1), + std::bind(&OdometryPlugin::odometryCallback, this, std::placeholders::_1)); + + RCLCPP_INFO(node_->get_logger(), "Subscribing to %s", topic_.c_str()); + } + } + } + + void OdometryPlugin::odometryCallback( + const nav_msgs::msg::Odometry::SharedPtr odometry) + { + if (!has_message_) + { + initialized_ = true; + has_message_ = true; + } + + // Note that unlike some plugins, this one does not store nor rely on the + // source_frame_ member variable. This one can potentially store many + // messages with different source frames, so we need to store and transform + // them individually. + StampedPoint stamped_point; + stamped_point.stamp = odometry->header.stamp; + stamped_point.source_frame = odometry->header.frame_id; + + stamped_point.point = tf2::Vector3(odometry->pose.pose.position.x, + odometry->pose.pose.position.y, + odometry->pose.pose.position.z); + + stamped_point.orientation = tf2::Quaternion( + odometry->pose.pose.orientation.x, + odometry->pose.pose.orientation.y, + odometry->pose.pose.orientation.z, + odometry->pose.pose.orientation.w); + + if ( ui_.show_covariance->isChecked() ) + { + tf2::Matrix3x3 tf_cov = + swri_transform_util::GetUpperLeft(odometry->pose.covariance); + + if (tf_cov[0][0] < 100000 && tf_cov[1][1] < 100000) + { + cv::Mat cov_matrix_3d(3, 3, CV_32FC1); + for (int32_t r = 0; r < 3; r++) + { + for (int32_t c = 0; c < 3; c++) + { + cov_matrix_3d.at(r, c) = tf_cov[r][c]; + } + } + + cv::Mat cov_matrix_2d = swri_image_util::ProjectEllipsoid(cov_matrix_3d); + + if (!cov_matrix_2d.empty()) + { + stamped_point.cov_points = swri_image_util::GetEllipsePoints( + cov_matrix_2d, stamped_point.point, 3, 32); + + stamped_point.transformed_cov_points = stamped_point.cov_points; + } else { + RCLCPP_ERROR(node_->get_logger(), "Failed to project x, y, z covariance to xy-plane."); + } + } + } + + pushPoint(std::move(stamped_point)); + } + + void OdometryPlugin::PrintError(const std::string& message) + { + PrintErrorHelper(ui_.status, message); + } + + void OdometryPlugin::PrintInfo(const std::string& message) + { + PrintInfoHelper(ui_.status, message); + } + + void OdometryPlugin::PrintWarning(const std::string& message) + { + PrintWarningHelper(ui_.status, message); + } + + QWidget* OdometryPlugin::GetConfigWidget(QWidget* parent) + { + config_widget_->setParent(parent); + + return config_widget_; + } + + bool OdometryPlugin::Initialize(QGLWidget* canvas) + { + canvas_ = canvas; + SetColor(ui_.color->color()); + + return true; + } + + void OdometryPlugin::Draw(double x, double y, double scale) + { + if (ui_.show_covariance->isChecked()) + { + DrawCovariance(); + } + if (DrawPoints(scale)) + { + PrintInfo("OK"); + } + } + + void OdometryPlugin::Paint(QPainter* painter, double x, double y, double scale) + { + // dont render any timestamps if the show_timestamps is set to 0 + int interval = ui_.show_timestamps->value(); + if (interval == 0) + { + return; + } + + QTransform tf = painter->worldTransform(); + QFont font("Helvetica", 10); + painter->setFont(font); + painter->save(); + painter->resetTransform(); + + // set the draw color for the text to be the same as the rest + QPen pen(QBrush(ui_.color->color()), 1); + painter->setPen(pen); + + int counter = 0; // used to alternate between rendering text on some points + for (const StampedPoint& point : points()) + { + // this renders a timestamp every 'interval' points + if (point.transformed && counter % interval == 0) + { + QPointF qpoint = tf.map(QPointF(point.transformed_point.getX(), + point.transformed_point.getY())); + QString time; + time.setNum(point.stamp.seconds(), 'g', 12); + painter->drawText(qpoint, time); + } + counter++; + } + + painter->restore(); + } + + void OdometryPlugin::LoadConfig(const YAML::Node& node, + const std::string& path) + { + if (node["topic"]) + { + std::string topic = node["topic"].as(); + ui_.topic->setText(topic.c_str()); + } + + if (node["color"]) + { + std::string color = node["color"].as(); + QColor qcolor(color.c_str()); + SetColor(qcolor); + ui_.color->setColor(qcolor); + } + + if (node["draw_style"]) + { + std::string draw_style; + draw_style = node["draw_style"].as(); + + if (draw_style == "lines") + { + ui_.drawstyle->setCurrentIndex(0); + SetDrawStyle( LINES ); + } else if (draw_style == "points") { + ui_.drawstyle->setCurrentIndex(1); + SetDrawStyle( POINTS ); + } else if (draw_style == "arrows") { + ui_.drawstyle->setCurrentIndex(2); + SetDrawStyle( ARROWS ); + } + } + + if (node["position_tolerance"]) + { + double position_tolerance = node["position_tolerance"].as(); + ui_.positiontolerance->setValue(position_tolerance); + PositionToleranceChanged(position_tolerance); + } + + if (node["buffer_size"]) + { + int buffer_size = node["buffer_size"].as(); + ui_.buffersize->setValue(buffer_size); + BufferSizeChanged(buffer_size); + } + + if (node["show_covariance"]) + { + bool show_covariance = node["show_covariance"].as(); + ui_.show_covariance->setChecked(show_covariance); + CovariancedToggled(show_covariance); + } + + if (node["show_all_covariances"]) + { + bool show_all_covariances = node["show_all_covariances"].as(); + ui_.show_all_covariances->setChecked(show_all_covariances); + ShowAllCovariancesToggled(show_all_covariances); + } + + if (node["show_laps"]) + { + bool show_laps = node["show_laps"].as(); + ui_.show_laps->setChecked(show_laps); + LapToggled(show_laps); + } + + if (node["static_arrow_sizes"]) + { + bool static_arrow_sizes = node["static_arrow_sizes"].as(); + ui_.static_arrow_sizes->setChecked(static_arrow_sizes); + SetStaticArrowSizes(static_arrow_sizes); + } + + if (node["arrow_size"]) + { + int arrow_size = node["arrow_size"].as(); + ui_.arrow_size->setValue(arrow_size); + SetArrowSize(arrow_size); + } + + if (node["show_timestamps"]) + { + ui_.show_timestamps->setValue(node["show_timestamps"].as()); + } + + TopicEdited(); + } + + void OdometryPlugin::SaveConfig(YAML::Emitter& emitter, + const std::string& path) + { + std::string topic = ui_.topic->text().toStdString(); + emitter << YAML::Key << "topic" << YAML::Value << topic; + + std::string color = ui_.color->color().name().toStdString(); + emitter << YAML::Key << "color" << YAML::Value << color; + + std::string draw_style = ui_.drawstyle->currentText().toStdString(); + emitter << YAML::Key << "draw_style" << YAML::Value << draw_style; + + emitter << YAML::Key << "position_tolerance" << + YAML::Value << positionTolerance(); + + emitter << YAML::Key << "buffer_size" << YAML::Value << bufferSize(); + + bool show_laps = ui_.show_laps->isChecked(); + emitter << YAML::Key << "show_laps" << YAML::Value << show_laps; + + bool show_covariance = ui_.show_covariance->isChecked(); + emitter << YAML::Key << "show_covariance" << YAML::Value << show_covariance; + + bool show_all_covariances = ui_.show_all_covariances->isChecked(); + emitter << YAML::Key << "show_all_covariances" << YAML::Value << show_all_covariances; + + emitter << YAML::Key + << "static_arrow_sizes" + << YAML::Value + << ui_.static_arrow_sizes->isChecked(); + + emitter << YAML::Key << "arrow_size" << YAML::Value << ui_.arrow_size->value(); + + emitter << YAML::Key << "show_timestamps" << YAML::Value << ui_.show_timestamps->value(); + } +} // namespace mapviz_plugins + + diff --git a/mapviz_plugins/src/path_plugin.cpp b/mapviz_plugins/src/path_plugin.cpp new file mode 100644 index 000000000..8c5e78bae --- /dev/null +++ b/mapviz_plugins/src/path_plugin.cpp @@ -0,0 +1,211 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include + +// QT libraries +#include +#include + +// ROS libraries +#include + +#include + +// Declare plugin +#include + +// C++ standard libraries +#include +#include +#include +#include + +PLUGINLIB_EXPORT_CLASS(mapviz_plugins::PathPlugin, mapviz::MapvizPlugin) + +namespace mapviz_plugins +{ + PathPlugin::PathPlugin() + : PointDrawingPlugin() + , ui_() + , config_widget_(new QWidget()) + , has_message_(false) + { + ui_.setupUi(config_widget_); + ui_.path_color->setColor(Qt::green); + + // Set background white + QPalette p(config_widget_->palette()); + p.setColor(QPalette::Window, Qt::white); + config_widget_->setPalette(p); + + // Set status text red + QPalette p3(ui_.status->palette()); + p3.setColor(QPalette::Text, Qt::red); + ui_.status->setPalette(p3); + + connect(ui_.selecttopic, SIGNAL(clicked()), this, SLOT(SelectTopic())); + connect(ui_.topic, SIGNAL(editingFinished()), this, SLOT(TopicEdited())); + connect(ui_.path_color, SIGNAL(colorEdited(const QColor&)), this, + SLOT(SetColor(const QColor&))); + } + + void PathPlugin::SelectTopic() + { + std::string topic = mapviz::SelectTopicDialog::selectTopic(node_, "nav_msgs/msg/Path"); + + if (!topic.empty()) + { + ui_.topic->setText(QString::fromStdString(topic)); + TopicEdited(); + } + } + + void PathPlugin::TopicEdited() + { + std::string topic = ui_.topic->text().trimmed().toStdString(); + if (topic != topic_) + { + initialized_ = false; + ClearPoints(); + has_message_ = false; + PrintWarning("No messages received."); + + path_sub_.reset(); + + topic_ = topic; + if (!topic.empty()) + { + path_sub_ = node_->create_subscription( + topic_, + rclcpp::QoS(1), + std::bind(&PathPlugin::pathCallback, this, std::placeholders::_1) + ); + + RCLCPP_INFO(node_->get_logger(), "Subscribing to %s", topic_.c_str()); + } + } + } + + void PathPlugin::pathCallback(const nav_msgs::msg::Path::SharedPtr path) + { + if (!has_message_) + { + initialized_ = true; + has_message_ = true; + } + + ClearPoints(); + + for (unsigned int i = 0; i < path->poses.size(); i++) + { + StampedPoint stamped_point; + stamped_point.stamp = path->header.stamp; + stamped_point.source_frame = path->header.frame_id; + stamped_point.point = tf2::Vector3(path->poses[i].pose.position.x, + path->poses[i].pose.position.y, 0); + + pushPoint( stamped_point ); + } + } + + void PathPlugin::PrintError(const std::string& message) + { + PrintErrorHelper(ui_.status, message); + } + + void PathPlugin::PrintInfo(const std::string& message) + { + PrintInfoHelper(ui_.status, message); + } + + void PathPlugin::PrintWarning(const std::string& message) + { + PrintWarningHelper(ui_.status, message); + } + + QWidget* PathPlugin::GetConfigWidget(QWidget* parent) + { + config_widget_->setParent(parent); + + return config_widget_; + } + + bool PathPlugin::Initialize(QGLWidget* canvas) + { + canvas_ = canvas; + DrawIcon(); + return true; + } + + void PathPlugin::Draw(double x, double y, double scale) + { + bool lines; + bool points; + QColor old_color = ui_.path_color->color(); + QColor color = old_color.darker(200); + SetDrawStyle( LINES ); + lines = DrawPoints(scale); + SetColor(color); + SetDrawStyle( POINTS ); + points = DrawPoints(scale); + SetColor(old_color); + if (lines && points) + { + PrintInfo("OK"); + } + } + + void PathPlugin::LoadConfig(const YAML::Node& node, const std::string& path) + { + if (node["topic"]) + { + std::string topic = node["topic"].as(); + ui_.topic->setText(topic.c_str()); + TopicEdited(); + } + + if (node["color"]) + { + std::string color = node["color"].as(); + QColor qcolor(color.c_str()); + SetColor(qcolor); + ui_.path_color->setColor(qcolor); + } + } + + void PathPlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path) + { + std::string topic = ui_.topic->text().toStdString(); + emitter << YAML::Key << "topic" << YAML::Value << topic; + + std::string color = ui_.path_color->color().name().toStdString(); + emitter << YAML::Key << "color" << YAML::Value << color; + } +} // namespace mapviz_plugins diff --git a/mapviz_plugins/src/placeable_window_proxy.cpp b/mapviz_plugins/src/placeable_window_proxy.cpp new file mode 100644 index 000000000..7ba0f216a --- /dev/null +++ b/mapviz_plugins/src/placeable_window_proxy.cpp @@ -0,0 +1,365 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +namespace mapviz_plugins +{ +PlaceableWindowProxy::PlaceableWindowProxy() + : QObject() + , target_(nullptr) + , visible_(true) + , has_cursor_(false) + , state_(INACTIVE) + , win_resize_timer_(-1) +{ +} + +PlaceableWindowProxy::~PlaceableWindowProxy() +{ + if (target_) + { + target_->removeEventFilter(this); + } +} + +void PlaceableWindowProxy::setContainer(QWidget *target) +{ + if (target_) + { + target_->removeEventFilter(this); + } + + target_ = target; + + if (target_) + { + target_->installEventFilter(this); + } +} + +QRect PlaceableWindowProxy::rect() const +{ + return rect_.toRect(); +} + +void PlaceableWindowProxy::setRect(const QRect &rect) +{ + rect_ = QRectF(rect); + state_ = INACTIVE; +} + +void PlaceableWindowProxy::setVisible(bool visible) +{ + if (visible == visible_) + { + return; + } + + visible_ = visible; + + if (!visible_ && state_ != INACTIVE) + { + if (has_cursor_) + { + QApplication::restoreOverrideCursor(); + has_cursor_ = false; + } + state_ = INACTIVE; + } +} + +bool PlaceableWindowProxy::eventFilter(QObject *, QEvent *event) +{ + // This should never happen, but doesn't hurt to be defensive. + if (!target_) + { + return false; + } + + if (!visible_) + { + return false; + } + + switch (event->type()) + { + case QEvent::MouseButtonPress: + return handleMousePress(dynamic_cast(event)); + case QEvent::MouseButtonRelease: + return handleMouseRelease(dynamic_cast(event)); + case QEvent::MouseMove: + return handleMouseMove(dynamic_cast(event)); + case QEvent::Resize: + return handleResize(dynamic_cast(event)); + default: + return false; + } +} + +bool PlaceableWindowProxy::handleMousePress(QMouseEvent *event) +{ + if (!visible_) + { + return false; + } + + if (!rect_.contains(event->pos())) + { + // We don't care about anything outside the rect. + return false; + } + + if (state_ != INACTIVE) + { + // We're already doing something, so we don't want to enter + // another state. But we also don't want someone else to start + // doing something, so we filter out the press. + return true; + } + + if (event->button() == Qt::LeftButton) + { + start_rect_ = rect_; + start_point_ = event->pos(); + state_ = getNextState(event->localPos()); + return true; + } + + // Event if we're not doing anything, we want to present a + // consistent interface that says "this region is belongs to me", so + // we filter out events. + return true; +} + +bool PlaceableWindowProxy::handleMouseRelease(QMouseEvent *event) +{ + if (!visible_) + { + return false; + } + + if (state_ == INACTIVE) + { + return false; + } + + if (event->button() == Qt::LeftButton) + { + state_ = INACTIVE; + return true; + } + + return false; +} + +bool PlaceableWindowProxy::handleMouseMove(QMouseEvent *event) +{ + if (!visible_) + { + return false; + } + + if (state_ == INACTIVE) + { + if (!rect_.contains(event->localPos())) + { + if (has_cursor_) + { + QApplication::restoreOverrideCursor(); + has_cursor_ = false; + } + return false; + } + + // The mouse cursor is over the rect, so we're going to change the + // cursor to indicate the state the user would enter by clicking. + + Qt::CursorShape shape; + switch(getNextState(event->localPos())) + { + case MOVE_TOP_LEFT: + case MOVE_BOTTOM_RIGHT: + shape = Qt::SizeFDiagCursor; + break; + case MOVE_TOP_RIGHT: + case MOVE_BOTTOM_LEFT: + shape = Qt::SizeBDiagCursor; + break; + default: + shape = Qt::SizeAllCursor; + } + + if (has_cursor_) + { + QApplication::changeOverrideCursor(QCursor(shape)); + } else { + QApplication::setOverrideCursor(QCursor(shape)); + has_cursor_ = true; + } + + return true; + } + + QPointF dp = event->localPos() - start_point_; + + // todo: enforce minimum size & constrain aspect ratio for resizes. + if (state_ == MOVE_ALL) + { + rect_ = start_rect_.translated(dp); + } else if (state_ == MOVE_TOP_LEFT) { + rect_ = resizeHelper(start_rect_, + start_rect_.bottomRight(), + start_rect_.topLeft(), + event->localPos()); + rect_.moveBottomRight(start_rect_.bottomRight()); + } else if (state_ == MOVE_BOTTOM_LEFT) { + rect_ = resizeHelper(start_rect_, + start_rect_.topRight(), + start_rect_.bottomLeft(), + event->localPos()); + rect_.moveTopRight(start_rect_.topRight()); + } else if (state_ == MOVE_BOTTOM_RIGHT) { + rect_ = resizeHelper(start_rect_, + start_rect_.topLeft(), + start_rect_.bottomRight(), + event->localPos()); + rect_.moveTopLeft(start_rect_.topLeft()); + } else if (state_ == MOVE_TOP_RIGHT) { + rect_ = resizeHelper(start_rect_, + start_rect_.bottomLeft(), + start_rect_.topRight(), + event->localPos()); + rect_.moveBottomLeft(start_rect_.bottomLeft()); + } else { + qWarning("Unhandled state in PlaceableWindowProxy: %d", state_); + } + + return true; +} + +QRectF PlaceableWindowProxy::resizeHelper(const QRectF &rect, + const QPointF &p1, + const QPointF &p2, + const QPointF &p3) const +{ + QPointF v1 = p2 - p1; + QPointF v2 = p3 - p1; + + double d = v1.x()*v2.y() - v1.y()*v2.x(); + if (d < 0) + { + double new_width = std::abs(p3.x() - p1.x()); + if (new_width < 10) + { + new_width = 10; + } + + double new_height = rect.height() / rect.width() * new_width; + return QRectF(0, 0, new_width, new_height); + } else { + double new_height = std::abs(p3.y() - p1.y()); + if (new_height < 10) + { + new_height = 10; + } + + double new_width = rect.width() / rect.height() * new_height; + return QRectF(0, 0, new_width, new_height); + } +} + + +bool PlaceableWindowProxy::handleResize(QResizeEvent *event) +{ + // We always want to pass the resize event along to other widgets. + return false; +} + +void PlaceableWindowProxy::timerEvent(QTimerEvent *event) +{ + if (event->timerId() == win_resize_timer_) + { + killTimer(win_resize_timer_); + win_resize_timer_ = -1; + if (target_) + { + winResize(target_->size()); + } + } +} + +void PlaceableWindowProxy::rectResize(int dx, int dy) +{ +} + +void PlaceableWindowProxy::winResize(const QSize &size) +{ +} + +PlaceableWindowProxy::State PlaceableWindowProxy::getNextState( + const QPointF &pt) const +{ + if (!rect_.contains(pt)) + { + return INACTIVE; + } + + const double threshold = 10.0; + bool near_left = std::fabs(pt.x() - rect_.left()) < threshold; + bool near_top = std::fabs(pt.y() - rect_.top()) < threshold; + bool near_right = std::fabs(rect_.right() - pt.x()) < threshold; + bool near_bottom = std::fabs(rect_.bottom() - pt.y()) < threshold; + + if (near_top && near_left) + { + return MOVE_TOP_LEFT; + } else if (near_top && near_right) { + return MOVE_TOP_RIGHT; + } else if (near_bottom && near_left) { + return MOVE_BOTTOM_LEFT; + } else if (near_bottom && near_right) { + return MOVE_BOTTOM_RIGHT; + } else { + return MOVE_ALL; + } +} +} // namespace mapviz_plugins diff --git a/mapviz_plugins/src/plan_route_plugin.cpp b/mapviz_plugins/src/plan_route_plugin.cpp new file mode 100644 index 000000000..d31325b9b --- /dev/null +++ b/mapviz_plugins/src/plan_route_plugin.cpp @@ -0,0 +1,497 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include + +// QT libraries +#include +#include +#include +#include +#include +#include +#include + +// ROS libraries +#include + +#include +#include + +#include + +// Declare plugin +#include + +// C++ standard libraries +#include +#include +#include +#include +#include +#include + +PLUGINLIB_EXPORT_CLASS(mapviz_plugins::PlanRoutePlugin, mapviz::MapvizPlugin) + +using namespace std::chrono_literals; + +namespace mnm = marti_nav_msgs; +namespace sru = swri_route_util; +namespace stu = swri_transform_util; + +namespace mapviz_plugins +{ + PlanRoutePlugin::PlanRoutePlugin() + : MapvizPlugin() + , ui_() + , config_widget_(new QWidget()) + , map_canvas_(nullptr) + , failed_service_(false) + , selected_point_(-1) + , is_mouse_down_(false) + , mouse_down_time_(0) + , max_ms_(Q_INT64_C(500)) + , max_distance_(2.0) + { + ui_.setupUi(config_widget_); + + ui_.color->setColor(Qt::green); + // Set background white + QPalette p(config_widget_->palette()); + p.setColor(QPalette::Window, Qt::white); + config_widget_->setPalette(p); + // Set status text red + QPalette p3(ui_.status->palette()); + p3.setColor(QPalette::Text, Qt::red); + ui_.status->setPalette(p3); + QObject::connect(ui_.service, SIGNAL(editingFinished()), this, + SLOT(PlanRoute())); + QObject::connect(ui_.publish, SIGNAL(clicked()), this, + SLOT(PublishRoute())); + QObject::connect(ui_.clear, SIGNAL(clicked()), this, + SLOT(Clear())); + QObject::connect(this, + SIGNAL(VisibleChanged(bool)), + this, + SLOT(VisibilityChanged(bool))); + } + + PlanRoutePlugin::~PlanRoutePlugin() + { + if (map_canvas_) + { + map_canvas_->removeEventFilter(this); + } + } + + void PlanRoutePlugin::VisibilityChanged(bool visible) + { + if (visible) + { + map_canvas_->installEventFilter(this); + } + else + { + map_canvas_->removeEventFilter(this); + } + } + + void PlanRoutePlugin::PublishRoute() + { + if (route_preview_) + { + if (route_topic_ != ui_.topic->text().toStdString()) + { + route_topic_ = ui_.topic->text().toStdString(); + route_pub_.reset(); + route_pub_ = node_->create_publisher( + route_topic_, + rclcpp::QoS(1)); + } + + route_pub_->publish(*route_preview_->toMsgPtr()); + } + } + + void PlanRoutePlugin::PlanRoute() + { + route_preview_ = sru::RoutePtr(); + bool start_from_vehicle = ui_.start_from_vehicle->isChecked(); + if (waypoints_.size() + start_from_vehicle < 2 || !Visible()) + { + return; + } + + std::string service = ui_.service->text().toStdString(); + if (service.empty()) + { + PrintError("Service name may not be empty."); + return; + } + auto client = node_->create_client(service); + client->wait_for_service(1ms); + + if (!client->service_is_ready()) + { + PrintError("Service is unavailable."); + return; + } + + auto plan_route = std::make_shared(); + + plan_route->header.frame_id = swri_transform_util::_wgs84_frame; + plan_route->header.stamp = node_->now(); + plan_route->plan_from_vehicle = static_cast(start_from_vehicle); + plan_route->waypoints = waypoints_; + + PrintInfo("Sending route..."); + auto result = client->async_send_request(plan_route, + std::bind(&PlanRoutePlugin::ClientCallback, this, std::placeholders::_1)); + } + + void PlanRoutePlugin::ClientCallback( + rclcpp::Client::SharedFuture future) + { + RCLCPP_ERROR(node_->get_logger(), "Request callback happened"); + const auto& result = future.get(); + if (future.valid()) + { + if (result->success) + { + PrintInfo("OK"); + route_preview_ = std::make_shared(result->route); + failed_service_ = false; + } else { + PrintError(result->message); + failed_service_ = true; + } + } else { + PrintError("Error calling PlanRoute service"); + failed_service_ = true; + } + } + + void PlanRoutePlugin::Retry() + { + PlanRoute(); + } + + void PlanRoutePlugin::Clear() + { + waypoints_.clear(); + route_preview_ = sru::RoutePtr(); + } + + void PlanRoutePlugin::PrintError(const std::string& message) + { + PrintErrorHelper(ui_.status, message, 1.0); + } + + void PlanRoutePlugin::PrintInfo(const std::string& message) + { + PrintInfoHelper(ui_.status, message, 1.0); + } + + void PlanRoutePlugin::PrintWarning(const std::string& message) + { + PrintWarningHelper(ui_.status, message, 1.0); + } + + QWidget* PlanRoutePlugin::GetConfigWidget(QWidget* parent) + { + config_widget_->setParent(parent); + + return config_widget_; + } + + bool PlanRoutePlugin::Initialize(QGLWidget* canvas) + { + map_canvas_ = dynamic_cast(canvas); + map_canvas_->installEventFilter(this); + + retry_timer_ = node_->create_wall_timer(1000ms, [this](){Retry();}); + + initialized_ = true; + return true; + } + + bool PlanRoutePlugin::eventFilter(QObject *object, QEvent* event) + { + switch (event->type()) + { + case QEvent::MouseButtonPress: + return handleMousePress(dynamic_cast(event)); + case QEvent::MouseButtonRelease: + return handleMouseRelease(dynamic_cast(event)); + case QEvent::MouseMove: + return handleMouseMove(dynamic_cast(event)); + default: + return false; + } + } + + bool PlanRoutePlugin::handleMousePress(QMouseEvent* event) + { + selected_point_ = -1; + int closest_point = 0; + double closest_distance = std::numeric_limits::max(); + + QPointF point = event->localPos(); + stu::Transform transform; + if (tf_manager_->GetTransform(target_frame_, stu::_wgs84_frame, transform)) + { + for (size_t i = 0; i < waypoints_.size(); i++) + { + tf2::Vector3 waypoint( + waypoints_[i].position.x, + waypoints_[i].position.y, + 0.0); + waypoint = transform * waypoint; + + QPointF transformed = + map_canvas_->FixedFrameToMapGlCoord(QPointF(waypoint.x(), waypoint.y())); + + double distance = QLineF(transformed, point).length(); + + if (distance < closest_distance) + { + closest_distance = distance; + closest_point = static_cast(i); + } + } + } + + if (event->button() == Qt::LeftButton) + { + if (closest_distance < 15) + { + selected_point_ = closest_point; + return true; + } else { + is_mouse_down_ = true; + mouse_down_pos_ = event->localPos(); + mouse_down_time_ = QDateTime::currentMSecsSinceEpoch(); + return false; + } + } else if (event->button() == Qt::RightButton) { + if (closest_distance < 15) + { + waypoints_.erase(waypoints_.begin() + closest_point); + PlanRoute(); + return true; + } + } + + return false; + } + + bool PlanRoutePlugin::handleMouseRelease(QMouseEvent* event) + { + QPointF point = event->localPos(); + if (selected_point_ >= 0 && static_cast(selected_point_) < waypoints_.size()) + { + stu::Transform transform; + if (tf_manager_->GetTransform(stu::_wgs84_frame, target_frame_, transform)) + { + QPointF transformed = map_canvas_->MapGlCoordToFixedFrame(point); + tf2::Vector3 position(transformed.x(), transformed.y(), 0.0); + position = transform * position; + waypoints_[selected_point_].position.x = position.x(); + waypoints_[selected_point_].position.y = position.y(); + PlanRoute(); + } + + selected_point_ = -1; + return true; + } else if (is_mouse_down_) { + qreal distance = QLineF(mouse_down_pos_, point).length(); + qint64 msecsDiff = QDateTime::currentMSecsSinceEpoch() - mouse_down_time_; + + // Only fire the event if the mouse has moved less than the maximum distance + // and was held for shorter than the maximum time.. This prevents click + // events from being fired if the user is dragging the mouse across the map + // or just holding the cursor in place. + if (msecsDiff < max_ms_ && distance <= max_distance_) + { + QPointF transformed = map_canvas_->MapGlCoordToFixedFrame(point); + + stu::Transform transform; + tf2::Vector3 position(transformed.x(), transformed.y(), 0.0); + if (tf_manager_->GetTransform(stu::_wgs84_frame, target_frame_, transform)) + { + position = transform * position; + + geometry_msgs::msg::Pose pose; + pose.position.x = position.x(); + pose.position.y = position.y(); + waypoints_.push_back(pose); + PlanRoute(); + } + } + } + is_mouse_down_ = false; + + return false; + } + + bool PlanRoutePlugin::handleMouseMove(QMouseEvent* event) + { + if (selected_point_ >= 0 && static_cast(selected_point_) < waypoints_.size()) + { + QPointF point = event->localPos(); + stu::Transform transform; + if (tf_manager_->GetTransform(stu::_wgs84_frame, target_frame_, transform)) + { + QPointF transformed = map_canvas_->MapGlCoordToFixedFrame(point); + tf2::Vector3 position(transformed.x(), transformed.y(), 0.0); + position = transform * position; + waypoints_[selected_point_].position.y = position.y(); + waypoints_[selected_point_].position.x = position.x(); + PlanRoute(); + } + + return true; + } + return false; + } + + void PlanRoutePlugin::Draw(double x, double y, double scale) + { + stu::Transform transform; + if (tf_manager_->GetTransform(target_frame_, stu::_wgs84_frame, transform)) + { + if (!failed_service_) + { + if (route_preview_) + { + sru::Route route = *route_preview_; + sru::transform(route, transform, target_frame_); + + glLineWidth(2); + const QColor color = ui_.color->color(); + glColor4d(color.redF(), color.greenF(), color.blueF(), 1.0); + glBegin(GL_LINE_STRIP); + + for (auto & point : route.points) + { + glVertex2d(point.position().x(), point.position().y()); + } + + glEnd(); + } + } + + // Draw waypoints + + glPointSize(20); + glColor4f(0.0, 1.0, 1.0, 1.0); + glBegin(GL_POINTS); + + for (auto & waypoint : waypoints_) + { + tf2::Vector3 point(waypoint.position.x, waypoint.position.y, 0); + point = transform * point; + glVertex2d(point.x(), point.y()); + } + glEnd(); + } else { + PrintError("Failed to transform."); + } + } + + void PlanRoutePlugin::Paint(QPainter* painter, double x, double y, double scale) + { + painter->save(); + painter->resetTransform(); + + QPen pen(QBrush(QColor(Qt::darkCyan).darker()), 1); + painter->setPen(pen); + painter->setFont(QFont("DejaVu Sans Mono", 7)); + + stu::Transform transform; + if (tf_manager_->GetTransform(target_frame_, stu::_wgs84_frame, transform)) + { + for (size_t i = 0; i < waypoints_.size(); i++) + { + tf2::Vector3 point(waypoints_[i].position.x, waypoints_[i].position.y, 0); + point = transform * point; + QPointF gl_point = map_canvas_->FixedFrameToMapGlCoord(QPointF(point.x(), point.y())); + QPointF corner(gl_point.x() - 20, gl_point.y() - 20); + QRectF rect(corner, QSizeF(40, 40)); + painter->drawText( + rect, + Qt::AlignHCenter | Qt::AlignVCenter, + QString::fromStdString(std::to_string(i + 1))); + } + } + + painter->restore(); + } + + void PlanRoutePlugin::LoadConfig(const YAML::Node& node, const std::string& path) + { + if (node["route_topic"]) + { + std::string route_topic = node["route_topic"].as(); + ui_.topic->setText(route_topic.c_str()); + } + if (node["color"]) + { + std::string color = node["color"].as(); + ui_.color->setColor(QColor(color.c_str())); + } + if (node["service"]) + { + std::string service = node["service"].as(); + ui_.service->setText(service.c_str()); + } + if (node["start_from_vehicle"]) + { + bool start_from_vehicle = node["start_from_vehicle"].as(); + ui_.start_from_vehicle->setChecked(start_from_vehicle); + } + + PlanRoute(); + } + + void PlanRoutePlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path) + { + std::string route_topic = ui_.topic->text().toStdString(); + emitter << YAML::Key << "route_topic" << YAML::Value << route_topic; + + std::string color = ui_.color->color().name().toStdString(); + emitter << YAML::Key << "color" << YAML::Value << color; + + std::string service = ui_.service->text().toStdString(); + emitter << YAML::Key << "service" << YAML::Value << service; + + bool start_from_vehicle = ui_.start_from_vehicle->isChecked(); + emitter << YAML::Key << "start_from_vehicle" << YAML::Value << start_from_vehicle; + } +} // namespace mapviz_plugins diff --git a/mapviz_plugins/src/point_click_publisher_plugin.cpp b/mapviz_plugins/src/point_click_publisher_plugin.cpp new file mode 100644 index 000000000..95e94cbee --- /dev/null +++ b/mapviz_plugins/src/point_click_publisher_plugin.cpp @@ -0,0 +1,254 @@ +// ***************************************************************************** +// +// Copyright (c) 2014-2020, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include +#include +#include + +// Declare plugin +#include + +// C++ Standard Libraries +#include +#include +#include + +PLUGINLIB_EXPORT_CLASS(mapviz_plugins::PointClickPublisherPlugin, mapviz::MapvizPlugin) + +namespace mapviz_plugins +{ + PointClickPublisherPlugin::PointClickPublisherPlugin() + : MapvizPlugin() + , ui_() + , config_widget_(new QWidget()) + , canvas_(nullptr) + { + ui_.setupUi(config_widget_); + + connect(&click_filter_, SIGNAL(pointClicked(const QPointF&)), + this, SLOT(pointClicked(const QPointF&))); + connect(ui_.topic, SIGNAL(textEdited(const QString&)), + this, SLOT(topicChanged(const QString&))); + + frame_timer_.start(1000); + connect(&frame_timer_, SIGNAL(timeout()), this, SLOT(updateFrames())); + } + + PointClickPublisherPlugin::~PointClickPublisherPlugin() + { + if (canvas_) + { + canvas_->removeEventFilter(&click_filter_); + } + } + + bool PointClickPublisherPlugin::Initialize(QGLWidget* canvas) + { + canvas_ = dynamic_cast(canvas); + canvas_->installEventFilter(&click_filter_); + + PrintInfo("Ready."); + + return true; + } + + void PointClickPublisherPlugin::Draw(double x, double y, double scale) + { + } + + void PointClickPublisherPlugin::LoadConfig(const YAML::Node& node, const std::string& path) + { + std::string tmp; + if (node["topic"]) + { + tmp = node["topic"].as(); + ui_.topic->setText(QString(tmp.c_str())); + topicChanged(ui_.topic->text()); + } + + if (node["output_frame"]) + { + tmp = node["output_frame"].as(); + ui_.outputframe->addItem(QString(tmp.c_str())); + } + } + + void PointClickPublisherPlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path) + { + emitter << YAML::Key << "topic" << YAML::Value << ui_.topic->text().toStdString(); + emitter << YAML::Key + << "output_frame" + << YAML::Value + << ui_.outputframe->currentText().toStdString(); + } + + QWidget* PointClickPublisherPlugin::GetConfigWidget(QWidget* parent) + { + config_widget_->setParent(parent); + + return config_widget_; + } + + + void PointClickPublisherPlugin::pointClicked(const QPointF& point) + { + QPointF transformed = canvas_->MapGlCoordToFixedFrame(point); + + std::string output_frame = ui_.outputframe->currentText().toStdString(); + + if (target_frame_ != output_frame) + { + swri_transform_util::Transform tf; + tf2::Vector3 tfPoint(transformed.x(), transformed.y(), 0.0); + if (tf_manager_->GetTransform(output_frame, target_frame_, tf)) + { + tfPoint = tf * tfPoint; + } else { + std::stringstream error; + error << "Unable to find transform from " << target_frame_ << " to " << output_frame << "."; + PrintError(error.str()); + return; + } + transformed.setX(tfPoint.x()); + transformed.setY(tfPoint.y()); + } + + std::unique_ptr stamped = + std::make_unique(); + stamped->header.frame_id = output_frame; + stamped->header.stamp = node_->get_clock()->now(); + stamped->point.x = transformed.x(); + stamped->point.y = transformed.y(); + stamped->point.z = 0.0; + + std::stringstream ss; + ss << "Point in " << output_frame.c_str() << ": " << transformed.x() << "," << transformed.y(); + + // Only publish if this plugin is visible + if(this->Visible()) + { + point_publisher_->publish(*stamped); + } + else + { + ss << " (but not publishing since plugin is hidden)"; + } + + PrintInfo(ss.str()); + } + + void PointClickPublisherPlugin::SetNode(rclcpp::Node& node) + { + mapviz::MapvizPlugin::SetNode(node); + + // We override this method so that we can initialize our publisher after + // our node has been set, ensuring that it's in mapviz's namespace. + topicChanged(ui_.topic->text()); + } + + void PointClickPublisherPlugin::PrintError(const std::string& message) + { + PrintErrorHelper(ui_.status, message); + } + + void PointClickPublisherPlugin::PrintInfo(const std::string& message) + { + PrintInfoHelper(ui_.status, message); + } + + void PointClickPublisherPlugin::PrintWarning(const std::string& message) + { + PrintWarningHelper(ui_.status, message); + } + + + void PointClickPublisherPlugin::topicChanged(const QString& topic) + { + std::stringstream ss; + ss << "Publishing points to topic: " << topic.toStdString().c_str(); + PrintInfo(ss.str()); + + if (!topic.isEmpty()) + { + point_publisher_ = node_->create_publisher( + topic.toStdString(), rclcpp::QoS(1000)); + } + } + + void PointClickPublisherPlugin::updateFrames() + { + std::vector frames; + tf_buf_->_getFrameStrings(frames); + + bool supports_wgs84 = tf_manager_->SupportsTransform( + swri_transform_util::_local_xy_frame, + swri_transform_util::_wgs84_frame); + + if (supports_wgs84) + { + frames.push_back(swri_transform_util::_wgs84_frame); + } + + if (ui_.outputframe->count() >= 0 && + static_cast(ui_.outputframe->count()) == frames.size()) + { + bool changed = false; + for (size_t i = 0; i < frames.size(); i++) + { + if (frames[i] != ui_.outputframe->itemText(static_cast(i)).toStdString()) + { + changed = true; + } + } + + if (!changed) + return; + } + + std::string current_output = ui_.outputframe->currentText().toStdString(); + + ui_.outputframe->clear(); + for (auto & frame : frames) + { + ui_.outputframe->addItem(frame.c_str()); + } + + if (!current_output.empty()) + { + int index = ui_.outputframe->findText(current_output.c_str()); + if (index < 0) + { + ui_.outputframe->addItem(current_output.c_str()); + } + + index = ui_.outputframe->findText(current_output.c_str()); + ui_.outputframe->setCurrentIndex(index); + } + } +} // namespace mapviz_plugins diff --git a/mapviz_plugins/src/point_drawing_plugin.cpp b/mapviz_plugins/src/point_drawing_plugin.cpp new file mode 100644 index 000000000..53169e01c --- /dev/null +++ b/mapviz_plugins/src/point_drawing_plugin.cpp @@ -0,0 +1,621 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include + +#include +#include +#include + +#include + +#include + +#include +#include +#include + +namespace mapviz_plugins +{ + PointDrawingPlugin::PointDrawingPlugin() + : MapvizPlugin() + , arrow_size_(25) + , draw_style_(LINES) + , position_tolerance_(0.0) + , buffer_size_(0) + , covariance_checked_(false) + , show_all_covariances_checked_(false) + , new_lap_(true) + , lap_checked_(false) + , buffer_holder_(false) + , scale_(1.0) + , static_arrow_sizes_(false) + , got_begin_(false) + { + QObject::connect(this, + SIGNAL(TargetFrameChanged(const std::string&)), + this, + SLOT(ResetTransformedPoints())); + } + + void PointDrawingPlugin::ClearHistory() + { + RCLCPP_INFO(node_->get_logger(), "PointDrawingPlugin::ClearHistory()"); + points_.clear(); + } + + void PointDrawingPlugin::DrawIcon() + { + if (icon_) + { + QPixmap icon(16, 16); + icon.fill(Qt::transparent); + + QPainter painter(&icon); + painter.setRenderHint(QPainter::Antialiasing, true); + + QPen pen(color_); + + if (draw_style_ == POINTS) + { + pen.setWidth(7); + pen.setCapStyle(Qt::RoundCap); + painter.setPen(pen); + painter.drawPoint(8, 8); + } else if (draw_style_ == LINES) { + pen.setWidth(3); + pen.setCapStyle(Qt::FlatCap); + painter.setPen(pen); + painter.drawLine(1, 14, 14, 1); + } else if (draw_style_ == ARROWS) { + pen.setWidth(2); + pen.setCapStyle(Qt::SquareCap); + painter.setPen(pen); + painter.drawLine(2, 13, 13, 2); + painter.drawLine(13, 2, 13, 8); + painter.drawLine(13, 2, 7, 2); + } + + icon_->SetPixmap(icon); + } + } + + void PointDrawingPlugin::SetArrowSize(int arrowSize) + { + arrow_size_ = arrowSize; + ResetTransformedPoints(); + } + + void PointDrawingPlugin::SetDrawStyle(QString style) + { + if (style == "lines") + { + draw_style_ = LINES; + } else if (style == "points") { + draw_style_ = POINTS; + } else if (style == "arrows") { + draw_style_ = ARROWS; + } + ResetTransformedPoints(); + DrawIcon(); + } + + void PointDrawingPlugin::SetDrawStyle(PointDrawingPlugin::DrawStyle style) + { + draw_style_ = style; + DrawIcon(); + } + + void PointDrawingPlugin::SetStaticArrowSizes(bool isChecked) + { + static_arrow_sizes_ = isChecked; + ResetTransformedPoints(); + } + + void PointDrawingPlugin::PositionToleranceChanged(double value) + { + position_tolerance_ = value; + } + + void PointDrawingPlugin::LapToggled(bool checked) + { + lap_checked_ = checked; + } + + void PointDrawingPlugin::CovariancedToggled(bool checked) + { + covariance_checked_ = checked; + } + + void PointDrawingPlugin::ShowAllCovariancesToggled(bool checked) + { + show_all_covariances_checked_ = checked; + } + + void PointDrawingPlugin::ResetTransformedPoints() + { + for (std::deque& lap : laps_) + { + for (StampedPoint& point : lap) + { + point.transformed = false; + } + } + for (StampedPoint& point : points_) + { + point.transformed = false; + } + Transform(); + } + + void PointDrawingPlugin::pushPoint(StampedPoint point) + { + cur_point_ = point; + + if (points_.empty() || + (point.point.distance(points_.back().point)) >= + (position_tolerance_)) + { + points_.push_back(std::move(point)); + } + + if (buffer_size_ > 0) + { + while (static_cast(points_.size()) >= buffer_size_) + { + points_.pop_front(); + } + } + } + + void PointDrawingPlugin::ClearPoints() + { + points_.clear(); + } + + double PointDrawingPlugin::bufferSize() const + { + if (!lap_checked_) + { + return buffer_size_; + } else { + return buffer_holder_; + } + } + + double PointDrawingPlugin::positionTolerance() const + { + return position_tolerance_; + } + + const std::deque &PointDrawingPlugin::points() const + { + return points_; + } + + void PointDrawingPlugin::BufferSizeChanged(int value) + { + buffer_size_ = value; + + if (buffer_size_ > 0) + { + while (static_cast(points_.size()) >= buffer_size_) + { + points_.pop_front(); + } + } + } + + bool PointDrawingPlugin::DrawPoints(double scale) + { + bool transformed = true; + + if (scale_ != scale && draw_style_ == ARROWS && static_arrow_sizes_) { + ResetTransformedPoints(); + } + scale_ = scale; + if (lap_checked_) { + CollectLaps(); + + if (draw_style_ == ARROWS) { + transformed &= DrawLapsArrows(); + } else { + transformed &= DrawLaps(); + } + } else if (buffer_size_ == INT_MAX) { + buffer_size_ = buffer_holder_; + laps_.clear(); + got_begin_ = false; + } + if (draw_style_ == ARROWS) { + transformed &= DrawArrows(); + } else { + transformed &= DrawLines(); + } + + return transformed; + } + + void PointDrawingPlugin::CollectLaps() + { + if (!got_begin_) + { + begin_ = cur_point_.point; + points_.clear(); + buffer_holder_ = buffer_size_; + buffer_size_ = INT_MAX; + got_begin_ = true; + } + tf2::Vector3 check = begin_ - cur_point_.point; + if (((std::fabs(check.x()) <= 3) && (std::fabs(check.y()) <= 3)) && + !new_lap_) + { + new_lap_ = true; + if (!points_.empty()) + { + laps_.push_back(points_); + laps_[0].pop_back(); + points_.clear(); + points_.push_back(cur_point_); + } + } + + if (((std::fabs(check.x()) > 25) && (std::fabs(check.y()) > 25)) && + new_lap_) + { + new_lap_ = false; + } + } + + bool PointDrawingPlugin::DrawLines() + { + bool success = cur_point_.transformed; + glColor4d(color_.redF(), color_.greenF(), color_.blueF(), 1.0); + if (draw_style_ == LINES && !points_.empty()) + { + glLineWidth(3); + glBegin(GL_LINE_STRIP); + } else { + glPointSize(6); + glBegin(GL_POINTS); + } + + for (const auto& pt : points_) + { + success &= pt.transformed; + if (pt.transformed) + { + glVertex2d(pt.transformed_point.getX(), pt.transformed_point.getY()); + } + } + + if (cur_point_.transformed) + { + glVertex2d(cur_point_.transformed_point.getX(), + cur_point_.transformed_point.getY()); + } + + glEnd(); + + return success; + } + + bool PointDrawingPlugin::DrawArrow(const StampedPoint& it) + { + if (it.transformed) + { + glVertex2d(it.transformed_point.getX(), + it.transformed_point.getY()); + + glVertex2d(it.transformed_arrow_point.getX(), + it.transformed_arrow_point.getY()); + + glVertex2d(it.transformed_arrow_point.getX(), + it.transformed_arrow_point.getY()); + glVertex2d(it.transformed_arrow_left.getX(), + it.transformed_arrow_left.getY()); + + glVertex2d(it.transformed_arrow_point.getX(), + it.transformed_arrow_point.getY()); + glVertex2d(it.transformed_arrow_right.getX(), + it.transformed_arrow_right.getY()); + return true; + } + return false; + } + + bool PointDrawingPlugin::DrawArrows() + { + bool success = true; + glLineWidth(4); + glBegin(GL_LINES); + glColor4d(color_.redF(), color_.greenF(), color_.blueF(), 0.5); + for (const auto &pt : points_) + { + success &= DrawArrow(pt); + } + + success &= DrawArrow(cur_point_); + + glEnd(); + + return success; + } + + void PointDrawingPlugin::SetColor(const QColor& color) + { + if (color != color_) + { + color_ = color; + DrawIcon(); + } + } + + bool PointDrawingPlugin::TransformPoint(StampedPoint& point) + { + if ( point.transformed ) + { + return true; + } + + swri_transform_util::Transform transform; + if( GetTransform(point.source_frame, point.stamp, transform)) + { + point.transformed_point = transform * point.point; + + if (draw_style_ == ARROWS) + { + tf2::Transform orientation(tf2::Transform(transform.GetOrientation()) * + point.orientation); + + double size = static_cast(arrow_size_); + if (static_arrow_sizes_) + { + size *= scale_; + } else { + size /= 10.0; + } + double arrow_width = size / 5.0; + double head_length = size * 0.75; + + // If quaternion malformed, just draw point instead + const tf2::Quaternion q(point.orientation); + if(std::fabs(q.x()*q.x() + q.y()*q.y() + q.z()*q.z() + q.w()*q.w() - 1) > 0.01) + { + orientation = tf2::Transform(tf2::Transform(transform.GetOrientation())); + arrow_width = 0.0; + head_length = 0.0; + size = 0; + } + + point.transformed_arrow_point = + point.transformed_point + orientation * tf2::Vector3(size, 0.0, 0.0); + point.transformed_arrow_left = + point.transformed_point + orientation * tf2::Vector3(head_length, -arrow_width, 0.0); + point.transformed_arrow_right = + point.transformed_point + orientation * tf2::Vector3(head_length, arrow_width, 0.0); + } + + if (covariance_checked_) + { + for (uint32_t i = 0; i < point.cov_points.size(); i++) + { + point.transformed_cov_points[i] = transform * point.cov_points[i]; + } + } + point.transformed = true; + return true; + } + point.transformed = false; + return false; + } + + void PointDrawingPlugin::Transform() + { + bool transformed = false; + + for (auto &pt : points_) + { + transformed = transformed | TransformPoint(pt); + } + + transformed = transformed | TransformPoint(cur_point_); + if (!laps_.empty()) + { + for (auto &lap : laps_) + { + for (auto &pt : lap) + { + transformed = transformed | TransformPoint(pt); + } + } + } + if (!points_.empty() && !transformed) + { + PrintError("No transform between " + cur_point_.source_frame + " and " + + target_frame_); + } + } + + bool PointDrawingPlugin::DrawLaps() + { + bool transformed = !points_.empty(); + glColor4d(color_.redF(), color_.greenF(), color_.blueF(), 0.5); + glLineWidth(3); + QColor base_color = color_; + + for (size_t i = 0; i < laps_.size(); i++) + { + UpdateColor(base_color, static_cast(i)); + if (draw_style_ == LINES) + { + glLineWidth(3); + glBegin(GL_LINE_STRIP); + } else { + glPointSize(6); + glBegin(GL_POINTS); + } + + for (const auto& pt : laps_[i]) + { + if (pt.transformed) + { + glVertex2d(pt.transformed_point.getX(), + pt.transformed_point.getY()); + } + } + glEnd(); + } + + if (draw_style_ == LINES) + { + glLineWidth(3); + glBegin(GL_LINE_STRIP); + } else { + glPointSize(6); + glBegin(GL_POINTS); + } + + glColor4d(base_color.redF(), base_color.greenF(), base_color.blueF(), 0.5); + + if (!points_.empty()) + { + for (const auto &pt : points_) + { + transformed &= pt.transformed; + if (pt.transformed) + { + glVertex2d(pt.transformed_point.getX(), + pt.transformed_point.getY()); + } + } + } + + glEnd(); + return transformed; + } + + void PointDrawingPlugin::UpdateColor(QColor base_color, int i) + { + int hue = static_cast(color_.hue() + (i + 1.0) * 10.0 * M_PI); + if (hue > 360) + { + hue %= 360; + } + int sat = color_.saturation(); + int v = color_.value(); + base_color.setHsv(hue, sat, v); + glColor4d(base_color.redF(), base_color.greenF(), base_color.blueF(), + 0.5); + } + + void PointDrawingPlugin::DrawCovariance() + { + glLineWidth(4); + + glColor4d(color_.redF(), color_.greenF(), color_.blueF(), 1.0); + + if (show_all_covariances_checked_) + { + for (const auto &pt : points_) + { + if (!pt.transformed || pt.transformed_cov_points.empty()) + { + continue; + } + glBegin(GL_LINE_STRIP); + + for (const auto & transformed_cov_point : pt.transformed_cov_points) + { + glVertex2d(transformed_cov_point.getX(), + transformed_cov_point.getY()); + } + + glVertex2d(pt.transformed_cov_points.front().getX(), + pt.transformed_cov_points.front().getY()); + + glEnd(); + } + } else if (cur_point_.transformed && !cur_point_.transformed_cov_points.empty()) { + glBegin(GL_LINE_STRIP); + + for (auto & transformed_cov_point : cur_point_.transformed_cov_points) + { + glVertex2d(transformed_cov_point.getX(), + transformed_cov_point.getY()); + } + + glVertex2d(cur_point_.transformed_cov_points.front().getX(), + cur_point_.transformed_cov_points.front().getY()); + + glEnd(); + } + } + + bool PointDrawingPlugin::DrawLapsArrows() + { + bool success = !laps_.empty() && !points_.empty(); + glColor4d(color_.redF(), color_.greenF(), color_.blueF(), 0.5); + glLineWidth(2); + QColor base_color = color_; + if (!laps_.empty()) + { + for (size_t i = 0; i < laps_.size(); i++) + { + UpdateColor(base_color, static_cast(i)); + for (const auto &pt : laps_[i]) + { + glBegin(GL_LINE_STRIP); + success &= DrawArrow(pt); + glEnd(); + } + } + glEnd(); + + int hue = static_cast(color_.hue() + laps_.size() * 10.0 * M_PI); + int sat = color_.saturation(); + int v = color_.value(); + base_color.setHsv(hue, sat, v); + glColor4d(base_color.redF(), base_color.greenF(), base_color.blueF(), + 0.5); + } + + if (!points_.empty()) + { + for (const auto& pt : points_) + { + glBegin(GL_LINE_STRIP); + success &= DrawArrow(pt); + glEnd(); + } + } + + return success; + } +} // namespace mapviz_plugins diff --git a/mapviz_plugins/src/pointcloud2_plugin.cpp b/mapviz_plugins/src/pointcloud2_plugin.cpp new file mode 100644 index 000000000..5dd29a7a3 --- /dev/null +++ b/mapviz_plugins/src/pointcloud2_plugin.cpp @@ -0,0 +1,888 @@ +// ***************************************************************************** +// +// Copyright (c) 2015, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include +#include + +// Boost libraries +#include + +// QT libraries +#include +#include + +// ROS libraries +#include +#include + +#include + +// Declare plugin +#include + +// C++ standard libraries +#include +#include +#include +#include +#include + +// QT Autogenerated +#include "ui_topic_select.h" + +PLUGINLIB_EXPORT_CLASS(mapviz_plugins::PointCloud2Plugin, mapviz::MapvizPlugin) + +namespace mapviz_plugins +{ + PointCloud2Plugin::PointCloud2Plugin() + : MapvizPlugin() + , ui_() + , config_widget_(new QWidget()) + , topic_("") + , alpha_(1.0) + , max_value_(100.0) + , min_value_(0.0) + , point_size_(3) + , buffer_size_(1) + , new_topic_(true) + , has_message_(false) + , num_of_feats_(0) + , need_new_list_(true) + , need_minmax_(false) + { + ui_.setupUi(config_widget_); + + // Set background white + QPalette p(config_widget_->palette()); + p.setColor(QPalette::Window, Qt::white); + config_widget_->setPalette(p); + + // Set status text red + QPalette p3(ui_.status->palette()); + p3.setColor(QPalette::Text, Qt::red); + ui_.status->setPalette(p3); + + // Initialize color selector colors + ui_.min_color->setColor(Qt::white); + ui_.max_color->setColor(Qt::black); + // Set color transformer choices + ui_.color_transformer->addItem(QString("Flat Color"), QVariant(0)); + + QObject::connect(ui_.selecttopic, + SIGNAL(clicked()), + this, + SLOT(SelectTopic())); + QObject::connect(ui_.buttonResetBuffer, + SIGNAL(clicked()), + this, + SLOT(ClearPointClouds())); + QObject::connect(ui_.topic, + SIGNAL(editingFinished()), + this, + SLOT(TopicEdited())); + QObject::connect(ui_.alpha, + SIGNAL(valueChanged(double)), + this, + SLOT(AlphaEdited(double))); + QObject::connect(ui_.color_transformer, + SIGNAL(currentIndexChanged(int)), + this, + SLOT(ColorTransformerChanged(int))); + QObject::connect(ui_.max_color, + SIGNAL(colorEdited(const QColor &)), + this, + SLOT(UpdateColors())); + QObject::connect(ui_.min_color, + SIGNAL(colorEdited(const QColor &)), + this, + SLOT(UpdateColors())); + QObject::connect(ui_.minValue, + SIGNAL(valueChanged(double)), + this, + SLOT(MinValueChanged(double))); + QObject::connect(ui_.maxValue, + SIGNAL(valueChanged(double)), + this, + SLOT(MaxValueChanged(double))); + QObject::connect(ui_.bufferSize, + SIGNAL(valueChanged(int)), + this, + SLOT(BufferSizeChanged(int))); + QObject::connect(ui_.pointSize, + SIGNAL(valueChanged(int)), + this, + SLOT(PointSizeChanged(int))); + QObject::connect(ui_.use_rainbow, + SIGNAL(stateChanged(int)), + this, + SLOT(UseRainbowChanged(int))); + QObject::connect(ui_.unpack_rgb, + SIGNAL(stateChanged(int)), + this, + SLOT(UseRainbowChanged(int))); + QObject::connect(ui_.use_automaxmin, + SIGNAL(stateChanged(int)), + this, + SLOT(UseAutomaxminChanged(int))); + QObject::connect(ui_.max_color, + SIGNAL(colorEdited(const QColor &)), + this, + SLOT(DrawIcon())); + QObject::connect(ui_.min_color, + SIGNAL(colorEdited( const QColor &)), + this, + SLOT(DrawIcon())); + QObject::connect(this, + SIGNAL(TargetFrameChanged(const std::string&)), + this, + SLOT(ResetTransformedPointClouds())); + QObject::connect(this, + SIGNAL(VisibleChanged(bool)), + this, + SLOT(SetSubscription(bool))); + } + + void PointCloud2Plugin::ClearHistory() + { + RCLCPP_DEBUG(node_->get_logger(), "PointCloud2Plugin::ClearHistory()"); + scans_.clear(); + } + + void PointCloud2Plugin::DrawIcon() + { + if (icon_) + { + QPixmap icon(16, 16); + icon.fill(Qt::transparent); + + QPainter painter(&icon); + painter.setRenderHint(QPainter::Antialiasing, true); + + QPen pen; + pen.setWidth(4); + pen.setCapStyle(Qt::RoundCap); + + pen.setColor(ui_.min_color->color()); + painter.setPen(pen); + painter.drawPoint(2, 13); + + pen.setColor(ui_.min_color->color()); + painter.setPen(pen); + painter.drawPoint(4, 6); + + pen.setColor(ui_.max_color->color()); + painter.setPen(pen); + painter.drawPoint(12, 9); + + pen.setColor(ui_.max_color->color()); + painter.setPen(pen); + painter.drawPoint(13, 2); + + icon_->SetPixmap(icon); + } + } + + void PointCloud2Plugin::ResetTransformedPointClouds() + { + QMutexLocker locker(&scan_mutex_); + for (Scan& scan : scans_) + { + scan.transformed = false; + scan.gl_color.clear(); + scan.gl_point.clear(); + } + } + + void PointCloud2Plugin::ClearPointClouds() + { + QMutexLocker locker(&scan_mutex_); + scans_.clear(); + } + + void PointCloud2Plugin::SetSubscription(bool subscribe) + { + pc2_sub_.reset(); + + if (subscribe && !topic_.empty()) + { + pc2_sub_ = node_->create_subscription( + topic_, + rclcpp::QoS(10), + std::bind(&PointCloud2Plugin::PointCloud2Callback, this, std::placeholders::_1) + ); + new_topic_ = true; + need_new_list_ = true; + max_.clear(); + min_.clear(); + } + } + + QColor PointCloud2Plugin::CalculateColor(const StampedPoint& point) + { + float val; + unsigned int color_transformer = + static_cast(ui_.color_transformer->currentIndex()); + unsigned int transformer_index = color_transformer -1; + if (num_of_feats_ > 0 && color_transformer > 0) + { + val = point.features[transformer_index]; + if (need_minmax_) + { + if (val > max_[transformer_index]) + { + max_[transformer_index] = val; + } + + if (val < min_[transformer_index]) + { + min_[transformer_index] = val; + } + } + } else { + // No intensity or (color_transformer == COLOR_FLAT) + return ui_.min_color->color(); + } + + if (ui_.unpack_rgb->isChecked()) + { + uint8_t* pixelColor = reinterpret_cast(&val); + return QColor(pixelColor[2], pixelColor[1], pixelColor[0], 255); + } + + if (max_value_ > min_value_) + { + val = (val - min_value_) / (max_value_ - min_value_); + } + val = std::max(0.0f, std::min(val, 1.0f)); + + if (ui_.use_automaxmin->isChecked()) + { + max_value_ = max_[transformer_index]; + min_value_ = min_[transformer_index]; + } + + if (ui_.use_rainbow->isChecked()) + { // Hue Interpolation + int hue = static_cast(val * 255.0); + return QColor::fromHsl(hue, 255, 127, 255); + } else { + const QColor min_color = ui_.min_color->color(); + const QColor max_color = ui_.max_color->color(); + // RGB Interpolation + int red, green, blue; + red = static_cast(val * max_color.red() + ((1.0 - val) * min_color.red())); + green = static_cast(val * max_color.green() + ((1.0 - val) * min_color.green())); + blue = static_cast(val * max_color.blue() + ((1.0 - val) * min_color.blue())); + return QColor(red, green, blue, 255); + } + } + + inline int32_t findChannelIndex( + const sensor_msgs::msg::PointCloud2::SharedPtr cloud, + const std::string& channel) + { + for (int32_t i = 0; static_cast(i) < cloud->fields.size(); ++i) + { + if (cloud->fields[i].name == channel) + { + return i; + } + } + + return -1; + } + + void PointCloud2Plugin::UpdateColors() + { + { + QMutexLocker locker(&scan_mutex_); + for (Scan& scan : scans_) + { + scan.gl_color.clear(); + scan.gl_color.reserve(scan.points.size()*4); + for (const StampedPoint& point : scan.points) + { + const QColor color = CalculateColor(point); + scan.gl_color.push_back( color.red()); + scan.gl_color.push_back( color.green()); + scan.gl_color.push_back( color.blue()); + scan.gl_color.push_back( static_cast(alpha_ * 255.0 ) ); + } + } + } + canvas_->update(); + } + + void PointCloud2Plugin::SelectTopic() + { + std::string topic = mapviz::SelectTopicDialog::selectTopic( + node_, + "sensor_msgs/msg/PointCloud2"); + + if (!topic.empty()) + { + ui_.topic->setText(QString::fromStdString(topic)); + TopicEdited(); + } + } + + + void PointCloud2Plugin::TopicEdited() + { + std::string topic = ui_.topic->text().trimmed().toStdString(); + if (topic != topic_) + { + initialized_ = false; + { + QMutexLocker locker(&scan_mutex_); + scans_.clear(); + } + has_message_ = false; + PrintWarning("No messages received."); + + topic_ = topic; + SetSubscription(this->Visible()); + } + } + + void PointCloud2Plugin::MinValueChanged(double value) + { + min_value_ = value; + UpdateColors(); + } + + void PointCloud2Plugin::MaxValueChanged(double value) + { + max_value_ = value; + UpdateColors(); + } + + void PointCloud2Plugin::BufferSizeChanged(int value) + { + buffer_size_ = (size_t)value; + + if (buffer_size_ > 0) + { + QMutexLocker locker(&scan_mutex_); + while (scans_.size() > buffer_size_) + { + scans_.pop_front(); + } + } + + canvas_->update(); + } + + void PointCloud2Plugin::PointSizeChanged(int value) + { + point_size_ = (size_t)value; + + canvas_->update(); + } + + void PointCloud2Plugin::PointCloud2Callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) + { + if (!has_message_) + { + initialized_ = true; + has_message_ = true; + } + + // Note that unlike some plugins, this one does not store nor rely on the + // source_frame_ member variable. This one can potentially store many + // messages with different source frames, so we need to store and transform + // them individually. + + Scan scan; + { + // recycle already allocated memory, reusing an old scan + QMutexLocker locker(&scan_mutex_); + if (buffer_size_ > 0 ) + { + if( scans_.size() >= buffer_size_) + { + scan = std::move( scans_.front() ); + } else { + glGenBuffers(1, &scan.color_vbo); + glGenBuffers(1, &scan.point_vbo); + } + while (scans_.size() >= buffer_size_) + { + scans_.pop_front(); + } + } + } + + scan.stamp = msg->header.stamp; + scan.color = QColor::fromRgbF(1.0f, 0.0f, 0.0f, 1.0f); + scan.source_frame = msg->header.frame_id; + scan.transformed = true; + + swri_transform_util::Transform transform; + if (!GetTransform(scan.source_frame, msg->header.stamp, transform)) + { + scan.transformed = false; + PrintError("No transform between " + scan.source_frame + " and " + target_frame_); + return; + } + + int32_t xi = findChannelIndex(msg, "x"); + int32_t yi = findChannelIndex(msg, "y"); + int32_t zi = findChannelIndex(msg, "z"); + + if (xi == -1 || yi == -1 || zi == -1) + { + return; + } + + if (new_topic_) + { + for (auto & field : msg->fields) + { + FieldInfo input; + std::string name = field.name; + + uint32_t offset_value = field.offset; + uint8_t datatype_value = field.datatype; + input.offset = offset_value; + input.datatype = datatype_value; + scan.new_features.insert(std::pair(name, input)); + } + + new_topic_ = false; + num_of_feats_ = scan.new_features.size(); + + max_.resize(num_of_feats_); + min_.resize(num_of_feats_); + + int label = 1; + if (need_new_list_) + { + int new_feature_index = ui_.color_transformer->currentIndex(); + std::map::const_iterator it; + for (it = scan.new_features.begin(); it != scan.new_features.end(); ++it) + { + ui_.color_transformer->removeItem(static_cast(num_of_feats_)); + num_of_feats_--; + } + + for (it = scan.new_features.begin(); it != scan.new_features.end(); ++it) + { + std::string const field = it->first; + if (field == saved_color_transformer_) + { + // The very first time we see a new set of features, that means the + // plugin was just created; if we have a saved value, set the current + // index to that and clear the saved value. + new_feature_index = label; + saved_color_transformer_ = ""; + } + + ui_.color_transformer->addItem(QString::fromStdString(field), QVariant(label)); + num_of_feats_++; + label++; + } + ui_.color_transformer->setCurrentIndex(new_feature_index); + need_new_list_ = false; + } + } + + if (!msg->data.empty()) + { + const uint8_t* ptr = &msg->data.front(); + const uint32_t point_step = msg->point_step; + const uint32_t xoff = msg->fields[xi].offset; + const uint32_t yoff = msg->fields[yi].offset; + const uint32_t zoff = msg->fields[zi].offset; + const size_t num_points = msg->data.size() / point_step; + const size_t num_features = scan.new_features.size(); + scan.points.resize(num_points); + + std::vector field_infos; + field_infos.reserve(num_features); + for (auto & new_feature : scan.new_features) + { + field_infos.push_back(new_feature.second); + } + + scan.gl_point.clear(); + scan.gl_point.reserve(num_points*2); + scan.gl_color.clear(); + scan.gl_color.reserve(num_points*4); + + for (size_t i = 0; i < num_points; i++, ptr += point_step) + { + float x = *reinterpret_cast(ptr + xoff); + float y = *reinterpret_cast(ptr + yoff); + float z = *reinterpret_cast(ptr + zoff); + + StampedPoint& point = scan.points[i]; + point.point = tf2::Vector3(x, y, z); + + point.features.resize(num_features); + + for (int count=0; count < field_infos.size(); count++) + { + point.features[count] = PointFeature(ptr, field_infos[count]); + } + if (scan.transformed) + { + const tf2::Vector3 transformed_point = transform * point.point; + scan.gl_point.push_back( transformed_point.getX() ); + scan.gl_point.push_back( transformed_point.getY() ); + } + const QColor color = CalculateColor(point); + scan.gl_color.push_back( color.red()); + scan.gl_color.push_back( color.green()); + scan.gl_color.push_back( color.blue()); + scan.gl_color.push_back( static_cast(alpha_ * 255.0 ) ); + } + } + + { + QMutexLocker locker(&scan_mutex_); + scans_.push_back( std::move(scan) ); + } + new_topic_ = true; + canvas_->update(); + } + + float PointCloud2Plugin::PointFeature(const uint8_t* data, const FieldInfo& feature_info) + { + switch (feature_info.datatype) + { + case 1: + return *reinterpret_cast(data + feature_info.offset); + case 2: + return *(data + feature_info.offset); + case 3: + return *reinterpret_cast(data + feature_info.offset); + case 4: + return *reinterpret_cast(data + feature_info.offset); + case 5: + return *reinterpret_cast(data + feature_info.offset); + case 6: + return *reinterpret_cast(data + feature_info.offset); + case 7: + return *reinterpret_cast(data + feature_info.offset); + case 8: + return static_cast(*reinterpret_cast(data + feature_info.offset)); + default: + RCLCPP_WARN(node_->get_logger(), "Unknown data type in point: %d", feature_info.datatype); + return 0.0; + } + } + + void PointCloud2Plugin::PrintError(const std::string& message) + { + PrintErrorHelper(ui_.status, message); + } + + void PointCloud2Plugin::PrintInfo(const std::string& message) + { + PrintInfoHelper(ui_.status, message); + } + + void PointCloud2Plugin::PrintWarning(const std::string& message) + { + PrintWarningHelper(ui_.status, message); + } + + QWidget* PointCloud2Plugin::GetConfigWidget(QWidget* parent) + { + config_widget_->setParent(parent); + + return config_widget_; + } + + bool PointCloud2Plugin::Initialize(QGLWidget* canvas) + { + canvas_ = canvas; + + DrawIcon(); + + return true; + } + + void PointCloud2Plugin::Draw(double x, double y, double scale) + { + glPointSize(point_size_); + + glEnableClientState(GL_VERTEX_ARRAY); + glEnableClientState(GL_COLOR_ARRAY); + + { + QMutexLocker locker(&scan_mutex_); + + for (Scan& scan : scans_) + { + if (scan.transformed && !scan.gl_color.empty()) + { + glBindBuffer(GL_ARRAY_BUFFER, scan.point_vbo); // coordinates + glBufferData( + GL_ARRAY_BUFFER, + scan.gl_point.size() * sizeof(float), + scan.gl_point.data(), + GL_STATIC_DRAW); + glVertexPointer( 2, GL_FLOAT, 0, nullptr); + + glBindBuffer(GL_ARRAY_BUFFER, scan.color_vbo); // color + glBufferData( + GL_ARRAY_BUFFER, + scan.gl_color.size() * sizeof(uint8_t), + scan.gl_color.data(), + GL_STATIC_DRAW); + glColorPointer( 4, GL_UNSIGNED_BYTE, 0, nullptr); + + glDrawArrays(GL_POINTS, 0, scan.gl_point.size() / 2 ); + } + } + } + glDisableClientState(GL_VERTEX_ARRAY); + glDisableClientState(GL_COLOR_ARRAY); + glBindBuffer(GL_ARRAY_BUFFER, 0); + + PrintInfo("OK"); + } + + void PointCloud2Plugin::UseRainbowChanged(int check_state) + { + UpdateMinMaxWidgets(); + UpdateColors(); + } + + void PointCloud2Plugin::UseAutomaxminChanged(int check_state) + { + need_minmax_ = check_state == Qt::Checked; + if( !need_minmax_ ) + { + min_value_ = ui_.minValue->value(); + max_value_ = ui_.maxValue->value(); + } + + UpdateMinMaxWidgets(); + UpdateColors(); + } + + void PointCloud2Plugin::Transform() + { + { + QMutexLocker locker(&scan_mutex_); + + bool was_using_latest_transforms = use_latest_transforms_; + use_latest_transforms_ = false; + for (Scan& scan : scans_) + { + if (!scan.transformed) + { + swri_transform_util::Transform transform; + if (GetTransform(scan.source_frame, scan.stamp, transform)) + { + scan.gl_point.clear(); + scan.gl_point.reserve(scan.points.size()*2); + + scan.transformed = true; + for (StampedPoint& point : scan.points) + { + const tf2::Vector3 transformed_point = transform * point.point; + scan.gl_point.push_back( transformed_point.getX() ); + scan.gl_point.push_back( transformed_point.getY() ); + } + } else { + RCLCPP_WARN(node_->get_logger(), "Unable to get transform."); + scan.transformed = false; + } + } + } + use_latest_transforms_ = was_using_latest_transforms; + } + // Z color is based on transformed color, so it is dependent on the + // transform + if (ui_.color_transformer->currentIndex() == COLOR_Z) + { + UpdateColors(); + } + } + + void PointCloud2Plugin::LoadConfig(const YAML::Node& node, + const std::string& path) + { + if (node["topic"]) + { + std::string topic = node["topic"].as(); + ui_.topic->setText(boost::trim_copy(topic).c_str()); + TopicEdited(); + } + + if (node["size"]) + { + point_size_ = node["size"].as(); + ui_.pointSize->setValue(static_cast(point_size_)); + } + + if (node["buffer_size"]) + { + buffer_size_ = node["buffer_size"].as(); + ui_.bufferSize->setValue(static_cast(buffer_size_)); + } + + if (node["color_transformer"]) + { + saved_color_transformer_ = node["color_transformer"].as(); + } + + if (node["min_color"]) + { + std::string min_color_str = node["min_color"].as(); + ui_.min_color->setColor(QColor(min_color_str.c_str())); + } + + if (node["max_color"]) + { + std::string max_color_str = node["max_color"].as(); + ui_.max_color->setColor(QColor(max_color_str.c_str())); + } + + if (node["value_min"]) + { + min_value_ = node["value_min"].as(); + ui_.minValue->setValue(min_value_); + } + + if (node["value_max"]) + { + max_value_ = node["value_max"].as(); + ui_.maxValue->setValue(max_value_); + } + + if (node["alpha"]) + { + alpha_ = node["alpha"].as(); + ui_.alpha->setValue(alpha_); + } + + if (node["use_rainbow"]) + { + bool use_rainbow = node["use_rainbow"].as(); + ui_.use_rainbow->setChecked(use_rainbow); + } + + if (node["unpack_rgb"]) + { + bool unpack_rgb = node["unpack_rgb"].as(); + ui_.unpack_rgb->setChecked(unpack_rgb); + } + + // UseRainbowChanged must be called *before* ColorTransformerChanged + UseRainbowChanged(ui_.use_rainbow->checkState()); + + if (node["use_automaxmin"]) + { + bool use_automaxmin = node["use_automaxmin"].as(); + ui_.use_automaxmin->setChecked(use_automaxmin); + } + // UseRainbowChanged must be called *before* ColorTransformerChanged + UseAutomaxminChanged(ui_.use_automaxmin->checkState()); + // ColorTransformerChanged will also update colors of all points + ColorTransformerChanged(ui_.color_transformer->currentIndex()); + } + + void PointCloud2Plugin::ColorTransformerChanged(int index) + { + RCLCPP_DEBUG(node_->get_logger(), "Color transformer changed to %d", index); + UpdateMinMaxWidgets(); + UpdateColors(); + } + + void PointCloud2Plugin::UpdateMinMaxWidgets() + { + bool color_is_flat = ui_.color_transformer->currentIndex() == COLOR_FLAT; + + if (color_is_flat) + { + ui_.maxColorLabel->hide(); + ui_.max_color->hide(); + ui_.minColorLabel->hide(); + ui_.min_max_color_widget->show(); + ui_.min_max_value_widget->hide(); + ui_.use_automaxmin->hide(); + ui_.use_rainbow->hide(); + } else { + ui_.maxColorLabel->show(); + ui_.max_color->show(); + ui_.minColorLabel->show(); + ui_.min_max_color_widget->setVisible(!ui_.use_rainbow->isChecked()); + ui_.min_max_value_widget->setVisible(!ui_.use_automaxmin->isChecked()); + ui_.use_automaxmin->show(); + ui_.use_rainbow->show(); + } + + config_widget_->updateGeometry(); + config_widget_->adjustSize(); + + Q_EMIT SizeChanged(); + } + + /** + * Coerces alpha to [0.0, 1.0] and stores it in alpha_ + */ + void PointCloud2Plugin::AlphaEdited(double value) + { + alpha_ = std::max(0.0f, std::min(static_cast(value), 1.0f)); + } + + void PointCloud2Plugin::SaveConfig(YAML::Emitter& emitter, + const std::string& path) + { + emitter << YAML::Key << "topic" << + YAML::Value << boost::trim_copy(ui_.topic->text().toStdString()); + emitter << YAML::Key << "size" << + YAML::Value << ui_.pointSize->value(); + emitter << YAML::Key << "buffer_size" << + YAML::Value << ui_.bufferSize->value(); + emitter << YAML::Key << "alpha" << + YAML::Value << alpha_; + emitter << YAML::Key << "color_transformer" << + YAML::Value << ui_.color_transformer->currentText().toStdString(); + emitter << YAML::Key << "min_color" << + YAML::Value << ui_.min_color->color().name().toStdString(); + emitter << YAML::Key << "max_color" << + YAML::Value << ui_.max_color->color().name().toStdString(); + emitter << YAML::Key << "value_min" << + YAML::Value << ui_.minValue->value(); + emitter << YAML::Key << "value_max" << + YAML::Value << ui_.maxValue->value(); + emitter << YAML::Key << "use_rainbow" << + YAML::Value << ui_.use_rainbow->isChecked(); + emitter << YAML::Key << "use_automaxmin" << + YAML::Value << ui_.use_automaxmin->isChecked(); + emitter << YAML::Key << "unpack_rgb" << + YAML::Value << ui_.unpack_rgb->isChecked(); + } +} // namespace mapviz_plugins diff --git a/mapviz_plugins/src/pose_plugin.cpp b/mapviz_plugins/src/pose_plugin.cpp new file mode 100644 index 000000000..a784803c9 --- /dev/null +++ b/mapviz_plugins/src/pose_plugin.cpp @@ -0,0 +1,299 @@ +/** + * Copyright 2019 Hatchbed L.L.C. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + **/ + +#include + +// QT libraries +#include +#include +#include + +#include + +// ROS libraries +#include + +#include +#include + +// Declare plugin +#include + +// C++ standard libraries +#include +#include +#include +#include + +PLUGINLIB_EXPORT_CLASS(mapviz_plugins::PosePlugin, mapviz::MapvizPlugin) + +namespace mapviz_plugins +{ + PosePlugin::PosePlugin() + : PointDrawingPlugin() + , ui_() + , config_widget_(new QWidget()) + , has_message_(false) + { + ui_.setupUi(config_widget_); + + ui_.color->setColor(Qt::green); + + // Set background white + QPalette p(config_widget_->palette()); + p.setColor(QPalette::Window, Qt::white); + config_widget_->setPalette(p); + + // Set status text red + QPalette p3(ui_.status->palette()); + p3.setColor(QPalette::Text, Qt::red); + ui_.status->setPalette(p3); + + QObject::connect(ui_.selecttopic, SIGNAL(clicked()), this, + SLOT(SelectTopic())); + QObject::connect(ui_.topic, SIGNAL(editingFinished()), this, + SLOT(TopicEdited())); + QObject::connect(ui_.positiontolerance, SIGNAL(valueChanged(double)), this, + SLOT(PositionToleranceChanged(double))); + QObject::connect(ui_.buffersize, SIGNAL(valueChanged(int)), this, + SLOT(BufferSizeChanged(int))); + QObject::connect(ui_.drawstyle, SIGNAL(activated(QString)), this, + SLOT(SetDrawStyle(QString))); + QObject::connect(ui_.static_arrow_sizes, SIGNAL(clicked(bool)), + this, SLOT(SetStaticArrowSizes(bool))); + QObject::connect(ui_.arrow_size, SIGNAL(valueChanged(int)), + this, SLOT(SetArrowSize(int))); + QObject::connect(ui_.color, SIGNAL(colorEdited(const QColor&)), this, + SLOT(SetColor(const QColor&))); + QObject::connect(ui_.show_laps, SIGNAL(toggled(bool)), this, + SLOT(LapToggled(bool))); + QObject::connect(ui_.buttonResetBuffer, SIGNAL(pressed()), this, + SLOT(ClearPoints())); + } + + void PosePlugin::SelectTopic() + { + std::string topic = + mapviz::SelectTopicDialog::selectTopic(node_, "geometry_msgs/msg/PoseStamped"); + + if (!topic.empty()) + { + ui_.topic->setText(QString::fromStdString(topic)); + TopicEdited(); + } + } + + void PosePlugin::TopicEdited() + { + std::string topic = ui_.topic->text().trimmed().toStdString(); + if (topic != topic_) + { + initialized_ = false; + ClearPoints(); + has_message_ = false; + PrintWarning("No messages received."); + + // pose_sub_.shutdown(); + pose_sub_.reset(); + + topic_ = topic; + if (!topic.empty()) + { + pose_sub_ = node_->create_subscription( + topic_, + rclcpp::QoS(1), + std::bind(&PosePlugin::PoseCallback, this, std::placeholders::_1)); + + RCLCPP_INFO(node_->get_logger(), "Subscribing to %s", topic_.c_str()); + } + } + } + + void PosePlugin::PoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr pose) + { + if (!has_message_) + { + initialized_ = true; + has_message_ = true; + } + + StampedPoint stamped_point; + stamped_point.stamp = pose->header.stamp; + stamped_point.source_frame = pose->header.frame_id; + + stamped_point.point = tf2::Vector3(pose->pose.position.x, + pose->pose.position.y, + pose->pose.position.z); + + stamped_point.orientation = tf2::Quaternion( + pose->pose.orientation.x, + pose->pose.orientation.y, + pose->pose.orientation.z, + pose->pose.orientation.w); + + pushPoint( std::move(stamped_point) ); + } + + void PosePlugin::PrintError(const std::string& message) + { + PrintErrorHelper(ui_.status, message); + } + + void PosePlugin::PrintInfo(const std::string& message) + { + PrintInfoHelper(ui_.status, message); + } + + void PosePlugin::PrintWarning(const std::string& message) + { + PrintWarningHelper(ui_.status, message); + } + + QWidget* PosePlugin::GetConfigWidget(QWidget* parent) + { + config_widget_->setParent(parent); + + return config_widget_; + } + + bool PosePlugin::Initialize(QGLWidget* canvas) + { + canvas_ = canvas; + SetColor(ui_.color->color()); + + return true; + } + + void PosePlugin::Draw(double x, double y, double scale) + { + if (DrawPoints(scale)) + { + PrintInfo("OK"); + } + } + + void PosePlugin::LoadConfig(const YAML::Node& node, const std::string& path) + { + if (node["topic"]) + { + std::string topic = node["topic"].as(); + ui_.topic->setText(topic.c_str()); + } + + if (node["color"]) + { + std::string color = node["color"].as(); + QColor qcolor(color.c_str()); + SetColor(qcolor); + ui_.color->setColor(qcolor); + } + + if (node["draw_style"]) + { + std::string draw_style = node["draw_style"].as(); + + if (draw_style == "lines") + { + ui_.drawstyle->setCurrentIndex(0); + SetDrawStyle( LINES ); + } else if (draw_style == "points") { + ui_.drawstyle->setCurrentIndex(1); + SetDrawStyle( POINTS ); + } else if (draw_style == "arrows") { + ui_.drawstyle->setCurrentIndex(2); + SetDrawStyle( ARROWS ); + } + } + + if (node["position_tolerance"]) + { + double position_tolerance = node["position_tolerance"].as(); + ui_.positiontolerance->setValue(position_tolerance); + PositionToleranceChanged(position_tolerance); + } + + if (node["buffer_size"]) + { + int buffer_size = node["buffer_size"].as(); + ui_.buffersize->setValue(buffer_size); + BufferSizeChanged(buffer_size); + } + + if (node["show_laps"]) + { + bool show_laps = node["show_laps"].as(); + ui_.show_laps->setChecked(show_laps); + LapToggled(show_laps); + } + + if (node["static_arrow_sizes"]) + { + bool static_arrow_sizes = node["static_arrow_sizes"].as(); + ui_.static_arrow_sizes->setChecked(static_arrow_sizes); + SetStaticArrowSizes(static_arrow_sizes); + } + + if (node["arrow_size"]) + { + int arrow_size = node["arrow_size"].as(); + ui_.arrow_size->setValue(arrow_size); + SetArrowSize(arrow_size); + } + + TopicEdited(); + } + + void PosePlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path) + { + std::string topic = ui_.topic->text().toStdString(); + emitter << YAML::Key << "topic" << YAML::Value << topic; + + emitter << YAML::Key << "color" << YAML::Value + << ui_.color->color().name().toStdString(); + + std::string draw_style = ui_.drawstyle->currentText().toStdString(); + emitter << YAML::Key << "draw_style" << YAML::Value << draw_style; + + emitter << YAML::Key << "position_tolerance" << + YAML::Value << positionTolerance(); + + emitter << YAML::Key << "buffer_size" << YAML::Value << bufferSize(); + + bool show_laps = ui_.show_laps->isChecked(); + emitter << YAML::Key << "show_laps" << YAML::Value << show_laps; + + emitter << YAML::Key + << "static_arrow_sizes" + << YAML::Value + << ui_.static_arrow_sizes->isChecked(); + + emitter << YAML::Key << "arrow_size" << YAML::Value << ui_.arrow_size->value(); + } +} // namespace mapviz_plugins diff --git a/mapviz_plugins/src/robot_image_plugin.cpp b/mapviz_plugins/src/robot_image_plugin.cpp new file mode 100644 index 000000000..947df56cb --- /dev/null +++ b/mapviz_plugins/src/robot_image_plugin.cpp @@ -0,0 +1,452 @@ +// ***************************************************************************** +// +// Copyright (c) 2014-2010, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include + +// QT libraries +#include +#include +#include +#include +#include + +// ROS libraries +#include +#include + +#include + +// Declare plugin +#include + +// C++ standard libraries +#include +#include +#include + +PLUGINLIB_EXPORT_CLASS(mapviz_plugins::RobotImagePlugin, mapviz::MapvizPlugin) + +namespace mapviz_plugins +{ + RobotImagePlugin::RobotImagePlugin() + : MapvizPlugin() + , ui_() + , config_widget_(new QWidget()) + , width_(2.0) + , height_(1.0) + , offset_x_(0.0) + , offset_y_(0.0) + , image_ratio_(1.0) + , dimension_(0) + , texture_id_(0) + , texture_loaded_(false) + , transformed_(false) + { + ui_.setupUi(config_widget_); + + // Set background white + QPalette p(config_widget_->palette()); + p.setColor(QPalette::Window, Qt::white); + config_widget_->setPalette(p); + + // Set status text red + QPalette p3(ui_.status->palette()); + p3.setColor(QPalette::Text, Qt::red); + ui_.status->setPalette(p3); + + UpdateShape(); + + QObject::connect(ui_.browse, SIGNAL(clicked()), this, SLOT(SelectFile())); + QObject::connect(ui_.selectframe, SIGNAL(clicked()), this, SLOT(SelectFrame())); + QObject::connect(ui_.frame, SIGNAL(editingFinished()), this, SLOT(FrameEdited())); + QObject::connect(ui_.image, SIGNAL(editingFinished()), this, SLOT(ImageEdited())); + QObject::connect(ui_.width, SIGNAL(valueChanged(double)), this, SLOT(WidthChanged(double))); + QObject::connect(ui_.height, SIGNAL(valueChanged(double)), this, SLOT(HeightChanged(double))); + QObject::connect(ui_.offset_x, + SIGNAL(valueChanged(double)), + this, + SLOT(OffsetXChanged(double))); + QObject::connect(ui_.offset_y, + SIGNAL(valueChanged(double)), + this, + SLOT(OffsetYChanged(double))); + ui_.offset_x->setMinimum(-99.99); // default is 0.0 but negative offset must be supported + ui_.offset_y->setMinimum(-99.99); + QObject::connect(ui_.ratio_equal, SIGNAL(toggled(bool)), this, SLOT(RatioEqualToggled(bool))); + QObject::connect(ui_.ratio_custom, SIGNAL(toggled(bool)), this, SLOT(RatioCustomToggled(bool))); + QObject::connect(ui_.ratio_original, + SIGNAL(toggled(bool)), + this, + SLOT(RatioOriginalToggled(bool))); + } + + void RobotImagePlugin::SelectFile() + { + QFileDialog dialog(config_widget_, "Select PNG Image"); + dialog.setFileMode(QFileDialog::ExistingFile); + dialog.setNameFilter(tr("PNG Image Files (*.png)")); + + dialog.exec(); + + if (dialog.result() == QDialog::Accepted && dialog.selectedFiles().count() == 1) + { + ui_.image->setText(dialog.selectedFiles().first()); + filename_ = dialog.selectedFiles().first().toStdString(); + LoadImage(); + } + } + + void RobotImagePlugin::SelectFrame() + { + std::string frame = mapviz::SelectFrameDialog::selectFrame(tf_buf_); + if (!frame.empty()) + { + ui_.frame->setText(QString::fromStdString(frame)); + FrameEdited(); + } + } + + void RobotImagePlugin::ImageEdited() + { + filename_ = ui_.image->text().toStdString(); + LoadImage(); + } + + void RobotImagePlugin::FrameEdited() + { + source_frame_ = ui_.frame->text().toStdString(); + PrintWarning("Waiting for transform."); + + RCLCPP_INFO(node_->get_logger(), "Setting target frame to to %s", source_frame_.c_str()); + + initialized_ = true; + + UpdateShape(); + } + + void RobotImagePlugin::WidthChanged(double value) + { + width_ = value; + if( ui_.ratio_equal->isChecked()){ + ui_.height->setValue( width_ ); + } else if( ui_.ratio_original->isChecked()) { + ui_.height->setValue( width_ * image_ratio_ ); + } + UpdateShape(); + } + + void RobotImagePlugin::HeightChanged(double value) + { + height_ = value; + UpdateShape(); + } + + void RobotImagePlugin::OffsetXChanged(double value) + { + offset_x_ = value; + UpdateShape(); + } + + void RobotImagePlugin::OffsetYChanged(double value) + { + offset_y_ = value; + UpdateShape(); + } + + void RobotImagePlugin::RatioEqualToggled(bool toggled) + { + if( toggled ) + { + ui_.height->setValue(width_); + ui_.height->setEnabled(false); + UpdateShape(); + } + } + + void RobotImagePlugin::RatioCustomToggled(bool toggled) + { + if( toggled ) + { + ui_.height->setEnabled(true); + UpdateShape(); + } + } + + void RobotImagePlugin::RatioOriginalToggled(bool toggled) + { + if( toggled ) + { + ui_.height->setValue(width_*image_ratio_); + ui_.height->setEnabled(false); + UpdateShape(); + } + } + + void RobotImagePlugin::UpdateShape() + { + double hw = 0.5*width_; // half width + double hh = 0.5*height_; // half height + top_left_ = tf2::Vector3(offset_x_ - hw, offset_y_ + hh, 0); + top_right_ = tf2::Vector3(offset_x_ + hw, offset_y_ + hh, 0); + bottom_left_ = tf2::Vector3(offset_x_ - hw, offset_y_ - hh, 0); + bottom_right_ = tf2::Vector3(offset_x_ + hw, offset_y_ - hh, 0); + } + + void RobotImagePlugin::PrintError(const std::string& message) + { + PrintErrorHelper(ui_.status, message); + } + + void RobotImagePlugin::PrintInfo(const std::string& message) + { + PrintInfoHelper(ui_.status, message); + } + + void RobotImagePlugin::PrintWarning(const std::string& message) + { + PrintWarningHelper(ui_.status, message); + } + + QWidget* RobotImagePlugin::GetConfigWidget(QWidget* parent) + { + config_widget_->setParent(parent); + + return config_widget_; + } + + bool RobotImagePlugin::Initialize(QGLWidget* canvas) + { + canvas_ = canvas; + + return true; + } + + void RobotImagePlugin::Draw(double x, double y, double scale) + { + if (texture_loaded_ && transformed_) + { + glColor3f(1.0f, 1.0f, 1.0f); + glEnable(GL_TEXTURE_2D); + glBindTexture(GL_TEXTURE_2D, static_cast(texture_id_)); + + glBegin(GL_QUADS); + + glTexCoord2f(0, 1); glVertex2d(top_left_transformed_.x(), top_left_transformed_.y()); + glTexCoord2f(1, 1); glVertex2d(top_right_transformed_.x(), top_right_transformed_.y()); + glTexCoord2f(1, 0); glVertex2d(bottom_right_transformed_.x(), bottom_right_transformed_.y()); + glTexCoord2f(0, 0); glVertex2d(bottom_left_transformed_.x(), bottom_left_transformed_.y()); + + glEnd(); + + glDisable(GL_TEXTURE_2D); + + PrintInfo("OK"); + } + } + + void RobotImagePlugin::Transform() + { + transformed_ = false; + + swri_transform_util::Transform transform; + if (GetTransform(node_->get_clock()->now(), transform)) + { + top_left_transformed_ = transform * top_left_; + top_right_transformed_ = transform * top_right_; + bottom_left_transformed_ = transform * bottom_left_; + bottom_right_transformed_ = transform * bottom_right_; + transformed_ = true; + } else { + PrintError("No transform between " + source_frame_ + " and " + target_frame_); + } + } + + void RobotImagePlugin::LoadImage() + { + RCLCPP_INFO(node_->get_logger(), "Loading image"); + try + { + QImage nullImage; + image_ = nullImage; + + if (texture_loaded_) + { + GLuint ids[1]; + ids[0] = static_cast(texture_id_); + glDeleteTextures(1, &ids[0]); + texture_loaded_ = false; + } + + const std::string prefix = "$(find "; + std::string real_filename; + size_t spos = filename_.find(prefix); + bool has_close = spos != -1 ? filename_.find(')', spos) != -1: false; + if (spos != -1 && spos + prefix.length() < filename_.size() && has_close) + { + std::string package = filename_.substr(spos + prefix.length()); + package = package.substr(0, package.find(')')); + + std::string package_path = ament_index_cpp::get_package_share_directory(package); + real_filename = QDir(QString::fromStdString(package_path)) + .filePath(QString::fromStdString(filename_.substr(filename_.find(')')+1))) + .toStdString(); + } else { + real_filename = filename_; + } + + + if (image_.load(real_filename.c_str())) + { + int width = image_.width(); + int height = image_.height(); + image_ratio_ = static_cast(height) / static_cast(width); + + float max_dim = std::max(width, height); + dimension_ = static_cast(std::pow(2, std::ceil(std::log(max_dim) / std::log(2.0f)))); + + if (width != dimension_ || height != dimension_) + { + image_ = image_.scaled(dimension_, + dimension_, + Qt::IgnoreAspectRatio, + Qt::FastTransformation); + } + + image_ = QGLWidget::convertToGLFormat(image_); + + GLuint ids[1]; + glGenTextures(1, &ids[0]); + texture_id_ = ids[0]; + + glBindTexture(GL_TEXTURE_2D, static_cast(texture_id_)); + glTexImage2D(GL_TEXTURE_2D, + 0, + GL_RGBA, + dimension_, + dimension_, + 0, + GL_RGBA, + GL_UNSIGNED_BYTE, + image_.bits()); + + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); + + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE); + + texture_loaded_ = true; + if ( ui_.ratio_original->isChecked() ) + { + RatioOriginalToggled(true); + } + } else { + PrintError("Failed to load image."); + } + } + catch (const std::exception& e) + { + PrintError("Failed to load image. Exception occured."); + } + } + + void RobotImagePlugin::LoadConfig(const YAML::Node& node, const std::string& path) + { + if (node["frame"]) + { + source_frame_ = node["frame"].as(); + ui_.frame->setText(source_frame_.c_str()); + } + if (node["offset_x"]) + { + offset_x_ = node["offset_x"].as(); + ui_.offset_x->setValue(offset_x_); + } + + if (node["offset_y"]) + { + offset_y_ = node["offset_y"].as(); + ui_.offset_y->setValue(offset_y_); + } + + if (node["image"]) + { + filename_ = node["image"].as(); + ui_.image->setText(filename_.c_str()); + } + + if (node["width"]) + { + width_ = node["width"].as(); + ui_.width->setValue(width_); + } + + if (node["height"]) + { + height_ = node["height"].as(); + ui_.height->setValue(height_); + } + + if (node["ratio"]) + { + std::string value = node["ratio"].as(); + if(value == "equal") + { + ui_.ratio_equal->setChecked(true); + } else if(value == "custom") { + ui_.ratio_custom->setChecked(true); + } else if(value == "original") { + ui_.ratio_original->setChecked(true); + } + } + + UpdateShape(); + LoadImage(); + FrameEdited(); + } + + void RobotImagePlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path) + { + emitter << YAML::Key << "frame" << YAML::Value << ui_.frame->text().toStdString(); + emitter << YAML::Key << "image" << YAML::Value << ui_.image->text().toStdString(); + emitter << YAML::Key << "width" << YAML::Value << width_; + emitter << YAML::Key << "height" << YAML::Value << height_; + emitter << YAML::Key << "offset_x" << YAML::Value << offset_x_; + emitter << YAML::Key << "offset_y" << YAML::Value << offset_y_; + if( ui_.ratio_custom->isChecked()) + { + emitter << YAML::Key << "ratio" << YAML::Value << "custom"; + } else if( ui_.ratio_equal->isChecked()) { + emitter << YAML::Key << "ratio" << YAML::Value << "equal"; + } else if( ui_.ratio_original->isChecked()) { + emitter << YAML::Key << "ratio" << YAML::Value << "original"; + } + } +} // namespace mapviz_plugins + diff --git a/mapviz_plugins/src/route_plugin.cpp b/mapviz_plugins/src/route_plugin.cpp new file mode 100644 index 000000000..4ccf85cc1 --- /dev/null +++ b/mapviz_plugins/src/route_plugin.cpp @@ -0,0 +1,423 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include + +// QT libraries +#include +#include +#include +#include + +#include + +// ROS libraries +#include + +#include +#include +#include +#include + +#include + +// Declare plugin +#include + +// C++ standard libraries +#include +#include +#include + +PLUGINLIB_EXPORT_CLASS(mapviz_plugins::RoutePlugin, mapviz::MapvizPlugin) + +namespace sru = swri_route_util; +namespace stu = swri_transform_util; + +namespace mapviz_plugins +{ + RoutePlugin::RoutePlugin() + : MapvizPlugin() + , ui_() + , config_widget_(new QWidget()), draw_style_(LINES) + { + ui_.setupUi(config_widget_); + + ui_.color->setColor(Qt::green); + // Set background white + QPalette p(config_widget_->palette()); + p.setColor(QPalette::Window, Qt::white); + config_widget_->setPalette(p); + // Set status text red + QPalette p3(ui_.status->palette()); + p3.setColor(QPalette::Text, Qt::red); + ui_.status->setPalette(p3); + QObject::connect(ui_.selecttopic, SIGNAL(clicked()), this, + SLOT(SelectTopic())); + QObject::connect(ui_.topic, SIGNAL(editingFinished()), this, + SLOT(TopicEdited())); + QObject::connect(ui_.selectpositiontopic, SIGNAL(clicked()), this, + SLOT(SelectPositionTopic())); + QObject::connect(ui_.positiontopic, SIGNAL(editingFinished()), this, + SLOT(PositionTopicEdited())); + QObject::connect(ui_.drawstyle, SIGNAL(activated(QString)), this, + SLOT(SetDrawStyle(QString))); + QObject::connect(ui_.color, SIGNAL(colorEdited(const QColor&)), this, + SLOT(DrawIcon())); + } + + void RoutePlugin::DrawIcon() + { + if (icon_) + { + QPixmap icon(16, 16); + icon.fill(Qt::transparent); + + QPainter painter(&icon); + painter.setRenderHint(QPainter::Antialiasing, true); + + QPen pen(ui_.color->color()); + + if (draw_style_ == POINTS) + { + pen.setWidth(7); + pen.setCapStyle(Qt::RoundCap); + painter.setPen(pen); + painter.drawPoint(8, 8); + } else if (draw_style_ == LINES) { + pen.setWidth(3); + pen.setCapStyle(Qt::FlatCap); + painter.setPen(pen); + painter.drawLine(1, 14, 14, 1); + } + + icon_->SetPixmap(icon); + } + } + + void RoutePlugin::SetDrawStyle(QString style) + { + if (style == "lines") + { + draw_style_ = LINES; + } else if (style == "points") { + draw_style_ = POINTS; + } + DrawIcon(); + } + + void RoutePlugin::SelectTopic() + { + std::string topic = + mapviz::SelectTopicDialog::selectTopic(node_, "marti_nav_msgs/msg/Route"); + + if (topic.empty()) + { + return; + } + + ui_.topic->setText(QString::fromStdString(topic)); + TopicEdited(); + } + + void RoutePlugin::SelectPositionTopic() + { + std::string topic = + mapviz::SelectTopicDialog::selectTopic(node_, "marti_nav_msgs/msg/RoutePosition"); + + if (topic.empty()) + { + return; + } + + ui_.positiontopic->setText(QString::fromStdString(topic)); + PositionTopicEdited(); + } + + void RoutePlugin::TopicEdited() + { + std::string topic = ui_.topic->text().trimmed().toStdString(); + if (topic != topic_) + { + src_route_ = sru::Route(); + + route_sub_.reset(); + + topic_ = topic; + if (!topic.empty()) + { + route_sub_ = + node_->create_subscription( + topic_, + rclcpp::QoS(1), + std::bind(&RoutePlugin::RouteCallback, this, std::placeholders::_1) + ); + + RCLCPP_INFO(node_->get_logger(), "Subscribing to %s", topic_.c_str()); + } + } + } + + void RoutePlugin::PositionTopicEdited() + { + std::string topic = ui_.positiontopic->text().trimmed().toStdString(); + if (topic != position_topic_) + { + src_route_position_.reset(); + position_sub_.reset(); + + if (!topic.empty()) + { + position_topic_ = topic; + position_sub_ = node_->create_subscription( + topic_, + rclcpp::QoS(1), + std::bind(&RoutePlugin::PositionCallback, this, std::placeholders::_1) + ); + + RCLCPP_INFO(node_->get_logger(), "Subscribing to %s", position_topic_.c_str()); + } + } + } + + void RoutePlugin::PositionCallback( + const marti_nav_msgs::msg::RoutePosition::SharedPtr msg) + { + src_route_position_ = msg; + } + + void RoutePlugin::RouteCallback(const marti_nav_msgs::msg::Route::SharedPtr msg) + { + src_route_ = sru::Route(*msg); + } + + void RoutePlugin::PrintError(const std::string& message) + { + PrintErrorHelper(ui_.status, message, 1.0); + } + + void RoutePlugin::PrintInfo(const std::string& message) + { + PrintInfoHelper(ui_.status, message, 1.0); + } + + void RoutePlugin::PrintWarning(const std::string& message) + { + PrintWarningHelper(ui_.status, message, 1.0); + } + + QWidget* RoutePlugin::GetConfigWidget(QWidget* parent) + { + config_widget_->setParent(parent); + + return config_widget_; + } + + bool RoutePlugin::Initialize(QGLWidget* canvas) + { + canvas_ = canvas; + + DrawIcon(); + + initialized_ = true; + return true; + } + + void RoutePlugin::Draw(double x, double y, double scale) + { + if (!src_route_.valid()) + { + PrintError("No valid route received."); + return; + } + + sru::Route route = src_route_; + if (route.header.frame_id.empty()) + { + route.header.frame_id = "wgs84"; + } + + stu::Transform transform; + if (!GetTransform(route.header.frame_id, rclcpp::Time(), transform)) + { + PrintError("Failed to transform route"); + return; + } + + sru::transform(route, transform, target_frame_); + sru::projectToXY(route); + sru::fillOrientations(route); + + DrawRoute(route); + + bool ok = true; + if (route.valid() && src_route_position_) + { + sru::RoutePoint point; + if (sru::interpolateRoutePosition(point, route, *src_route_position_, true)) + { + DrawRoutePoint(point); + } else { + PrintError("Failed to find route position in route."); + ok = false; + } + } + + if (ok) + { + PrintInfo("OK"); + } + } + + void RoutePlugin::DrawStopWaypoint(double x, double y) + { + const double a = 2; + const double S = a * 2.414213562373095; + + glBegin(GL_POLYGON); + + glColor3f(1.0, 0.0, 0.0); + + glVertex2d(x + S / 2.0, y - a / 2.0); + glVertex2d(x + S / 2.0, y + a / 2.0); + glVertex2d(x + a / 2.0, y + S / 2.0); + glVertex2d(x - a / 2.0, y + S / 2.0); + glVertex2d(x - S / 2.0, y + a / 2.0); + glVertex2d(x - S / 2.0, y - a / 2.0); + glVertex2d(x - a / 2.0, y - S / 2.0); + glVertex2d(x + a / 2.0, y - S / 2.0); + + glEnd(); + } + + void RoutePlugin::DrawRoute(const sru::Route& route) + { + const QColor color = ui_.color->color(); + glColor4d(color.redF(), color.greenF(), color.blueF(), 1.0); + + if (draw_style_ == LINES) + { + glLineWidth(3); + glBegin(GL_LINE_STRIP); + } else { + glPointSize(2); + glBegin(GL_POINTS); + } + + for (const auto & point : route.points) + { + glVertex2d(point.position().x(), + point.position().y()); + } + glEnd(); + } + + void RoutePlugin::DrawRoutePoint(const sru::RoutePoint& point) + { + const double arrow_size = ui_.iconsize->value(); + + tf2::Vector3 v1(arrow_size, 0.0, 0.0); + tf2::Vector3 v2(0.0, arrow_size / 2.0, 0.0); + tf2::Vector3 v3(0.0, -arrow_size / 2.0, 0.0); + + tf2::Transform point_g(point.orientation(), point.position()); + + v1 = point_g * v1; + v2 = point_g * v2; + v3 = point_g * v3; + + const QColor color = ui_.positioncolor->color(); + glLineWidth(3); + glBegin(GL_POLYGON); + glColor4d(color.redF(), color.greenF(), color.blueF(), 1.0); + glVertex2d(v1.x(), v1.y()); + glVertex2d(v2.x(), v2.y()); + glVertex2d(v3.x(), v3.y()); + glEnd(); + } + + void RoutePlugin::LoadConfig(const YAML::Node& node, const std::string& path) + { + if (node["topic"]) + { + std::string route_topic = node["topic"].as(); + ui_.topic->setText(route_topic.c_str()); + } + if (node["color"]) + { + std::string color = node["color"].as(); + ui_.color->setColor(QColor(color.c_str())); + } + if (node["postopic"]) + { + std::string pos_topic = node["postopic"].as(); + ui_.positiontopic->setText(pos_topic.c_str()); + } + if (node["poscolor"]) + { + std::string poscolor = node["poscolor"].as(); + ui_.positioncolor->setColor(QColor(poscolor.c_str())); + } + + if (node["draw_style"]) + { + std::string draw_style = node["draw_style"].as(); + + if (draw_style == "lines") + { + draw_style_ = LINES; + ui_.drawstyle->setCurrentIndex(0); + } else if (draw_style == "points") { + draw_style_ = POINTS; + ui_.drawstyle->setCurrentIndex(1); + } + } + + TopicEdited(); + PositionTopicEdited(); + } + + void RoutePlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path) + { + std::string route_topic = ui_.topic->text().toStdString(); + emitter << YAML::Key << "topic" << YAML::Value << route_topic; + + std::string color = ui_.color->color().name().toStdString(); + emitter << YAML::Key << "color" << YAML::Value << color; + + std::string pos_topic = ui_.positiontopic->text().toStdString(); + emitter << YAML::Key << "postopic" << YAML::Value << pos_topic; + + std::string pos_color = ui_.positioncolor->color().name().toStdString(); + emitter << YAML::Key << "poscolor" << YAML::Value << pos_color; + + std::string draw_style = ui_.drawstyle->currentText().toStdString(); + emitter << YAML::Key << "draw_style" << YAML::Value << draw_style; + } +} // namespace mapviz_plugins diff --git a/mapviz_plugins/src/string_plugin.cpp b/mapviz_plugins/src/string_plugin.cpp new file mode 100644 index 000000000..05a55f1b5 --- /dev/null +++ b/mapviz_plugins/src/string_plugin.cpp @@ -0,0 +1,435 @@ +// ***************************************************************************** +// +// Copyright (c) 2015, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL Southwest Research Institute® BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY +// OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +// DAMAGE. +// +// ***************************************************************************** + +#include + +#include + +#include + +#include + +#include +PLUGINLIB_EXPORT_CLASS(mapviz_plugins::StringPlugin, mapviz::MapvizPlugin) + +namespace mapviz_plugins +{ + const char* StringPlugin::COLOR_KEY = "color"; + const char* StringPlugin::FONT_KEY = "font"; + const char* StringPlugin::TOPIC_KEY = "topic"; + const char* StringPlugin::ANCHOR_KEY = "anchor"; + const char* StringPlugin::UNITS_KEY = "units"; + const char* StringPlugin::OFFSET_X_KEY = "offset_x"; + const char* StringPlugin::OFFSET_Y_KEY = "offset_y"; + + StringPlugin::StringPlugin() + : MapvizPlugin() + , ui_() + , config_widget_(new QWidget()) + , anchor_(TOP_LEFT) + , units_(PIXELS) + , offset_x_(0) + , offset_y_(0) + , has_message_(false) + , has_painted_(false) + , color_(Qt::black) + { + ui_.setupUi(config_widget_); + // Set background white + QPalette p(config_widget_->palette()); + p.setColor(QPalette::Window, Qt::white); + config_widget_->setPalette(p); + + // Set status text red + QPalette p3(ui_.status->palette()); + p3.setColor(QPalette::Text, Qt::red); + ui_.status->setPalette(p3); + + QObject::connect(ui_.selecttopic, SIGNAL(clicked()), this, SLOT(SelectTopic())); + QObject::connect(ui_.topic, SIGNAL(editingFinished()), this, SLOT(TopicEdited())); + QObject::connect(ui_.anchor, SIGNAL(activated(QString)), this, SLOT(SetAnchor(QString))); + QObject::connect(ui_.units, SIGNAL(activated(QString)), this, SLOT(SetUnits(QString))); + QObject::connect(ui_.offsetx, SIGNAL(valueChanged(int)), this, SLOT(SetOffsetX(int))); + QObject::connect(ui_.offsety, SIGNAL(valueChanged(int)), this, SLOT(SetOffsetY(int))); + QObject::connect(ui_.font_button, SIGNAL(clicked()), this, SLOT(SelectFont())); + QObject::connect(ui_.color, SIGNAL(colorEdited(const QColor &)), this, SLOT(SelectColor())); + + font_.setFamily(tr("Helvetica")); + ui_.font_button->setFont(font_); + ui_.font_button->setText(font_.family()); + + ui_.color->setColor(color_); + } + + bool StringPlugin::Initialize(QGLWidget* canvas) + { + canvas_ = canvas; + return true; + } + + void StringPlugin::Draw(double, double, double) + { + // This plugin doesn't do any OpenGL drawing. + } + + void StringPlugin::Paint(QPainter* painter, double, double, double) + { + if (has_message_) + { + painter->save(); + painter->resetTransform(); + painter->setFont(font_); + + if (!has_painted_) + { + // After the first time we get a new message, we do not know how wide it's + // going to be when rendered, so we can't accurately calculate the top left + // coordinate if it's offset from the right or bottom borders. + // The easiest workaround I've found for this is to draw it once using + // a completely transparent pen, which will cause the QStaticText class to + // know how wide it is; then we can recalculate the offsets and draw it + // again with a visible pen. + QPen invisPen(QBrush(Qt::transparent), 1); + painter->setPen(invisPen); + PaintText(painter); + has_painted_ = true; + } + QPen pen(QBrush(color_), 1); + painter->setPen(pen); + PaintText(painter); + + painter->restore(); + PrintInfo("OK"); + } else { + PrintWarning("No messages received."); + } + } + + void StringPlugin::PaintText(QPainter* painter) + { + // Calculate the correct offsets and dimensions + int x_offset = offset_x_; + int y_offset = offset_y_; + if (units_ == PERCENT) + { + x_offset = static_cast(static_cast(offset_x_ * canvas_->width()) / 100.0); + y_offset = static_cast(static_cast(offset_y_ * canvas_->height()) / 100.0); + } + + int right = static_cast( + static_cast(canvas_->width()) - message_.size().width()) - x_offset; + int bottom = static_cast( + static_cast(canvas_->height()) - message_.size().height()) - y_offset; + int yCenter = static_cast( + static_cast(canvas_->height()) / 2.0 - message_.size().height()/2.0); + int xCenter = static_cast( + static_cast(canvas_->width()) / 2.0 - message_.size().width()/2.0); + + QPoint ulPoint; + + switch (anchor_) + { + case TOP_LEFT: + ulPoint.setX(x_offset); + ulPoint.setY(y_offset); + break; + case TOP_CENTER: + ulPoint.setX(xCenter); + ulPoint.setY(y_offset); + break; + case TOP_RIGHT: + ulPoint.setX(right); + ulPoint.setY(y_offset); + break; + case CENTER_LEFT: + ulPoint.setX(x_offset); + ulPoint.setY(yCenter); + break; + case CENTER: + ulPoint.setX(xCenter); + ulPoint.setY(yCenter); + break; + case CENTER_RIGHT: + ulPoint.setX(right); + ulPoint.setY(yCenter); + break; + case BOTTOM_LEFT: + ulPoint.setX(x_offset); + ulPoint.setY(bottom); + break; + case BOTTOM_CENTER: + ulPoint.setX(xCenter); + ulPoint.setY(bottom); + break; + case BOTTOM_RIGHT: + ulPoint.setX(right); + ulPoint.setY(bottom); + break; + } + painter->drawStaticText(ulPoint, message_); + } + + void StringPlugin::LoadConfig(const YAML::Node& node, const std::string& path) + { + if (node[TOPIC_KEY]) + { + ui_.topic->setText(QString(node[TOPIC_KEY].as().c_str())); + TopicEdited(); + } + + if (node[FONT_KEY]) + { + font_.fromString(QString(node[FONT_KEY].as().c_str())); + ui_.font_button->setFont(font_); + ui_.font_button->setText(font_.family()); + } + + if (node[COLOR_KEY]) + { + color_ = QColor(node[COLOR_KEY].as().c_str()); + ui_.color->setColor(QColor(color_.name().toStdString().c_str())); + } + + if (node[ANCHOR_KEY]) + { + std::string anchor = node[ANCHOR_KEY].as(); + ui_.anchor->setCurrentIndex(ui_.anchor->findText(anchor.c_str())); + SetAnchor(anchor.c_str()); + } + + if (node[UNITS_KEY]) + { + std::string units = node[UNITS_KEY].as(); + ui_.units->setCurrentIndex(ui_.units->findText(units.c_str())); + SetUnits(units.c_str()); + } + + if (node[OFFSET_X_KEY]) + { + offset_x_ = node[OFFSET_X_KEY].as(); + ui_.offsetx->setValue(offset_x_); + } + + if (node[OFFSET_Y_KEY]) + { + offset_y_ = node[OFFSET_Y_KEY].as(); + ui_.offsety->setValue(offset_y_); + } + } + + void StringPlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path) + { + emitter << YAML::Key << FONT_KEY << YAML::Value << font_.toString().toStdString(); + emitter << YAML::Key << COLOR_KEY << YAML::Value << color_.name().toStdString(); + emitter << YAML::Key << TOPIC_KEY << YAML::Value << ui_.topic->text().toStdString(); + emitter << YAML::Key << ANCHOR_KEY << YAML::Value << AnchorToString(anchor_); + emitter << YAML::Key << UNITS_KEY << YAML::Value << UnitsToString(units_); + emitter << YAML::Key << OFFSET_X_KEY << YAML::Value << offset_x_; + emitter << YAML::Key << OFFSET_Y_KEY << YAML::Value << offset_y_; + } + + QWidget* StringPlugin::GetConfigWidget(QWidget* parent) + { + config_widget_->setParent(parent); + return config_widget_; + } + + void StringPlugin::PrintError(const std::string& message) + { + PrintErrorHelper(ui_.status, message); + } + + void StringPlugin::PrintInfo(const std::string& message) + { + PrintInfoHelper(ui_.status, message); + } + + void StringPlugin::PrintWarning(const std::string& message) + { + PrintWarningHelper(ui_.status, message); + } + + void StringPlugin::SelectColor() + { + color_ = ui_.color->color(); + } + + void StringPlugin::SelectFont() + { + bool ok; + QFont font = QFontDialog::getFont(&ok, font_, canvas_); + if (ok) + { + font_ = font; + message_.prepare(QTransform(), font_); + ui_.font_button->setFont(font_); + ui_.font_button->setText(font_.family()); + } + } + + void StringPlugin::SelectTopic() + { + std::string topic = mapviz::SelectTopicDialog::selectTopic( + node_, "std_msgs/msg/String" + ); + + if (!topic.empty()) + { + ui_.topic->setText(QString::fromStdString(topic)); + TopicEdited(); + } + } + + void StringPlugin::TopicEdited() + { + std::string topic = ui_.topic->text().trimmed().toStdString(); + if (topic != topic_) + { + initialized_ = false; + has_message_ = false; + PrintWarning("No messages received."); + + string_sub_.reset(); + + topic_ = topic; + if (!topic.empty()) + { + string_sub_ = node_->create_subscription(topic_, + rclcpp::QoS(1), + [this](const std_msgs::msg::String::ConstSharedPtr str) { + SetText(QString(str->data.c_str())); + }); + string_stamped_sub_ = node_->create_subscription(topic_, + rclcpp::QoS(1), + [this](const marti_common_msgs::msg::StringStamped::ConstSharedPtr str) { + SetText(QString(str->value.c_str())); + }); + + RCLCPP_INFO(node_->get_logger(), "Subscribing to %s", topic_.c_str()); + } + } + } + + void StringPlugin::SetText(const QString& text) + { + message_.setText(text); + message_.prepare(QTransform(), font_); + + has_message_ = true; + has_painted_ = false; + initialized_ = true; + } + + void StringPlugin::SetAnchor(QString anchor) + { + if (anchor == "top left") + { + anchor_ = TOP_LEFT; + } else if (anchor == "top center") { + anchor_ = TOP_CENTER; + } else if (anchor == "top right") { + anchor_ = TOP_RIGHT; + } else if (anchor == "center left") { + anchor_ = CENTER_LEFT; + } else if (anchor == "center") { + anchor_ = CENTER; + } else if (anchor == "center right") { + anchor_ = CENTER_RIGHT; + } else if (anchor == "bottom left") { + anchor_ = BOTTOM_LEFT; + } else if (anchor == "bottom center") { + anchor_ = BOTTOM_CENTER; + } else if (anchor == "bottom right") { + anchor_ = BOTTOM_RIGHT; + } + } + + void StringPlugin::SetUnits(QString units) + { + if (units == "pixels") + { + units_ = PIXELS; + } else if (units == "percent") { + units_ = PERCENT; + } + } + + void StringPlugin::SetOffsetX(int offset) + { + offset_x_ = offset; + } + + void StringPlugin::SetOffsetY(int offset) + { + offset_y_ = offset; + } + + std::string StringPlugin::AnchorToString(StringPlugin::Anchor anchor) + { + std::string anchor_string = "top left"; + + if (anchor == TOP_LEFT) + { + anchor_string = "top left"; + } else if (anchor == TOP_CENTER) { + anchor_string = "top center"; + } else if (anchor == TOP_RIGHT) { + anchor_string = "top right"; + } else if (anchor == CENTER_LEFT) { + anchor_string = "center left"; + } else if (anchor == CENTER) { + anchor_string = "center"; + } else if (anchor == CENTER_RIGHT) { + anchor_string = "center right"; + } else if (anchor == BOTTOM_LEFT) { + anchor_string = "bottom left"; + } else if (anchor == BOTTOM_CENTER) { + anchor_string = "bottom center"; + } else if (anchor == BOTTOM_RIGHT) { + anchor_string = "bottom right"; + } + + return anchor_string; + } + + std::string StringPlugin::UnitsToString(StringPlugin::Units units) + { + std::string units_string = "pixels"; + + if (units == PIXELS) + { + units_string = "pixels"; + } else if (units == PERCENT) { + units_string = "percent"; + } + + return units_string; + } +} // namespace mapviz_plugins diff --git a/mapviz_plugins/src/textured_marker_plugin.cpp b/mapviz_plugins/src/textured_marker_plugin.cpp new file mode 100755 index 000000000..3771e062b --- /dev/null +++ b/mapviz_plugins/src/textured_marker_plugin.cpp @@ -0,0 +1,522 @@ +// ***************************************************************************** +// +// Copyright (c) 2014-2020, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include + +// Boost libraries +#include + +// QT libraries +#include +#include + +// ROS libraries +#include +#include + +#include + +// Declare plugin +#include + +// C++ standard libraries +#include +#include +#include +#include +#include +#include +#include + +using namespace std::chrono_literals; + +PLUGINLIB_EXPORT_CLASS(mapviz_plugins::TexturedMarkerPlugin, mapviz::MapvizPlugin) + +namespace mapviz_plugins +{ +TexturedMarkerPlugin::TexturedMarkerPlugin() +: MapvizPlugin() +, ui_() +, alphaVal_(1.0f) +, config_widget_(new QWidget()) +, has_message_(false) +{ + ui_.setupUi(config_widget_); + + // Set background white + QPalette p(config_widget_->palette()); + p.setColor(QPalette::Window, Qt::white); + config_widget_->setPalette(p); + + // Set status text red + QPalette p3(ui_.status->palette()); + p3.setColor(QPalette::Text, Qt::red); + ui_.status->setPalette(p3); + + QObject::connect(ui_.selecttopic, SIGNAL(clicked()), this, SLOT(SelectTopic())); + QObject::connect(ui_.topic, SIGNAL(editingFinished()), this, SLOT(TopicEdited())); + QObject::connect(ui_.clear, SIGNAL(clicked()), this, SLOT(ClearHistory())); + QObject::connect(ui_.alphaSlide, SIGNAL(valueChanged(int)), this, SLOT(SetAlphaLevel(int))); + + // By using a signal/slot connection, we ensure that we only generate GL textures on the + // main thread in case a non-main thread handles the ROS callbacks. + qRegisterMetaType("TexturedMarker"); + + QObject::connect(this, + SIGNAL(MarkerReceived(marti_visualization_msgs::msg::TexturedMarker)), + this, + SLOT(ProcessMarker(marti_visualization_msgs::msg::TexturedMarker))); +} + +void TexturedMarkerPlugin::ClearHistory() +{ + RCLCPP_DEBUG(node_->get_logger(), "TexturedMarkerPlugin::ClearHistory()"); + markers_.clear(); +} + +// TODO(P. J. Reed) could instead use the value() function on alphaSlide when needed, +// assuming value is always good +// Modify min and max values by adjusting textured_marker_config.ui +void TexturedMarkerPlugin::SetAlphaLevel(int alpha) +{ + int max = ui_.alphaSlide->maximum(); + int min = ui_.alphaSlide->minimum(); + + if (max < 1 || + min < 0 || + alpha > max || + alpha < min) // ignore negative min and max + { + alphaVal_ = 1.0f; + PrintWarning("Invalid alpha input."); + } else { + // Ex. convert int in range 0-100 to float in range 0-1 + alphaVal_ = (static_cast(alpha) / max); + RCLCPP_INFO(node_->get_logger(), "Adjusting alpha value to: %f", alphaVal_); + } +} + +void TexturedMarkerPlugin::SelectTopic() +{ + std::string topic = mapviz::SelectTopicDialog::selectTopic( + node_, + "marti_visualization_msgs/msg/TexturedMarker", + "marti_visualization_msgs/msg/TexturedMarkerArray" + ); + + if (!topic.empty()) { + ui_.topic->setText(QString::fromStdString(topic)); + + TopicEdited(); + } +} + +void TexturedMarkerPlugin::TopicEdited() +{ + std::string topic = ui_.topic->text().trimmed().toStdString(); + if (topic != topic_) { + initialized_ = false; + markers_.clear(); + has_message_ = false; + PrintWarning("No messages received."); + + marker_sub_.reset(); + marker_arr_sub_.reset(); + + topic_ = topic; + if (!topic.empty()) { + marker_arr_sub_ = + node_->create_subscription( + topic_, + rclcpp::QoS(1000), + std::bind(&TexturedMarkerPlugin::MarkerArrayCallback, this, std::placeholders::_1) + ); + marker_sub_ = node_->create_subscription( + topic_, + rclcpp::QoS(1000), + std::bind(&TexturedMarkerPlugin::MarkerCallback, this, std::placeholders::_1) + ); + + RCLCPP_INFO(node_->get_logger(), "Subscribing to %s", topic_.c_str()); + } + } +} + +void TexturedMarkerPlugin::ProcessMarker(const marti_visualization_msgs::msg::TexturedMarker marker) +{ + if (!has_message_) { + initialized_ = true; + has_message_ = true; + } + + // Note that unlike some plugins, this one does not store nor rely on the + // source_frame_ member variable. This one can potentially store many + // messages with different source frames, so we need to store and transform + // them individually. + + if (marker.action == marti_visualization_msgs::msg::TexturedMarker::ADD) { + MarkerData & markerData = markers_[marker.ns][marker.id]; + markerData.stamp = marker.header.stamp; + + markerData.transformed = true; + markerData.alpha_ = marker.alpha; + markerData.source_frame_ = marker.header.frame_id; + + swri_transform_util::Transform transform; + if (!GetTransform(markerData.source_frame_, marker.header.stamp, transform)) { + markerData.transformed = false; + PrintError("No transform between " + markerData.source_frame_ + " and " + target_frame_); + } + + // Handle lifetime parameter + rclcpp::Duration lifetime = marker.lifetime; + if (lifetime.seconds() == 0 && lifetime.nanoseconds() == 0) { + markerData.expire_time = rclcpp::Time::max(); + } else { + // Temporarily add 5 seconds to fix some existing markers. + markerData.expire_time = rclcpp::Time() + lifetime + rclcpp::Duration(5s); + } + + tf2::Transform offset( + tf2::Quaternion( + marker.pose.orientation.x, + marker.pose.orientation.y, + marker.pose.orientation.z, + marker.pose.orientation.w), + tf2::Vector3( + marker.pose.position.x, + marker.pose.position.y, + marker.pose.position.z)); + + double right = marker.image.width * marker.resolution / 2.0; + double left = -right; + double top = marker.image.height * marker.resolution / 2.0; + double bottom = -top; + + tf2::Vector3 top_left(left, top, 0); + tf2::Vector3 top_right(right, top, 0); + tf2::Vector3 bottom_left(left, bottom, 0); + tf2::Vector3 bottom_right(right, bottom, 0); + + top_left = offset * top_left; + top_right = offset * top_right; + bottom_left = offset * bottom_left; + bottom_right = offset * bottom_right; + + markerData.quad_.clear(); + markerData.quad_.push_back(top_left); + markerData.quad_.push_back(top_right); + markerData.quad_.push_back(bottom_right); + + markerData.quad_.push_back(top_left); + markerData.quad_.push_back(bottom_right); + markerData.quad_.push_back(bottom_left); + + markerData.transformed_quad_.clear(); + for (size_t i = 0; i < markerData.quad_.size(); i++) { + markerData.transformed_quad_.push_back(transform * markerData.quad_[i]); + } + + int32_t max_dimension = std::max(marker.image.height, marker.image.width); + int32_t new_size = 1; + while (new_size < max_dimension) { + new_size = new_size << 1; + } + + if (new_size != markerData.texture_size_ || markerData.encoding_ != marker.image.encoding) { + markerData.texture_size_ = new_size; + + markerData.encoding_ = marker.image.encoding; + + GLuint ids[1]; + + // Free the current texture. + if (markerData.texture_id_ != -1) { + ids[0] = static_cast(markerData.texture_id_); + glDeleteTextures(1, &ids[0]); + } + + // Get a new texture id. + glGenTextures(1, &ids[0]); + markerData.texture_id_ = ids[0]; + + // Bind the texture with the correct size and null memory. + glBindTexture(GL_TEXTURE_2D, static_cast(markerData.texture_id_)); + + glPixelStorei(GL_UNPACK_ALIGNMENT, 1); + + size_t bpp = 0; + if (markerData.encoding_ == sensor_msgs::image_encodings::BGRA8) { + bpp = 4; + markerData.texture_.resize(static_cast(markerData.texture_size_ * + markerData.texture_size_ * 4)); + } else if (markerData.encoding_ == sensor_msgs::image_encodings::BGR8) { + bpp = 3; + markerData.texture_.resize(static_cast(markerData.texture_size_ * + markerData.texture_size_ * 3)); + } else if (markerData.encoding_ == sensor_msgs::image_encodings::MONO8) { + bpp = 1; + markerData.texture_.resize(static_cast(markerData.texture_size_ * + markerData.texture_size_)); + } else { + RCLCPP_WARN(node_->get_logger(), "Unsupported encoding: %s", markerData.encoding_.c_str()); + } + + size_t expected = marker.image.height * marker.image.width * bpp; + if (!markerData.texture_.empty() && marker.image.data.size() < expected) { + RCLCPP_ERROR( + node_->get_logger(), + "TexturedMarker image had expected data size %li but only got %li. Dropping message.", + expected, + marker.image.data.size()); + return; + } + + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); + + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE); + + glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE); + + glBindTexture(GL_TEXTURE_2D, 0); + + glPixelStorei(GL_UNPACK_ALIGNMENT, 4); + } + + glBindTexture(GL_TEXTURE_2D, static_cast(markerData.texture_id_)); + + if (markerData.encoding_ == sensor_msgs::image_encodings::BGRA8) { + for (size_t row = 0; row < marker.image.height; row++) { + for (size_t col = 0; col < marker.image.width; col++) { + size_t src_index = (row * marker.image.width + col) * 4; + size_t dst_index = (row * markerData.texture_size_ + col) * 4; + + markerData.texture_[dst_index + 0] = marker.image.data[src_index + 0]; + markerData.texture_[dst_index + 1] = marker.image.data[src_index + 1]; + markerData.texture_[dst_index + 2] = marker.image.data[src_index + 2]; + markerData.texture_[dst_index + 3] = marker.image.data[src_index + 3]; + } + } + + glTexImage2D( + GL_TEXTURE_2D, + 0, + GL_RGBA, + markerData.texture_size_, + markerData.texture_size_, + 0, + GL_BGRA, + GL_UNSIGNED_BYTE, + markerData.texture_.data()); + } else if (markerData.encoding_ == sensor_msgs::image_encodings::BGR8) { + for (size_t row = 0; row < marker.image.height; row++) { + for (size_t col = 0; col < marker.image.width; col++) { + size_t src_index = (row * marker.image.width + col) * 3; + size_t dst_index = (row * markerData.texture_size_ + col) * 3; + + markerData.texture_[dst_index + 0] = marker.image.data[src_index + 0]; + markerData.texture_[dst_index + 1] = marker.image.data[src_index + 1]; + markerData.texture_[dst_index + 2] = marker.image.data[src_index + 2]; + } + } + + glTexImage2D( + GL_TEXTURE_2D, + 0, + GL_RGB, + markerData.texture_size_, + markerData.texture_size_, + 0, + GL_BGR, + GL_UNSIGNED_BYTE, + markerData.texture_.data()); + } else if (markerData.encoding_ == sensor_msgs::image_encodings::MONO8) { + for (size_t row = 0; row < marker.image.height; row++) { + for (size_t col = 0; col < marker.image.width; col++) { + size_t src_index = row * marker.image.width + col; + size_t dst_index = row * markerData.texture_size_ + col; + + markerData.texture_[dst_index] = marker.image.data[src_index]; + } + } + + glTexImage2D( + GL_TEXTURE_2D, + 0, + GL_LUMINANCE, + markerData.texture_size_, + markerData.texture_size_, + 0, + GL_LUMINANCE, + GL_UNSIGNED_BYTE, + markerData.texture_.data()); + } + + glBindTexture(GL_TEXTURE_2D, 0); + + markerData.texture_x_ = static_cast(marker.image.width) / + static_cast(markerData.texture_size_); + markerData.texture_y_ = static_cast(marker.image.height) / + static_cast(markerData.texture_size_); + } else { + markers_[marker.ns].erase(marker.id); + } +} + +void TexturedMarkerPlugin::MarkerCallback( + marti_visualization_msgs::msg::TexturedMarker::ConstSharedPtr marker) +{ + Q_EMIT MarkerReceived(*marker); +} + +void TexturedMarkerPlugin::MarkerArrayCallback( + marti_visualization_msgs::msg::TexturedMarkerArray::ConstSharedPtr markers) +{ + for (const auto & marker : markers->markers) { + Q_EMIT MarkerReceived(marker); + } +} + +void TexturedMarkerPlugin::PrintError(const std::string & message) +{ + PrintErrorHelper(ui_.status, message); +} + +void TexturedMarkerPlugin::PrintInfo(const std::string & message) +{ + PrintInfoHelper(ui_.status, message); +} + +void TexturedMarkerPlugin::PrintWarning(const std::string & message) +{ + PrintWarningHelper(ui_.status, message); +} + +QWidget * TexturedMarkerPlugin::GetConfigWidget(QWidget * parent) +{ + config_widget_->setParent(parent); + + return config_widget_; +} + +bool TexturedMarkerPlugin::Initialize(QGLWidget * canvas) +{ + canvas_ = canvas; + + return true; +} + +void TexturedMarkerPlugin::Draw(double x, double y, double scale) +{ + rclcpp::Time now = rclcpp::Time(); + + float alphaVal = alphaVal_; // Set all markers to same alpha value + + std::map>::iterator nsIter; + for (nsIter = markers_.begin(); nsIter != markers_.end(); ++nsIter) { + std::map::iterator markerIter; + for (markerIter = nsIter->second.begin(); markerIter != nsIter->second.end(); ++markerIter) { + MarkerData & marker = markerIter->second; + marker.alpha_ = alphaVal; // Update current marker's alpha value + + if (marker.expire_time > now) { + if (marker.transformed) { + glEnable(GL_TEXTURE_2D); + + glBindTexture(GL_TEXTURE_2D, static_cast(marker.texture_id_)); + + glBegin(GL_TRIANGLES); + + glColor4f(1.0f, 1.0f, 1.0f, marker.alpha_); + + double marker_x = marker.texture_x_; + double marker_y = marker.texture_y_; + + glTexCoord2d(0, 0); glVertex2d( + marker.transformed_quad_[0].x(), marker.transformed_quad_[0].y()); + glTexCoord2d(marker_x, 0); glVertex2d( + marker.transformed_quad_[1].x(), marker.transformed_quad_[1].y()); + glTexCoord2d(marker_x, marker_y); glVertex2d( + marker.transformed_quad_[2].x(), marker.transformed_quad_[2].y()); + + glTexCoord2d(0, 0); glVertex2d( + marker.transformed_quad_[3].x(), marker.transformed_quad_[3].y()); + glTexCoord2d(marker_x, marker_y); glVertex2d( + marker.transformed_quad_[4].x(), marker.transformed_quad_[4].y()); + glTexCoord2d(0, marker_y); glVertex2d( + marker.transformed_quad_[5].x(), marker.transformed_quad_[5].y()); + + glEnd(); + + glBindTexture(GL_TEXTURE_2D, 0); + + glDisable(GL_TEXTURE_2D); + + PrintInfo("OK"); + } + } + } + } +} + +void TexturedMarkerPlugin::Transform() +{ + std::map>::iterator nsIter; + for (nsIter = markers_.begin(); nsIter != markers_.end(); ++nsIter) { + std::map::iterator markerIter; + for (markerIter = nsIter->second.begin(); markerIter != nsIter->second.end(); ++markerIter) { + swri_transform_util::Transform transform; + if (GetTransform(markerIter->second.source_frame_, markerIter->second.stamp, transform)) { + markerIter->second.transformed_quad_.clear(); + for (size_t i = 0; i < markerIter->second.quad_.size(); i++) { + markerIter->second.transformed_quad_.push_back(transform * markerIter->second.quad_[i]); + } + } + } + } +} + +void TexturedMarkerPlugin::LoadConfig(const YAML::Node & node, const std::string & path) +{ + if (node["topic"]) { + std::string topic = node["topic"].as(); + ui_.topic->setText(boost::trim_copy(topic).c_str()); + } + + TopicEdited(); +} + +void TexturedMarkerPlugin::SaveConfig(YAML::Emitter & emitter, const std::string & path) +{ + emitter << YAML::Key << "topic" << YAML::Value << + boost::trim_copy(ui_.topic->text().toStdString()); +} +} // namespace mapviz_plugins diff --git a/mapviz_plugins/src/tf_frame_plugin.cpp b/mapviz_plugins/src/tf_frame_plugin.cpp new file mode 100644 index 000000000..5bc23847f --- /dev/null +++ b/mapviz_plugins/src/tf_frame_plugin.cpp @@ -0,0 +1,255 @@ +// ***************************************************************************** +// +// Copyright (c) 2014-2020, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include + +// QT libraries +#include +#include + +#include + +// Declare plugin +#include + +// C++ standard libraries +#include +#include +#include +#include + +PLUGINLIB_EXPORT_CLASS(mapviz_plugins::TfFramePlugin, mapviz::MapvizPlugin) + +namespace mapviz_plugins +{ + TfFramePlugin::TfFramePlugin() + : PointDrawingPlugin() + , ui_() + , config_widget_(new QWidget()) + { + ui_.setupUi(config_widget_); + + ui_.color->setColor(Qt::green); + + // Set background white + QPalette p(config_widget_->palette()); + p.setColor(QPalette::Window, Qt::white); + config_widget_->setPalette(p); + + // Set status text red + QPalette p3(ui_.status->palette()); + p3.setColor(QPalette::Text, Qt::red); + ui_.status->setPalette(p3); + + QObject::connect(ui_.selectframe, SIGNAL(clicked()), this, + SLOT(SelectFrame())); + QObject::connect(ui_.frame, SIGNAL(editingFinished()), this, + SLOT(FrameEdited())); + QObject::connect(ui_.positiontolerance, SIGNAL(valueChanged(double)), this, + SLOT(PositionToleranceChanged(double))); + QObject::connect(ui_.buffersize, SIGNAL(valueChanged(int)), this, + SLOT(BufferSizeChanged(int))); + QObject::connect(ui_.drawstyle, SIGNAL(activated(QString)), this, + SLOT(SetDrawStyle(QString))); + QObject::connect(ui_.static_arrow_sizes, SIGNAL(clicked(bool)), + this, SLOT(SetStaticArrowSizes(bool))); + QObject::connect(ui_.arrow_size, SIGNAL(valueChanged(int)), + this, SLOT(SetArrowSize(int))); + QObject::connect(ui_.color, SIGNAL(colorEdited(const QColor&)), this, + SLOT(SetColor(const QColor&))); + QObject::connect(ui_.buttonResetBuffer, SIGNAL(pressed()), this, + SLOT(ClearPoints())); + } + + void TfFramePlugin::SelectFrame() + { + std::string frame = mapviz::SelectFrameDialog::selectFrame(tf_buf_); + if (!frame.empty()) + { + ui_.frame->setText(QString::fromStdString(frame)); + FrameEdited(); + } + } + + void TfFramePlugin::FrameEdited() + { + source_frame_ = ui_.frame->text().toStdString(); + PrintWarning("Waiting for transform."); + + RCLCPP_INFO(node_->get_logger(), "Setting target frame to to %s", source_frame_.c_str()); + + initialized_ = true; + } + + void TfFramePlugin::TimerCallback() + { + swri_transform_util::Transform transform; + if (GetTransform(node_->get_clock()->now(), transform)) + { + StampedPoint stamped_point; + stamped_point.point = transform.GetOrigin(); + stamped_point.orientation = transform.GetOrientation(); + stamped_point.source_frame = target_frame_; + stamped_point.stamp = tf2_ros::toMsg(transform.GetStamp()); + stamped_point.transformed = false; + + pushPoint( stamped_point ); + } + } + + void TfFramePlugin::PrintError(const std::string& message) + { + PrintErrorHelper(ui_.status, message); + } + + void TfFramePlugin::PrintInfo(const std::string& message) + { + PrintInfoHelper(ui_.status, message); + } + + void TfFramePlugin::PrintWarning(const std::string& message) + { + PrintWarningHelper(ui_.status, message); + } + + QWidget* TfFramePlugin::GetConfigWidget(QWidget* parent) + { + config_widget_->setParent(parent); + + return config_widget_; + } + + bool TfFramePlugin::Initialize(QGLWidget* canvas) + { + canvas_ = canvas; + + timer_ = node_->create_wall_timer(std::chrono::milliseconds(100), + std::bind(&TfFramePlugin::TimerCallback, this)); + + SetColor(ui_.color->color()); + + return true; + } + + void TfFramePlugin::Draw(double x, double y, double scale) + { + if (DrawPoints(scale)) + { + PrintInfo("OK"); + } + } + void TfFramePlugin::LoadConfig(const YAML::Node& node, + const std::string& path) + { + if (node["frame"]) + { + source_frame_ = node["frame"].as(); + ui_.frame->setText(source_frame_.c_str()); + } + + if (node["color"]) + { + std::string color = node["color"].as(); + QColor qcolor(color.c_str()); + SetColor(qcolor); + ui_.color->setColor(qcolor); + } + + if (node["draw_style"]) + { + std::string draw_style = node["draw_style"].as(); + + if (draw_style == "lines") + { + ui_.drawstyle->setCurrentIndex(0); + SetDrawStyle( LINES ); + } else if (draw_style == "points") { + ui_.drawstyle->setCurrentIndex(1); + SetDrawStyle( POINTS ); + } else if (draw_style == "arrows") { + ui_.drawstyle->setCurrentIndex(2); + SetDrawStyle( ARROWS ); + } + } + + if (node["position_tolerance"]) + { + auto position_tolerance = node["position_tolerance"].as(); + ui_.positiontolerance->setValue(position_tolerance); + PositionToleranceChanged(position_tolerance); + } + + if (node["buffer_size"]) + { + auto buffer_size = node["buffer_size"].as(); + ui_.buffersize->setValue(buffer_size); + BufferSizeChanged(buffer_size); + } + + if (node["static_arrow_sizes"]) + { + bool static_arrow_sizes = node["static_arrow_sizes"].as(); + ui_.static_arrow_sizes->setChecked(static_arrow_sizes); + SetStaticArrowSizes(static_arrow_sizes); + } + + if (node["arrow_size"]) + { + int arrow_size = node["arrow_size"].as(); + ui_.arrow_size->setValue(arrow_size); + SetArrowSize(arrow_size); + } + + FrameEdited(); + } + + void TfFramePlugin::SaveConfig(YAML::Emitter& emitter, + const std::string& path) + { + emitter << YAML::Key << "frame" << YAML::Value + << ui_.frame->text().toStdString(); + emitter << YAML::Key << "color" << YAML::Value + << ui_.color->color().name().toStdString(); + + std::string draw_style = ui_.drawstyle->currentText().toStdString(); + emitter << YAML::Key << "draw_style" << YAML::Value << draw_style; + + emitter << YAML::Key << "position_tolerance" << + YAML::Value << positionTolerance(); + + emitter << YAML::Key << "buffer_size" << YAML::Value << bufferSize(); + + emitter << YAML::Key + << "static_arrow_sizes" + << YAML::Value + << ui_.static_arrow_sizes->isChecked(); + + emitter << YAML::Key << "arrow_size" << YAML::Value << ui_.arrow_size->value(); + } +} // namespace mapviz_plugins diff --git a/mapviz_plugins/ui/attitude_indicator_config.ui b/mapviz_plugins/ui/attitude_indicator_config.ui new file mode 100644 index 000000000..9d9753818 --- /dev/null +++ b/mapviz_plugins/ui/attitude_indicator_config.ui @@ -0,0 +1,107 @@ + + + attitude_indicator_config + + + + 0 + 0 + 401 + 68 + + + + false + + + Form + + + + + + + 2 + + + 2 + + + 2 + + + 2 + + + 4 + + + + + + Sans Serif + 8 + + + + Status: + + + + + + + + Sans Serif + 8 + + + + + + + No topic + + + true + + + + + + + + Sans Serif + 10 + + + + Topic: + + + + + + + + + + false + + + Select + + + + + + + + + SelectColor() + SelectTopic() + TopicEdited() + PositionToleranceChanged(double) + AngleToleranceChanged(double) + + diff --git a/mapviz_plugins/ui/coordinate_picker_config.ui b/mapviz_plugins/ui/coordinate_picker_config.ui new file mode 100644 index 000000000..608896f37 --- /dev/null +++ b/mapviz_plugins/ui/coordinate_picker_config.ui @@ -0,0 +1,119 @@ + + + coordinate_picker_config + + + + 0 + 0 + 404 + 304 + + + + Form + + + + + + + 2 + + + 2 + + + 2 + + + 2 + + + 4 + + + + + + + + Status: + + + + + + + + + + + + + No topic + + + true + + + + + + + QPlainTextEdit::NoWrap + + + true + + + 4.000000000000000 + + + Qt::TextSelectableByKeyboard|Qt::TextSelectableByMouse + + + + + + + Frame: + + + + + + + + + + Select + + + + + + + Copy to Clipboard on Click + + + + + + + Clear List + + + + + + + + + SelectColor() + SelectTopic() + TopicEdited() + PositionToleranceChanged(double) + AngleToleranceChanged(double) + + diff --git a/mapviz_plugins/ui/disparity_config.ui b/mapviz_plugins/ui/disparity_config.ui new file mode 100644 index 000000000..50ec85fd5 --- /dev/null +++ b/mapviz_plugins/ui/disparity_config.ui @@ -0,0 +1,313 @@ + + + disparity_config + + + + 0 + 0 + 400 + 300 + + + + Form + + + + + + + 4 + + + 2 + + + + + + Sans Serif + 8 + + + + Status: + + + + + + + + 55 + 16777215 + + + + + Sans Serif + 8 + + + + + + + Select + + + + + + + + Sans Serif + 8 + + + + + + + + + Sans Serif + 8 + + + + Topic: + + + + + + + + 16777215 + 25 + + + + + Sans Serif + 9 + + + + + top left + + + + + top center + + + + + top right + + + + + center left + + + + + center + + + + + center right + + + + + bottom left + + + + + bottom center + + + + + bottom right + + + + + + + + + Sans Serif + 8 + + + + Anchor: + + + + + + + + Sans Serif + 8 + + + + Offset X: + + + + + + + 2000 + + + + + + + + Sans Serif + 8 + + + + + + + No topic + + + true + + + + + + + + Sans Serif + 8 + + + + Offset Y: + + + + + + + 2000 + + + + + + + + Sans Serif + 8 + + + + Width: + + + + + + + 2000 + + + 320 + + + + + + + + Sans Serif + 8 + + + + Height: + + + + + + + 2000 + + + 240 + + + + + + + + Sans Serif + 8 + + + + Units: + + + + + + + + 16777213 + 25 + + + + + Sans Serif + 9 + + + + + pixels + + + + + percent + + + + + + + + + + SelectColor() + SelectTopic() + TopicEdited() + PositionToleranceChanged(double) + AngleToleranceChanged(double) + + diff --git a/mapviz_plugins/ui/draw_polygon_config.ui b/mapviz_plugins/ui/draw_polygon_config.ui new file mode 100644 index 000000000..c171604c7 --- /dev/null +++ b/mapviz_plugins/ui/draw_polygon_config.ui @@ -0,0 +1,152 @@ + + + draw_polygon_config + + + + 0 + 0 + 404 + 304 + + + + Form + + + + + + + 2 + + + 2 + + + 2 + + + 2 + + + 4 + + + + + + + + + 24 + 24 + + + + false + + + + + + + + + + + + + + + + Topic: + + + + + + + Publish Polygon + + + + + + + + + + Status: + + + + + + + Clear + + + + + + + + + + Color: + + + + + + + + + + + + + No topic + + + true + + + + + + + Frame: + + + + + + + + + + Select + + + + + + + + mapviz::ColorButton + QPushButton +
mapviz/color_button.h
+ + + + + + SelectColor() + SelectTopic() + TopicEdited() + PositionToleranceChanged(double) + AngleToleranceChanged(double) + + diff --git a/mapviz_plugins/ui/float_config.ui b/mapviz_plugins/ui/float_config.ui new file mode 100644 index 000000000..b3a1c8424 --- /dev/null +++ b/mapviz_plugins/ui/float_config.ui @@ -0,0 +1,358 @@ + + + float_config + + + + 0 + 0 + 400 + 300 + + + + Form + + + + + + + 2 + + + 2 + + + 2 + + + 2 + + + 4 + + + + + + Sans Serif + 8 + + + + Status: + + + + + + + + 55 + 16777215 + + + + + Sans Serif + 8 + + + + + + + Select + + + + + + + + Sans Serif + 8 + + + + Topic: + + + + + + + + Sans Serif + 8 + + + + + + + + + Sans Serif + 8 + + + + Font: + + + + + + + + 16777215 + 16777215 + + + + + Sans Serif + 8 + + + + Helvetica + + + + + + + + Sans Serif + 8 + + + + Color: + + + + + + + + 24 + 24 + + + + false + + + + + + + + + + + + + + 16777215 + 25 + + + + + Sans Serif + 9 + + + + + top left + + + + + top center + + + + + top right + + + + + center left + + + + + center + + + + + center right + + + + + bottom left + + + + + bottom center + + + + + bottom right + + + + + + + + 2000 + + + + + + + + Sans Serif + 8 + + + + Anchor: + + + + + + + + Sans Serif + 8 + + + + Offset X: + + + + + + + + Sans Serif + 8 + + + + + + + No topic + + + true + + + + + + + 2000 + + + + + + + + Sans Serif + 8 + + + + Offset Y: + + + + + + + + Sans Serif + 8 + + + + Units: + + + + + + + + 16777213 + 25 + + + + + Sans Serif + 9 + + + + + pixels + + + + + percent + + + + + + + + + + + Postfix: + + + + + + + + mapviz::ColorButton + QPushButton +
mapviz/color_button.h
+ + + + + + SelectColor() + SelectTopic() + TopicEdited() + SelectFont() + PositionToleranceChanged(double) + AngleToleranceChanged(double) + + diff --git a/mapviz_plugins/ui/gps_config.ui b/mapviz_plugins/ui/gps_config.ui new file mode 100644 index 000000000..876e6b207 --- /dev/null +++ b/mapviz_plugins/ui/gps_config.ui @@ -0,0 +1,330 @@ + + + gps_config + + + + 0 + 0 + 400 + 222 + + + + Form + + + + + + + + + + Sans Serif + 8 + + + + Topic: + + + + + + + + Sans Serif + 8 + + + + + + + + + 55 + 16777215 + + + + + Sans Serif + 8 + + + + + + + Select + + + + + + + + Sans Serif + 8 + + + + Color: + + + + + + + + 24 + 24 + + + + false + + + + + + + + + + + + + + Sans Serif + 8 + + + + Draw Style: + + + + + + + + 16777215 + 25 + + + + + Sans Serif + 9 + + + + + lines + + + + + points + + + + + arrows + + + + + + + + + Sans Serif + 8 + + + + Static Arrow Sizes: + + + + + + + + + + + + + + + + 1 + + + 500 + + + 25 + + + Qt::Horizontal + + + + + + + + + + Sans Serif + 8 + + + + Position Tolerance: + + + + + + + + 0 + 0 + + + + QAbstractSpinBox::PlusMinus + + + + + + 1.000000000000000 + + + + + + + + Sans Serif + 8 + + + + Buffer Size: + + + + + + + + + QAbstractSpinBox::PlusMinus + + + 99999999 + + + + + + + + 55 + 16777215 + + + + Clear + + + + + + + + + Show Laps + + + + + + + + + + + + + + + Sans Serif + 8 + + + + Status: + + + + + + + + Sans Serif + 8 + + + + + + + No topic + + + true + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + mapviz::ColorButton + QPushButton +
mapviz/color_button.h
+ + + + + + SelectColor() + SelectTopic() + TopicEdited() + PositionToleranceChanged(double) + AngleToleranceChanged(double) + + diff --git a/mapviz_plugins/ui/grid_config.ui b/mapviz_plugins/ui/grid_config.ui new file mode 100644 index 000000000..f7a7239f5 --- /dev/null +++ b/mapviz_plugins/ui/grid_config.ui @@ -0,0 +1,328 @@ + + + grid_config + + + + 0 + 0 + 400 + 300 + + + + Form + + + + + + + 4 + + + 2 + + + + + + Sans Serif + 8 + + + + Frame: + + + + + + + + Sans Serif + 8 + + + + Color: + + + + + + + + 24 + 24 + + + + false + + + + + + + + + + + + + + Sans Serif + 8 + + + + X: + + + + + + + + Sans Serif + 8 + + + + Y: + + + + + + + + Sans Serif + 8 + + + + Size: + + + + + + + + Sans Serif + 8 + + + + Status: + + + + + + + + Sans Serif + 8 + + + + + + + No frame + + + true + + + + + + + + Sans Serif + 8 + + + + Rows: + + + + + + + + Sans Serif + 8 + + + + Columns: + + + + + + + + Sans Serif + 8 + + + + Alpha: + + + + + + + + 8 + + + + Select + + + + + + + + 8 + + + + + + + + true + + + 1.000000000000000 + + + 0.010000000000000 + + + 1.000000000000000 + + + + + + + + 0 + 0 + + + + QAbstractSpinBox::UpDownArrows + + + + + + -999999999.000000000000000 + + + 999999999.000000000000000 + + + 0.000000000000000 + + + + + + + + 0 + 0 + + + + QAbstractSpinBox::UpDownArrows + + + + + + -999999999.000000000000000 + + + 999999999.000000000000000 + + + 0.000000000000000 + + + + + + + QAbstractSpinBox::UpDownArrows + + + 999999999.000000000000000 + + + 1.000000000000000 + + + + + + + QAbstractSpinBox::PlusMinus + + + 1 + + + 10000 + + + + + + + QAbstractSpinBox::PlusMinus + + + 1 + + + 10000 + + + 1 + + + + + + + + mapviz::ColorButton + QPushButton +
mapviz/color_button.h
+ + + + + + SelectColor() + SelectTopic() + TopicEdited() + PositionToleranceChanged(double) + AngleToleranceChanged(double) + + diff --git a/mapviz_plugins/ui/image_config.ui b/mapviz_plugins/ui/image_config.ui new file mode 100644 index 000000000..31c5ff450 --- /dev/null +++ b/mapviz_plugins/ui/image_config.ui @@ -0,0 +1,367 @@ + + + image_config + + + + 0 + 0 + 396 + 371 + + + + Form + + + + + + + + + + Sans Serif + 8 + + + + Topic: + + + + + + + + 16777215 + 25 + + + + + default + + + + + + + + + Sans Serif + 8 + + + + Anchor: + + + + + + + Keep original aspect ratio + + + + + + + + Sans Serif + 8 + + + + Units: + + + + + + + + Sans Serif + 8 + + + + Height: + + + + + + + + 16777213 + 25 + + + + + Sans Serif + 9 + + + + + pixels + + + + + percent + + + + + + + + + Sans Serif + 8 + + + + Transport: + + + + + + + + Sans Serif + 8 + + + + Status: + + + + + + + + Sans Serif + 8 + + + + Width: + + + + + + + + Sans Serif + 8 + + + + + + + No topic + + + true + + + + + + + + Sans Serif + 8 + + + + Offset Y: + + + + + + + + Sans Serif + 8 + + + + Offset X: + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + 2000 + + + + + + + 2000 + + + + + + + + 16777215 + 25 + + + + + Sans Serif + 9 + + + + + top left + + + + + top center + + + + + top right + + + + + center left + + + + + center + + + + + center right + + + + + bottom left + + + + + bottom center + + + + + bottom right + + + + + + + + + Sans Serif + 8 + + + + + + + + + 55 + 16777215 + + + + + Sans Serif + 8 + + + + + + + Select + + + + + + + 0 + + + 1.000000000000000 + + + 10000.000000000000000 + + + 320.000000000000000 + + + + + + + 0 + + + 1.000000000000000 + + + 10000.000000000000000 + + + 240.000000000000000 + + + + + + + + + SelectColor() + SelectTopic() + TopicEdited() + PositionToleranceChanged(double) + AngleToleranceChanged(double) + + diff --git a/mapviz_plugins/ui/laserscan_config.ui b/mapviz_plugins/ui/laserscan_config.ui new file mode 100644 index 000000000..3997613c3 --- /dev/null +++ b/mapviz_plugins/ui/laserscan_config.ui @@ -0,0 +1,376 @@ + + + laserscan_config + + + + 0 + 0 + 400 + 300 + + + + Form + + + + + + + 2 + + + 2 + + + 2 + + + 2 + + + 4 + + + + + + Sans Serif + 8 + + + + Status: + + + + + + + + Sans Serif + 8 + + + + + + + No topic + + + true + + + + + + + + 55 + 16777215 + + + + + Sans Serif + 8 + + + + + + + Select + + + + + + + + Sans Serif + 8 + + + + + + + + + Sans Serif + 8 + + + + Topic: + + + + + + + + Sans Serif + 8 + + + + Min Value: + + + + + + + -999999999.000000000000000 + + + 999999999.000000000000000 + + + + + + + + Sans Serif + 8 + + + + Max Value: + + + + + + + -999999999.000000000000000 + + + 999999999.000000000000000 + + + 100.000000000000000 + + + + + + + 99999999 + + + 1 + + + + + + + 1.000000000000000 + + + 0.100000000000000 + + + 1.000000000000000 + + + + + + + + Sans Serif + 8 + + + + Alpha: + + + + + + + + + + + Sans Serif + 8 + + + + Point Size: + + + + + + + pixel + + + 1 + + + 3 + + + + + + + + Sans Serif + 8 + + + + Buffer Size: + + + + + + + + Sans Serif + 8 + + + + Color Transformer: + + + + + + + + + + Sans Serif + 8 + + + + Use Rainbow + + + + + + + + + + Sans Serif + 8 + false + + + + Min: + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 24 + 24 + + + + + + + + + + + + Sans Serif + 8 + + + + Max: + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 24 + 24 + + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + + Sans Serif + 8 + + + + Color: + + + + + + + + mapviz::ColorButton + QPushButton +
mapviz/color_button.h
+ + + + + + SelectColor() + SelectTopic() + TopicEdited() + PositionToleranceChanged(double) + AngleToleranceChanged(double) + + diff --git a/mapviz_plugins/ui/marker_config.ui b/mapviz_plugins/ui/marker_config.ui new file mode 100644 index 000000000..745e0c95c --- /dev/null +++ b/mapviz_plugins/ui/marker_config.ui @@ -0,0 +1,133 @@ + + + marker_config + + + + 0 + 0 + 400 + 300 + + + + Form + + + + + + + 2 + + + 2 + + + 2 + + + 2 + + + 4 + + + + + + Sans Serif + 8 + + + + + + + No topic + + + true + + + + + + + + Sans Serif + 8 + + + + + + + + + Sans Serif + 8 + + + + Status: + + + + + + + + 55 + 16777215 + + + + + Sans Serif + 8 + + + + + + + Select + + + + + + + + Sans Serif + 8 + + + + Topic: + + + + + + + Clear All Markers + + + + + + + + + + + + SelectColor() + SelectTopic() + TopicEdited() + PositionToleranceChanged(double) + AngleToleranceChanged(double) + + diff --git a/mapviz_plugins/ui/measuring_config.ui b/mapviz_plugins/ui/measuring_config.ui new file mode 100644 index 000000000..0639690e3 --- /dev/null +++ b/mapviz_plugins/ui/measuring_config.ui @@ -0,0 +1,269 @@ + + + measuring_config + + + + 0 + 0 + 300 + 240 + + + + + 8 + + + + Form + + + + + + + + + + 100 + 16777215 + + + + Distance: + + + + + + + + + + true + + + + + + + + 100 + 16777215 + + + + Total Distance: + + + + + + + + + + true + + + + + + + + 24 + 24 + + + + + + + false + + + + + + + Status: + + + + + + + + + + No topic + + + true + + + + + + + + + + true + + + + + + + Show Measurements: + + + + + + + + 140 + 16777215 + + + + Clear + + + + 16 + 16 + + + + + + + + + 24 + 24 + + + + + + + false + + + + + + + Main Color: + + + + + + + Background Color: + + + + + + + Show Background Color: + + + + + + + + + + true + + + + + + + + 48 + 16777215 + + + + QAbstractSpinBox::PlusMinus + + + 5 + + + 40 + + + 10 + + + + + + + Font Size: + + + + + + + + 48 + 16777215 + + + + QAbstractSpinBox::PlusMinus + + + 0.0 + + + 1.0 + + + 0.5 + + + 0.1 + + + + + + + Alpha: + + + + + + + + mapviz::ColorButton + QPushButton +
mapviz/color_button.h
+ + + + + + SelectColor() + SelectTopic() + TopicEdited() + PositionToleranceChanged(double) + AngleToleranceChanged(double) + + diff --git a/mapviz_plugins/ui/move_base_config.ui b/mapviz_plugins/ui/move_base_config.ui new file mode 100644 index 000000000..4700a653d --- /dev/null +++ b/mapviz_plugins/ui/move_base_config.ui @@ -0,0 +1,202 @@ + + + move_base_config + + + + 0 + 0 + 394 + 218 + + + + Form + + + + + + + + + 11 + + + + + + 65 + 0 + + + + + 65 + 16777215 + + + + + 8 + + + + <html><head/><body><p>Abort [2D Nav Goal] execution</p></body></html> + + + Abort + + + + :/images/remove-icon-th.png:/images/remove-icon-th.png + + + + + + + + 100 + 0 + + + + + 110 + 16777215 + + + + + 8 + + + + <html><head/><body><p>Publish robot pose on topic [/initialpose]</p></body></html> + + + Estimated Pose + + + + :/images/green-arrow.png:/images/green-arrow.png + + + true + + + + + + + + 100 + 0 + + + + + 100 + 16777215 + + + + + 8 + + + + <html><head/><body><p>Send a goal to [move_base] action server</p></body></html> + + + 2D Nav Goal + + + + :/images/green-arrow.png:/images/green-arrow.png + + + true + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + Qt::Horizontal + + + + + + + + + + + + Status: + + + + + + + + + + + + + No topic + + + true + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + + SelectColor() + SelectTopic() + TopicEdited() + PositionToleranceChanged(double) + AngleToleranceChanged(double) + + diff --git a/mapviz_plugins/ui/navsat_config.ui b/mapviz_plugins/ui/navsat_config.ui new file mode 100644 index 000000000..f811fa51e --- /dev/null +++ b/mapviz_plugins/ui/navsat_config.ui @@ -0,0 +1,282 @@ + + + navsat_config + + + + 0 + 0 + 400 + 159 + + + + Form + + + + + + + 2 + + + 2 + + + 2 + + + 2 + + + 4 + + + + + + Sans Serif + 8 + + + + Buffer Size: + + + + + + + + Sans Serif + 8 + + + + + + + No topic + + + true + + + + + + + + Sans Serif + 8 + + + + Position Tolerance: + + + + + + + QAbstractSpinBox::PlusMinus + + + 99999999 + + + + + + + + Sans Serif + 8 + + + + Color: + + + + + + + + 55 + 16777215 + + + + Clear + + + + + + + + 55 + 16777215 + + + + + Sans Serif + 8 + + + + + + + Select + + + + + + + + 0 + 0 + + + + QAbstractSpinBox::PlusMinus + + + + + + 1.000000000000000 + + + + + + + + Sans Serif + 8 + + + + + + + + + 24 + 24 + + + + false + + + + + + + + + + + + + + 16777215 + 25 + + + + + Sans Serif + 9 + + + + + lines + + + + + points + + + + + + + + + Sans Serif + 8 + + + + Topic: + + + + + + + + Sans Serif + 8 + + + + Status: + + + + + + + + Sans Serif + 8 + + + + Draw Style: + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + mapviz::ColorButton + QPushButton +
mapviz/color_button.h
+ + + + + + SelectColor() + SelectTopic() + TopicEdited() + PositionToleranceChanged(double) + AngleToleranceChanged(double) + + diff --git a/mapviz_plugins/ui/occupancy_grid_config.ui b/mapviz_plugins/ui/occupancy_grid_config.ui new file mode 100644 index 000000000..ad5601214 --- /dev/null +++ b/mapviz_plugins/ui/occupancy_grid_config.ui @@ -0,0 +1,181 @@ + + + occupancy_grid_config + + + + 0 + 0 + 347 + 123 + + + + Form + + + + + + + 2 + + + 2 + + + 2 + + + 2 + + + 4 + + + + + + 50 + 16777215 + + + + 1 + + + 1.000000000000000 + + + 0.100000000000000 + + + 1.000000000000000 + + + + + + + Subscribe to grid_updates + + + true + + + + + + + + Sans Serif + 8 + + + + Status: + + + + + + + + 8 + + + + Alpha: + + + + + + + + 8 + + + + + + + + + Sans Serif + 8 + + + + + + + No frame + + + true + + + + + + + + 100 + 16777215 + + + + + 8 + + + + Select Topic: + + + + + + + + 8 + + + + Color Scheme: + + + + + + + + 100 + 16777215 + + + + + map + + + + + costmap + + + + + + + + + + SelectColor() + SelectTopic() + TopicEdited() + PositionToleranceChanged(double) + AngleToleranceChanged(double) + + diff --git a/mapviz_plugins/ui/odometry_config.ui b/mapviz_plugins/ui/odometry_config.ui new file mode 100644 index 000000000..8dd0484b1 --- /dev/null +++ b/mapviz_plugins/ui/odometry_config.ui @@ -0,0 +1,409 @@ + + + odometry_config + + + + 0 + 0 + 286 + 312 + + + + Form + + + + + + + + + + Sans Serif + 8 + + + + Timestamp Interval: + + + + + + + + Sans Serif + 8 + + + + Topic: + + + + + + + + + + + + + + + + + + + + + + Sans Serif + 8 + + + + + + + + + 0 + 0 + + + + QAbstractSpinBox::PlusMinus + + + + + + 1.000000000000000 + + + + + + + + Sans Serif + 8 + + + + + + + No topic + + + true + + + + + + + + 24 + 24 + + + + false + + + + + + + + + + + + + + Sans Serif + 8 + + + + Status: + + + + + + + + 55 + 16777215 + + + + + Sans Serif + 8 + + + + + + + Select + + + + + + + + Sans Serif + 8 + + + + Buffer Size: + + + + + + + QAbstractSpinBox::PlusMinus + + + 99999999 + + + + + + + + 55 + 16777215 + + + + Clear + + + + + + + + Sans Serif + 8 + + + + Show Covariance: + + + + + + + + 16777215 + 25 + + + + + Sans Serif + 9 + + + + + lines + + + + + points + + + + + arrows + + + + + + + + + Sans Serif + 8 + + + + Draw Style: + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + 8 + + + + Show Laps + + + + + + + + Sans Serif + 8 + + + + Color: + + + + + + + + Sans Serif + 8 + + + + Static Arrow Sizes: + + + + + + + + Sans Serif + 8 + + + + Position Tolerance: + + + + + + + + 0 + 0 + + + + QAbstractSpinBox::PlusMinus + + + + + + 0.000000000000000 + + + + + + + 6 + + + + + + + + + + + + 1 + + + 500 + + + 25 + + + Qt::Horizontal + + + QSlider::NoTicks + + + + + + + + + + Sans Serif + 8 + + + + All Covariances: + + + + + + + + + + + + + + + mapviz::ColorButton + QPushButton +
mapviz/color_button.h
+ + + + + + SelectColor() + SelectTopic() + TopicEdited() + PositionToleranceChanged(double) + AngleToleranceChanged(double) + + diff --git a/mapviz_plugins/ui/path_config.ui b/mapviz_plugins/ui/path_config.ui new file mode 100644 index 000000000..5a43a980e --- /dev/null +++ b/mapviz_plugins/ui/path_config.ui @@ -0,0 +1,159 @@ + + + path_config + + + + 0 + 0 + 400 + 79 + + + + Form + + + + + + + 4 + + + 2 + + + + + + Sans Serif + 8 + + + + Status: + + + + + + + + 8 + + + + Color: + + + + + + + + Sans Serif + 8 + + + + + + + No topic + + + true + + + + + + + + 55 + 16777215 + + + + + Sans Serif + 8 + + + + + + + Select + + + + + + + + Sans Serif + 8 + + + + + + + + + Sans Serif + 8 + + + + Topic: + + + + + + + + 24 + 24 + + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + mapviz::ColorButton + QPushButton +
mapviz/color_button.h
+ + + + + + SelectColor() + SelectTopic() + TopicEdited() + PositionToleranceChanged(double) + AngleToleranceChanged(double) + + diff --git a/mapviz_plugins/ui/plan_route_config.ui b/mapviz_plugins/ui/plan_route_config.ui new file mode 100644 index 000000000..5d970b3a2 --- /dev/null +++ b/mapviz_plugins/ui/plan_route_config.ui @@ -0,0 +1,178 @@ + + + plan_route_config + + + + 0 + 0 + 404 + 304 + + + + Form + + + + + + + 2 + + + 2 + + + 2 + + + 2 + + + 4 + + + + + + + + + -1 + + + + + + + No topic + + + true + + + + + + + + -1 + + + + Preview Color: + + + + + + + + -1 + + + + Plan Route Service: + + + + + + + + 24 + 24 + + + + false + + + + + + + + + + + + + + -1 + + + + + + + + Publish Route + + + + + + + + -1 + + + + Status: + + + + + + + + -1 + + + + Route Topic: + + + + + + + + + + + + + + Start From Vehicle: + + + + + + + Clear + + + + + + + + mapviz::ColorButton + QPushButton +
mapviz/color_button.h
+ + + + + + SelectColor() + SelectTopic() + TopicEdited() + PositionToleranceChanged(double) + AngleToleranceChanged(double) + + diff --git a/mapviz_plugins/ui/point_click_publisher_config.ui b/mapviz_plugins/ui/point_click_publisher_config.ui new file mode 100644 index 000000000..341925eed --- /dev/null +++ b/mapviz_plugins/ui/point_click_publisher_config.ui @@ -0,0 +1,132 @@ + + + point_click_publisher_config + + + + 0 + 0 + 400 + 300 + + + + Form + + + + + + + 4 + + + 2 + + + + + + Sans Serif + 8 + + + + clicked_point + + + + + + + + Sans Serif + 8 + + + + Topic: + + + + + + + + Sans Serif + 8 + + + + Frame: + + + + + + + + 0 + 0 + + + + + 16777215 + 25 + + + + + Sans Serif + 8 + + + + The reference frame that points will be published in. + + + + true + + + + + + + + + Sans Serif + 8 + + + + Status: + + + + + + + + Sans Serif + 8 + + + + + + + No topic + + + true + + + + + + + + + diff --git a/mapviz_plugins/ui/pointcloud2_config.ui b/mapviz_plugins/ui/pointcloud2_config.ui new file mode 100644 index 000000000..745bae579 --- /dev/null +++ b/mapviz_plugins/ui/pointcloud2_config.ui @@ -0,0 +1,476 @@ + + + PointCloud2_config + + + + 0 + 0 + 307 + 341 + + + + Form + + + + + + + 2 + + + 2 + + + 2 + + + 2 + + + 4 + + + + + + 55 + 16777215 + + + + + Sans Serif + 8 + + + + + + + Select + + + + + + + + Sans Serif + 8 + + + + + + + + + Sans Serif + 8 + + + + Topic: + + + + + + + 1 + + + 100 + + + 1 + + + + + + + + + + 1.000000000000000 + + + 0.100000000000000 + + + 1.000000000000000 + + + + + + + + Sans Serif + 8 + + + + Alpha: + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + + + + Sans Serif + 8 + + + + Point Size: + + + + + + + pixel + + + 1 + + + 3 + + + + + + + + Sans Serif + 8 + + + + Buffer Size: + + + + + + + + Sans Serif + 8 + + + + Color Transformer: + + + + + + + + Sans Serif + 8 + + + + Color: + + + + + + + + + + Sans Serif + 8 + + + + Use Rainbow + + + + + + + + 8 + + + + Use Auto Max/Min + + + + + + + + 8 + + + + Unpack RGB + + + + + + + + 0 + + + 0 + + + 0 + + + 0 + + + + + + Sans Serif + 8 + false + + + + Min: + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 24 + 24 + + + + + + + + + + + + Sans Serif + 8 + + + + Max: + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 24 + 24 + + + + + + + + + + + Qt::Horizontal + + + + 4 + 20 + + + + + + + + + + + + + + 0 + 0 + + + + + 0 + + + 0 + + + 0 + + + 0 + + + 4 + + + + + + 99 + 0 + + + + + Sans Serif + 8 + + + + Min Value: + + + + + + + + Sans Serif + 8 + + + + Max Value: + + + + + + + -999999999.000000000000000 + + + 999999999.000000000000000 + + + + + + + -999999999.000000000000000 + + + 999999999.000000000000000 + + + 100.000000000000000 + + + + + + + + + + + Sans Serif + 8 + + + + + + + No topic + + + true + + + + + + + + Sans Serif + 8 + + + + Status: + + + + + + + + 55 + 16777215 + + + + Clear + + + + + + + + mapviz::ColorButton + QPushButton +
mapviz/color_button.h
+ + + + + + SelectColor() + SelectTopic() + TopicEdited() + PositionToleranceChanged(double) + AngleToleranceChanged(double) + + diff --git a/mapviz_plugins/ui/pose_config.ui b/mapviz_plugins/ui/pose_config.ui new file mode 100644 index 000000000..9659b12b5 --- /dev/null +++ b/mapviz_plugins/ui/pose_config.ui @@ -0,0 +1,330 @@ + + + pose_config + + + + 0 + 0 + 400 + 222 + + + + Form + + + + + + + + + + Sans Serif + 8 + + + + Topic: + + + + + + + + Sans Serif + 8 + + + + + + + + + 55 + 16777215 + + + + + Sans Serif + 8 + + + + + + + Select + + + + + + + + Sans Serif + 8 + + + + Color: + + + + + + + + 24 + 24 + + + + false + + + + + + + + + + + + + + Sans Serif + 8 + + + + Draw Style: + + + + + + + + 16777215 + 25 + + + + + Sans Serif + 9 + + + + + lines + + + + + points + + + + + arrows + + + + + + + + + Sans Serif + 8 + + + + Static Arrow Sizes: + + + + + + + + + + + + + + + + 1 + + + 500 + + + 25 + + + Qt::Horizontal + + + + + + + + + + Sans Serif + 8 + + + + Position Tolerance: + + + + + + + + 0 + 0 + + + + QAbstractSpinBox::PlusMinus + + + + + + 1.000000000000000 + + + + + + + + Sans Serif + 8 + + + + Buffer Size: + + + + + + + + + QAbstractSpinBox::PlusMinus + + + 99999999 + + + + + + + + 55 + 16777215 + + + + Clear + + + + + + + + + Show Laps + + + + + + + + + + + + + + + Sans Serif + 8 + + + + Status: + + + + + + + + Sans Serif + 8 + + + + + + + No topic + + + true + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + mapviz::ColorButton + QPushButton +
mapviz/color_button.h
+ + + + + + SelectColor() + SelectTopic() + TopicEdited() + PositionToleranceChanged(double) + AngleToleranceChanged(double) + + diff --git a/mapviz_plugins/ui/robot_image_config.ui b/mapviz_plugins/ui/robot_image_config.ui new file mode 100644 index 000000000..548be82ef --- /dev/null +++ b/mapviz_plugins/ui/robot_image_config.ui @@ -0,0 +1,315 @@ + + + robot_image_config + + + + 0 + 0 + 383 + 300 + + + + Form + + + + + + + + + + Sans Serif + 8 + + + + Image File: + + + + + + + + 55 + 16777215 + + + + + Sans Serif + 8 + + + + + + + Browse + + + + + + + + Sans Serif + 8 + + + + Frame: + + + + + + + + Sans Serif + 8 + + + + + + + + + 55 + 16777215 + + + + + Sans Serif + 8 + + + + + + + Select + + + + + + + + Sans Serif + 8 + + + + Width: + + + + + + + + 0 + 0 + + + + QAbstractSpinBox::PlusMinus + + + + + + 2.000000000000000 + + + + + + + + Sans Serif + 8 + + + + Height: + + + + + + + + 0 + 0 + + + + QAbstractSpinBox::PlusMinus + + + + + + 1.000000000000000 + + + + + + + + Sans Serif + 8 + + + + Offset x: + + + + + + + + 0 + 0 + + + + QAbstractSpinBox::PlusMinus + + + + + + 0.000000000000000 + + + + + + + + Sans Serif + 8 + + + + Offset y: + + + + + + + + 0 + 0 + + + + QAbstractSpinBox::PlusMinus + + + + + + 0.000000000000000 + + + + + + + + Sans Serif + 8 + + + + Status: + + + + + + + + Sans Serif + 8 + + + + + + + No topic + + + true + + + + + + + Image ratio: + + + + + + + + Sans Serif + 8 + + + + + + + + + + + Custom + + + + + + + 1:1 + + + + + + + Original + + + + + + + + + + + + SelectColor() + SelectTopic() + TopicEdited() + PositionToleranceChanged(double) + AngleToleranceChanged(double) + + diff --git a/mapviz_plugins/ui/route_config.ui b/mapviz_plugins/ui/route_config.ui new file mode 100644 index 000000000..5eb43aec9 --- /dev/null +++ b/mapviz_plugins/ui/route_config.ui @@ -0,0 +1,296 @@ + + + route_config + + + + 0 + 0 + 400 + 300 + + + + Form + + + + + + + 2 + + + 2 + + + 2 + + + 2 + + + 4 + + + + + + 24 + 24 + + + + + + + + + + + + 16777215 + 25 + + + + + Sans Serif + 9 + + + + 2 + + + 0 + + + + lines + + + + + points + + + + + + + + + Sans Serif + 8 + + + + + + + No topic + + + true + + + + + + + + Sans Serif + 8 + + + + Color: + + + + + + + + Sans Serif + 8 + + + + Topic: + + + + + + + + 55 + 16777215 + + + + + Sans Serif + 8 + + + + CrossCursor + + + + + + Select + + + + + + + + Sans Serif + 8 + + + + Draw Style: + + + + + + + + 24 + 24 + + + + false + + + + + + + + + + + + + + + + + Sans Serif + 8 + + + + + + + + + 8 + + + + Position Topic: + + + + + + + true + + + + 55 + 16777215 + + + + + 8 + + + + Select + + + + + + + + 8 + + + + Waypoint Color: + + + + + + + + Sans Serif + 8 + + + + Status: + + + + + + + 1 + + + 30 + + + Qt::Horizontal + + + + + + + + 8 + + + + Icon Size: + + + + + + + + mapviz::ColorButton + QPushButton +
mapviz/color_button.h
+ + + + + + SelectColor() + SelectTopic() + TopicEdited() + PositionToleranceChanged(double) + AngleToleranceChanged(double) + + diff --git a/mapviz_plugins/ui/string_config.ui b/mapviz_plugins/ui/string_config.ui new file mode 100644 index 000000000..3fa16e11c --- /dev/null +++ b/mapviz_plugins/ui/string_config.ui @@ -0,0 +1,339 @@ + + + string_config + + + + 0 + 0 + 400 + 300 + + + + Form + + + + + + + 4 + + + 2 + + + + + + Sans Serif + 8 + + + + Status: + + + + + + + + 55 + 16777215 + + + + + Sans Serif + 8 + + + + + + + Select + + + + + + + + Sans Serif + 8 + + + + + + + + + Sans Serif + 8 + + + + Topic: + + + + + + + + Sans Serif + 8 + + + + Font: + + + + + + + + 16777215 + 16777215 + + + + + Sans Serif + 8 + + + + Helvetica + + + + + + + + Sans Serif + 8 + + + + Color: + + + + + + + + 24 + 24 + + + + false + + + + + + + + + + + + + + 16777215 + 25 + + + + + Sans Serif + 9 + + + + + top left + + + + + top center + + + + + top right + + + + + center left + + + + + center + + + + + center right + + + + + bottom left + + + + + bottom center + + + + + bottom right + + + + + + + + + Sans Serif + 8 + + + + Anchor: + + + + + + + + Sans Serif + 8 + + + + Offset X: + + + + + + + 2000 + + + + + + + + Sans Serif + 8 + + + + + + + No topic + + + true + + + + + + + + Sans Serif + 8 + + + + Offset Y: + + + + + + + 2000 + + + + + + + + Sans Serif + 8 + + + + Units: + + + + + + + + 16777213 + 25 + + + + + Sans Serif + 9 + + + + + pixels + + + + + percent + + + + + + + + + mapviz::ColorButton + QPushButton +
mapviz/color_button.h
+ + + + + + SelectColor() + SelectTopic() + TopicEdited() + SelectFont() + PositionToleranceChanged(double) + AngleToleranceChanged(double) + + diff --git a/mapviz_plugins/ui/textured_marker_config.ui b/mapviz_plugins/ui/textured_marker_config.ui new file mode 100644 index 000000000..4699395ef --- /dev/null +++ b/mapviz_plugins/ui/textured_marker_config.ui @@ -0,0 +1,162 @@ + + + textured_marker_config + + + + 0 + 0 + 400 + 330 + + + + Form + + + + + + + 2 + + + 2 + + + 2 + + + 2 + + + 4 + + + + + + Sans Serif + 8 + + + + + + + No topic + + + true + + + + + + + + Sans Serif + 8 + + + + + + + + + Sans Serif + 8 + + + + Status: + + + + + + + + 55 + 16777215 + + + + + Sans Serif + 8 + + + + + + + Select + + + + + + + + Sans Serif + 8 + + + + Topic: + + + + + + + Clear All Markers + + + + + + + + Sans Serif + 8 + + + + Alpha: + + + + + + + 5 + + + 25 + + + 25 + + + Qt::Horizontal + + + QSlider::NoTicks + + + + + + + + + SelectColor() + SelectTopic() + TopicEdited() + PositionToleranceChanged(double) + AngleToleranceChanged(double) + + diff --git a/mapviz_plugins/ui/tf_frame_config.ui b/mapviz_plugins/ui/tf_frame_config.ui new file mode 100644 index 000000000..5e44049c9 --- /dev/null +++ b/mapviz_plugins/ui/tf_frame_config.ui @@ -0,0 +1,312 @@ + + + tf_frame_config + + + + 0 + 0 + 383 + 197 + + + + Form + + + + + + + + + + Sans Serif + 8 + + + + Color: + + + + + + + + 0 + 0 + + + + QAbstractSpinBox::PlusMinus + + + + + + 1.000000000000000 + + + + + + + + Sans Serif + 8 + + + + Frame: + + + + + + + + 16777215 + 25 + + + + + Sans Serif + 9 + + + + + lines + + + + + points + + + + + arrows + + + + + + + + + 55 + 16777215 + + + + + Sans Serif + 8 + + + + + + + Select + + + + + + + + 24 + 24 + + + + false + + + + + + + + + + + + + + 55 + 16777215 + + + + Clear + + + + + + + + Sans Serif + 8 + + + + Status: + + + + + + + + Sans Serif + 8 + + + + Buffer Size: + + + + + + + QAbstractSpinBox::PlusMinus + + + 99999999 + + + + + + + + Sans Serif + 8 + + + + Position Tolerance: + + + + + + + + Sans Serif + 8 + + + + + + + No topic + + + true + + + + + + + + + + + + + + + + 1 + + + 500 + + + 25 + + + Qt::Horizontal + + + + + + + + + + Sans Serif + 8 + + + + Draw Style: + + + + + + + + Sans Serif + 8 + + + + + + + + + Sans Serif + 8 + + + + Static Arrow Sizes: + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + mapviz::ColorButton + QPushButton +
mapviz/color_button.h
+ + + + + + SelectColor() + SelectTopic() + TopicEdited() + PositionToleranceChanged(double) + AngleToleranceChanged(double) + + diff --git a/mapviz_plugins/ui/topic_select.ui b/mapviz_plugins/ui/topic_select.ui new file mode 100644 index 000000000..4642c4c81 --- /dev/null +++ b/mapviz_plugins/ui/topic_select.ui @@ -0,0 +1,74 @@ + + + topicselect + + + + 0 + 0 + 400 + 300 + + + + Select Topic + + + true + + + + + + true + + + + + + + Qt::Vertical + + + QDialogButtonBox::Cancel|QDialogButtonBox::Ok + + + + + + + + + buttonBox + accepted() + topicselect + accept() + + + 306 + 243 + + + 157 + 274 + + + + + buttonBox + rejected() + topicselect + reject() + + + 316 + 260 + + + 286 + 274 + + + + + diff --git a/multires_image/CHANGELOG.rst b/multires_image/CHANGELOG.rst new file mode 100644 index 000000000..460df5203 --- /dev/null +++ b/multires_image/CHANGELOG.rst @@ -0,0 +1,129 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package multires_image +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.3.0 (2023-08-24) +------------------ + +2.2.2 (2023-06-07) +------------------ +* Iron Compatibility (`#779 `_) +* Contributors: David Anthony + +2.2.1 (2023-05-30) +------------------ +* Updating maintainers list (`#778 `_) +* Merge pull request `#754 `_ from cottsay/python3-shebang +* Use python3 in mapviz_tile_loader shebang +* Contributors: David Anthony, Scott K Logan + +2.1.0 (2020-10-22) +------------------ +* ROS Foxy support (`#695 `_) +* Contributors: P. J. Reed + +2.0.0 (2020-05-13) +------------------ +* Port mapviz to ROS 2 (`#672 `_) +* Remove OpenGL warning (`#667 `_) +* Contributors: Daniel D'Souza, P. J. Reed, Jacob Hassold, Kevin Nickels, Roger Strain + +1.2.0 (2019-09-04) +------------------ +* Use local_xy_origin for loading tiles of GPSFix not available (`#634 `_) +* Contributors: agyoungs + +1.1.1 (2019-05-17) +------------------ + +1.1.0 (2019-02-20) +------------------ + +1.0.1 (2019-01-25) +------------------ + +1.0.0 (2019-01-23) +------------------ +* Sharing tf_manager\_ between main app and plugins (`#555 `_) +* Contributors: Davide Faconti + +0.3.0 (2018-11-16) +------------------ +* Merge all -devel branches into a single master branch +* Contributors: P. J. Reed + +0.2.6 (2018-07-31) +------------------ + +0.2.5 (2018-04-12) +------------------ +* Add ability to set offset for multires image (`#565 `_) +* Fix multires image scale when projection is WGS84. +* update to use non deprecated pluginlib macro +* Mapviz tile loader (Kinetic) (`#509 `_) +* Change package.xml dep order +* Support transparent tiles in multires_image +* Contributors: Marc Alban, Mikael Arguedas, P. J. Reed, jgassaway + +0.2.4 (2017-08-11) +------------------ + +0.2.3 (2016-12-10) +------------------ + +0.2.2 (2016-12-07) +------------------ +* Migrated OpenCV to 3.1 (default in Kinetic) +* Contributors: Brian Holt + +0.2.1 (2016-10-23) +------------------ + +0.2.0 (2016-06-23) +------------------ +* Update Qt to version 5 +* Contributors: Ed Venator + +0.1.3 (2016-05-20) +------------------ +* Implement service for adding and modifying mapviz displays. +* Fix for `#339 `_; explicitly depending on OpenCV 2 +* Contributors: Marc Alban, P. J. Reed + +0.1.2 (2016-01-06) +------------------ +* Enables the possibility to load a one-layer tile set +* Contributors: Vincent Rousseau + +0.1.1 (2015-11-17) +------------------ +* Use extension from geo file +* Contributors: Vincent Rousseau + +0.1.0 (2015-09-29) +------------------ + +0.0.3 (2015-09-28) +------------------ + +0.0.2 (2015-09-27) +------------------ +* Adds missing qt4_opengl dependency + +0.0.1 (2015-09-27) +------------------ +* Renames all marti_common packages that were renamed. + (See http://github.com/swri-robotics/marti_common/issues/231) +* Fixes catkin_lint problems that could prevent installation. +* Cleans up dependencies +* Adds find_package(OpenCV REQUIRED) to cmake config +* fixes lint issues +* updates cmake version to squash the CMP0003 warning +* removes dependencies on build_tools +* uses format 2 package definition +* fix missing organization in license text +* exports the multires_image library +* catkinizes mapviz +* changes license to BSD +* adds license and readme files +* Contributors: Ed Venator, Edward Venator, Jerry Towler, Marc Alban, P. J. Reed diff --git a/multires_image/CMakeLists.txt b/multires_image/CMakeLists.txt new file mode 100644 index 000000000..a1ac52338 --- /dev/null +++ b/multires_image/CMakeLists.txt @@ -0,0 +1,169 @@ +### SET CMAKE POLICIES ### +cmake_minimum_required(VERSION 3.10...3.17) + +if(${CMAKE_VERSION} VERSION_LESS 3.12) + cmake_policy(VERSION ${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION}) +endif() + +if (NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(${CMAKE_VERSION} VERSION_EQUAL "3.11.0" OR ${CMAKE_VERSION} VERSION_GREATER "3.11.0") + cmake_policy(SET CMP0072 NEW) +endif() + +if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() +### END CMAKE POLICIES ### + +project(multires_image + LANGUAGES CXX) + +find_package(ament_cmake REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(gps_msgs REQUIRED) +find_package(mapviz REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tf2 REQUIRED) +find_package(swri_transform_util REQUIRED) +find_package(swri_math_util REQUIRED) + +### Boost ### +find_package(Boost REQUIRED filesystem thread) + +### OpenGL ### +find_package(OpenGL REQUIRED) + +### OpenCV ### +# multires_image doesn't directly use OpenCV, but it uses swri_transform_util, and +# we need to include OpenCV to make one of its headers compile. +find_package(OpenCV COMPONENTS core imgproc REQUIRED) + +find_package(Qt5Core REQUIRED) +find_package(Qt5Gui REQUIRED) +find_package(Qt5OpenGL REQUIRED) +find_package(Qt5Widgets REQUIRED) +add_definitions(-DWFlags=WindowFlags) + +set(CMAKE_INCLUDE_CURRENT_DIR ON) + +# Build libtile_cache +set(TILE_SRC_FILES + src/tile.cpp + src/tile_cache.cpp + src/tile_set.cpp + src/tile_set_layer.cpp) + +qt5_wrap_cpp(TILE_SRC_FILES include/${PROJECT_NAME}/tile_cache.h) + +add_library(${PROJECT_NAME} + ${TILE_SRC_FILES} +) +target_include_directories(${PROJECT_NAME} + PUBLIC + include) +target_link_libraries(${PROJECT_NAME} + ${Boost_LIBRARIES} + OpenGL::GL + Qt5::Core + Qt5::Gui + Qt5::OpenGL + Qt5::Widgets +) +ament_target_dependencies(${PROJECT_NAME} + ament_cmake + cv_bridge + gps_msgs + mapviz + pluginlib + rclcpp + tf2 + swri_transform_util + swri_math_util +) + +# Build libmultires_widget +set(UI_FILES + src/QGLMap.ui + src/multires_config.ui +) +set(WIDGET_SRC_FILES src/tile_view.cpp src/QGLMap.cpp) + +qt5_wrap_ui(WIDGET_SRC_FILES ${UI_FILES}) +qt5_wrap_cpp(WIDGET_SRC_FILES include/${PROJECT_NAME}/QGLMap.h) + +add_library(multires_widget + ${WIDGET_SRC_FILES} +) +target_link_libraries(multires_widget + ${PROJECT_NAME} + OpenGL::GL + Qt5::Core + Qt5::Gui + Qt5::OpenGL + Qt5::Widgets +) + +# Build nodes +set(VIEW_NODE_SRC_FILES src/nodes/multires_view_node.cpp) +qt5_wrap_cpp(VIEW_NODE_SRC_FILES include/${PROJECT_NAME}/multires_view_node.h) +add_executable(multires_view_node ${VIEW_NODE_SRC_FILES}) +target_link_libraries(multires_view_node + multires_widget + Boost::filesystem + Boost::thread +) +ament_target_dependencies(multires_view_node + ament_cmake + rclcpp + swri_transform_util +) + +# Build mapviz plugin +set(MAPVIZ_SRC_FILES + src/${PROJECT_NAME}_plugin.cpp + src/multires_view.cpp) +qt5_wrap_ui(MAPVIZ_SRC_FILES src/multires_config.ui) +qt5_wrap_cpp(MAPVIZ_SRC_FILES include/${PROJECT_NAME}/${PROJECT_NAME}_plugin.h) + +add_library(${PROJECT_NAME}_plugin SHARED + ${MAPVIZ_SRC_FILES} +) +target_compile_definitions(${PROJECT_NAME}_plugin PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +target_link_libraries(${PROJECT_NAME}_plugin + ${PROJECT_NAME} +) + +### Install ${PROJECT_NAME} plugin ### +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} + FILES_MATCHING PATTERN "*.h" +) + +install(TARGETS ${PROJECT_NAME} + multires_view_node + multires_widget + ${PROJECT_NAME}_plugin + RUNTIME DESTINATION lib/${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib +) + +install(PROGRAMS nodes/mapviz_tile_loader + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} + FILES_MATCHING PATTERN ".py" PATTERN ".launch" +) + +pluginlib_export_plugin_description_file(mapviz mapviz_plugins.xml) + +ament_export_dependencies(${RUNTIME_DEPS} Qt) +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_package() diff --git a/multires_image/include/multires_image/QGLMap.h b/multires_image/include/multires_image/QGLMap.h new file mode 100644 index 000000000..f56070aa1 --- /dev/null +++ b/multires_image/include/multires_image/QGLMap.h @@ -0,0 +1,110 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MULTIRES_IMAGE_QGLMAP_H_ +#define MULTIRES_IMAGE_QGLMAP_H_ + +// QT libraries +#include +#include +#include + +// QT auto-generated headers +#include "ui_QGLMap.h" + +#include + +#include +#include + +namespace multires_image +{ + class QGLMap : public QGLWidget + { + Q_OBJECT + + public: + explicit QGLMap(QWidget *parent = 0); + ~QGLMap() override = default; + + void Exit(); + void UpdateView(); + void SetTiles(TileSet* tiles); + + tf2::Vector3 SceneCenter() { return m_scene_center; } + tf2::Vector3 ViewCenter() { return m_view_center; } + + signals: + void SignalZoomChange(double z); + void SignalViewChange(double x1, double y1, double x2, double y2); + void SignalMemorySize(int64_t bytes); + + public slots: + void LoadTexture(Tile* tile); + void DeleteTexture(Tile* tile); + void ChangeCenter(double x, double y); + void SetTextureMemory(int64_t bytes); + + protected: + void initializeGL() override; + void resizeGL(int w, int h) override; + void paintGL() override; + void mousePressEvent(QMouseEvent* e) override; + void mouseDoubleClickEvent(QMouseEvent* e) override; + void mouseReleaseEvent(QMouseEvent* e) override; + void mouseMoveEvent(QMouseEvent* e) override; + void wheelEvent(QWheelEvent* e) override; + + private: + Ui::QGLMapClass ui; + + bool m_initialized; + + double m_scale; + + bool m_mouseDown; + int m_mouseDownX; + int m_mouseDownY; + + TileView* m_tileView; + + tf2::Vector3 m_view_top_left; + tf2::Vector3 m_view_bottom_right; + tf2::Vector3 m_view_center; + + tf2::Vector3 m_scene_top_left; + tf2::Vector3 m_scene_bottom_right; + tf2::Vector3 m_scene_center; + + void Recenter(); + void MousePan(int x, int y); + }; +} + +#endif // MULTIRES_IMAGE_QGLMAP_H_ diff --git a/multires_image/include/multires_image/multires_image_plugin.h b/multires_image/include/multires_image/multires_image_plugin.h new file mode 100644 index 000000000..570c93b14 --- /dev/null +++ b/multires_image/include/multires_image/multires_image_plugin.h @@ -0,0 +1,113 @@ +// ***************************************************************************** +// +// Copyright (c) 2014-2020, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MAPVIZ_PLUGINS_MULTIRES_IMAGE_PLUGIN_H_ +#define MAPVIZ_PLUGINS_MULTIRES_IMAGE_PLUGIN_H_ + +// C++ standard libraries +#include + +// Boost libraries +#include + +#include + +// QT libraries +#include +#include +#include + +#include +#include + +#include + +// QT autogenerated files +#include "ui_multires_config.h" + +namespace mapviz_plugins +{ + class MultiresImagePlugin : public mapviz::MapvizPlugin + { + Q_OBJECT + + public: + MultiresImagePlugin(); + ~MultiresImagePlugin() override; + + bool Initialize(QGLWidget* canvas) override; + void Shutdown() override {} + + void Draw(double x, double y, double scale) override; + + void Transform() override; + + void LoadConfig(const YAML::Node& node, const std::string& path) override; + void SaveConfig(YAML::Emitter& emitter, const std::string& path) override; + + QWidget* GetConfigWidget(QWidget* parent) override; + + protected: + void PrintError(const std::string& message) override; + void PrintInfo(const std::string& message) override; + void PrintWarning(const std::string& message) override; + + protected Q_SLOTS: + void SelectFile(); + void AcceptConfiguration(); + void SetXOffset(double long_offset); + void SetYOffset(double latitude_offset); + + private: + bool loaded_; + double center_x_; + double center_y_; + double offset_x_; + double offset_y_; + + multires_image::TileSet* tile_set_; + MultiresView* tile_view_; + + Ui::multires_config ui_; + QWidget* config_widget_; + + swri_transform_util::Transform transform_; + swri_transform_util::Transform inverse_transform_; + + bool transformed_; + + void GetCenterPoint(double x, double y); + + boost::filesystem::path MakePathRelative( + boost::filesystem::path path, + boost::filesystem::path base); + }; +} + +#endif // MAPVIZ_PLUGINS_MULTIRES_IMAGE_PLUGIN_H_ diff --git a/multires_image/include/multires_image/multires_view.h b/multires_image/include/multires_image/multires_view.h new file mode 100644 index 000000000..a94d743fb --- /dev/null +++ b/multires_image/include/multires_image/multires_view.h @@ -0,0 +1,68 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MAPVIZ_PLUGINS_MULTIRES_VIEW_H_ +#define MAPVIZ_PLUGINS_MULTIRES_VIEW_H_ + +// QT libraries +#include + +#include +#include + +namespace mapviz_plugins +{ +class MultiresView +{ + public: + MultiresView(multires_image::TileSet* tiles, QGLWidget* widget); + ~MultiresView() = default; + + const multires_image::TileCache* Cache() { return &m_cache; } + + void SetView(double x, double y, double radius, double scale); + + void Draw(); + + void Exit() { m_cache.Exit(); } + + private: + multires_image::TileSet* m_tiles; + multires_image::TileCache m_cache; + int m_currentLayer; + int m_startRow; + int m_startColumn; + int m_endRow; + int m_endColumn; + + double min_scale_; + }; +} + +#endif // MAPVIZ_PLUGINS_MULTIRES_VIEW_H_ diff --git a/multires_image/include/multires_image/multires_view_node.h b/multires_image/include/multires_image/multires_view_node.h new file mode 100644 index 000000000..b821ce6ab --- /dev/null +++ b/multires_image/include/multires_image/multires_view_node.h @@ -0,0 +1,84 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MULTIRES_IMAGE_MULTIRES_VIEW_NODE_H_ +#define MULTIRES_IMAGE_MULTIRES_VIEW_NODE_H_ + +// C++ standard libraries +#include + +// Boost libraries +#include + +// QT libraries +#include +#include +#include +#include + +// ROS libraries +#include + +#include +#include + +namespace multires_image +{ + class MultiresViewNode : public QMainWindow + { + Q_OBJECT + + public: + MultiresViewNode(int argc, char **argv, QWidget *parent = 0, Qt::WindowFlags flags = Qt::WindowFlags()); + ~MultiresViewNode() override = default; + + virtual void showEvent(QShowEvent* event) override; + + void Initialize(); + + void Spin(); + + private: + void SpinLoop(); + + int argc_; + char** argv_; + + rclcpp::Node::SharedPtr node_; + boost::thread* thread_; + + bool initialized_; + + std::string image_path_; + + TileSet* tile_set_; + }; +} + +#endif // MULTIRES_IMAGE_MULTIRES_VIEW_NODE_H_ diff --git a/multires_image/include/multires_image/tile.h b/multires_image/include/multires_image/tile.h new file mode 100644 index 000000000..6284e2241 --- /dev/null +++ b/multires_image/include/multires_image/tile.h @@ -0,0 +1,109 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MULTIRES_IMAGE_TILE_H_ +#define MULTIRES_IMAGE_TILE_H_ + +// C++ standard libraries +#include + +// QT libraries +#include +#include + +#include + +#include + +#ifndef GL_CLAMP_TO_EDGE +#define GL_CLAMP_TO_EDGE 0x812F +#endif + +namespace multires_image +{ + class Tile + { + public: + Tile( + const std::string& path, int column, int row, int level, + const tf2::Vector3& topLeft, + const tf2::Vector3& topRight, + const tf2::Vector3& bottomLeft, + const tf2::Vector3& bottomRight); + ~Tile() = default; + + bool Exists(); + bool Failed() const { return m_failed; } + bool TextureLoaded() const { return m_textureLoaded; } + const QImage& Image() const { return m_image; } + int64_t TileID() const { return m_tileId; } + int Layer() const { return m_level; } + int MemorySize() const { return m_memorySize; } + int Row() const { return m_row; } + int Column() const { return m_column; } + + bool LoadImageToMemory(bool gl = true); + void UnloadImage(); + + bool LoadTexture(); + void UnloadTexture(); + + void Draw(); + + void Transform(const swri_transform_util::Transform& transform); + void Transform(const swri_transform_util::Transform& transform, const swri_transform_util::Transform& offset_tf); + + private: + const std::string m_path; + const int m_column; + const int m_row; + const int m_level; + + tf2::Vector3 m_top_left; + tf2::Vector3 m_top_right; + tf2::Vector3 m_bottom_right; + tf2::Vector3 m_bottom_left; + + tf2::Vector3 m_transformed_top_left; + tf2::Vector3 m_transformed_top_right; + tf2::Vector3 m_transformed_bottom_right; + tf2::Vector3 m_transformed_bottom_left; + + bool m_failed; + bool m_textureLoaded; + int m_dimension; + int m_textureId; + int64_t m_tileId; + int m_memorySize; + QImage m_image; + QMutex m_mutex; + }; +} + +#endif // MULTIRES_IMAGE_TILE_H_ diff --git a/multires_image/include/multires_image/tile_cache.h b/multires_image/include/multires_image/tile_cache.h new file mode 100644 index 000000000..5392f6948 --- /dev/null +++ b/multires_image/include/multires_image/tile_cache.h @@ -0,0 +1,128 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MULTIRES_IMAGE_TILE_CACHE_H_ +#define MULTIRES_IMAGE_TILE_CACHE_H_ + +// C++ standard libraries +#include +#include +#include +#include + +// QT libraries +#include +#include +#include +#include + +#include + +#include +#include + +namespace multires_image +{ + class TileCache : public QObject + { + Q_OBJECT + + public: + TileCache(TileSet* tileSet, QGLWidget* widget); + ~TileCache() override; + + void Load(Tile* tile); + void Precache(const tf2::Vector3& position); + void Precache(double x, double y); + + void SetCurrentLayer(int layer) { m_currentLayer = layer; } + + void Exit(); + + public Q_SLOTS: + void LoadTextureSlot(Tile*); + void DeleteTextureSlot(Tile*); + + Q_SIGNALS: + void SignalLoadTexture(Tile*); + void SignalDeleteTexture(Tile*); + void SignalMemorySize(int64_t); + + private: + TileSet* m_tileSet; + QGLWidget* m_widget; + int32_t m_currentLayer; + tf2::Vector3 m_currentPosition; + bool m_exit; + int64_t m_memorySize; + + std::vector > m_precacheRequests; + std::stack m_renderRequests; + std::map m_textureLoaded; + std::map m_renderRequestSet; + std::map m_precacheRequestSet; + + void PrecacheLayer(int layer, const tf2::Vector3& position, int size); + void LoadTexture(Tile* tile); + void UnloadTexture(Tile* tile); + + class CacheThread : public QThread + { + public: + explicit CacheThread(TileCache* parent) : p(parent) {} + virtual void run(); + + private: + TileCache* p; + }; + friend class CacheThread; + + class FreeThread : public QThread + { + public: + explicit FreeThread(TileCache* parent) : p(parent) {} + virtual void run(); + + private: + TileCache* p; + }; + friend class FreeThread; + + CacheThread m_cacheThread; + FreeThread m_freeThread; + + QMutex m_renderRequestsLock; + QMutex m_renderRequestSetLock; + QMutex m_precacheRequestsLock; + QMutex m_precacheRequestSetLock; + QMutex m_textureLoadedLock; + }; +} + +#endif // MULTIRES_IMAGE_TILE_CACHE_H_ diff --git a/multires_image/include/multires_image/tile_set.h b/multires_image/include/multires_image/tile_set.h new file mode 100644 index 000000000..a8d9dfd38 --- /dev/null +++ b/multires_image/include/multires_image/tile_set.h @@ -0,0 +1,78 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MULTIRES_IMAGE_TILE_SET_H_ +#define MULTIRES_IMAGE_TILE_SET_H_ + +// C++ standard libraries +#include +#include + +#include + +#include + +namespace multires_image +{ + class TileSet + { + public: + explicit TileSet(const std::string& geofile); + TileSet(const std::string& geofile, const std::string extension); + explicit TileSet(const swri_transform_util::GeoReference& georeference); + TileSet(const swri_transform_util::GeoReference& georeference, + const std::string extension); + + ~TileSet(); + + bool Load(); + + int LayerCount() { return m_layerCount; } + int TileSize() { return m_tileSize; } + + swri_transform_util::GeoReference& GeoReference() { return m_geo; } + + TileSetLayer* GetLayer(int layer) { return m_layers[layer]; } + + private: + swri_transform_util::GeoReference m_geo; + int m_tileSize{}; + int m_width{}; + int m_height{}; + + std::string m_cacheDir; + std::string m_extension; + + int m_layerCount{}; + + std::vector m_layers; + }; +} + +#endif // MULTIRES_IMAGE_TILE_SET_H_ diff --git a/multires_image/include/multires_image/tile_set_layer.h b/multires_image/include/multires_image/tile_set_layer.h new file mode 100644 index 000000000..65d9e9f94 --- /dev/null +++ b/multires_image/include/multires_image/tile_set_layer.h @@ -0,0 +1,87 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MULTIRES_IMAGE_TILE_SET_LAYER_H_ +#define MULTIRES_IMAGE_TILE_SET_LAYER_H_ + +// C++ standard libraries +#include +#include + +#include + +#include + +#include + +namespace multires_image +{ + class TileSetLayer + { + public: + TileSetLayer( + const swri_transform_util::GeoReference& geo, + const std::string& path, + int tileSize, int layer); + + ~TileSetLayer() = default; + + bool Load(); + bool Load(const std::string extension); + + Tile* GetTile(int column, int row) { return m_tiles[column][row]; } + + void GetTileIndex(const tf2::Vector3& position, int& row, int& column) const; + void GetTileIndex(double x, double y, int& row, int& column) const; + void GetTileRange( + const tf2::Vector3& top_left, + const tf2::Vector3& bottom_right, + int& startRow, int& startColumn, + int& endRow, int& endColumn) const; + + int RowCount() { return m_rows; } + int ColumnCount() { return m_columns; } + + private: + const swri_transform_util::GeoReference& m_geo; + const std::string m_path; + const int m_tileSize; + const int m_layer; + const double m_scale; + + bool m_expectTiles; + + int m_columns; + int m_rows; + + std::vector > m_tiles; + }; +} + +#endif // MULTIRES_IMAGE_TILE_SET_LAYER_H_ diff --git a/multires_image/include/multires_image/tile_view.h b/multires_image/include/multires_image/tile_view.h new file mode 100644 index 000000000..638ad842a --- /dev/null +++ b/multires_image/include/multires_image/tile_view.h @@ -0,0 +1,67 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef MULTIRES_IMAGE_TILE_VIEW_H_ +#define MULTIRES_IMAGE_TILE_VIEW_H_ + +// QT libraries +#include + +#include +#include + +namespace multires_image +{ + class TileView + { + public: + TileView(TileSet* tiles, QGLWidget* widget); + ~TileView() = default; + + const TileCache* Cache() { return &m_cache; } + + void SetView(double x, double y, double radius, double scale); + + void Draw(); + + void Exit() { m_cache.Exit(); } + + private: + TileSet* m_tiles; + TileCache m_cache; + int m_currentLayer; + int m_startRow; + int m_startColumn; + int m_endRow; + int m_endColumn; + double min_scale_; + }; +} + +#endif // MULTIRES_IMAGE_TILE_VIEW_H_ diff --git a/multires_image/launch/mapviz_tile_loader.launch b/multires_image/launch/mapviz_tile_loader.launch new file mode 100644 index 000000000..2bffbed21 --- /dev/null +++ b/multires_image/launch/mapviz_tile_loader.launch @@ -0,0 +1,31 @@ + + + + + + + [{ name: swri, + latitude: 29.45196669, + longitude: -98.61370577, + altitude: 233.719, + heading: 0.0}, + + { name: back_40, + latitude: 29.447507, + longitude: -98.629367, + altitude: 200.0, + heading: 0.0}] + + + + + + + + + + + + + + diff --git a/multires_image/launch/multires.launch b/multires_image/launch/multires.launch new file mode 100644 index 000000000..d5178d01f --- /dev/null +++ b/multires_image/launch/multires.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/multires_image/mainpage.dox b/multires_image/mainpage.dox new file mode 100644 index 000000000..c595901e9 --- /dev/null +++ b/multires_image/mainpage.dox @@ -0,0 +1,33 @@ +/** +\mainpage +\section multires_image + +\b multires_image is a set of API's and nodes for ... + +This package... (see sumet_perception/material_classification/mainpage.dox for example description) + + +\subsection codeapi Code API + +The C++ API consists of the following main classes: + +- \b multires_image::GeoRegister - \copybrief multires_image::GeoRegister +- \b multires_image::ImageManip - \copybrief multires_image::ImageManip +- \b multires_image::MathUtil - \copybrief multires_image::MathUtil +- \b multires_image::Point - \copybrief multires_image::Point +- \b multires_image::QGLMap - \copybrief multires_image::QGLMap +- \b multires_image::StringUtil - \copybrief multires_image::StringUtil +- \b multires_image::TileCache - \copybrief multires_image::TileCache +- \b multires_image::TileSetLayer - \copybrief multires_image::TileSetLayer +- \b multires_image::TileSet - \copybrief multires_image::TileSet +- \b multires_image::TileView - \copybrief multires_image::TileView +- \b multires_image::Tile - \copybrief multires_image::Tile + + +\subsection nodes Nodes + +\subsubsection multires_view_node +\copydetails src/nodes/multires_view_node.cpp + + +**/ diff --git a/multires_image/mapviz_plugins.xml b/multires_image/mapviz_plugins.xml new file mode 100644 index 000000000..81aabc578 --- /dev/null +++ b/multires_image/mapviz_plugins.xml @@ -0,0 +1,6 @@ + + + Multires image mapviz plugin. + + + diff --git a/multires_image/nodes/mapviz_tile_loader b/multires_image/nodes/mapviz_tile_loader new file mode 100755 index 000000000..66f3a7d2e --- /dev/null +++ b/multires_image/nodes/mapviz_tile_loader @@ -0,0 +1,204 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +# +# Copyright (C) 2016 All Right Reserved, Southwest Research Institute® (SwRI®) +# + +import fnmatch +import math +import os +import pyproj +import rospy +import yaml +from collections import namedtuple +from geometry_msgs.msg import PoseStamped +from gps_common.msg import GPSFix +from mapviz.srv import AddMapvizDisplay, AddMapvizDisplayRequest +from marti_common_msgs.msg import KeyValue + +GeoReference = namedtuple("GeoReference", "path min_lat min_lon max_lat max_lon area") +_gps_fix = None + +def distance(lat1, lon1, lat2, lon2): + x1 = math.radians(lat1) + y1 = math.radians(lon1) + x2 = math.radians(lat2) + y2 = math.radians(lon2) + return math.acos(math.sin(x1) * math.sin(x2) + math.cos(x1) * math.cos(x2) * math.cos(y1 - y2)) + +def gps_callback(data): + global _gps_fix + _gps_fix = data + +def load_tiles(): + rospy.init_node('mapviz_tile_loader', anonymous=True) + + base_directory = rospy.get_param('~base_directory', os.path.expanduser('~') + "/.ros") + max_search_depth = rospy.get_param('~max_search_depth', 1) + rate = max(0.1, rospy.get_param('~rate', 1.0)) + display_name = rospy.get_param('~display_name', 'satellite') + draw_order = rospy.get_param('~draw_order', 1) + use_local_xy = rospy.get_param('~use_local_xy', False) + + gps_sub = rospy.Subscriber("gps", GPSFix, gps_callback) + + geofiles = [] + initial_depth = base_directory.count(os.sep) + for path, dirs, filenames in os.walk(base_directory): + for filename in fnmatch.filter(filenames, '*.geo'): + geofiles.append(os.path.join(path, filename)) + current_depth = path.count(os.sep) - initial_depth + if current_depth >= max_search_depth: + dirs[:] = [] + + georeferences = [] + for geofile in geofiles: + rospy.loginfo("Parsing %s...", geofile) + f = open(geofile) + min_lat = 90.0 + max_lat = -90.0 + min_lon = 180.0 + max_lon = -180.0 + geodata = yaml.safe_load(f) + if str(geodata['projection']).lower() == 'utm': + rospy.loginfo(" projection: utm") + zone = 1 + band = 'N' + if ('utm_zone' in geodata): + zone = int(geodata['utm_zone']) + if zone >= 1 or zone <= 60: + rospy.loginfo(" utm zone: %d", zone) + else: + rospy.logwarn(" invalid utm zone!") + f.close() + continue + else: + rospy.logwarn(" no utm zone!") + f.close() + continue + if ('utm_band' in geodata): + band = str(geodata['utm_band']) + if band >= 'C' or band <= 'X': + rospy.loginfo(" utm band: %s", band) + else: + rospy.logwarn(" invalid utm band!") + f.close() + continue + else: + rospy.logwarn(" no utm band!") + f.close() + continue + + utm_proj = pyproj.Proj(proj='utm', zone=zone, ellps='WGS84', south=(band < 'N')) + + if 'tiepoints' in geodata: + if len(geodata['tiepoints']) > 1: + for point in geodata['tiepoints']: + easting = point['point'][2] + northing = point['point'][3] + lon, lat = utm_proj(easting, northing, inverse=True) + min_lon = min(lon, min_lon) + max_lon = max(lon, max_lon) + min_lat = min(lat, min_lat) + max_lat = max(lat, max_lat) + else: + rospy.logwarn(" not enough tiepoints!") + f.close() + continue + else: + rospy.logwarn(" no tiepoints!") + f.close() + continue + + area = (max_lat - min_lat) * (max_lon - min_lon) + + georeferences.append(GeoReference(geofile, min_lat, min_lon, max_lat, max_lon, area)) + rospy.loginfo(" lat/lon bounds: (%lf, %lf) - (%lf, %lf)", min_lat, min_lon, max_lat, max_lon) + + f.close() + + rospy.loginfo("waiting for service: %s ...", rospy.resolve_name('add_mapviz_display')) + rospy.wait_for_service('add_mapviz_display') + + last_path = None + while not rospy.is_shutdown(): + if use_local_xy: + if last_path is None and georeferences: + # Select the geofile that contains the current local_xy origin point. If multiple + # geo-files contain the point, select the largest. If no geo-files + # contain the point, select the closest one. + + # get local xy origin + local_xy = rospy.wait_for_message("/local_xy_origin", PoseStamped) + max_area = 0 + path = None + for georef in georeferences: + if local_xy.pose.position.y > georef.min_lat and local_xy.pose.position.y < georef.max_lat and local_xy.pose.position.x > georef.min_lon and local_xy.pose.position.x < georef.max_lon and georef.area > max_area: + path = georef.path + max_area = georef.area + + if path is None: + min_dist = float("inf") + for georef in georeferences: + lat = (georef.min_lat + georef.max_lat) / 2.0 + lon = (georef.min_lon + georef.max_lon) / 2.0 + dist = distance(lat, lon, local_xy.pose.position.y, local_xy.pose.position.x) + if (dist < min_dist): + min_dist = dist + path = georef.path + + # If the geo-file has changed, call the service for adding/updating + # the mapviz display + if path is not None and path != last_path: + rospy.loginfo("updating tileset: %s", path); + add_mapviz_display = rospy.ServiceProxy('add_mapviz_display', AddMapvizDisplay) + request = AddMapvizDisplayRequest(type='mapviz_plugins/multires_image', draw_order=draw_order, name=display_name, visible=True) + request.properties.append(KeyValue(key='path', value=path)) + response = add_mapviz_display(request) + if response.success: + last_path = path + else: + rospy.logwarn("failed to update tileset: %s", response.message) + else: + if _gps_fix is not None: + # Select the geofile that contains the current GPS point. If multiple + # geo-files contain the point, select the largest. If no geo-files + # contain the point, select the closest one. + max_area = 0 + path = None + for georef in georeferences: + if _gps_fix.latitude > georef.min_lat and _gps_fix.latitude < georef.max_lat and _gps_fix.longitude > georef.min_lon and _gps_fix.longitude < georef.max_lon and georef.area > max_area: + path = georef.path + max_area = georef.area + + if path is None: + min_dist = float("inf") + for georef in georeferences: + lat = (georef.min_lat + georef.max_lat) / 2.0 + lon = (georef.min_lon + georef.max_lon) / 2.0 + dist = distance(lat, lon, _gps_fix.latitude, _gps_fix.longitude) + if (dist < min_dist): + min_dist = dist + path = georef.path + + # If the geo-file has changed, call the service for adding/updating + # the mapviz display + if path is not None and path != last_path: + rospy.loginfo("updating tileset: %s", path); + add_mapviz_display = rospy.ServiceProxy('add_mapviz_display', AddMapvizDisplay) + request = AddMapvizDisplayRequest(type='mapviz_plugins/multires_image', draw_order=draw_order, name=display_name, visible=True) + request.properties.append(KeyValue(key='path', value=path)) + response = add_mapviz_display(request) + if response.success: + last_path = path + else: + rospy.logwarn("failed to update tileset: %s", response.message) + else: + rospy.loginfo("waiting for gps message: %s", rospy.resolve_name('gps')) + rospy.sleep(1.0 / rate) + +if __name__ == '__main__': + try: + load_tiles() + except rospy.ROSInterruptException: pass diff --git a/multires_image/package.xml b/multires_image/package.xml new file mode 100644 index 000000000..f9ef40e51 --- /dev/null +++ b/multires_image/package.xml @@ -0,0 +1,41 @@ + + multires_image + 2.3.0 + + + multires_image + + + Marc Alban + P. J. Reed + Southwest Research Institute + BSD + https://github.com/swri-robotics/mapviz + + ament_cmake + qt5-qmake + + libqt5-core + libqt5-opengl-dev + + cv_bridge + geometry_msgs + gps_msgs + mapviz + pluginlib + rclcpp + swri_math_util + swri_transform_util + tf2 + + libqt5-opengl + rclpy + + + ament_cmake + + + + + + diff --git a/multires_image/src/QGLMap.cpp b/multires_image/src/QGLMap.cpp new file mode 100644 index 000000000..834a9a6e8 --- /dev/null +++ b/multires_image/src/QGLMap.cpp @@ -0,0 +1,301 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include + +// C++ standard libraries +#include + +namespace multires_image +{ +QGLMap::QGLMap(QWidget *parent) + : QGLWidget(parent) + , ui() + , m_initialized(false) + , m_scale(1.0) + , m_mouseDown(false) + , m_mouseDownX(0) + , m_mouseDownY(0) + , m_tileView(nullptr) + , m_view_top_left(0, 0, 0) + , m_view_bottom_right(0, 0, 0) + , m_view_center(0, 0, 0) + , m_scene_top_left(0, 0, 0) + , m_scene_bottom_right(0, 0, 0) + , m_scene_center(0, 0, 0) +{ + ui.setupUi(this); +} + +void QGLMap::Exit() +{ + if (m_tileView != nullptr) + { + m_tileView->Exit(); + } +} + +void QGLMap::UpdateView() +{ + if (m_initialized) + { + Recenter(); + + if (m_tileView != nullptr) + { + m_tileView->SetView(m_view_center.x(), m_view_center.y(), 1, m_scale); + } + + glViewport(0, 0, width(), height()); + glMatrixMode(GL_PROJECTION); + glLoadIdentity(); + glOrtho(m_view_top_left.x(), m_view_bottom_right.x(), + m_view_bottom_right.y(), m_view_top_left.y(), -0.5f, 0.5f); + + update(); + + // Signal a view change as occured. The minimap listens for this + // so that it can update its view box. + emit SignalViewChange(m_view_top_left.x(), m_view_top_left.y(), + m_view_bottom_right.x(), m_view_bottom_right.y()); + } +} + +void QGLMap::SetTiles(TileSet* tiles) +{ + double top, left, bottom, right; + tiles->GeoReference().GetCoordinate(0, 0, left, top); + tiles->GeoReference().GetCoordinate( + tiles->GeoReference().Width(), + tiles->GeoReference().Height(), + right, + bottom); + + m_scene_top_left = tf2::Vector3(left, top, 0); + m_scene_bottom_right = tf2::Vector3(right, bottom, 0); + m_scene_center = (m_scene_top_left + m_scene_bottom_right) / 2.0; + + m_view_center = m_scene_center; + + m_tileView = new TileView(tiles, this); + + connect(m_tileView->Cache(), SIGNAL(SignalMemorySize(int64_t)), + SLOT(SetTextureMemory(int64_t))); + + // Create connections for the texture loading functions which must + // be executed on this object's thread. + + m_tileView->SetView(m_view_center.x(), m_view_center.y(), 1, m_scale); +} + +void QGLMap::wheelEvent(QWheelEvent* e) +{ + float numDegrees = static_cast(e->angleDelta().y()) / -8.0f; + + m_scale *= pow(1.1, numDegrees / 10.0); + + UpdateView(); +} + +void QGLMap::LoadTexture(Tile* tile) +{ + tile->LoadTexture(); +} + +void QGLMap::DeleteTexture(Tile* tile) +{ + tile->UnloadTexture(); +} + +void QGLMap::SetTextureMemory(int64_t bytes) +{ + // Signal that the texture memory size has changed. The status bar listens + // to this so that the user can see how much memory the map is using. + emit SignalMemorySize(bytes); +} + +void QGLMap::ChangeCenter(double x, double y) +{ + if (x != 0) + m_view_center.setX(x); + + if (y != 0) + m_view_center.setY(y); + + UpdateView(); +} + +void QGLMap::initializeGL() +{ + glClearColor(0.58f, 0.56f, 0.5f, 1); + glEnable(GL_POINT_SMOOTH); + glEnable(GL_LINE_SMOOTH); + glEnable(GL_POLYGON_SMOOTH); + glHint(GL_LINE_SMOOTH_HINT, GL_NICEST); + glHint(GL_POINT_SMOOTH_HINT, GL_NICEST); + glHint(GL_POLYGON_SMOOTH_HINT, GL_NICEST); + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + glDepthFunc(GL_NEVER); + glDisable(GL_DEPTH_TEST); + m_initialized = true; +} + +void QGLMap::resizeGL(int w, int h) +{ + UpdateView(); +} + +void QGLMap::paintGL() +{ + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + if (m_tileView != nullptr) + { + m_tileView->Draw(); + } +} + +void QGLMap::mousePressEvent(QMouseEvent* e) +{ + m_mouseDownX = e->x(); + m_mouseDownY = e->y(); + m_mouseDown = true; + + update(); +} + +void QGLMap::mouseDoubleClickEvent(QMouseEvent* e) +{ + update(); +} + +void QGLMap::mouseReleaseEvent(QMouseEvent* e) +{ + m_mouseDown = false; + + update(); +} + +void QGLMap::mouseMoveEvent(QMouseEvent* e) +{ + if (m_mouseDown) + MousePan(e->x(), e->y()); +} + +void QGLMap::MousePan(int x, int y) +{ + bool changed = false; + if (m_mouseDown) + { + double diffX = ((m_mouseDownX - x) * m_scale); + double diffY = ((m_mouseDownY - y) * m_scale); + + if (diffX != 0) + { + m_view_center.setX(m_view_center.x() + diffX); + m_mouseDownX = x; + changed = true; + } + if (diffY != 0) + { + m_view_center.setY(m_view_center.y() + diffY); + m_mouseDownY = y; + changed = true; + } + } + + if (changed) + { + UpdateView(); + } +} + +void QGLMap::Recenter() +{ + double scene_width = std::fabs(m_scene_top_left.x() - m_scene_bottom_right.x()); + double scene_height = std::fabs(m_scene_top_left.y() - m_scene_bottom_right.y()); + double view_width = width() * m_scale; + double view_height = height() * m_scale; + + m_view_top_left.setX(m_view_center.x() - (view_width * 0.5)); + m_view_top_left.setY(m_view_center.y() - (view_width * 0.5)); + + m_view_bottom_right.setX(m_view_center.x() + (view_width * 0.5)); + m_view_bottom_right.setY(m_view_center.y() + (view_width * 0.5)); + + if (view_width > scene_width) + { + m_view_center.setX(m_scene_center.x()); + m_view_top_left.setX(m_view_center.x() - (view_width * 0.5)); + m_view_bottom_right.setX(m_view_center.x() + (view_width * 0.5)); + } + else + { + if (m_view_top_left.x() < m_scene_top_left.x()) + { + m_view_top_left.setX(m_scene_top_left.x()); + m_view_bottom_right.setX(m_view_top_left.x() + view_width); + m_view_center.setX(m_view_top_left.x() + (view_width * 0.5)); + } + + if (m_view_bottom_right.x() > m_scene_bottom_right.x()) + { + m_view_bottom_right.setX(m_scene_bottom_right.x()); + m_view_top_left.setX(m_view_bottom_right.x() - view_width); + m_view_center.setX(m_view_top_left.x() + (view_width * 0.5)); + } + } + + if (view_height < scene_height) + { + m_view_center.setY(m_scene_center.y()); + m_view_top_left.setY(m_scene_center.y() - (view_height * 0.5)); + m_view_bottom_right.setY(m_scene_center.y() + (view_height * 0.5)); + } + else + { + if (m_view_top_left.y() > m_scene_top_left.y()) + { + m_view_top_left.setY(m_scene_top_left.y()); + m_view_bottom_right.setY(m_view_top_left.y() + (view_height)); + m_view_center.setY(m_view_top_left.y() + (view_height * 0.5)); + } + + if (m_view_bottom_right.y() < m_scene_bottom_right.y()) + { + m_view_bottom_right.setY(m_scene_bottom_right.y()); + m_view_top_left.setY(m_view_bottom_right.y() - (view_height)); + m_view_center.setY(m_view_top_left.y() + (view_height * 0.5)); + } + } +} + +} + diff --git a/multires_image/src/QGLMap.ui b/multires_image/src/QGLMap.ui new file mode 100644 index 000000000..cab3bba1c --- /dev/null +++ b/multires_image/src/QGLMap.ui @@ -0,0 +1,32 @@ + + + QGLMapClass + + + + 0 + 0 + 400 + 300 + + + + QGLMap + + + + + + QGLWidget + QWidget +
qglwidget.h
+ 1 + + + + + + LoadTexture(Tile*tile) + DeleteTexture(Tile*tile) + + diff --git a/multires_image/src/multires_config.ui b/multires_image/src/multires_config.ui new file mode 100644 index 000000000..7d2343097 --- /dev/null +++ b/multires_image/src/multires_config.ui @@ -0,0 +1,165 @@ + + + multires_config + + + + 0 + 0 + 400 + 300 + + + + Form + + + + + + + 2 + + + 2 + + + 2 + + + 2 + + + 4 + + + + + + Sans Serif + 8 + + + + Status: + + + + + + + + Sans Serif + 8 + + + + + + + Unconfigured + + + true + + + + + + + + 55 + 16777215 + + + + + Sans Serif + 8 + + + + + + + Browse + + + + + + + + Sans Serif + 8 + + + + Geo File: + + + + + + + + Sans Serif + 8 + + + + + + + + + Sans + 8 + + + + X (East) offset + + + + + + + + Sans + 8 + + + + Y (North) offset + + + + + + + -100.000000000000000 + + + 0.100000000000000 + + + 0.000000000000000 + + + + + + + -100.000000000000000 + + + 0.100000000000000 + + + + + + + + diff --git a/multires_image/src/multires_image_plugin.cpp b/multires_image/src/multires_image_plugin.cpp new file mode 100644 index 000000000..f215702c4 --- /dev/null +++ b/multires_image/src/multires_image_plugin.cpp @@ -0,0 +1,363 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include + +// C++ standard libraries +#include + +// QT libraries +#include +#include +#include + +// ROS libraries +#include +#include + +// Declare plugin +#include +PLUGINLIB_EXPORT_CLASS(mapviz_plugins::MultiresImagePlugin, mapviz::MapvizPlugin) + +namespace mapviz_plugins +{ + MultiresImagePlugin::MultiresImagePlugin() + : MapvizPlugin() + , ui_() + , loaded_(false) + , center_x_(0.0) + , center_y_(0.0) + , offset_x_(0.0) + , offset_y_(0.0) + , tile_set_(nullptr) + , tile_view_(nullptr) + , config_widget_(new QWidget()) + , transformed_(false) + { + ui_.setupUi(config_widget_); + + QPalette p(config_widget_->palette()); + p.setColor(QPalette::Window, Qt::white); + config_widget_->setPalette(p); + + QPalette p2(ui_.status->palette()); + p2.setColor(QPalette::Text, Qt::red); + ui_.status->setPalette(p2); + + QObject::connect(ui_.browse, SIGNAL(clicked()), this, SLOT(SelectFile())); + QObject::connect(ui_.path, SIGNAL(editingFinished()), this, SLOT(AcceptConfiguration())); + QObject::connect(ui_.x_offset_spin_box, SIGNAL(valueChanged(double)), this, SLOT(SetXOffset(double))); + QObject::connect(ui_.y_offset_spin_box, SIGNAL(valueChanged(double)), this, SLOT(SetYOffset(double))); + + source_frame_ = "/"; + } + + MultiresImagePlugin::~MultiresImagePlugin() + { + delete tile_view_; + delete tile_set_; + } + + void MultiresImagePlugin::PrintError(const std::string& message) + { + if (message == ui_.status->text().toStdString()) { + return; + } + + RCLCPP_ERROR(node_->get_logger(), "Error: %s", message.c_str()); + QPalette p(ui_.status->palette()); + p.setColor(QPalette::Text, Qt::red); + ui_.status->setPalette(p); + ui_.status->setText(message.c_str()); + } + + void MultiresImagePlugin::PrintInfo(const std::string& message) + { + if (message == ui_.status->text().toStdString()) { + return; + } + + RCLCPP_INFO(node_->get_logger(), "%s", message.c_str()); + QPalette p(ui_.status->palette()); + p.setColor(QPalette::Text, Qt::green); + ui_.status->setPalette(p); + ui_.status->setText(message.c_str()); + } + + void MultiresImagePlugin::PrintWarning(const std::string& message) + { + if (message == ui_.status->text().toStdString()) { + return; + } + + RCLCPP_WARN(node_->get_logger(), "%s", message.c_str()); + QPalette p(ui_.status->palette()); + p.setColor(QPalette::Text, Qt::darkYellow); + ui_.status->setPalette(p); + ui_.status->setText(message.c_str()); + } + + void MultiresImagePlugin::AcceptConfiguration() + { + RCLCPP_INFO(node_->get_logger(), "Accept multires image configuration."); + if (tile_set_ != NULL && tile_set_->GeoReference().GeoPath() == ui_.path->text().toStdString()) + { + // Nothing to do. + } + else + { + loaded_ = false; + delete tile_set_; + delete tile_view_; + tile_set_ = new multires_image::TileSet(ui_.path->text().toStdString()); + + if (tile_set_->Load()) + { + loaded_ = true; + + source_frame_ = tile_set_->GeoReference().Projection(); + if (source_frame_.empty() || source_frame_[0] != '/') + { + source_frame_ = std::string("/") + source_frame_; + } + + QPalette p(ui_.status->palette()); + p.setColor(QPalette::Text, Qt::green); + ui_.status->setPalette(p); + ui_.status->setText("OK"); + + initialized_ = true; + + MultiresView* view = new MultiresView(tile_set_, canvas_); + tile_view_ = view; + } + else + { + PrintError("Failed to load image."); + delete tile_set_; + tile_set_ = 0; + tile_view_ = 0; + } + } + } + + void MultiresImagePlugin::SelectFile() + { + QFileDialog dialog(config_widget_, "Select Multires Image"); + dialog.setFileMode(QFileDialog::ExistingFile); + dialog.setNameFilter(tr("Geo Files (*.geo)")); + + dialog.exec(); + + if (dialog.result() == QDialog::Accepted && dialog.selectedFiles().count() == 1) + { + ui_.path->setText(dialog.selectedFiles().first()); + AcceptConfiguration(); + } + } + + + void MultiresImagePlugin::SetXOffset(double offset_x) + { + offset_x_ = offset_x; + } + + void MultiresImagePlugin::SetYOffset(double offset_y) + { + offset_y_ = offset_y; + } + + QWidget* MultiresImagePlugin::GetConfigWidget(QWidget* parent) + { + config_widget_->setParent(parent); + + return config_widget_; + } + + bool MultiresImagePlugin::Initialize(QGLWidget* canvas) + { + canvas_ = canvas; + + return true; + } + + void MultiresImagePlugin::GetCenterPoint(double x, double y) + { + tf2::Vector3 point(x, y, 0); + tf2::Vector3 center = inverse_transform_ * point; + center_x_ = center.getX(); + center_y_ = center.getY(); + } + + void MultiresImagePlugin::Draw(double x, double y, double scale) + { + if (transformed_ && tile_set_ != NULL && tile_view_ != NULL) + { + GetCenterPoint(x, y); + tile_view_->SetView(center_x_, center_y_, 1, scale); + + tile_view_->Draw(); + + PrintInfo("OK"); + } + } + + void MultiresImagePlugin::Transform() + { + transformed_ = false; + + if (!loaded_) + return; + + if (!tf_manager_->GetTransform(target_frame_, source_frame_, transform_)) + { + PrintError("Failed transform from " + source_frame_ + " to " + target_frame_); + return; + } + + if (!tf_manager_->GetTransform(source_frame_, target_frame_, inverse_transform_)) + { + PrintError("Failed inverse transform from " + target_frame_ + " to " + source_frame_); + return; + } + + // Add in user-specified offset to map + swri_transform_util::Transform offset( + tf2::Transform( + tf2::Quaternion(0, 0, 0, 1), + tf2::Vector3(offset_x_, offset_y_, 0.0))); + + // Set relative positions of tile points based on tf transform + for (int i = 0; i < tile_set_->LayerCount(); i++) + { + multires_image::TileSetLayer* layer = tile_set_->GetLayer(i); + for (int r = 0; r < layer->RowCount(); r++) + { + for (int c = 0; c < layer->ColumnCount(); c++) + { + multires_image::Tile* tile = layer->GetTile(c, r); + + tile->Transform(transform_, offset); + } + } + } + + transformed_ = true; + } + + boost::filesystem::path MultiresImagePlugin::MakePathRelative(boost::filesystem::path path, boost::filesystem::path base) + { + // Borrowed from: https://svn.boost.org/trac/boost/ticket/1976#comment:2 + if (path.has_root_path()) + { + if (path.root_path() != base.root_path()) + { + return path; + } + else + { + return MakePathRelative(path.relative_path(), base.relative_path()); + } + } + else + { + if (base.has_root_path()) + { + RCLCPP_WARN(node_->get_logger(), "Cannot uncomplete a path relative path from a rooted base."); + return path; + } + else + { + typedef boost::filesystem::path::const_iterator path_iterator; + path_iterator path_it = path.begin(); + path_iterator base_it = base.begin(); + while (path_it != path.end() && base_it != base.end()) + { + if (*path_it != *base_it) + break; + ++path_it; + ++base_it; + } + boost::filesystem::path result; + for (; base_it != base.end(); ++base_it) + { + result /= ".."; + } + for (; path_it != path.end(); ++path_it) + { + result /= *path_it; + } + return result; + } + } + } + + void MultiresImagePlugin::LoadConfig(const YAML::Node& node, const std::string& path) + { + if (node["path"]) + { + std::string path_string = node["path"].as(); + + boost::filesystem::path image_path(path_string); + if (!image_path.is_complete()) + { + boost::filesystem::path base_path(path); + path_string = + (path / image_path.relative_path()).normalize().string(); + } + + ui_.path->setText(path_string.c_str()); + + AcceptConfiguration(); + } + + if (node["offset_x"]) + { + offset_x_ = node["offset_x"].as(); + ui_.x_offset_spin_box->setValue(offset_x_); + } + if (node["offset_y"]) + { + offset_y_ = node["offset_y"].as(); + ui_.y_offset_spin_box->setValue(offset_y_); + } + } + + void MultiresImagePlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path) + { + boost::filesystem::path abs_path(ui_.path->text().toStdString()); + boost::filesystem::path base_path(path); + boost::filesystem::path rel_path = MakePathRelative(abs_path, base_path); + + emitter << YAML::Key << "path" << YAML::Value << rel_path.string(); + emitter << YAML::Key << "offset_x" << YAML::Value << offset_x_; + emitter << YAML::Key << "offset_y" << YAML::Value << offset_y_; + } +} + diff --git a/multires_image/src/multires_view.cpp b/multires_image/src/multires_view.cpp new file mode 100644 index 000000000..43f1189a0 --- /dev/null +++ b/multires_image/src/multires_view.cpp @@ -0,0 +1,199 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include + +// C++ standard libraries +#include +#include + +#include + +namespace mapviz_plugins +{ + MultiresView::MultiresView(multires_image::TileSet* tiles, QGLWidget* widget) : + m_tiles(tiles), + m_cache(tiles, widget), + m_currentLayer(tiles->LayerCount() - 1), + m_startRow(0), + m_startColumn(0), + m_endRow(0), + m_endColumn(0) + { + double top, left, bottom, right; + + tiles->GeoReference().GetCoordinate(0, 0, left, top); + + tiles->GeoReference().GetCoordinate( + tiles->GeoReference().Width(), + tiles->GeoReference().Height(), + right, + bottom); + + double width_m, height_m; + if (tiles->GeoReference().Projection() == "wgs84") + { + width_m = swri_transform_util::GreatCircleDistance(top, left, top, right); + height_m = swri_transform_util::GreatCircleDistance(top, left, bottom, left); + } + else + { + width_m = std::fabs(right - left); + height_m = std::fabs(top - bottom); + } + + double scale_x = width_m / tiles->GeoReference().Width(); + double scale_y = height_m / tiles->GeoReference().Height(); + + min_scale_ = scale_x; + if (scale_y > scale_x) { + min_scale_ = scale_y; + } + } + + void MultiresView::SetView(double x, double y, double radius, double scale) + { + int layer = 0; + while (min_scale_ * std::pow(2.0, layer + 1) < scale) layer++; + + if (layer >= m_tiles->LayerCount()) { + layer = m_tiles->LayerCount() - 1; + } + + if (layer != m_currentLayer) + { + m_currentLayer = layer; + m_cache.SetCurrentLayer(layer); + } + + int row, column; + m_tiles->GetLayer(m_currentLayer)->GetTileIndex(x, y, row, column); + + int size = 3; + + m_startRow = row - size; + if (m_startRow < 0) { + m_startRow = 0; + } + if (m_startRow >= m_tiles->GetLayer(m_currentLayer)->RowCount()) { + m_startRow = m_tiles->GetLayer(m_currentLayer)->RowCount() - 1; + } + + m_endRow = row + size; + if (m_endRow < 0) { + m_endRow = 0; + } + if (m_endRow >= m_tiles->GetLayer(m_currentLayer)->RowCount()) { + m_endRow = m_tiles->GetLayer(m_currentLayer)->RowCount() - 1; + } + + m_startColumn = column - size; + if (m_startColumn < 0) { + m_startColumn = 0; + } + if (m_startColumn >= m_tiles->GetLayer(m_currentLayer)->ColumnCount()) { + m_startColumn = m_tiles->GetLayer(m_currentLayer)->ColumnCount() - 1; + } + + m_endColumn = column + size; + if (m_endColumn < 0) { + m_endColumn = 0; + } + if (m_endColumn >= m_tiles->GetLayer(m_currentLayer)->ColumnCount()) { + m_endColumn = m_tiles->GetLayer(m_currentLayer)->ColumnCount() - 1; + } + + m_cache.Precache(x, y); + } + + void MultiresView::Draw() + { + glEnable(GL_TEXTURE_2D); + + glColor4f(1.0f, 1.0f, 1.0f, 1.0f); + + // Always draw bottom layers + + multires_image::TileSetLayer* baseLayer = m_tiles->GetLayer(m_tiles->LayerCount() - 1); + multires_image::Tile* tile = baseLayer->GetTile(0, 0); + if (tile->TextureLoaded()) + { + tile->Draw(); + } + else + { + m_cache.Load(tile); + } + + if(m_tiles->LayerCount() >= 2) + { + baseLayer = m_tiles->GetLayer(m_tiles->LayerCount() - 2); + for (int c = 0; c < baseLayer->ColumnCount(); c++) + { + for (int r = 0; r < baseLayer->RowCount(); r++) + { + tile = baseLayer->GetTile(c, r); + if (tile->TextureLoaded()) + { + tile->Draw(); + } + else + { + m_cache.Load(tile); + } + } + } + } + + if (m_tiles->LayerCount() >= 2 && m_currentLayer < m_tiles->LayerCount() - 2) + { + multires_image::TileSetLayer* layer = m_tiles->GetLayer(m_currentLayer); + if (m_endColumn < layer->ColumnCount() && m_endRow < layer->RowCount()) + { + for (int c = m_startColumn; c <= m_endColumn; c++) + { + for (int r = m_startRow; r <= m_endRow; r++) + { + tile = layer->GetTile(c, r); + if (tile->TextureLoaded()) + { + tile->Draw(); + } + else + { + m_cache.Load(tile); + } + } + } + } + } + + glDisable(GL_TEXTURE_2D); + } +} diff --git a/multires_image/src/nodes/multires_view_node.cpp b/multires_image/src/nodes/multires_view_node.cpp new file mode 100644 index 000000000..82109f92a --- /dev/null +++ b/multires_image/src/nodes/multires_view_node.cpp @@ -0,0 +1,128 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +/** + * \file + * + * multires_image::MultiresViewNode. Description. + * - \b Parameters + * - \e "node_name"/image_path [string] - Description. [""] + */ + +#include + +// QT libraries +#include +#include +#include +#include +#include + +#include + +namespace multires_image +{ + MultiresViewNode::MultiresViewNode(int argc, char **argv, QWidget *parent, Qt::WindowFlags flags) + : QMainWindow(parent, flags) + , argc_(argc) + , argv_(argv) + , node_(nullptr) + , thread_(nullptr) + , initialized_(false) + , tile_set_(nullptr) + { + setCentralWidget(new QGLMap()); + this->setMinimumSize(640, 480); + } + + void MultiresViewNode::Spin() + { + if (!thread_) + { + thread_ = new boost::thread(&MultiresViewNode::SpinLoop, this); + } + } + + void MultiresViewNode::SpinLoop() + { + while (rclcpp::ok()) + { + rclcpp::spin_some(node_); + + usleep(10); + } + } + + void MultiresViewNode::showEvent(QShowEvent* event) + { + Initialize(); + } + + void MultiresViewNode::Initialize() + { + if (!initialized_) + { + rclcpp::init(argc_, argv_); + + node_ = std::make_shared("multires_view_node"); + + node_->declare_parameter("image_path", ""); + + node_->get_parameter("image_path", image_path_); + + tile_set_ = new TileSet(image_path_); + + if (tile_set_->Load()) + { + QGLMap* glMap = reinterpret_cast(centralWidget()); + glMap->SetTiles(tile_set_); + glMap->UpdateView(); + } + else + { + QMessageBox::warning(this, "Error", "Failed to load tiles."); + } + + Spin(); + + initialized_ = true; + } + } +} + +int main(int argc, char **argv) +{ + // Initialize QT + QApplication app(argc, argv); + + multires_image::MultiresViewNode node(argc, argv); + node.show(); + + return QApplication::exec(); +} diff --git a/multires_image/src/tile.cpp b/multires_image/src/tile.cpp new file mode 100644 index 000000000..233a20ffd --- /dev/null +++ b/multires_image/src/tile.cpp @@ -0,0 +1,223 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include + +// C++ standard libraries +#include +#include +#include +#include + +// QT libraries +#include +#include + +#include + +namespace multires_image +{ + Tile::Tile( + const std::string& path, int column, int row, int level, + const tf2::Vector3& topLeft, const tf2::Vector3& topRight, + const tf2::Vector3& bottomLeft, const tf2::Vector3& bottomRight) : + m_path(path), + m_column(column), + m_row(row), + m_level(level), + m_top_left(topLeft), + m_top_right(topRight), + m_bottom_right(bottomRight), + m_bottom_left(bottomLeft), + m_transformed_top_left(topLeft), + m_transformed_top_right(topRight), + m_transformed_bottom_right(bottomRight), + m_transformed_bottom_left(bottomLeft), + m_failed(false), + m_textureLoaded(false), + m_dimension(0), + m_textureId(0), + m_tileId(1000000 * level + 1000 * column + row), + m_memorySize(0) + { + } + + bool Tile::Exists() + { + return QFile::exists(m_path.c_str()); + } + + bool Tile::LoadImageToMemory(bool gl) + { + if (!m_failed) + { + m_mutex.lock(); + + try + { + QImage nullImage; + m_image = nullImage; + + if (m_image.load(m_path.c_str())) + { + if (gl) + { + int width = m_image.width(); + int height = m_image.height(); + + float max_dim = std::max(width, height); + m_dimension = swri_math_util::Round( + std::pow(2.0f, std::ceil(std::log(max_dim)/std::log(2.0f)))); + + if (width != m_dimension || height != m_dimension) + { + m_image = m_image.scaled(m_dimension, m_dimension, Qt::IgnoreAspectRatio, Qt::FastTransformation); + } + + m_memorySize = m_dimension * m_dimension * 4; + + m_image = QGLWidget::convertToGLFormat(m_image); + } + } + else + { + m_failed = true; + } + } + catch(std::exception& e) + { + std::cout << "An exception occurred loading image: " << e.what() << std::endl; + m_failed = true; + } + + m_mutex.unlock(); + } + + return !m_failed; + } + + void Tile::UnloadImage() + { + m_mutex.lock(); + + QImage nullImage; + m_image = nullImage; + + m_mutex.unlock(); + } + + bool Tile::LoadTexture() + { + if (!m_textureLoaded && !m_failed) + { + m_mutex.lock(); + + try + { + GLuint ids[1]; + glGenTextures(1, &ids[0]); + m_textureId = ids[0]; + + glBindTexture(GL_TEXTURE_2D, m_textureId); + glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, m_dimension, m_dimension, 0, GL_RGBA, GL_UNSIGNED_BYTE, m_image.bits()); + + // TODO(malban): check for GL error + + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); + + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE); + + m_textureLoaded = true; + } + catch (const std::exception& e) + { + std::cout << "An exception occured loading texture: " << e.what() << std::endl; + m_failed = true; + } + + m_mutex.unlock(); + } + + return m_textureLoaded; + } + + void Tile::UnloadTexture() + { + m_mutex.lock(); + + if (m_textureLoaded) + { + m_textureLoaded = false; + GLuint ids[1]; + ids[0] = m_textureId; + glDeleteTextures(1, &ids[0]); + } + + m_mutex.unlock(); + } + + void Tile::Draw() + { + if (!m_failed) + { + if (m_textureLoaded) + { + glBindTexture(GL_TEXTURE_2D, m_textureId); + + glBegin(GL_QUADS); + + glTexCoord2f(0, 1); glVertex2f(m_transformed_top_left.x(), m_transformed_top_left.y()); + glTexCoord2f(1, 1); glVertex2f(m_transformed_top_right.x(), m_transformed_top_right.y()); + glTexCoord2f(1, 0); glVertex2f(m_transformed_bottom_right.x(), m_transformed_bottom_right.y()); + glTexCoord2f(0, 0); glVertex2f(m_transformed_bottom_left.x(), m_transformed_bottom_left.y()); + + glEnd(); + } + } + } + + void Tile::Transform(const swri_transform_util::Transform& transform) + { + m_transformed_top_left = transform * m_top_left; + m_transformed_top_right = transform * m_top_right; + m_transformed_bottom_left = transform * m_bottom_left; + m_transformed_bottom_right = transform * m_bottom_right; + } + + void Tile::Transform(const swri_transform_util::Transform& transform, const swri_transform_util::Transform& offset_tf) + { + m_transformed_top_left = offset_tf * (transform * m_top_left); + m_transformed_top_right = offset_tf * (transform * m_top_right); + m_transformed_bottom_left = offset_tf * (transform * m_bottom_left); + m_transformed_bottom_right = offset_tf * (transform * m_bottom_right); + } +} + diff --git a/multires_image/src/tile_cache.cpp b/multires_image/src/tile_cache.cpp new file mode 100644 index 000000000..d31d553d1 --- /dev/null +++ b/multires_image/src/tile_cache.cpp @@ -0,0 +1,396 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include + +// C++ standard libraries +#include +#include +#include +#include + +// QT libraries +#include +#include + +#include + +namespace multires_image +{ + TileCache::TileCache(TileSet* tileSet, QGLWidget* widget) + : QObject() + , m_tileSet(tileSet) + , m_widget(widget) + , m_currentLayer(0) + , m_currentPosition(0, 0, 0) + , m_exit(false) + , m_memorySize(0) + , m_cacheThread(this) + , m_freeThread(this) + , m_renderRequestsLock(QMutex::Recursive) + , m_renderRequestSetLock(QMutex::Recursive) + , m_precacheRequestsLock(QMutex::Recursive) + , m_precacheRequestSetLock(QMutex::Recursive) + , m_textureLoadedLock(QMutex::Recursive) + { + connect(this, SIGNAL(SignalLoadTexture(Tile*)), + SLOT(LoadTextureSlot(Tile*)), Qt::BlockingQueuedConnection); + + connect(this, SIGNAL(SignalDeleteTexture(Tile*)), + SLOT(DeleteTextureSlot(Tile*)), Qt::BlockingQueuedConnection); + + m_cacheThread.setPriority(QThread::NormalPriority); + m_cacheThread.start(); + + m_freeThread.setPriority(QThread::LowPriority); + m_freeThread.start(); + + for (int i = 0; i < m_tileSet->LayerCount(); i++) + { + m_precacheRequests.emplace_back(std::queue()); + } + } + + TileCache::~TileCache() + { + m_exit = true; + m_cacheThread.wait(); + m_freeThread.wait(); + } + + void TileCache::LoadTextureSlot(Tile* tile) + { + tile->LoadTexture(); + } + + void TileCache::DeleteTextureSlot(Tile* tile) + { + tile->UnloadTexture(); + } + + void TileCache::Load(Tile* tile) + { + m_renderRequestsLock.lock(); + m_renderRequestSetLock.lock(); + + try + { + if (m_renderRequestSet.count(tile->TileID()) == 0) + { + m_renderRequests.push(tile); + m_renderRequestSet[tile->TileID()] = tile; + } + } + catch(const std::exception& e) + { + std::cout << "An exception occurred queuing a tile to be cached: " << e.what() << std::endl; + } + + m_renderRequestSetLock.unlock(); + m_renderRequestsLock.unlock(); + } + + void TileCache::Precache(double x, double y) + { + tf2::Vector3 point(x, y, 0); + Precache(point); + } + + void TileCache::Precache(const tf2::Vector3& position) + { + m_currentPosition = position; + + int sizes[] = {3, 2, 2, 1, 1, 1}; + + PrecacheLayer(m_currentLayer, m_currentPosition, sizes[0]); + + for (int i = 1; i <= 5; i++) + { + int layer = m_currentLayer + i; + if (layer < m_tileSet->LayerCount()) + { + PrecacheLayer(layer, m_currentPosition, sizes[i]); + } + + layer = m_currentLayer - i; + if (layer >= 0) + { + PrecacheLayer(layer, m_currentPosition, sizes[i]); + } + } + } + + void TileCache::PrecacheLayer(int layerNum, const tf2::Vector3& position, int size) + { + TileSetLayer* layer = m_tileSet->GetLayer(layerNum); + + int row, column; + layer->GetTileIndex(position, row, column); + + int startRow = std::max(0, row - size); + int endRow = std::min(layer->RowCount() - 1, row + size); + int startColumn = std::max(0, column - size); + int endColumn = std::min(layer->ColumnCount() - 1, column + size); + + for (int c = startColumn; c <= endColumn; c++) + { + for (int r = startRow; r <= endRow; r++) + { + Tile* tile = layer->GetTile(c, r); + + m_precacheRequestsLock.lock(); + m_precacheRequestSetLock.lock(); + + try + { + if (m_precacheRequestSet.count(tile->TileID()) == 0) + { + m_precacheRequests[layerNum].push(tile); + m_precacheRequestSet[tile->TileID()] = tile; + } + } + catch (const std::exception& e) + { + std::cout << "An exception occurred queuing tiles for precaching: " << e.what() << std::endl; + } + + m_precacheRequestSetLock.unlock(); + m_precacheRequestsLock.unlock(); + } + } + } + + void TileCache::Exit() + { + m_exit = true; + } + + void TileCache::LoadTexture(Tile* tile) + { + Q_EMIT SignalLoadTexture(tile); + + m_memorySize += tile->MemorySize(); + Q_EMIT SignalMemorySize(m_memorySize); + + m_textureLoadedLock.lock(); + + try + { + m_textureLoaded[tile->TileID()] = tile; + } + catch (const std::exception& e) + { + std::cout << "An exception occurred loading texture: " << e.what() << std::endl; + } + + m_textureLoadedLock.unlock(); + + if (tile->Layer() == m_currentLayer) + { + QApplication::postEvent(m_widget, new QEvent(QEvent::UpdateRequest)); + } + } + + void TileCache::UnloadTexture(Tile* tile) + { + Q_EMIT SignalDeleteTexture(tile); + + m_memorySize -= tile->MemorySize(); + Q_EMIT SignalMemorySize(m_memorySize); + + m_textureLoadedLock.lock(); + + try + { + m_textureLoaded.erase(tile->TileID()); + } + catch (const std::exception& e) + { + std::cout << "An exception occurred unloading texture: " << e.what() << std::endl; + } + + m_textureLoadedLock.unlock(); + } + + void TileCache::CacheThread::run() + { + while (!p->m_exit) + { + Tile* tile = nullptr; + p->m_renderRequestsLock.lock(); + + if (!p->m_renderRequests.empty()) + { + tile = p->m_renderRequests.top(); + p->m_renderRequests.pop(); + } + + p->m_renderRequestsLock.unlock(); + + if (tile != nullptr) + { + if (!tile->Failed()) + { + if (tile->Layer() == p->m_currentLayer) + { + int row, column; + p->m_tileSet->GetLayer(tile->Layer())->GetTileIndex(p->m_currentPosition, row, column); + + if (abs(tile->Row() - row) <= 3 || abs(tile->Column() - column) < 3) + { + if (!tile->TextureLoaded()) + { + if (tile->LoadImageToMemory()) + { + p->LoadTexture(tile); + tile->UnloadImage(); + } + else + { + printf("failed to load image\n"); + } + } + } + } + else + { + p->m_precacheRequests[tile->Layer()].push(tile); + } + + p->m_renderRequestSetLock.lock(); + p->m_renderRequestSet.erase(tile->TileID()); + p->m_renderRequestSetLock.unlock(); + } + else + { + } + } + else + { + p->m_precacheRequestsLock.lock(); + + try + { + for (uint32_t i = 0; (i < p->m_precacheRequests.size()) && (tile == nullptr); i++) + { + int32_t index = p->m_currentLayer + i; + if ((index < (int64_t)p->m_precacheRequests.size()) && + (!p->m_precacheRequests[index].empty())) + { + tile = p->m_precacheRequests[index].front(); + p->m_precacheRequests[index].pop(); + } + else if (i != 0) + { + index = p->m_currentLayer - i; + if (index >= 0 && !p->m_precacheRequests[index].empty()) + { + tile = p->m_precacheRequests[index].front(); + p->m_precacheRequests[index].pop(); + } + } + } + } + catch (const std::exception& e) + { + std::cout << "An exception occurred precaching texture: " << e.what() << std::endl; + } + + p->m_precacheRequestsLock.unlock(); + + if (tile != nullptr && !tile->Failed() && !tile->TextureLoaded()) + { + int row, column; + p->m_tileSet->GetLayer(tile->Layer())->GetTileIndex(p->m_currentPosition, row, column); + if (abs(tile->Row() - row) <= 3 || abs(tile->Column() - column) <= 3) + { + if (tile->LoadImageToMemory()) + { + p->LoadTexture(tile); + + tile->UnloadImage(); + } + else + { + printf("failed to precache load image\n"); + } + } + + p->m_precacheRequestSetLock.lock(); + p->m_precacheRequestSet.erase(tile->TileID()); + p->m_precacheRequestSetLock.unlock(); + } + } + + if (tile == nullptr) + { + usleep(10); + } + } + } + + void TileCache::FreeThread::run() + { + while (!p->m_exit) + { + std::map* tiles; + p->m_textureLoadedLock.lock(); + + tiles = new std::map(p->m_textureLoaded); + + p->m_textureLoadedLock.unlock(); + + std::map::iterator iter; + + for (iter = tiles->begin(); iter != tiles->end(); ++iter) + { + Tile* tile = iter->second; + int row, column; + p->m_tileSet->GetLayer(tile->Layer())->GetTileIndex(p->m_currentPosition, row, column); + + if (abs(tile->Row() - row) > 6 || abs(tile->Column() - column) > 6) + { + p->m_renderRequestSetLock.lock(); + p->m_renderRequestSet.erase(tile->TileID()); + p->m_renderRequestSetLock.unlock(); + + p->m_precacheRequestSetLock.lock(); + p->m_precacheRequestSet.erase(tile->TileID()); + p->m_precacheRequestSetLock.unlock(); + + p->UnloadTexture(tile); + } + } + + delete tiles; + + sleep(2); + } + } +} diff --git a/multires_image/src/tile_set.cpp b/multires_image/src/tile_set.cpp new file mode 100644 index 000000000..77627492a --- /dev/null +++ b/multires_image/src/tile_set.cpp @@ -0,0 +1,124 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include + +// C++ standard libraries +#include +#include +#include + +// QT libraries +#include +#include +#include +#include + +namespace multires_image +{ + TileSet::TileSet(const std::string& geofile) : + m_geo(geofile), + m_extension("jpg") + { + } + + TileSet::TileSet(const std::string& geofile, const std::string extension) : + m_geo(geofile), + m_extension(extension) + { + } + + TileSet::TileSet(const swri_transform_util::GeoReference& georeference) : + m_geo(georeference), + m_extension("jpg") + { + } + + TileSet::TileSet(const swri_transform_util::GeoReference& georeference, + const std::string extension) : + m_geo(georeference), + m_extension(extension) + { + } + + TileSet::~TileSet() + { + // Free each of the layers. + for (auto & m_layer : m_layers) + { + delete m_layer; + } + } + + bool TileSet::Load() + { + if (!m_geo.Load()) + { + return false; + } + + m_cacheDir = m_geo.Path(); + m_width = m_geo.Width(); + m_height = m_geo.Height(); + m_tileSize = m_geo.TileSize(); + m_extension = m_geo.Extension(); + + float max_dim = std::max(m_width, m_height); + m_layerCount = std::ceil(std::log(max_dim / m_tileSize) / std::log(2.0f)) + 1; + m_layers.reserve(m_layerCount); + + // Check if the cache directory for this image exists. + QDir directory(m_cacheDir.c_str()); + if (!directory.exists()) + { + return false; + } + + // Load each layer. + for (int i = 0; i < m_layerCount; i++) + { + QString layerNum = QString::number(i); + + // Check if this layer exists in the cache. + QDir layerDir(directory.absolutePath() + "/layer" + layerNum); + if (!layerDir.exists(layerDir.absolutePath())) + { + return false; + } + + // Load the base layer. + m_layers.push_back(new TileSetLayer(m_geo, layerDir.absolutePath().toStdString(), m_tileSize, i)); + + if (!m_layers[i]->Load(m_extension)) + return false; + } + + return true; + } +} diff --git a/multires_image/src/tile_set_layer.cpp b/multires_image/src/tile_set_layer.cpp new file mode 100644 index 000000000..cbbcf39a7 --- /dev/null +++ b/multires_image/src/tile_set_layer.cpp @@ -0,0 +1,197 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include + +// C++ standard libraries +#include +#include + +// QT libraries +#include + +namespace multires_image +{ + TileSetLayer::TileSetLayer(const swri_transform_util::GeoReference& geo, + const std::string& path, + int tileSize, int layer) : + m_geo(geo), + m_path(path), + m_tileSize(tileSize), + m_layer(layer), + m_scale(std::pow(2.0, m_layer)), + m_expectTiles(true) + { + // Calculate the width and height in pixels of this layer + float width = std::ceil(m_geo.Width() / std::pow(2.0f, layer)); + float height = std::ceil(m_geo.Height() / std::pow(2.0f, layer)); + + // Calculate the number for tile rows and columns for this layer + m_columns = std::ceil(width / static_cast(tileSize)); + m_rows = std::ceil(height / static_cast(tileSize)); + + m_tiles.reserve(m_columns); + for (int c = 0; c < m_columns; c++) + { + m_tiles.emplace_back(std::vector()); + m_tiles[c].reserve(m_rows); + } + } + + bool TileSetLayer::Load() + { + return Load("jpg"); + } + + bool TileSetLayer::Load(const std::string extension) + { + bool needsTiles = false; + + for (int32_t c = 0; c < m_columns; c++) + { + for (int32_t r = 0; r < m_rows; r++) + { + std::string rowString = QString::number(r).toStdString(); + while (rowString.length() < 5) { + rowString = '0' + rowString; + } + + std::string columnString = QString::number(c).toStdString(); + while (columnString.length() < 5) { + columnString = '0' + columnString; + } + + // Get 4 corners of this tile + int left = c * m_tileSize * m_scale; + int top = r * m_tileSize * m_scale; + int bottom = (r + 1) * m_tileSize * m_scale; + int right = (c + 1) * m_tileSize * m_scale; + + if (right > (int64_t)m_geo.Width()) + { + right = m_geo.Width(); + } + if (bottom > (int64_t)m_geo.Height()) + { + bottom = m_geo.Height(); + } + + double x, y; + m_geo.GetCoordinate(left, top, x, y); + tf2::Vector3 top_left(x, y, 0); + + m_geo.GetCoordinate(right, top, x, y); + tf2::Vector3 top_right(x, y, 0); + + m_geo.GetCoordinate(left, bottom, x, y); + tf2::Vector3 bottom_left(x, y, 0); + + m_geo.GetCoordinate(right, bottom, x, y); + tf2::Vector3 bottom_right(x, y, 0); + + std::stringstream ss; + ss << m_path << "/tile" << rowString << "x" << columnString << "." << extension; + m_tiles[c].push_back(new Tile( + ss.str(), c, r, m_layer, top_left, top_right, bottom_left, bottom_right)); + + needsTiles |= !m_tiles[c][r]->Exists(); + } + } + + if (needsTiles) + { + if (m_expectTiles) + { + printf("Error: Missing expected tiles\n"); + return false; + } + } + + return true; + } + + void TileSetLayer::GetTileIndex(double x, double y, int& row, int& column) const + { + tf2::Vector3 position(x, y, 0); + GetTileIndex(position, row, column); + } + + void TileSetLayer::GetTileIndex(const tf2::Vector3& position, int& row, int& column) const + { + int x, y; + m_geo.GetPixel(position.x(), position.y(), x, y); + + column = static_cast(x / (m_scale * m_tileSize)); + row = static_cast(y / (m_scale * m_tileSize)); + } + + void TileSetLayer::GetTileRange( + const tf2::Vector3& top_left, + const tf2::Vector3& bottom_right, + int& startRow, int& startColumn, + int& endRow, int& endColumn) const + { + GetTileIndex(top_left.x(), top_left.y(), startRow, startColumn); + if (startColumn < 0) + { + startColumn = 0; + } + if ((uint32_t)startColumn >= m_tiles.size()) + { + startColumn = m_tiles.size() - 1; + } + if (startRow < 0) + { + startRow = 0; + } + if ((uint32_t)startRow >= m_tiles[0].size()) + { + startRow = m_tiles[0].size() - 1; + } + + GetTileIndex(bottom_right.x(), bottom_right.y(), endRow, endColumn); + if (endColumn < 0) + { + endColumn = 0; + } + if ((uint32_t)endColumn >= m_tiles.size()) + { + endColumn = m_tiles.size() - 1; + } + if (endRow < 0) + { + endRow = 0; + } + if ((uint32_t)endRow >= m_tiles[0].size()) + { + endRow = m_tiles[0].size() - 1; + } + } +} + diff --git a/multires_image/src/tile_view.cpp b/multires_image/src/tile_view.cpp new file mode 100644 index 000000000..7826ce5a8 --- /dev/null +++ b/multires_image/src/tile_view.cpp @@ -0,0 +1,175 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include + +// C++ standard libraries +#include +#include + +namespace multires_image +{ + TileView::TileView(TileSet* tiles, QGLWidget* widget) : + m_tiles(tiles), + m_cache(tiles, widget), + m_currentLayer(tiles->LayerCount() - 1), + m_startRow(0), + m_startColumn(0), + m_endRow(0), + m_endColumn(0) + { + double top, left, bottom, right; + tiles->GeoReference().GetCoordinate(0, 0, left, top); + tiles->GeoReference().GetCoordinate(tiles->GeoReference().Width(),tiles->GeoReference().Height(), right, bottom); + + double scale_x = std::abs(right - left) / tiles->GeoReference().Width(); + double scale_y = std::abs(top - bottom) / tiles->GeoReference().Height(); + + min_scale_ = scale_x; + if (scale_y > scale_x) + min_scale_ = scale_y; + } + + void TileView::SetView(double x, double y, double radius, double scale) + { + int layer = 0; + while (min_scale_ * std::pow(2.0, layer + 1) < scale) layer++; + + if (layer >= m_tiles->LayerCount()) + layer = m_tiles->LayerCount() - 1; + + if (layer != m_currentLayer) + { + m_currentLayer = layer; + m_cache.SetCurrentLayer(layer); + } + + int row, column; + m_tiles->GetLayer(m_currentLayer)->GetTileIndex(x, y, row, column); + + m_startRow = row - 2; + if (m_startRow < 0) { + m_startRow = 0; + } + if (m_startRow >= m_tiles->GetLayer(m_currentLayer)->RowCount()) { + m_startRow = m_tiles->GetLayer(m_currentLayer)->RowCount() - 1; + } + + m_endRow = row + 2; + if (m_endRow < 0) { + m_endRow = 0; + } + if (m_endRow >= m_tiles->GetLayer(m_currentLayer)->RowCount()) { + m_endRow = m_tiles->GetLayer(m_currentLayer)->RowCount() - 1; + } + + m_startColumn = column - 2; + if (m_startColumn < 0) { + m_startColumn = 0; + } + if (m_startColumn >= m_tiles->GetLayer(m_currentLayer)->ColumnCount()) { + m_startColumn = m_tiles->GetLayer(m_currentLayer)->ColumnCount() - 1; + } + + m_endColumn = column + 2; + if (m_endColumn < 0) { + m_endColumn = 0; + } + if (m_endColumn >= m_tiles->GetLayer(m_currentLayer)->ColumnCount()) { + m_endColumn = m_tiles->GetLayer(m_currentLayer)->ColumnCount() - 1; + } + + m_cache.Precache(x, y); + } + + void TileView::Draw() + { + glEnable(GL_TEXTURE_2D); + + glColor4f(1.0f, 1.0f, 1.0f, 1.0f); + + // Always draw bottom layers + if (m_currentLayer != m_tiles->LayerCount() - 1) + { + TileSetLayer* baseLayer = m_tiles->GetLayer(m_tiles->LayerCount() - 1); + Tile* tile = baseLayer->GetTile(0, 0); + if (tile->TextureLoaded()) + { + tile->Draw(); + } + else + { + m_cache.Load(tile); + } + } + + if (m_tiles->LayerCount() >= 2 && m_currentLayer != m_tiles->LayerCount() - 2) + { + TileSetLayer* baseLayer = m_tiles->GetLayer(m_tiles->LayerCount() - 2); + for (int c = 0; c < baseLayer->ColumnCount(); c++) + { + for (int r = 0; r < baseLayer->RowCount(); r++) + { + Tile* tile = baseLayer->GetTile(c, r); + if (tile->TextureLoaded()) + { + tile->Draw(); + } + else + { + m_cache.Load(tile); + } + } + } + } + + TileSetLayer* layer = m_tiles->GetLayer(m_currentLayer); + if (m_endColumn < layer->ColumnCount() && m_endRow < layer->RowCount()) + { + for (int c = m_startColumn; c <= m_endColumn; c++) + { + for (int r = m_startRow; r <= m_endRow; r++) + { + Tile* tile = layer->GetTile(c, r); + if (tile->TextureLoaded()) + { + tile->Draw(); + } + else + { + m_cache.Load(tile); + } + } + } + } + + glDisable(GL_TEXTURE_2D); + } +} + diff --git a/multires_image/test.geo b/multires_image/test.geo new file mode 100644 index 000000000..505e21f47 --- /dev/null +++ b/multires_image/test.geo @@ -0,0 +1,13 @@ +// Image properties +image_path: tiles +image_width: 25600 +image_height: 17920 +tile_size: 512 + +// Coordinate System +datum: wgs84 +projection: geographic + +// Georeference +tiepoint: [6785, 336, 29.45196669, -98.61370577] +pixel_scale: [0.00000157, 0.000001375] diff --git a/tile_map/CHANGELOG.rst b/tile_map/CHANGELOG.rst new file mode 100644 index 000000000..d5ac1e4f5 --- /dev/null +++ b/tile_map/CHANGELOG.rst @@ -0,0 +1,154 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package tile_map +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.3.0 (2023-08-24) +------------------ + +2.2.2 (2023-06-07) +------------------ +* Iron Compatibility (`#779 `_) +* Contributors: David Anthony + +2.2.1 (2023-05-30) +------------------ +* Updating maintainers list (`#778 `_) +* Contributors: David Anthony + +2.1.0 (2020-10-22) +------------------ + +2.0.0 (2020-05-13) +------------------ +* Port mapviz to ROS 2 (`#672 `_) +* Remove OpenGL warning (`#667 `_) +* Contributors: Daniel D'Souza, P. J. Reed, Jacob Hassold, Kevin Nickels, Roger Strain, Matthew Bries + +1.2.0 (2019-09-04) +------------------ + +1.1.1 (2019-05-17) +------------------ + +1.1.0 (2019-02-20) +------------------ +* Fix non-Bing sources (`#615 `_) +* Contributors: P. J. Reed + +1.0.1 (2019-01-25) +------------------ + +1.0.0 (2019-01-23) +------------------ +* Sharing tf_manager\_ between main app and plugins (`#555 `_) +* Fix issue with loading Bing map tiles (`#599 `_) +* Contributors: Davide Faconti, P. J. Reed + +0.3.0 (2018-11-16) +------------------ +* Merge all -devel branches into a single master branch +* Contributors: P. J. Reed + +0.2.6 (2018-07-31) +------------------ + +0.2.5 (2018-04-12) +------------------ +* Bug fix in TileMap. GenTexture was invoked over and over again (`#559 `_) +* Improve tile loading prioritization. +* Glew warning fixed (`#539 `_) +* update to use non deprecated pluginlib macro +* Contributors: Davide Faconti, Marc Alban, Mikael Arguedas, P. J. Reed + +0.2.4 (2017-08-11) +------------------ +* add include for boost::algorithm::trim_copy to fix tile_map_plugin.cpp:408:31: error: ‘trim_copy’ is not a member of ‘boost’ (`#497 `_) +* Contributors: Austin Deric + +0.2.3 (2016-12-10) +------------------ + +0.2.2 (2016-12-07) +------------------ + +0.2.1 (2016-10-23) +------------------ +* Rewrite tile_map loading to be more reliable + This changes how the tile_map plugin handles making network requires for tiles. + It will now: + - Use thread conditions to prompt loading rather than spinning + - Prioritize loading tiles in the visible area + - Retry a failed network request up to 5 times + - Not discard tile requests if there are more than 100 in the queue + This changes should significantly reduce (if not completely eliminate) the + number of tiles that fail to load and hopefully make tiles within the visible + area appear faster when there are many in the queue. + Fixes `#342 `_ and `#421 `_. +* Adding support for Bing Maps (`#409 `_) + This makes a number of changes in the `tile_map` plugin in order to support + different types of tile servers, including Bing Maps. Notable changes include: + - TileSource is now an abstract class + - WMTS server-specific behavior has been moved into a new WmtsSource class + - BingSource provides support for obtaining tiles from Bing Maps + - The UI for specifying server URLs has changed + - Prefix and coordinate order are no longer separate fields + - In URLs for WMTS sources, the variables {level}, {x}, and {y} will be substituted with appropriate values when tiles are requested + - Rather than generating hashes for image tiles based on their URLs, hashes are now generated by the TileSource implementations in order to support sources that can pull tiles from multiple servers + - Idle performance has been improved by removing redundant recalculations of the map view + - Added a dependency on libjsoncpp + Resolves `#227 `_ + Conflicts: + tile_map/CMakeLists.txt + tile_map/package.xml +* Giving `tile_map` an interface overhaul + MapQuest has turned off their public API for map tiles, so this plugin needed some work. I have: + - Removed the MapQuest sources + - Made the interface for adding new sources more powerful + - Overhauled how sources are saved and loaded under the hood + - Added a button to reset the current tile cache + Resolves `#402 `_ + Conflicts: + tile_map/CMakeLists.txt +* Contributors: P. J. Reed + +0.2.0 (2016-06-23) +------------------ +* Update tile_map to Qt5. +* Contributors: Ed Venator + +0.1.3 (2016-05-20) +------------------ +* Fix typo in tile map view size comparison. +* Contributors: Marc Alban + +0.1.2 (2016-01-06) +------------------ + +0.1.1 (2015-11-17) +------------------ +* Mark single argument constructors explicit. +* Contributors: Marc Alban + +0.1.0 (2015-09-29) +------------------ + +0.0.3 (2015-09-28) +------------------ + +0.0.2 (2015-09-27) +------------------ + +0.0.1 (2015-09-27) +------------------ +* Adds missing qt-opengl dependency to tile_map. +* Renames all marti_common packages that were renamed. + (See http://github.com/swri-robotics/marti_common/issues/231) +* Fixes catkin_lint problems that could prevent installation. +* updates cmake version to squash the CMP0003 warning +* removes dependencies on build_tools +* uses format 2 package definition +* implements subdivision of map tiles at the highest zoom levels to correctly warp map to the canvas coordinate system +* only transform tile map when the transform changes +* fixes related to merging catkin branch into tile_map and building on Ubuntu 12.04 +* initial working implementation of tile map plugin +* Contributors: Ed Venator, Edward Venator, Marc Alban, P. J. Reed diff --git a/tile_map/CMakeLists.txt b/tile_map/CMakeLists.txt new file mode 100644 index 000000000..1850bb005 --- /dev/null +++ b/tile_map/CMakeLists.txt @@ -0,0 +1,141 @@ +### SET CMAKE POLICIES ### +cmake_minimum_required(VERSION 3.10...3.17) + +if(${CMAKE_VERSION} VERSION_LESS 3.12) + cmake_policy(VERSION ${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION}) +endif() + +if (NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(${CMAKE_VERSION} VERSION_EQUAL "3.11.0" OR ${CMAKE_VERSION} VERSION_GREATER "3.11.0") + cmake_policy(SET CMP0072 NEW) +endif() + +if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() +### END CMAKE POLICIES ### + +project(tile_map + LANGUAGES CXX) + +# ROS packages +find_package(ament_cmake REQUIRED) +find_package(mapviz REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(swri_math_util REQUIRED) +find_package(swri_transform_util REQUIRED) +find_package(tf2 REQUIRED) + +# Qt 5 +find_package(Qt5Core REQUIRED) +find_package(Qt5Gui REQUIRED) +find_package(Qt5Network REQUIRED) +find_package(Qt5OpenGL REQUIRED) +find_package(Qt5Widgets REQUIRED) +# Still useful? +# add_definitions(-DWFlags=WindowFlags) + +### OpenGL ### +find_package(OpenGL REQUIRED) + +### GLEW ### +find_package(GLEW REQUIRED) + +### PkgConfig ### +find_package(PkgConfig REQUIRED) + +### yaml-cpp ### +pkg_check_modules(YamlCpp yaml-cpp) + +### Use PkgConfig to find jsoncpp ### +pkg_check_modules(JSONCPP REQUIRED jsoncpp) + +# Fix conflict between Boost signals used by tf and QT signals used by mapviz +add_definitions(-DQT_NO_KEYWORDS) + +# Need to include the current dir to include the results of building Qt UI files +set(CMAKE_INCLUDE_CURRENT_DIR ON) + +set(TILE_SRC_FILES + src/image_cache.cpp + src/texture_cache.cpp + src/bing_source.cpp + src/tile_source.cpp + src/wmts_source.cpp + src/${PROJECT_NAME}_view.cpp +) +set(QT_HEADERS + include/${PROJECT_NAME}/image_cache.h + include/${PROJECT_NAME}/tile_source.h + include/${PROJECT_NAME}/wmts_source.h + include/${PROJECT_NAME}/bing_source.h +) +qt5_wrap_cpp(TILE_SRC_FILES ${QT_HEADERS}) +add_library(${PROJECT_NAME} ${TILE_SRC_FILES}) +target_include_directories(${PROJECT_NAME} + PUBLIC + include) +target_include_directories(${PROJECT_NAME} SYSTEM + PUBLIC + GLEW::GLEW + ${JSONCPP_INCLUDE_DIRS} + ${YamlCpp_INCLUDE_DIRS}) +ament_target_dependencies(${PROJECT_NAME} + ament_cmake + mapviz + pluginlib + rclcpp + swri_math_util + swri_transform_util + tf2 +) +target_link_libraries(${PROJECT_NAME} + ${GLU_LIBRARY} + ${JSONCPP_LIBRARIES} + ${YamlCpp_LIBRARIES} + Qt5::Network + Qt5::Core + Qt5::Gui + Qt5::OpenGL + Qt5::Widgets +) + +set(PLUGIN_SRC_FILES + src/${PROJECT_NAME}_plugin.cpp) +set(PLUGIN_UI_FILES + src/${PROJECT_NAME}_config.ui) + +qt5_wrap_ui(PLUGIN_SRC_FILES ${PLUGIN_UI_FILES}) +qt5_wrap_cpp(PLUGIN_SRC_FILES include/${PROJECT_NAME}/${PROJECT_NAME}_plugin.h) + +add_library(${PROJECT_NAME}_plugin SHARED ${PLUGIN_SRC_FILES}) +target_compile_definitions(${PROJECT_NAME}_plugin PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +target_link_libraries(${PROJECT_NAME}_plugin ${PROJECT_NAME}) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} + FILES_MATCHING PATTERN "*.h" +) + +install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_plugin + RUNTIME DESTINATION lib/${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib +) + +pluginlib_export_plugin_description_file(mapviz mapviz_plugins.xml) + +ament_export_dependencies(${RUNTIME_DEPS} + Qt5::Network + Qt5::Core + Qt5::Gui + Qt5::OpenGL + Qt5::Widgets +) +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_package() diff --git a/tile_map/include/tile_map/bing_source.h b/tile_map/include/tile_map/bing_source.h new file mode 100644 index 000000000..8413de547 --- /dev/null +++ b/tile_map/include/tile_map/bing_source.h @@ -0,0 +1,141 @@ +// ***************************************************************************** +// +// Copyright (c) 2015, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL Southwest Research Institute® BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY +// OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +// DAMAGE. +// +// ***************************************************************************** + +#ifndef TILE_MAP_BING_SOURCE_H +#define TILE_MAP_BING_SOURCE_H + +#include "tile_source.h" + +#include +#include + +#include + +#include +#include +#include + +namespace tile_map +{ + class BingSource : public TileSource + { + Q_OBJECT + public: + /** + * Initializes a Bing map source with a given name. + * + * Note that currently, only a single, hard-coded Bing map source is supported. + * There's only one Bing Maps, after all. + * In the future, though, it would probably make sense to extend its + * functionality to allow pulling different tile sets from Bing. + * @param name The name the source will appear as in the combo box. + */ + explicit BingSource(const QString& name); + + /** + * Generates a unique hash that identifies the tile as the given coordinates. + * + * Note that Bing Maps tiles could potentially be pulled from one of many + * different servers, depending on the subdomain list given to us after we + * authenticate with our API Key. That means the exact URL to any given tile + * should not be used as part of the hash, because there are many valid URLs + * for a tile. + * @param level The zoom level + * @param x The X coordinate + * @param y The Y coordinate + * @return A hash that uniquely identifies this tile + */ + size_t GenerateTileHash(int32_t level, int64_t x, int64_t y) override; + + /** + * Generates a URL that will retrieve a tile for the given coordinates. + * + * Since Bing can give us a list of subdomains to pull tiles from, the + * exact subdomain for a tile is chosen at random every time this function + * is called. That means you are not guaranteed to get the same URL for a + * tile every time you call this function. + * @param level The zoom level + * @param x The X coordinate + * @param y The Y coordinate + * @return A URL that points to this tile + */ + QString GenerateTileUrl(int32_t level, int64_t x, int64_t y) override; + + QString GetType() const override; + + QString GetApiKey() const; + + /** + * Bing requires an API key in order to access its tiles. The key provided + * will determine the URL we use to retrieve map tiles, so setting the API + * key will also cause this object to make a network request to the Bing Map + * server to get the appropriate URL. + * + * More information about getting an API key: + * https://msdn.microsoft.com/en-us/library/ff428642.aspx + * @param api_key A valid Bing Maps key + */ + void SetApiKey(const QString& api_key); + + static const QString BING_TYPE; + + protected Q_SLOTS: + void ReplyFinished(QNetworkReply* reply); + + protected: + /** + * Bing Maps identifies tiles using a quadkey that is generated from the zoom + * level and x and y coordinates. Details on how the quadkey is generated can + * be found here: + * https://msdn.microsoft.com/en-us/library/bb259689.aspx + * + * @param level The zoom level + * @param x The X coordinate + * @param y The Y coordinate + * @return The quadkey that represents the tile at the requested location + */ + QString GenerateQuadKey(int32_t level, int64_t x, int64_t y) const; + + QString api_key_; + boost::hash hash_; + QNetworkAccessManager network_manager_; + boost::random::mt19937 rng_; + std::vector subdomains_; + QString tile_url_; + + static const std::string BING_IMAGE_URL_KEY; + static const std::string BING_IMAGE_URL_SUBDOMAIN_KEY; + static const std::string BING_RESOURCE_SET_KEY; + static const std::string BING_RESOURCE_KEY; + static const std::string BING_STATUS_CODE_KEY; + }; +} + +#endif //TILE_MAP_BING_SOURCE_H diff --git a/tile_map/include/tile_map/image_cache.h b/tile_map/include/tile_map/image_cache.h new file mode 100644 index 000000000..cdfb8483f --- /dev/null +++ b/tile_map/include/tile_map/image_cache.h @@ -0,0 +1,171 @@ +// ***************************************************************************** +// +// Copyright (c) 2014-2020, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef TILE_MAP_IMAGE_CACHE_H_ +#define TILE_MAP_IMAGE_CACHE_H_ + +#include +#include + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace tile_map +{ + class CacheThread; + + class Image + { + public: + Image(const QString& uri, size_t uri_hash, uint64_t priority = 0); + ~Image() = default; + + QString Uri() const { return uri_; } + size_t UriHash() const { return uri_hash_; } + + std::shared_ptr GetImage() { return image_; } + + void InitializeImage(); + void ClearImage(); + + void AddFailure(); + bool Failed() const { return failed_; } + + void IncreasePriority() + { + if (priority_ < std::numeric_limits::max()) + { + priority_++; + } + } + void SetPriority(uint64_t priority) { priority_ = priority; } + uint64_t Priority() const { return priority_; } + + bool Loading() const { return loading_; } + void SetLoading(bool loading) { loading_ = loading; } + + private: + QString uri_; + + size_t uri_hash_; + + bool loading_; + int32_t failures_; + bool failed_; + uint64_t priority_; + + mutable std::shared_ptr image_; + + static const int MAXIMUM_FAILURES; + }; + typedef std::shared_ptr ImagePtr; + + class ImageCache : public QObject + { + Q_OBJECT + + public: + explicit ImageCache(const QString& cache_dir, + size_t size = 4096, + rclcpp::Logger logger = rclcpp::get_logger("tile_map::ImageCache")); + ~ImageCache() override; + + ImagePtr GetImage(size_t uri_hash, const QString& uri, int32_t priority = 0); + + void SetLogger(rclcpp::Logger logger); + + public Q_SLOTS: + void ProcessRequest(QString uri); + void ProcessReply(QNetworkReply* reply); + void Clear(); + + private: + QNetworkAccessManager network_manager_; + + QString cache_dir_; + + QCache cache_; + QMap unprocessed_; + QSet failed_; + QMap uri_to_hash_map_; + + QMutex cache_mutex_; + QMutex unprocessed_mutex_; + bool exit_; + + uint64_t tick_; + + CacheThread* cache_thread_; + + QSemaphore network_request_semaphore_; + + rclcpp::Logger logger_; + + friend class CacheThread; + + static const int MAXIMUM_NETWORK_REQUESTS; + }; + + class CacheThread : public QThread + { + Q_OBJECT + public: + explicit CacheThread(ImageCache* parent); + + void run() override; + + void notify(); + + Q_SIGNALS: + void RequestImage(QString); + + private: + ImageCache* image_cache_; + QMutex waiting_mutex_; + + static const int MAXIMUM_SEQUENTIAL_REQUESTS; + }; + + + typedef std::shared_ptr ImageCachePtr; +} + +#endif // TILE_MAP_IMAGE_CACHE_H_ diff --git a/tile_map/include/tile_map/texture_cache.h b/tile_map/include/tile_map/texture_cache.h new file mode 100644 index 000000000..39f62531d --- /dev/null +++ b/tile_map/include/tile_map/texture_cache.h @@ -0,0 +1,78 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef TILE_MAP_TEXTURE_CACHE_H_ +#define TILE_MAP_TEXTURE_CACHE_H_ + +#include + +#include + +#include + +namespace tile_map +{ + class Texture + { + public: + Texture(int32_t texture_id, size_t hash); + ~Texture(); + + const int32_t id; + const size_t url_hash; + + bool failed; + }; + typedef std::shared_ptr TexturePtr; + + class TextureCache + { + public: + explicit TextureCache(ImageCachePtr image_cache, + size_t size = 512, + rclcpp::Logger logger = rclcpp::get_logger("tile_map::TextureCache")); + + TexturePtr GetTexture(size_t url_hash, const QString& url, bool& failed, int priority); + void AddTexture(const TexturePtr& texture); + + void SetLogger(rclcpp::Logger logger); + + void Clear(); + + private: + QCache cache_; + + ImageCachePtr image_cache_; + + rclcpp::Logger logger_; + }; + typedef std::shared_ptr TextureCachePtr; +} + +#endif // TILE_MAP_TEXTURE_CACHE_H_ diff --git a/tile_map/include/tile_map/tile_map_plugin.h b/tile_map/include/tile_map/tile_map_plugin.h new file mode 100644 index 000000000..d0b9cb2da --- /dev/null +++ b/tile_map/include/tile_map/tile_map_plugin.h @@ -0,0 +1,126 @@ +// ***************************************************************************** +// +// Copyright (c) 2015, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef TILE_MAP_TILE_MAP_PLUGIN_H_ +#define TILE_MAP_TILE_MAP_PLUGIN_H_ + +// C++ standard libraries +#include +#include + +// Boost libraries +#include +#include + +#include + +// QT libraries +#include +#include +#include + +#include +#include + +// QT autogenerated files +#include "ui_tile_map_config.h" + +namespace tile_map +{ + class TileSource; + + class TileMapPlugin : public mapviz::MapvizPlugin + { + Q_OBJECT + + public: + TileMapPlugin(); + ~TileMapPlugin() override = default; + + bool Initialize(QGLWidget* canvas) override; + void Shutdown() override {} + + void Draw(double x, double y, double scale) override; + + void Transform() override; + + void LoadConfig(const YAML::Node& node, const std::string& path) override; + void SaveConfig(YAML::Emitter& emitter, const std::string& path) override; + + QWidget* GetConfigWidget(QWidget* parent) override; + + void SetNode(rclcpp::Node& node) override; + + protected Q_SLOTS: + void PrintError(const std::string& message) override; + void PrintInfo(const std::string& message) override; + void PrintWarning(const std::string& message) override; + + void DeleteTileSource(); + void SelectSource(const QString& source_name); + void SaveCustomSource(); + void ResetTileCache(); + + private: + void selectTileSource(const std::shared_ptr& tile_source); + void startCustomEditing(); + void stopCustomEditing(); + + Ui::tile_map_config ui_; + QWidget* config_widget_; + + swri_transform_util::Transform transform_; + swri_transform_util::Transform inverse_transform_; + + bool transformed_; + + TileMapView tile_map_; + std::map > tile_sources_; + + double last_center_x_; + double last_center_y_; + double last_scale_; + int32_t last_height_; + int32_t last_width_; + + static std::string BASE_URL_KEY; + static std::string BING_API_KEY; + static std::string CUSTOM_SOURCES_KEY; + static std::string MAX_ZOOM_KEY; + static std::string NAME_KEY; + static std::string SOURCE_KEY; + static std::string TYPE_KEY; + static QString BING_NAME; + static QString STAMEN_TERRAIN_NAME; + static QString STAMEN_TONER_NAME; + static QString STAMEN_WATERCOLOR_NAME; + }; +} + +#endif // TILE_MAP_TILE_MAP_PLUGIN_H_ diff --git a/tile_map/include/tile_map/tile_map_view.h b/tile_map/include/tile_map/tile_map_view.h new file mode 100644 index 000000000..eb9232243 --- /dev/null +++ b/tile_map/include/tile_map/tile_map_view.h @@ -0,0 +1,115 @@ +// ***************************************************************************** +// +// Copyright (c) 2015, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef TILE_MAP_TILE_MAP_VIEW_H_ +#define TILE_MAP_TILE_MAP_VIEW_H_ + +#include + +#include +#include + +#include + +#include + +namespace tile_map +{ + class TileSource; + + struct Tile + { + public: + QString url; + size_t url_hash; + int32_t level; + int32_t subdiv_count; + double subwidth; + + TexturePtr texture; + + std::vector points; + std::vector points_t; + }; + + class TileMapView + { + public: + explicit TileMapView(rclcpp::Logger logger = rclcpp::get_logger("tile_map::TileMapView")); + + bool IsReady(); + + void ResetCache(); + + void SetLogger(rclcpp::Logger logger); + + void SetTileSource(const std::shared_ptr& tile_source); + + void SetTransform(const swri_transform_util::Transform& transform); + + void SetView( + double latitude, + double longitude, + double scale, + int32_t width, + int32_t height); + + void Draw(); + + private: + void DrawTiles(std::vector &tiles ,int priority); + + std::shared_ptr tile_source_; + + swri_transform_util::Transform transform_; + + int32_t level_; + + int64_t center_x_; + int64_t center_y_; + + int64_t size_; + + int32_t width_; + int32_t height_; + + std::vector tiles_; + std::vector precache_; + + TextureCachePtr tile_cache_; + + rclcpp::Logger logger_; + + void ToLatLon(int32_t level, double x, double y, double& latitude, double& longitude); + + void InitializeTile(int32_t level, int64_t x, int64_t y, Tile& tile, int priority); + }; +} + +#endif // TILE_MAP_TILE_MAP_VIEW_H_ diff --git a/tile_map/include/tile_map/tile_source.h b/tile_map/include/tile_map/tile_source.h new file mode 100644 index 000000000..d1c5d504e --- /dev/null +++ b/tile_map/include/tile_map/tile_source.h @@ -0,0 +1,122 @@ +// ***************************************************************************** +// +// Copyright (c) 2015, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL Southwest Research Institute® BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY +// OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +// DAMAGE. +// +// ***************************************************************************** + +#ifndef TILE_MAP_TILE_SOURCE_H +#define TILE_MAP_TILE_SOURCE_H + +#include +#include + +namespace tile_map +{ + /** + * Represents a network source for map tiles; contains information about how to + * connect to the source and how to retrieve tiles from it. + * + * Implementations of this should be specific to a type of map server such as + * WMS, WMTS, or TMS, and implement the appropriate methods for retrieiving tiles + * from those servers. + */ + class TileSource : public QObject + { + Q_OBJECT + public: + ~TileSource() override = default; + + virtual const QString& GetBaseUrl() const; + + virtual void SetBaseUrl(const QString& base_url); + + virtual bool IsCustom() const; + + virtual bool IsReady() const { return is_ready_; }; + + virtual void SetCustom(bool is_custom); + + virtual int32_t GetMaxZoom() const; + + virtual void SetMaxZoom(int32_t max_zoom); + + virtual int32_t GetMinZoom() const; + + virtual void SetMinZoom(int32_t min_zoom); + + virtual const QString& GetName() const; + + virtual void SetName(const QString& name); + + /** + * Generates a hash that uniquely identifies the tile from this source at the + * specified level and coordinates. + * @param level The zoom level + * @param x The X coordinate + * @param y The Y coordinate + * @return A hash identifying the tile + */ + virtual size_t GenerateTileHash(int32_t level, int64_t x, int64_t y) = 0; + + /** + * Generates an HTTP or HTTPS URL that refers to a map tile from this source + * at the given level and x and y coordinates. + * @param level The zoom level + * @param x The x coordinate + * @param y The y coordinate + * @return A URL referring to the map tile + */ + virtual QString GenerateTileUrl(int32_t level, int64_t x, int64_t y) = 0; + + /** + * Returns a string identifying the type of map source ("wmts", "bing", etc.) + * @return + */ + virtual QString GetType() const = 0; + + Q_SIGNALS: + void ErrorMessage(const std::string& error_msg) const; + void InfoMessage(const std::string& info_msg) const; + + protected: + TileSource() : + is_custom_(false), + is_ready_(true), + max_zoom_(20), + min_zoom_(0) + {}; + + QString base_url_; + bool is_custom_; + bool is_ready_; + int32_t max_zoom_; + int32_t min_zoom_; + QString name_; + }; +} + +#endif //TILE_MAP_TILE_SOURCE_H diff --git a/tile_map/include/tile_map/wmts_source.h b/tile_map/include/tile_map/wmts_source.h new file mode 100644 index 000000000..b9d645823 --- /dev/null +++ b/tile_map/include/tile_map/wmts_source.h @@ -0,0 +1,86 @@ +// ***************************************************************************** +// +// Copyright (c) 2015, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL Southwest Research Institute® BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY +// OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +// DAMAGE. +// +// ***************************************************************************** + +#ifndef TILE_MAP_WMTS_SOURCE_H +#define TILE_MAP_WMTS_SOURCE_H + +#include "tile_source.h" + +#include + +namespace tile_map +{ + class WmtsSource : public TileSource + { + Q_OBJECT + public: + /** + * Creates a new tile source from a set of known parameters. + * + * @param[in] name A user-friendly display name + * @param[in] base_url The base HTTP URL of the data source; e. g.: + * "http://tile.stamen.com/terrain/" + * @param[in] is_custom If this is a custom (i. e. not one of the default) + * tile source; custom sources are saved and loaded from our settings + * @param[in] max_zoom The maximum zoom level + */ + explicit WmtsSource(const QString& name, + const QString& base_url, + bool is_custom, + int32_t max_zoom); + + virtual size_t GenerateTileHash(int32_t level, int64_t x, int64_t y); + + /** + * Given a zoom level and x and y coordinates appropriate for the tile source's + * projection, this will generate a URL that points to an image tile for that + * location. + * + * This expects the URL to have three strings in it, "{level}", "{x}", and "{y}", + * which will be replaced with the passed values. See tile_map_plugin.cpp for + * example URLs. + * + * @param[in] level The zoom level + * @param[in] x The X coordinate of the tile + * @param[in] y The Y coordinate of the tile + * @return A URL that references that tile + */ + virtual QString GenerateTileUrl(int32_t level, int64_t x, int64_t y); + + virtual QString GetType() const; + + static const QString WMTS_TYPE; + + private: + boost::hash hash_; + }; +} + +#endif //TILE_MAP_WMTS_SOURCE_H diff --git a/tile_map/mapviz_plugins.xml b/tile_map/mapviz_plugins.xml new file mode 100644 index 000000000..27829fd45 --- /dev/null +++ b/tile_map/mapviz_plugins.xml @@ -0,0 +1,6 @@ + + + Plugin for viewing slippy maps from a map server. + + + diff --git a/tile_map/package.xml b/tile_map/package.xml new file mode 100644 index 000000000..9abce9a36 --- /dev/null +++ b/tile_map/package.xml @@ -0,0 +1,42 @@ + + tile_map + 2.3.0 + + + Tile map provides a slippy map style interface for visualizing + OpenStreetMap and GoogleMap tiles. A mapviz visualization plug-in is also + implemented + + + Marc Alban + P. J. Reed + Southwest Research Institute + BSD + https://github.com/swri-robotics/mapviz + + ament_cmake + qt5-qmake + + libjsoncpp-dev + libqt5-core + libqt5-opengl-dev + + libglew-dev + mapviz + pluginlib + rclcpp + swri_math_util + swri_transform_util + tf2 + yaml-cpp + + libjsoncpp + libqt5-opengl + + + ament_cmake + + + + + diff --git a/tile_map/src/bing_source.cpp b/tile_map/src/bing_source.cpp new file mode 100644 index 000000000..cb3c3a1d6 --- /dev/null +++ b/tile_map/src/bing_source.cpp @@ -0,0 +1,187 @@ +// ***************************************************************************** +// +// Copyright (c) 2015, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL Southwest Research Institute® BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY +// OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +// DAMAGE. +// +// ***************************************************************************** + +#include +#include +#include + +#include +#include + +#include + +namespace tile_map +{ + const QString BingSource::BING_TYPE = "bing"; + const std::string BingSource::BING_IMAGE_URL_KEY = "imageUrl"; + const std::string BingSource::BING_IMAGE_URL_SUBDOMAIN_KEY = "imageUrlSubdomains"; + const std::string BingSource::BING_RESOURCE_SET_KEY = "resourceSets"; + const std::string BingSource::BING_RESOURCE_KEY = "resources"; + const std::string BingSource::BING_STATUS_CODE_KEY = "statusCode"; + + BingSource::BingSource(const QString& name) : + TileSource(), + network_manager_(this) + { + name_ = name; + is_custom_ = false; + max_zoom_ = 19; + base_url_ = "https://dev.virtualearth.net/REST/v1/Imagery/Metadata/Aerial?uriScheme=https&include=ImageryProviders&key={api_key}"; + tile_url_ = ""; + min_zoom_ = 2; + + QObject::connect(&network_manager_, SIGNAL(finished(QNetworkReply*)), + this, SLOT(ReplyFinished(QNetworkReply*))); + } + + size_t BingSource::GenerateTileHash(int32_t level, int64_t x, int64_t y) + { + size_t hash = hash_((base_url_ + api_key_ + GenerateQuadKey(level, x, y)).toStdString()); + return hash; + } + + QString BingSource::GenerateTileUrl(int32_t level, int64_t x, int64_t y) + { + QString url = tile_url_; + if (!subdomains_.empty()) + { + boost::random::uniform_int_distribution<> random(0, (int) subdomains_.size() - 1); + url.replace(QString::fromStdString("{subdomain}"), subdomains_[random(rng_)]); + } + url.replace(QString::fromStdString("{quadkey}"), GenerateQuadKey(level, x, y)); + return url; + } + + QString BingSource::GetType() const + { + return BING_TYPE; + } + + QString BingSource::GetApiKey() const + { + return api_key_; + } + + void BingSource::SetApiKey(const QString& api_key) + { + api_key_ = api_key.trimmed(); + if (!api_key_.isEmpty()) + { + QString url(base_url_); + url.replace(QString::fromStdString("{api_key}"), api_key_); + // Changing the API key will result in the tile URL changing; go ahead + // and blank it out so we don't make requests using the old one. + tile_url_= ""; + subdomains_.clear(); + network_manager_.get(QNetworkRequest(QUrl(url))); + } + } + + QString BingSource::GenerateQuadKey(int32_t level, int64_t x, int64_t y) const + { + QString quadkey; + for (int32_t i = level; i > 0; i--) + { + int32_t bitmask = 1 << (i-1); + int32_t digit = 0; + if ((x & bitmask) != 0) + { + digit |= 1; + } + if ((y & bitmask) != 0) + { + digit |= 2; + } + quadkey.append(QString::number(digit)); + } + + return quadkey; + } + + void BingSource::ReplyFinished(QNetworkReply* reply) + { + QString reply_string(reply->readAll()); + Json::Reader reader; + Json::Value root; + reader.parse(reply_string.toStdString(), root); + + int status = root[BING_STATUS_CODE_KEY].asInt(); + if (status != 200) + { + Q_EMIT ErrorMessage("Bing authorization error: " + std::to_string(status)); + } + else + { + if (!root[BING_RESOURCE_SET_KEY].isArray() || + root[BING_RESOURCE_SET_KEY].empty()) + { + Q_EMIT ErrorMessage("No Bing resource sets found."); + return; + } + Json::Value firstResourceSet = root[BING_RESOURCE_SET_KEY][0]; + + if (!firstResourceSet[BING_RESOURCE_KEY].isArray() || + firstResourceSet[BING_RESOURCE_KEY].empty()) + { + Q_EMIT ErrorMessage("No Bing resources found."); + return; + } + + Json::Value first_resource = firstResourceSet[BING_RESOURCE_KEY][0]; + + std::string image_url = first_resource[BING_IMAGE_URL_KEY].asString(); + + if (image_url.empty()) + { + Q_EMIT ErrorMessage("No Bing image URL found."); + return; + } + + tile_url_ = QString::fromStdString(image_url); + SetMaxZoom(19); + + + if (!first_resource[BING_IMAGE_URL_SUBDOMAIN_KEY].isArray() || + first_resource[BING_IMAGE_URL_SUBDOMAIN_KEY].empty()) + { + Q_EMIT ErrorMessage("No image URL subdomains; maybe that's ok sometimes?"); + } + + for (const auto& subdomain : first_resource[BING_IMAGE_URL_SUBDOMAIN_KEY]) + { + subdomains_.push_back(QString::fromStdString(subdomain.asString())); + } + + Q_EMIT InfoMessage("API Key Set."); + + is_ready_ = true; + } + } +} diff --git a/tile_map/src/image_cache.cpp b/tile_map/src/image_cache.cpp new file mode 100644 index 000000000..510446e76 --- /dev/null +++ b/tile_map/src/image_cache.cpp @@ -0,0 +1,337 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace tile_map +{ + bool ComparePriority(const ImagePtr left, const ImagePtr right) + { + return left->Priority() > right->Priority(); + } + + const int Image::MAXIMUM_FAILURES = 5; + + Image::Image(const QString& uri, size_t uri_hash, uint64_t priority) : + uri_(uri), + uri_hash_(uri_hash), + loading_(false), + failures_(0), + failed_(false), + priority_(priority) + { + } + + void Image::InitializeImage() + { + image_ = std::make_shared(); + } + + void Image::ClearImage() + { + image_.reset(); + } + + void Image::AddFailure() + { + failures_++; + failed_ = failures_ > MAXIMUM_FAILURES; + } + + const int ImageCache::MAXIMUM_NETWORK_REQUESTS = 6; + + ImageCache::ImageCache(const QString& cache_dir, + size_t size, + rclcpp::Logger logger) : + network_manager_(this), + cache_dir_(cache_dir), + cache_(size), + exit_(false), + tick_(0), + cache_thread_(new CacheThread(this)), + network_request_semaphore_(MAXIMUM_NETWORK_REQUESTS), + logger_(logger) + { + QNetworkDiskCache* disk_cache = new QNetworkDiskCache(this); + disk_cache->setCacheDirectory(cache_dir_); + network_manager_.setCache(disk_cache); + + connect(&network_manager_, SIGNAL(finished(QNetworkReply*)), this, SLOT(ProcessReply(QNetworkReply*))); + connect(cache_thread_, SIGNAL(RequestImage(QString)), this, SLOT(ProcessRequest(QString))); + + cache_thread_->start(); + cache_thread_->setPriority(QThread::NormalPriority); + } + + ImageCache::~ImageCache() + { + // After setting our exit flag to true, release any conditions the cache thread + // might be waiting on so that it will exit. + exit_ = true; + cache_thread_->notify(); + network_request_semaphore_.release(MAXIMUM_NETWORK_REQUESTS); + cache_thread_->wait(); + delete cache_thread_; + } + + void ImageCache::Clear() + { + cache_.clear(); + network_manager_.cache()->clear(); + } + + ImagePtr ImageCache::GetImage(size_t uri_hash, const QString& uri, int32_t priority) + { + ImagePtr image; + + // Retrieve the image reference from the cache, updating the freshness. + cache_mutex_.lock(); + + if (failed_.contains(uri_hash)) + { + cache_mutex_.unlock(); + return image; + } + + ImagePtr* image_ptr = cache_.take(uri_hash); + if (!image_ptr) + { + // If the image is not in the cache, create a new reference. + image_ptr = new ImagePtr(std::make_shared(uri, uri_hash)); + image = *image_ptr; + if (!cache_.insert(uri_hash, image_ptr)) + { + RCLCPP_ERROR(logger_, "FAILED TO CREATE HANDLE: %s", uri.toStdString().c_str()); + image_ptr = nullptr; + } + } + else + { + image = *image_ptr; + + // Add raw pointer back to cache. + cache_.insert(uri_hash, image_ptr); + } + + cache_mutex_.unlock(); + + unprocessed_mutex_.lock(); + if (image && !image->GetImage()) + { + if (!image->Failed()) + { + if (!unprocessed_.contains(uri_hash)) + { + // Set an image's starting priority so that it's higher than the + // starting priority of every other image we've requested so + // far; that ensures that, all other things being equal, the + // most recently requested images will be loaded first. + image->SetPriority(priority + tick_++); + unprocessed_[uri_hash] = image; + uri_to_hash_map_[uri] = uri_hash; + cache_thread_->notify(); + } + else + { + // Every time an image is requested but hasn't been loaded yet, + // increase its priority. Tiles within the visible area will + // be requested more frequently, so this will make them load faster + // than tiles the user can't see. + image->SetPriority(priority + tick_++); + } + } + else + { + failed_.insert(uri_hash); + } + } + + unprocessed_mutex_.unlock(); + + return image; + } + + void ImageCache::SetLogger(rclcpp::Logger logger) + { + logger_ = logger; + } + + void ImageCache::ProcessRequest(QString uri) + { + QNetworkRequest request; + request.setUrl(QUrl(uri)); + request.setRawHeader("User-Agent", "mapviz-1.0"); + request.setAttribute( + QNetworkRequest::CacheLoadControlAttribute, + QNetworkRequest::PreferCache); + request.setAttribute( + QNetworkRequest::HttpPipeliningAllowedAttribute, + true); + + network_manager_.get(request); + } + + void ImageCache::ProcessReply(QNetworkReply* reply) + { + QString url = reply->url().toString(); + + ImagePtr image; + unprocessed_mutex_.lock(); + + size_t hash = uri_to_hash_map_[url]; + image = unprocessed_[hash]; + if (image) + { + if (reply->error() == QNetworkReply::NoError) + { + QByteArray data = reply->readAll(); + image->InitializeImage(); + if (!image->GetImage()->loadFromData(data)) + { + image->ClearImage(); + image->AddFailure(); + } + } + else + { + auto steady_clock = rclcpp::Clock(); + RCLCPP_ERROR_THROTTLE(logger_, steady_clock, 1.0, "NETWORK ERROR: %s", reply->errorString().toStdString().c_str()); + image->AddFailure(); + } + } + + unprocessed_.remove(hash); + uri_to_hash_map_.remove(url); + if (image) + { + image->SetLoading(false); + } + network_request_semaphore_.release(); + + unprocessed_mutex_.unlock(); + + reply->deleteLater(); + } + + const int CacheThread::MAXIMUM_SEQUENTIAL_REQUESTS = 12; + + CacheThread::CacheThread(ImageCache* parent) : + image_cache_(parent), + waiting_mutex_() + { + waiting_mutex_.lock(); + } + + void CacheThread::notify() + { + waiting_mutex_.unlock(); + } + + void CacheThread::run() + { + while (!image_cache_->exit_) + { + // Wait until we're told there are images we need to request. + waiting_mutex_.lock(); + + // Next, get all of them and sort them by priority. + image_cache_->unprocessed_mutex_.lock(); + QList images = image_cache_->unprocessed_.values(); + image_cache_->unprocessed_mutex_.unlock(); + + std::sort(images.begin(), images.end(), ComparePriority); + + // Go through all of them and request them. Qt's network manager will + // only handle six simultaneous requests at once, so we use a semaphore + // to limit ourselves to that many. + // Each individual image will release the semaphore when it is done loading. + // Also, only load up to a certain number at a time in this loop. If there + // are more left afterward, we'll start over. This ensures that we + // concentrate on processing the highest-priority images. + int count = 0; + while (!image_cache_->exit_ && !images.empty() && count < MAXIMUM_SEQUENTIAL_REQUESTS) + { + image_cache_->network_request_semaphore_.acquire(); + + ImagePtr image = images.front(); + image_cache_->unprocessed_mutex_.lock(); + if (!image->Loading() && !image->Failed()) + { + count++; + image->SetLoading(true); + images.pop_front(); + + QString uri = image->Uri(); + size_t hash = image_cache_->uri_to_hash_map_[uri]; + if (uri.startsWith(QString("file:///"))) + { + image->InitializeImage(); + QString filepath = uri.replace(QString("file:///"), QString("/")); + if (!image->GetImage()->load(filepath)) + { + image->ClearImage(); + image->AddFailure(); + } + + image_cache_->unprocessed_.remove(hash); + image_cache_->uri_to_hash_map_.remove(uri); + image->SetLoading(false); + image_cache_->network_request_semaphore_.release(); + } + else + { + Q_EMIT RequestImage(image->Uri()); + } + } + else + { + images.pop_front(); + } + image_cache_->unprocessed_mutex_.unlock(); + + } + if (!images.empty()) + { + waiting_mutex_.unlock(); + } + } + } +} diff --git a/tile_map/src/texture_cache.cpp b/tile_map/src/texture_cache.cpp new file mode 100644 index 000000000..8bb4fedeb --- /dev/null +++ b/tile_map/src/texture_cache.cpp @@ -0,0 +1,178 @@ +// ***************************************************************************** +// +// Copyright (c) 2014, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include + +#include + +#include + +#include +#include +#include + +#include + +#include +#include + +#include + +namespace tile_map +{ + Texture::Texture(int32_t texture_id, size_t hash) : + id(texture_id), + url_hash(hash) + { + } + + Texture::~Texture() + { + //ROS_ERROR("==== DELETING TEXTURE: %d ====", id); + // The texture will automatically be freed from the GPU memory when it goes + // out of scope. This is effectively when it is no longer in the texture + // cache or being referenced for a render. + GLuint ids[1]; + ids[0] = id; + glDeleteTextures(1, &ids[0]); + } + + TextureCache::TextureCache(ImageCachePtr image_cache, + size_t size, + rclcpp::Logger logger) : + cache_(size), + image_cache_(image_cache), + logger_(logger) + { + + } + + TexturePtr TextureCache::GetTexture(size_t url_hash, const QString& url, bool& failed, int priority) + { + TexturePtr texture; + + failed = false; + + TexturePtr* texture_ptr = cache_.take(url_hash); + if (texture_ptr) + { + texture = *texture_ptr; + delete texture_ptr; + } + + if (!texture) + { + ImagePtr image = image_cache_->GetImage(url_hash, url, priority); + + if (image) + { + failed = image->Failed(); + std::shared_ptr image_ptr = image->GetImage(); + if (image_ptr) + { + // All of the OpenGL calls need to occur on the main thread and so + // can't be done in the background. The QImage calls could + // potentially be done in a background thread by the image cache. + QImage qimage = *image_ptr; + + GLuint ids[1]; + uint32_t check = 9999999; + ids[0] = check; + + glGenTextures(1, &ids[0]); + + if (check == ids[0]) + { + RCLCPP_ERROR(logger_, "FAILED TO CREATE TEXTURE"); + + GLenum err = glGetError(); + const GLubyte *errString = gluErrorString(err); + RCLCPP_ERROR(logger_, "GL ERROR(%u): %s", err, errString); + return texture; + } + + texture_ptr = new TexturePtr(std::make_shared(ids[0], url_hash)); + texture = *texture_ptr; + + float max_dim = std::max(qimage.width(), qimage.height()); + int32_t dimension = swri_math_util::Round( + std::pow(2, std::ceil(std::log(max_dim) / std::log(2.0f)))); + + if (qimage.width() != dimension || qimage.height() != dimension) + { + qimage = qimage.scaled(dimension, dimension, Qt::IgnoreAspectRatio, Qt::FastTransformation); + } + + glBindTexture(GL_TEXTURE_2D, texture->id); + glTexImage2D( + GL_TEXTURE_2D, + 0, + GL_RGBA, + dimension, + dimension, + 0, + GL_RGBA, + GL_UNSIGNED_BYTE, + QGLWidget::convertToGLFormat(qimage).bits()); + + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); + + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE); + + cache_.insert(url_hash, texture_ptr); + } + } + } + + return texture; + } + + void TextureCache::AddTexture(const TexturePtr& texture) + { + if (texture) + { + TexturePtr* texture_ptr = new TexturePtr(texture); + cache_.insert(texture->url_hash, texture_ptr); + } + } + + void TextureCache::SetLogger(rclcpp::Logger logger) + { + logger_ = logger; + image_cache_->SetLogger(logger_); + } + + void TextureCache::Clear() + { + image_cache_->Clear(); + cache_.clear(); + } +} diff --git a/tile_map/src/tile_map_config.ui b/tile_map/src/tile_map_config.ui new file mode 100644 index 000000000..068e61d13 --- /dev/null +++ b/tile_map/src/tile_map_config.ui @@ -0,0 +1,198 @@ + + + tile_map_config + + + + 0 + 0 + 305 + 141 + + + + Form + + + + + + + 4 + + + 2 + + + + + + Sans Serif + 8 + + + + Base URL: + + + + + + + + Sans Serif + 8 + + + + Source: + + + + + + + + Sans Serif + 8 + + + + + + + Unconfigured + + + true + + + + + + + + Sans Serif + 8 + + + + Max Zoom: + + + + + + + Reset Cache + + + + + + + + 16777215 + 27 + + + + false + + + + Stamen (terrain) + + + + + Stamen (watercolor) + + + + + Stamen (toner) + + + + + Bing Maps (terrain) + + + + + Custom WMTS Source... + + + + + + + + false + + + http://tile.stamen.com/terrain/ + + + + + + + false + + + Delete + + + + + + + false + + + Save... + + + + + + + + Sans Serif + 8 + + + + Status: + + + + + + + false + + + + 0 + 0 + + + + + 50 + 16777215 + + + + 15 + + + + + + + + diff --git a/tile_map/src/tile_map_plugin.cpp b/tile_map/src/tile_map_plugin.cpp new file mode 100644 index 000000000..85991b143 --- /dev/null +++ b/tile_map/src/tile_map_plugin.cpp @@ -0,0 +1,469 @@ +// ***************************************************************************** +// +// Copyright (c) 2015-2020, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include +#include +#include +#include + +#include + +#include +#include +#include + +// QT libraries +#include +#include +#include +#include + +// ROS libraries +#include +#include + +#include + +// Declare plugin +#include +PLUGINLIB_EXPORT_CLASS(tile_map::TileMapPlugin, mapviz::MapvizPlugin) + +namespace tile_map +{ + std::string TileMapPlugin::BASE_URL_KEY = "base_url"; + std::string TileMapPlugin::BING_API_KEY = "bing_api_key"; + std::string TileMapPlugin::CUSTOM_SOURCES_KEY = "custom_sources"; + std::string TileMapPlugin::MAX_ZOOM_KEY = "max_zoom"; + std::string TileMapPlugin::NAME_KEY = "name"; + std::string TileMapPlugin::SOURCE_KEY = "source"; + std::string TileMapPlugin::TYPE_KEY = "type"; + QString TileMapPlugin::BING_NAME = "Bing Maps (terrain)"; + QString TileMapPlugin::STAMEN_TERRAIN_NAME = "Stamen (terrain)"; + QString TileMapPlugin::STAMEN_TONER_NAME = "Stamen (toner)"; + QString TileMapPlugin::STAMEN_WATERCOLOR_NAME = "Stamen (watercolor)"; + + TileMapPlugin::TileMapPlugin() + : MapvizPlugin() + , ui_() + , config_widget_(new QWidget()) + , transformed_(false) + , last_center_x_(0.0) + , last_center_y_(0.0) + , last_scale_(0.0) + , last_height_(0) + , last_width_(0) + { + ui_.setupUi(config_widget_); + + tile_sources_[STAMEN_TERRAIN_NAME] = + std::make_shared(STAMEN_TERRAIN_NAME, + "http://tile.stamen.com/terrain/{level}/{x}/{y}.png", + false, + 15); + tile_sources_[STAMEN_TONER_NAME] = + std::make_shared(STAMEN_TONER_NAME, + "http://tile.stamen.com/toner/{level}/{x}/{y}.png", + false, + 19); + tile_sources_[STAMEN_WATERCOLOR_NAME] = + std::make_shared(STAMEN_WATERCOLOR_NAME, + "http://tile.stamen.com/watercolor/{level}/{x}/{y}.jpg", + false, + 19); + std::shared_ptr bing = std::make_shared(BING_NAME); + tile_sources_[BING_NAME] = bing; + + QPalette p(config_widget_->palette()); + p.setColor(QPalette::Window, Qt::white); + config_widget_->setPalette(p); + + QPalette p2(ui_.status->palette()); + p2.setColor(QPalette::Text, Qt::red); + ui_.status->setPalette(p2); + + source_frame_ = swri_transform_util::_wgs84_frame; + + QObject::connect(bing.get(), SIGNAL(ErrorMessage(const std::string&)), + this, SLOT(PrintError(const std::string&))); + QObject::connect(bing.get(), SIGNAL(InfoMessage(const std::string&)), + this, SLOT(PrintInfo(const std::string&))); + QObject::connect(ui_.delete_button, SIGNAL(clicked()), this, SLOT(DeleteTileSource())); + QObject::connect(ui_.source_combo, SIGNAL(activated(const QString&)), this, SLOT(SelectSource(const QString&))); + QObject::connect(ui_.save_button, SIGNAL(clicked()), this, SLOT(SaveCustomSource())); + QObject::connect(ui_.reset_cache_button, SIGNAL(clicked()), this, SLOT(ResetTileCache())); + } + + void TileMapPlugin::DeleteTileSource() + { + int source_index = ui_.source_combo->currentIndex(); + QString current_name = ui_.source_combo->currentText(); + + QMessageBox mbox; + mbox.setText("Are you sure you want to delete the source \"" + current_name + "\"?"); + mbox.setIcon(QMessageBox::Warning); + mbox.setStandardButtons(QMessageBox::Ok | QMessageBox::Cancel); + mbox.setDefaultButton(QMessageBox::Cancel); + int ret = mbox.exec(); + + if (ret == QMessageBox::Ok) + { + ui_.source_combo->removeItem(source_index); + tile_sources_.erase(current_name); + ui_.source_combo->setCurrentIndex(0); + SelectSource(ui_.source_combo->currentText()); + } + } + + void TileMapPlugin::SelectSource(const QString& source) + { + if (source == STAMEN_TERRAIN_NAME || + source == STAMEN_WATERCOLOR_NAME || + source == STAMEN_TONER_NAME || + source == BING_NAME) + { + stopCustomEditing(); + } + else + { + startCustomEditing(); + } + + std::map >::iterator iter = tile_sources_.find(source); + + // If the previously selected source was Bing, these will have been changed, so + // they should be changed back. There's not an easy way to know here what the + // previously selected item was, so just always change them. + ui_.url_label->setText("Base URL:"); + ui_.save_button->setText("Save..."); + if (iter != tile_sources_.end()) + { + selectTileSource(iter->second); + initialized_ = true; + // For the Bing map type, change a couple of the fields to have more appropriate + // labels. There should probably be a cleaner way to do this if we end up adding + // more tile source types.... + if (iter->second->GetType() == BingSource::BING_TYPE) + { + ui_.url_label->setText("API Key:"); + ui_.save_button->setText("Save"); + ui_.base_url_text->setEnabled(true); + ui_.save_button->setEnabled(true); + } + } + else + { + ui_.delete_button->setEnabled(false); + } + } + + void TileMapPlugin::SaveCustomSource() + { + // If the user is editing a custom source, we want to fill in the default + // name for it with its current name. + // Otherwise, they're creating a new custom source, in which case we + // should leave the default blank. + QString current_source = ui_.source_combo->currentText(); + QString default_name = ""; + + auto iter = tile_sources_.find(current_source); + if (iter != tile_sources_.end()) + { + if (iter->second->IsCustom()) + { + default_name = current_source; + } + else if (iter->second->GetType() == BingSource::BING_TYPE) + { + // If the user has picked Bing as they're source, we're not actually + // saving a custom map source, just updating the API key + BingSource* bing_source = dynamic_cast(iter->second.get()); + bing_source->SetApiKey(ui_.base_url_text->text()); + return; + } + } + + bool ok; + QString name = QInputDialog::getText(config_widget_, + tr("Save New Tile Source"), + tr("Tile Source Name:"), + QLineEdit::Normal, + default_name, + &ok); + name = name.trimmed(); + if (ok && !name.isEmpty()) + { + std::shared_ptr source = std::make_shared(name, + ui_.base_url_text->text(), + true, + ui_.max_zoom_spin_box->value()); + int existing_index = ui_.source_combo->findText(name); + if (existing_index != -1) + { + ui_.source_combo->removeItem(existing_index); + } + tile_sources_[name] = source; + ui_.source_combo->addItem(name); + int new_index = ui_.source_combo->findText(name); + ui_.source_combo->setCurrentIndex(new_index); + SelectSource(name); + } + } + + void TileMapPlugin::ResetTileCache() + { + tile_map_.ResetCache(); + } + + void TileMapPlugin::PrintError(const std::string& message) + { + if (message == ui_.status->text().toStdString()) + return; + + RCLCPP_ERROR(node_->get_logger(), "Error: %s", message.c_str()); + QPalette p(ui_.status->palette()); + p.setColor(QPalette::Text, Qt::red); + ui_.status->setPalette(p); + ui_.status->setText(message.c_str()); + } + + void TileMapPlugin::PrintInfo(const std::string& message) + { + if (message == ui_.status->text().toStdString()) + return; + + RCLCPP_INFO(node_->get_logger(), "%s", message.c_str()); + QPalette p(ui_.status->palette()); + p.setColor(QPalette::Text, Qt::green); + ui_.status->setPalette(p); + ui_.status->setText(message.c_str()); + } + + void TileMapPlugin::PrintWarning(const std::string& message) + { + if (message == ui_.status->text().toStdString()) + return; + + RCLCPP_WARN(node_->get_logger(), "%s", message.c_str()); + QPalette p(ui_.status->palette()); + p.setColor(QPalette::Text, Qt::darkYellow); + ui_.status->setPalette(p); + ui_.status->setText(message.c_str()); + } + + QWidget* TileMapPlugin::GetConfigWidget(QWidget* parent) + { + config_widget_->setParent(parent); + + return config_widget_; + } + + bool TileMapPlugin::Initialize(QGLWidget* canvas) + { + canvas_ = canvas; + + SelectSource(STAMEN_TERRAIN_NAME); + + return true; + } + + void TileMapPlugin::Draw(double x, double y, double scale) + { + if (!tile_map_.IsReady()) + { + return; + } + + swri_transform_util::Transform to_wgs84; + if (tf_manager_->GetTransform(source_frame_, target_frame_, to_wgs84)) + { + tf2::Vector3 center(x, y, 0); + center = to_wgs84 * center; + + if (center.y() != last_center_y_ || + center.x() != last_center_x_ || + scale != last_scale_ || + canvas_->width() != last_width_ || + canvas_->height() != last_height_) + { + // Draw() is called very frequently, and SetView is a fairly expensive operation, so we + // can save some CPU time by only calling it when the relevant parameters have changed. + last_center_y_ = center.y(); + last_center_x_ = center.x(); + last_scale_ = scale; + last_width_ = canvas_->width(); + last_height_ = canvas_->height(); + tile_map_.SetView(center.y(), center.x(), scale, canvas_->width(), canvas_->height()); + RCLCPP_DEBUG(node_->get_logger(), "TileMapPlugin::Draw: Successfully set view"); + } + tile_map_.Draw(); + } + } + + void TileMapPlugin::Transform() + { + swri_transform_util::Transform to_target; + if (tf_manager_->GetTransform(target_frame_, source_frame_, to_target)) + { + tile_map_.SetTransform(to_target); + PrintInfo("OK"); + } + else + { + PrintError("No transform between " + source_frame_ + " and " + target_frame_); + } + } + + void TileMapPlugin::LoadConfig(const YAML::Node& node, const std::string&) + { + if (node[CUSTOM_SOURCES_KEY]) + { + const YAML::Node& sources = node[CUSTOM_SOURCES_KEY]; + YAML::Node::const_iterator source_iter; + for (auto source_yaml : sources) + { + std::string type; + if (source_yaml[TYPE_KEY]) + { + // If the type isn't set, we'll assume it's WMTS + type = source_yaml[TYPE_KEY].as(); + } + std::shared_ptr source; + if (type == "wmts" || type.empty()) + { + std::string name; + std::string base_url; + int max_zoom; + name = source_yaml[NAME_KEY].as(); + base_url = source_yaml[BASE_URL_KEY].as(); + max_zoom = source_yaml[MAX_ZOOM_KEY].as(); + source = std::make_shared( + QString::fromStdString(name), + QString::fromStdString(base_url), + true, + max_zoom); + } + else if (type == "bing") + { + std::string name; + name = source_yaml[NAME_KEY].as(); + source = std::make_shared(QString::fromStdString(name)); + } + tile_sources_[source->GetName()] = source; + ui_.source_combo->addItem(source->GetName()); + } + } + + if (node[BING_API_KEY]) + { + std::string key = node[BING_API_KEY].as(); + BingSource* source = dynamic_cast(tile_sources_[BING_NAME].get()); + source->SetApiKey(QString::fromStdString(key)); + } + + if (node[SOURCE_KEY]) + { + std::string source = node[SOURCE_KEY].as(); + + int index = ui_.source_combo->findText(QString::fromStdString(source), Qt::MatchExactly); + + if (index >= 0) + { + ui_.source_combo->setCurrentIndex(index); + } + + SelectSource(QString::fromStdString(source)); + } + } + + void TileMapPlugin::SaveConfig(YAML::Emitter& emitter, const std::string&) + { + emitter << YAML::Key << CUSTOM_SOURCES_KEY << YAML::Value << YAML::BeginSeq; + + std::map >::iterator iter; + for (iter = tile_sources_.begin(); iter != tile_sources_.end(); iter++) + { + if (iter->second->IsCustom()) + { + emitter << YAML::BeginMap; + emitter << YAML::Key << BASE_URL_KEY << YAML::Value << iter->second->GetBaseUrl().toStdString(); + emitter << YAML::Key << MAX_ZOOM_KEY << YAML::Value << iter->second->GetMaxZoom(); + emitter << YAML::Key << NAME_KEY << YAML::Value << iter->second->GetName().toStdString(); + emitter << YAML::Key << TYPE_KEY << YAML::Value << iter->second->GetType().toStdString(); + emitter << YAML::EndMap; + } + } + emitter << YAML::EndSeq; + + BingSource* bing_source = dynamic_cast(tile_sources_[BING_NAME].get()); + emitter << YAML::Key << BING_API_KEY << + YAML::Value << boost::trim_copy(bing_source->GetApiKey().toStdString()); + + emitter << YAML::Key << SOURCE_KEY << + YAML::Value << boost::trim_copy(ui_.source_combo->currentText().toStdString()); + } + + void TileMapPlugin::selectTileSource(const std::shared_ptr& tile_source) + { + last_height_ = 0; // This will force us to recalculate our view + tile_map_.SetTileSource(tile_source); + if (tile_source->GetType() == BingSource::BING_TYPE) + { + BingSource* bing_source = dynamic_cast(tile_source.get()); + ui_.base_url_text->setText(bing_source->GetApiKey()); + } + else + { + ui_.base_url_text->setText(tile_source->GetBaseUrl()); + } + ui_.max_zoom_spin_box->setValue(tile_source->GetMaxZoom()); + } + + void TileMapPlugin::startCustomEditing() + { + ui_.base_url_text->setEnabled(true); + ui_.delete_button->setEnabled(true); + ui_.max_zoom_spin_box->setEnabled(true); + ui_.save_button->setEnabled(true); + } + + void TileMapPlugin::stopCustomEditing() + { + ui_.base_url_text->setEnabled(false); + ui_.delete_button->setEnabled(false); + ui_.max_zoom_spin_box->setEnabled(false); + ui_.save_button->setEnabled(false); + } + + void TileMapPlugin::SetNode(rclcpp::Node& node) + { + MapvizPlugin::SetNode(node); + tile_map_.SetLogger(node.get_logger()); + } +} + diff --git a/tile_map/src/tile_map_view.cpp b/tile_map/src/tile_map_view.cpp new file mode 100644 index 000000000..92d39191a --- /dev/null +++ b/tile_map/src/tile_map_view.cpp @@ -0,0 +1,325 @@ +// ***************************************************************************** +// +// Copyright (c) 2015, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include + +#include + +#include + +#include +#include +#include + +#include + +#include +#include +#include + +#include + +namespace tile_map +{ + TileMapView::TileMapView(rclcpp::Logger logger) : + level_(-1), + width_(100), + height_(100), + logger_(logger) + { + ImageCachePtr image_cache = std::make_shared("/tmp/tile_map", 4096, logger); + tile_cache_ = std::make_shared(image_cache, 512, logger); + } + + bool TileMapView::IsReady() + { + return tile_source_ && tile_source_->IsReady(); + } + + void TileMapView::ResetCache() + { + tile_cache_->Clear(); + } + + void TileMapView::SetLogger(rclcpp::Logger logger) + { + logger_ = logger; + tile_cache_->SetLogger(logger_); + } + + void TileMapView::SetTileSource(const std::shared_ptr& tile_source) + { + tile_source_ = tile_source; + level_ = -1; + } + + void TileMapView::SetTransform(const swri_transform_util::Transform& transform) + { + if (transform.GetOrigin() == transform_.GetOrigin() && + transform.GetOrientation() == transform_.GetOrientation()) + { + return; + } + + transform_ = transform; + + for (auto & tile : tiles_) + { + for (size_t j = 0; j < tile.points_t.size(); j++) + { + tile.points_t[j] = transform_ * tile.points[j]; + } + } + + for (auto & i : precache_) + { + for (size_t j = 0; j < i.points_t.size(); j++) + { + i.points_t[j] = transform_ * i.points[j]; + } + } + } + + void TileMapView::SetView( + double latitude, + double longitude, + double scale, + int32_t width, + int32_t height) + { + latitude = std::max(-90.0, std::min(90.0, latitude)); + longitude = std::max(-180.0, std::min(180.0, longitude)); + + double lat = swri_math_util::ToRadians(latitude); + + // Calculate the current zoom level: + // + // According to http://wiki.openstreetmap.org/wiki/Zoom_levels: + // meters_per_pixel = earth_circumference * cos(lat) / 2^(level + 8) + // + // Therefore, + // level = log2(earth_circumference * cos(lat) / meters_per_pixel) - 8 + // + double lat_circumference = + swri_transform_util::_earth_equator_circumference * std::cos(lat) / scale; + int32_t level = std::min(tile_source_->GetMaxZoom(), + std::max(tile_source_->GetMinZoom(), static_cast(std::ceil(std::log(lat_circumference) / std::log(2) - 8)))); + int64_t max_size = std::pow(2, level); + + int64_t center_x = std::min(max_size - 1, static_cast( + std::floor(((longitude + 180.0) / 360.0) * std::pow(2.0, level)))); + int64_t center_y = std::min(max_size - 1, static_cast( + std::floor((1.0 - std::log(std::tan(lat) + 1.0 / std::cos(lat)) / swri_math_util::_pi) / 2.0 * std::pow(2.0, level)))); + + width_ = width; + height_ = height; + + double max_dimension = std::max(width, height); + + double meters_per_pixel = swri_transform_util::_earth_equator_circumference * std::cos(lat) / std::pow(2, level + 8); + double tile_size = 256.0 * (meters_per_pixel / scale); + + int64_t size = std::max(static_cast(1L), std::min(max_size, static_cast( + std::ceil(0.5 * max_dimension / tile_size) * 2 + 1))); + + if (size > 50) + { + RCLCPP_ERROR(logger_, "Invalid map size: %ld", size); + return; + } + + if (size_ != size || level_ != level || center_x_ != center_x || center_y_ != center_y) + { + size_ = size; + level_ = level; + center_x_ = center_x; + center_y_ = center_y; + + int64_t top = std::max(static_cast(0L), center_y_ - size_ / 2); + int64_t left = std::max(static_cast(0L), center_x_ - size_ / 2); + + int64_t right = std::min(max_size, left + size_); + int64_t bottom = std::min(max_size, top + size_); + + for (auto & tile : tiles_) + { + tile_cache_->AddTexture(tile.texture); + } + tiles_.clear(); + + for (int64_t i = top; i < bottom; i++) + { + for (int64_t j = left; j < right; j++) + { + Tile tile; + InitializeTile(level_, j, i, tile, 10000); + tiles_.push_back(tile); + } + } + + for (auto & i : precache_) + { + tile_cache_->AddTexture(i.texture); + } + precache_.clear(); + + if (level_ > 0) + { + int64_t precache_x = std::floor(((longitude + 180.0) / 360.0) * std::pow(2.0, level - 1)); + int64_t precache_y = std::floor((1.0 - std::log(std::tan(lat) + 1.0 / std::cos(lat)) / swri_math_util::_pi) / 2.0 * std::pow(2.0, level - 1)); + + int64_t precache_max_size = std::pow(2, level - 1); + + int64_t precache_top = std::max(static_cast(0L), precache_y - (size_ - 1) / 2); + int64_t precache_left = std::max(static_cast(0L), precache_x - (size_ - 1) / 2); + + int64_t precache_right = std::min(precache_max_size, precache_left + size_); + int64_t precache_bottom = std::min(precache_max_size, precache_top + size_); + + for (int64_t i = precache_top; i < precache_bottom; i++) + { + for (int64_t j = precache_left; j < precache_right; j++) + { + Tile tile; + InitializeTile(level_ - 1, j, i, tile, 0); + precache_.push_back(tile); + } + } + } + } + } + + void TileMapView::DrawTiles(std::vector& tiles, int priority) + { + for (auto & tile : tiles) + { + TexturePtr& texture = tile.texture; + + if (!texture) + { + bool failed; + texture = tile_cache_->GetTexture(tile.url_hash, tile.url, failed, priority); + } + + if (texture) + { + glBindTexture(GL_TEXTURE_2D, texture->id); + + glBegin(GL_TRIANGLES); + + glColor4f(1.0f, 1.0f, 1.0f, 1.0f); + + for (int32_t row = 0; row < tile.subdiv_count; row++) + { + for (int32_t col = 0; col < tile.subdiv_count; col++) + { + double u_0 = col * tile.subwidth; + double v_0 = 1.0 - row * tile.subwidth; + double u_1 = (col + 1.0) * tile.subwidth; + double v_1 = 1.0 - (row + 1.0) * tile.subwidth; + + const tf2::Vector3& tl = tile.points_t[row * (tile.subdiv_count + 1) + col]; + const tf2::Vector3& tr = tile.points_t[row * (tile.subdiv_count + 1) + col + 1]; + const tf2::Vector3& br = tile.points_t[(row + 1) * (tile.subdiv_count + 1) + col + 1]; + const tf2::Vector3& bl = tile.points_t[(row + 1) * (tile.subdiv_count + 1) + col]; + + // Triangle 1 + glTexCoord2f(u_0, v_0); glVertex2d(tl.x(), tl.y()); + glTexCoord2f(u_1, v_0); glVertex2d(tr.x(), tr.y()); + glTexCoord2f(u_1, v_1); glVertex2d(br.x(), br.y()); + + // Triangle 2 + glTexCoord2f(u_0, v_0); glVertex2d(tl.x(), tl.y()); + glTexCoord2f(u_1, v_1); glVertex2d(br.x(), br.y()); + glTexCoord2f(u_0, v_1); glVertex2d(bl.x(), bl.y()); + } + } + + glEnd(); + + glBindTexture(GL_TEXTURE_2D, 0); + } + } + } + + void TileMapView::Draw() + { + if (!tile_source_) + { + return; + } + + glEnable(GL_TEXTURE_2D); + + DrawTiles( precache_, 0 ); + DrawTiles( tiles_, 10000 ); + + glDisable(GL_TEXTURE_2D); + } + + void TileMapView::ToLatLon(int32_t level, double x, double y, double& latitude, double& longitude) + { + double n = std::pow(2, level); + longitude = x / n * 360.0 - 180.0; + + double r = swri_math_util::_pi - swri_math_util::_2pi * y / n; + latitude = swri_math_util::_rad_2_deg * std::atan(0.5 * (std::exp(r) - std::exp(-r))); + } + + void TileMapView::InitializeTile(int32_t level, int64_t x, int64_t y, Tile& tile, int priority) + { + tile.url = tile_source_->GenerateTileUrl(level, x, y); + + tile.url_hash = tile_source_->GenerateTileHash(level, x, y); + + tile.level = level; + + bool failed; + tile.texture = tile_cache_->GetTexture(tile.url_hash, tile.url, failed, priority); + + int32_t subdivs = std::max(0, 4 - level); + tile.subwidth = 1.0 / (subdivs + 1.0); + tile.subdiv_count = std::pow(2, subdivs); + for (int32_t row = 0; row <= tile.subdiv_count; row++) + { + for (int32_t col = 0; col <= tile.subdiv_count; col++) + { + double t_lat, t_lon; + ToLatLon(level, x + col * tile.subwidth, y + row * tile.subwidth, t_lat, t_lon); + tile.points.emplace_back(tf2::Vector3(t_lon, t_lat, 0)); + } + } + + tile.points_t = tile.points; + for (auto & i : tile.points_t) + { + i = transform_ * i; + } + } +} diff --git a/tile_map/src/tile_source.cpp b/tile_map/src/tile_source.cpp new file mode 100644 index 000000000..383186aa0 --- /dev/null +++ b/tile_map/src/tile_source.cpp @@ -0,0 +1,84 @@ +// ***************************************************************************** +// +// Copyright (c) 2015, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL Southwest Research Institute® BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY +// OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +// DAMAGE. +// +// ***************************************************************************** + +#include + +namespace tile_map +{ + const QString& TileSource::GetBaseUrl() const + { + return base_url_; + } + + void TileSource::SetBaseUrl(const QString& base_url) + { + base_url_ = base_url; + } + + bool TileSource::IsCustom() const + { + return is_custom_; + } + + void TileSource::SetCustom(bool is_custom) + { + is_custom_ = is_custom; + } + + int32_t TileSource::GetMaxZoom() const + { + return max_zoom_; + } + + void TileSource::SetMaxZoom(int32_t max_zoom) + { + max_zoom_ = max_zoom; + } + + int32_t TileSource::GetMinZoom() const + { + return min_zoom_; + } + + void TileSource::SetMinZoom(int32_t min_zoom) + { + min_zoom_ = min_zoom; + } + + const QString& TileSource::GetName() const + { + return name_; + } + + void TileSource::SetName(const QString& name) + { + name_ = name; + } +} diff --git a/tile_map/src/wmts_source.cpp b/tile_map/src/wmts_source.cpp new file mode 100644 index 000000000..b056825aa --- /dev/null +++ b/tile_map/src/wmts_source.cpp @@ -0,0 +1,71 @@ +// ***************************************************************************** +// +// Copyright (c) 2015, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL Southwest Research Institute® BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY +// OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +// DAMAGE. +// +// ***************************************************************************** + +#include + +#include + +namespace tile_map +{ + const QString WmtsSource::WMTS_TYPE = "wmts"; + + WmtsSource::WmtsSource(const QString& name, + const QString& base_url, + bool is_custom, + int32_t max_zoom) : + TileSource() + { + name_ = name; + base_url_ = base_url; + is_custom_ = is_custom; + max_zoom_ = max_zoom; + min_zoom_ = 1; + } + + QString WmtsSource::GetType() const + { + return WMTS_TYPE; + } + + size_t WmtsSource::GenerateTileHash(int32_t level, int64_t x, int64_t y) + { + return hash_(GenerateTileUrl(level, x, y).toStdString()); + } + + QString WmtsSource::GenerateTileUrl(int32_t level, int64_t x, int64_t y) + { + QString url(base_url_); + url.replace(QString::fromStdString("{level}"), QString::number(level)); + url.replace(QString::fromStdString("{x}"), QString::number(x)); + url.replace(QString::fromStdString("{y}"), QString::number(y)); + + return url; + } +}