diff --git a/README.md b/README.md index 8723bd0..b2b3f98 100644 --- a/README.md +++ b/README.md @@ -78,7 +78,7 @@ cd .. && catkin_make -DCMAKE_BUILD_TYPE=Release * Via the default way of cmake creating a build folder: ``` -git clone https://github.com/fada-catec/piloting_grcs && cd piloting_gcs +git clone https://github.com/fada-catec/piloting_grcs && cd piloting_grcs mkdir build && cd build && cmake .. -DCMAKE_BUILD_TYPE=Release diff --git a/changelog.txt b/changelog.txt index 4cb2968..ff031d6 100644 --- a/changelog.txt +++ b/changelog.txt @@ -1,3 +1,19 @@ +---------------------------------------------- +-- Changelog for version 1.0.4 (2023-07-05) -- +---------------------------------------------- +The changes of this version are small improvements after the release of the code and a the fix of a bug in the representation of the Inspection Areas. + +* In DDHL, new fields have been added in the Inspection Location properties to define the Inspection Plan orientation in the global + reference frame: quatW, quatX, quatY and quatZ. These fields are read instead of the "repr" matrix to represent the Inspection Area. + +* When using "Ctrl + Arrows Keys" to edit a waypoint manually in route editing mode the last one was moved. Now the one that is selected + in the gRCS is edited. + +* Added server response to the logging message when POST call fails during post-mission data upload. + +---------------------------------------------------------------------------------------------------------------------------------------------- +---------------------------------------------------------------------------------------------------------------------------------------------- + ---------------------------------------------- -- Changelog for version 1.0.3 (2023-05-21) -- ---------------------------------------------- diff --git a/cmake_stuff/versions.cmake b/cmake_stuff/versions.cmake index 3b8f455..6c6ed5c 100644 --- a/cmake_stuff/versions.cmake +++ b/cmake_stuff/versions.cmake @@ -1,4 +1,4 @@ -SET(VERSION "1.0.3") +SET(VERSION "1.0.4") STRING(REGEX MATCHALL "[0-9]" VERSION_PARTS "${VERSION}") LIST(GET VERSION_PARTS 0 VERSION_MAJOR) LIST(GET VERSION_PARTS 1 VERSION_MINOR) diff --git a/data/icons/cloud-icon.png b/data/icons/cloud-icon.png deleted file mode 100644 index ebfc5ce..0000000 Binary files a/data/icons/cloud-icon.png and /dev/null differ diff --git a/data/icons/config-visualiz-icon-2.png b/data/icons/config-visualiz-icon-2.png deleted file mode 100644 index 14a217e..0000000 Binary files a/data/icons/config-visualiz-icon-2.png and /dev/null differ diff --git a/data/icons/config-visualiz-icon.png b/data/icons/config-visualiz-icon.png index bccbe6b..14a217e 100644 Binary files a/data/icons/config-visualiz-icon.png and b/data/icons/config-visualiz-icon.png differ diff --git a/package.xml b/package.xml index cab730c..8fff5fa 100644 --- a/package.xml +++ b/package.xml @@ -2,7 +2,7 @@ piloting_grcs - 1.0.3 + 1.0.4 The piloting_grcs package Marco A. Montes Grova diff --git a/src/communications/DDHL/JSon2DTO.cpp b/src/communications/DDHL/JSon2DTO.cpp index 147422f..be10500 100644 --- a/src/communications/DDHL/JSon2DTO.cpp +++ b/src/communications/DDHL/JSon2DTO.cpp @@ -176,34 +176,20 @@ QList JSon2DTO::convertInspectionTaskJsonArrayToList(const QJ inspTaskDTO.getLocationType() = InspectionTaskLocationType::POINT; } else if (QString::compare(readedType, "InspectionArea", Qt::CaseInsensitive) == 0) { inspTaskDTO.getLocationType() = InspectionTaskLocationType::AREA; - _jsonKeys.checkIfValidInspTaskAreaDimensions(inspTaskDimensionsJO); + _jsonKeys.checkIfValidInspTaskAreaParams(inspTaskDimensionsJO); inspTaskDTO.getWidth() = inspTaskDimensionsJO["width"].toDouble(); inspTaskDTO.getDepth() = inspTaskDimensionsJO["depth"].toDouble(); inspTaskDTO.getHeight() = inspTaskDimensionsJO["height"].toDouble(); - const QJsonObject reprJO = inspTaskDimensionsJO["repr"].toObject(); - _jsonKeys.checkKeyExist(reprJO, "matrix"); - - const QJsonArray matrixJA = reprJO["matrix"].toArray(); - if (matrixJA.size() != 16) { - throw std::runtime_error("Invalid repr matrix size"); - } - - /// \note. Column-major order arrived matrix - Eigen::Matrix3f m(3, 3); - m << matrixJA[0].toDouble(), matrixJA[4].toDouble(), matrixJA[8].toDouble(), matrixJA[1].toDouble(), - matrixJA[5].toDouble(), matrixJA[9].toDouble(), matrixJA[2].toDouble(), matrixJA[6].toDouble(), - matrixJA[10].toDouble(); - Eigen::Quaternionf q(m); - inspTaskDTO.getOrientation().getX() = q.x(); - inspTaskDTO.getOrientation().getY() = q.y(); - inspTaskDTO.getOrientation().getZ() = q.z(); - inspTaskDTO.getOrientation().getW() = q.w(); + inspTaskDTO.getOrientation().getW() = inspTaskDimensionsJO["quatW"].toDouble(); + inspTaskDTO.getOrientation().getX() = inspTaskDimensionsJO["quatX"].toDouble(); + inspTaskDTO.getOrientation().getY() = inspTaskDimensionsJO["quatY"].toDouble(); + inspTaskDTO.getOrientation().getZ() = inspTaskDimensionsJO["quatZ"].toDouble(); } else { throw std::runtime_error("Invalid inspection task type readed from DDHL"); } - _jsonKeys.checkIfValidInspTaskPointDimensions(inspTaskDimensionsJO); + _jsonKeys.checkIfValidInspTaskPointParams(inspTaskDimensionsJO); inspTaskDTO.getPosition().getX() = inspTaskDimensionsJO["posX"].toDouble(); inspTaskDTO.getPosition().getY() = inspTaskDimensionsJO["posY"].toDouble(); inspTaskDTO.getPosition().getZ() = inspTaskDimensionsJO["posZ"].toDouble(); diff --git a/src/communications/DDHL/JSonKeys.cpp b/src/communications/DDHL/JSonKeys.cpp index ab1f7ff..b9f305f 100644 --- a/src/communications/DDHL/JSonKeys.cpp +++ b/src/communications/DDHL/JSonKeys.cpp @@ -61,19 +61,22 @@ void JSonKeys::checkIfValidInspTaskLocation(const QJsonObject& inspTaskLocation) checkKeyExist(inspTaskLocation, "Properties"); } -void JSonKeys::checkIfValidInspTaskAreaDimensions(const QJsonObject& inspTaskAreaDimensions) +void JSonKeys::checkIfValidInspTaskAreaParams(const QJsonObject& inspTaskAreaParams) { - checkKeyExist(inspTaskAreaDimensions, "width"); - checkKeyExist(inspTaskAreaDimensions, "depth"); - checkKeyExist(inspTaskAreaDimensions, "height"); - checkKeyExist(inspTaskAreaDimensions, "repr"); + checkKeyExist(inspTaskAreaParams, "width"); + checkKeyExist(inspTaskAreaParams, "depth"); + checkKeyExist(inspTaskAreaParams, "height"); + checkKeyExist(inspTaskAreaParams, "quatW"); + checkKeyExist(inspTaskAreaParams, "quatX"); + checkKeyExist(inspTaskAreaParams, "quatY"); + checkKeyExist(inspTaskAreaParams, "quatZ"); } -void JSonKeys::checkIfValidInspTaskPointDimensions(const QJsonObject& inspTaskPointDimensions) +void JSonKeys::checkIfValidInspTaskPointParams(const QJsonObject& inspTaskPointParams) { - checkKeyExist(inspTaskPointDimensions, "posX"); - checkKeyExist(inspTaskPointDimensions, "posY"); - checkKeyExist(inspTaskPointDimensions, "posZ"); + checkKeyExist(inspTaskPointParams, "posX"); + checkKeyExist(inspTaskPointParams, "posY"); + checkKeyExist(inspTaskPointParams, "posZ"); } void JSonKeys::checkIfValidInspTaskType(const QJsonObject& inspTaskType) diff --git a/src/communications/DDHL/JSonKeys.h b/src/communications/DDHL/JSonKeys.h index 317410e..13fb797 100644 --- a/src/communications/DDHL/JSonKeys.h +++ b/src/communications/DDHL/JSonKeys.h @@ -19,8 +19,8 @@ class JSonKeys : public QObject void checkIfValidInspPlan(const QJsonObject&); void checkIfValidInspTask(const QJsonObject&); void checkIfValidInspTaskLocation(const QJsonObject&); - void checkIfValidInspTaskAreaDimensions(const QJsonObject&); - void checkIfValidInspTaskPointDimensions(const QJsonObject&); + void checkIfValidInspTaskAreaParams(const QJsonObject&); + void checkIfValidInspTaskPointParams(const QJsonObject&); void checkIfValidInspTaskType(const QJsonObject&); void checkIfValidAsset(const QJsonObject&); void checkIfValidSite(const QJsonObject&); diff --git a/src/communications/DDHL/RestApiClient.cpp b/src/communications/DDHL/RestApiClient.cpp index 594b9e1..9d3c083 100644 --- a/src/communications/DDHL/RestApiClient.cpp +++ b/src/communications/DDHL/RestApiClient.cpp @@ -177,7 +177,8 @@ void RestApiClient::handlePostResult(HttpRequestWorker* worker) QLOG_INFO() << __PRETTY_FUNCTION__ << "Post request response: " << worker->response; if (worker->error_type != QNetworkReply::NoError) { - throw std::runtime_error(worker->error_str.toStdString()); + throw std::runtime_error( + worker->error_str.toStdString() + " | Server Response: " + worker->response.toStdString()); } QJsonParseError* e = nullptr; diff --git a/src/gui/MainWindowUi.cpp b/src/gui/MainWindowUi.cpp index c85e8bf..3806153 100644 --- a/src/gui/MainWindowUi.cpp +++ b/src/gui/MainWindowUi.cpp @@ -92,11 +92,11 @@ MainWindowUi::MainWindowUi(QWidget* parent) : /// \note LeftDockWidgetArea setTelemetryTreeDock(Qt::LeftDockWidgetArea); setInspectionPlanDock(Qt::LeftDockWidgetArea); + setMissionDock(Qt::LeftDockWidgetArea); /// \note RightDockWidgetArea setRobotAlarmsDock(Qt::RightDockWidgetArea); setRobotControlDock(Qt::RightDockWidgetArea); - setMissionDock(Qt::RightDockWidgetArea); setHighLevelActionsDock(Qt::RightDockWidgetArea); /// \note BottomDockWidgetArea @@ -427,6 +427,8 @@ void MainWindowUi::manageCoreDTO(QSharedPointer idto) _inspectionPlanWidget.setInspectionPlan(dto); _waypointEditorWidget.updateInspectionTaskList(dto->getInspectionTaskList()); + + updateMissionButtons(); } break; } @@ -1163,7 +1165,6 @@ void MainWindowUi::updateMissionButtons() const auto hasRoutes = _guiDTO.getHasRoutes(); const auto isConnected = _guiDTO.getCommsIsConnected(); const auto isEditMode = _ui->editModeAct->isChecked(); - // const auto hasInspPlan = !_guiDTO.getInspectionPlan().getInspectionTaskList().isEmpty(); const auto hasInspPlan = !_guiDTO.getInspectionPlan().getUUID().isEmpty(); _ui->saveCurrWptsListAct->setEnabled(hasInspPlan && hasRoutes); diff --git a/src/gui/widgets/robotAlarms/RobotAlarmsWidget.cpp b/src/gui/widgets/robotAlarms/RobotAlarmsWidget.cpp index ef3452a..8c7ec8c 100644 --- a/src/gui/widgets/robotAlarms/RobotAlarmsWidget.cpp +++ b/src/gui/widgets/robotAlarms/RobotAlarmsWidget.cpp @@ -140,7 +140,7 @@ void RobotAlarmsWidget::update(const AlarmStatusDTO& alarmStatusDTO) const auto current_item = _items[alarmStatusDTO.getIndex()]; if (!current_item) { - QLOG_ERROR() << __PRETTY_FUNCTION__ << "Invalid alarm index to update"; + QLOG_ERROR() << __PRETTY_FUNCTION__ << "Invalid alarm index to update: " << alarmStatusDTO.getIndex(); return; } diff --git a/src/images.qrc b/src/images.qrc index 2b3fafd..7b928d9 100644 --- a/src/images.qrc +++ b/src/images.qrc @@ -4,10 +4,9 @@ ../data/icons/3d-view.png ../data/icons/checkbox-icon.png ../data/icons/checklist.png - ../data/icons/cloud-icon.png ../data/icons/cloud-cut.png ../data/icons/config.png - ../data/icons/config-visualiz-icon-2.png + ../data/icons/config-visualiz-icon.png ../data/icons/connect.png ../data/icons/ddhl-config.png ../data/icons/disconnect.png @@ -52,6 +51,7 @@ ../data/svg/pfd/pfd_hsi_marks.svg ../data/svg/pfd/pfd_mask.svg + ../data/styles/indoor_style.qss ../data/styles/outdoor_style.qss diff --git a/src/maps/data_types/tools/QMouseTool.cpp b/src/maps/data_types/tools/QMouseTool.cpp index c122b46..efcb37d 100644 --- a/src/maps/data_types/tools/QMouseTool.cpp +++ b/src/maps/data_types/tools/QMouseTool.cpp @@ -227,17 +227,24 @@ int QMouseTool::processKeyEvent(QKeyEvent* event, rviz::RenderPanel* panel) return Render; } - if (_mapDTO.getVisibleWaypointsList().isEmpty()) { + /// \note. Get selected waypoint + const auto it = std::find_if( + _mapDTO.getVisibleWaypointsList().cbegin(), + _mapDTO.getVisibleWaypointsList().cend(), + [&](const QSharedPointer wp) { return wp->isSelectedWaypoint(); }); + if (it == _mapDTO.getVisibleWaypointsList().cend()) { + QLOG_WARN() << __PRETTY_FUNCTION__ << " - " + << "Unable to move waypoint. No waypoint selected"; return Render; } - float targetAngle; - const auto waypoint_id = _mapDTO.getVisibleWaypointsList().back()->id(); - auto waypoint_pt = _mapDTO.getVisibleWaypointsList().back()->waypoint().getPosition(); + const auto waypoint_id = (*it)->id(); + const auto waypoint_pt = (*it)->waypoint().getPosition(); Ogre::Vector3 updated_waypoint(waypoint_pt.getX(), waypoint_pt.getY(), waypoint_pt.getZ()); /// \note. To be able to move waypoint using arrow keys, must be pressed Control - int visible = false; + float targetAngle; + int visible = false; if (event->type() == QKeyEvent::KeyPress && event->modifiers() & Qt::ControlModifier) { visible = true; const float movementIncrement = 0.1f;