From c69dd4208d778be476832501c9e0a80d16be60bc Mon Sep 17 00:00:00 2001 From: Matthew Fricke Date: Wed, 18 Jul 2018 17:18:48 -0600 Subject: [PATCH 1/7] Can now draw a rectangle in the map This will allow users to define the rectangle map. The "center" is currently the corner of the rectangle. Variable names should be changed to reflect this (center to corner) and the center and dimensions calulated when the rectangle has finished being drawn and the ROS message is ready to be published. Also added support for circles but they are not drawable yet. --- src/rqt_rover_gui/src/MapData.cpp | 35 ++++++++++ src/rqt_rover_gui/src/MapData.h | 18 +++++ src/rqt_rover_gui/src/MapFrame.cpp | 108 ++++++++++++++++++++++++++++- src/rqt_rover_gui/src/MapFrame.h | 15 ++++ 4 files changed, 175 insertions(+), 1 deletion(-) diff --git a/src/rqt_rover_gui/src/MapData.cpp b/src/rqt_rover_gui/src/MapData.cpp index 0e486016..e46c6b1c 100644 --- a/src/rqt_rover_gui/src/MapData.cpp +++ b/src/rqt_rover_gui/src/MapData.cpp @@ -7,6 +7,26 @@ MapData::MapData() display_global_offset = false; } +// Define virtual fences for display to the user. +void MapData::setVirtualFenceCircle(float center_x, float center_y, float radius) +{ + update_mutex.lock(); + virtual_fence_center = pair(center_x,center_y); + virtual_fence_radius = radius; + virtual_fence_shape = CIRCLE; + update_mutex.unlock(); +} + +void MapData::setVirtualFenceRect(float center_x, float center_y, float height, float width) +{ + update_mutex.lock(); + virtual_fence_center = pair(center_x,center_y); + virtual_fence_height = height; + virtual_fence_width = width; + virtual_fence_shape = RECTANGLE; + update_mutex.unlock(); +} + void MapData::addToGPSRoverPath(string rover, float x, float y) { // Negate the y direction to orient the map so up is north. @@ -230,6 +250,21 @@ void MapData::clear(string rover) update_mutex.unlock(); } +std::tuple MapData::getVirtualFenceRect() +{ + return std::tuple(virtual_fence_center.first, virtual_fence_center.second, virtual_fence_height, virtual_fence_width); +} + +std::tuple MapData::getVirtualFenceCircle() +{ + return std::tuple(virtual_fence_center.first, virtual_fence_center.second, virtual_fence_radius); +} + +VirtualFenceShape MapData::getVirtualFenceShape() +{ + return virtual_fence_shape; +} + std::vector< std::pair >* MapData::getEKFPath(std::string rover_name) { if(display_global_offset) diff --git a/src/rqt_rover_gui/src/MapData.h b/src/rqt_rover_gui/src/MapData.h index f6540c9d..049f8784 100644 --- a/src/rqt_rover_gui/src/MapData.h +++ b/src/rqt_rover_gui/src/MapData.h @@ -11,6 +11,8 @@ #include "MapData.h" +enum VirtualFenceShape {UNDEFINED, CIRCLE, RECTANGLE}; + // This class is the "model" for std::map frame in the model-view UI pattern, // where std::mapFrame is the view. // This allows the creation of multiple std::maps without duplicating large amounts of std::map data. @@ -25,6 +27,16 @@ class MapData void addTargetLocation(std::string rover, float x, float y); void addCollectionPoint(std::string rover, float x, float y); + // Define virtual fences (also called search range). The virtual fence constrains + // Robot movement. They currently apply to all robots, though setting a different virtual + // fence for each robot could be useful. + void setVirtualFenceRect(float center_x, float center_y, float height, float width); + void setVirtualFenceCircle(float center_x, float center_y, float radius); + + VirtualFenceShape getVirtualFenceShape(); + std::tuple getVirtualFenceRect(); + std::tuple getVirtualFenceCircle(); + // Add a waypoint for the specified rover name // Returns the id of the waypoint. IDs are use to issue waypoint commands to the rover. int addToWaypointPath(std::string rover, float x, float y); @@ -106,6 +118,12 @@ class MapData std::map min_ekf_seen_x; std::map min_ekf_seen_y; + std::pair virtual_fence_center; + float virtual_fence_height = 0; + float virtual_fence_width = 0; + float virtual_fence_radius = 0; + VirtualFenceShape virtual_fence_shape = UNDEFINED; + bool display_global_offset; QMutex update_mutex; // To prevent race conditions when the data is being displayed by MapFrame diff --git a/src/rqt_rover_gui/src/MapFrame.cpp b/src/rqt_rover_gui/src/MapFrame.cpp index 9e3d925a..ed838c8c 100644 --- a/src/rqt_rover_gui/src/MapFrame.cpp +++ b/src/rqt_rover_gui/src/MapFrame.cpp @@ -52,6 +52,9 @@ MapFrame::MapFrame(QWidget *parent, Qt::WindowFlags flags) : QFrame(parent) // Trigger mouseMoveEvent even when button not pressed setMouseTracking(true); + // Accept keyboard events when a mouse click occurs in this frame + // Mouse press events are latched so that they are recieved here even when the key was pressed before the map click + setFocusPolicy(Qt::FocusPolicy::ClickFocus); } // This can't go in the constructor or there will be an infinite regression. @@ -530,6 +533,40 @@ void MapFrame::paintEvent(QPaintEvent* event) { hardware_rover_color_index = (hardware_rover_color_index + 1) % 8; + // Draw the virtual fence + if (map_data->getVirtualFenceShape() == CIRCLE) + { + + painter.setPen(Qt::yellow); + tuple virtual_fence = map_data->getVirtualFenceCircle(); + float center_x = map_origin_x+((get<0>(virtual_fence)-min_seen_x)/max_seen_width)*(map_width-map_origin_x); + float center_y = map_origin_y+((get<1>(virtual_fence)-min_seen_y)/max_seen_height)*(map_height-map_origin_y); + + // Radius x and y because the map may not be scaled isomorphically + float radius_x = map_origin_x+((get<2>(virtual_fence)-min_seen_x)/max_seen_width)*(map_width-map_origin_x); + float radius_y = map_origin_y+((get<2>(virtual_fence)-min_seen_y)/max_seen_height)*(map_height-map_origin_y); + + //emit sendInfoLogMessage("Circular fence detected: Fence center: <" + QString::number(center_x) + ", " + QString::number(center_y) + ">; Radius_x: " + QString::number(radius_x) + "Radius_y: " + QString::number(radius_y)); + painter.drawEllipse(center_x, center_y, radius_x, radius_y); + } + else if (map_data->getVirtualFenceShape() == RECTANGLE) + { + painter.setPen(Qt::yellow); + tuple virtual_fence = map_data->getVirtualFenceRect(); + float center_x = map_origin_x+((get<0>(virtual_fence)-min_seen_x)/max_seen_width)*(map_width-map_origin_x); + float center_y = map_origin_y+((get<1>(virtual_fence)-min_seen_y)/max_seen_height)*(map_height-map_origin_y); + float width = map_origin_x+((get<2>(virtual_fence)-min_seen_x)/max_seen_width)*(map_width-map_origin_x); + float height = map_origin_y+((get<3>(virtual_fence)-min_seen_y)/max_seen_height)*(map_height-map_origin_y); + + width -= center_x; + height -= center_y; + + QPainterPath rectangle_fence; + rectangle_fence.addRect(center_x, center_y, width, height); + painter.drawPath(rectangle_fence); + + } + painter.setPen(Qt::white); } // End rover display list set iteration @@ -620,10 +657,31 @@ void MapFrame::mouseReleaseEvent(QMouseEvent *event) { previous_translate_x = translate_x; previous_translate_y = translate_y; + + if (user_is_drawing_fence) + user_is_drawing_fence = false; } void MapFrame::mousePressEvent(QMouseEvent *event) { + // Handle user defining the virtual fence by left clicking and dragging with shift held down + // Holding shift while dragging indicates the user wants to create a circular virtual fence + // If the user is not already dragging the mouse to draw the fence then set the center + if (event->buttons() == Qt::LeftButton && !user_is_drawing_fence && shift_key_down) + { + user_is_drawing_fence = true; + + float mouse_map_x = ((event->pos().x() - map_origin_x*1.0f)/(map_width-map_origin_x))*max_seen_width + min_seen_x; + float mouse_map_y = ((event->pos().y() - map_origin_y*1.0f)/(map_height-map_origin_y))*max_seen_height + min_seen_y; + + fence_center_x = mouse_map_x; + fence_center_y = mouse_map_y; + + emit sendInfoLogMessage("MapFrame: Mouse left button press with shift key down. Fence center: <" + QString::number(fence_center_x) + ", " + QString::number(fence_center_y) + ">"); + } + + emit sendInfoLogMessage("MapFrame: Mouse left button press."); + std::set::iterator it = display_list.find(rover_currently_selected); // Failure condition: a valid rover is not selected. @@ -693,7 +751,7 @@ emit sendInfoLogMessage(" x1: " + QString::number(x1) { previous_clicked_position = event->pos(); } - + // emit sendInfoLogMessage("MapFrame: mouse press. x: " + QString::number(mouse_event->pos().x()) + ", y: " + QString::number(mouse_event->pos().y())); } @@ -704,6 +762,31 @@ void MapFrame::mouseMoveEvent(QMouseEvent *event) // Get the mouse pointer position to print on the map mouse_pointer_position = event->pos(); + if ( event->type() == QEvent::MouseMove && event->buttons() == Qt::LeftButton && shift_key_down && user_is_drawing_fence ) + { + float mouse_map_x = ((event->pos().x() - map_origin_x*1.0f)/(map_width-map_origin_x))*max_seen_width + min_seen_x; + float mouse_map_y = ((event->pos().y() - map_origin_y*1.0f)/(map_height-map_origin_y))*max_seen_height + min_seen_y; + + //float delta_x = mouse_map_x - fence_center_x; + //float delta_y = mouse_map_y - fence_center_y; + //float fence_radius = sqrt(delta_x*delta_x + delta_y*delta_y); + + + + map_data->setVirtualFenceRect(fence_center_x, fence_center_y, mouse_map_x, mouse_map_y); + emit sendInfoLogMessage(QString("MapFrame: mouse move drawing fence.") + " Center: <" + + QString::number(fence_center_x) + + "," + + QString::number(fence_center_y) + + ">; Width: " + + QString::number(mouse_map_x) + + "; Height: " + + QString::number(mouse_map_y) + ); + + return; + } + // Do not adjust panning in auto-transform mode.. the changes will not be reflected // and will be applied only when the user clicks on manual panning mode which will // cause undesired results. @@ -735,6 +818,8 @@ void MapFrame::mouseMoveEvent(QMouseEvent *event) // emit sendInfoLogMessage("MapFrame: mouse move: xp: " + QString::number(previous_clicked_position.x()) + " yp: " + QString::number(previous_clicked_position.y())); // Store the current mouse position for use in the map + + } } } @@ -929,6 +1014,27 @@ void MapFrame::receiveCurrentRoverName( QString rover_name ) } } +void MapFrame::keyPressEvent(QKeyEvent* event) +{ + if(event->key() == Qt::Key_Shift) + { + emit sendInfoLogMessage("MapFrame: shift key pressed."); + shift_key_down = true; + } + + +} + +void MapFrame::keyReleaseEvent(QKeyEvent* event) +{ + if(event->key() == Qt::Key_Shift) + { + emit sendInfoLogMessage("MapFrame: shift key released."); + shift_key_down = false; + user_is_drawing_fence = false; + } +} + void MapFrame::enableWaypoints(string rover_name) { map_data->setManualMode(rover_name); diff --git a/src/rqt_rover_gui/src/MapFrame.h b/src/rqt_rover_gui/src/MapFrame.h index 11bbce8b..e7541fb1 100644 --- a/src/rqt_rover_gui/src/MapFrame.h +++ b/src/rqt_rover_gui/src/MapFrame.h @@ -106,12 +106,19 @@ namespace rqt_rover_gui protected: + // React to draw requests void paintEvent(QPaintEvent *event); + + // React to mouse events void mouseReleaseEvent(QMouseEvent *event); void mousePressEvent(QMouseEvent *event); void mouseMoveEvent(QMouseEvent *event); void wheelEvent(QWheelEvent *); + // React to keyboard events + void keyPressEvent(QKeyEvent* event); + void keyReleaseEvent(QKeyEvent* event); + private: mutable QMutex update_mutex; @@ -185,6 +192,14 @@ namespace rqt_rover_gui float max_seen_width = -std::numeric_limits::max(); float max_seen_height = -std::numeric_limits::max(); + float fence_center_x = 0; + float fence_center_y = 0; + bool user_is_drawing_fence = false; + + // Keyboard state + // This can be fooled when multiple shift keys are pressed or released at the same time. + bool shift_key_down = false; + std::string rover_currently_selected; // This is the rover selected in the main GUI. enum mode { From a6b8e9ee6d4da4feea35055aad0b2dc696b45c94 Mon Sep 17 00:00:00 2001 From: Matthew Fricke Date: Thu, 19 Jul 2018 11:48:53 -0600 Subject: [PATCH 2/7] Updated the rectangular virtual fence variable names They now reflect that the region is defined using the corners not the center Also fixed a debug output error. Can't put < and > in the string because that is parsed as an html tag. --- src/rqt_rover_gui/src/MapData.cpp | 8 +++---- src/rqt_rover_gui/src/MapData.h | 5 ++-- src/rqt_rover_gui/src/MapFrame.cpp | 38 ++++++++++++++---------------- src/rqt_rover_gui/src/MapFrame.h | 4 ++-- 4 files changed, 27 insertions(+), 28 deletions(-) diff --git a/src/rqt_rover_gui/src/MapData.cpp b/src/rqt_rover_gui/src/MapData.cpp index e46c6b1c..7031d3c4 100644 --- a/src/rqt_rover_gui/src/MapData.cpp +++ b/src/rqt_rover_gui/src/MapData.cpp @@ -17,10 +17,10 @@ void MapData::setVirtualFenceCircle(float center_x, float center_y, float radius update_mutex.unlock(); } -void MapData::setVirtualFenceRect(float center_x, float center_y, float height, float width) +void MapData::setVirtualFenceRectangle(float corner_x, float corner_y, float height, float width) { update_mutex.lock(); - virtual_fence_center = pair(center_x,center_y); + virtual_fence_corner = pair(corner_x, corner_y); virtual_fence_height = height; virtual_fence_width = width; virtual_fence_shape = RECTANGLE; @@ -250,9 +250,9 @@ void MapData::clear(string rover) update_mutex.unlock(); } -std::tuple MapData::getVirtualFenceRect() +std::tuple MapData::getVirtualFenceRectangle() { - return std::tuple(virtual_fence_center.first, virtual_fence_center.second, virtual_fence_height, virtual_fence_width); + return std::tuple(virtual_fence_corner.first, virtual_fence_corner.second, virtual_fence_height, virtual_fence_width); } std::tuple MapData::getVirtualFenceCircle() diff --git a/src/rqt_rover_gui/src/MapData.h b/src/rqt_rover_gui/src/MapData.h index 049f8784..6b5bbe3a 100644 --- a/src/rqt_rover_gui/src/MapData.h +++ b/src/rqt_rover_gui/src/MapData.h @@ -30,11 +30,11 @@ class MapData // Define virtual fences (also called search range). The virtual fence constrains // Robot movement. They currently apply to all robots, though setting a different virtual // fence for each robot could be useful. - void setVirtualFenceRect(float center_x, float center_y, float height, float width); + void setVirtualFenceRectangle(float corner_x, float corner_y, float height, float width); void setVirtualFenceCircle(float center_x, float center_y, float radius); VirtualFenceShape getVirtualFenceShape(); - std::tuple getVirtualFenceRect(); + std::tuple getVirtualFenceRectangle(); std::tuple getVirtualFenceCircle(); // Add a waypoint for the specified rover name @@ -118,6 +118,7 @@ class MapData std::map min_ekf_seen_x; std::map min_ekf_seen_y; + std::pair virtual_fence_corner; std::pair virtual_fence_center; float virtual_fence_height = 0; float virtual_fence_width = 0; diff --git a/src/rqt_rover_gui/src/MapFrame.cpp b/src/rqt_rover_gui/src/MapFrame.cpp index ed838c8c..36aa45bb 100644 --- a/src/rqt_rover_gui/src/MapFrame.cpp +++ b/src/rqt_rover_gui/src/MapFrame.cpp @@ -546,23 +546,22 @@ void MapFrame::paintEvent(QPaintEvent* event) { float radius_x = map_origin_x+((get<2>(virtual_fence)-min_seen_x)/max_seen_width)*(map_width-map_origin_x); float radius_y = map_origin_y+((get<2>(virtual_fence)-min_seen_y)/max_seen_height)*(map_height-map_origin_y); - //emit sendInfoLogMessage("Circular fence detected: Fence center: <" + QString::number(center_x) + ", " + QString::number(center_y) + ">; Radius_x: " + QString::number(radius_x) + "Radius_y: " + QString::number(radius_y)); + //emit sendInfoLogMessage("Circular fence detected: Fence center: (" + QString::number(center_x) + ", " + QString::number(center_y) + "); Radius_x: " + QString::number(radius_x) + "Radius_y: " + QString::number(radius_y)); painter.drawEllipse(center_x, center_y, radius_x, radius_y); } else if (map_data->getVirtualFenceShape() == RECTANGLE) { painter.setPen(Qt::yellow); - tuple virtual_fence = map_data->getVirtualFenceRect(); - float center_x = map_origin_x+((get<0>(virtual_fence)-min_seen_x)/max_seen_width)*(map_width-map_origin_x); - float center_y = map_origin_y+((get<1>(virtual_fence)-min_seen_y)/max_seen_height)*(map_height-map_origin_y); - float width = map_origin_x+((get<2>(virtual_fence)-min_seen_x)/max_seen_width)*(map_width-map_origin_x); - float height = map_origin_y+((get<3>(virtual_fence)-min_seen_y)/max_seen_height)*(map_height-map_origin_y); + tuple virtual_fence = map_data->getVirtualFenceRectangle(); + float corner_x = map_origin_x+((get<0>(virtual_fence)-min_seen_x)/max_seen_width)*(map_width-map_origin_x); + float corner_y = map_origin_y+((get<1>(virtual_fence)-min_seen_y)/max_seen_height)*(map_height-map_origin_y); + float width = map_origin_x+((get<2>(virtual_fence)-min_seen_x)/max_seen_width)*(map_width-map_origin_x) - corner_x; + float height = map_origin_y+((get<3>(virtual_fence)-min_seen_y)/max_seen_height)*(map_height-map_origin_y) - corner_y; - width -= center_x; - height -= center_y; + //emit sendInfoLogMessage("Rectangle fence detected: Fence corner: (" + QString::number(get<0>(virtual_fence)) + ", " + QString::number(get<1>(virtual_fence)) + "); Width: " + QString::number(width) + "Height: " + QString::number(height)); QPainterPath rectangle_fence; - rectangle_fence.addRect(center_x, center_y, width, height); + rectangle_fence.addRect(corner_x, corner_y, width, height); painter.drawPath(rectangle_fence); } @@ -669,15 +668,16 @@ void MapFrame::mousePressEvent(QMouseEvent *event) // If the user is not already dragging the mouse to draw the fence then set the center if (event->buttons() == Qt::LeftButton && !user_is_drawing_fence && shift_key_down) { - user_is_drawing_fence = true; + float mouse_map_x = ((event->pos().x() - map_origin_x*1.0f)/(map_width-map_origin_x))*max_seen_width + min_seen_x; float mouse_map_y = ((event->pos().y() - map_origin_y*1.0f)/(map_height-map_origin_y))*max_seen_height + min_seen_y; - fence_center_x = mouse_map_x; - fence_center_y = mouse_map_y; + fence_corner_x = mouse_map_x; + fence_corner_y = mouse_map_y; - emit sendInfoLogMessage("MapFrame: Mouse left button press with shift key down. Fence center: <" + QString::number(fence_center_x) + ", " + QString::number(fence_center_y) + ">"); + emit sendInfoLogMessage("MapFrame: Mouse left button press with shift key down. Fence corner: (" + QString::number(fence_corner_x) + ", " + QString::number(fence_corner_y) + ")"); + user_is_drawing_fence = true; } emit sendInfoLogMessage("MapFrame: Mouse left button press."); @@ -771,14 +771,12 @@ void MapFrame::mouseMoveEvent(QMouseEvent *event) //float delta_y = mouse_map_y - fence_center_y; //float fence_radius = sqrt(delta_x*delta_x + delta_y*delta_y); - - - map_data->setVirtualFenceRect(fence_center_x, fence_center_y, mouse_map_x, mouse_map_y); - emit sendInfoLogMessage(QString("MapFrame: mouse move drawing fence.") + " Center: <" - + QString::number(fence_center_x) + map_data->setVirtualFenceRectangle(fence_corner_x, fence_corner_y, mouse_map_x, mouse_map_y); + emit sendInfoLogMessage(QString("MapFrame: mouse move drawing fence.") + " Corner: (" + + QString::number(fence_corner_x) + "," - + QString::number(fence_center_y) - + ">; Width: " + + QString::number(fence_corner_y) + + "); Width: " + QString::number(mouse_map_x) + "; Height: " + QString::number(mouse_map_y) diff --git a/src/rqt_rover_gui/src/MapFrame.h b/src/rqt_rover_gui/src/MapFrame.h index e7541fb1..e533dcb5 100644 --- a/src/rqt_rover_gui/src/MapFrame.h +++ b/src/rqt_rover_gui/src/MapFrame.h @@ -192,8 +192,8 @@ namespace rqt_rover_gui float max_seen_width = -std::numeric_limits::max(); float max_seen_height = -std::numeric_limits::max(); - float fence_center_x = 0; - float fence_center_y = 0; + float fence_corner_x = 0; + float fence_corner_y = 0; bool user_is_drawing_fence = false; // Keyboard state From 3c4c7b67f4f5ad7b9580f9d7f04387303921fd97 Mon Sep 17 00:00:00 2001 From: Matthew Fricke Date: Thu, 19 Jul 2018 12:49:50 -0600 Subject: [PATCH 3/7] Virtual fence displays geometry While the virtual fence is being drawn the center, width and height of the rectangle is displayed. The units are currently pixels. Next they need to be converted into meters. --- src/rqt_rover_gui/src/MapFrame.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/rqt_rover_gui/src/MapFrame.cpp b/src/rqt_rover_gui/src/MapFrame.cpp index 36aa45bb..3d5a25ff 100644 --- a/src/rqt_rover_gui/src/MapFrame.cpp +++ b/src/rqt_rover_gui/src/MapFrame.cpp @@ -564,6 +564,15 @@ void MapFrame::paintEvent(QPaintEvent* event) { rectangle_fence.addRect(corner_x, corner_y, width, height); painter.drawPath(rectangle_fence); + float rectangle_center_x = corner_x+width/2; + float rectangle_center_y = corner_y+height/2; + + if (user_is_drawing_fence) + { + painter.drawEllipse(QPointF(rectangle_center_x,rectangle_center_y), 2.5, 2.5); + QString rect_geometry = "Center: (" + QString::number(rectangle_center_x) + ", " + QString::number(rectangle_center_y) + ")\nWidth: " + QString::number(width) + "\nHeight: " + QString::number(height); + painter.drawText(QRect(rectangle_center_x, rectangle_center_y, 300, 200), rect_geometry); + } } painter.setPen(Qt::white); From 2be5251486534c4463a708034708a09357ad9c70 Mon Sep 17 00:00:00 2001 From: Matthew Fricke Date: Mon, 30 Jul 2018 14:41:35 -0600 Subject: [PATCH 4/7] Added fence label to GUI in meters Converted fence geometry from pixel space to meters for display as the user is drawing the fence. --- src/rqt_rover_gui/src/MapFrame.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/rqt_rover_gui/src/MapFrame.cpp b/src/rqt_rover_gui/src/MapFrame.cpp index 3d5a25ff..20e2922a 100644 --- a/src/rqt_rover_gui/src/MapFrame.cpp +++ b/src/rqt_rover_gui/src/MapFrame.cpp @@ -564,13 +564,17 @@ void MapFrame::paintEvent(QPaintEvent* event) { rectangle_fence.addRect(corner_x, corner_y, width, height); painter.drawPath(rectangle_fence); - float rectangle_center_x = corner_x+width/2; - float rectangle_center_y = corner_y+height/2; - if (user_is_drawing_fence) { - painter.drawEllipse(QPointF(rectangle_center_x,rectangle_center_y), 2.5, 2.5); - QString rect_geometry = "Center: (" + QString::number(rectangle_center_x) + ", " + QString::number(rectangle_center_y) + ")\nWidth: " + QString::number(width) + "\nHeight: " + QString::number(height); + float rectangle_center_x = corner_x+width/2; + float rectangle_center_y = corner_y+width/2; + float meter_conversion_factor = max_seen_width/map_width; // Convert pixels into meters + float rectangle_center_x_in_meters = (corner_x+width/2) * meter_conversion_factor; + float rectangle_center_y_in_meters = (corner_y+height/2) * meter_conversion_factor; + float width_in_meters = abs(width * meter_conversion_factor); // abs because distances must be positive values + float height_in_meters = abs(height * meter_conversion_factor); + painter.drawEllipse(QPointF(rectangle_center_x_in_meters, rectangle_center_y_in_meters), 2.5, 2.5); + QString rect_geometry = "Center: (" + QString::number(rectangle_center_x_in_meters) + ", " + QString::number(rectangle_center_y_in_meters) + ")\nWidth: " + QString::number(width_in_meters) + " m\nHeight: " + QString::number(height_in_meters) + " m"; painter.drawText(QRect(rectangle_center_x, rectangle_center_y, 300, 200), rect_geometry); } } From 63fec7ecb192c490c74f26dba0313eba5a3e9d19 Mon Sep 17 00:00:00 2001 From: Matthew Fricke Date: Tue, 31 Jul 2018 17:05:36 -0600 Subject: [PATCH 5/7] Added infrastructure to support UI VirtualFence cmds Implemented Virtual Fence commands as a custom message type and setup QT signals and slots. The next step is to add a sendVirtualFenceCmd call when the user finishes drawing a fence. Then the pipeline can be debugged. --- src/behaviours/src/ROSAdapter.cpp | 26 +++++++++++----------- src/rqt_rover_gui/src/MapData.cpp | 8 +++---- src/rqt_rover_gui/src/MapData.h | 9 ++++---- src/rqt_rover_gui/src/MapFrame.cpp | 5 ++--- src/rqt_rover_gui/src/MapFrame.h | 2 ++ src/rqt_rover_gui/src/rover_gui_plugin.cpp | 25 +++++++++++++++++++++ src/rqt_rover_gui/src/rover_gui_plugin.h | 7 ++++++ src/swarmie_msgs/CMakeLists.txt | 1 + 8 files changed, 58 insertions(+), 25 deletions(-) diff --git a/src/behaviours/src/ROSAdapter.cpp b/src/behaviours/src/ROSAdapter.cpp index d213abef..dd70b2fb 100644 --- a/src/behaviours/src/ROSAdapter.cpp +++ b/src/behaviours/src/ROSAdapter.cpp @@ -22,6 +22,7 @@ #include #include #include "swarmie_msgs/Waypoint.h" +#include "swarmie_msgs/VirtualFence.h" // Include Controllers #include "LogicController.h" @@ -169,7 +170,7 @@ void modeHandler(const std_msgs::UInt8::ConstPtr& message); void targetHandler(const apriltags_ros::AprilTagDetectionArray::ConstPtr& tagInfo); void odometryHandler(const nav_msgs::Odometry::ConstPtr& message); void mapHandler(const nav_msgs::Odometry::ConstPtr& message); -void virtualFenceHandler(const std_msgs::Float32MultiArray& message); +void virtualFenceHandler(const swarmie_msgs::VirtualFence& message); void manualWaypointHandler(const swarmie_msgs::Waypoint& message); void behaviourStateMachine(const ros::TimerEvent&); void publishStatusTimerEventHandler(const ros::TimerEvent& event); @@ -509,16 +510,16 @@ void odometryHandler(const nav_msgs::Odometry::ConstPtr& message) { } // Allows a virtual fence to be defined and enabled or disabled through ROS -void virtualFenceHandler(const std_msgs::Float32MultiArray& message) +void virtualFenceHandler(const swarmie_msgs::VirtualFence& message) { // Read data from the message array // The first element is an integer indicating the shape type // 0 = Disable the virtual fence // 1 = circle // 2 = rectangle - int shape_type = static_cast(message.data[0]); // Shape type + int cmd = static_cast(message.cmd); // Shape type - if (shape_type == 0) + if (cmd == 0) { logicController.setVirtualFenceOff(); } @@ -526,24 +527,23 @@ void virtualFenceHandler(const std_msgs::Float32MultiArray& message) { // Elements 2 and 3 are the x and y coordinates of the range center Point center; - center.x = message.data[1]; // Range center x - center.y = message.data[2]; // Range center y + center.x = message.center_x; // Range center x + center.y = message.center_y; // Range center y // If the shape type is "circle" then element 4 is the radius, if rectangle then width - switch ( shape_type ) + switch ( cmd ) { case 1: // Circle { - if ( message.data.size() != 4 ) throw ROSAdapterRangeShapeInvalidTypeException("Wrong number of parameters for circle shape type in ROSAdapter.cpp:virtualFenceHandler()"); - float radius = message.data[3]; - logicController.setVirtualFenceOn( new RangeCircle(center, radius) ); + //if ( message.data.size() != 4 ) throw ROSAdapterRangeShapeInvalidTypeException("Wrong number of parameters for circle shape type in ROSAdapter.cpp:virtualFenceHandler()"); + //float radius = message.data; + //logicController.setVirtualFenceOn( new RangeCircle(center, radius) ); break; } case 2: // Rectangle { - if ( message.data.size() != 5 ) throw ROSAdapterRangeShapeInvalidTypeException("Wrong number of parameters for rectangle shape type in ROSAdapter.cpp:virtualFenceHandler()"); - float width = message.data[3]; - float height = message.data[4]; + float width = message.width; + float height = message.height; logicController.setVirtualFenceOn( new RangeRectangle(center, width, height) ); break; } diff --git a/src/rqt_rover_gui/src/MapData.cpp b/src/rqt_rover_gui/src/MapData.cpp index 7031d3c4..11d14fc6 100644 --- a/src/rqt_rover_gui/src/MapData.cpp +++ b/src/rqt_rover_gui/src/MapData.cpp @@ -13,7 +13,7 @@ void MapData::setVirtualFenceCircle(float center_x, float center_y, float radius update_mutex.lock(); virtual_fence_center = pair(center_x,center_y); virtual_fence_radius = radius; - virtual_fence_shape = CIRCLE; + virtual_fence_cmd = CIRCLE; update_mutex.unlock(); } @@ -23,7 +23,7 @@ void MapData::setVirtualFenceRectangle(float corner_x, float corner_y, float hei virtual_fence_corner = pair(corner_x, corner_y); virtual_fence_height = height; virtual_fence_width = width; - virtual_fence_shape = RECTANGLE; + virtual_fence_cmd = RECTANGLE; update_mutex.unlock(); } @@ -260,9 +260,9 @@ std::tuple MapData::getVirtualFenceCircle() return std::tuple(virtual_fence_center.first, virtual_fence_center.second, virtual_fence_radius); } -VirtualFenceShape MapData::getVirtualFenceShape() +VirtualFenceCmd MapData::getVirtualFenceCmd() { - return virtual_fence_shape; + return virtual_fence_cmd; } std::vector< std::pair >* MapData::getEKFPath(std::string rover_name) diff --git a/src/rqt_rover_gui/src/MapData.h b/src/rqt_rover_gui/src/MapData.h index 6b5bbe3a..6357a1a3 100644 --- a/src/rqt_rover_gui/src/MapData.h +++ b/src/rqt_rover_gui/src/MapData.h @@ -9,9 +9,8 @@ #include #include -#include "MapData.h" - -enum VirtualFenceShape {UNDEFINED, CIRCLE, RECTANGLE}; +// Virtual fence command definitions that can be sent to the rover +enum VirtualFenceCmd {UNDEFINED, CIRCLE, RECTANGLE}; // This class is the "model" for std::map frame in the model-view UI pattern, // where std::mapFrame is the view. @@ -33,7 +32,7 @@ class MapData void setVirtualFenceRectangle(float corner_x, float corner_y, float height, float width); void setVirtualFenceCircle(float center_x, float center_y, float radius); - VirtualFenceShape getVirtualFenceShape(); + VirtualFenceCmd getVirtualFenceCmd(); std::tuple getVirtualFenceRectangle(); std::tuple getVirtualFenceCircle(); @@ -123,7 +122,7 @@ class MapData float virtual_fence_height = 0; float virtual_fence_width = 0; float virtual_fence_radius = 0; - VirtualFenceShape virtual_fence_shape = UNDEFINED; + VirtualFenceCmd virtual_fence_cmd = UNDEFINED; bool display_global_offset; diff --git a/src/rqt_rover_gui/src/MapFrame.cpp b/src/rqt_rover_gui/src/MapFrame.cpp index 20e2922a..7b0d0609 100644 --- a/src/rqt_rover_gui/src/MapFrame.cpp +++ b/src/rqt_rover_gui/src/MapFrame.cpp @@ -8,7 +8,6 @@ #include #include #include -#include #include "MapFrame.h" namespace rqt_rover_gui @@ -534,7 +533,7 @@ void MapFrame::paintEvent(QPaintEvent* event) { hardware_rover_color_index = (hardware_rover_color_index + 1) % 8; // Draw the virtual fence - if (map_data->getVirtualFenceShape() == CIRCLE) + if (map_data->getVirtualFenceCmd() == CIRCLE) { painter.setPen(Qt::yellow); @@ -549,7 +548,7 @@ void MapFrame::paintEvent(QPaintEvent* event) { //emit sendInfoLogMessage("Circular fence detected: Fence center: (" + QString::number(center_x) + ", " + QString::number(center_y) + "); Radius_x: " + QString::number(radius_x) + "Radius_y: " + QString::number(radius_y)); painter.drawEllipse(center_x, center_y, radius_x, radius_y); } - else if (map_data->getVirtualFenceShape() == RECTANGLE) + else if (map_data->getVirtualFenceCmd() == RECTANGLE) { painter.setPen(Qt::yellow); tuple virtual_fence = map_data->getVirtualFenceRectangle(); diff --git a/src/rqt_rover_gui/src/MapFrame.h b/src/rqt_rover_gui/src/MapFrame.h index e533dcb5..3bf9d3a2 100644 --- a/src/rqt_rover_gui/src/MapFrame.h +++ b/src/rqt_rover_gui/src/MapFrame.h @@ -27,6 +27,7 @@ #include // For STL pair #include #include +#include "MapData.h" // Forward declarations class QMainWindow; @@ -95,6 +96,7 @@ namespace rqt_rover_gui signals: + void sendVirtualFenceCmd(VirtualFenceCmd vf, float center_x, float center_y, float width, float height); void sendInfoLogMessage(QString msg); void sendWaypointCmd(WaypointCmd, int, float, float); void delayedUpdate(); diff --git a/src/rqt_rover_gui/src/rover_gui_plugin.cpp b/src/rqt_rover_gui/src/rover_gui_plugin.cpp index 11e86921..1d6e20af 100644 --- a/src/rqt_rover_gui/src/rover_gui_plugin.cpp +++ b/src/rqt_rover_gui/src/rover_gui_plugin.cpp @@ -171,6 +171,9 @@ namespace rqt_rover_gui connect(ui.map_frame, SIGNAL(sendWaypointCmd(WaypointCmd, int, float, float)), this, SLOT(receiveWaypointCmd(WaypointCmd, int, float, float))); connect(this, SIGNAL(sendWaypointReached(int)), ui.map_frame, SLOT(receiveWaypointReached(int))); + // Receive virtual fence commands from MapFrame + connect(ui.map_frame, SIGNAL(sendVirtualFenceCmd(VirtualFenceCmd, float, float, float, float)), this, SLOT(receiveVirtualFenceCmd(VirtualFenceCmd, float, float, float, float))); + // Receive log messages from contained frames connect(ui.map_frame, SIGNAL(sendInfoLogMessage(QString)), this, SLOT(receiveInfoLogMessage(QString))); @@ -224,11 +227,14 @@ namespace rqt_rover_gui info_log_subscriber = nh.subscribe("/infoLog", 10, &RoverGUIPlugin::infoLogMessageEventHandler, this); diag_log_subscriber = nh.subscribe("/diagsLog", 10, &RoverGUIPlugin::diagLogMessageEventHandler, this); + virtualfence_cmd_publisher = nh.advertise("/virtualFence/", 10, true); + emit updateNumberOfSatellites("---"); } void RoverGUIPlugin::shutdownPlugin() { + virtualfence_cmd_publisher.shutdown(); map_data->clear(); // Clear the map and stop drawing before the map_frame is destroyed ui.map_frame->clear(); clearSimulationButtonEventHandler(); @@ -780,6 +786,7 @@ void RoverGUIPlugin::pollRoversTimerEventHandler() control_mode_publishers.erase(*it); waypoint_cmd_publishers.erase(*it); + ui.map_frame->resetWaypointPathForSelectedRover(*it); } @@ -2965,6 +2972,24 @@ void RoverGUIPlugin::receiveWaypointCmd(WaypointCmd cmd, int id, float x, float waypoint_cmd_publishers[selected_rover_name].publish(msg); } +// Publish the virtual fence commands recieved from MapFrame to ROS +void RoverGUIPlugin::receiveVirtualFenceCmd(VirtualFenceCmd cmd, float center_x, float center_y, float width, float height) +{ + sendInfoLogMessage("Virtual Fence Command Recieved"); + + // At the moment there is only one global virtual fence topic that all rovers listen to + // Use the Virtual Fence custom message type to package up the command to publish. Relies on the VirtualFence enum matching the message definition. + swarmie_msgs::VirtualFence msg; + msg.cmd = cmd; + msg.center_x = center_x; + msg.center_y = center_y; + msg.width = width; + msg.height = height; + + virtualfence_cmd_publisher.publish(msg); +} + + // Clean up memory when this object is deleted RoverGUIPlugin::~RoverGUIPlugin() { diff --git a/src/rqt_rover_gui/src/rover_gui_plugin.h b/src/rqt_rover_gui/src/rover_gui_plugin.h index a0431526..7014aee6 100644 --- a/src/rqt_rover_gui/src/rover_gui_plugin.h +++ b/src/rqt_rover_gui/src/rover_gui_plugin.h @@ -52,6 +52,7 @@ #include #include #include "swarmie_msgs/Waypoint.h" // For waypoint commands +#include "swarmie_msgs/VirtualFence.h" // For virtual fence commands //ROS msg types //#include "rover_onboard_target_detection/ATag.h" @@ -204,6 +205,11 @@ namespace rqt_rover_gui { void displayDiagLogMessage(QString msg); void receiveWaypointCmd(WaypointCmd, int, float, float); + // This slot recives parameters for building a virtual fence (aka range) command to send to rovers. + // The slot will publish the command to all known rovers. + void receiveVirtualFenceCmd(VirtualFenceCmd cmd, float center_x, float center_y, float width, float height); + + // Needed to refocus the keyboard events when the user clicks on the widget list // to the main widget so keyboard manual control is handled properly void refocusKeyboardEventHandler(); @@ -216,6 +222,7 @@ namespace rqt_rover_gui { // ROS Publishers map control_mode_publishers; map waypoint_cmd_publishers; + ros::Publisher virtualfence_cmd_publisher; ros::Publisher joystick_publisher; // ROS Subscribers diff --git a/src/swarmie_msgs/CMakeLists.txt b/src/swarmie_msgs/CMakeLists.txt index d8e1a63a..877ead96 100644 --- a/src/swarmie_msgs/CMakeLists.txt +++ b/src/swarmie_msgs/CMakeLists.txt @@ -46,6 +46,7 @@ find_package(catkin REQUIRED COMPONENTS message_generation) add_message_files( FILES Waypoint.msg + VirtualFence.msg ) ## Generate services in the 'srv' folder From 048dc2db5aae5fe07719eeb149a3c7480fce3de9 Mon Sep 17 00:00:00 2001 From: Matthew Fricke Date: Thu, 2 Aug 2018 09:21:19 -0600 Subject: [PATCH 6/7] Fixed calulation of the virtual fence center The center calculation in meters was not properly offset in the conversion from pixel space. Added the VirtualFence msg type - should have been added previously --- src/rqt_rover_gui/src/MapFrame.cpp | 37 ++++++++++++++++++++++----- src/swarmie_msgs/msg/VirtualFence.msg | 9 +++++++ 2 files changed, 40 insertions(+), 6 deletions(-) create mode 100644 src/swarmie_msgs/msg/VirtualFence.msg diff --git a/src/rqt_rover_gui/src/MapFrame.cpp b/src/rqt_rover_gui/src/MapFrame.cpp index 7b0d0609..45084c1a 100644 --- a/src/rqt_rover_gui/src/MapFrame.cpp +++ b/src/rqt_rover_gui/src/MapFrame.cpp @@ -221,8 +221,8 @@ void MapFrame::paintEvent(QPaintEvent* event) { map_width = this->width()-1;// Minus 1 or will go off the edge map_height = this->height()-1;// - int map_center_x = map_origin_x+((map_width-map_origin_x)/2); - int map_center_y = map_origin_y+((map_height-map_origin_y)/2); + map_center_x = map_origin_x+((map_width-map_origin_x)/2); + map_center_y = map_origin_y+((map_height-map_origin_y)/2); // The map axes do not need to be redrawn for each rover so this code is // sandwiched between the two rover display list loops @@ -566,13 +566,13 @@ void MapFrame::paintEvent(QPaintEvent* event) { if (user_is_drawing_fence) { float rectangle_center_x = corner_x+width/2; - float rectangle_center_y = corner_y+width/2; + float rectangle_center_y = corner_y+height/2; float meter_conversion_factor = max_seen_width/map_width; // Convert pixels into meters - float rectangle_center_x_in_meters = (corner_x+width/2) * meter_conversion_factor; - float rectangle_center_y_in_meters = (corner_y+height/2) * meter_conversion_factor; + float rectangle_center_x_in_meters = ((corner_x+width/2)-map_center_x) * meter_conversion_factor; + float rectangle_center_y_in_meters = ((corner_y+height/2)-map_center_y) * meter_conversion_factor; float width_in_meters = abs(width * meter_conversion_factor); // abs because distances must be positive values float height_in_meters = abs(height * meter_conversion_factor); - painter.drawEllipse(QPointF(rectangle_center_x_in_meters, rectangle_center_y_in_meters), 2.5, 2.5); + painter.drawEllipse(QPointF(rectangle_center_x, rectangle_center_y), 2.5, 2.5); QString rect_geometry = "Center: (" + QString::number(rectangle_center_x_in_meters) + ", " + QString::number(rectangle_center_y_in_meters) + ")\nWidth: " + QString::number(width_in_meters) + " m\nHeight: " + QString::number(height_in_meters) + " m"; painter.drawText(QRect(rectangle_center_x, rectangle_center_y, 300, 200), rect_geometry); } @@ -670,7 +670,32 @@ void MapFrame::mouseReleaseEvent(QMouseEvent *event) previous_translate_y = translate_y; if (user_is_drawing_fence) + { + // Stop drawing the fence user_is_drawing_fence = false; + + // Extract the geometry of the virtual fence from map data + tuple virtual_fence = map_data->getVirtualFenceRectangle(); + float corner_x = map_origin_x+((get<0>(virtual_fence)-min_seen_x)/max_seen_width)*(map_width-map_origin_x); + float corner_y = map_origin_y+((get<1>(virtual_fence)-min_seen_y)/max_seen_height)*(map_height-map_origin_y); + float width = map_origin_x+((get<2>(virtual_fence)-min_seen_x)/max_seen_width)*(map_width-map_origin_x) - corner_x; + float height = map_origin_y+((get<3>(virtual_fence)-min_seen_y)/max_seen_height)*(map_height-map_origin_y) - corner_y; + float rectangle_center_x = corner_x+width/2; + float rectangle_center_y = corner_y+width/2; + + // Convert the geometry into meters using robot data plotted in the map to relate pixel space to actual meters + float meter_conversion_factor = max_seen_width/map_width; // Convert pixels into meters + float rectangle_center_x_in_meters = ((corner_x+width/2)-map_center_x) * meter_conversion_factor; + float rectangle_center_y_in_meters = ((corner_y+height/2)-map_center_y) * meter_conversion_factor; + float width_in_meters = abs(width * meter_conversion_factor); // abs because distances must be positive values + float height_in_meters = abs(height * meter_conversion_factor); + + // Gett the command associated with the virtual fence. This can be the shape or undefined. Undefined will delete the virtual fence. + VirtualFenceCmd cmd = map_data->getVirtualFenceCmd(); + + // Send the command to Rover GUI. Rover GUI will package it up for ROS. + sendVirtualFenceCmd( cmd, rectangle_center_x_in_meters, rectangle_center_y_in_meters, width_in_meters, height_in_meters ); + } } void MapFrame::mousePressEvent(QMouseEvent *event) diff --git a/src/swarmie_msgs/msg/VirtualFence.msg b/src/swarmie_msgs/msg/VirtualFence.msg new file mode 100644 index 00000000..f23752d6 --- /dev/null +++ b/src/swarmie_msgs/msg/VirtualFence.msg @@ -0,0 +1,9 @@ +# Defines the virtual fence's geometry +uint32 DISABLE=0 +uint32 CIRCLE=1 +uint32 RECTANGLE=2 +uint32 cmd +float32 center_x +float32 center_y +float32 width +float32 height \ No newline at end of file From d322650ef8219d5436ef68d6b712b261607bc20a Mon Sep 17 00:00:00 2001 From: Matthew Fricke Date: Thu, 2 Aug 2018 09:50:35 -0600 Subject: [PATCH 7/7] Resolve local merge conflict ROS adapter had comments that were added to the call back handler since the virtual fence branch was created. This commit fixes the merge conflict. --- src/behaviours/src/ROSAdapter.cpp | 23 +++++++++++------------ 1 file changed, 11 insertions(+), 12 deletions(-) diff --git a/src/behaviours/src/ROSAdapter.cpp b/src/behaviours/src/ROSAdapter.cpp index dd70b2fb..bf0a1426 100644 --- a/src/behaviours/src/ROSAdapter.cpp +++ b/src/behaviours/src/ROSAdapter.cpp @@ -20,7 +20,6 @@ #include #include #include -#include #include "swarmie_msgs/Waypoint.h" #include "swarmie_msgs/VirtualFence.h" @@ -165,17 +164,17 @@ tf::TransformListener *tfListener; void sigintEventHandler(int signal); //Callback handlers -void joyCmdHandler(const sensor_msgs::Joy::ConstPtr& message); -void modeHandler(const std_msgs::UInt8::ConstPtr& message); -void targetHandler(const apriltags_ros::AprilTagDetectionArray::ConstPtr& tagInfo); -void odometryHandler(const nav_msgs::Odometry::ConstPtr& message); -void mapHandler(const nav_msgs::Odometry::ConstPtr& message); -void virtualFenceHandler(const swarmie_msgs::VirtualFence& message); -void manualWaypointHandler(const swarmie_msgs::Waypoint& message); -void behaviourStateMachine(const ros::TimerEvent&); -void publishStatusTimerEventHandler(const ros::TimerEvent& event); -void publishHeartBeatTimerEventHandler(const ros::TimerEvent& event); -void sonarHandler(const sensor_msgs::Range::ConstPtr& sonarLeft, const sensor_msgs::Range::ConstPtr& sonarCenter, const sensor_msgs::Range::ConstPtr& sonarRight); +void joyCmdHandler(const sensor_msgs::Joy::ConstPtr& message); //for joystick control +void modeHandler(const std_msgs::UInt8::ConstPtr& message); //for detecting which mode the robot needs to be in +void targetHandler(const apriltags_ros::AprilTagDetectionArray::ConstPtr& tagInfo); //receives and stores April Tag Data using the TAG class +void odometryHandler(const nav_msgs::Odometry::ConstPtr& message); //receives and stores ODOM information +void mapHandler(const nav_msgs::Odometry::ConstPtr& message); //receives and stores GPS information +void virtualFenceHandler(const swarmie_msgs::VirtualFence& message); //Used to set an invisible boundary for robots to keep them from traveling outside specific bounds +void manualWaypointHandler(const swarmie_msgs::Waypoint& message); //Receives a waypoint (from GUI) and sets the coordinates +void behaviourStateMachine(const ros::TimerEvent&); //Upper most state machine, calls logic controller to perform all actions +void publishStatusTimerEventHandler(const ros::TimerEvent& event); //Publishes "ONLINE" when rover is successfully connected +void publishHeartBeatTimerEventHandler(const ros::TimerEvent& event); +void sonarHandler(const sensor_msgs::Range::ConstPtr& sonarLeft, const sensor_msgs::Range::ConstPtr& sonarCenter, const sensor_msgs::Range::ConstPtr& sonarRight); //handles ultrasound data and stores data // Converts the time passed as reported by ROS (which takes Gazebo simulation rate into account) into milliseconds as an integer. long int getROSTimeInMilliSecs();