diff --git a/README.md b/README.md index 92641ab25..a3986844c 100644 --- a/README.md +++ b/README.md @@ -106,7 +106,7 @@ Overlays a [sensor_msgs::Image](http://docs.ros.org/api/sensor_msgs/html/msg/Ima * Height: Display height * Units: (pixels | percent of window) -### LaserScan +### LaserScan Projects a [sensor_msgs::LaserScan](http://docs.ros.org/api/sensor_msgs/html/msg/LaserScan.html) message into the scene. @@ -135,7 +135,7 @@ This plugin allows the user to send goals to [move_base](wiki.ros.org/move_base) * Abort: stop the execution of the current goal. ### Multi-res Image -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. +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. ![](https://github.com/swri-robotics/mapviz/wiki/multires2.png) @@ -148,11 +148,11 @@ A custom format is currently used to store the map tiles and geo-reference. The image_width: 29184 # The full pixel width of the map image_height: 15872 # The full pixel height of the map tile_size: 512 # The pixel size of the individual tiles - + datum: "wgs84" # Datum is currently ignored projection: "utm" # (utm|wgs84) - - # At least 2 tie points are required for + + # At least 2 tie points are required for # scale, and 3 for orientation. tiepoints: # [pixel x, pixel y, geo x, geo y] - point: [4799, 209, 535674.5, 3258382.5] @@ -162,7 +162,7 @@ A custom format is currently used to store the map tiles and geo-reference. The The map tiles are stored in directories for each resolution starting with layer0, the full resolution. In subsequent layers the resolution is halved until the entire map fits within a single tile. -Tiles are named using the following format: +Tiles are named using the following format: tile%05dx%05d.png % (row, column) @@ -204,6 +204,15 @@ Publishes a [geometry_msgs::PointStamped](http://docs.ros.org/api/geometry_msgs/ * Topic: The topic to publish the point to * Frame: The target frame to transform the point to before publishing it +### Right Click Services + +Shows a Custom Context Menu with available services at this sepcific gps location. +**Parameters** + * Topic: The topic to publish gps point to + * Frame: The target frame to transform the point to before publishing it + * available services topic: Service topic for Mapviz to retreive available services From + * gps command topic: Service topic for Mapviz to request the chosen gps command to be executed + ### Robot Image Projects an image loaded from file into the scene to represent the robot platform. diff --git a/mapviz/CMakeLists.txt b/mapviz/CMakeLists.txt index 69a74af8f..6b7d0ba24 100644 --- a/mapviz/CMakeLists.txt +++ b/mapviz/CMakeLists.txt @@ -194,6 +194,7 @@ set_target_properties(${PROJECT_NAME} PROPERTIES ) add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencpp + ${catkin_EXPORTED_TARGETS} ) ### Install mapviz ### diff --git a/mapviz/include/mapviz/map_canvas.h b/mapviz/include/mapviz/map_canvas.h index e209d9aa8..fcc157c08 100644 --- a/mapviz/include/mapviz/map_canvas.h +++ b/mapviz/include/mapviz/map_canvas.h @@ -62,6 +62,9 @@ namespace mapviz explicit MapCanvas(QWidget *parent = 0); ~MapCanvas(); + std::string showCustomContextMenu(const QPoint &pos,std::vector result); + + void InitializeTf(boost::shared_ptr tf); void AddPlugin(MapvizPluginPtr plugin, int order); @@ -181,6 +184,7 @@ namespace mapviz void mouseMoveEvent(QMouseEvent* e); void keyPressEvent(QKeyEvent* e); + void Recenter(); void TransformTarget(QPainter* painter); void Zoom(float factor); diff --git a/mapviz/src/map_canvas.cpp b/mapviz/src/map_canvas.cpp index b8ba8a5fa..830045eef 100644 --- a/mapviz/src/map_canvas.cpp +++ b/mapviz/src/map_canvas.cpp @@ -31,8 +31,14 @@ #include #include #include - +#include #include +#include +#include +#include "ros/ros.h" +#include +#include +#include // C++ standard libraries #include @@ -90,6 +96,7 @@ MapCanvas::MapCanvas(QWidget* parent) : setFrameRate(50.0); frame_rate_timer_.start(); setFocusPolicy(Qt::StrongFocus); + this->setContextMenuPolicy(Qt::CustomContextMenu); } MapCanvas::~MapCanvas() @@ -319,6 +326,7 @@ void MapCanvas::Zoom(float factor) void MapCanvas::mousePressEvent(QMouseEvent* e) { + mouse_x_ = e->x(); mouse_y_ = e->y(); mouse_previous_y_ = mouse_y_; @@ -326,6 +334,7 @@ void MapCanvas::mousePressEvent(QMouseEvent* e) drag_y_ = 0; mouse_pressed_ = true; mouse_button_ = e->button(); + } void MapCanvas::keyPressEvent(QKeyEvent* event) @@ -614,4 +623,38 @@ double MapCanvas::frameRate() const { return 1000.0 / frame_rate_timer_.interval(); } -} // namespace mapviz + +std::string MapCanvas::showCustomContextMenu(const QPoint &pos,std::vector result){ + QMenu contextMenu(this); + std::vector actions; + + for(auto service : result) + { + QAction * act= new QAction(service.c_str(),this); + actions.push_back(act); + } + for(auto action: actions) + { + contextMenu.addAction(action); + } + + QAction* selectedItem=contextMenu.exec(pos); + for(auto action: actions) + { + action->deleteLater(); + } + + if (selectedItem) + { + ROS_INFO("make call to %s",selectedItem->text().toStdString().c_str()); + return selectedItem->text().toStdString().c_str(); + }else{ + ROS_ERROR("Please choose a service from the context menu by clicking on a displayed item"); + return ""; + } + +} + + +} + diff --git a/mapviz_plugins/CHANGELOG.rst b/mapviz_plugins/CHANGELOG.rst index 573226252..0c88fd699 100644 --- a/mapviz_plugins/CHANGELOG.rst +++ b/mapviz_plugins/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package mapviz_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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 `_) diff --git a/mapviz_plugins/CMakeLists.txt b/mapviz_plugins/CMakeLists.txt index 06eb50440..002e48bd7 100644 --- a/mapviz_plugins/CMakeLists.txt +++ b/mapviz_plugins/CMakeLists.txt @@ -12,6 +12,7 @@ set(DEPENDENCIES marti_visualization_msgs move_base_msgs nav_msgs + map_msgs pluginlib roscpp sensor_msgs @@ -28,6 +29,36 @@ set(DEPENDENCIES find_package(catkin REQUIRED COMPONENTS ${DEPENDENCIES}) +set(CMAKE_INCLUDE_CURRENT_DIR ON) +find_package(Qt5Core REQUIRED) +find_package(Qt5Gui REQUIRED) +find_package(Qt5OpenGL REQUIRED) +find_package(Qt5Widgets REQUIRED) +set(QT_LIBRARIES + ${Qt5Core_LIBRARIES} + ${Qt5Gui_LIBRARIES} + ${Qt5OpenGL_LIBRARIES} + ${Qt5Widgets_LIBRARIES} +) +include_directories( + ${Qt5Core_INCLUDE_DIRS} + ${Qt5Gui_INCLUDE_DIRS} + ${Qt5OpenGL_INCLUDE_DIRS} + ${Qt5Widgets_INCLUDE_DIRS} +) +add_definitions( + ${Qt5Core_DEFINITIONS} + ${Qt5Gui_DEFINITIONS} + ${Qt5OpenGL_DEFINITIONS} + ${Qt5Widgets_DEFINITIONS} +) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${Qt5Widgets_EXECUTABLE_COMPILE_FLAGS}") + + +add_service_files(FILES + GPSCommand.srv +) +generate_messages(DEPENDENCIES sensor_msgs std_msgs) ### QT ### if("$ENV{ROS_DISTRO}" STRLESS "kinetic") find_package(Qt4 COMPONENTS REQUIRED QtCore QtGui QtOpenGL) @@ -97,10 +128,12 @@ set(UI_FILES ui/marker_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/right_click_services_config.ui ui/pointcloud2_config.ui ui/robot_image_config.ui ui/route_config.ui @@ -122,11 +155,13 @@ set(SRC_FILES src/marker_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/right_click_services_plugin.cpp src/pointcloud2_plugin.cpp src/point_drawing_plugin.cpp src/robot_image_plugin.cpp @@ -149,12 +184,14 @@ set(HEADER_FILES include/${PROJECT_NAME}/marker_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}/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}/right_click_services_plugin.h include/${PROJECT_NAME}/pointcloud2_plugin.h include/${PROJECT_NAME}/point_drawing_plugin.h include/${PROJECT_NAME}/robot_image_plugin.h diff --git a/mapviz_plugins/include/mapviz_plugins/canvas_click_filter.h b/mapviz_plugins/include/mapviz_plugins/canvas_click_filter.h index 81d7d98b0..c3a196dcf 100644 --- a/mapviz_plugins/include/mapviz_plugins/canvas_click_filter.h +++ b/mapviz_plugins/include/mapviz_plugins/canvas_click_filter.h @@ -55,7 +55,7 @@ namespace mapviz_plugins void setMaxClickMovement(qreal max_distance); Q_SIGNALS: - void pointClicked(const QPointF&); + void pointClicked(const QPointF&, const Qt::MouseButton&); protected: bool eventFilter(QObject *object, QEvent* event); diff --git a/mapviz_plugins/include/mapviz_plugins/image_plugin.h b/mapviz_plugins/include/mapviz_plugins/image_plugin.h index 916243bb0..c2174a105 100644 --- a/mapviz_plugins/include/mapviz_plugins/image_plugin.h +++ b/mapviz_plugins/include/mapviz_plugins/image_plugin.h @@ -126,7 +126,7 @@ namespace mapviz_plugins int offset_y_; double width_; double height_; - QString transport_; + std::string transport_; bool force_resubscribe_; bool has_image_; diff --git a/mapviz_plugins/include/mapviz_plugins/laserscan_plugin.h b/mapviz_plugins/include/mapviz_plugins/laserscan_plugin.h index 97af1ca5d..4137acfa1 100644 --- a/mapviz_plugins/include/mapviz_plugins/laserscan_plugin.h +++ b/mapviz_plugins/include/mapviz_plugins/laserscan_plugin.h @@ -99,6 +99,7 @@ namespace mapviz_plugins void UseRainbowChanged(int check_state); void UpdateColors(); void DrawIcon(); + void ResetTransformedScans(); private: struct StampedPoint @@ -146,6 +147,7 @@ namespace mapviz_plugins size_t prev_ranges_size_; float prev_angle_min_; float prev_increment_; + bool GetScanTransform(const Scan &scan, swri_transform_util::Transform& transform); }; } 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..b5701f2bf --- /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_GRID_PLUGIN_H_ +#define MAPVIZ_PLUGINS_GRID_PLUGIN_H_ + +// C++ standard libraries +#include +#include + +#include + +// QT libraries +#include +#include +#include +#include + +// ROS libraries +#include +#include + +#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(); + virtual ~OccupancyGridPlugin(); + + bool Initialize(QGLWidget* canvas); + void Shutdown(); + + void Draw(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: + void PrintError(const std::string& message); + void PrintInfo(const std::string& message); + void PrintWarning(const std::string& message); + + protected Q_SLOTS: + + void SelectTopicGrid(); + void TopicGridEdited(); + void upgradeCheckBoxToggled(bool); + void colorSchemeUpdated(const QString &); + + void DrawIcon(); + + void FrameChanged(std::string); + + private: + Ui::occupancy_grid_config ui_; + QWidget* config_widget_; + + nav_msgs::OccupancyGridConstPtr grid_; + + ros::Subscriber grid_sub_; + ros::Subscriber 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_; + int32_t texture_size_; + + Palette map_palette_; + Palette costmap_palette_; + + void Callback(const nav_msgs::OccupancyGridConstPtr& msg); + void CallbackUpdate(const map_msgs::OccupancyGridUpdateConstPtr& msg); + void updateTexture(); + + }; +} + +#endif // MAPVIZ_PLUGINS_GRID_PLUGIN_H_ diff --git a/mapviz_plugins/include/mapviz_plugins/odometry_plugin.h b/mapviz_plugins/include/mapviz_plugins/odometry_plugin.h index 71098d93b..37ee7e16c 100644 --- a/mapviz_plugins/include/mapviz_plugins/odometry_plugin.h +++ b/mapviz_plugins/include/mapviz_plugins/odometry_plugin.h @@ -89,7 +89,6 @@ namespace mapviz_plugins void TopicEdited(); private: - void DrawCovariance(); Ui::odometry_config ui_; QWidget* config_widget_; diff --git a/mapviz_plugins/include/mapviz_plugins/point_click_publisher_plugin.h b/mapviz_plugins/include/mapviz_plugins/point_click_publisher_plugin.h index 9c8e08879..e13daadf8 100644 --- a/mapviz_plugins/include/mapviz_plugins/point_click_publisher_plugin.h +++ b/mapviz_plugins/include/mapviz_plugins/point_click_publisher_plugin.h @@ -78,7 +78,7 @@ namespace mapviz_plugins QWidget* GetConfigWidget(QWidget* parent); protected Q_SLOTS: - void pointClicked(const QPointF& point); + void pointClicked(const QPointF& point, const Qt::MouseButton& button); void topicChanged(const QString& topic); void updateFrames(); diff --git a/mapviz_plugins/include/mapviz_plugins/point_drawing_plugin.h b/mapviz_plugins/include/mapviz_plugins/point_drawing_plugin.h index f28fd865c..c162205cc 100644 --- a/mapviz_plugins/include/mapviz_plugins/point_drawing_plugin.h +++ b/mapviz_plugins/include/mapviz_plugins/point_drawing_plugin.h @@ -55,6 +55,8 @@ namespace mapviz_plugins public: struct StampedPoint { + StampedPoint(): transformed(false) {} + tf::Point point; tf::Quaternion orientation; tf::Point transformed_point; @@ -93,21 +95,33 @@ namespace mapviz_plugins 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); virtual void DrawIcon(); 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); + 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::list points_; + std::deque points_; double position_tolerance_; int buffer_size_; bool covariance_checked_; @@ -119,7 +133,7 @@ namespace mapviz_plugins bool static_arrow_sizes_; private: - std::vector > laps_; + std::vector > laps_; bool got_begin_; tf::Point begin_; }; diff --git a/mapviz_plugins/include/mapviz_plugins/pointcloud2_plugin.h b/mapviz_plugins/include/mapviz_plugins/pointcloud2_plugin.h index 9d8e674bf..6d68a34ff 100644 --- a/mapviz_plugins/include/mapviz_plugins/pointcloud2_plugin.h +++ b/mapviz_plugins/include/mapviz_plugins/pointcloud2_plugin.h @@ -106,13 +106,13 @@ namespace mapviz_plugins void UpdateColors(); void DrawIcon(); void ResetTransformedPointClouds(); + void ClearPointClouds(); + void SetSubscription(bool subscribe); private: struct StampedPoint { tf::Point point; - tf::Point transformed_point; - QColor color; std::vector features; }; @@ -124,6 +124,11 @@ namespace mapviz_plugins 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&); diff --git a/mapviz_plugins/include/mapviz_plugins/right_click_services_plugin.h b/mapviz_plugins/include/mapviz_plugins/right_click_services_plugin.h new file mode 100644 index 000000000..f95a74a79 --- /dev/null +++ b/mapviz_plugins/include/mapviz_plugins/right_click_services_plugin.h @@ -0,0 +1,116 @@ +// ***************************************************************************** +// +// 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. +// +// ***************************************************************************** + +// ***************************************************************************** +// handling right click event, calls service which will display a +// number of services that are available on that specific location +// +// ***************************************************************************** + + +#ifndef MAPVIZ_PLUGINS_RIGHT_CLICK_SERVICES_H +#define MAPVIZ_PLUGINS_RIGHT_CLICK_SERVICES_H + +// Include mapviz_plugin.h first to ensure GL deps are included in the right order +#include + +#include +#include +#include +#include +#include + +#include + +// QT autogenerated files +#include "ui_right_click_services_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 RightClickServicesPlugin : public mapviz::MapvizPlugin + { + Q_OBJECT; + public: + + RightClickServicesPlugin(); + virtual ~RightClickServicesPlugin(); + + bool Initialize(QGLWidget* canvas); + void Shutdown() {} + + virtual void SetNode(const ros::NodeHandle& node); + virtual void PrintError(const std::string& message); + virtual void PrintInfo(const std::string& message); + virtual void PrintWarning(const std::string& message); + + void Draw(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); + + + void serviceCall(std::string service); + + + protected Q_SLOTS: + + void pointClicked(const QPointF& point, const Qt::MouseButton& button); + void topicChanged(const QString& topic); + void availableServiceTopicChanged(const QString& topic); + void gpsCommandTopicChanged(const QString& topic); + void updateFrames(); + + + private: + void showContextMenu(const QPoint& pos, boost::shared_ptr stamped); + Ui::right_click_services_config ui_; + QWidget* config_widget_; + + CanvasClickFilter click_filter_; + mapviz::MapCanvas* canvas_; + + QTimer frame_timer_; + ros::Publisher point_publisher_; + + std::string available_service_topic_; + std::string gps_command_execute_topic_; + }; +} + +#endif //MAPVIZ_PLUGINS_right_click_services_H diff --git a/mapviz_plugins/mapviz_plugins.xml b/mapviz_plugins/mapviz_plugins.xml index 46eb61fe5..b8c220dcf 100644 --- a/mapviz_plugins/mapviz_plugins.xml +++ b/mapviz_plugins/mapviz_plugins.xml @@ -64,8 +64,14 @@ Publishes a StampedPoint when a point on the map canvas is clicked. + + displays list of Services availableat that GPS location + Displays a std_msgs/String at a fixed point on the canvas. + + Display maps and other OccupancyGrids + diff --git a/mapviz_plugins/package.xml b/mapviz_plugins/package.xml index de4dbadeb..1d933b126 100644 --- a/mapviz_plugins/package.xml +++ b/mapviz_plugins/package.xml @@ -16,6 +16,9 @@ libqt_dev libqt_opengl_dev + message_generation + + message_runtime actionlib cv_bridge @@ -27,6 +30,7 @@ marti_visualization_msgs move_base_msgs nav_msgs + map_msgs pluginlib roscpp sensor_msgs @@ -42,6 +46,7 @@ libqt_core libqt_opengl + message_runtime diff --git a/mapviz_plugins/src/canvas_click_filter.cpp b/mapviz_plugins/src/canvas_click_filter.cpp index 42bfb3d72..f28af14c2 100644 --- a/mapviz_plugins/src/canvas_click_filter.cpp +++ b/mapviz_plugins/src/canvas_click_filter.cpp @@ -83,9 +83,9 @@ namespace mapviz_plugins if (msecsDiff < max_ms_ && distance <= max_distance_) { #if QT_VERSION >= 0x050000 - Q_EMIT pointClicked(me->localPos()); + Q_EMIT pointClicked(me->localPos(), me->button()); #else - Q_EMIT pointClicked(me->posF()); + Q_EMIT pointClicked(me->posF(), me->button()); #endif } } diff --git a/mapviz_plugins/src/gps_plugin.cpp b/mapviz_plugins/src/gps_plugin.cpp index 9dffa6e9a..fb8f3f25a 100644 --- a/mapviz_plugins/src/gps_plugin.cpp +++ b/mapviz_plugins/src/gps_plugin.cpp @@ -73,8 +73,12 @@ namespace mapviz_plugins this, SLOT(SetStaticArrowSizes(bool))); QObject::connect(ui_.arrow_size, SIGNAL(valueChanged(int)), this, SLOT(SetArrowSize(int))); - connect(ui_.color, SIGNAL(colorEdited(const QColor&)), this, + 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())); } GpsPlugin::~GpsPlugin() @@ -99,7 +103,7 @@ namespace mapviz_plugins if (topic != topic_) { initialized_ = false; - points_.clear(); + ClearPoints(); has_message_ = false; PrintWarning("No messages received."); @@ -135,7 +139,7 @@ namespace mapviz_plugins local_xy_util_.ToLocalXy(gps->latitude, gps->longitude, x, y); stamped_point.point = tf::Point(x, y, gps->altitude); - lap_checked_ = ui_.show_laps->isChecked(); + // The GPS "track" is in degrees, but createQuaternionFromYaw expects // radians. // Furthermore, the track rotates in the opposite direction and is also @@ -143,22 +147,7 @@ namespace mapviz_plugins stamped_point.orientation = tf::createQuaternionFromYaw((-gps->track * (M_PI / 180.0)) + M_PI_2); - if (points_.empty() || - (stamped_point.point.distance(points_.back().point)) >= - (position_tolerance_)) - { - points_.push_back(stamped_point); - } - - if (buffer_size_ > 0) - { - while (static_cast(points_.size()) > buffer_size_) - { - points_.pop_front(); - } - } - - cur_point_ = stamped_point; + pushPoint( std::move( stamped_point) ); } void GpsPlugin::PrintError(const std::string& message) @@ -212,8 +201,9 @@ namespace mapviz_plugins { std::string color; node["color"] >> color; - SetColor(QColor(color.c_str())); - ui_.color->setColor(color_); + QColor qcolor(color.c_str()); + SetColor(qcolor); + ui_.color->setColor(qcolor); } if (node["draw_style"]) @@ -223,31 +213,35 @@ namespace mapviz_plugins if (draw_style == "lines") { - draw_style_ = LINES; ui_.drawstyle->setCurrentIndex(0); + SetDrawStyle( LINES ); } else if (draw_style == "points") { - draw_style_ = POINTS; ui_.drawstyle->setCurrentIndex(1); + SetDrawStyle( POINTS ); } else if (draw_style == "arrows") { - draw_style_ = ARROWS; ui_.drawstyle->setCurrentIndex(2); + SetDrawStyle( ARROWS ); } } if (node["position_tolerance"]) { - node["position_tolerance"] >> position_tolerance_; - ui_.positiontolerance->setValue(position_tolerance_); + double position_tolerance; + node["position_tolerance"] >> position_tolerance; + ui_.positiontolerance->setValue(position_tolerance); + PositionToleranceChanged(position_tolerance); } if (node["buffer_size"]) { - node["buffer_size"] >> buffer_size_; - ui_.buffersize->setValue(buffer_size_); + double buffer_size; + node["buffer_size"] >> buffer_size; + ui_.buffersize->setValue(buffer_size); + BufferSizeChanged(buffer_size); } if (node["show_laps"]) @@ -255,6 +249,7 @@ namespace mapviz_plugins bool show_laps = false; node["show_laps"] >> show_laps; ui_.show_laps->setChecked(show_laps); + LapToggled(show_laps); } if (node["static_arrow_sizes"]) @@ -266,7 +261,9 @@ namespace mapviz_plugins if (node["arrow_size"]) { - ui_.arrow_size->setValue(node["arrow_size"].as()); + int arrow_size = node["arrow_size"].as(); + ui_.arrow_size->setValue(arrow_size); + SetArrowSize(arrow_size); } TopicEdited(); @@ -284,16 +281,9 @@ namespace mapviz_plugins emitter << YAML::Key << "draw_style" << YAML::Value << draw_style; emitter << YAML::Key << "position_tolerance" << - YAML::Value << position_tolerance_; + YAML::Value << positionTolerance(); - if (!lap_checked_) - { - emitter << YAML::Key << "buffer_size" << YAML::Value << buffer_size_; - } - else - { - emitter << YAML::Key << "buffer_size" << YAML::Value << buffer_holder_; - } + emitter << YAML::Key << "buffer_size" << YAML::Value << bufferSize(); bool show_laps = ui_.show_laps->isChecked(); emitter << YAML::Key << "show_laps" << YAML::Value << show_laps; diff --git a/mapviz_plugins/src/image_plugin.cpp b/mapviz_plugins/src/image_plugin.cpp index 7cd60850b..204aefbb5 100644 --- a/mapviz_plugins/src/image_plugin.cpp +++ b/mapviz_plugins/src/image_plugin.cpp @@ -206,17 +206,14 @@ namespace mapviz_plugins } else { - image_transport::ImageTransport it(local_node_); - image_sub_ = it.subscribe(topic_, 1, &ImagePlugin::imageCallback, this); - - ROS_INFO("Subscribing to %s", topic_.c_str()); + Resubscribe(); } } void ImagePlugin::SetTransport(const QString& transport) { - ROS_INFO("Changing image_transport to %s.", transport.toStdString().c_str()); - transport_ = transport; + transport_ = transport.toStdString(); + ROS_INFO("Changing image_transport to %s.", transport_.c_str()); TopicEdited(); } @@ -231,7 +228,7 @@ namespace mapviz_plugins void ImagePlugin::Resubscribe() { - if (transport_ == QString::fromStdString("default")) + if (transport_ == "default") { force_resubscribe_ = true; TopicEdited(); @@ -275,7 +272,7 @@ namespace mapviz_plugins // have changed. if (force_resubscribe_ || topic != topic_ || - image_sub_.getTransport() != transport_.toStdString()) + image_sub_.getTransport() != transport_) { force_resubscribe_ = false; initialized_ = false; @@ -288,23 +285,23 @@ namespace mapviz_plugins if (!topic_.empty()) { boost::shared_ptr it; - if (transport_ == QString::fromStdString("default")) + if (transport_ == "default") { ROS_DEBUG("Using default transport."); - it = boost::make_shared(node_); - image_sub_ = it->subscribe(topic_, 1, &ImagePlugin::imageCallback, this); + image_transport::ImageTransport it(node_); + image_sub_ = it.subscribe(topic_, 1, &ImagePlugin::imageCallback, this); } else { ROS_DEBUG("Setting transport to %s on %s.", - transport_.toStdString().c_str(), local_node_.getNamespace().c_str()); - - local_node_.setParam("image_transport", transport_.toStdString()); - it = boost::make_shared(local_node_); - image_sub_ = it->subscribe(topic_, 1, &ImagePlugin::imageCallback, this, - image_transport::TransportHints(transport_.toStdString(), - ros::TransportHints(), - local_node_)); + transport_.c_str(), local_node_.getNamespace().c_str()); + + local_node_.setParam("image_transport", transport_); + image_transport::ImageTransport it(local_node_); + image_sub_ = it.subscribe(topic_, 1, &ImagePlugin::imageCallback, this, + image_transport::TransportHints(transport_, + ros::TransportHints(), + local_node_)); } ROS_INFO("Subscribing to %s", topic_.c_str()); @@ -518,10 +515,8 @@ namespace mapviz_plugins // subscribe. if (node["image_transport"]) { - std::string transport; - node["image_transport"] >> transport; - transport_ = QString::fromStdString(transport); - int index = ui_.transport_combo_box->findText(transport_); + node["image_transport"] >> transport_; + int index = ui_.transport_combo_box->findText( QString::fromStdString(transport_) ); if (index != -1) { ui_.transport_combo_box->setCurrentIndex(index); @@ -529,7 +524,7 @@ namespace mapviz_plugins else { ROS_WARN("Saved image transport %s is unavailable.", - transport_.toStdString().c_str()); + transport_.c_str()); } } @@ -599,7 +594,7 @@ namespace mapviz_plugins 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_.toStdString(); + emitter << YAML::Key << "image_transport" << YAML::Value << transport_; } std::string ImagePlugin::AnchorToString(Anchor anchor) diff --git a/mapviz_plugins/src/laserscan_plugin.cpp b/mapviz_plugins/src/laserscan_plugin.cpp index 53326fadb..e19ce516b 100644 --- a/mapviz_plugins/src/laserscan_plugin.cpp +++ b/mapviz_plugins/src/laserscan_plugin.cpp @@ -147,6 +147,10 @@ namespace mapviz_plugins SIGNAL(colorEdited(const QColor &)), this, SLOT(DrawIcon())); + QObject::connect(this, + SIGNAL(TargetFrameChanged(const std::string&)), + this, + SLOT(ResetTransformedScans())); PrintInfo("Constructed LaserScanPlugin"); } @@ -194,6 +198,14 @@ namespace mapviz_plugins } } + void LaserScanPlugin::ResetTransformedScans() + { + for (Scan& scan: scans_) + { + scan.transformed = false; + } + } + QColor LaserScanPlugin::CalculateColor(const StampedPoint& point, bool has_intensity) { @@ -347,6 +359,23 @@ namespace mapviz_plugins } } + 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::LaserScanConstPtr& msg) { if (!has_message_) @@ -364,21 +393,15 @@ namespace mapviz_plugins 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; scan.has_intensity = !msg->intensities.empty(); - scan.points.reserve( msg->ranges.size() ); - swri_transform_util::Transform transform; - if (!GetTransform(scan.source_frame_, msg->header.stamp, transform)) - { - scan.transformed = false; - PrintError("No transform between " + source_frame_ + " and " + target_frame_); - } 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 @@ -393,6 +416,7 @@ namespace mapviz_plugins point.range = msg->ranges[i]; if (i < msg->intensities.size()) point.intensity = msg->intensities[i]; + if (scan.transformed) { point.transformed_point = transform * point.point; @@ -500,24 +524,23 @@ namespace mapviz_plugins { Scan& scan = *scan_it; - swri_transform_util::Transform transform; - - bool was_using_latest_transforms = this->use_latest_transforms_; - this->use_latest_transforms_ = false; - if (GetTransform(scan.source_frame_, scan.stamp, transform)) + if( !scan.transformed ) { - scan.transformed = true; - std::vector::iterator point_it = scan.points.begin(); - for (; point_it != scan.points.end(); ++point_it) - { - point_it->transformed_point = transform * point_it->point; - } - } - else - { - scan.transformed = false; + swri_transform_util::Transform transform; + + if ( GetScanTransform( scan, transform) ) + { + scan.transformed = true; + std::vector::iterator point_it = scan.points.begin(); + for (; point_it != scan.points.end(); ++point_it) + { + point_it->transformed_point = transform * point_it->point; + } + } + else{ + PrintError("No transform between " + scan.source_frame_ + " and " + target_frame_); + } } - this->use_latest_transforms_ = was_using_latest_transforms; } // Z color is based on transformed color, so it is dependent on the // transform diff --git a/mapviz_plugins/src/navsat_plugin.cpp b/mapviz_plugins/src/navsat_plugin.cpp index b16669de6..fa66a17ea 100644 --- a/mapviz_plugins/src/navsat_plugin.cpp +++ b/mapviz_plugins/src/navsat_plugin.cpp @@ -72,6 +72,8 @@ namespace mapviz_plugins 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())); } NavSatPlugin::~NavSatPlugin() @@ -96,7 +98,7 @@ namespace mapviz_plugins if (topic != topic_) { initialized_ = false; - points_.clear(); + ClearPoints(); has_message_ = false; PrintWarning("No messages received."); @@ -132,27 +134,10 @@ namespace mapviz_plugins local_xy_util_.ToLocalXy(navsat->latitude, navsat->longitude, x, y); stamped_point.point = tf::Point(x, y, navsat->altitude); - stamped_point.orientation = tf::createQuaternionFromYaw(0.0); - stamped_point.source_frame = local_xy_util_.Frame(); - if (points_.empty() || - stamped_point.point.distance(points_.back().point) >= - position_tolerance_) - { - points_.push_back(stamped_point); - } - - if (buffer_size_ > 0) - { - while (static_cast(points_.size()) > buffer_size_) - { - points_.pop_front(); - } - } - - cur_point_ = stamped_point; + pushPoint( std::move(stamped_point ) ); } void NavSatPlugin::PrintError(const std::string& message) @@ -205,8 +190,9 @@ namespace mapviz_plugins { std::string color; node["color"] >> color; - SetColor(QColor(color.c_str())); - ui_.color->setColor(color_); + QColor qcolor(color.c_str()); + SetColor(qcolor); + ui_.color->setColor(qcolor); } if (node["draw_style"]) @@ -216,26 +202,30 @@ namespace mapviz_plugins if (draw_style == "lines") { - draw_style_ = LINES; ui_.drawstyle->setCurrentIndex(0); + SetDrawStyle( LINES ); } else if (draw_style == "points") { - draw_style_ = POINTS; ui_.drawstyle->setCurrentIndex(1); + SetDrawStyle( POINTS ); } } if (node["position_tolerance"]) { - node["position_tolerance"] >> position_tolerance_; - ui_.positiontolerance->setValue(position_tolerance_); + double position_tolerance; + node["position_tolerance"] >> position_tolerance; + ui_.positiontolerance->setValue(position_tolerance); + PositionToleranceChanged(position_tolerance); } if (node["buffer_size"]) { - node["buffer_size"] >> buffer_size_; - ui_.buffersize->setValue(buffer_size_); + double buffer_size; + node["buffer_size"] >> buffer_size; + ui_.buffersize->setValue(buffer_size); + BufferSizeChanged(buffer_size); } TopicEdited(); @@ -253,8 +243,8 @@ namespace mapviz_plugins emitter << YAML::Key << "draw_style" << YAML::Value << draw_style; emitter << YAML::Key << "position_tolerance" << - YAML::Value << position_tolerance_; + YAML::Value << positionTolerance(); - emitter << YAML::Key << "buffer_size" << YAML::Value << buffer_size_; + emitter << YAML::Key << "buffer_size" << YAML::Value << bufferSize(); } } diff --git a/mapviz_plugins/src/occupancy_grid_plugin.cpp b/mapviz_plugins/src/occupancy_grid_plugin.cpp new file mode 100644 index 000000000..d0c78a5c6 --- /dev/null +++ b/mapviz_plugins/src/occupancy_grid_plugin.cpp @@ -0,0 +1,533 @@ +// ***************************************************************************** +// +// 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 +#include + +// C++ standard libraries +#include +#include + +// QT libraries +#include +#include + +#include + +// Declare plugin +#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() : + config_widget_(new QWidget()), + transformed_(false), + texture_id_(0), + map_palette_( makeMapPalette() ), + costmap_palette_( makeCostmapPalette() ) + { + 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 + 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 &))); + + PrintWarning("waiting for first message"); + } + + OccupancyGridPlugin::~OccupancyGridPlugin() + { + Shutdown(); + } + + void OccupancyGridPlugin::Shutdown() + { + } + + 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() + { + ros::master::TopicInfo topic = mapviz::SelectTopicDialog::selectTopic("nav_msgs/OccupancyGrid"); + if (!topic.name.empty()) + { + QString str = QString::fromStdString(topic.name); + 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_.shutdown(); + update_sub_.shutdown(); + + if (!topic.empty()) + { + grid_sub_ = node_.subscribe(topic, 10, &OccupancyGridPlugin::Callback, this); + if( ui_.checkbox_update) + { + update_sub_ = node_.subscribe(topic+ "_updates", 10, &OccupancyGridPlugin::CallbackUpdate, this); + } + ROS_INFO("Subscribing to %s", topic.c_str()); + } + } + + void OccupancyGridPlugin::upgradeCheckBoxToggled(bool) + { + const std::string topic = ui_.topic_grid->text().trimmed().toStdString(); + update_sub_.shutdown(); + + if( ui_.checkbox_update) + { + update_sub_ = node_.subscribe(topic+ "_updates", 10, &OccupancyGridPlugin::CallbackUpdate, this); + } + } + + void OccupancyGridPlugin::colorSchemeUpdated(const QString &) + { + + if( grid_ && raw_buffer_.size()>0) + { + 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); + } + + + void OccupancyGridPlugin::Callback(const nav_msgs::OccupancyGridConstPtr& 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::OccupancyGridUpdateConstPtr &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; + + tfScalar yaw, pitch, roll; + tf::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_, ros::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"] >> topic; + ui_.topic_grid->setText(QString::fromStdString(topic)); + } + + if (node["update"]) + { + bool checked; + node["update"] >> checked; + ui_.checkbox_update->setChecked( checked ); + } + + if (node["alpha"]) + { + double alpha; + node["alpha"] >> alpha; + ui_.alpha->setValue(alpha); + } + + 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(); + } +} + diff --git a/mapviz_plugins/src/odometry_plugin.cpp b/mapviz_plugins/src/odometry_plugin.cpp index d191644f5..ac237ccf6 100644 --- a/mapviz_plugins/src/odometry_plugin.cpp +++ b/mapviz_plugins/src/odometry_plugin.cpp @@ -57,7 +57,6 @@ namespace mapviz_plugins OdometryPlugin::OdometryPlugin() : config_widget_(new QWidget()) { ui_.setupUi(config_widget_); - covariance_checked_ = ui_.show_covariance->isChecked(); ui_.color->setColor(Qt::green); // Set background white @@ -70,8 +69,6 @@ namespace mapviz_plugins p3.setColor(QPalette::Text, Qt::red); ui_.status->setPalette(p3); - QObject::connect(ui_.show_timestamps, SIGNAL(valueChanged(int)), this, - SLOT(TopicEditor())); QObject::connect(ui_.selecttopic, SIGNAL(clicked()), this, SLOT(SelectTopic())); QObject::connect(ui_.topic, SIGNAL(editingFinished()), this, @@ -86,8 +83,14 @@ namespace mapviz_plugins this, SLOT(SetStaticArrowSizes(bool))); QObject::connect(ui_.arrow_size, SIGNAL(valueChanged(int)), this, SLOT(SetArrowSize(int))); - connect(ui_.color, SIGNAL(colorEdited(const QColor&)), this, - SLOT(SetColor(const QColor&))); + 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_.buttonResetBuffer, SIGNAL(pressed()), this, + SLOT(ClearPoints())); } OdometryPlugin::~OdometryPlugin() @@ -112,7 +115,7 @@ namespace mapviz_plugins if (topic != topic_) { initialized_ = false; - points_.clear(); + ClearPoints(); has_message_ = false; PrintWarning("No messages received."); @@ -156,26 +159,7 @@ namespace mapviz_plugins odometry->pose.pose.orientation.z, odometry->pose.pose.orientation.w); - if (points_.empty() || - stamped_point.point.distance(points_.back().point) >= - position_tolerance_) - { - points_.push_back(stamped_point); - } - - if (buffer_size_ > 0) - { - while (static_cast(points_.size()) > buffer_size_) - { - points_.pop_front(); - } - } - - cur_point_ = stamped_point; - covariance_checked_ = ui_.show_covariance->isChecked(); - lap_checked_ = ui_.show_laps->isChecked(); - - if (covariance_checked_) + if ( ui_.show_covariance->isChecked() ) { tf::Matrix3x3 tf_cov = swri_transform_util::GetUpperLeft(odometry->pose.covariance); @@ -191,15 +175,14 @@ namespace mapviz_plugins } } - cv::Mat cov_matrix_2d = - swri_image_util::ProjectEllipsoid(cov_matrix_3d); + cv::Mat cov_matrix_2d = swri_image_util::ProjectEllipsoid(cov_matrix_3d); if (!cov_matrix_2d.empty()) { - cur_point_.cov_points = swri_image_util::GetEllipsePoints( - cov_matrix_2d, cur_point_.point, 3, 32); + stamped_point.cov_points = swri_image_util::GetEllipsePoints( + cov_matrix_2d, stamped_point.point, 3, 32); - cur_point_.transformed_cov_points = cur_point_.cov_points; + stamped_point.transformed_cov_points = stamped_point.cov_points; } else { @@ -207,6 +190,9 @@ namespace mapviz_plugins } } } + + pushPoint( std::move(stamped_point) ); + } void OdometryPlugin::PrintError(const std::string& message) @@ -271,17 +257,16 @@ namespace mapviz_plugins QPen pen(QBrush(ui_.color->color()), 1); painter->setPen(pen); - std::list::iterator it = points_.begin(); int counter = 0;//used to alternate between rendering text on some points - for (; it != points_.end(); ++it) + for (const StampedPoint& point: points()) { - if (it->transformed && counter % interval == 0)//this renders a timestamp every 'interval' points + if (point.transformed && counter % interval == 0)//this renders a timestamp every 'interval' points { - QPointF point = tf.map(QPointF(it->transformed_point.getX(), - it->transformed_point.getY())); + QPointF qpoint = tf.map(QPointF(point.transformed_point.getX(), + point.transformed_point.getY())); QString time; - time.setNum(it->stamp.toSec(), 'g', 12); - painter->drawText(point, time); + time.setNum(point.stamp.toSec(), 'g', 12); + painter->drawText(qpoint, time); } counter++; } @@ -289,29 +274,6 @@ namespace mapviz_plugins painter->restore(); } - void OdometryPlugin::DrawCovariance() - { - glLineWidth(4); - - glColor4d(color_.redF(), color_.greenF(), color_.blueF(), 1.0); - - if (cur_point_.transformed && !cur_point_.transformed_cov_points.empty()) - { - glBegin(GL_LINE_STRIP); - - for (uint32_t i = 0; i < cur_point_.transformed_cov_points.size(); i++) - { - glVertex2d(cur_point_.transformed_cov_points[i].getX(), - cur_point_.transformed_cov_points[i].getY()); - } - - glVertex2d(cur_point_.transformed_cov_points.front().getX(), - cur_point_.transformed_cov_points.front().getY()); - - glEnd(); - } - } - void OdometryPlugin::LoadConfig(const YAML::Node& node, const std::string& path) { @@ -326,8 +288,9 @@ namespace mapviz_plugins { std::string color; node["color"] >> color; - SetColor(QColor(color.c_str())); - ui_.color->setColor(color_); + QColor qcolor(color.c_str()); + SetColor(qcolor); + ui_.color->setColor(qcolor); } if (node["draw_style"]) @@ -337,31 +300,35 @@ namespace mapviz_plugins if (draw_style == "lines") { - draw_style_ = LINES; ui_.drawstyle->setCurrentIndex(0); + SetDrawStyle( LINES ); } else if (draw_style == "points") { - draw_style_ = POINTS; ui_.drawstyle->setCurrentIndex(1); + SetDrawStyle( POINTS ); } else if (draw_style == "arrows") { - draw_style_ = ARROWS; ui_.drawstyle->setCurrentIndex(2); + SetDrawStyle( ARROWS ); } } if (node["position_tolerance"]) { - node["position_tolerance"] >> position_tolerance_; - ui_.positiontolerance->setValue(position_tolerance_); + double position_tolerance; + node["position_tolerance"] >> position_tolerance; + ui_.positiontolerance->setValue(position_tolerance); + PositionToleranceChanged(position_tolerance); } if (node["buffer_size"]) { - node["buffer_size"] >> buffer_size_; - ui_.buffersize->setValue(buffer_size_); + double buffer_size; + node["buffer_size"] >> buffer_size; + ui_.buffersize->setValue(buffer_size); + BufferSizeChanged(buffer_size); } if (node["show_covariance"]) @@ -369,12 +336,14 @@ namespace mapviz_plugins bool show_covariance = false; node["show_covariance"] >> show_covariance; ui_.show_covariance->setChecked(show_covariance); + CovariancedToggled(show_covariance); } if (node["show_laps"]) { bool show_laps = false; node["show_laps"] >> show_laps; ui_.show_laps->setChecked(show_laps); + LapToggled(show_laps); } if (node["static_arrow_sizes"]) @@ -386,7 +355,9 @@ namespace mapviz_plugins if (node["arrow_size"]) { - ui_.arrow_size->setValue(node["arrow_size"].as()); + double arrow_size = node["arrow_size"].as(); + ui_.arrow_size->setValue(arrow_size); + SetArrowSize(arrow_size); } if (node["show_timestamps"]) @@ -410,15 +381,10 @@ namespace mapviz_plugins emitter << YAML::Key << "draw_style" << YAML::Value << draw_style; emitter << YAML::Key << "position_tolerance" << - YAML::Value << position_tolerance_; - if (!lap_checked_) - { - emitter << YAML::Key << "buffer_size" << YAML::Value << buffer_size_; - } - else - { - emitter << YAML::Key << "buffer_size" << YAML::Value << buffer_holder_; - } + 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; @@ -429,7 +395,7 @@ namespace mapviz_plugins emitter << YAML::Key << "arrow_size" << YAML::Value << ui_.arrow_size->value(); - emitter << YAML::Key << "show_timestamps" << YAML::Value << ui_.show_timestamps->value(); + emitter << YAML::Key << "tampstamps" << YAML::Value << ui_.show_timestamps->value(); } } diff --git a/mapviz_plugins/src/path_plugin.cpp b/mapviz_plugins/src/path_plugin.cpp index e011b3922..0774f9624 100644 --- a/mapviz_plugins/src/path_plugin.cpp +++ b/mapviz_plugins/src/path_plugin.cpp @@ -67,6 +67,7 @@ namespace mapviz_plugins connect(ui_.topic, SIGNAL(editingFinished()), this, SLOT(TopicEdited())); connect(ui_.path_color, SIGNAL(colorEdited(const QColor&)), this, SLOT(SetColor(const QColor&))); + } PathPlugin::~PathPlugin() @@ -91,7 +92,7 @@ namespace mapviz_plugins if (topic != topic_) { initialized_ = false; - points_.clear(); + ClearPoints(); has_message_ = false; PrintWarning("No messages received."); @@ -115,7 +116,7 @@ namespace mapviz_plugins has_message_ = true; } - points_.clear(); + ClearPoints(); for (unsigned int i = 0; i < path->poses.size(); i++) { @@ -125,7 +126,7 @@ namespace mapviz_plugins stamped_point.point = tf::Point(path->poses[i].pose.position.x, path->poses[i].pose.position.y, 0); - points_.push_back(stamped_point); + pushPoint( std::move(stamped_point) ); } } @@ -163,12 +164,13 @@ namespace mapviz_plugins bool lines; bool points; QColor old_color = ui_.path_color->color(); - draw_style_ = LINES; + QColor color = old_color.dark(200); + SetDrawStyle( LINES ); lines = DrawPoints(scale); - color_ = color_.dark(200); - draw_style_ = POINTS; + SetColor(color); + SetDrawStyle( POINTS ); points = DrawPoints(scale); - color_ = old_color; + SetColor(old_color); if (lines && points) { PrintInfo("OK"); @@ -189,8 +191,9 @@ namespace mapviz_plugins { std::string color; node["color"] >> color; - SetColor(QColor(color.c_str())); - ui_.path_color->setColor(color_); + QColor qcolor(color.c_str()); + SetColor(qcolor); + ui_.path_color->setColor(qcolor); } } diff --git a/mapviz_plugins/src/point_click_publisher_plugin.cpp b/mapviz_plugins/src/point_click_publisher_plugin.cpp index 06c9242ec..ce7a9258f 100644 --- a/mapviz_plugins/src/point_click_publisher_plugin.cpp +++ b/mapviz_plugins/src/point_click_publisher_plugin.cpp @@ -108,7 +108,7 @@ namespace mapviz_plugins } - void PointClickPublisherPlugin::pointClicked(const QPointF& point) + void PointClickPublisherPlugin::pointClicked(const QPointF& point, const Qt::MouseButton& button) { QPointF transformed = canvas_->MapGlCoordToFixedFrame(point); diff --git a/mapviz_plugins/src/point_drawing_plugin.cpp b/mapviz_plugins/src/point_drawing_plugin.cpp index b0c199dc8..ac7eba4b1 100644 --- a/mapviz_plugins/src/point_drawing_plugin.cpp +++ b/mapviz_plugins/src/point_drawing_plugin.cpp @@ -57,6 +57,10 @@ namespace mapviz_plugins static_arrow_sizes_(false), got_begin_(false) { + QObject::connect(this, + SIGNAL(TargetFrameChanged(const std::string&)), + this, + SLOT(ResetTransformedPoints())); } void PointDrawingPlugin::ClearHistory() @@ -107,6 +111,7 @@ namespace mapviz_plugins void PointDrawingPlugin::SetArrowSize(int arrowSize) { arrow_size_ = arrowSize; + ResetTransformedPoints(); } void PointDrawingPlugin::SetDrawStyle(QString style) @@ -123,13 +128,20 @@ namespace mapviz_plugins { 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) @@ -137,13 +149,86 @@ namespace mapviz_plugins position_tolerance_ = value; } + void PointDrawingPlugin::LapToggled(bool checked) + { + lap_checked_ = checked; + } + + void PointDrawingPlugin::CovariancedToggled(bool checked) + { + covariance_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(PointDrawingPlugin::StampedPoint stamped_point) + { + cur_point_ = stamped_point; + + if (points_.empty() || + (stamped_point.point.distance(points_.back().point)) >= + (position_tolerance_)) + { + points_.push_back(stamped_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_) + while (static_cast(points_.size()) >= buffer_size_) { points_.pop_front(); } @@ -152,6 +237,10 @@ namespace mapviz_plugins bool PointDrawingPlugin::DrawPoints(double scale) { + if( scale_ != scale && draw_style_ == ARROWS && static_arrow_sizes_) + { + ResetTransformedPoints(); + } scale_ = scale; bool transformed = true; if (lap_checked_) @@ -220,7 +309,7 @@ namespace mapviz_plugins { bool success = cur_point_.transformed; glColor4d(color_.redF(), color_.greenF(), color_.blueF(), 1.0); - if (draw_style_ == LINES) + if (draw_style_ == LINES && points_.size()>0) { glLineWidth(3); glBegin(GL_LINE_STRIP); @@ -231,13 +320,12 @@ namespace mapviz_plugins glBegin(GL_POINTS); } - std::list::iterator it = points_.begin(); - for (; it != points_.end(); ++it) + for (const StampedPoint& point: points_) { - success &= it->transformed; - if (it->transformed) + success &= point.transformed; + if (point.transformed) { - glVertex2d(it->transformed_point.getX(), it->transformed_point.getY()); + glVertex2d(point.transformed_point.getX(), point.transformed_point.getY()); } } @@ -279,13 +367,12 @@ namespace mapviz_plugins bool PointDrawingPlugin::DrawArrows() { bool success = true; - glLineWidth(2); + glLineWidth(4); glBegin(GL_LINES); glColor4d(color_.redF(), color_.greenF(), color_.blueF(), 0.5); - std::list::iterator it = points_.begin(); - for (; it != points_.end(); ++it) + for (const StampedPoint& point: points_) { - success &= DrawArrow(*it); + success &= DrawArrow(point); } success &= DrawArrow(cur_point_); @@ -306,32 +393,40 @@ namespace mapviz_plugins bool PointDrawingPlugin::TransformPoint(StampedPoint& point) { + if ( point.transformed ) + { + return true; + } + swri_transform_util::Transform transform; - if (GetTransform(point.source_frame, point.stamp, transform)) + if( GetTransform(point.source_frame, point.stamp, transform)) { point.transformed_point = transform * point.point; - tf::Transform orientation(tf::Transform(transform.GetOrientation()) * - point.orientation); - - double size = static_cast(arrow_size_); - if (static_arrow_sizes_) - { - size *= scale_; - } - else + if (draw_style_ == ARROWS) { - size /= 10.0; - } - double arrow_width = size / 5.0; - double head_length = size * 0.75; + tf::Transform orientation(tf::Transform(transform.GetOrientation()) * + point.orientation); - point.transformed_arrow_point = - point.transformed_point + orientation * tf::Point(size, 0.0, 0.0); - point.transformed_arrow_left = - point.transformed_point + orientation * tf::Point(head_length, -arrow_width, 0.0); - point.transformed_arrow_right = - point.transformed_point + orientation * tf::Point(head_length, arrow_width, 0.0); + 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; + + point.transformed_arrow_point = + point.transformed_point + orientation * tf::Point(size, 0.0, 0.0); + point.transformed_arrow_left = + point.transformed_point + orientation * tf::Point(head_length, -arrow_width, 0.0); + point.transformed_arrow_right = + point.transformed_point + orientation * tf::Point(head_length, arrow_width, 0.0); + } if (covariance_checked_) { @@ -340,11 +435,9 @@ namespace mapviz_plugins point.transformed_cov_points[i] = transform * point.cov_points[i]; } } - point.transformed = true; return true; } - point.transformed = false; return false; } @@ -353,21 +446,19 @@ namespace mapviz_plugins { bool transformed = false; - std::list::iterator points_it = points_.begin(); - for (; points_it != points_.end(); ++points_it) + for (StampedPoint& point: points_) { - transformed = transformed | TransformPoint(*points_it); + transformed = transformed | TransformPoint(point); } transformed = transformed | TransformPoint(cur_point_); if (laps_.size() > 0) { - for (size_t i = 0; i < laps_.size(); i++) + for (std::deque& lap: laps_) { - std::list::iterator lap_it = laps_[i].begin(); - for (; lap_it != laps_[i].end(); ++lap_it) + for (StampedPoint& point: lap) { - transformed = transformed | TransformPoint(*lap_it); + transformed = transformed | TransformPoint(point); } } } @@ -384,33 +475,30 @@ namespace mapviz_plugins glColor4d(color_.redF(), color_.greenF(), color_.blueF(), 0.5); glLineWidth(3); QColor base_color = color_; - if (laps_.size() != 0) + + for (size_t i = 0; i < laps_.size(); i++) { - for (size_t i = 0; i < laps_.size(); i++) + UpdateColor(base_color, static_cast(i)); + if (draw_style_ == LINES) { - UpdateColor(base_color, static_cast(i)); - if (draw_style_ == LINES) - { - glLineWidth(3); - glBegin(GL_LINE_STRIP); - } - else - { - glPointSize(6); - glBegin(GL_POINTS); - } + glLineWidth(3); + glBegin(GL_LINE_STRIP); + } + else + { + glPointSize(6); + glBegin(GL_POINTS); + } - std::list::iterator it = laps_[i].begin(); - for (; it != laps_[i].end(); it++) + for (const StampedPoint& point: laps_[i]) + { + if (point.transformed) { - if (it->transformed) - { - glVertex2d(it->transformed_point.getX(), - it->transformed_point.getY()); - } + glVertex2d(point.transformed_point.getX(), + point.transformed_point.getY()); } - glEnd(); } + glEnd(); } if (draw_style_ == LINES) @@ -428,14 +516,13 @@ namespace mapviz_plugins if (points_.size() > 0) { - std::list::iterator it = points_.begin(); - for (; it != points_.end(); ++it) + for (const StampedPoint& point: points_) { - transformed &= it->transformed; - if (it->transformed) + transformed &= point.transformed; + if (point.transformed) { - glVertex2d(it->transformed_point.getX(), - it->transformed_point.getY()); + glVertex2d(point.transformed_point.getX(), + point.transformed_point.getY()); } } } @@ -458,6 +545,29 @@ namespace mapviz_plugins 0.5); } + void PointDrawingPlugin::DrawCovariance() + { + glLineWidth(4); + + glColor4d(color_.redF(), color_.greenF(), color_.blueF(), 1.0); + + if (cur_point_.transformed && !cur_point_.transformed_cov_points.empty()) + { + glBegin(GL_LINE_STRIP); + + for (uint32_t i = 0; i < cur_point_.transformed_cov_points.size(); i++) + { + glVertex2d(cur_point_.transformed_cov_points[i].getX(), + cur_point_.transformed_cov_points[i].getY()); + } + + glVertex2d(cur_point_.transformed_cov_points.front().getX(), + cur_point_.transformed_cov_points.front().getY()); + + glEnd(); + } + } + bool PointDrawingPlugin::DrawLapsArrows() { bool success = laps_.size() != 0 && points_.size() != 0; @@ -469,11 +579,10 @@ namespace mapviz_plugins for (size_t i = 0; i < laps_.size(); i++) { UpdateColor(base_color, static_cast(i)); - std::list::iterator it = laps_[i].begin(); - for (; it != laps_[i].end(); ++it) + for (const StampedPoint& point: laps_[i]) { glBegin(GL_LINE_STRIP); - success &= DrawArrow(*it); + success &= DrawArrow(point); glEnd(); } } @@ -489,11 +598,10 @@ namespace mapviz_plugins if (points_.size() > 0) { - std::list::iterator it = points_.begin(); - for (; it != points_.end(); ++it) + for (const StampedPoint& point: points_) { glBegin(GL_LINE_STRIP); - success &= DrawArrow(*it); + success &= DrawArrow(point); glEnd(); } } diff --git a/mapviz_plugins/src/pointcloud2_plugin.cpp b/mapviz_plugins/src/pointcloud2_plugin.cpp index b3e597f46..867b41277 100644 --- a/mapviz_plugins/src/pointcloud2_plugin.cpp +++ b/mapviz_plugins/src/pointcloud2_plugin.cpp @@ -27,6 +27,7 @@ // // ***************************************************************************** +#include #include // C++ standard libraries @@ -97,6 +98,10 @@ namespace mapviz_plugins SIGNAL(clicked()), this, SLOT(SelectTopic())); + QObject::connect(ui_.buttonResetBuffer, + SIGNAL(clicked()), + this, + SLOT(ClearPointClouds())); QObject::connect(ui_.topic, SIGNAL(editingFinished()), this, @@ -110,13 +115,11 @@ namespace mapviz_plugins this, SLOT(ColorTransformerChanged(int))); QObject::connect(ui_.max_color, - SIGNAL(colorEdited( - const QColor &)), + SIGNAL(colorEdited(const QColor &)), this, SLOT(UpdateColors())); QObject::connect(ui_.min_color, - SIGNAL(colorEdited( - const QColor &)), + SIGNAL(colorEdited(const QColor &)), this, SLOT(UpdateColors())); QObject::connect(ui_.minValue, @@ -148,19 +151,21 @@ namespace mapviz_plugins this, SLOT(UseAutomaxminChanged(int))); QObject::connect(ui_.max_color, - SIGNAL(colorEdited( - const QColor &)), + SIGNAL(colorEdited(const QColor &)), this, SLOT(DrawIcon())); QObject::connect(ui_.min_color, - SIGNAL(colorEdited( - const QColor &)), + 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))); PrintInfo("Constructed PointCloud2Plugin"); } @@ -210,10 +215,32 @@ namespace mapviz_plugins void PointCloud2Plugin::ResetTransformedPointClouds() { - std::deque::iterator scan_it = scans_.begin(); - for (; scan_it != scans_.end(); ++scan_it) + 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_.shutdown(); + + if (subscribe && !topic_.empty()) { - scan_it->transformed = false; + pc2_sub_ = node_.subscribe(topic_, 10, &PointCloud2Plugin::PointCloud2Callback, this); + new_topic_ = true; + need_new_list_ = true; + max_.clear(); + min_.clear(); } } @@ -297,13 +324,17 @@ namespace mapviz_plugins { { QMutexLocker locker(&scan_mutex_); - std::deque::iterator scan_it = scans_.begin(); - for (; scan_it != scans_.end(); ++scan_it) + for (Scan& scan: scans_) { - std::vector::iterator point_it = scan_it->points.begin(); - for (; point_it != scan_it->points.end(); point_it++) + scan.gl_color.clear(); + scan.gl_color.reserve(scan.points.size()*4); + for (const StampedPoint& point: scan.points) { - point_it->color = CalculateColor(*point_it); + 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 ) ); } } } @@ -336,21 +367,8 @@ namespace mapviz_plugins has_message_ = false; PrintWarning("No messages received."); - pc2_sub_.shutdown(); - topic_ = topic; - if (!topic.empty()) - { - pc2_sub_ = node_.subscribe(topic_, - 100, - &PointCloud2Plugin::PointCloud2Callback, - this); - new_topic_ = true; - need_new_list_ = true; - max_.clear(); - min_.clear(); - ROS_INFO("Subscribing to %s", topic_.c_str()); - } + SetSubscription(this->Visible()); } } @@ -412,6 +430,10 @@ namespace mapviz_plugins { scan = std::move( scans_.front() ); } + else{ + glGenBuffers(1, &scan.color_vbo); + glGenBuffers(1, &scan.point_vbo); + } while (scans_.size() >= buffer_size_) { scans_.pop_front(); @@ -511,6 +533,11 @@ namespace mapviz_plugins field_infos.push_back(it->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); @@ -528,9 +555,15 @@ namespace mapviz_plugins } if (scan.transformed) { - point.transformed_point = transform * point.point; + const tf::Point transformed_point = transform * point.point; + scan.gl_point.push_back( transformed_point.getX() ); + scan.gl_point.push_back( transformed_point.getY() ); } - point.color = CalculateColor(point); + 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 ) ); } } @@ -602,33 +635,32 @@ namespace mapviz_plugins void PointCloud2Plugin::Draw(double x, double y, double scale) { glPointSize(point_size_); - glBegin(GL_POINTS); - std::deque::const_iterator scan_it; - std::vector::const_iterator point_it; + glEnableClientState(GL_VERTEX_ARRAY); + glEnableClientState(GL_COLOR_ARRAY); + { QMutexLocker locker(&scan_mutex_); - for (scan_it = scans_.begin(); scan_it != scans_.end(); scan_it++) + for (Scan& scan: scans_) { - if (scan_it->transformed) + if (scan.transformed) { - for (point_it = scan_it->points.begin(); point_it != scan_it->points.end(); ++point_it) - { - glColor4d( - point_it->color.redF(), - point_it->color.greenF(), - point_it->color.blueF(), - alpha_); - glVertex2d( - point_it->transformed_point.getX(), - point_it->transformed_point.getY()); - } + 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, 0); + + 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, 0); + + glDrawArrays(GL_POINTS, 0, scan.gl_point.size() / 2 ); } } } - - glEnd(); + glDisableClientState(GL_VERTEX_ARRAY); + glDisableClientState(GL_COLOR_ARRAY); + glBindBuffer(GL_ARRAY_BUFFER, 0); PrintInfo("OK"); } @@ -657,23 +689,24 @@ namespace mapviz_plugins { QMutexLocker locker(&scan_mutex_); - std::deque::iterator scan_it = scans_.begin(); bool was_using_latest_transforms = use_latest_transforms_; use_latest_transforms_ = false; - for (; scan_it != scans_.end(); ++scan_it) + for (Scan& scan: scans_) { - Scan& scan = *scan_it; - - if (!scan_it->transformed) + 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; - std::vector::iterator point_it = scan.points.begin(); - for (; point_it != scan.points.end(); ++point_it) + for (StampedPoint& point: scan.points) { - point_it->transformed_point = transform * point_it->point; + const tf::Point transformed_point = transform * point.point; + scan.gl_point.push_back( transformed_point.getX() ); + scan.gl_point.push_back( transformed_point.getY() ); } } else diff --git a/mapviz_plugins/src/right_click_services_plugin.cpp b/mapviz_plugins/src/right_click_services_plugin.cpp new file mode 100644 index 000000000..a5146da37 --- /dev/null +++ b/mapviz_plugins/src/right_click_services_plugin.cpp @@ -0,0 +1,342 @@ +// ***************************************************************************** +// +// 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. +// +// ***************************************************************************** + +// ***************************************************************************** +// handling right click event, calls service which will display a +// number of services that are available on that specific location +// +// ***************************************************************************** +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Declare plugin +#include +PLUGINLIB_EXPORT_CLASS(mapviz_plugins::RightClickServicesPlugin, mapviz::MapvizPlugin) + +namespace mapviz_plugins +{ + RightClickServicesPlugin::RightClickServicesPlugin() : + config_widget_(new QWidget()), + canvas_(NULL) + { + ui_.setupUi(config_widget_); + + connect(&click_filter_, SIGNAL(pointClicked(const QPointF&, const Qt::MouseButton&)), + this, SLOT(pointClicked(const QPointF&, const Qt::MouseButton&))); + connect(ui_.topic, SIGNAL(textEdited(const QString&)), + this, SLOT(topicChanged(const QString&))); + + connect(ui_.availableservicestopic, SIGNAL(textEdited(const QString&)), this, SLOT(availableServiceTopicChanged(const QString&))); + connect(ui_.gpscommandtopic, SIGNAL(textEdited(const QString&)), this, SLOT(gpsCommandTopicChanged(const QString&))); + + + frame_timer_.start(1000); + connect(&frame_timer_, SIGNAL(timeout()), this, SLOT(updateFrames())); + + } + + RightClickServicesPlugin::~RightClickServicesPlugin() + { + if (canvas_) + { + canvas_->removeEventFilter(&click_filter_); + } + } + + bool RightClickServicesPlugin::Initialize(QGLWidget* canvas) + { + canvas_ = static_cast(canvas); + canvas_->installEventFilter(&click_filter_); + + PrintInfo("Ready."); + + + return true; + } + void RightClickServicesPlugin::availableServiceTopicChanged(const QString& topic) + { + std::stringstream ss; + RightClickServicesPlugin::available_service_topic_=topic.toStdString().c_str(); + ss << "switched available service topic to: " << topic.toStdString().c_str(); + PrintInfo(ss.str()); + } + void RightClickServicesPlugin::gpsCommandTopicChanged(const QString& topic) + { + std::stringstream ss; + RightClickServicesPlugin::gps_command_execute_topic_=topic.toStdString().c_str(); + ss << "switched gps command service topic to: " << topic.toStdString().c_str(); + PrintInfo(ss.str()); + } + + void RightClickServicesPlugin::Draw(double x, double y, double scale) + { + } + + void RightClickServicesPlugin::LoadConfig(const YAML::Node& node, const std::string& path) + { + std::string tmp; + if (swri_yaml_util::FindValue(node, "topic")) + { + node["topic"] >> tmp; + ui_.topic->setText(QString(tmp.c_str())); + topicChanged(ui_.topic->text()); + } + + if (swri_yaml_util::FindValue(node, "output_frame")) + { + node["output_frame"] >> tmp; + ui_.outputframe->addItem(QString(tmp.c_str())); + } + if (swri_yaml_util::FindValue(node, "gpscommandtopic")) + { + node["gpscommandtopic"] >> tmp; + ui_.gpscommandtopic->setText(QString(tmp.c_str())); + gpsCommandTopicChanged(ui_.gpscommandtopic->text()); + } + if (swri_yaml_util::FindValue(node, "availableservicestopic")) + { + node["availableservicestopic"] >> tmp; + ui_.availableservicestopic->setText(QString(tmp.c_str())); + availableServiceTopicChanged(ui_.availableservicestopic->text()); + } + + } + + void RightClickServicesPlugin::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(); + emitter << YAML::Key << "availableservicestopic" << YAML::Value << ui_.availableservicestopic->text().toStdString(); + emitter << YAML::Key << "gpscommandtopic" << YAML::Value << ui_.gpscommandtopic->text().toStdString(); + } + + QWidget* RightClickServicesPlugin::GetConfigWidget(QWidget* parent) + { + config_widget_->setParent(parent); + + return config_widget_; + } + + + void RightClickServicesPlugin::pointClicked(const QPointF & point, const Qt::MouseButton& button) + { + QPointF transformed = canvas_->MapGlCoordToFixedFrame(point); + + std::string output_frame = ui_.outputframe->currentText().toStdString(); + + if (target_frame_ != output_frame) + { + swri_transform_util::Transform tf; + tf::Point 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::stringstream ss; + ss << "Point in " << output_frame.c_str() << ": " << transformed.x() << "," << transformed.y(); + PrintInfo(ss.str()); + + + boost::shared_ptr stamped = boost::make_shared(); + stamped->header.frame_id = output_frame; + stamped->header.stamp = ros::Time::now(); + stamped->point.x = transformed.x(); + stamped->point.y = transformed.y(); + stamped->point.z = 0.0; + + point_publisher_.publish(stamped); + + if (button==Qt::RightButton){ + const QPoint pos=point.toPoint(); + + showContextMenu(pos,stamped); + } + } + + void RightClickServicesPlugin::SetNode(const ros::NodeHandle& 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 RightClickServicesPlugin::PrintError(const std::string& message) + { + PrintErrorHelper(ui_.status, message); + } + + void RightClickServicesPlugin::PrintInfo(const std::string& message) + { + PrintInfoHelper(ui_.status, message); + } + + void RightClickServicesPlugin::PrintWarning(const std::string& message) + { + PrintWarningHelper(ui_.status, message); + } + + + void RightClickServicesPlugin::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_.advertise(topic.toStdString(), 1000); + } + } + + void RightClickServicesPlugin::updateFrames() + { + std::vector frames; + tf_->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 (size_t i = 0; i < frames.size(); i++) + { + ui_.outputframe->addItem(frames[i].c_str()); + } + + if (current_output != "") + { + 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); + } + } + void RightClickServicesPlugin::showContextMenu(const QPoint& pos,boost::shared_ptr stamped) + { + // retreive Available Services,Commands + ros::NodeHandle n; + ros::service::waitForService(RightClickServicesPlugin::available_service_topic_,10); + ros::ServiceClient client = n.serviceClient(RightClickServicesPlugin::available_service_topic_); + mapviz_plugins::GPSCommand srv; + srv.request.command = ""; + srv.request.location.longitude =stamped->point.x; + srv.request.location.latitude =stamped->point.y; + std::vector service_name_list; + if(client.exists()) + { + if (client.call(srv)) + { + service_name_list= srv.response.message; + } + } + //call service to execute chosen command + ros::service::waitForService(RightClickServicesPlugin::gps_command_execute_topic_,10); + ros::ServiceClient command_client = n.serviceClient(RightClickServicesPlugin::gps_command_execute_topic_); + std::stringstream ss; + mapviz_plugins::GPSCommand gps_command; + gps_command.request.command = canvas_->showCustomContextMenu(canvas_->mapToGlobal(pos),service_name_list); + gps_command.request.location.longitude =stamped->point.x; + gps_command.request.location.latitude =stamped->point.y; + if (gps_command.request.command.length()<=0) + { + PrintInfo("Empty Service was not called"); + } + else{ + if (command_client.exists()) + { + if(command_client.call(gps_command)) + { + + if(gps_command.response.success) + { + ss << "Calling " << gps_command.request.command << " was successful " << gps_command.response.message[0] ; + }else{ + ss << "Calling " << gps_command.request.command << " was unsuccessful "<< gps_command.response.message[0]; + } + PrintInfo(ss.str()); + } + else + { + ss << "Calling " << gps_command.request.command << " was unsuccessful "<< gps_command.response.message[0]; + } + PrintInfo(ss.str()); + } + } + } + } diff --git a/mapviz_plugins/src/tf_frame_plugin.cpp b/mapviz_plugins/src/tf_frame_plugin.cpp index 047a89a0e..db7b1ddc7 100644 --- a/mapviz_plugins/src/tf_frame_plugin.cpp +++ b/mapviz_plugins/src/tf_frame_plugin.cpp @@ -81,6 +81,8 @@ namespace mapviz_plugins 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())); } TfFramePlugin::~TfFramePlugin() @@ -119,24 +121,7 @@ namespace mapviz_plugins stamped_point.stamp = transform.GetStamp(); stamped_point.transformed = false; - double distance = std::sqrt( - std::pow(stamped_point.point.x() - points_.back().point.x(), 2) + - std::pow(stamped_point.point.y() - points_.back().point.y(), 2)); - - if (points_.empty() || distance >= position_tolerance_) - { - points_.push_back(stamped_point); - } - - if (buffer_size_ > 0) - { - while (static_cast(points_.size()) > buffer_size_) - { - points_.pop_front(); - } - } - - cur_point_ = stamped_point; + pushPoint( std::move(stamped_point) ); } } @@ -194,8 +179,9 @@ namespace mapviz_plugins { std::string color; node["color"] >> color; - SetColor(QColor(color.c_str())); - ui_.color->setColor(color_); + QColor qcolor(color.c_str()); + SetColor(qcolor); + ui_.color->setColor(qcolor); } if (node["draw_style"]) @@ -205,31 +191,35 @@ namespace mapviz_plugins if (draw_style == "lines") { - draw_style_ = LINES; ui_.drawstyle->setCurrentIndex(0); + SetDrawStyle( LINES ); } else if (draw_style == "points") { - draw_style_ = POINTS; ui_.drawstyle->setCurrentIndex(1); + SetDrawStyle( POINTS ); } else if (draw_style == "arrows") { - draw_style_ = ARROWS; ui_.drawstyle->setCurrentIndex(2); + SetDrawStyle( ARROWS ); } } if (node["position_tolerance"]) { - node["position_tolerance"] >> position_tolerance_; - ui_.positiontolerance->setValue(position_tolerance_); + double position_tolerance; + node["position_tolerance"] >> position_tolerance; + ui_.positiontolerance->setValue(position_tolerance); + PositionToleranceChanged(position_tolerance); } if (node["buffer_size"]) { - node["buffer_size"] >> buffer_size_; - ui_.buffersize->setValue(buffer_size_); + double buffer_size; + node["buffer_size"] >> buffer_size; + ui_.buffersize->setValue(buffer_size); + BufferSizeChanged(buffer_size); } if (node["static_arrow_sizes"]) @@ -241,7 +231,9 @@ namespace mapviz_plugins if (node["arrow_size"]) { - ui_.arrow_size->setValue(node["arrow_size"].as()); + int arrow_size = node["arrow_size"].as(); + ui_.arrow_size->setValue(arrow_size); + SetArrowSize(arrow_size); } FrameEdited(); @@ -259,9 +251,9 @@ namespace mapviz_plugins emitter << YAML::Key << "draw_style" << YAML::Value << draw_style; emitter << YAML::Key << "position_tolerance" << - YAML::Value << position_tolerance_; + YAML::Value << positionTolerance(); - emitter << YAML::Key << "buffer_size" << YAML::Value << buffer_size_; + emitter << YAML::Key << "buffer_size" << YAML::Value << bufferSize(); emitter << YAML::Key << "static_arrow_sizes" << YAML::Value << ui_.static_arrow_sizes->isChecked(); diff --git a/mapviz_plugins/srv/GPSCommand.srv b/mapviz_plugins/srv/GPSCommand.srv new file mode 100644 index 000000000..6bbb1ca35 --- /dev/null +++ b/mapviz_plugins/srv/GPSCommand.srv @@ -0,0 +1,5 @@ +sensor_msgs/NavSatFix location +string command +--- +bool success +string[] message diff --git a/mapviz_plugins/ui/gps_config.ui b/mapviz_plugins/ui/gps_config.ui index 150d51bbb..876e6b207 100644 --- a/mapviz_plugins/ui/gps_config.ui +++ b/mapviz_plugins/ui/gps_config.ui @@ -16,15 +16,9 @@ - - - 4 - - - 2 - - - + + + Sans Serif @@ -32,106 +26,43 @@ - Buffer Size: + Topic: - - + + Sans Serif 8 - - Static Arrow Sizes: - - - - - - - - - - - - - - 1 - - - 500 - - - 25 - - - Qt::Horizontal - - - - - - - + + - 24 - 24 + 55 + 16777215 - - false - - - - - - - - - - - - - QAbstractSpinBox::PlusMinus - - - 99999999 - - - - - Sans Serif 8 - - Draw Style: - - - - - - - - Sans Serif - 8 - + + - Position Tolerance: + Select - + @@ -144,15 +75,27 @@ - - + + + + + 24 + 24 + + + + false + + + + - Show Laps + - - + + Sans Serif @@ -160,11 +103,11 @@ - Status: + Draw Style: - + @@ -195,15 +138,48 @@ - - + + + + + Sans Serif + 8 + + - + Static Arrow Sizes: - - + + + + + + + + + + + + + 1 + + + 500 + + + 25 + + + Qt::Horizontal + + + + + + + Sans Serif @@ -211,11 +187,11 @@ - Topic: + Position Tolerance: - + @@ -234,55 +210,90 @@ - - - - - 55 - 16777215 - - + + Sans Serif 8 - - + + Buffer Size: + + + + + + + + QAbstractSpinBox::PlusMinus + + + 99999999 + + + + + + + + 55 + 16777215 + + + + Clear + + + + + + + - Select + Show Laps - - + + + + + + + + + Sans Serif 8 - - - - No topic - - - true + Status: - - + + Sans Serif 8 + + + + + No topic + + + true + diff --git a/mapviz_plugins/ui/navsat_config.ui b/mapviz_plugins/ui/navsat_config.ui index 89217a8d0..f811fa51e 100644 --- a/mapviz_plugins/ui/navsat_config.ui +++ b/mapviz_plugins/ui/navsat_config.ui @@ -7,7 +7,7 @@ 0 0 400 - 197 + 159 @@ -32,80 +32,48 @@ 4 - - - - - 24 - 24 - - - - false - - - - - - - - - - - - - - 16777215 - 25 - - + + Sans Serif - 9 + 8 - - - lines - - - - - points - - + + Buffer Size: + - - + + Sans Serif 8 + + + + + No topic + + + true + - - - - - 55 - 16777215 - - + + Sans Serif 8 - - - - Select + Position Tolerance: @@ -132,79 +100,117 @@ - - - - - Sans Serif - 8 - + + + + + 55 + 16777215 + - Position Tolerance: + Clear - - - - Qt::Vertical - - + + + - 20 - 40 + 55 + 16777215 - - - - Sans Serif 8 + + + - Topic: + Select - - + + + + + 0 + 0 + + + + QAbstractSpinBox::PlusMinus + + + + + + 1.000000000000000 + + + + + Sans Serif 8 + + + + + + + 24 + 24 + + + + false + - No topic - - - true + - - + + + + + 16777215 + 25 + + Sans Serif - 8 + 9 - - Draw Style: - + + + lines + + + + + points + + - - + + Sans Serif @@ -212,31 +218,25 @@ - Buffer Size: + Topic: - - - - - 0 - 0 - - - - QAbstractSpinBox::PlusMinus - - - + + + + + Sans Serif + 8 + - - 1.000000000000000 + + Status: - - + + Sans Serif @@ -244,10 +244,23 @@ - Status: + Draw Style: + + + + Qt::Vertical + + + + 20 + 40 + + + + 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 index 989502f34..19e83be02 100644 --- a/mapviz_plugins/ui/odometry_config.ui +++ b/mapviz_plugins/ui/odometry_config.ui @@ -6,8 +6,8 @@ 0 0 - 400 - 287 + 286 + 312 @@ -16,7 +16,20 @@ - + + + + + + Sans Serif + 8 + + + + Timestamp Interval: + + + @@ -30,6 +43,20 @@ + + + + + + + + + + + + + + @@ -40,7 +67,77 @@ + + + + + 0 + 0 + + + + QAbstractSpinBox::PlusMinus + + + + + + 1.000000000000000 + + + + + + + + Sans Serif + 8 + + + + + + + No topic + + + true + + + + + + + 24 + 24 + + + + false + + + + + + + + + + + + + + Sans Serif + 8 + + + + Status: + + + + @@ -62,8 +159,8 @@ - - + + Sans Serif @@ -71,31 +168,35 @@ - Color: + Buffer Size: - - + + + + QAbstractSpinBox::PlusMinus + + + 99999999 + + + + + - 24 - 24 + 55 + 16777215 - - false - - - - - + Clear - - + + Sans Serif @@ -103,11 +204,11 @@ - Draw Style: + Show Covariance: - + @@ -138,8 +239,8 @@ - - + + Sans Serif @@ -147,64 +248,24 @@ - Static Arrow Sizes: + Timestamp Interval: - - - - 6 - - - - - - - - - - - - 1 - - - 500 - - - 25 - - - Qt::Horizontal - - - QSlider::NoTicks - - - - - - - - - - Sans Serif - 8 - - - - Show Covariance: + + + + Qt::Vertical - - - - - - + + + 20 + 40 + - + - + @@ -216,15 +277,8 @@ - - - - - - - - - + + Sans Serif @@ -232,31 +286,12 @@ - Position Tolerance: - - - - - - - - 0 - 0 - - - - QAbstractSpinBox::PlusMinus - - - - - - 1.000000000000000 + Color: - - + + Sans Serif @@ -264,22 +299,12 @@ - Buffer Size: - - - - - - - QAbstractSpinBox::PlusMinus - - - 99999999 + Static Arrow Sizes: - - + + Sans Serif @@ -287,11 +312,11 @@ - Timestamp Interval: + Position Tolerance: - + @@ -310,50 +335,38 @@ - - - - Qt::Vertical - - - - 20 - 40 - - - - - - - - - Sans Serif - 8 - - - - Status: - - - - - - - - Sans Serif - 8 - - - - - - - No topic - - - true + + + + 6 - + + + + + + + + + + + 1 + + + 500 + + + 25 + + + Qt::Horizontal + + + QSlider::NoTicks + + + + @@ -374,4 +387,3 @@ AngleToleranceChanged(double) - diff --git a/mapviz_plugins/ui/pointcloud2_config.ui b/mapviz_plugins/ui/pointcloud2_config.ui index 586188506..745bae579 100644 --- a/mapviz_plugins/ui/pointcloud2_config.ui +++ b/mapviz_plugins/ui/pointcloud2_config.ui @@ -32,38 +32,6 @@ 4 - - - - - Sans Serif - 8 - - - - Status: - - - - - - - - Sans Serif - 8 - - - - - - - No topic - - - true - - - @@ -122,6 +90,9 @@ + + + @@ -148,7 +119,7 @@ - + Qt::Vertical @@ -161,9 +132,6 @@ - - - @@ -442,6 +410,51 @@ + + + + + Sans Serif + 8 + + + + + + + No topic + + + true + + + + + + + + Sans Serif + 8 + + + + Status: + + + + + + + + 55 + 16777215 + + + + Clear + + + diff --git a/mapviz_plugins/ui/right_click_services_config.ui b/mapviz_plugins/ui/right_click_services_config.ui new file mode 100644 index 000000000..07d2c4b0d --- /dev/null +++ b/mapviz_plugins/ui/right_click_services_config.ui @@ -0,0 +1,194 @@ + + + right_click_services_config + + + + 0 + 0 + 400 + 300 + + + + Form + + + + + + + 2 + + + 2 + + + 2 + + + 2 + + + 4 + + + + + true + + + + Sans Serif + 8 + + + + right_click_services + + + + + + + + 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 + + + + + + + + Sans Serif + 8 + + + + + + + + + + + + Sans Serif + 8 + + + + + + + + + + + + Sans Serif + 8 + + + + available services topic + + + + + + + + Sans Serif + 8 + + + + gps command topic + + + + + + + + diff --git a/mapviz_plugins/ui/tf_frame_config.ui b/mapviz_plugins/ui/tf_frame_config.ui index 9832eafe6..5e44049c9 100644 --- a/mapviz_plugins/ui/tf_frame_config.ui +++ b/mapviz_plugins/ui/tf_frame_config.ui @@ -6,7 +6,7 @@ 0 0 - 400 + 383 197 @@ -17,33 +17,8 @@ - - 4 - - - 2 - - - - - - 24 - 24 - - - - false - - - - - - - - - - - + + Sans Serif @@ -51,12 +26,31 @@ - Status: + Color: - - + + + + + 0 + 0 + + + + QAbstractSpinBox::PlusMinus + + + + + + 1.000000000000000 + + + + + Sans Serif @@ -64,30 +58,42 @@ - Static Arrow Sizes: + Frame: - - + + + + + 16777215 + 25 + + Sans Serif - 8 + 9 - - - - - No topic - - - true - + + + lines + + + + + points + + + + + arrows + + - + @@ -109,44 +115,40 @@ - - - - - Sans Serif - 8 - + + + + + 24 + 24 + - - - - - - - Sans Serif - 8 - + + false + + + - Frame: + - - - - - Sans Serif - 8 - + + + + + 55 + 16777215 + - Color: + Clear - + Sans Serif @@ -154,30 +156,11 @@ - Position Tolerance: - - - - - - - - 0 - 0 - - - - QAbstractSpinBox::PlusMinus - - - - - - 1.000000000000000 + Status: - + @@ -190,7 +173,7 @@ - + QAbstractSpinBox::PlusMinus @@ -201,7 +184,7 @@ - + Sans Serif @@ -209,42 +192,30 @@ - Draw Style: + Position Tolerance: - - - - - 16777215 - 25 - - + + Sans Serif - 9 + 8 - - - lines - - - - - points - - - - - arrows - - + + + + + No topic + + + true + - + @@ -271,7 +242,43 @@ - + + + + + Sans Serif + 8 + + + + Draw Style: + + + + + + + + Sans Serif + 8 + + + + + + + + + Sans Serif + 8 + + + + Static Arrow Sizes: + + + + Qt::Vertical diff --git a/multires_image/test.geo b/multires_image/test.geo index 505e21f47..a276f48b4 100644 --- a/multires_image/test.geo +++ b/multires_image/test.geo @@ -6,7 +6,7 @@ tile_size: 512 // Coordinate System datum: wgs84 -projection: geographic +projection: geographic // Georeference tiepoint: [6785, 336, 29.45196669, -98.61370577]