From aa937e2da26eb7325b5df1df4c54f0dd78fe11f9 Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Wed, 26 Aug 2026 23:37:20 +0100 Subject: [PATCH 1/4] chore: add pre commit, clang format and clang tidy Signed-off-by: Beniamino Pozzan --- .clang-format | 173 ++++++++++++++++++++++++++++++++++++++++ .clang-tidy | 100 +++++++++++++++++++++++ .pre-commit-config.yaml | 48 +++++++++++ 3 files changed, 321 insertions(+) create mode 100644 .clang-format create mode 100644 .clang-tidy create mode 100644 .pre-commit-config.yaml diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..ed3c76c --- /dev/null +++ b/.clang-format @@ -0,0 +1,173 @@ +--- +Language: Cpp +# BasedOnStyle: Google +AccessModifierOffset: -1 +AlignAfterOpenBracket: Align +AlignConsecutiveMacros: false +AlignConsecutiveAssignments: false +AlignConsecutiveDeclarations: false +AlignEscapedNewlines: Left +AlignOperands: true +AlignTrailingComments: true +AllowAllArgumentsOnNextLine: true +AllowAllConstructorInitializersOnNextLine: true +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortBlocksOnASingleLine: Never +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: InlineOnly +AllowShortLambdasOnASingleLine: All +AllowShortIfStatementsOnASingleLine: WithoutElse +AllowShortLoopsOnASingleLine: true +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: true +AlwaysBreakTemplateDeclarations: Yes +BinPackArguments: true +BinPackParameters: true +BraceWrapping: + AfterCaseLabel: false + AfterClass: false + AfterControlStatement: false + AfterEnum: false + AfterFunction: true + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + AfterExternBlock: false + BeforeCatch: false + BeforeElse: false + IndentBraces: false + SplitEmptyFunction: true + SplitEmptyRecord: true + SplitEmptyNamespace: true +BreakBeforeBinaryOperators: None +BreakBeforeBraces: Custom +BreakBeforeInheritanceComma: false +BreakInheritanceList: BeforeColon +BreakBeforeTernaryOperators: true +BreakConstructorInitializersBeforeComma: false +BreakConstructorInitializers: BeforeColon +BreakAfterJavaFieldAnnotations: false +BreakStringLiterals: true +ColumnLimit: 100 +CommentPragmas: '^ IWYU pragma:' +CompactNamespaces: false +ConstructorInitializerAllOnOneLineOrOnePerLine: true +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: true +DeriveLineEnding: true +DisableFormat: false +ExperimentalAutoDetectBinPacking: false +FixNamespaceComments: true +ForEachMacros: + - foreach + - Q_FOREACH + - BOOST_FOREACH +IncludeBlocks: Regroup +IncludeCategories: + - Regex: '^' + Priority: 2 + SortPriority: 0 + - Regex: '^<.*\.h>' + Priority: 1 + SortPriority: 0 + - Regex: '^<.*' + Priority: 2 + SortPriority: 0 + - Regex: '.*' + Priority: 3 + SortPriority: 0 +IncludeIsMainRegex: '([-_](test|unittest))?$' +IncludeIsMainSourceRegex: '' +IndentCaseLabels: true +IndentGotoLabels: true +IndentPPDirectives: None +IndentWidth: 2 +IndentWrappedFunctionNames: false +JavaScriptQuotes: Leave +JavaScriptWrapImports: true +KeepEmptyLinesAtTheStartOfBlocks: false +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCBinPackProtocolList: Never +ObjCBlockIndentWidth: 2 +ObjCSpaceAfterProperty: false +ObjCSpaceBeforeProtocolList: true +PenaltyBreakAssignment: 2 +PenaltyBreakBeforeFirstCallParameter: 1 +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakString: 1000 +PenaltyBreakTemplateDeclaration: 10 +PenaltyExcessCharacter: 1000000 +PenaltyReturnTypeOnItsOwnLine: 200 +PointerAlignment: Left +RawStringFormats: + - Language: Cpp + Delimiters: + - cc + - CC + - cpp + - Cpp + - CPP + - 'c++' + - 'C++' + CanonicalDelimiter: '' + BasedOnStyle: google + - Language: TextProto + Delimiters: + - pb + - PB + - proto + - PROTO + EnclosingFunctions: + - EqualsProto + - EquivToProto + - PARSE_PARTIAL_TEXT_PROTO + - PARSE_TEST_PROTO + - PARSE_TEXT_PROTO + - ParseTextOrDie + - ParseTextProtoOrDie + CanonicalDelimiter: '' + BasedOnStyle: google +ReflowComments: true +SortIncludes: true +SortUsingDeclarations: true +SpaceAfterCStyleCast: false +SpaceAfterLogicalNot: false +SpaceAfterTemplateKeyword: true +SpaceBeforeAssignmentOperators: true +SpaceBeforeCpp11BracedList: false +SpaceBeforeCtorInitializerColon: true +SpaceBeforeInheritanceColon: true +SpaceBeforeParens: ControlStatements +SpaceBeforeRangeBasedForLoopColon: true +SpaceInEmptyBlock: false +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 2 +SpacesInAngles: false +SpacesInConditionalStatement: false +SpacesInContainerLiterals: true +SpacesInCStyleCastParentheses: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +SpaceBeforeSquareBrackets: false +Standard: Latest +StatementMacros: + - Q_UNUSED + - QT_REQUIRE_VERSION +TabWidth: 8 +UseCRLF: false +UseTab: Never +--- +Language: Json +BasedOnStyle: llvm +--- +Language: JavaScript +BasedOnStyle: Google +ColumnLimit: 100 +... diff --git a/.clang-tidy b/.clang-tidy new file mode 100644 index 0000000..cf7fa67 --- /dev/null +++ b/.clang-tidy @@ -0,0 +1,100 @@ +--- +Checks: > + -*, + bugprone-*, + google-*, + misc-*, + modernize-*, + performance-*, + portability-*, + readability-*, + -bugprone-infinite-loop, + -google-readability-braces-around-statements, + -google-readability-todo, + -google-readability-namespace-comments, + -google-runtime-references, + -misc-non-private-member-variables-in-classes, + -misc-unused-parameters, + -misc-no-recursion, + -misc-confusable-identifiers, + -misc-include-cleaner, + -modernize-return-braced-init-list, + -modernize-use-trailing-return-type, + -modernize-avoid-c-arrays, + -modernize-use-auto, + -modernize-use-nodiscard, + -performance-enum-size, + -performance-move-const-arg, + -portability-avoid-pragma-once, + -portability-template-virtual-member-function, # from rclcpp + -readability-braces-around-statements, + -readability-convert-member-functions-to-static, + -readability-enum-initial-value, + -readability-function-cognitive-complexity, + -readability-identifier-length, + -readability-implicit-bool-conversion, + -readability-magic-numbers, + -readability-make-member-function-const, + -readability-math-missing-parentheses, + -readability-named-parameter, + -readability-redundant-declaration, + -readability-redundant-inline-specifier, + -readability-redundant-member-init, + -bugprone-narrowing-conversions, + -bugprone-easily-swappable-parameters, + -bugprone-exception-escape, + -bugprone-implicit-widening-of-multiplication-result +WarningsAsErrors: '*' +HeaderFilterRegex: '' +FormatStyle: google +CheckOptions: + - key: readability-identifier-naming.ClassCase + value: CamelCase + - key: readability-identifier-naming.ClassMemberCase + value: lower_case + - key: readability-identifier-naming.ConstexprVariableCase + value: CamelCase + - key: readability-identifier-naming.ConstexprVariablePrefix + value: k + - key: readability-identifier-naming.EnumCase + value: CamelCase + - key: readability-identifier-naming.EnumConstantCase + value: CamelCase + - key: readability-identifier-naming.FunctionCase + value: camelBack + - key: readability-identifier-naming.GlobalConstantCase + value: CamelCase + - key: readability-identifier-naming.GlobalConstantPrefix + value: k + - key: readability-identifier-naming.StaticConstantCase + value: CamelCase + - key: readability-identifier-naming.StaticConstantPrefix + value: k + - key: readability-identifier-naming.StaticVariableCase + value: lower_case + - key: readability-identifier-naming.MacroDefinitionCase + value: UPPER_CASE + - key: readability-identifier-naming.MacroDefinitionIgnoredRegexp + value: '^[A-Z]+(_[A-Z]+)*_$' + - key: readability-identifier-naming.MemberCase + value: lower_case + - key: readability-identifier-naming.PrivateMemberPrefix + value: _ + - key: readability-identifier-naming.ProtectedMemberPrefix + value: _ + - key: readability-identifier-naming.PublicMemberPrefix + value: '' + - key: readability-identifier-naming.NamespaceCase + value: lower_case + - key: readability-identifier-naming.ParameterCase + value: lower_case + - key: readability-identifier-naming.TypeAliasCase + value: CamelCase + - key: readability-identifier-naming.TypedefCase + value: CamelCase + - key: readability-identifier-naming.VariableCase + value: lower_case + - key: readability-identifier-naming.IgnoreMainLikeFunctions + value: 1 + - key: readability-uppercase-literal-suffix.NewSuffixes + value: 'L;LL;LU;LLU;U;UL;ULL' diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000..ddc1487 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,48 @@ +# To use: +# +# pre-commit run -a +# +# Or: +# +# pre-commit install # (runs every time you commit in git) +# +# To update this file: +# +# pre-commit autoupdate +# +# See https://github.com/pre-commit/pre-commit + +repos: + # Standard hooks + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v4.3.0 + hooks: + - id: check-added-large-files + - id: check-ast + - id: check-case-conflict + - id: check-docstring-first + - id: check-merge-conflict + - id: check-symlinks + - id: check-xml + - id: check-yaml + args: [--allow-multiple-documents] + - id: debug-statements + - id: mixed-line-ending + - id: trailing-whitespace + exclude_types: [rst] + - id: fix-byte-order-marker + + # Python hooks + - repo: https://github.com/astral-sh/ruff-pre-commit + rev: v0.14.11 + hooks: + - id: ruff-check + args: [ --fix ] + - id: ruff-format + exclude: ^scripts/run-clang-tidy.py$ + + # CPP hooks + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: 'v21.1.5' + hooks: + - id: clang-format From 7c5a8d52f25903e008542100973fbd4781f7572a Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Wed, 26 Aug 2026 23:45:03 +0100 Subject: [PATCH 2/4] chore: run first clang-format Signed-off-by: Beniamino Pozzan --- .devcontainer/linux/devcontainer.json | 53 +- .devcontainer/nogui/devcontainer.json | 27 +- .devcontainer/nvidia/devcontainer.json | 61 +- foxglove/px4_roscon_workshop.json | 0 .../aruco_tracker/ArucoTracker.cpp | 372 +++++------ .../aruco_tracker/ArucoTracker.hpp | 49 +- .../custom_executor_demo/CustomMode.cpp | 144 ++--- .../custom_executor_demo/CustomMode.hpp | 79 ++- .../CustomModeExecutor.cpp | 117 ++-- .../CustomModeExecutor.hpp | 35 +- .../custom_mode_demo/CustomMode.cpp | 80 +-- .../custom_mode_demo/CustomMode.hpp | 34 +- .../custom_mode_demo/solution.cpp | 120 ++-- .../include/formation_control.hpp | 28 +- .../include/formation_executor_solution.hpp | 18 +- .../formation_control/src/exercise.cpp | 6 +- .../src/formation_control.cpp | 21 +- .../formation_control/src/main.cpp | 6 +- .../formation_control/src/solution.cpp | 22 +- .../precision_land/PrecisionLand.cpp | 557 +++++++++-------- .../precision_land/PrecisionLand.hpp | 181 +++--- .../CustomModeExecutor_v2.cpp | 109 ++-- .../CustomModeExecutor_v2.hpp | 33 +- .../precision_land_executor/CustomMode_v2.cpp | 584 +++++++++--------- .../precision_land_executor/CustomMode_v2.hpp | 217 ++++--- .../include/px4_tf/px4_tf_publisher_node.hpp | 24 +- .../px4_tf/src/px4_tf_publisher.cpp | 2 +- .../px4_tf/src/px4_tf_publisher_node.cpp | 108 ++-- px4_roscon_workshop/rover_teleop/Teleop.cpp | 134 ++-- px4_roscon_workshop/rover_teleop/Teleop.hpp | 40 +- px4_roscon_workshop/teleop/Teleop.cpp | 122 ++-- px4_roscon_workshop/teleop/Teleop.hpp | 52 +- px4_roscon_workshop/teleop/TeleopExecutor.cpp | 117 ++-- px4_roscon_workshop/teleop/TeleopExecutor.hpp | 32 +- 34 files changed, 1796 insertions(+), 1788 deletions(-) mode change 100755 => 100644 foxglove/px4_roscon_workshop.json diff --git a/.devcontainer/linux/devcontainer.json b/.devcontainer/linux/devcontainer.json index 7bb3637..0f664bc 100644 --- a/.devcontainer/linux/devcontainer.json +++ b/.devcontainer/linux/devcontainer.json @@ -1,27 +1,30 @@ { - "name": "px4_roscon_workshop_linux", - "image": "dronecode/roscon-25-workshop:latest", - "forwardPorts": [8765], - "remoteEnv": { - "DISPLAY": "${localEnv:DISPLAY}" - }, - "runArgs": [ - "--device", "/dev/dri:/dev/dri" - ], - "mounts": [ - { - "source": "/tmp/.X11-unix", - "target": "/tmp/.X11-unix", - "type": "bind" - } - ], - "customizations": { - "vscode": { - "settings": { - "terminal.integrated.defaultProfile.linux": "bash" - } - } - }, - "workspaceMount": "source=${localWorkspaceFolder},target=/home/ubuntu/roscon-25-workshop_ws/src/roscon-25-workshop,type=bind", - "workspaceFolder": "/home/ubuntu/roscon-25-workshop_ws" + "name": "px4_roscon_workshop_linux", + "image": "dronecode/roscon-25-workshop:latest", + "forwardPorts": [ + 8765 + ], + "remoteEnv": { + "DISPLAY": "${localEnv:DISPLAY}" + }, + "runArgs": [ + "--device", + "/dev/dri:/dev/dri" + ], + "mounts": [ + { + "source": "/tmp/.X11-unix", + "target": "/tmp/.X11-unix", + "type": "bind" + } + ], + "customizations": { + "vscode": { + "settings": { + "terminal.integrated.defaultProfile.linux": "bash" + } + } + }, + "workspaceMount": "source=${localWorkspaceFolder},target=/home/ubuntu/roscon-25-workshop_ws/src/roscon-25-workshop,type=bind", + "workspaceFolder": "/home/ubuntu/roscon-25-workshop_ws" } \ No newline at end of file diff --git a/.devcontainer/nogui/devcontainer.json b/.devcontainer/nogui/devcontainer.json index 0c4fc93..0bc8917 100644 --- a/.devcontainer/nogui/devcontainer.json +++ b/.devcontainer/nogui/devcontainer.json @@ -1,14 +1,17 @@ { - "name": "px4_roscon_workshop_nogui", - "image": "dronecode/roscon-25-workshop:latest", - "forwardPorts": [8765, 18570], - "customizations": { - "vscode": { - "settings": { - "terminal.integrated.defaultProfile.linux": "bash" - } - } - }, - "workspaceMount": "source=${localWorkspaceFolder},target=/home/ubuntu/roscon-25-workshop_ws/src/roscon-25-workshop,type=bind", - "workspaceFolder": "/home/ubuntu/roscon-25-workshop_ws" + "name": "px4_roscon_workshop_nogui", + "image": "dronecode/roscon-25-workshop:latest", + "forwardPorts": [ + 8765, + 18570 + ], + "customizations": { + "vscode": { + "settings": { + "terminal.integrated.defaultProfile.linux": "bash" + } + } + }, + "workspaceMount": "source=${localWorkspaceFolder},target=/home/ubuntu/roscon-25-workshop_ws/src/roscon-25-workshop,type=bind", + "workspaceFolder": "/home/ubuntu/roscon-25-workshop_ws" } \ No newline at end of file diff --git a/.devcontainer/nvidia/devcontainer.json b/.devcontainer/nvidia/devcontainer.json index f9dfb88..a34a4e7 100644 --- a/.devcontainer/nvidia/devcontainer.json +++ b/.devcontainer/nvidia/devcontainer.json @@ -1,31 +1,34 @@ { - "name": "px4_roscon_workshop_nvidia", - "image": "dronecode/roscon-25-workshop:latest", - "forwardPorts": [8765], - "containerEnv": { - "NVIDIA_VISIBLE_DEVICES": "all", - "NVIDIA_DRIVER_CAPABILITIES": "all" - }, - "remoteEnv": { - "DISPLAY": "${localEnv:DISPLAY}" - }, - "runArgs": [ - "--runtime", "nvidia" - ], - "mounts": [ - { - "source": "/tmp/.X11-unix", - "target": "/tmp/.X11-unix", - "type": "bind" - } - ], - "customizations": { - "vscode": { - "settings": { - "terminal.integrated.defaultProfile.linux": "bash" - } - } - }, - "workspaceMount": "source=${localWorkspaceFolder},target=/home/ubuntu/roscon-25-workshop_ws/src/roscon-25-workshop,type=bind", - "workspaceFolder": "/home/ubuntu/roscon-25-workshop_ws" + "name": "px4_roscon_workshop_nvidia", + "image": "dronecode/roscon-25-workshop:latest", + "forwardPorts": [ + 8765 + ], + "containerEnv": { + "NVIDIA_VISIBLE_DEVICES": "all", + "NVIDIA_DRIVER_CAPABILITIES": "all" + }, + "remoteEnv": { + "DISPLAY": "${localEnv:DISPLAY}" + }, + "runArgs": [ + "--runtime", + "nvidia" + ], + "mounts": [ + { + "source": "/tmp/.X11-unix", + "target": "/tmp/.X11-unix", + "type": "bind" + } + ], + "customizations": { + "vscode": { + "settings": { + "terminal.integrated.defaultProfile.linux": "bash" + } + } + }, + "workspaceMount": "source=${localWorkspaceFolder},target=/home/ubuntu/roscon-25-workshop_ws/src/roscon-25-workshop,type=bind", + "workspaceFolder": "/home/ubuntu/roscon-25-workshop_ws" } \ No newline at end of file diff --git a/foxglove/px4_roscon_workshop.json b/foxglove/px4_roscon_workshop.json old mode 100755 new mode 100644 diff --git a/px4_roscon_workshop/aruco_tracker/ArucoTracker.cpp b/px4_roscon_workshop/aruco_tracker/ArucoTracker.cpp index 6966fb0..e9ac775 100644 --- a/px4_roscon_workshop/aruco_tracker/ArucoTracker.cpp +++ b/px4_roscon_workshop/aruco_tracker/ArucoTracker.cpp @@ -2,188 +2,197 @@ // ORIGINAL VERSION - ArucoTracker.cpp // ============================================================================ #include "ArucoTracker.hpp" -#include + #include #include + +#include #include -ArucoTrackerNode::ArucoTrackerNode() - : Node("aruco_tracker_node") +ArucoTrackerNode::ArucoTrackerNode() : Node("aruco_tracker_node") { - loadParameters(); + loadParameters(); - // TODO: params to adjust detector params - // See: https://docs.opencv.org/4.x/d1/dcd/structcv_1_1aruco_1_1DetectorParameters.html - _detector_params = cv::aruco::DetectorParameters::create(); + // TODO: params to adjust detector params + // See: https://docs.opencv.org/4.x/d1/dcd/structcv_1_1aruco_1_1DetectorParameters.html + _detector_params = cv::aruco::DetectorParameters::create(); - // See: https://docs.opencv.org/4.x/d1/d21/aruco__dictionary_8hpp.html - _dictionary = cv::aruco::getPredefinedDictionary(_param_dictionary); + // See: https://docs.opencv.org/4.x/d1/d21/aruco__dictionary_8hpp.html + _dictionary = cv::aruco::getPredefinedDictionary(_param_dictionary); - auto qos = rclcpp::QoS(1).best_effort(); + auto qos = rclcpp::QoS(1).best_effort(); - _image_sub = create_subscription( - "/camera", qos, std::bind(&ArucoTrackerNode::image_callback, this, std::placeholders::_1)); + _image_sub = create_subscription( + "/camera", qos, std::bind(&ArucoTrackerNode::image_callback, this, std::placeholders::_1)); - _camera_info_sub = create_subscription( - "/camera_info", qos, std::bind(&ArucoTrackerNode::camera_info_callback, this, std::placeholders::_1)); + _camera_info_sub = create_subscription( + "/camera_info", qos, + std::bind(&ArucoTrackerNode::camera_info_callback, this, std::placeholders::_1)); - // Publishers - _image_pub = create_publisher("/image_proc", qos); - _target_pose_pub = create_publisher("/target_pose", qos); + // Publishers + _image_pub = create_publisher("/image_proc", qos); + _target_pose_pub = create_publisher("/target_pose", qos); } void ArucoTrackerNode::loadParameters() { - declare_parameter("aruco_id", 0); - declare_parameter("dictionary", 2); // DICT_4X4_250 - declare_parameter("marker_size", 0.5); + declare_parameter("aruco_id", 0); + declare_parameter("dictionary", 2); // DICT_4X4_250 + declare_parameter("marker_size", 0.5); - get_parameter("aruco_id", _param_aruco_id); - get_parameter("dictionary", _param_dictionary); - get_parameter("marker_size", _param_marker_size); + get_parameter("aruco_id", _param_aruco_id); + get_parameter("dictionary", _param_dictionary); + get_parameter("marker_size", _param_marker_size); } void ArucoTrackerNode::image_callback(const sensor_msgs::msg::Image::SharedPtr msg) { - try { - // Convert ROS image message to OpenCV image - cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); - - // Detect markers - std::vector ids; - std::vector> corners; - cv::aruco::detectMarkers(cv_ptr->image, _dictionary, corners, ids, _detector_params); - cv::aruco::drawDetectedMarkers(cv_ptr->image, corners, ids); - - if (!_camera_matrix.empty() && !_dist_coeffs.empty()) { - - std::vector> undistortedCorners; - - for (const auto& corner : corners) { - std::vector undistortedCorner; - cv::undistortPoints(corner, undistortedCorner, _camera_matrix, _dist_coeffs, cv::noArray(), _camera_matrix); - undistortedCorners.push_back(undistortedCorner); - } - - for (size_t i = 0; i < ids.size(); i++) { - if (ids[i] != _param_aruco_id) { - continue; - } - - // Calculate marker size from camera intrinsics - float half_size = _param_marker_size / 2.0f; - std::vector objectPoints = { - cv::Point3f(-half_size, half_size, 0), // top left - cv::Point3f(half_size, half_size, 0), // top right - cv::Point3f(half_size, -half_size, 0), // bottom right - cv::Point3f(-half_size, -half_size, 0) // bottom left - }; - - // Use PnP solver to estimate pose - cv::Vec3d rvec, tvec; - cv::solvePnP(objectPoints, undistortedCorners[i], _camera_matrix, cv::noArray(), rvec, tvec); - // Annotate the image - cv::drawFrameAxes(cv_ptr->image, _camera_matrix, cv::noArray(), rvec, tvec, _param_marker_size); - - // Quaternion from rotation matrix - cv::Mat rot_mat; - cv::Rodrigues(rvec, rot_mat); - tf2::Matrix3x3 tf_rotation( - rot_mat.at(0, 0), rot_mat.at(0, 1), rot_mat.at(0, 2), - rot_mat.at(1, 0), rot_mat.at(1, 1), rot_mat.at(1, 2), - rot_mat.at(2, 0), rot_mat.at(2, 1), rot_mat.at(2, 2)); - tf2::Quaternion quat; - tf_rotation.getRotation(quat); - quat.normalize(); - - // Publish target pose - geometry_msgs::msg::PoseStamped pose_msg; - pose_msg.header.stamp = msg->header.stamp; - pose_msg.header.frame_id = "camera_frame"; - pose_msg.pose.position.x = tvec[0]; - pose_msg.pose.position.y = tvec[1]; - pose_msg.pose.position.z = tvec[2]; - pose_msg.pose.orientation = tf2::toMsg(quat); - - _target_pose_pub->publish(pose_msg); - - // Annotate the image - annotate_image(cv_ptr, tvec); - - // NOTE: we break here, meaning we only publish the pose of the first target we see - break; - } - - } else { - RCLCPP_ERROR(get_logger(), "Missing camera calibration"); - } - - // Always publish image - cv_bridge::CvImage out_msg; - out_msg.header = msg->header; - out_msg.encoding = sensor_msgs::image_encodings::BGR8; - out_msg.image = cv_ptr->image; - _image_pub->publish(*out_msg.toImageMsg().get()); - - } catch (const cv_bridge::Exception& e) { - RCLCPP_ERROR(get_logger(), "cv_bridge exception: %s", e.what()); - } + try { + // Convert ROS image message to OpenCV image + cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); + + // Detect markers + std::vector ids; + std::vector> corners; + cv::aruco::detectMarkers(cv_ptr->image, _dictionary, corners, ids, _detector_params); + cv::aruco::drawDetectedMarkers(cv_ptr->image, corners, ids); + + if (!_camera_matrix.empty() && !_dist_coeffs.empty()) { + std::vector> undistortedCorners; + + for (const auto& corner : corners) { + std::vector undistortedCorner; + cv::undistortPoints(corner, undistortedCorner, _camera_matrix, _dist_coeffs, cv::noArray(), + _camera_matrix); + undistortedCorners.push_back(undistortedCorner); + } + + for (size_t i = 0; i < ids.size(); i++) { + if (ids[i] != _param_aruco_id) { + continue; + } + + // Calculate marker size from camera intrinsics + float half_size = _param_marker_size / 2.0f; + std::vector objectPoints = { + cv::Point3f(-half_size, half_size, 0), // top left + cv::Point3f(half_size, half_size, 0), // top right + cv::Point3f(half_size, -half_size, 0), // bottom right + cv::Point3f(-half_size, -half_size, 0) // bottom left + }; + + // Use PnP solver to estimate pose + cv::Vec3d rvec, tvec; + cv::solvePnP(objectPoints, undistortedCorners[i], _camera_matrix, cv::noArray(), rvec, + tvec); + // Annotate the image + cv::drawFrameAxes(cv_ptr->image, _camera_matrix, cv::noArray(), rvec, tvec, + _param_marker_size); + + // Quaternion from rotation matrix + cv::Mat rot_mat; + cv::Rodrigues(rvec, rot_mat); + tf2::Matrix3x3 tf_rotation( + rot_mat.at(0, 0), rot_mat.at(0, 1), rot_mat.at(0, 2), + rot_mat.at(1, 0), rot_mat.at(1, 1), rot_mat.at(1, 2), + rot_mat.at(2, 0), rot_mat.at(2, 1), rot_mat.at(2, 2)); + tf2::Quaternion quat; + tf_rotation.getRotation(quat); + quat.normalize(); + + // Publish target pose + geometry_msgs::msg::PoseStamped pose_msg; + pose_msg.header.stamp = msg->header.stamp; + pose_msg.header.frame_id = "camera_frame"; + pose_msg.pose.position.x = tvec[0]; + pose_msg.pose.position.y = tvec[1]; + pose_msg.pose.position.z = tvec[2]; + pose_msg.pose.orientation = tf2::toMsg(quat); + + _target_pose_pub->publish(pose_msg); + + // Annotate the image + annotate_image(cv_ptr, tvec); + + // NOTE: we break here, meaning we only publish the pose of the first target we see + break; + } + + } else { + RCLCPP_ERROR(get_logger(), "Missing camera calibration"); + } + + // Always publish image + cv_bridge::CvImage out_msg; + out_msg.header = msg->header; + out_msg.encoding = sensor_msgs::image_encodings::BGR8; + out_msg.image = cv_ptr->image; + _image_pub->publish(*out_msg.toImageMsg().get()); + + } catch (const cv_bridge::Exception& e) { + RCLCPP_ERROR(get_logger(), "cv_bridge exception: %s", e.what()); + } } void ArucoTrackerNode::camera_info_callback(const sensor_msgs::msg::CameraInfo::SharedPtr msg) { - // Always update the camera matrix and distortion coefficients from the new message - _camera_matrix = cv::Mat(3, 3, CV_64F, const_cast(msg->k.data())).clone(); // Use clone to ensure a deep copy - _dist_coeffs = cv::Mat(msg->d.size(), 1, CV_64F, const_cast(msg->d.data())).clone(); // Use clone to ensure a deep copy - - // Log the first row of the camera matrix to verify correct values - RCLCPP_INFO(get_logger(), "Camera matrix updated:\n[%f, %f, %f]\n[%f, %f, %f]\n[%f, %f, %f]", - _camera_matrix.at(0, 0), _camera_matrix.at(0, 1), _camera_matrix.at(0, 2), - _camera_matrix.at(1, 0), _camera_matrix.at(1, 1), _camera_matrix.at(1, 2), - _camera_matrix.at(2, 0), _camera_matrix.at(2, 1), _camera_matrix.at(2, 2)); - RCLCPP_INFO(get_logger(), "Camera Matrix: fx=%f, fy=%f, cx=%f, cy=%f", - _camera_matrix.at(0, 0), // fx - _camera_matrix.at(1, 1), // fy - _camera_matrix.at(0, 2), // cx - _camera_matrix.at(1, 2) // cy - ); - - // Check if focal length is zero after update - if (_camera_matrix.at(0, 0) == 0) { - RCLCPP_ERROR(get_logger(), "Focal length is zero after update!"); - - } else { - RCLCPP_INFO(get_logger(), "Updated camera intrinsics from camera_info topic."); - - RCLCPP_INFO(get_logger(), "Unsubscribing from camera info topic"); - _camera_info_sub.reset(); - } + // Always update the camera matrix and distortion coefficients from the new message + _camera_matrix = cv::Mat(3, 3, CV_64F, const_cast(msg->k.data())) + .clone(); // Use clone to ensure a deep copy + _dist_coeffs = cv::Mat(msg->d.size(), 1, CV_64F, const_cast(msg->d.data())) + .clone(); // Use clone to ensure a deep copy + + // Log the first row of the camera matrix to verify correct values + RCLCPP_INFO(get_logger(), "Camera matrix updated:\n[%f, %f, %f]\n[%f, %f, %f]\n[%f, %f, %f]", + _camera_matrix.at(0, 0), _camera_matrix.at(0, 1), + _camera_matrix.at(0, 2), _camera_matrix.at(1, 0), + _camera_matrix.at(1, 1), _camera_matrix.at(1, 2), + _camera_matrix.at(2, 0), _camera_matrix.at(2, 1), + _camera_matrix.at(2, 2)); + RCLCPP_INFO(get_logger(), "Camera Matrix: fx=%f, fy=%f, cx=%f, cy=%f", + _camera_matrix.at(0, 0), // fx + _camera_matrix.at(1, 1), // fy + _camera_matrix.at(0, 2), // cx + _camera_matrix.at(1, 2) // cy + ); + + // Check if focal length is zero after update + if (_camera_matrix.at(0, 0) == 0) { + RCLCPP_ERROR(get_logger(), "Focal length is zero after update!"); + + } else { + RCLCPP_INFO(get_logger(), "Updated camera intrinsics from camera_info topic."); + + RCLCPP_INFO(get_logger(), "Unsubscribing from camera info topic"); + _camera_info_sub.reset(); + } } void ArucoTrackerNode::annotate_image(cv_bridge::CvImagePtr image, const cv::Vec3d& target) { - // Annotate the image with the target position and marker size - std::ostringstream stream; - stream << std::fixed << std::setprecision(2); - stream << "X: " << target[0] << " Y: " << target[1] << " Z: " << target[2]; - std::string text_xyz = stream.str(); - - int fontFace = cv::FONT_HERSHEY_SIMPLEX; - double fontScale = 1; - int thickness = 2; - int baseline = 0; - cv::Size textSize = cv::getTextSize(text_xyz, fontFace, fontScale, thickness, &baseline); - baseline += thickness; - cv::Point textOrg((image->image.cols - textSize.width - 10), (image->image.rows - 10)); - cv::putText(image->image, text_xyz, textOrg, fontFace, fontScale, cv::Scalar(0, 255, 255), thickness, 8); + // Annotate the image with the target position and marker size + std::ostringstream stream; + stream << std::fixed << std::setprecision(2); + stream << "X: " << target[0] << " Y: " << target[1] << " Z: " << target[2]; + std::string text_xyz = stream.str(); + + int fontFace = cv::FONT_HERSHEY_SIMPLEX; + double fontScale = 1; + int thickness = 2; + int baseline = 0; + cv::Size textSize = cv::getTextSize(text_xyz, fontFace, fontScale, thickness, &baseline); + baseline += thickness; + cv::Point textOrg((image->image.cols - textSize.width - 10), (image->image.rows - 10)); + cv::putText(image->image, text_xyz, textOrg, fontFace, fontScale, cv::Scalar(0, 255, 255), + thickness, 8); } int main(int argc, char** argv) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; } // // ============================================================================ // // ALTERNATIVE VERSION - For exercises @@ -208,14 +217,17 @@ int main(int argc, char** argv) // auto qos = rclcpp::QoS(1).best_effort(); // _image_sub = create_subscription( -// "/camera", qos, std::bind(&ArucoTrackerNode::image_callback, this, std::placeholders::_1)); +// "/camera", qos, std::bind(&ArucoTrackerNode::image_callback, this, +// std::placeholders::_1)); // _camera_info_sub = create_subscription( -// "/camera_info", qos, std::bind(&ArucoTrackerNode::camera_info_callback, this, std::placeholders::_1)); +// "/camera_info", qos, +// std::bind(&ArucoTrackerNode::camera_info_callback, this, std::placeholders::_1)); // _local_position_sub = create_subscription( // "fmu/out/vehicle_local_position", rclcpp::QoS(10).best_effort(), -// std::bind(&ArucoTrackerNode::local_position_callback, this, std::placeholders::_1)); +// std::bind(&ArucoTrackerNode::local_position_callback, this, +// std::placeholders::_1)); // // Publishers // _image_pub = create_publisher("/image_proc", qos); @@ -233,7 +245,8 @@ int main(int argc, char** argv) // get_parameter("marker_size", _param_marker_size); // } -// void ArucoTrackerNode::local_position_callback(const px4_msgs::msg::VehicleLocalPosition::SharedPtr msg) +// void ArucoTrackerNode::local_position_callback(const +// px4_msgs::msg::VehicleLocalPosition::SharedPtr msg) // { // // Extract distance to ground // _distance_to_ground = msg->dist_bottom; @@ -244,7 +257,8 @@ int main(int argc, char** argv) // { // try { // // Convert ROS image message to OpenCV image -// cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); +// cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, +// sensor_msgs::image_encodings::BGR8); // // Detect markers // std::vector ids; @@ -258,8 +272,8 @@ int main(int argc, char** argv) // for (const auto& corner : corners) { // std::vector undistortedCorner; -// cv::undistortPoints(corner, undistortedCorner, _camera_matrix, _dist_coeffs, cv::noArray(), _camera_matrix); -// undistortedCorners.push_back(undistortedCorner); +// cv::undistortPoints(corner, undistortedCorner, _camera_matrix, +// _dist_coeffs, cv::noArray(), _camera_matrix); undistortedCorners.push_back(undistortedCorner); // } // for (size_t i = 0; i < ids.size(); i++) { @@ -267,10 +281,12 @@ int main(int argc, char** argv) // continue; // } // // verify the marker size using the distance to ground -// float pixel_width = cv::norm(undistortedCorners[i][0] - undistortedCorners[i][1]); -// float focal_length = _camera_matrix.at(0, 0); -// _calculated_marker_size = (pixel_width / focal_length) * _distance_to_ground; -// // RCLCPP_INFO(get_logger(), "Calculated marker size: %f", _calculated_marker_size); +// float pixel_width = cv::norm(undistortedCorners[i][0] - +// undistortedCorners[i][1]); float focal_length = _camera_matrix.at(0, 0); +// _calculated_marker_size = (pixel_width / focal_length) * +// _distance_to_ground; +// // RCLCPP_INFO(get_logger(), "Calculated marker size: %f", +// _calculated_marker_size); // // Calculate marker size from camera intrinsics // float half_size = _param_marker_size / 2.0f; @@ -283,9 +299,11 @@ int main(int argc, char** argv) // // Use PnP solver to estimate pose // cv::Vec3d rvec, tvec; -// cv::solvePnP(objectPoints, undistortedCorners[i], _camera_matrix, cv::noArray(), rvec, tvec); +// cv::solvePnP(objectPoints, undistortedCorners[i], _camera_matrix, +// cv::noArray(), rvec, tvec); // // Annotate the image -// cv::drawFrameAxes(cv_ptr->image, _camera_matrix, cv::noArray(), rvec, tvec, _param_marker_size); +// cv::drawFrameAxes(cv_ptr->image, _camera_matrix, cv::noArray(), +// rvec, tvec, _param_marker_size); // // Quaternion from rotation matrix // cv::Mat rot_mat; @@ -309,8 +327,8 @@ int main(int argc, char** argv) // // Annotate the image // annotate_image(cv_ptr, tvec); -// // NOTE: we break here, meaning we only publish the pose of the first target we see -// break; +// // NOTE: we break here, meaning we only publish the pose of the +// first target we see break; // } // } else { @@ -332,16 +350,17 @@ int main(int argc, char** argv) // void ArucoTrackerNode::camera_info_callback(const sensor_msgs::msg::CameraInfo::SharedPtr msg) // { // // Always update the camera matrix and distortion coefficients from the new message -// _camera_matrix = cv::Mat(3, 3, CV_64F, const_cast(msg->k.data())).clone(); // Use clone to ensure a deep copy -// _dist_coeffs = cv::Mat(msg->d.size(), 1, CV_64F, const_cast(msg->d.data())).clone(); // Use clone to ensure a deep copy +// _camera_matrix = cv::Mat(3, 3, CV_64F, const_cast(msg->k.data())).clone(); // Use +// clone to ensure a deep copy _dist_coeffs = cv::Mat(msg->d.size(), 1, CV_64F, +// const_cast(msg->d.data())).clone(); // Use clone to ensure a deep copy // // Log the first row of the camera matrix to verify correct values -// RCLCPP_INFO(get_logger(), "Camera matrix updated:\n[%f, %f, %f]\n[%f, %f, %f]\n[%f, %f, %f]", -// _camera_matrix.at(0, 0), _camera_matrix.at(0, 1), _camera_matrix.at(0, 2), -// _camera_matrix.at(1, 0), _camera_matrix.at(1, 1), _camera_matrix.at(1, 2), -// _camera_matrix.at(2, 0), _camera_matrix.at(2, 1), _camera_matrix.at(2, 2)); -// RCLCPP_INFO(get_logger(), "Camera Matrix: fx=%f, fy=%f, cx=%f, cy=%f", -// _camera_matrix.at(0, 0), // fx +// RCLCPP_INFO(get_logger(), "Camera matrix updated:\n[%f, %f, %f]\n[%f, %f, %f]\n[%f, %f, +// %f]", _camera_matrix.at(0, 0), _camera_matrix.at(0, 1), +// _camera_matrix.at(0, 2), _camera_matrix.at(1, 0), _camera_matrix.at(1, +// 1), _camera_matrix.at(1, 2), _camera_matrix.at(2, 0), +// _camera_matrix.at(2, 1), _camera_matrix.at(2, 2)); RCLCPP_INFO(get_logger(), +// "Camera Matrix: fx=%f, fy=%f, cx=%f, cy=%f", _camera_matrix.at(0, 0), // fx // _camera_matrix.at(1, 1), // fy // _camera_matrix.at(0, 2), // cx // _camera_matrix.at(1, 2) // cy @@ -364,8 +383,8 @@ int main(int argc, char** argv) // // Annotate the image with the target position and marker size and calculated marker size // std::ostringstream stream; // stream << std::fixed << std::setprecision(2); -// stream << "X: " << target[0] << " Y: " << target[1] << " Z: " << target[2] << " M: " << _calculated_marker_size; -// std::string text_xyz = stream.str(); +// stream << "X: " << target[0] << " Y: " << target[1] << " Z: " << target[2] << " M: " << +// _calculated_marker_size; std::string text_xyz = stream.str(); // int fontFace = cv::FONT_HERSHEY_SIMPLEX; // double fontScale = 1; @@ -374,7 +393,8 @@ int main(int argc, char** argv) // cv::Size textSize = cv::getTextSize(text_xyz, fontFace, fontScale, thickness, &baseline); // baseline += thickness; // cv::Point textOrg((image->image.cols - textSize.width - 10), (image->image.rows - 10)); -// cv::putText(image->image, text_xyz, textOrg, fontFace, fontScale, cv::Scalar(0, 255, 255), thickness, 8); +// cv::putText(image->image, text_xyz, textOrg, fontFace, fontScale, cv::Scalar(0, 255, 255), +// thickness, 8); // } // int main(int argc, char** argv) diff --git a/px4_roscon_workshop/aruco_tracker/ArucoTracker.hpp b/px4_roscon_workshop/aruco_tracker/ArucoTracker.hpp index 8346d41..c9cc1bb 100644 --- a/px4_roscon_workshop/aruco_tracker/ArucoTracker.hpp +++ b/px4_roscon_workshop/aruco_tracker/ArucoTracker.hpp @@ -2,40 +2,39 @@ // ORIGINAL VERSION - ArucoTracker.hpp // ============================================================================ #pragma once +#include +#include #include +#include +#include #include -#include #include -#include -#include -#include -#include +#include -class ArucoTrackerNode : public rclcpp::Node -{ -public: - ArucoTrackerNode(); +class ArucoTrackerNode : public rclcpp::Node { + public: + ArucoTrackerNode(); -private: - void loadParameters(); + private: + void loadParameters(); - void image_callback(const sensor_msgs::msg::Image::SharedPtr msg); - void camera_info_callback(const sensor_msgs::msg::CameraInfo::SharedPtr msg); - void annotate_image(cv_bridge::CvImagePtr image, const cv::Vec3d& target); + void image_callback(const sensor_msgs::msg::Image::SharedPtr msg); + void camera_info_callback(const sensor_msgs::msg::CameraInfo::SharedPtr msg); + void annotate_image(cv_bridge::CvImagePtr image, const cv::Vec3d& target); - rclcpp::Subscription::SharedPtr _image_sub; - rclcpp::Subscription::SharedPtr _camera_info_sub; - rclcpp::Publisher::SharedPtr _image_pub; - rclcpp::Publisher::SharedPtr _target_pose_pub; + rclcpp::Subscription::SharedPtr _image_sub; + rclcpp::Subscription::SharedPtr _camera_info_sub; + rclcpp::Publisher::SharedPtr _image_pub; + rclcpp::Publisher::SharedPtr _target_pose_pub; - cv::Ptr _dictionary; - cv::Ptr _detector_params; - cv::Mat _camera_matrix; - cv::Mat _dist_coeffs; + cv::Ptr _dictionary; + cv::Ptr _detector_params; + cv::Mat _camera_matrix; + cv::Mat _dist_coeffs; - int _param_aruco_id {}; - int _param_dictionary {}; - double _param_marker_size {}; + int _param_aruco_id{}; + int _param_dictionary{}; + double _param_marker_size{}; }; // // ============================================================================ // // ALTERNATIVE VERSION - For exercises diff --git a/px4_roscon_workshop/custom_executor_demo/CustomMode.cpp b/px4_roscon_workshop/custom_executor_demo/CustomMode.cpp index ae2246e..e243be1 100644 --- a/px4_roscon_workshop/custom_executor_demo/CustomMode.cpp +++ b/px4_roscon_workshop/custom_executor_demo/CustomMode.cpp @@ -8,97 +8,99 @@ static const std::string kModeNameCustomWaypoints = "CustomWaypoints"; static const std::string kModeNameCustomYaw = "CustomYaw"; -CustomWaypoints::CustomWaypoints(rclcpp::Node &node) - : px4_ros2::ModeBase(node, kModeNameCustomWaypoints), - _node(node) +CustomWaypoints::CustomWaypoints(rclcpp::Node& node) + : px4_ros2::ModeBase(node, kModeNameCustomWaypoints), _node(node) { - loadParameters(); + loadParameters(); - _trajectory_setpoint = std::make_shared(*this); - _local_position = std::make_shared(*this); - - RCLCPP_INFO(node.get_logger(), "CustomWaypoints mode initialized."); + _trajectory_setpoint = std::make_shared(*this); + _local_position = std::make_shared(*this); + RCLCPP_INFO(node.get_logger(), "CustomWaypoints mode initialized."); } -CustomYaw::CustomYaw(rclcpp::Node &node) - : px4_ros2::ModeBase(node, kModeNameCustomYaw), - _node(node) +CustomYaw::CustomYaw(rclcpp::Node& node) : px4_ros2::ModeBase(node, kModeNameCustomYaw), _node(node) { - loadParameters(); + loadParameters(); - _trajectory_setpoint = std::make_shared(*this); - _vehicle_attitude = std::make_shared(*this); - _local_position = std::make_shared(*this); + _trajectory_setpoint = std::make_shared(*this); + _vehicle_attitude = std::make_shared(*this); + _local_position = std::make_shared(*this); - RCLCPP_INFO(node.get_logger(), "CustomYaw mode initialized."); + RCLCPP_INFO(node.get_logger(), "CustomYaw mode initialized."); } -void CustomWaypoints::loadParameters() { - // Load parameters specific to the CustomWaypoints mode +void CustomWaypoints::loadParameters() +{ + // Load parameters specific to the CustomWaypoints mode } -void CustomYaw::loadParameters() { - // Load parameters specific to the CustomYaw mode +void CustomYaw::loadParameters() +{ + // Load parameters specific to the CustomYaw mode } -void CustomWaypoints::onActivate() { - // Initialize waypoints +void CustomWaypoints::onActivate() +{ + // Initialize waypoints - _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 0.0f, -1.5f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 5.0f, -1.5f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(-5.0f, 5.0f, -1.5f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(-5.0f, -5.0f, -1.5f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, -5.0f, -1.5f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 0.0f, -1.5f)); + _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 0.0f, -1.5f)); + _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 5.0f, -1.5f)); + _trajectory_waypoints.push_back(Eigen::Vector3f(-5.0f, 5.0f, -1.5f)); + _trajectory_waypoints.push_back(Eigen::Vector3f(-5.0f, -5.0f, -1.5f)); + _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, -5.0f, -1.5f)); + _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 0.0f, -1.5f)); - _current_waypoint_index = 0; // Start at the first waypoint - RCLCPP_INFO(_node.get_logger(), "CustomWaypoints mode activated"); - // Set initial trajectory setpoint + _current_waypoint_index = 0; // Start at the first waypoint + RCLCPP_INFO(_node.get_logger(), "CustomWaypoints mode activated"); + // Set initial trajectory setpoint } -void CustomWaypoints::onDeactivate() { - RCLCPP_INFO(_node.get_logger(), "CustomWaypoints mode deactivated"); - // Reset trajectory setpoint +void CustomWaypoints::onDeactivate() +{ + RCLCPP_INFO(_node.get_logger(), "CustomWaypoints mode deactivated"); + // Reset trajectory setpoint } -void CustomWaypoints::updateSetpoint([[maybe_unused]] float dt_s) { - if (_current_waypoint_index < _trajectory_waypoints.size()) { - // Set the trajectory setpoint to the current waypoint - auto current_waypoint = _trajectory_waypoints[_current_waypoint_index]; - _trajectory_setpoint->updatePosition(current_waypoint); - +void CustomWaypoints::updateSetpoint([[maybe_unused]] float dt_s) +{ + if (_current_waypoint_index < _trajectory_waypoints.size()) { + // Set the trajectory setpoint to the current waypoint + auto current_waypoint = _trajectory_waypoints[_current_waypoint_index]; + _trajectory_setpoint->updatePosition(current_waypoint); - // Check if we reached the current waypoint - if ((_local_position->positionNed() - current_waypoint).norm() < 0.5f) { - _current_waypoint_index++; // Move to the next waypoint - } - } else { - // All waypoints completed, reset or stop - RCLCPP_INFO(_node.get_logger(), "All waypoints completed."); - completed(px4_ros2::Result::Success); - return; // Exit the update loop + // Check if we reached the current waypoint + if ((_local_position->positionNed() - current_waypoint).norm() < 0.5f) { + _current_waypoint_index++; // Move to the next waypoint } - + } else { + // All waypoints completed, reset or stop + RCLCPP_INFO(_node.get_logger(), "All waypoints completed."); + completed(px4_ros2::Result::Success); + return; // Exit the update loop + } } -void CustomYaw::onActivate() { - _start_yaw = _vehicle_attitude->yaw(); // Store the starting yaw angle - _yaw_accumulator = 0.0f; // Initialize yaw accumulator - RCLCPP_INFO(_node.get_logger(), "CustomYaw mode activated"); - // Set initial trajectory setpoint +void CustomYaw::onActivate() +{ + _start_yaw = _vehicle_attitude->yaw(); // Store the starting yaw angle + _yaw_accumulator = 0.0f; // Initialize yaw accumulator + RCLCPP_INFO(_node.get_logger(), "CustomYaw mode activated"); + // Set initial trajectory setpoint } -void CustomYaw::onDeactivate() { - RCLCPP_INFO(_node.get_logger(), "CustomYaw mode deactivated"); - // Reset trajectory setpoint +void CustomYaw::onDeactivate() +{ + RCLCPP_INFO(_node.get_logger(), "CustomYaw mode deactivated"); + // Reset trajectory setpoint } -void CustomYaw::updateSetpoint([[maybe_unused]] float dt_s) { - // Update the trajectory setpoint based on the current heading - Eigen::Vector3f velocity{0.0f, 0.0f, 0.0f}; - std::optional acceleration = std::nullopt; - std::optional yaw = std::nullopt; - std::optional yaw_rate = 0.2f; - _trajectory_setpoint->update(velocity, acceleration, yaw, yaw_rate); - _yaw_accumulator += yaw_rate.value() * dt_s; // Accumulate yaw rotation - if (std::abs(_yaw_accumulator) > 2 * M_PI - 0.1f) { // full rotation (tolerant) - RCLCPP_INFO(_node.get_logger(), "CustomYaw mode completed a full rotation."); - completed(px4_ros2::Result::Success); - return; - } +void CustomYaw::updateSetpoint([[maybe_unused]] float dt_s) +{ + // Update the trajectory setpoint based on the current heading + Eigen::Vector3f velocity{0.0f, 0.0f, 0.0f}; + std::optional acceleration = std::nullopt; + std::optional yaw = std::nullopt; + std::optional yaw_rate = 0.2f; + _trajectory_setpoint->update(velocity, acceleration, yaw, yaw_rate); + _yaw_accumulator += yaw_rate.value() * dt_s; // Accumulate yaw rotation + if (std::abs(_yaw_accumulator) > 2 * M_PI - 0.1f) { // full rotation (tolerant) + RCLCPP_INFO(_node.get_logger(), "CustomYaw mode completed a full rotation."); + completed(px4_ros2::Result::Success); + return; + } } \ No newline at end of file diff --git a/px4_roscon_workshop/custom_executor_demo/CustomMode.hpp b/px4_roscon_workshop/custom_executor_demo/CustomMode.hpp index 793a8de..b5574aa 100644 --- a/px4_roscon_workshop/custom_executor_demo/CustomMode.hpp +++ b/px4_roscon_workshop/custom_executor_demo/CustomMode.hpp @@ -4,68 +4,65 @@ #pragma once // PX4 Interface Library -#include -#include -#include -#include -#include #include +#include #include +#include +#include +#include +#include // ROS 2 Core -#include #include +#include #include // C++ Std -#include // for M_PI #include #include +#include // for M_PI class CustomWaypoints : public px4_ros2::ModeBase { -public: - explicit CustomWaypoints(rclcpp::Node &node); + public: + explicit CustomWaypoints(rclcpp::Node& node); - // See ModeBase - void onActivate() override; - void onDeactivate() override; - void updateSetpoint([[maybe_unused]] float dt_s) override; + // See ModeBase + void onActivate() override; + void onDeactivate() override; + void updateSetpoint([[maybe_unused]] float dt_s) override; -private: - void loadParameters(); - // ROS 2 - rclcpp::Node &_node; + private: + void loadParameters(); + // ROS 2 + rclcpp::Node& _node; + // px4_ros2_cpp + std::shared_ptr _trajectory_setpoint; + std::shared_ptr _local_position; - // px4_ros2_cpp - std::shared_ptr _trajectory_setpoint; - std::shared_ptr _local_position; - - std::vector _trajectory_waypoints; // Vector to hold waypoints - size_t _current_waypoint_index; // Index of the current waypoint + std::vector _trajectory_waypoints; // Vector to hold waypoints + size_t _current_waypoint_index; // Index of the current waypoint }; class CustomYaw : public px4_ros2::ModeBase { -public: - explicit CustomYaw(rclcpp::Node &node); - - // See ModeBase - void onActivate() override; - void onDeactivate() override; - void updateSetpoint([[maybe_unused]] float dt_s) override; + public: + explicit CustomYaw(rclcpp::Node& node); -private: - void loadParameters(); - // ROS 2 - rclcpp::Node &_node; + // See ModeBase + void onActivate() override; + void onDeactivate() override; + void updateSetpoint([[maybe_unused]] float dt_s) override; + private: + void loadParameters(); + // ROS 2 + rclcpp::Node& _node; - // px4_ros2_cpp - std::shared_ptr _vehicle_attitude; - std::shared_ptr _trajectory_setpoint; - std::shared_ptr _local_position; - + // px4_ros2_cpp + std::shared_ptr _vehicle_attitude; + std::shared_ptr _trajectory_setpoint; + std::shared_ptr _local_position; - float _start_yaw; // Starting yaw angle - float _yaw_accumulator; // Increment for yaw rotation + float _start_yaw; // Starting yaw angle + float _yaw_accumulator; // Increment for yaw rotation }; \ No newline at end of file diff --git a/px4_roscon_workshop/custom_executor_demo/CustomModeExecutor.cpp b/px4_roscon_workshop/custom_executor_demo/CustomModeExecutor.cpp index ae0768e..6abc179 100644 --- a/px4_roscon_workshop/custom_executor_demo/CustomModeExecutor.cpp +++ b/px4_roscon_workshop/custom_executor_demo/CustomModeExecutor.cpp @@ -3,75 +3,76 @@ // ============================================================================ #include "CustomModeExecutor.hpp" -using CustomModeWithExecutor = px4_ros2::NodeWithModeExecutor; +using CustomModeWithExecutor = + px4_ros2::NodeWithModeExecutor; static const std::string kNodeName = "CustomModeDemo"; static const bool kEnableDebugOutput = true; -CustomModeExecutor::CustomModeExecutor(px4_ros2::ModeBase &owned_mode, px4_ros2::ModeBase &second_mode) - : ModeExecutorBase(Settings{}, owned_mode), _second_mode(second_mode) {} +CustomModeExecutor::CustomModeExecutor(px4_ros2::ModeBase& owned_mode, + px4_ros2::ModeBase& second_mode) + : ModeExecutorBase(Settings{}, owned_mode), _second_mode(second_mode) +{ +} -void CustomModeExecutor::onActivate() { - RCLCPP_INFO(node().get_logger(), "CustomModeExecutor activated"); - switchToState(State::Takeoff, px4_ros2::Result::Success); +void CustomModeExecutor::onActivate() +{ + RCLCPP_INFO(node().get_logger(), "CustomModeExecutor activated"); + switchToState(State::Takeoff, px4_ros2::Result::Success); } -void CustomModeExecutor::onDeactivate(DeactivateReason reason) { - const char *reason_str = (reason == DeactivateReason::FailsafeActivated) - ? "failsafe activated" - : "other reason"; - RCLCPP_INFO(node().get_logger(), "CustomModeExecutor deactivated: %s", reason_str); +void CustomModeExecutor::onDeactivate(DeactivateReason reason) +{ + const char* reason_str = + (reason == DeactivateReason::FailsafeActivated) ? "failsafe activated" : "other reason"; + RCLCPP_INFO(node().get_logger(), "CustomModeExecutor deactivated: %s", reason_str); } -void CustomModeExecutor::switchToState(State state, px4_ros2::Result previous_result) { - _state = state; - if (previous_result != px4_ros2::Result::Success) { - RCLCPP_WARN(node().get_logger(), - "Switching to state %d due to previous result: %d", - static_cast(state), static_cast(previous_result)); - } +void CustomModeExecutor::switchToState(State state, px4_ros2::Result previous_result) +{ + _state = state; + if (previous_result != px4_ros2::Result::Success) { + RCLCPP_WARN(node().get_logger(), "Switching to state %d due to previous result: %d", + static_cast(state), static_cast(previous_result)); + } - RCLCPP_INFO(node().get_logger(), "Switched to state: %d", static_cast(state)); + RCLCPP_INFO(node().get_logger(), "Switched to state: %d", static_cast(state)); - // Handle state-specific logic here - switch (state) { - case State::Takeoff: - RCLCPP_INFO(node().get_logger(), "Initiating takeoff..."); - takeoff( - [this](px4_ros2::Result result) { - switchToState(State::CustomWaypoints, result); - }, - 2.0f); - break; - case State::CustomWaypoints: - scheduleMode(ownedMode().id(), [this](px4_ros2::Result result) { - // This callback triggers when the mode completes - switchToState(State::CustomYaw, result); - }); - break; - case State::CustomYaw: - scheduleMode(_second_mode.id(), [this](px4_ros2::Result result) { - // This callback triggers when the mode completes - switchToState(State::Land, result); - }); - break; - case State::Land: - land([this](px4_ros2::Result result) { - switchToState(State::WaitUntilDisarmed, result); - }); - break; - case State::WaitUntilDisarmed: - waitUntilDisarmed([this](px4_ros2::Result result) { - RCLCPP_INFO(node().get_logger(), "All states complete (%s)", resultToString(result)); - }); - break; - } + // Handle state-specific logic here + switch (state) { + case State::Takeoff: + RCLCPP_INFO(node().get_logger(), "Initiating takeoff..."); + takeoff([this](px4_ros2::Result result) { switchToState(State::CustomWaypoints, result); }, + 2.0f); + break; + case State::CustomWaypoints: + scheduleMode(ownedMode().id(), [this](px4_ros2::Result result) { + // This callback triggers when the mode completes + switchToState(State::CustomYaw, result); + }); + break; + case State::CustomYaw: + scheduleMode(_second_mode.id(), [this](px4_ros2::Result result) { + // This callback triggers when the mode completes + switchToState(State::Land, result); + }); + break; + case State::Land: + land([this](px4_ros2::Result result) { switchToState(State::WaitUntilDisarmed, result); }); + break; + case State::WaitUntilDisarmed: + waitUntilDisarmed([this](px4_ros2::Result result) { + RCLCPP_INFO(node().get_logger(), "All states complete (%s)", resultToString(result)); + }); + break; + } } -int main(int argc, char *argv[]) { - rclcpp::init(argc, argv); - auto node_with_mode = std::make_shared(kNodeName, kEnableDebugOutput); - rclcpp::spin(node_with_mode); - rclcpp::shutdown(); - return 0; +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + auto node_with_mode = std::make_shared(kNodeName, kEnableDebugOutput); + rclcpp::spin(node_with_mode); + rclcpp::shutdown(); + return 0; } \ No newline at end of file diff --git a/px4_roscon_workshop/custom_executor_demo/CustomModeExecutor.hpp b/px4_roscon_workshop/custom_executor_demo/CustomModeExecutor.hpp index 2d831e6..b2f3afc 100644 --- a/px4_roscon_workshop/custom_executor_demo/CustomModeExecutor.hpp +++ b/px4_roscon_workshop/custom_executor_demo/CustomModeExecutor.hpp @@ -9,25 +9,24 @@ #include "CustomMode.hpp" class CustomModeExecutor : public px4_ros2::ModeExecutorBase { -public: - CustomModeExecutor(px4_ros2::ModeBase &owned_mode, - px4_ros2::ModeBase &second_mode); + public: + CustomModeExecutor(px4_ros2::ModeBase& owned_mode, px4_ros2::ModeBase& second_mode); - // See ModeExecutorBase - void onActivate() override; - void onDeactivate(DeactivateReason reason) override; + // See ModeExecutorBase + void onActivate() override; + void onDeactivate(DeactivateReason reason) override; -private: - px4_ros2::ModeBase &_second_mode; + private: + px4_ros2::ModeBase& _second_mode; - // State management - enum class State { - Takeoff, // Initial state, takeoff to a predefined altitude - CustomWaypoints, // Custom waypoints mode - CustomYaw, // Custom yaw mode - Land, // Land state - WaitUntilDisarmed // Final state, wait until the vehicle is disarmed - }; - State _state; - void switchToState(State state, px4_ros2::Result previous_result); + // State management + enum class State { + Takeoff, // Initial state, takeoff to a predefined altitude + CustomWaypoints, // Custom waypoints mode + CustomYaw, // Custom yaw mode + Land, // Land state + WaitUntilDisarmed // Final state, wait until the vehicle is disarmed + }; + State _state; + void switchToState(State state, px4_ros2::Result previous_result); }; \ No newline at end of file diff --git a/px4_roscon_workshop/custom_mode_demo/CustomMode.cpp b/px4_roscon_workshop/custom_mode_demo/CustomMode.cpp index a191b5a..f5168b1 100644 --- a/px4_roscon_workshop/custom_mode_demo/CustomMode.cpp +++ b/px4_roscon_workshop/custom_mode_demo/CustomMode.cpp @@ -6,59 +6,61 @@ static const std::string kModeNameCustomWaypoints = "CustomWaypoints"; static const std::string kNodeName = "custom_mode_demo"; static const bool kEnableDebugOutput = true; -CustomWaypoints::CustomWaypoints(rclcpp::Node &node) - : px4_ros2::ModeBase(node, kModeNameCustomWaypoints), - _node(node) +CustomWaypoints::CustomWaypoints(rclcpp::Node& node) + : px4_ros2::ModeBase(node, kModeNameCustomWaypoints), _node(node) { - loadParameters(); + loadParameters(); - _trajectory_setpoint = std::make_shared(*this); - _local_position = std::make_shared(*this); - - RCLCPP_INFO(node.get_logger(), "CustomWaypoints mode initialized."); + _trajectory_setpoint = std::make_shared(*this); + _local_position = std::make_shared(*this); + RCLCPP_INFO(node.get_logger(), "CustomWaypoints mode initialized."); } -void CustomWaypoints::loadParameters() { +void CustomWaypoints::loadParameters() +{ } -void CustomWaypoints::onActivate() { - // Initialize waypoints +void CustomWaypoints::onActivate() +{ + // Initialize waypoints - _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 0.0f, -1.5f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 5.0f, -1.5f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(-5.0f, 5.0f, -1.5f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(-5.0f, -5.0f, -1.5f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, -5.0f, -1.5f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 0.0f, -1.5f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(0.0f, 0.0f, -1.5f)); + _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 0.0f, -1.5f)); + _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 5.0f, -1.5f)); + _trajectory_waypoints.push_back(Eigen::Vector3f(-5.0f, 5.0f, -1.5f)); + _trajectory_waypoints.push_back(Eigen::Vector3f(-5.0f, -5.0f, -1.5f)); + _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, -5.0f, -1.5f)); + _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 0.0f, -1.5f)); + _trajectory_waypoints.push_back(Eigen::Vector3f(0.0f, 0.0f, -1.5f)); - _current_waypoint_index = 0; - RCLCPP_INFO(_node.get_logger(), "CustomWaypoints mode activated"); + _current_waypoint_index = 0; + RCLCPP_INFO(_node.get_logger(), "CustomWaypoints mode activated"); } -void CustomWaypoints::onDeactivate() { - RCLCPP_INFO(_node.get_logger(), "CustomWaypoints mode deactivated"); +void CustomWaypoints::onDeactivate() +{ + RCLCPP_INFO(_node.get_logger(), "CustomWaypoints mode deactivated"); } -void CustomWaypoints::updateSetpoint([[maybe_unused]] float dt_s) { - if (_current_waypoint_index < _trajectory_waypoints.size()) { - auto current_waypoint = _trajectory_waypoints[_current_waypoint_index]; - _trajectory_setpoint->updatePosition(current_waypoint); +void CustomWaypoints::updateSetpoint([[maybe_unused]] float dt_s) +{ + if (_current_waypoint_index < _trajectory_waypoints.size()) { + auto current_waypoint = _trajectory_waypoints[_current_waypoint_index]; + _trajectory_setpoint->updatePosition(current_waypoint); - if ((_local_position->positionNed() - current_waypoint).norm() < 0.5f) { - _current_waypoint_index++; - } - } else { - RCLCPP_INFO_ONCE(_node.get_logger(), "All waypoints completed."); - completed(px4_ros2::Result::Success); - return; + if ((_local_position->positionNed() - current_waypoint).norm() < 0.5f) { + _current_waypoint_index++; } + } else { + RCLCPP_INFO_ONCE(_node.get_logger(), "All waypoints completed."); + completed(px4_ros2::Result::Success); + return; + } } -int main(int argc, char *argv[]) +int main(int argc, char* argv[]) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared>( - kNodeName, kEnableDebugOutput)); - rclcpp::shutdown(); - return 0; + rclcpp::init(argc, argv); + rclcpp::spin( + std::make_shared>(kNodeName, kEnableDebugOutput)); + rclcpp::shutdown(); + return 0; } diff --git a/px4_roscon_workshop/custom_mode_demo/CustomMode.hpp b/px4_roscon_workshop/custom_mode_demo/CustomMode.hpp index 059643e..aa380d7 100644 --- a/px4_roscon_workshop/custom_mode_demo/CustomMode.hpp +++ b/px4_roscon_workshop/custom_mode_demo/CustomMode.hpp @@ -1,31 +1,29 @@ #pragma once +#include +#include #include -#include #include - +#include #include - -#include -#include #include class CustomWaypoints : public px4_ros2::ModeBase { -public: - explicit CustomWaypoints(rclcpp::Node &node); + public: + explicit CustomWaypoints(rclcpp::Node& node); - // See ModeBase - void onActivate() override; - void onDeactivate() override; - void updateSetpoint([[maybe_unused]] float dt_s) override; + // See ModeBase + void onActivate() override; + void onDeactivate() override; + void updateSetpoint([[maybe_unused]] float dt_s) override; -private: - void loadParameters(); - rclcpp::Node &_node; + private: + void loadParameters(); + rclcpp::Node& _node; - std::shared_ptr _trajectory_setpoint; - std::shared_ptr _local_position; + std::shared_ptr _trajectory_setpoint; + std::shared_ptr _local_position; - std::vector _trajectory_waypoints; - size_t _current_waypoint_index{}; + std::vector _trajectory_waypoints; + size_t _current_waypoint_index{}; }; diff --git a/px4_roscon_workshop/custom_mode_demo/solution.cpp b/px4_roscon_workshop/custom_mode_demo/solution.cpp index 10e2d8a..b29fc00 100644 --- a/px4_roscon_workshop/custom_mode_demo/solution.cpp +++ b/px4_roscon_workshop/custom_mode_demo/solution.cpp @@ -1,12 +1,10 @@ +#include +#include #include #include #include #include - #include - -#include -#include #include static const std::string kModeNameCustomWaypoints = "CustomWaypoints"; @@ -16,76 +14,74 @@ static const bool kEnableDebugOutput = true; // Solution: same waypoint mission as CustomMode.cpp, but driven by go-to // setpoints (smooth position/heading control) instead of trajectory setpoints. class CustomWaypointsGoto : public px4_ros2::ModeBase { -public: - explicit CustomWaypointsGoto(rclcpp::Node &node) - : px4_ros2::ModeBase(node, kModeNameCustomWaypoints), - _node(node) - { - _goto_setpoint = std::make_shared(*this); - _local_position = std::make_shared(*this); + public: + explicit CustomWaypointsGoto(rclcpp::Node& node) + : px4_ros2::ModeBase(node, kModeNameCustomWaypoints), _node(node) + { + _goto_setpoint = std::make_shared(*this); + _local_position = std::make_shared(*this); - RCLCPP_INFO(node.get_logger(), "CustomWaypointsGoto mode initialized."); - } + RCLCPP_INFO(node.get_logger(), "CustomWaypointsGoto mode initialized."); + } - void onActivate() override - { - _trajectory_waypoints.clear(); - _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 0.0f, -1.5f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 5.0f, -1.5f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(-5.0f, 5.0f, -1.5f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(-5.0f, -5.0f, -1.5f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, -5.0f, -1.5f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 0.0f, -1.5f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(0.0f, 0.0f, -1.5f)); + void onActivate() override + { + _trajectory_waypoints.clear(); + _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 0.0f, -1.5f)); + _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 5.0f, -1.5f)); + _trajectory_waypoints.push_back(Eigen::Vector3f(-5.0f, 5.0f, -1.5f)); + _trajectory_waypoints.push_back(Eigen::Vector3f(-5.0f, -5.0f, -1.5f)); + _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, -5.0f, -1.5f)); + _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 0.0f, -1.5f)); + _trajectory_waypoints.push_back(Eigen::Vector3f(0.0f, 0.0f, -1.5f)); - _current_waypoint_index = 0; - RCLCPP_INFO(_node.get_logger(), "CustomWaypointsGoto mode activated"); - } + _current_waypoint_index = 0; + RCLCPP_INFO(_node.get_logger(), "CustomWaypointsGoto mode activated"); + } - void onDeactivate() override - { - RCLCPP_INFO(_node.get_logger(), "CustomWaypointsGoto mode deactivated"); - } + void onDeactivate() override + { + RCLCPP_INFO(_node.get_logger(), "CustomWaypointsGoto mode deactivated"); + } - void updateSetpoint([[maybe_unused]] float dt_s) override - { - if (_current_waypoint_index < _trajectory_waypoints.size()) { - const auto ¤t_waypoint = _trajectory_waypoints[_current_waypoint_index]; - _goto_setpoint->update(current_waypoint); + void updateSetpoint([[maybe_unused]] float dt_s) override + { + if (_current_waypoint_index < _trajectory_waypoints.size()) { + const auto& current_waypoint = _trajectory_waypoints[_current_waypoint_index]; + _goto_setpoint->update(current_waypoint); - if (positionReached(current_waypoint)) { - _current_waypoint_index++; - } - } else { - RCLCPP_INFO_ONCE(_node.get_logger(), "All waypoints completed."); - completed(px4_ros2::Result::Success); - return; - } + if (positionReached(current_waypoint)) { + _current_waypoint_index++; + } + } else { + RCLCPP_INFO_ONCE(_node.get_logger(), "All waypoints completed."); + completed(px4_ros2::Result::Success); + return; } + } -private: - bool positionReached(const Eigen::Vector3f &target_position_m) const - { - static constexpr float kPositionErrorThreshold = 0.5f; // [m] - const Eigen::Vector3f position_error_m = - target_position_m - _local_position->positionNed(); - return position_error_m.norm() < kPositionErrorThreshold; - } + private: + bool positionReached(const Eigen::Vector3f& target_position_m) const + { + static constexpr float kPositionErrorThreshold = 0.5f; // [m] + const Eigen::Vector3f position_error_m = target_position_m - _local_position->positionNed(); + return position_error_m.norm() < kPositionErrorThreshold; + } - rclcpp::Node &_node; + rclcpp::Node& _node; - std::shared_ptr _goto_setpoint; - std::shared_ptr _local_position; + std::shared_ptr _goto_setpoint; + std::shared_ptr _local_position; - std::vector _trajectory_waypoints; - size_t _current_waypoint_index{}; + std::vector _trajectory_waypoints; + size_t _current_waypoint_index{}; }; -int main(int argc, char *argv[]) +int main(int argc, char* argv[]) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared>( - kNodeName, kEnableDebugOutput)); - rclcpp::shutdown(); - return 0; + rclcpp::init(argc, argv); + rclcpp::spin( + std::make_shared>(kNodeName, kEnableDebugOutput)); + rclcpp::shutdown(); + return 0; } diff --git a/px4_roscon_workshop/formation_control/include/formation_control.hpp b/px4_roscon_workshop/formation_control/include/formation_control.hpp index 039a4b9..71a8345 100644 --- a/px4_roscon_workshop/formation_control/include/formation_control.hpp +++ b/px4_roscon_workshop/formation_control/include/formation_control.hpp @@ -5,24 +5,24 @@ #pragma once #include -#include #include +#include #include +#include #include -#include #include -#include +#include #include + +#include "tf2_ros/buffer.hpp" #include "tf2_ros/transform_broadcaster.hpp" #include "tf2_ros/transform_listener.hpp" -#include "tf2_ros/buffer.hpp" using namespace px4_ros2::literals; // NOLINT class FormationControlMode : public px4_ros2::ModeBase { public: - explicit FormationControlMode( - rclcpp::Node& node, const std::string& topic_namespace_prefix = ""); + explicit FormationControlMode(rclcpp::Node& node, const std::string& topic_namespace_prefix = ""); void onActivate() override; void updateSetpoint(float dt_s) override; @@ -32,14 +32,14 @@ class FormationControlMode : public px4_ros2::ModeBase { std::shared_ptr _vehicle_local_position; bool _has_global_position{false}; uint64_t _last_global_ref_timestamp{0}; - std::unique_ptr _tf_broadcaster; - geometry_msgs::msg::TransformStamped _ekf_origin; - const std::string _tf_prefix; - const std::vector _neighbor_distances; - const std::vector _neighbor_prefixes; - const double _gain; - std::vector _neighbor_base_link_frames; - std::shared_ptr _tf_listener{nullptr}; + std::unique_ptr _tf_broadcaster; + geometry_msgs::msg::TransformStamped _ekf_origin; + const std::string _tf_prefix; + const std::vector _neighbor_distances; + const std::vector _neighbor_prefixes; + const double _gain; + std::vector _neighbor_base_link_frames; + std::shared_ptr _tf_listener{nullptr}; std::unique_ptr _tf_buffer; std::shared_ptr _trajectory_setpoint; }; diff --git a/px4_roscon_workshop/formation_control/include/formation_executor_solution.hpp b/px4_roscon_workshop/formation_control/include/formation_executor_solution.hpp index 2b0cf98..b1a5b0d 100644 --- a/px4_roscon_workshop/formation_control/include/formation_executor_solution.hpp +++ b/px4_roscon_workshop/formation_control/include/formation_executor_solution.hpp @@ -4,17 +4,17 @@ class FormationExecutor : public px4_ros2::ModeExecutorBase { public: - explicit FormationExecutor(px4_ros2::ModeBase& formation_mode); + explicit FormationExecutor(px4_ros2::ModeBase& formation_mode); - void onActivate() override; - void onDeactivate(DeactivateReason reason) override; + void onActivate() override; + void onDeactivate(DeactivateReason reason) override; private: - enum class State { - Takeoff, - Formation, - WaitUntilDisarmed, - }; + enum class State { + Takeoff, + Formation, + WaitUntilDisarmed, + }; - void switchToState(State state, px4_ros2::Result previous_result); + void switchToState(State state, px4_ros2::Result previous_result); }; diff --git a/px4_roscon_workshop/formation_control/src/exercise.cpp b/px4_roscon_workshop/formation_control/src/exercise.cpp index 1189391..883a37a 100644 --- a/px4_roscon_workshop/formation_control/src/exercise.cpp +++ b/px4_roscon_workshop/formation_control/src/exercise.cpp @@ -15,10 +15,10 @@ int main(int argc, char* argv[]) node->declare_parameter("neighbor_distances", std::vector{}); node->declare_parameter("neighbor_prefixes", std::vector{}); node->declare_parameter("gain", 1.0); - + if (kEnableDebugOutput) { - auto ret = rcutils_logging_set_logger_level(node->get_logger().get_name(), - RCUTILS_LOG_SEVERITY_DEBUG); + auto ret = + rcutils_logging_set_logger_level(node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); if (ret != RCUTILS_RET_OK) { RCLCPP_ERROR(node->get_logger(), "Error setting severity: %s", diff --git a/px4_roscon_workshop/formation_control/src/formation_control.cpp b/px4_roscon_workshop/formation_control/src/formation_control.cpp index 171a705..585e841 100644 --- a/px4_roscon_workshop/formation_control/src/formation_control.cpp +++ b/px4_roscon_workshop/formation_control/src/formation_control.cpp @@ -4,8 +4,8 @@ using namespace px4_ros2::literals; // NOLINT static const std::string kName = "Formation"; -FormationControlMode::FormationControlMode( - rclcpp::Node& node, const std::string& topic_namespace_prefix) +FormationControlMode::FormationControlMode(rclcpp::Node& node, + const std::string& topic_namespace_prefix) : ModeBase(node, Settings{kName}, topic_namespace_prefix), _tf_prefix(node.get_parameter("tf_prefix").as_string()), _neighbor_distances(node.get_parameter("neighbor_distances").as_double_array()), @@ -28,10 +28,10 @@ FormationControlMode::FormationControlMode( double x, y, z; _geocentric.Forward(msg.ref_lat, msg.ref_lon, msg.ref_alt, x, y, z); RCLCPP_INFO(this->node().get_logger(), - "Global position reference updated: lat=%f, lon=%f, alt=%f", - msg.ref_lat, msg.ref_lon, msg.ref_alt); - RCLCPP_INFO(this->node().get_logger(), - "ECEF position reference updated: x=%f, y=%f, z=%f", x, y, z); + "Global position reference updated: lat=%f, lon=%f, alt=%f", msg.ref_lat, + msg.ref_lon, msg.ref_alt); + RCLCPP_INFO(this->node().get_logger(), "ECEF position reference updated: x=%f, y=%f, z=%f", + x, y, z); _ekf_origin.header.frame_id = "earth"; _ekf_origin.child_frame_id = _tf_prefix + "map"; _ekf_origin.transform.translation.x = x; @@ -94,8 +94,7 @@ void FormationControlMode::updateSetpoint(float dt_s) for (const auto& toFrameRel : _neighbor_base_link_frames) { geometry_msgs::msg::TransformStamped t; try { - t = _tf_buffer->lookupTransform( - toFrameRel, _tf_prefix + "base_link", tf2::TimePointZero); + t = _tf_buffer->lookupTransform(toFrameRel, _tf_prefix + "base_link", tf2::TimePointZero); const Eigen::Vector2f relative_en{t.transform.translation.x, t.transform.translation.y}; const float distance = relative_en.norm(); const Eigen::Vector2f direction = relative_en.normalized(); @@ -109,8 +108,8 @@ void FormationControlMode::updateSetpoint(float dt_s) } } const px4_ros2::TrajectorySetpoint setpoint = px4_ros2::TrajectorySetpoint() - .withVelocityX(velocity_en.y()) - .withVelocityY(velocity_en.x()) - .withPositionZ(-2.0f); + .withVelocityX(velocity_en.y()) + .withVelocityY(velocity_en.x()) + .withPositionZ(-2.0f); _trajectory_setpoint->update(setpoint); } diff --git a/px4_roscon_workshop/formation_control/src/main.cpp b/px4_roscon_workshop/formation_control/src/main.cpp index 71cc396..2996965 100644 --- a/px4_roscon_workshop/formation_control/src/main.cpp +++ b/px4_roscon_workshop/formation_control/src/main.cpp @@ -15,10 +15,10 @@ int main(int argc, char* argv[]) node->declare_parameter("neighbor_distances", std::vector{}); node->declare_parameter("neighbor_prefixes", std::vector{}); node->declare_parameter("gain", 1.0); - + if (kEnableDebugOutput) { - auto ret = rcutils_logging_set_logger_level(node->get_logger().get_name(), - RCUTILS_LOG_SEVERITY_DEBUG); + auto ret = + rcutils_logging_set_logger_level(node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); if (ret != RCUTILS_RET_OK) { RCLCPP_ERROR(node->get_logger(), "Error setting severity: %s", diff --git a/px4_roscon_workshop/formation_control/src/solution.cpp b/px4_roscon_workshop/formation_control/src/solution.cpp index 2bbab30..4fce336 100644 --- a/px4_roscon_workshop/formation_control/src/solution.cpp +++ b/px4_roscon_workshop/formation_control/src/solution.cpp @@ -18,8 +18,7 @@ void FormationExecutor::onActivate() void FormationExecutor::onDeactivate(DeactivateReason reason) { - RCLCPP_INFO(node().get_logger(), "Formation executor deactivated: %d", - static_cast(reason)); + RCLCPP_INFO(node().get_logger(), "Formation executor deactivated: %d", static_cast(reason)); } void FormationExecutor::switchToState(State state, px4_ros2::Result previous_result) @@ -32,17 +31,12 @@ void FormationExecutor::switchToState(State state, px4_ros2::Result previous_res switch (state) { case State::Takeoff: - takeoff( - [this](px4_ros2::Result result) { - switchToState(State::Formation, result); - }, - 2.0f); + takeoff([this](px4_ros2::Result result) { switchToState(State::Formation, result); }, 2.0f); break; case State::Formation: - scheduleMode(ownedMode().id(), - [this](px4_ros2::Result result) { - switchToState(State::WaitUntilDisarmed, result); - }); + scheduleMode(ownedMode().id(), [this](px4_ros2::Result result) { + switchToState(State::WaitUntilDisarmed, result); + }); break; case State::WaitUntilDisarmed: waitUntilDisarmed([](px4_ros2::Result) {}); @@ -60,10 +54,10 @@ int main(int argc, char* argv[]) node->declare_parameter("neighbor_distances", std::vector{}); node->declare_parameter("neighbor_prefixes", std::vector{}); node->declare_parameter("gain", 1.0); - + if (kEnableDebugOutput) { - auto ret = rcutils_logging_set_logger_level(node->get_logger().get_name(), - RCUTILS_LOG_SEVERITY_DEBUG); + auto ret = + rcutils_logging_set_logger_level(node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); if (ret != RCUTILS_RET_OK) { RCLCPP_ERROR(node->get_logger(), "Error setting severity: %s", diff --git a/px4_roscon_workshop/precision_land/PrecisionLand.cpp b/px4_roscon_workshop/precision_land/PrecisionLand.cpp index f2fbcb8..043ee0e 100644 --- a/px4_roscon_workshop/precision_land/PrecisionLand.cpp +++ b/px4_roscon_workshop/precision_land/PrecisionLand.cpp @@ -1,359 +1,358 @@ #include "PrecisionLand.hpp" -#include -#include #include #include +#include +#include static const std::string kModeName = "PrecisionLandCustom"; static const bool kEnableDebugOutput = true; using namespace px4_ros2::literals; -PrecisionLand::PrecisionLand(rclcpp::Node& node) - : ModeBase(node, kModeName) - , _node(node) +PrecisionLand::PrecisionLand(rclcpp::Node& node) : ModeBase(node, kModeName), _node(node) { + _trajectory_setpoint = std::make_shared(*this); - _trajectory_setpoint = std::make_shared(*this); + _vehicle_local_position = std::make_shared(*this); - _vehicle_local_position = std::make_shared(*this); + _vehicle_attitude = std::make_shared(*this); - _vehicle_attitude = std::make_shared(*this); + _target_pose_sub = _node.create_subscription( + "/target_pose", rclcpp::QoS(1).best_effort(), + std::bind(&PrecisionLand::targetPoseCallback, this, std::placeholders::_1)); - _target_pose_sub = _node.create_subscription("/target_pose", - rclcpp::QoS(1).best_effort(), std::bind(&PrecisionLand::targetPoseCallback, this, std::placeholders::_1)); + _vehicle_land_detected_sub = _node.create_subscription( + "/fmu/out/vehicle_land_detected", rclcpp::QoS(1).best_effort(), + std::bind(&PrecisionLand::vehicleLandDetectedCallback, this, std::placeholders::_1)); - _vehicle_land_detected_sub = _node.create_subscription("/fmu/out/vehicle_land_detected", - rclcpp::QoS(1).best_effort(), std::bind(&PrecisionLand::vehicleLandDetectedCallback, this, std::placeholders::_1)); - - loadParameters(); + loadParameters(); } void PrecisionLand::loadParameters() { - _node.declare_parameter("descent_vel", 1.0); - _node.declare_parameter("vel_p_gain", 1.5); - _node.declare_parameter("vel_i_gain", 0.0); - _node.declare_parameter("max_velocity", 3.0); - _node.declare_parameter("target_timeout", 3.0); - _node.declare_parameter("delta_position", 0.25); - _node.declare_parameter("delta_velocity", 0.25); - - _node.get_parameter("descent_vel", _param_descent_vel); - _node.get_parameter("vel_p_gain", _param_vel_p_gain); - _node.get_parameter("vel_i_gain", _param_vel_i_gain); - _node.get_parameter("max_velocity", _param_max_velocity); - _node.get_parameter("target_timeout", _param_target_timeout); - _node.get_parameter("delta_position", _param_delta_position); - _node.get_parameter("delta_velocity", _param_delta_velocity); - - RCLCPP_INFO(_node.get_logger(), "descent_vel: %f", _param_descent_vel); - RCLCPP_INFO(_node.get_logger(), "vel_i_gain: %f", _param_vel_i_gain); + _node.declare_parameter("descent_vel", 1.0); + _node.declare_parameter("vel_p_gain", 1.5); + _node.declare_parameter("vel_i_gain", 0.0); + _node.declare_parameter("max_velocity", 3.0); + _node.declare_parameter("target_timeout", 3.0); + _node.declare_parameter("delta_position", 0.25); + _node.declare_parameter("delta_velocity", 0.25); + + _node.get_parameter("descent_vel", _param_descent_vel); + _node.get_parameter("vel_p_gain", _param_vel_p_gain); + _node.get_parameter("vel_i_gain", _param_vel_i_gain); + _node.get_parameter("max_velocity", _param_max_velocity); + _node.get_parameter("target_timeout", _param_target_timeout); + _node.get_parameter("delta_position", _param_delta_position); + _node.get_parameter("delta_velocity", _param_delta_velocity); + + RCLCPP_INFO(_node.get_logger(), "descent_vel: %f", _param_descent_vel); + RCLCPP_INFO(_node.get_logger(), "vel_i_gain: %f", _param_vel_i_gain); } -void PrecisionLand::vehicleLandDetectedCallback(const px4_msgs::msg::VehicleLandDetected::SharedPtr msg) +void PrecisionLand::vehicleLandDetectedCallback( + const px4_msgs::msg::VehicleLandDetected::SharedPtr msg) { - _land_detected = msg->landed; + _land_detected = msg->landed; } void PrecisionLand::targetPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) { - if (_search_started) { - auto tag = ArucoTag { - .position = Eigen::Vector3d(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z), - .orientation = Eigen::Quaterniond(msg->pose.orientation.w, msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z), - .timestamp = _node.now(), - }; - - // Save tag position/orientation in NED world frame - _tag = getTagWorld(tag); - } - + if (_search_started) { + auto tag = ArucoTag{ + .position = + Eigen::Vector3d(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z), + .orientation = Eigen::Quaterniond(msg->pose.orientation.w, msg->pose.orientation.x, + msg->pose.orientation.y, msg->pose.orientation.z), + .timestamp = _node.now(), + }; + + // Save tag position/orientation in NED world frame + _tag = getTagWorld(tag); + } } PrecisionLand::ArucoTag PrecisionLand::getTagWorld(const ArucoTag& tag) { - // Convert from optical to FRD. This depends on camera mounting! - // Optical (camera frame): X right, Y down, Z towards focal axis - // FRD (drone frame/baselink): X forward, Y right, Z down - Eigen::Matrix3d R; - R << 0, -1, 0, - 1, 0, 0, - 0, 0, 1; - Eigen::Quaterniond quat_NED(R); - - auto vehicle_position = Eigen::Vector3d(_vehicle_local_position->positionNed().cast()); - auto vehicle_orientation = Eigen::Quaterniond(_vehicle_attitude->attitude().cast()); - - Eigen::Affine3d drone_transform = Eigen::Translation3d(vehicle_position) * vehicle_orientation; - Eigen::Affine3d camera_transform = Eigen::Translation3d(0, 0, 0) * quat_NED; - Eigen::Affine3d tag_transform = Eigen::Translation3d(tag.position) * tag.orientation; - Eigen::Affine3d tag_world_transform = drone_transform * camera_transform * tag_transform; - - ArucoTag world_tag = { - .position = tag_world_transform.translation(), - .orientation = Eigen::Quaterniond(tag_world_transform.rotation()), - .timestamp = tag.timestamp, - }; - - return world_tag; + // Convert from optical to FRD. This depends on camera mounting! + // Optical (camera frame): X right, Y down, Z towards focal axis + // FRD (drone frame/baselink): X forward, Y right, Z down + Eigen::Matrix3d R; + R << 0, -1, 0, 1, 0, 0, 0, 0, 1; + Eigen::Quaterniond quat_NED(R); + + auto vehicle_position = Eigen::Vector3d(_vehicle_local_position->positionNed().cast()); + auto vehicle_orientation = Eigen::Quaterniond(_vehicle_attitude->attitude().cast()); + + Eigen::Affine3d drone_transform = Eigen::Translation3d(vehicle_position) * vehicle_orientation; + Eigen::Affine3d camera_transform = Eigen::Translation3d(0, 0, 0) * quat_NED; + Eigen::Affine3d tag_transform = Eigen::Translation3d(tag.position) * tag.orientation; + Eigen::Affine3d tag_world_transform = drone_transform * camera_transform * tag_transform; + + ArucoTag world_tag = { + .position = tag_world_transform.translation(), + .orientation = Eigen::Quaterniond(tag_world_transform.rotation()), + .timestamp = tag.timestamp, + }; + + return world_tag; } void PrecisionLand::onActivate() { - generateSearchWaypoints(); - _search_started = true; - switchToState(State::Search); + generateSearchWaypoints(); + _search_started = true; + switchToState(State::Search); } void PrecisionLand::onDeactivate() { - // No-op + // No-op } void PrecisionLand::updateSetpoint(float dt_s) { - bool target_lost = checkTargetTimeout(); - - if (target_lost && !_target_lost_prev) { - RCLCPP_INFO(_node.get_logger(), "Target lost: State %s", stateName(_state).c_str()); - - } else if (!target_lost && _target_lost_prev) { - RCLCPP_INFO(_node.get_logger(), "Target acquired"); - } - - _target_lost_prev = target_lost; - - // State machine - switch (_state) { - case State::Idle: { - // No-op -- just spin - break; - } - - case State::Search: { - - if (!std::isnan(_tag.position.x())) { - _approach_altitude = _vehicle_local_position->positionNed().z(); - switchToState(State::Approach); - break; - } - - auto waypoint_position = _search_waypoints[_search_waypoint_index]; - - _trajectory_setpoint->updatePosition(waypoint_position); - - if (positionReached(waypoint_position)) { - _search_waypoint_index++; - - // If we have searched all waypoints, start over - if (_search_waypoint_index >= static_cast(_search_waypoints.size())) { - _search_waypoint_index = 0; - } - } - - break; - } - - case State::Approach: { - - if (target_lost) { - RCLCPP_INFO(_node.get_logger(), "Failed! Target lost during %s", stateName(_state).c_str()); - ModeBase::completed(px4_ros2::Result::ModeFailureOther); - switchToState(State::Idle); - return; - } - - // Approach using position setpoints - auto target_position = Eigen::Vector3f(_tag.position.x(), _tag.position.y(), _approach_altitude); - - _trajectory_setpoint->updatePosition(target_position); - - if (positionReached(target_position)) { - switchToState(State::Descend); - } - - break; - } - - case State::Descend: { - - if (target_lost) { - RCLCPP_INFO(_node.get_logger(), "Failed! Target lost during %s", stateName(_state).c_str()); - ModeBase::completed(px4_ros2::Result::ModeFailureOther); - switchToState(State::Idle); - return; - } - - // Descend using velocity setpoints and P velocity controller for XY - Eigen::Vector2f vel = calculateVelocitySetpointXY(); - _trajectory_setpoint->update(Eigen::Vector3f(vel.x(), vel.y(), _param_descent_vel), std::nullopt, - px4_ros2::quaternionToYaw(_tag.orientation)); - - if (_land_detected) { - switchToState(State::Finished); - } - - break; - } - - case State::Finished: { - ModeBase::completed(px4_ros2::Result::Success); - break; - } - } // end switch/case + bool target_lost = checkTargetTimeout(); + + if (target_lost && !_target_lost_prev) { + RCLCPP_INFO(_node.get_logger(), "Target lost: State %s", stateName(_state).c_str()); + + } else if (!target_lost && _target_lost_prev) { + RCLCPP_INFO(_node.get_logger(), "Target acquired"); + } + + _target_lost_prev = target_lost; + + // State machine + switch (_state) { + case State::Idle: { + // No-op -- just spin + break; + } + + case State::Search: { + if (!std::isnan(_tag.position.x())) { + _approach_altitude = _vehicle_local_position->positionNed().z(); + switchToState(State::Approach); + break; + } + + auto waypoint_position = _search_waypoints[_search_waypoint_index]; + + _trajectory_setpoint->updatePosition(waypoint_position); + + if (positionReached(waypoint_position)) { + _search_waypoint_index++; + + // If we have searched all waypoints, start over + if (_search_waypoint_index >= static_cast(_search_waypoints.size())) { + _search_waypoint_index = 0; + } + } + + break; + } + + case State::Approach: { + if (target_lost) { + RCLCPP_INFO(_node.get_logger(), "Failed! Target lost during %s", stateName(_state).c_str()); + ModeBase::completed(px4_ros2::Result::ModeFailureOther); + switchToState(State::Idle); + return; + } + + // Approach using position setpoints + auto target_position = + Eigen::Vector3f(_tag.position.x(), _tag.position.y(), _approach_altitude); + + _trajectory_setpoint->updatePosition(target_position); + + if (positionReached(target_position)) { + switchToState(State::Descend); + } + + break; + } + + case State::Descend: { + if (target_lost) { + RCLCPP_INFO(_node.get_logger(), "Failed! Target lost during %s", stateName(_state).c_str()); + ModeBase::completed(px4_ros2::Result::ModeFailureOther); + switchToState(State::Idle); + return; + } + + // Descend using velocity setpoints and P velocity controller for XY + Eigen::Vector2f vel = calculateVelocitySetpointXY(); + _trajectory_setpoint->update(Eigen::Vector3f(vel.x(), vel.y(), _param_descent_vel), + std::nullopt, px4_ros2::quaternionToYaw(_tag.orientation)); + + if (_land_detected) { + switchToState(State::Finished); + } + + break; + } + + case State::Finished: { + ModeBase::completed(px4_ros2::Result::Success); + break; + } + } // end switch/case } Eigen::Vector2f PrecisionLand::calculateVelocitySetpointXY() { - float p_gain = _param_vel_p_gain; - float i_gain = _param_vel_i_gain; - - // P component - float delta_pos_x = _vehicle_local_position->positionNed().x() - _tag.position.x(); - float delta_pos_y = _vehicle_local_position->positionNed().y() - _tag.position.y(); - - // I component - _vel_x_integral += delta_pos_x; - _vel_y_integral += delta_pos_y; - float max_integral = _param_max_velocity; - _vel_x_integral = std::clamp(_vel_x_integral, -1.f * max_integral, max_integral); - _vel_y_integral = std::clamp(_vel_y_integral, -1.f * max_integral, max_integral); - - float Xp = delta_pos_x * p_gain; - float Xi = _vel_x_integral * i_gain; - float Yp = delta_pos_y * p_gain; - float Yi = _vel_y_integral * i_gain; - - // Sum P and I gains - float vx = -1.f * (Xp + Xi); - float vy = -1.f * (Yp + Yi); - - // 0.1m/s min vel and 3m/s max vel - vx = std::clamp(vx, -1.f * _param_max_velocity, _param_max_velocity); - vy = std::clamp(vy, -1.f * _param_max_velocity, _param_max_velocity); - - return Eigen::Vector2f(vx, vy); + float p_gain = _param_vel_p_gain; + float i_gain = _param_vel_i_gain; + + // P component + float delta_pos_x = _vehicle_local_position->positionNed().x() - _tag.position.x(); + float delta_pos_y = _vehicle_local_position->positionNed().y() - _tag.position.y(); + + // I component + _vel_x_integral += delta_pos_x; + _vel_y_integral += delta_pos_y; + float max_integral = _param_max_velocity; + _vel_x_integral = std::clamp(_vel_x_integral, -1.f * max_integral, max_integral); + _vel_y_integral = std::clamp(_vel_y_integral, -1.f * max_integral, max_integral); + + float Xp = delta_pos_x * p_gain; + float Xi = _vel_x_integral * i_gain; + float Yp = delta_pos_y * p_gain; + float Yi = _vel_y_integral * i_gain; + + // Sum P and I gains + float vx = -1.f * (Xp + Xi); + float vy = -1.f * (Yp + Yi); + + // 0.1m/s min vel and 3m/s max vel + vx = std::clamp(vx, -1.f * _param_max_velocity, _param_max_velocity); + vy = std::clamp(vy, -1.f * _param_max_velocity, _param_max_velocity); + + return Eigen::Vector2f(vx, vy); } bool PrecisionLand::checkTargetTimeout() { - if (!_tag.valid()) { - return true; - } + if (!_tag.valid()) { + return true; + } - if (_node.now().seconds() - _tag.timestamp.seconds() > _param_target_timeout) { - return true; - } + if (_node.now().seconds() - _tag.timestamp.seconds() > _param_target_timeout) { + return true; + } - return false; + return false; } void PrecisionLand::generateSearchWaypoints() { - // Generate spiral search waypoints - // The search waypoints are generated in the NED frame - // Parameters for the search pattern - double start_x = _vehicle_local_position->positionNed().x(); - double start_y = _vehicle_local_position->positionNed().y(); - double current_z = _vehicle_local_position->positionNed().z(); - auto min_z = -1.0; - - double max_radius = 2.0; - double layer_spacing = 0.5; - int points_per_layer = 16; - std::vector waypoints; - - // Generate waypoints - // Calculate the number of layers needed - int num_layers = (static_cast((min_z - current_z) / layer_spacing) / 2) < 1 ? 1 : (static_cast(( - min_z - current_z) / layer_spacing) / 2); - - // Generate waypoints - for (int layer = 0; layer < num_layers; ++layer) { - std::vector layer_waypoints; - - // Spiral out to max radius - double radius = 0.0; - - for (int point = 0; point < points_per_layer + 1; ++point) { - double angle = 2.0 * M_PI * point / points_per_layer; - double x = start_x + radius * cos(angle); - double y = start_y + radius * sin(angle); - double z = current_z; - - layer_waypoints.push_back(Eigen::Vector3f(x, y, z)); - radius += max_radius / points_per_layer; - } - - // Push the spiral out waypoints to the main waypoints vector - waypoints.insert(waypoints.end(), layer_waypoints.begin(), layer_waypoints.end()); - - // Decrease the altitude for the inward spiral - current_z += layer_spacing; - - // Reverse the layer waypoints for spiral in - std::reverse(layer_waypoints.begin(), layer_waypoints.end()); - - // Adjust the z-coordinate for the inward spiral - for (auto& waypoint : layer_waypoints) { - waypoint.z() = current_z; - } - - // Push the reversed waypoints to the main waypoints vector - waypoints.insert(waypoints.end(), layer_waypoints.begin(), layer_waypoints.end()); - - // Decrease the altitude for the next outward spiral - current_z += layer_spacing; - } - - _search_waypoints = waypoints; + // Generate spiral search waypoints + // The search waypoints are generated in the NED frame + // Parameters for the search pattern + double start_x = _vehicle_local_position->positionNed().x(); + double start_y = _vehicle_local_position->positionNed().y(); + double current_z = _vehicle_local_position->positionNed().z(); + auto min_z = -1.0; + + double max_radius = 2.0; + double layer_spacing = 0.5; + int points_per_layer = 16; + std::vector waypoints; + + // Generate waypoints + // Calculate the number of layers needed + int num_layers = (static_cast((min_z - current_z) / layer_spacing) / 2) < 1 + ? 1 + : (static_cast((min_z - current_z) / layer_spacing) / 2); + + // Generate waypoints + for (int layer = 0; layer < num_layers; ++layer) { + std::vector layer_waypoints; + + // Spiral out to max radius + double radius = 0.0; + + for (int point = 0; point < points_per_layer + 1; ++point) { + double angle = 2.0 * M_PI * point / points_per_layer; + double x = start_x + radius * cos(angle); + double y = start_y + radius * sin(angle); + double z = current_z; + + layer_waypoints.push_back(Eigen::Vector3f(x, y, z)); + radius += max_radius / points_per_layer; + } + + // Push the spiral out waypoints to the main waypoints vector + waypoints.insert(waypoints.end(), layer_waypoints.begin(), layer_waypoints.end()); + + // Decrease the altitude for the inward spiral + current_z += layer_spacing; + + // Reverse the layer waypoints for spiral in + std::reverse(layer_waypoints.begin(), layer_waypoints.end()); + + // Adjust the z-coordinate for the inward spiral + for (auto& waypoint : layer_waypoints) { + waypoint.z() = current_z; + } + + // Push the reversed waypoints to the main waypoints vector + waypoints.insert(waypoints.end(), layer_waypoints.begin(), layer_waypoints.end()); + + // Decrease the altitude for the next outward spiral + current_z += layer_spacing; + } + + _search_waypoints = waypoints; } bool PrecisionLand::positionReached(const Eigen::Vector3f& target) const { - auto position = _vehicle_local_position->positionNed(); - auto velocity = _vehicle_local_position->velocityNed(); + auto position = _vehicle_local_position->positionNed(); + auto velocity = _vehicle_local_position->velocityNed(); - const auto delta_pos = target - position; - // NOTE: this does NOT handle a moving target! - return (delta_pos.norm() < _param_delta_position) && (velocity.norm() < _param_delta_velocity); + const auto delta_pos = target - position; + // NOTE: this does NOT handle a moving target! + return (delta_pos.norm() < _param_delta_position) && (velocity.norm() < _param_delta_velocity); } std::string PrecisionLand::stateName(State state) { - switch (state) { - case State::Idle: - return "Idle"; + switch (state) { + case State::Idle: + return "Idle"; - case State::Search: - return "Search"; + case State::Search: + return "Search"; - case State::Approach: - return "Approach"; + case State::Approach: + return "Approach"; - case State::Descend: - return "Descend"; + case State::Descend: + return "Descend"; - case State::Finished: - return "Finished"; + case State::Finished: + return "Finished"; - default: - return "Unknown"; - } + default: + return "Unknown"; + } } void PrecisionLand::switchToState(State state) { - RCLCPP_INFO(_node.get_logger(), "Switching to %s", stateName(state).c_str()); - _state = state; + RCLCPP_INFO(_node.get_logger(), "Switching to %s", stateName(state).c_str()); + _state = state; } int main(int argc, char* argv[]) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared>(kModeName, kEnableDebugOutput)); - rclcpp::shutdown(); - return 0; + rclcpp::init(argc, argv); + rclcpp::spin( + std::make_shared>(kModeName, kEnableDebugOutput)); + rclcpp::shutdown(); + return 0; } \ No newline at end of file diff --git a/px4_roscon_workshop/precision_land/PrecisionLand.hpp b/px4_roscon_workshop/precision_land/PrecisionLand.hpp index 30c4b94..0360af1 100644 --- a/px4_roscon_workshop/precision_land/PrecisionLand.hpp +++ b/px4_roscon_workshop/precision_land/PrecisionLand.hpp @@ -1,100 +1,99 @@ #pragma once -#include -#include -#include -#include -#include -#include +#include +#include -#include -#include #include -#include -#include #include +#include #include +#include +#include +#include +#include +#include +#include +#include #include -class PrecisionLand : public px4_ros2::ModeBase -{ -public: - explicit PrecisionLand(rclcpp::Node& node); - - void targetPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg); - void vehicleLandDetectedCallback(const px4_msgs::msg::VehicleLandDetected::SharedPtr msg); - - // See ModeBasep - void onActivate() override; - void onDeactivate() override; - void updateSetpoint(float dt_s) override; - -private: - struct ArucoTag { - // Initialize position with NaN values directly in the struct - Eigen::Vector3d position = Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()); - Eigen::Quaterniond orientation; - rclcpp::Time timestamp; - - bool valid() { return timestamp.nanoseconds() > 0; }; - }; - - void loadParameters(); - - ArucoTag getTagWorld(const ArucoTag& tag); - - Eigen::Vector2f calculateVelocitySetpointXY(); - bool checkTargetTimeout(); - bool positionReached(const Eigen::Vector3f& target) const; - - enum class State { - Idle, - Search, // Searches for target using a search pattern - Approach, // Positioning over landing target while maintaining altitude - Descend, // Stay over landing target while descending - Finished - }; - - void switchToState(State state); - std::string stateName(State state); - - // ros2 - rclcpp::Node& _node; - rclcpp::Subscription::SharedPtr _target_pose_sub; - rclcpp::Subscription::SharedPtr _vehicle_land_detected_sub; - - // px4_ros2_cpp - std::shared_ptr _vehicle_local_position; - std::shared_ptr _vehicle_attitude; - std::shared_ptr _trajectory_setpoint; - - // Data - State _state = State::Search; - bool _search_started = false; - - ArucoTag _tag; - float _approach_altitude = {}; - - // Land detection - bool _land_detected = false; - bool _target_lost_prev = true; - - // Waypoints for Search pattern - std::vector _search_waypoints; - // Search pattern generation - void generateSearchWaypoints(); - // Search pattern index - int _search_waypoint_index = 0; - - // Parameters - float _param_descent_vel = {}; - float _param_vel_p_gain = {}; - float _param_vel_i_gain = {}; - float _param_max_velocity = {}; - float _param_target_timeout = {}; - float _param_delta_position = {}; - float _param_delta_velocity = {}; - - float _vel_x_integral {}; - float _vel_y_integral {}; +class PrecisionLand : public px4_ros2::ModeBase { + public: + explicit PrecisionLand(rclcpp::Node& node); + + void targetPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg); + void vehicleLandDetectedCallback(const px4_msgs::msg::VehicleLandDetected::SharedPtr msg); + + // See ModeBasep + void onActivate() override; + void onDeactivate() override; + void updateSetpoint(float dt_s) override; + + private: + struct ArucoTag { + // Initialize position with NaN values directly in the struct + Eigen::Vector3d position = Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()); + Eigen::Quaterniond orientation; + rclcpp::Time timestamp; + + bool valid() { return timestamp.nanoseconds() > 0; }; + }; + + void loadParameters(); + + ArucoTag getTagWorld(const ArucoTag& tag); + + Eigen::Vector2f calculateVelocitySetpointXY(); + bool checkTargetTimeout(); + bool positionReached(const Eigen::Vector3f& target) const; + + enum class State { + Idle, + Search, // Searches for target using a search pattern + Approach, // Positioning over landing target while maintaining altitude + Descend, // Stay over landing target while descending + Finished + }; + + void switchToState(State state); + std::string stateName(State state); + + // ros2 + rclcpp::Node& _node; + rclcpp::Subscription::SharedPtr _target_pose_sub; + rclcpp::Subscription::SharedPtr _vehicle_land_detected_sub; + + // px4_ros2_cpp + std::shared_ptr _vehicle_local_position; + std::shared_ptr _vehicle_attitude; + std::shared_ptr _trajectory_setpoint; + + // Data + State _state = State::Search; + bool _search_started = false; + + ArucoTag _tag; + float _approach_altitude = {}; + + // Land detection + bool _land_detected = false; + bool _target_lost_prev = true; + + // Waypoints for Search pattern + std::vector _search_waypoints; + // Search pattern generation + void generateSearchWaypoints(); + // Search pattern index + int _search_waypoint_index = 0; + + // Parameters + float _param_descent_vel = {}; + float _param_vel_p_gain = {}; + float _param_vel_i_gain = {}; + float _param_max_velocity = {}; + float _param_target_timeout = {}; + float _param_delta_position = {}; + float _param_delta_velocity = {}; + + float _vel_x_integral{}; + float _vel_y_integral{}; }; \ No newline at end of file diff --git a/px4_roscon_workshop/precision_land_executor/CustomModeExecutor_v2.cpp b/px4_roscon_workshop/precision_land_executor/CustomModeExecutor_v2.cpp index 8a91857..100992f 100644 --- a/px4_roscon_workshop/precision_land_executor/CustomModeExecutor_v2.cpp +++ b/px4_roscon_workshop/precision_land_executor/CustomModeExecutor_v2.cpp @@ -3,70 +3,73 @@ // ============================================================================ #include "CustomModeExecutor_v2.hpp" -using CustomModeWithExecutor = px4_ros2::NodeWithModeExecutor; +using CustomModeWithExecutor = + px4_ros2::NodeWithModeExecutor; static const std::string kNodeName = "CustomModeDemo"; static const bool kEnableDebugOutput = true; -CustomModeExecutor::CustomModeExecutor(px4_ros2::ModeBase &owned_mode, px4_ros2::ModeBase &second_mode) - : ModeExecutorBase(Settings{}, owned_mode), _second_mode(second_mode) {} +CustomModeExecutor::CustomModeExecutor(px4_ros2::ModeBase& owned_mode, + px4_ros2::ModeBase& second_mode) + : ModeExecutorBase(Settings{}, owned_mode), _second_mode(second_mode) +{ +} -void CustomModeExecutor::onActivate() { - RCLCPP_INFO(node().get_logger(), "CustomModeExecutor activated"); - switchToState(State::Takeoff, px4_ros2::Result::Success); +void CustomModeExecutor::onActivate() +{ + RCLCPP_INFO(node().get_logger(), "CustomModeExecutor activated"); + switchToState(State::Takeoff, px4_ros2::Result::Success); } -void CustomModeExecutor::onDeactivate(DeactivateReason reason) { - const char *reason_str = (reason == DeactivateReason::FailsafeActivated) - ? "failsafe activated" - : "other reason"; - RCLCPP_INFO(node().get_logger(), "CustomModeExecutor deactivated: %s", reason_str); +void CustomModeExecutor::onDeactivate(DeactivateReason reason) +{ + const char* reason_str = + (reason == DeactivateReason::FailsafeActivated) ? "failsafe activated" : "other reason"; + RCLCPP_INFO(node().get_logger(), "CustomModeExecutor deactivated: %s", reason_str); } -void CustomModeExecutor::switchToState(State state, px4_ros2::Result previous_result) { - _state = state; - if (previous_result != px4_ros2::Result::Success) { - RCLCPP_WARN(node().get_logger(), - "Switching to state %d due to previous result: %d", - static_cast(state), static_cast(previous_result)); - } +void CustomModeExecutor::switchToState(State state, px4_ros2::Result previous_result) +{ + _state = state; + if (previous_result != px4_ros2::Result::Success) { + RCLCPP_WARN(node().get_logger(), "Switching to state %d due to previous result: %d", + static_cast(state), static_cast(previous_result)); + } - RCLCPP_INFO(node().get_logger(), "Switched to state: %d", static_cast(state)); + RCLCPP_INFO(node().get_logger(), "Switched to state: %d", static_cast(state)); - // Handle state-specific logic here - switch (state) { - case State::Takeoff: - RCLCPP_INFO(node().get_logger(), "Initiating takeoff..."); - takeoff( - [this](px4_ros2::Result result) { - switchToState(State::CustomWaypoints, result); - }, - 2.0f); - break; - case State::CustomWaypoints: - scheduleMode(ownedMode().id(), [this](px4_ros2::Result result) { - // This callback triggers when the mode completes - switchToState(State::PrecisionLand, result); - }); - break; - case State::PrecisionLand: - scheduleMode(_second_mode.id(), [this](px4_ros2::Result result) { - // This callback triggers when the mode completes - switchToState(State::WaitUntilDisarmed, result); - }); - break; - case State::WaitUntilDisarmed: - waitUntilDisarmed([this](px4_ros2::Result result) { - RCLCPP_INFO(node().get_logger(), "All states complete (%s)", resultToString(result)); - }); - break; - } + // Handle state-specific logic here + switch (state) { + case State::Takeoff: + RCLCPP_INFO(node().get_logger(), "Initiating takeoff..."); + takeoff([this](px4_ros2::Result result) { switchToState(State::CustomWaypoints, result); }, + 2.0f); + break; + case State::CustomWaypoints: + scheduleMode(ownedMode().id(), [this](px4_ros2::Result result) { + // This callback triggers when the mode completes + switchToState(State::PrecisionLand, result); + }); + break; + case State::PrecisionLand: + scheduleMode(_second_mode.id(), [this](px4_ros2::Result result) { + // This callback triggers when the mode completes + switchToState(State::WaitUntilDisarmed, result); + }); + break; + case State::WaitUntilDisarmed: + waitUntilDisarmed([this](px4_ros2::Result result) { + RCLCPP_INFO(node().get_logger(), "All states complete (%s)", resultToString(result)); + }); + break; + } } -int main(int argc, char *argv[]) { - rclcpp::init(argc, argv); - auto node_with_mode = std::make_shared(kNodeName, kEnableDebugOutput); - rclcpp::spin(node_with_mode); - rclcpp::shutdown(); - return 0; +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + auto node_with_mode = std::make_shared(kNodeName, kEnableDebugOutput); + rclcpp::spin(node_with_mode); + rclcpp::shutdown(); + return 0; } diff --git a/px4_roscon_workshop/precision_land_executor/CustomModeExecutor_v2.hpp b/px4_roscon_workshop/precision_land_executor/CustomModeExecutor_v2.hpp index d232e49..273bb1d 100644 --- a/px4_roscon_workshop/precision_land_executor/CustomModeExecutor_v2.hpp +++ b/px4_roscon_workshop/precision_land_executor/CustomModeExecutor_v2.hpp @@ -9,24 +9,23 @@ #include "CustomMode_v2.hpp" class CustomModeExecutor : public px4_ros2::ModeExecutorBase { -public: - CustomModeExecutor(px4_ros2::ModeBase &owned_mode, - px4_ros2::ModeBase &second_mode); + public: + CustomModeExecutor(px4_ros2::ModeBase& owned_mode, px4_ros2::ModeBase& second_mode); - // See ModeExecutorBase - void onActivate() override; - void onDeactivate(DeactivateReason reason) override; + // See ModeExecutorBase + void onActivate() override; + void onDeactivate(DeactivateReason reason) override; -private: - px4_ros2::ModeBase &_second_mode; + private: + px4_ros2::ModeBase& _second_mode; - // State management - enum class State { - Takeoff, // Initial state, takeoff to a predefined altitude - CustomWaypoints, // Custom waypoints mode - PrecisionLand, // Precision landing mode - WaitUntilDisarmed // Final state, wait until the vehicle is disarmed - }; - State _state; - void switchToState(State state, px4_ros2::Result previous_result); + // State management + enum class State { + Takeoff, // Initial state, takeoff to a predefined altitude + CustomWaypoints, // Custom waypoints mode + PrecisionLand, // Precision landing mode + WaitUntilDisarmed // Final state, wait until the vehicle is disarmed + }; + State _state; + void switchToState(State state, px4_ros2::Result previous_result); }; \ No newline at end of file diff --git a/px4_roscon_workshop/precision_land_executor/CustomMode_v2.cpp b/px4_roscon_workshop/precision_land_executor/CustomMode_v2.cpp index 2bfc3c6..7109401 100644 --- a/px4_roscon_workshop/precision_land_executor/CustomMode_v2.cpp +++ b/px4_roscon_workshop/precision_land_executor/CustomMode_v2.cpp @@ -8,395 +8,393 @@ static const std::string kModeNameCustomWaypoints = "CustomWaypoints"; static const std::string kModeNamePrecisionLandCustom = "PrecisionLandCustom"; -CustomWaypoints::CustomWaypoints(rclcpp::Node &node) - : px4_ros2::ModeBase(node, kModeNameCustomWaypoints), - _node(node) +CustomWaypoints::CustomWaypoints(rclcpp::Node& node) + : px4_ros2::ModeBase(node, kModeNameCustomWaypoints), _node(node) { - loadParameters(); + loadParameters(); - _trajectory_setpoint = std::make_shared(*this); - _local_position = std::make_shared(*this); - - RCLCPP_INFO(node.get_logger(), "CustomWaypoints mode initialized."); + _trajectory_setpoint = std::make_shared(*this); + _local_position = std::make_shared(*this); + RCLCPP_INFO(node.get_logger(), "CustomWaypoints mode initialized."); } -void CustomWaypoints::loadParameters() { - // Load parameters specific to the CustomWaypoints mode +void CustomWaypoints::loadParameters() +{ + // Load parameters specific to the CustomWaypoints mode } -void CustomWaypoints::onActivate() { - // Initialize waypoints - _trajectory_waypoints.push_back(Eigen::Vector3f(1.0f, 2.5f, -3.0f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(14.0f, 2.5f, -3.0f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(14.0f, 8.0f, -3.0f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(-4.0f, 8.0f, -3.0f)); - - _current_waypoint_index = 0; // Start at the first waypoint - RCLCPP_INFO(_node.get_logger(), "CustomWaypoints mode activated"); - // Set initial trajectory setpoint +void CustomWaypoints::onActivate() +{ + // Initialize waypoints + _trajectory_waypoints.push_back(Eigen::Vector3f(1.0f, 2.5f, -3.0f)); + _trajectory_waypoints.push_back(Eigen::Vector3f(14.0f, 2.5f, -3.0f)); + _trajectory_waypoints.push_back(Eigen::Vector3f(14.0f, 8.0f, -3.0f)); + _trajectory_waypoints.push_back(Eigen::Vector3f(-4.0f, 8.0f, -3.0f)); + + _current_waypoint_index = 0; // Start at the first waypoint + RCLCPP_INFO(_node.get_logger(), "CustomWaypoints mode activated"); + // Set initial trajectory setpoint } -void CustomWaypoints::onDeactivate() { - RCLCPP_INFO(_node.get_logger(), "CustomWaypoints mode deactivated"); - // Reset trajectory setpoint +void CustomWaypoints::onDeactivate() +{ + RCLCPP_INFO(_node.get_logger(), "CustomWaypoints mode deactivated"); + // Reset trajectory setpoint } -void CustomWaypoints::updateSetpoint([[maybe_unused]] float dt_s) { - if (_current_waypoint_index < _trajectory_waypoints.size()) { - // Set the trajectory setpoint to the current waypoint - auto current_waypoint = _trajectory_waypoints[_current_waypoint_index]; - _trajectory_setpoint->updatePosition(current_waypoint); - - - // Check if we reached the current waypoint - if ((_local_position->positionNed() - current_waypoint).norm() < 0.5f) { - _current_waypoint_index++; // Move to the next waypoint - } - } else { - // All waypoints completed, reset or stop - RCLCPP_INFO(_node.get_logger(), "All waypoints completed."); - completed(px4_ros2::Result::Success); - return; // Exit the update loop +void CustomWaypoints::updateSetpoint([[maybe_unused]] float dt_s) +{ + if (_current_waypoint_index < _trajectory_waypoints.size()) { + // Set the trajectory setpoint to the current waypoint + auto current_waypoint = _trajectory_waypoints[_current_waypoint_index]; + _trajectory_setpoint->updatePosition(current_waypoint); + + // Check if we reached the current waypoint + if ((_local_position->positionNed() - current_waypoint).norm() < 0.5f) { + _current_waypoint_index++; // Move to the next waypoint } - + } else { + // All waypoints completed, reset or stop + RCLCPP_INFO(_node.get_logger(), "All waypoints completed."); + completed(px4_ros2::Result::Success); + return; // Exit the update loop + } } PrecisionLand::PrecisionLand(rclcpp::Node& node) - : ModeBase(node, kModeNamePrecisionLandCustom) - , _node(node) + : ModeBase(node, kModeNamePrecisionLandCustom), _node(node) { + _trajectory_setpoint = std::make_shared(*this); - _trajectory_setpoint = std::make_shared(*this); + _vehicle_local_position = std::make_shared(*this); - _vehicle_local_position = std::make_shared(*this); + _vehicle_attitude = std::make_shared(*this); - _vehicle_attitude = std::make_shared(*this); + _target_pose_sub = _node.create_subscription( + "/target_pose", rclcpp::QoS(1).best_effort(), + std::bind(&PrecisionLand::targetPoseCallback, this, std::placeholders::_1)); - _target_pose_sub = _node.create_subscription("/target_pose", - rclcpp::QoS(1).best_effort(), std::bind(&PrecisionLand::targetPoseCallback, this, std::placeholders::_1)); + _vehicle_land_detected_sub = _node.create_subscription( + "/fmu/out/vehicle_land_detected", rclcpp::QoS(1).best_effort(), + std::bind(&PrecisionLand::vehicleLandDetectedCallback, this, std::placeholders::_1)); - _vehicle_land_detected_sub = _node.create_subscription("/fmu/out/vehicle_land_detected", - rclcpp::QoS(1).best_effort(), std::bind(&PrecisionLand::vehicleLandDetectedCallback, this, std::placeholders::_1)); - - loadParameters(); + loadParameters(); } void PrecisionLand::loadParameters() { - _node.declare_parameter("descent_vel", 1.0); - _node.declare_parameter("vel_p_gain", 1.5); - _node.declare_parameter("vel_i_gain", 0.0); - _node.declare_parameter("max_velocity", 3.0); - _node.declare_parameter("target_timeout", 3.0); - _node.declare_parameter("delta_position", 0.25); - _node.declare_parameter("delta_velocity", 0.25); - - _node.get_parameter("descent_vel", _param_descent_vel); - _node.get_parameter("vel_p_gain", _param_vel_p_gain); - _node.get_parameter("vel_i_gain", _param_vel_i_gain); - _node.get_parameter("max_velocity", _param_max_velocity); - _node.get_parameter("target_timeout", _param_target_timeout); - _node.get_parameter("delta_position", _param_delta_position); - _node.get_parameter("delta_velocity", _param_delta_velocity); - - RCLCPP_INFO(_node.get_logger(), "descent_vel: %f", _param_descent_vel); - RCLCPP_INFO(_node.get_logger(), "vel_i_gain: %f", _param_vel_i_gain); + _node.declare_parameter("descent_vel", 1.0); + _node.declare_parameter("vel_p_gain", 1.5); + _node.declare_parameter("vel_i_gain", 0.0); + _node.declare_parameter("max_velocity", 3.0); + _node.declare_parameter("target_timeout", 3.0); + _node.declare_parameter("delta_position", 0.25); + _node.declare_parameter("delta_velocity", 0.25); + + _node.get_parameter("descent_vel", _param_descent_vel); + _node.get_parameter("vel_p_gain", _param_vel_p_gain); + _node.get_parameter("vel_i_gain", _param_vel_i_gain); + _node.get_parameter("max_velocity", _param_max_velocity); + _node.get_parameter("target_timeout", _param_target_timeout); + _node.get_parameter("delta_position", _param_delta_position); + _node.get_parameter("delta_velocity", _param_delta_velocity); + + RCLCPP_INFO(_node.get_logger(), "descent_vel: %f", _param_descent_vel); + RCLCPP_INFO(_node.get_logger(), "vel_i_gain: %f", _param_vel_i_gain); } -void PrecisionLand::vehicleLandDetectedCallback(const px4_msgs::msg::VehicleLandDetected::SharedPtr msg) +void PrecisionLand::vehicleLandDetectedCallback( + const px4_msgs::msg::VehicleLandDetected::SharedPtr msg) { - _land_detected = msg->landed; + _land_detected = msg->landed; } void PrecisionLand::targetPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) { - if (_search_started) { - auto tag = ArucoTag { - .position = Eigen::Vector3d(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z), - .orientation = Eigen::Quaterniond(msg->pose.orientation.w, msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z), - .timestamp = _node.now(), - }; - - // Save tag position/orientation in NED world frame - _tag = getTagWorld(tag); - } - + if (_search_started) { + auto tag = ArucoTag{ + .position = + Eigen::Vector3d(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z), + .orientation = Eigen::Quaterniond(msg->pose.orientation.w, msg->pose.orientation.x, + msg->pose.orientation.y, msg->pose.orientation.z), + .timestamp = _node.now(), + }; + + // Save tag position/orientation in NED world frame + _tag = getTagWorld(tag); + } } PrecisionLand::ArucoTag PrecisionLand::getTagWorld(const ArucoTag& tag) { - // Convert from optical to NED - // Optical: X right, Y down, Z away from lens - // NED: X forward, Y right, Z away from viewer - Eigen::Matrix3d R; - R << 0, -1, 0, - 1, 0, 0, - 0, 0, 1; - Eigen::Quaterniond quat_NED(R); - - auto vehicle_position = Eigen::Vector3d(_vehicle_local_position->positionNed().cast()); - auto vehicle_orientation = Eigen::Quaterniond(_vehicle_attitude->attitude().cast()); - - Eigen::Affine3d drone_transform = Eigen::Translation3d(vehicle_position) * vehicle_orientation; - Eigen::Affine3d camera_transform = Eigen::Translation3d(0, 0, 0) * quat_NED; - Eigen::Affine3d tag_transform = Eigen::Translation3d(tag.position) * tag.orientation; - Eigen::Affine3d tag_world_transform = drone_transform * camera_transform * tag_transform; - - ArucoTag world_tag = { - .position = tag_world_transform.translation(), - .orientation = Eigen::Quaterniond(tag_world_transform.rotation()), - .timestamp = tag.timestamp, - }; - - return world_tag; + // Convert from optical to NED + // Optical: X right, Y down, Z away from lens + // NED: X forward, Y right, Z away from viewer + Eigen::Matrix3d R; + R << 0, -1, 0, 1, 0, 0, 0, 0, 1; + Eigen::Quaterniond quat_NED(R); + + auto vehicle_position = Eigen::Vector3d(_vehicle_local_position->positionNed().cast()); + auto vehicle_orientation = Eigen::Quaterniond(_vehicle_attitude->attitude().cast()); + + Eigen::Affine3d drone_transform = Eigen::Translation3d(vehicle_position) * vehicle_orientation; + Eigen::Affine3d camera_transform = Eigen::Translation3d(0, 0, 0) * quat_NED; + Eigen::Affine3d tag_transform = Eigen::Translation3d(tag.position) * tag.orientation; + Eigen::Affine3d tag_world_transform = drone_transform * camera_transform * tag_transform; + + ArucoTag world_tag = { + .position = tag_world_transform.translation(), + .orientation = Eigen::Quaterniond(tag_world_transform.rotation()), + .timestamp = tag.timestamp, + }; + + return world_tag; } void PrecisionLand::onActivate() { - generateSearchWaypoints(); - _search_started = true; - switchToState(State::Search); + generateSearchWaypoints(); + _search_started = true; + switchToState(State::Search); } void PrecisionLand::onDeactivate() { - // No-op + // No-op } void PrecisionLand::updateSetpoint([[maybe_unused]] float dt_s) { - bool target_lost = checkTargetTimeout(); - - if (target_lost && !_target_lost_prev) { - RCLCPP_INFO(_node.get_logger(), "Target lost: State %s", stateName(_state).c_str()); + bool target_lost = checkTargetTimeout(); - } else if (!target_lost && _target_lost_prev) { - RCLCPP_INFO(_node.get_logger(), "Target acquired"); - } + if (target_lost && !_target_lost_prev) { + RCLCPP_INFO(_node.get_logger(), "Target lost: State %s", stateName(_state).c_str()); - _target_lost_prev = target_lost; + } else if (!target_lost && _target_lost_prev) { + RCLCPP_INFO(_node.get_logger(), "Target acquired"); + } - // State machine - switch (_state) { - case State::Idle: { - // No-op -- just spin - break; - } + _target_lost_prev = target_lost; - case State::Search: { - - if (!std::isnan(_tag.position.x())) { - _approach_altitude = _vehicle_local_position->positionNed().z(); - switchToState(State::Approach); - break; - } - - auto waypoint_position = _search_waypoints[_search_waypoint_index]; + // State machine + switch (_state) { + case State::Idle: { + // No-op -- just spin + break; + } - _trajectory_setpoint->updatePosition(waypoint_position); + case State::Search: { + if (!std::isnan(_tag.position.x())) { + _approach_altitude = _vehicle_local_position->positionNed().z(); + switchToState(State::Approach); + break; + } - if (positionReached(waypoint_position)) { - _search_waypoint_index++; + auto waypoint_position = _search_waypoints[_search_waypoint_index]; - // If we have searched all waypoints, start over - if (_search_waypoint_index >= static_cast(_search_waypoints.size())) { - _search_waypoint_index = 0; - } - } + _trajectory_setpoint->updatePosition(waypoint_position); - break; - } + if (positionReached(waypoint_position)) { + _search_waypoint_index++; - case State::Approach: { + // If we have searched all waypoints, start over + if (_search_waypoint_index >= static_cast(_search_waypoints.size())) { + _search_waypoint_index = 0; + } + } - if (target_lost) { - RCLCPP_INFO(_node.get_logger(), "Failed! Target lost during %s", stateName(_state).c_str()); - ModeBase::completed(px4_ros2::Result::ModeFailureOther); - switchToState(State::Idle); - return; - } + break; + } - // Approach using position setpoints - auto target_position = Eigen::Vector3f(_tag.position.x(), _tag.position.y(), _approach_altitude); + case State::Approach: { + if (target_lost) { + RCLCPP_INFO(_node.get_logger(), "Failed! Target lost during %s", stateName(_state).c_str()); + ModeBase::completed(px4_ros2::Result::ModeFailureOther); + switchToState(State::Idle); + return; + } - _trajectory_setpoint->updatePosition(target_position); + // Approach using position setpoints + auto target_position = + Eigen::Vector3f(_tag.position.x(), _tag.position.y(), _approach_altitude); - if (positionReached(target_position)) { - switchToState(State::Descend); - } + _trajectory_setpoint->updatePosition(target_position); - break; - } + if (positionReached(target_position)) { + switchToState(State::Descend); + } - case State::Descend: { + break; + } - if (target_lost) { - RCLCPP_INFO(_node.get_logger(), "Failed! Target lost during %s", stateName(_state).c_str()); - ModeBase::completed(px4_ros2::Result::ModeFailureOther); - switchToState(State::Idle); - return; - } + case State::Descend: { + if (target_lost) { + RCLCPP_INFO(_node.get_logger(), "Failed! Target lost during %s", stateName(_state).c_str()); + ModeBase::completed(px4_ros2::Result::ModeFailureOther); + switchToState(State::Idle); + return; + } - // Descend using velocity setpoints and P velocity controller for XY - Eigen::Vector2f vel = calculateVelocitySetpointXY(); - _trajectory_setpoint->update(Eigen::Vector3f(vel.x(), vel.y(), _param_descent_vel), std::nullopt, - px4_ros2::quaternionToYaw(_tag.orientation)); + // Descend using velocity setpoints and P velocity controller for XY + Eigen::Vector2f vel = calculateVelocitySetpointXY(); + _trajectory_setpoint->update(Eigen::Vector3f(vel.x(), vel.y(), _param_descent_vel), + std::nullopt, px4_ros2::quaternionToYaw(_tag.orientation)); - if (_land_detected) { - switchToState(State::Finished); - } + if (_land_detected) { + switchToState(State::Finished); + } - break; - } + break; + } - case State::Finished: { - ModeBase::completed(px4_ros2::Result::Success); - break; - } - } // end switch/case + case State::Finished: { + ModeBase::completed(px4_ros2::Result::Success); + break; + } + } // end switch/case } Eigen::Vector2f PrecisionLand::calculateVelocitySetpointXY() { - float p_gain = _param_vel_p_gain; - float i_gain = _param_vel_i_gain; - - // P component - float delta_pos_x = _vehicle_local_position->positionNed().x() - _tag.position.x(); - float delta_pos_y = _vehicle_local_position->positionNed().y() - _tag.position.y(); - - // I component - _vel_x_integral += delta_pos_x; - _vel_y_integral += delta_pos_y; - float max_integral = _param_max_velocity; - _vel_x_integral = std::clamp(_vel_x_integral, -1.f * max_integral, max_integral); - _vel_y_integral = std::clamp(_vel_y_integral, -1.f * max_integral, max_integral); - - float Xp = delta_pos_x * p_gain; - float Xi = _vel_x_integral * i_gain; - float Yp = delta_pos_y * p_gain; - float Yi = _vel_y_integral * i_gain; - - // Sum P and I gains - float vx = -1.f * (Xp + Xi); - float vy = -1.f * (Yp + Yi); - - // 0.1m/s min vel and 3m/s max vel - vx = std::clamp(vx, -1.f * _param_max_velocity, _param_max_velocity); - vy = std::clamp(vy, -1.f * _param_max_velocity, _param_max_velocity); - - return Eigen::Vector2f(vx, vy); + float p_gain = _param_vel_p_gain; + float i_gain = _param_vel_i_gain; + + // P component + float delta_pos_x = _vehicle_local_position->positionNed().x() - _tag.position.x(); + float delta_pos_y = _vehicle_local_position->positionNed().y() - _tag.position.y(); + + // I component + _vel_x_integral += delta_pos_x; + _vel_y_integral += delta_pos_y; + float max_integral = _param_max_velocity; + _vel_x_integral = std::clamp(_vel_x_integral, -1.f * max_integral, max_integral); + _vel_y_integral = std::clamp(_vel_y_integral, -1.f * max_integral, max_integral); + + float Xp = delta_pos_x * p_gain; + float Xi = _vel_x_integral * i_gain; + float Yp = delta_pos_y * p_gain; + float Yi = _vel_y_integral * i_gain; + + // Sum P and I gains + float vx = -1.f * (Xp + Xi); + float vy = -1.f * (Yp + Yi); + + // 0.1m/s min vel and 3m/s max vel + vx = std::clamp(vx, -1.f * _param_max_velocity, _param_max_velocity); + vy = std::clamp(vy, -1.f * _param_max_velocity, _param_max_velocity); + + return Eigen::Vector2f(vx, vy); } bool PrecisionLand::checkTargetTimeout() { - if (!_tag.valid()) { - return true; - } + if (!_tag.valid()) { + return true; + } - if (_node.now().seconds() - _tag.timestamp.seconds() > _param_target_timeout) { - return true; - } + if (_node.now().seconds() - _tag.timestamp.seconds() > _param_target_timeout) { + return true; + } - return false; + return false; } void PrecisionLand::generateSearchWaypoints() { - // Generate spiral search waypoints - // The search waypoints are generated in the NED frame - // Parameters for the search pattern - double start_x = _vehicle_local_position->positionNed().x(); - double start_y = _vehicle_local_position->positionNed().y(); - double current_z = _vehicle_local_position->positionNed().z(); - auto min_z = -1.0; - - double max_radius = 1.0; - double layer_spacing = 0.5; - int points_per_layer = 16; - std::vector waypoints; - - // Generate waypoints - // Calculate the number of layers needed - int num_layers = (static_cast((min_z - current_z) / layer_spacing) / 2) < 1 ? 1 : (static_cast(( - min_z - current_z) / layer_spacing) / 2); - - // Generate waypoints - for (int layer = 0; layer < num_layers; ++layer) { - std::vector layer_waypoints; - - // Spiral out to max radius - double radius = 0.0; - - for (int point = 0; point < points_per_layer + 1; ++point) { - double angle = 2.0 * M_PI * point / points_per_layer; - double x = start_x + radius * cos(angle); - double y = start_y + radius * sin(angle); - double z = current_z; - - layer_waypoints.push_back(Eigen::Vector3f(x, y, z)); - radius += max_radius / points_per_layer; - } - - // Push the spiral out waypoints to the main waypoints vector - waypoints.insert(waypoints.end(), layer_waypoints.begin(), layer_waypoints.end()); - - // Decrease the altitude for the inward spiral - current_z += layer_spacing; - - // Reverse the layer waypoints for spiral in - std::reverse(layer_waypoints.begin(), layer_waypoints.end()); - - // Adjust the z-coordinate for the inward spiral - for (auto& waypoint : layer_waypoints) { - waypoint.z() = current_z; - } - - // Push the reversed waypoints to the main waypoints vector - waypoints.insert(waypoints.end(), layer_waypoints.begin(), layer_waypoints.end()); - - // Decrease the altitude for the next outward spiral - current_z += layer_spacing; - } - - _search_waypoints = waypoints; + // Generate spiral search waypoints + // The search waypoints are generated in the NED frame + // Parameters for the search pattern + double start_x = _vehicle_local_position->positionNed().x(); + double start_y = _vehicle_local_position->positionNed().y(); + double current_z = _vehicle_local_position->positionNed().z(); + auto min_z = -1.0; + + double max_radius = 1.0; + double layer_spacing = 0.5; + int points_per_layer = 16; + std::vector waypoints; + + // Generate waypoints + // Calculate the number of layers needed + int num_layers = (static_cast((min_z - current_z) / layer_spacing) / 2) < 1 + ? 1 + : (static_cast((min_z - current_z) / layer_spacing) / 2); + + // Generate waypoints + for (int layer = 0; layer < num_layers; ++layer) { + std::vector layer_waypoints; + + // Spiral out to max radius + double radius = 0.0; + + for (int point = 0; point < points_per_layer + 1; ++point) { + double angle = 2.0 * M_PI * point / points_per_layer; + double x = start_x + radius * cos(angle); + double y = start_y + radius * sin(angle); + double z = current_z; + + layer_waypoints.push_back(Eigen::Vector3f(x, y, z)); + radius += max_radius / points_per_layer; + } + + // Push the spiral out waypoints to the main waypoints vector + waypoints.insert(waypoints.end(), layer_waypoints.begin(), layer_waypoints.end()); + + // Decrease the altitude for the inward spiral + current_z += layer_spacing; + + // Reverse the layer waypoints for spiral in + std::reverse(layer_waypoints.begin(), layer_waypoints.end()); + + // Adjust the z-coordinate for the inward spiral + for (auto& waypoint : layer_waypoints) { + waypoint.z() = current_z; + } + + // Push the reversed waypoints to the main waypoints vector + waypoints.insert(waypoints.end(), layer_waypoints.begin(), layer_waypoints.end()); + + // Decrease the altitude for the next outward spiral + current_z += layer_spacing; + } + + _search_waypoints = waypoints; } bool PrecisionLand::positionReached(const Eigen::Vector3f& target) const { - auto position = _vehicle_local_position->positionNed(); - auto velocity = _vehicle_local_position->velocityNed(); + auto position = _vehicle_local_position->positionNed(); + auto velocity = _vehicle_local_position->velocityNed(); - const auto delta_pos = target - position; - // NOTE: this does NOT handle a moving target! - return (delta_pos.norm() < _param_delta_position) && (velocity.norm() < _param_delta_velocity); + const auto delta_pos = target - position; + // NOTE: this does NOT handle a moving target! + return (delta_pos.norm() < _param_delta_position) && (velocity.norm() < _param_delta_velocity); } std::string PrecisionLand::stateName(State state) { - switch (state) { - case State::Idle: - return "Idle"; + switch (state) { + case State::Idle: + return "Idle"; - case State::Search: - return "Search"; + case State::Search: + return "Search"; - case State::Approach: - return "Approach"; + case State::Approach: + return "Approach"; - case State::Descend: - return "Descend"; + case State::Descend: + return "Descend"; - case State::Finished: - return "Finished"; + case State::Finished: + return "Finished"; - default: - return "Unknown"; - } + default: + return "Unknown"; + } } void PrecisionLand::switchToState(State state) { - RCLCPP_INFO(_node.get_logger(), "Switching to %s", stateName(state).c_str()); - _state = state; + RCLCPP_INFO(_node.get_logger(), "Switching to %s", stateName(state).c_str()); + _state = state; } - diff --git a/px4_roscon_workshop/precision_land_executor/CustomMode_v2.hpp b/px4_roscon_workshop/precision_land_executor/CustomMode_v2.hpp index b5ae396..d28682f 100644 --- a/px4_roscon_workshop/precision_land_executor/CustomMode_v2.hpp +++ b/px4_roscon_workshop/precision_land_executor/CustomMode_v2.hpp @@ -4,133 +4,132 @@ #pragma once // PX4 Interface Library -#include -#include -#include -#include -#include #include -#include #include +#include +#include +#include +#include +#include +#include // ROS 2 Core -#include -#include -#include -#include -#include #include +#include + #include +#include #include +#include +#include +#include // C++ Std -#include // for M_PI #include #include +#include // for M_PI #include class CustomWaypoints : public px4_ros2::ModeBase { -public: - explicit CustomWaypoints(rclcpp::Node &node); - - // See ModeBase - void onActivate() override; - void onDeactivate() override; - void updateSetpoint([[maybe_unused]] float dt_s) override; + public: + explicit CustomWaypoints(rclcpp::Node& node); -private: - void loadParameters(); - // ROS 2 - rclcpp::Node &_node; + // See ModeBase + void onActivate() override; + void onDeactivate() override; + void updateSetpoint([[maybe_unused]] float dt_s) override; + private: + void loadParameters(); + // ROS 2 + rclcpp::Node& _node; - // px4_ros2_cpp - std::shared_ptr _trajectory_setpoint; - std::shared_ptr _local_position; + // px4_ros2_cpp + std::shared_ptr _trajectory_setpoint; + std::shared_ptr _local_position; - std::vector _trajectory_waypoints; // Vector to hold waypoints - size_t _current_waypoint_index; // Index of the current waypoint + std::vector _trajectory_waypoints; // Vector to hold waypoints + size_t _current_waypoint_index; // Index of the current waypoint }; // PrecisionLand -class PrecisionLand : public px4_ros2::ModeBase -{ -public: - explicit PrecisionLand(rclcpp::Node& node); - - void targetPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg); - void vehicleLandDetectedCallback(const px4_msgs::msg::VehicleLandDetected::SharedPtr msg); - - // See ModeBase - void onActivate() override; - void onDeactivate() override; - void updateSetpoint(float dt_s) override; - -private: - struct ArucoTag { - // Initialize position with NaN values directly in the struct - Eigen::Vector3d position = Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()); - Eigen::Quaterniond orientation; - rclcpp::Time timestamp; - - bool valid() { return timestamp.nanoseconds() > 0; }; - }; - - void loadParameters(); - - ArucoTag getTagWorld(const ArucoTag& tag); - - Eigen::Vector2f calculateVelocitySetpointXY(); - bool checkTargetTimeout(); - bool positionReached(const Eigen::Vector3f& target) const; - - enum class State { - Idle, - Search, // Searches for target using a search pattern - Approach, // Positioning over landing target while maintaining altitude - Descend, // Stay over landing target while descending - Finished - }; - - void switchToState(State state); - std::string stateName(State state); - - // ros2 - rclcpp::Node& _node; - rclcpp::Subscription::SharedPtr _target_pose_sub; - rclcpp::Subscription::SharedPtr _vehicle_land_detected_sub; - - // px4_ros2_cpp - std::shared_ptr _vehicle_local_position; - std::shared_ptr _vehicle_attitude; - std::shared_ptr _trajectory_setpoint; - - // Data - State _state = State::Search; - bool _search_started = false; - - ArucoTag _tag; - float _approach_altitude = {}; - - // Land detection - bool _land_detected = false; - bool _target_lost_prev = true; - - // Waypoints for Search pattern - std::vector _search_waypoints; - // Search pattern generation - void generateSearchWaypoints(); - // Search pattern index - int _search_waypoint_index = 0; - - // Parameters - float _param_descent_vel = {}; - float _param_vel_p_gain = {}; - float _param_vel_i_gain = {}; - float _param_max_velocity = {}; - float _param_target_timeout = {}; - float _param_delta_position = {}; - float _param_delta_velocity = {}; - - float _vel_x_integral {}; - float _vel_y_integral {}; +class PrecisionLand : public px4_ros2::ModeBase { + public: + explicit PrecisionLand(rclcpp::Node& node); + + void targetPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg); + void vehicleLandDetectedCallback(const px4_msgs::msg::VehicleLandDetected::SharedPtr msg); + + // See ModeBase + void onActivate() override; + void onDeactivate() override; + void updateSetpoint(float dt_s) override; + + private: + struct ArucoTag { + // Initialize position with NaN values directly in the struct + Eigen::Vector3d position = Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()); + Eigen::Quaterniond orientation; + rclcpp::Time timestamp; + + bool valid() { return timestamp.nanoseconds() > 0; }; + }; + + void loadParameters(); + + ArucoTag getTagWorld(const ArucoTag& tag); + + Eigen::Vector2f calculateVelocitySetpointXY(); + bool checkTargetTimeout(); + bool positionReached(const Eigen::Vector3f& target) const; + + enum class State { + Idle, + Search, // Searches for target using a search pattern + Approach, // Positioning over landing target while maintaining altitude + Descend, // Stay over landing target while descending + Finished + }; + + void switchToState(State state); + std::string stateName(State state); + + // ros2 + rclcpp::Node& _node; + rclcpp::Subscription::SharedPtr _target_pose_sub; + rclcpp::Subscription::SharedPtr _vehicle_land_detected_sub; + + // px4_ros2_cpp + std::shared_ptr _vehicle_local_position; + std::shared_ptr _vehicle_attitude; + std::shared_ptr _trajectory_setpoint; + + // Data + State _state = State::Search; + bool _search_started = false; + + ArucoTag _tag; + float _approach_altitude = {}; + + // Land detection + bool _land_detected = false; + bool _target_lost_prev = true; + + // Waypoints for Search pattern + std::vector _search_waypoints; + // Search pattern generation + void generateSearchWaypoints(); + // Search pattern index + int _search_waypoint_index = 0; + + // Parameters + float _param_descent_vel = {}; + float _param_vel_p_gain = {}; + float _param_vel_i_gain = {}; + float _param_max_velocity = {}; + float _param_target_timeout = {}; + float _param_delta_position = {}; + float _param_delta_velocity = {}; + + float _vel_x_integral{}; + float _vel_y_integral{}; }; diff --git a/px4_roscon_workshop/px4_tf/include/px4_tf/px4_tf_publisher_node.hpp b/px4_roscon_workshop/px4_tf/include/px4_tf/px4_tf_publisher_node.hpp index db683eb..4c4a269 100644 --- a/px4_roscon_workshop/px4_tf/include/px4_tf/px4_tf_publisher_node.hpp +++ b/px4_roscon_workshop/px4_tf/include/px4_tf/px4_tf_publisher_node.hpp @@ -1,18 +1,20 @@ #pragma once #include -#include "tf2_ros/transform_broadcaster.h" -#include "tf2_ros/static_transform_broadcaster.h" + #include "px4_msgs/msg/vehicle_odometry.hpp" +#include "tf2_ros/static_transform_broadcaster.h" +#include "tf2_ros/transform_broadcaster.h" class Px4TfPublisherNode : public rclcpp::Node { -public: - Px4TfPublisherNode(); -private: - std::string px4_tf_prefix_; - std::unique_ptr tf_broadcaster_; - std::unique_ptr tf_static_broadcaster_; - rclcpp::Subscription::SharedPtr odom_sub_; - void make_static_transforms(); - void handle_odometry(const px4_msgs::msg::VehicleOdometry::SharedPtr msg); + public: + Px4TfPublisherNode(); + + private: + std::string px4_tf_prefix_; + std::unique_ptr tf_broadcaster_; + std::unique_ptr tf_static_broadcaster_; + rclcpp::Subscription::SharedPtr odom_sub_; + void make_static_transforms(); + void handle_odometry(const px4_msgs::msg::VehicleOdometry::SharedPtr msg); }; \ No newline at end of file diff --git a/px4_roscon_workshop/px4_tf/src/px4_tf_publisher.cpp b/px4_roscon_workshop/px4_tf/src/px4_tf_publisher.cpp index 0678c1d..153815f 100644 --- a/px4_roscon_workshop/px4_tf/src/px4_tf_publisher.cpp +++ b/px4_roscon_workshop/px4_tf/src/px4_tf_publisher.cpp @@ -2,7 +2,7 @@ #include "px4_tf/px4_tf_publisher_node.hpp" -int main(int argc, char ** argv) +int main(int argc, char** argv) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); diff --git a/px4_roscon_workshop/px4_tf/src/px4_tf_publisher_node.cpp b/px4_roscon_workshop/px4_tf/src/px4_tf_publisher_node.cpp index ce0f5f4..2f0d240 100644 --- a/px4_roscon_workshop/px4_tf/src/px4_tf_publisher_node.cpp +++ b/px4_roscon_workshop/px4_tf/src/px4_tf_publisher_node.cpp @@ -1,67 +1,63 @@ #include "px4_tf/px4_tf_publisher_node.hpp" + #include "tf2/LinearMath/Quaternion.h" -Px4TfPublisherNode::Px4TfPublisherNode() - : Node("px4_tf_publisher") { - px4_tf_prefix_ = this->declare_parameter("px4_tf_prefix", ""); - tf_broadcaster_ = - std::make_unique(*this); - tf_static_broadcaster_ = - std::make_unique(*this); - this->make_static_transforms(); - odom_sub_ = this->create_subscription( - "fmu/out/vehicle_odometry", - rclcpp::SensorDataQoS(), - [this](const px4_msgs::msg::VehicleOdometry::SharedPtr msg) { - this->handle_odometry(msg); - } - ); +Px4TfPublisherNode::Px4TfPublisherNode() : Node("px4_tf_publisher") +{ + px4_tf_prefix_ = this->declare_parameter("px4_tf_prefix", ""); + tf_broadcaster_ = std::make_unique(*this); + tf_static_broadcaster_ = std::make_unique(*this); + this->make_static_transforms(); + odom_sub_ = this->create_subscription( + "fmu/out/vehicle_odometry", rclcpp::SensorDataQoS(), + [this](const px4_msgs::msg::VehicleOdometry::SharedPtr msg) { this->handle_odometry(msg); }); } -void Px4TfPublisherNode::make_static_transforms() { - std::vector static_transforms; - geometry_msgs::msg::TransformStamped t; +void Px4TfPublisherNode::make_static_transforms() +{ + std::vector static_transforms; + geometry_msgs::msg::TransformStamped t; - // odom ENU to odom NED - t.header.stamp = this->get_clock()->now(); - t.header.frame_id = px4_tf_prefix_ + "odom"; - t.child_frame_id = px4_tf_prefix_ + "odom_ned"; - t.transform.translation.x = 0.0; - t.transform.translation.y = 0.0; - t.transform.translation.z = 0.0; - tf2::Quaternion q; - q.setRPY(M_PI, 0, M_PI/2); - t.transform.rotation.x = q.x(); - t.transform.rotation.y = q.y(); - t.transform.rotation.z = q.z(); - t.transform.rotation.w = q.w(); - static_transforms.push_back(t); + // odom ENU to odom NED + t.header.stamp = this->get_clock()->now(); + t.header.frame_id = px4_tf_prefix_ + "odom"; + t.child_frame_id = px4_tf_prefix_ + "odom_ned"; + t.transform.translation.x = 0.0; + t.transform.translation.y = 0.0; + t.transform.translation.z = 0.0; + tf2::Quaternion q; + q.setRPY(M_PI, 0, M_PI / 2); + t.transform.rotation.x = q.x(); + t.transform.rotation.y = q.y(); + t.transform.rotation.z = q.z(); + t.transform.rotation.w = q.w(); + static_transforms.push_back(t); - // base link flu to base link frd - t.header.frame_id = px4_tf_prefix_ + "base_link_frd"; - t.child_frame_id = px4_tf_prefix_ + "base_link"; - q.setRPY(M_PI, 0, 0); - t.transform.rotation.x = q.x(); - t.transform.rotation.y = q.y(); - t.transform.rotation.z = q.z(); - t.transform.rotation.w = q.w(); - static_transforms.push_back(t); + // base link flu to base link frd + t.header.frame_id = px4_tf_prefix_ + "base_link_frd"; + t.child_frame_id = px4_tf_prefix_ + "base_link"; + q.setRPY(M_PI, 0, 0); + t.transform.rotation.x = q.x(); + t.transform.rotation.y = q.y(); + t.transform.rotation.z = q.z(); + t.transform.rotation.w = q.w(); + static_transforms.push_back(t); - tf_static_broadcaster_->sendTransform(static_transforms); + tf_static_broadcaster_->sendTransform(static_transforms); } -void Px4TfPublisherNode::handle_odometry( - const px4_msgs::msg::VehicleOdometry::SharedPtr msg) { - geometry_msgs::msg::TransformStamped t; - t.header.stamp = this->get_clock()->now(); - t.header.frame_id = px4_tf_prefix_ + "odom_ned"; - t.child_frame_id = px4_tf_prefix_ + "base_link_frd"; - t.transform.translation.x = msg->position[0]; - t.transform.translation.y = msg->position[1]; - t.transform.translation.z = msg->position[2]; - t.transform.rotation.x = msg->q[1]; - t.transform.rotation.y = msg->q[2]; - t.transform.rotation.z = msg->q[3]; - t.transform.rotation.w = msg->q[0]; - tf_broadcaster_->sendTransform(t); +void Px4TfPublisherNode::handle_odometry(const px4_msgs::msg::VehicleOdometry::SharedPtr msg) +{ + geometry_msgs::msg::TransformStamped t; + t.header.stamp = this->get_clock()->now(); + t.header.frame_id = px4_tf_prefix_ + "odom_ned"; + t.child_frame_id = px4_tf_prefix_ + "base_link_frd"; + t.transform.translation.x = msg->position[0]; + t.transform.translation.y = msg->position[1]; + t.transform.translation.z = msg->position[2]; + t.transform.rotation.x = msg->q[1]; + t.transform.rotation.y = msg->q[2]; + t.transform.rotation.z = msg->q[3]; + t.transform.rotation.w = msg->q[0]; + tf_broadcaster_->sendTransform(t); } diff --git a/px4_roscon_workshop/rover_teleop/Teleop.cpp b/px4_roscon_workshop/rover_teleop/Teleop.cpp index 6f38b15..c02d614 100644 --- a/px4_roscon_workshop/rover_teleop/Teleop.cpp +++ b/px4_roscon_workshop/rover_teleop/Teleop.cpp @@ -1,98 +1,98 @@ #include "Teleop.hpp" -#include - #include // for std::clamp +#include static const std::string kModeName = "Teleoperation"; static const bool kEnableDebug = true; -Teleop::Teleop(rclcpp::Node& node) - : px4_ros2::ModeBase(node, kModeName) - , _node(node) +Teleop::Teleop(rclcpp::Node& node) : px4_ros2::ModeBase(node, kModeName), _node(node) { - _rover_throttle_steering_setpoint = std::make_shared(*this); - _clock = std::make_shared(RCL_SYSTEM_TIME); - loadParameters(); - _twist_sub = _node.create_subscription( - "/cmd_vel", 10, - [this](const geometry_msgs::msg::Twist::SharedPtr msg) { - _last_twist = *msg; - _last_twist_time = _clock->now(); - }); - _active_sub = _node.create_subscription( - "/teleop/active", 10, - [this](const std_msgs::msg::Bool::SharedPtr msg) { - _teleop_active = msg->data; - RCLCPP_INFO(_node.get_logger(), "Teleop active: %s", _teleop_active ? "true" : "false"); - }); + _rover_throttle_steering_setpoint = + std::make_shared(*this); + _clock = std::make_shared(RCL_SYSTEM_TIME); + loadParameters(); + _twist_sub = _node.create_subscription( + "/cmd_vel", 10, [this](const geometry_msgs::msg::Twist::SharedPtr msg) { + _last_twist = *msg; + _last_twist_time = _clock->now(); + }); + _active_sub = _node.create_subscription( + "/teleop/active", 10, [this](const std_msgs::msg::Bool::SharedPtr msg) { + _teleop_active = msg->data; + RCLCPP_INFO(_node.get_logger(), "Teleop active: %s", _teleop_active ? "true" : "false"); + }); } void Teleop::loadParameters() { - _node.declare_parameter("teleop_duration", 60.0); - double duration_sec = _node.get_parameter("teleop_duration").as_double(); - - RCLCPP_INFO(_node.get_logger(), "Teleoperation duration set to: %.2f seconds", duration_sec); - - // Ensure the value is sane - if (duration_sec < 5.0) { - RCLCPP_WARN(_node.get_logger(), "Invalid teleop_duration (%f), using default 60.0", duration_sec); - duration_sec = 60.0; - } - - _teleop_duration = std::chrono::duration(duration_sec); + _node.declare_parameter("teleop_duration", 60.0); + double duration_sec = _node.get_parameter("teleop_duration").as_double(); + + RCLCPP_INFO(_node.get_logger(), "Teleoperation duration set to: %.2f seconds", duration_sec); + + // Ensure the value is sane + if (duration_sec < 5.0) { + RCLCPP_WARN(_node.get_logger(), "Invalid teleop_duration (%f), using default 60.0", + duration_sec); + duration_sec = 60.0; + } + + _teleop_duration = std::chrono::duration(duration_sec); } void Teleop::onActivate() { - _last_twist_time = _clock->now(); - _teleop_active = true; - RCLCPP_INFO(_node.get_logger(), "Teleop mode activated"); + _last_twist_time = _clock->now(); + _teleop_active = true; + RCLCPP_INFO(_node.get_logger(), "Teleop mode activated"); } void Teleop::onDeactivate() { - RCLCPP_INFO(_node.get_logger(), "Teleop mode deactivated"); + RCLCPP_INFO(_node.get_logger(), "Teleop mode deactivated"); } void Teleop::updateSetpoint([[maybe_unused]] float dt_s) { - const auto now = _clock->now(); - - if (((now - _last_twist_time) > _teleop_duration) || (_teleop_active == false)) { - RCLCPP_WARN(_node.get_logger(), "Teleop keyboard was closed or no Twist commands for %.0f seconds, exiting Teleop mode.", _teleop_duration.count()); - completed(px4_ros2::Result::Success); - return; - } - - // Default values: no throttle, no steering - float throttle_body_x = 0.f; - float normalized_steering_setpoint = 0.f; - - if ((now - _last_twist_time).seconds() <= 0.2) { - const geometry_msgs::msg::Twist &twist = _last_twist; - - // Twist.linear.x maps directly to the forward/backward throttle setpoint. - throttle_body_x = std::clamp(static_cast(twist.linear.x), -1.f, 1.f); - - // For a differential rover the steering setpoint is the normalized speed - // difference between the left and right wheels, not a steering angle. - // Twist.angular.z is CCW-positive (left turn), while the steering setpoint - // is positive to the right, hence the sign flip. - normalized_steering_setpoint = std::clamp(static_cast(-twist.angular.z), -1.f, 1.f); - } - - _rover_throttle_steering_setpoint->update(throttle_body_x, normalized_steering_setpoint); + const auto now = _clock->now(); + + if (((now - _last_twist_time) > _teleop_duration) || (_teleop_active == false)) { + RCLCPP_WARN( + _node.get_logger(), + "Teleop keyboard was closed or no Twist commands for %.0f seconds, exiting Teleop mode.", + _teleop_duration.count()); + completed(px4_ros2::Result::Success); + return; + } + + // Default values: no throttle, no steering + float throttle_body_x = 0.f; + float normalized_steering_setpoint = 0.f; + + if ((now - _last_twist_time).seconds() <= 0.2) { + const geometry_msgs::msg::Twist& twist = _last_twist; + + // Twist.linear.x maps directly to the forward/backward throttle setpoint. + throttle_body_x = std::clamp(static_cast(twist.linear.x), -1.f, 1.f); + + // For a differential rover the steering setpoint is the normalized speed + // difference between the left and right wheels, not a steering angle. + // Twist.angular.z is CCW-positive (left turn), while the steering setpoint + // is positive to the right, hence the sign flip. + normalized_steering_setpoint = std::clamp(static_cast(-twist.angular.z), -1.f, 1.f); + } + + _rover_throttle_steering_setpoint->update(throttle_body_x, normalized_steering_setpoint); } using TeleopNodeWithMode = px4_ros2::NodeWithMode; static const std::string kNodeName = "teleop_node"; -int main(int argc, char *argv[]) +int main(int argc, char* argv[]) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared(kNodeName, kEnableDebug)); - rclcpp::shutdown(); - return 0; + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared(kNodeName, kEnableDebug)); + rclcpp::shutdown(); + return 0; } diff --git a/px4_roscon_workshop/rover_teleop/Teleop.hpp b/px4_roscon_workshop/rover_teleop/Teleop.hpp index 96bc491..35b398a 100644 --- a/px4_roscon_workshop/rover_teleop/Teleop.hpp +++ b/px4_roscon_workshop/rover_teleop/Teleop.hpp @@ -5,34 +5,34 @@ #include // ROS 2 Core -#include #include +#include #include // C++ Std #include class Teleop : public px4_ros2::ModeBase { -public: - explicit Teleop(rclcpp::Node &node); + public: + explicit Teleop(rclcpp::Node& node); - // See ModeBase - void onActivate() override; - void onDeactivate() override; - void updateSetpoint([[maybe_unused]] float dt_s) override; + // See ModeBase + void onActivate() override; + void onDeactivate() override; + void updateSetpoint([[maybe_unused]] float dt_s) override; -private: - void loadParameters(); - // ROS 2 - rclcpp::Node &_node; - rclcpp::Subscription::SharedPtr _twist_sub; - rclcpp::Subscription::SharedPtr _active_sub; - geometry_msgs::msg::Twist _last_twist; - rclcpp::Time _last_twist_time; - rclcpp::Clock::SharedPtr _clock; + private: + void loadParameters(); + // ROS 2 + rclcpp::Node& _node; + rclcpp::Subscription::SharedPtr _twist_sub; + rclcpp::Subscription::SharedPtr _active_sub; + geometry_msgs::msg::Twist _last_twist; + rclcpp::Time _last_twist_time; + rclcpp::Clock::SharedPtr _clock; - // px4_ros2_cpp - std::shared_ptr _rover_throttle_steering_setpoint; - std::chrono::duration _teleop_duration; - bool _teleop_active; + // px4_ros2_cpp + std::shared_ptr _rover_throttle_steering_setpoint; + std::chrono::duration _teleop_duration; + bool _teleop_active; }; diff --git a/px4_roscon_workshop/teleop/Teleop.cpp b/px4_roscon_workshop/teleop/Teleop.cpp index 9fd01c7..51aa716 100644 --- a/px4_roscon_workshop/teleop/Teleop.cpp +++ b/px4_roscon_workshop/teleop/Teleop.cpp @@ -5,88 +5,88 @@ static const std::string kModeName = "Teleoperation"; static const bool kEnableDebug = true; -Teleop::Teleop(rclcpp::Node& node) - : px4_ros2::ModeBase(node, kModeName) - , _node(node) +Teleop::Teleop(rclcpp::Node& node) : px4_ros2::ModeBase(node, kModeName), _node(node) { - _trajectory_setpoint = std::make_shared(*this); - _vehicle_attitude = std::make_shared(*this); - _clock = std::make_shared(RCL_SYSTEM_TIME); - loadParameters(); - _twist_sub = _node.create_subscription( - "/cmd_vel", 10, - [this](const geometry_msgs::msg::Twist::SharedPtr msg) { - _last_twist = *msg; - _last_twist_time = _clock->now(); - }); - _active_sub = _node.create_subscription( - "/teleop/active", 10, - [this](const std_msgs::msg::Bool::SharedPtr msg) { - _teleop_active = msg->data; - RCLCPP_INFO(_node.get_logger(), "Teleop active: %s", _teleop_active ? "true" : "false"); - }); + _trajectory_setpoint = std::make_shared(*this); + _vehicle_attitude = std::make_shared(*this); + _clock = std::make_shared(RCL_SYSTEM_TIME); + loadParameters(); + _twist_sub = _node.create_subscription( + "/cmd_vel", 10, [this](const geometry_msgs::msg::Twist::SharedPtr msg) { + _last_twist = *msg; + _last_twist_time = _clock->now(); + }); + _active_sub = _node.create_subscription( + "/teleop/active", 10, [this](const std_msgs::msg::Bool::SharedPtr msg) { + _teleop_active = msg->data; + RCLCPP_INFO(_node.get_logger(), "Teleop active: %s", _teleop_active ? "true" : "false"); + }); } void Teleop::loadParameters() { - _node.declare_parameter("teleop_duration", 60.0); - double duration_sec = _node.get_parameter("teleop_duration").as_double(); - - RCLCPP_INFO(_node.get_logger(), "Teleoperation duration set to: %.2f seconds", duration_sec); - - // Ensure the value is sane - if (duration_sec < 5.0) { - RCLCPP_WARN(_node.get_logger(), "Invalid teleop_duration (%f), using default 60.0", duration_sec); - duration_sec = 60.0; - } + _node.declare_parameter("teleop_duration", 60.0); + double duration_sec = _node.get_parameter("teleop_duration").as_double(); - _teleop_duration = std::chrono::duration(duration_sec); + RCLCPP_INFO(_node.get_logger(), "Teleoperation duration set to: %.2f seconds", duration_sec); + + // Ensure the value is sane + if (duration_sec < 5.0) { + RCLCPP_WARN(_node.get_logger(), "Invalid teleop_duration (%f), using default 60.0", + duration_sec); + duration_sec = 60.0; + } + + _teleop_duration = std::chrono::duration(duration_sec); } void Teleop::onActivate() { - _last_twist_time = _clock->now(); - _teleop_active = true; - RCLCPP_INFO(_node.get_logger(), "Teleop mode activated"); + _last_twist_time = _clock->now(); + _teleop_active = true; + RCLCPP_INFO(_node.get_logger(), "Teleop mode activated"); } void Teleop::onDeactivate() { - RCLCPP_INFO(_node.get_logger(), "Teleop mode deactivated"); + RCLCPP_INFO(_node.get_logger(), "Teleop mode deactivated"); } void Teleop::updateSetpoint([[maybe_unused]] float dt_s) { - const auto now = _clock->now(); + const auto now = _clock->now(); - if (((now - _last_twist_time) > _teleop_duration) || (_teleop_active == false)) { - RCLCPP_WARN(_node.get_logger(), "Teleop keyboard was closed or no Twist commands for %.0f seconds, exiting Teleop mode.", _teleop_duration.count()); - completed(px4_ros2::Result::Success); - return; - } + if (((now - _last_twist_time) > _teleop_duration) || (_teleop_active == false)) { + RCLCPP_WARN( + _node.get_logger(), + "Teleop keyboard was closed or no Twist commands for %.0f seconds, exiting Teleop mode.", + _teleop_duration.count()); + completed(px4_ros2::Result::Success); + return; + } - // Default values: zero velocity, no acceleration, no yaw input - Eigen::Vector3f velocity_ned{0.f, 0.f, 0.f}; - std::optional acceleration = std::nullopt; - std::optional yaw = std::nullopt; - std::optional yaw_rate = std::nullopt; + // Default values: zero velocity, no acceleration, no yaw input + Eigen::Vector3f velocity_ned{0.f, 0.f, 0.f}; + std::optional acceleration = std::nullopt; + std::optional yaw = std::nullopt; + std::optional yaw_rate = std::nullopt; - if ((now - _last_twist_time).seconds() <= 0.2) { - // Convert Twist (assumed ENU) to NED - // ENU: x=forward, y=left, z=up → NED: x=forward, y=right, z=down - const geometry_msgs::msg::Twist &twist = _last_twist; - float yaw = _vehicle_attitude->yaw(); // Get current yaw from attitude - // Convert to NED frame - Eigen::Vector3f velocity_body; - velocity_body.x() = twist.linear.x; - velocity_body.y() = twist.linear.y; - velocity_body.z() = twist.linear.z; + if ((now - _last_twist_time).seconds() <= 0.2) { + // Convert Twist (assumed ENU) to NED + // ENU: x=forward, y=left, z=up → NED: x=forward, y=right, z=down + const geometry_msgs::msg::Twist& twist = _last_twist; + float yaw = _vehicle_attitude->yaw(); // Get current yaw from attitude + // Convert to NED frame + Eigen::Vector3f velocity_body; + velocity_body.x() = twist.linear.x; + velocity_body.y() = twist.linear.y; + velocity_body.z() = twist.linear.z; - velocity_ned.x() = velocity_body.x() * cos(yaw) - velocity_body.y() * sin(yaw); - velocity_ned.y() = velocity_body.x() * sin(yaw) + velocity_body.y() * cos(yaw); - velocity_ned.z() = velocity_body.z(); + velocity_ned.x() = velocity_body.x() * cos(yaw) - velocity_body.y() * sin(yaw); + velocity_ned.y() = velocity_body.x() * sin(yaw) + velocity_body.y() * cos(yaw); + velocity_ned.z() = velocity_body.z(); - yaw_rate = -twist.angular.z; // ENU and NED both define yaw CCW from north - } + yaw_rate = -twist.angular.z; // ENU and NED both define yaw CCW from north + } - _trajectory_setpoint->update(velocity_ned, acceleration, yaw, yaw_rate); + _trajectory_setpoint->update(velocity_ned, acceleration, yaw, yaw_rate); } diff --git a/px4_roscon_workshop/teleop/Teleop.hpp b/px4_roscon_workshop/teleop/Teleop.hpp index 5c7292e..e867202 100644 --- a/px4_roscon_workshop/teleop/Teleop.hpp +++ b/px4_roscon_workshop/teleop/Teleop.hpp @@ -1,45 +1,45 @@ #pragma once // PX4 Interface Library -#include -#include -#include -#include #include +#include #include +#include +#include +#include // ROS 2 Core -#include #include +#include #include // C++ Std -#include // for M_PI #include #include +#include // for M_PI class Teleop : public px4_ros2::ModeBase { -public: - explicit Teleop(rclcpp::Node &node); + public: + explicit Teleop(rclcpp::Node& node); - // See ModeBase - void onActivate() override; - void onDeactivate() override; - void updateSetpoint([[maybe_unused]] float dt_s) override; + // See ModeBase + void onActivate() override; + void onDeactivate() override; + void updateSetpoint([[maybe_unused]] float dt_s) override; -private: - void loadParameters(); - // ROS 2 - rclcpp::Node &_node; - rclcpp::Subscription::SharedPtr _twist_sub; - rclcpp::Subscription::SharedPtr _active_sub; - geometry_msgs::msg::Twist _last_twist; - rclcpp::Time _last_twist_time; - rclcpp::Clock::SharedPtr _clock; + private: + void loadParameters(); + // ROS 2 + rclcpp::Node& _node; + rclcpp::Subscription::SharedPtr _twist_sub; + rclcpp::Subscription::SharedPtr _active_sub; + geometry_msgs::msg::Twist _last_twist; + rclcpp::Time _last_twist_time; + rclcpp::Clock::SharedPtr _clock; - // px4_ros2_cpp - std::shared_ptr _vehicle_attitude; - std::shared_ptr _trajectory_setpoint; - std::chrono::duration _teleop_duration; - bool _teleop_active; + // px4_ros2_cpp + std::shared_ptr _vehicle_attitude; + std::shared_ptr _trajectory_setpoint; + std::chrono::duration _teleop_duration; + bool _teleop_active; }; diff --git a/px4_roscon_workshop/teleop/TeleopExecutor.cpp b/px4_roscon_workshop/teleop/TeleopExecutor.cpp index 68efe0e..f2334a1 100644 --- a/px4_roscon_workshop/teleop/TeleopExecutor.cpp +++ b/px4_roscon_workshop/teleop/TeleopExecutor.cpp @@ -5,73 +5,70 @@ using TeleopNodeWithExecutor = px4_ros2::NodeWithModeExecutor(state), static_cast(previous_result)); - } +void TeleopExecutor::switchToState(State state, px4_ros2::Result previous_result) +{ + _state = state; + if (previous_result != px4_ros2::Result::Success) { + RCLCPP_WARN(node().get_logger(), "Switching to state %d due to previous result: %d", + static_cast(state), static_cast(previous_result)); + } - RCLCPP_INFO(node().get_logger(), "Switched to state: %d", static_cast(state)); + RCLCPP_INFO(node().get_logger(), "Switched to state: %d", static_cast(state)); - // Handle state-specific logic here - switch (state) { - case State::Takeoff: - RCLCPP_INFO(node().get_logger(), "Initiating takeoff..."); - takeoff( - [this](px4_ros2::Result result) { - switchToState(State::TeleOperation, result); - }, - 5.0f); - break; - case State::TeleOperation: - scheduleMode(ownedMode().id(), [this](px4_ros2::Result result) { - // This callback triggers when the mode completes - if (result == px4_ros2::Result::Success) { - switchToState(State::Land, result); - } else { - switchToState(State::RTL, result); - } - }); - break; - case State::RTL: - rtl([this](px4_ros2::Result result) { - switchToState(State::WaitUntilDisarmed, result); - }); - break; - case State::Land: - land([this](px4_ros2::Result result) { - switchToState(State::WaitUntilDisarmed, result); - }); - break; - case State::WaitUntilDisarmed: - waitUntilDisarmed([this](px4_ros2::Result result) { - RCLCPP_INFO(node().get_logger(), "All states complete (%s)", resultToString(result)); - }); - break; - } + // Handle state-specific logic here + switch (state) { + case State::Takeoff: + RCLCPP_INFO(node().get_logger(), "Initiating takeoff..."); + takeoff([this](px4_ros2::Result result) { switchToState(State::TeleOperation, result); }, + 5.0f); + break; + case State::TeleOperation: + scheduleMode(ownedMode().id(), [this](px4_ros2::Result result) { + // This callback triggers when the mode completes + if (result == px4_ros2::Result::Success) { + switchToState(State::Land, result); + } else { + switchToState(State::RTL, result); + } + }); + break; + case State::RTL: + rtl([this](px4_ros2::Result result) { switchToState(State::WaitUntilDisarmed, result); }); + break; + case State::Land: + land([this](px4_ros2::Result result) { switchToState(State::WaitUntilDisarmed, result); }); + break; + case State::WaitUntilDisarmed: + waitUntilDisarmed([this](px4_ros2::Result result) { + RCLCPP_INFO(node().get_logger(), "All states complete (%s)", resultToString(result)); + }); + break; + } } -int main(int argc, char *argv[]) { - rclcpp::init(argc, argv); - auto node_with_mode = std::make_shared(kNodeName, kEnableDebugOutput); - rclcpp::spin(node_with_mode); - rclcpp::shutdown(); - return 0; +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + auto node_with_mode = std::make_shared(kNodeName, kEnableDebugOutput); + rclcpp::spin(node_with_mode); + rclcpp::shutdown(); + return 0; } diff --git a/px4_roscon_workshop/teleop/TeleopExecutor.hpp b/px4_roscon_workshop/teleop/TeleopExecutor.hpp index 7880fd0..bb553d3 100644 --- a/px4_roscon_workshop/teleop/TeleopExecutor.hpp +++ b/px4_roscon_workshop/teleop/TeleopExecutor.hpp @@ -6,22 +6,22 @@ #include "Teleop.hpp" class TeleopExecutor : public px4_ros2::ModeExecutorBase { -public: - TeleopExecutor(px4_ros2::ModeBase &owned_mode); + public: + TeleopExecutor(px4_ros2::ModeBase& owned_mode); - // See ModeExecutorBase - void onActivate() override; - void onDeactivate(DeactivateReason reason) override; + // See ModeExecutorBase + void onActivate() override; + void onDeactivate(DeactivateReason reason) override; -private: - // State management - enum class State { - Takeoff, // Initial state, takeoff to a predefined altitude - TeleOperation, // Custom teleoperation mode - RTL, // Return to Launch state - Land, // Land state - WaitUntilDisarmed // Final state, wait until the vehicle is disarmed - }; - State _state; - void switchToState(State state, px4_ros2::Result previous_result); + private: + // State management + enum class State { + Takeoff, // Initial state, takeoff to a predefined altitude + TeleOperation, // Custom teleoperation mode + RTL, // Return to Launch state + Land, // Land state + WaitUntilDisarmed // Final state, wait until the vehicle is disarmed + }; + State _state; + void switchToState(State state, px4_ros2::Result previous_result); }; From db791a181dec4cbb6041a8fb115a3abd7d0d96a2 Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Thu, 27 Aug 2026 00:02:12 +0100 Subject: [PATCH 3/4] chore: run clang-tidy Signed-off-by: Beniamino Pozzan --- .../aruco_tracker/ArucoTracker.cpp | 72 +++++++------- .../aruco_tracker/ArucoTracker.hpp | 7 +- .../custom_executor_demo/CustomMode.cpp | 18 ++-- .../custom_mode_demo/CustomMode.cpp | 14 +-- .../custom_mode_demo/solution.cpp | 14 +-- .../src/formation_control.cpp | 42 ++++---- .../precision_land/PrecisionLand.cpp | 88 +++++++++-------- .../precision_land/PrecisionLand.hpp | 6 +- .../precision_land_executor/CustomMode_v2.cpp | 96 ++++++++++--------- .../precision_land_executor/CustomMode_v2.hpp | 6 +- .../include/px4_tf/px4_tf_publisher_node.hpp | 2 +- .../px4_tf/src/px4_tf_publisher_node.cpp | 4 +- px4_roscon_workshop/rover_teleop/Teleop.cpp | 2 +- px4_roscon_workshop/teleop/Teleop.cpp | 16 ++-- 14 files changed, 214 insertions(+), 173 deletions(-) diff --git a/px4_roscon_workshop/aruco_tracker/ArucoTracker.cpp b/px4_roscon_workshop/aruco_tracker/ArucoTracker.cpp index e9ac775..9752785 100644 --- a/px4_roscon_workshop/aruco_tracker/ArucoTracker.cpp +++ b/px4_roscon_workshop/aruco_tracker/ArucoTracker.cpp @@ -23,11 +23,13 @@ ArucoTrackerNode::ArucoTrackerNode() : Node("aruco_tracker_node") auto qos = rclcpp::QoS(1).best_effort(); _image_sub = create_subscription( - "/camera", qos, std::bind(&ArucoTrackerNode::image_callback, this, std::placeholders::_1)); + "/camera", qos, + [this](const sensor_msgs::msg::Image::SharedPtr msg) { image_callback(msg); }); _camera_info_sub = create_subscription( - "/camera_info", qos, - std::bind(&ArucoTrackerNode::camera_info_callback, this, std::placeholders::_1)); + "/camera_info", qos, [this](const sensor_msgs::msg::CameraInfo::SharedPtr msg) { + camera_info_callback(msg); + }); // Publishers _image_pub = create_publisher("/image_proc", qos); @@ -45,11 +47,12 @@ void ArucoTrackerNode::loadParameters() get_parameter("marker_size", _param_marker_size); } -void ArucoTrackerNode::image_callback(const sensor_msgs::msg::Image::SharedPtr msg) -{ +void ArucoTrackerNode::image_callback( + const sensor_msgs::msg::Image::SharedPtr& msg) { try { // Convert ROS image message to OpenCV image - cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); + cv_bridge::CvImagePtr const cv_ptr = + cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); // Detect markers std::vector ids; @@ -58,13 +61,13 @@ void ArucoTrackerNode::image_callback(const sensor_msgs::msg::Image::SharedPtr m cv::aruco::drawDetectedMarkers(cv_ptr->image, corners, ids); if (!_camera_matrix.empty() && !_dist_coeffs.empty()) { - std::vector> undistortedCorners; + std::vector> undistorted_corners; for (const auto& corner : corners) { - std::vector undistortedCorner; - cv::undistortPoints(corner, undistortedCorner, _camera_matrix, _dist_coeffs, cv::noArray(), - _camera_matrix); - undistortedCorners.push_back(undistortedCorner); + std::vector undistorted_corner; + cv::undistortPoints(corner, undistorted_corner, _camera_matrix, + _dist_coeffs, cv::noArray(), _camera_matrix); + undistorted_corners.push_back(undistorted_corner); } for (size_t i = 0; i < ids.size(); i++) { @@ -73,8 +76,8 @@ void ArucoTrackerNode::image_callback(const sensor_msgs::msg::Image::SharedPtr m } // Calculate marker size from camera intrinsics - float half_size = _param_marker_size / 2.0f; - std::vector objectPoints = { + float const half_size = _param_marker_size / 2.0f; + std::vector const object_points = { cv::Point3f(-half_size, half_size, 0), // top left cv::Point3f(half_size, half_size, 0), // top right cv::Point3f(half_size, -half_size, 0), // bottom right @@ -82,9 +85,10 @@ void ArucoTrackerNode::image_callback(const sensor_msgs::msg::Image::SharedPtr m }; // Use PnP solver to estimate pose - cv::Vec3d rvec, tvec; - cv::solvePnP(objectPoints, undistortedCorners[i], _camera_matrix, cv::noArray(), rvec, - tvec); + cv::Vec3d rvec; + cv::Vec3d tvec; + cv::solvePnP(object_points, undistorted_corners[i], _camera_matrix, + cv::noArray(), rvec, tvec); // Annotate the image cv::drawFrameAxes(cv_ptr->image, _camera_matrix, cv::noArray(), rvec, tvec, _param_marker_size); @@ -92,10 +96,12 @@ void ArucoTrackerNode::image_callback(const sensor_msgs::msg::Image::SharedPtr m // Quaternion from rotation matrix cv::Mat rot_mat; cv::Rodrigues(rvec, rot_mat); - tf2::Matrix3x3 tf_rotation( - rot_mat.at(0, 0), rot_mat.at(0, 1), rot_mat.at(0, 2), - rot_mat.at(1, 0), rot_mat.at(1, 1), rot_mat.at(1, 2), - rot_mat.at(2, 0), rot_mat.at(2, 1), rot_mat.at(2, 2)); + tf2::Matrix3x3 const tf_rotation( + rot_mat.at(0, 0), rot_mat.at(0, 1), + rot_mat.at(0, 2), rot_mat.at(1, 0), + rot_mat.at(1, 1), rot_mat.at(1, 2), + rot_mat.at(2, 0), rot_mat.at(2, 1), + rot_mat.at(2, 2)); tf2::Quaternion quat; tf_rotation.getRotation(quat); quat.normalize(); @@ -134,8 +140,8 @@ void ArucoTrackerNode::image_callback(const sensor_msgs::msg::Image::SharedPtr m } } -void ArucoTrackerNode::camera_info_callback(const sensor_msgs::msg::CameraInfo::SharedPtr msg) -{ +void ArucoTrackerNode::camera_info_callback( + const sensor_msgs::msg::CameraInfo::SharedPtr& msg) { // Always update the camera matrix and distortion coefficients from the new message _camera_matrix = cv::Mat(3, 3, CV_64F, const_cast(msg->k.data())) .clone(); // Use clone to ensure a deep copy @@ -168,23 +174,25 @@ void ArucoTrackerNode::camera_info_callback(const sensor_msgs::msg::CameraInfo:: } } -void ArucoTrackerNode::annotate_image(cv_bridge::CvImagePtr image, const cv::Vec3d& target) -{ +void ArucoTrackerNode::annotate_image(const cv_bridge::CvImagePtr& image, + const cv::Vec3d& target) { // Annotate the image with the target position and marker size std::ostringstream stream; stream << std::fixed << std::setprecision(2); stream << "X: " << target[0] << " Y: " << target[1] << " Z: " << target[2]; - std::string text_xyz = stream.str(); + std::string const text_xyz = stream.str(); - int fontFace = cv::FONT_HERSHEY_SIMPLEX; - double fontScale = 1; - int thickness = 2; + int const font_face = cv::FONT_HERSHEY_SIMPLEX; + double const font_scale = 1; + int const thickness = 2; int baseline = 0; - cv::Size textSize = cv::getTextSize(text_xyz, fontFace, fontScale, thickness, &baseline); + cv::Size const text_size = + cv::getTextSize(text_xyz, font_face, font_scale, thickness, &baseline); baseline += thickness; - cv::Point textOrg((image->image.cols - textSize.width - 10), (image->image.rows - 10)); - cv::putText(image->image, text_xyz, textOrg, fontFace, fontScale, cv::Scalar(0, 255, 255), - thickness, 8); + cv::Point const text_org((image->image.cols - text_size.width - 10), + (image->image.rows - 10)); + cv::putText(image->image, text_xyz, text_org, font_face, font_scale, + cv::Scalar(0, 255, 255), thickness, 8); } int main(int argc, char** argv) diff --git a/px4_roscon_workshop/aruco_tracker/ArucoTracker.hpp b/px4_roscon_workshop/aruco_tracker/ArucoTracker.hpp index c9cc1bb..b411a2c 100644 --- a/px4_roscon_workshop/aruco_tracker/ArucoTracker.hpp +++ b/px4_roscon_workshop/aruco_tracker/ArucoTracker.hpp @@ -18,9 +18,10 @@ class ArucoTrackerNode : public rclcpp::Node { private: void loadParameters(); - void image_callback(const sensor_msgs::msg::Image::SharedPtr msg); - void camera_info_callback(const sensor_msgs::msg::CameraInfo::SharedPtr msg); - void annotate_image(cv_bridge::CvImagePtr image, const cv::Vec3d& target); + void image_callback(const sensor_msgs::msg::Image::SharedPtr& msg); + void camera_info_callback(const sensor_msgs::msg::CameraInfo::SharedPtr& msg); + void annotate_image(const cv_bridge::CvImagePtr& image, + const cv::Vec3d& target); rclcpp::Subscription::SharedPtr _image_sub; rclcpp::Subscription::SharedPtr _camera_info_sub; diff --git a/px4_roscon_workshop/custom_executor_demo/CustomMode.cpp b/px4_roscon_workshop/custom_executor_demo/CustomMode.cpp index e243be1..b3c0165 100644 --- a/px4_roscon_workshop/custom_executor_demo/CustomMode.cpp +++ b/px4_roscon_workshop/custom_executor_demo/CustomMode.cpp @@ -43,12 +43,12 @@ void CustomWaypoints::onActivate() { // Initialize waypoints - _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 0.0f, -1.5f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 5.0f, -1.5f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(-5.0f, 5.0f, -1.5f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(-5.0f, -5.0f, -1.5f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, -5.0f, -1.5f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 0.0f, -1.5f)); + _trajectory_waypoints.emplace_back(5.0f, 0.0f, -1.5f); + _trajectory_waypoints.emplace_back(5.0f, 5.0f, -1.5f); + _trajectory_waypoints.emplace_back(-5.0f, 5.0f, -1.5f); + _trajectory_waypoints.emplace_back(-5.0f, -5.0f, -1.5f); + _trajectory_waypoints.emplace_back(5.0f, -5.0f, -1.5f); + _trajectory_waypoints.emplace_back(5.0f, 0.0f, -1.5f); _current_waypoint_index = 0; // Start at the first waypoint RCLCPP_INFO(_node.get_logger(), "CustomWaypoints mode activated"); @@ -92,9 +92,9 @@ void CustomYaw::onDeactivate() void CustomYaw::updateSetpoint([[maybe_unused]] float dt_s) { // Update the trajectory setpoint based on the current heading - Eigen::Vector3f velocity{0.0f, 0.0f, 0.0f}; - std::optional acceleration = std::nullopt; - std::optional yaw = std::nullopt; + Eigen::Vector3f const velocity{0.0f, 0.0f, 0.0f}; + std::optional const acceleration = std::nullopt; + std::optional const yaw = std::nullopt; std::optional yaw_rate = 0.2f; _trajectory_setpoint->update(velocity, acceleration, yaw, yaw_rate); _yaw_accumulator += yaw_rate.value() * dt_s; // Accumulate yaw rotation diff --git a/px4_roscon_workshop/custom_mode_demo/CustomMode.cpp b/px4_roscon_workshop/custom_mode_demo/CustomMode.cpp index f5168b1..64d92d6 100644 --- a/px4_roscon_workshop/custom_mode_demo/CustomMode.cpp +++ b/px4_roscon_workshop/custom_mode_demo/CustomMode.cpp @@ -25,13 +25,13 @@ void CustomWaypoints::onActivate() { // Initialize waypoints - _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 0.0f, -1.5f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 5.0f, -1.5f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(-5.0f, 5.0f, -1.5f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(-5.0f, -5.0f, -1.5f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, -5.0f, -1.5f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 0.0f, -1.5f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(0.0f, 0.0f, -1.5f)); + _trajectory_waypoints.emplace_back(5.0f, 0.0f, -1.5f); + _trajectory_waypoints.emplace_back(5.0f, 5.0f, -1.5f); + _trajectory_waypoints.emplace_back(-5.0f, 5.0f, -1.5f); + _trajectory_waypoints.emplace_back(-5.0f, -5.0f, -1.5f); + _trajectory_waypoints.emplace_back(5.0f, -5.0f, -1.5f); + _trajectory_waypoints.emplace_back(5.0f, 0.0f, -1.5f); + _trajectory_waypoints.emplace_back(0.0f, 0.0f, -1.5f); _current_waypoint_index = 0; RCLCPP_INFO(_node.get_logger(), "CustomWaypoints mode activated"); diff --git a/px4_roscon_workshop/custom_mode_demo/solution.cpp b/px4_roscon_workshop/custom_mode_demo/solution.cpp index b29fc00..fe8e9d2 100644 --- a/px4_roscon_workshop/custom_mode_demo/solution.cpp +++ b/px4_roscon_workshop/custom_mode_demo/solution.cpp @@ -27,13 +27,13 @@ class CustomWaypointsGoto : public px4_ros2::ModeBase { void onActivate() override { _trajectory_waypoints.clear(); - _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 0.0f, -1.5f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 5.0f, -1.5f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(-5.0f, 5.0f, -1.5f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(-5.0f, -5.0f, -1.5f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, -5.0f, -1.5f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 0.0f, -1.5f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(0.0f, 0.0f, -1.5f)); + _trajectory_waypoints.emplace_back(5.0f, 0.0f, -1.5f); + _trajectory_waypoints.emplace_back(5.0f, 5.0f, -1.5f); + _trajectory_waypoints.emplace_back(-5.0f, 5.0f, -1.5f); + _trajectory_waypoints.emplace_back(-5.0f, -5.0f, -1.5f); + _trajectory_waypoints.emplace_back(5.0f, -5.0f, -1.5f); + _trajectory_waypoints.emplace_back(5.0f, 0.0f, -1.5f); + _trajectory_waypoints.emplace_back(0.0f, 0.0f, -1.5f); _current_waypoint_index = 0; RCLCPP_INFO(_node.get_logger(), "CustomWaypointsGoto mode activated"); diff --git a/px4_roscon_workshop/formation_control/src/formation_control.cpp b/px4_roscon_workshop/formation_control/src/formation_control.cpp index 585e841..35280d5 100644 --- a/px4_roscon_workshop/formation_control/src/formation_control.cpp +++ b/px4_roscon_workshop/formation_control/src/formation_control.cpp @@ -25,7 +25,9 @@ FormationControlMode::FormationControlMode(rclcpp::Node& node, _has_global_position = true; if (msg.ref_timestamp != _last_global_ref_timestamp) { _last_global_ref_timestamp = msg.ref_timestamp; - double x, y, z; + double x; + double y; + double z; _geocentric.Forward(msg.ref_lat, msg.ref_lon, msg.ref_alt, x, y, z); RCLCPP_INFO(this->node().get_logger(), "Global position reference updated: lat=%f, lon=%f, alt=%f", msg.ref_lat, @@ -37,21 +39,21 @@ FormationControlMode::FormationControlMode(rclcpp::Node& node, _ekf_origin.transform.translation.x = x; _ekf_origin.transform.translation.y = y; _ekf_origin.transform.translation.z = z; - double cos_lat = std::cos(msg.ref_lat * M_PI / 180.0); - double sin_lat = std::sin(msg.ref_lat * M_PI / 180.0); - double cos_lon = std::cos(msg.ref_lon * M_PI / 180.0); - double sin_lon = std::sin(msg.ref_lon * M_PI / 180.0); - Eigen::Matrix3d R; - R(0, 0) = -sin_lon; - R(1, 0) = cos_lon; - R(2, 0) = 0.0; - R(0, 1) = -sin_lat * cos_lon; - R(1, 1) = -sin_lat * sin_lon; - R(2, 1) = cos_lat; - R(0, 2) = cos_lat * cos_lon; - R(1, 2) = cos_lat * sin_lon; - R(2, 2) = sin_lat; - Eigen::Quaterniond q(R); + double const cos_lat = std::cos(msg.ref_lat * M_PI / 180.0); + double const sin_lat = std::sin(msg.ref_lat * M_PI / 180.0); + double const cos_lon = std::cos(msg.ref_lon * M_PI / 180.0); + double const sin_lon = std::sin(msg.ref_lon * M_PI / 180.0); + Eigen::Matrix3d r; + r(0, 0) = -sin_lon; + r(1, 0) = cos_lon; + r(2, 0) = 0.0; + r(0, 1) = -sin_lat * cos_lon; + r(1, 1) = -sin_lat * sin_lon; + r(2, 1) = cos_lat; + r(0, 2) = cos_lat * cos_lon; + r(1, 2) = cos_lat * sin_lon; + r(2, 2) = sin_lat; + Eigen::Quaterniond q(r); _ekf_origin.transform.rotation.x = q.x(); _ekf_origin.transform.rotation.y = q.y(); _ekf_origin.transform.rotation.z = q.z(); @@ -91,15 +93,17 @@ void FormationControlMode::onActivate() void FormationControlMode::updateSetpoint(float dt_s) { Eigen::Vector2f velocity_en{0.0, 0.0}; - for (const auto& toFrameRel : _neighbor_base_link_frames) { + for (const auto& to_frame_rel : _neighbor_base_link_frames) { geometry_msgs::msg::TransformStamped t; try { - t = _tf_buffer->lookupTransform(toFrameRel, _tf_prefix + "base_link", tf2::TimePointZero); + t = _tf_buffer->lookupTransform(to_frame_rel, _tf_prefix + "base_link", + tf2::TimePointZero); const Eigen::Vector2f relative_en{t.transform.translation.x, t.transform.translation.y}; const float distance = relative_en.norm(); const Eigen::Vector2f direction = relative_en.normalized(); const float distance_error = - distance - _neighbor_distances[&toFrameRel - &_neighbor_base_link_frames[0]]; + distance - _neighbor_distances[&to_frame_rel - + _neighbor_base_link_frames.data()]; const Eigen::Vector2f individual_control = -distance_error * direction * _gain; velocity_en.x() += individual_control.x(); velocity_en.y() += individual_control.y(); diff --git a/px4_roscon_workshop/precision_land/PrecisionLand.cpp b/px4_roscon_workshop/precision_land/PrecisionLand.cpp index 043ee0e..8846bba 100644 --- a/px4_roscon_workshop/precision_land/PrecisionLand.cpp +++ b/px4_roscon_workshop/precision_land/PrecisionLand.cpp @@ -20,11 +20,16 @@ PrecisionLand::PrecisionLand(rclcpp::Node& node) : ModeBase(node, kModeName), _n _target_pose_sub = _node.create_subscription( "/target_pose", rclcpp::QoS(1).best_effort(), - std::bind(&PrecisionLand::targetPoseCallback, this, std::placeholders::_1)); + [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { + targetPoseCallback(msg); + }); - _vehicle_land_detected_sub = _node.create_subscription( - "/fmu/out/vehicle_land_detected", rclcpp::QoS(1).best_effort(), - std::bind(&PrecisionLand::vehicleLandDetectedCallback, this, std::placeholders::_1)); + _vehicle_land_detected_sub = + _node.create_subscription( + "/fmu/out/vehicle_land_detected", rclcpp::QoS(1).best_effort(), + [this](const px4_msgs::msg::VehicleLandDetected::SharedPtr msg) { + vehicleLandDetectedCallback(msg); + }); loadParameters(); } @@ -52,13 +57,12 @@ void PrecisionLand::loadParameters() } void PrecisionLand::vehicleLandDetectedCallback( - const px4_msgs::msg::VehicleLandDetected::SharedPtr msg) -{ + const px4_msgs::msg::VehicleLandDetected::SharedPtr& msg) { _land_detected = msg->landed; } -void PrecisionLand::targetPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) -{ +void PrecisionLand::targetPoseCallback( + const geometry_msgs::msg::PoseStamped::SharedPtr& msg) { if (_search_started) { auto tag = ArucoTag{ .position = @@ -78,16 +82,19 @@ PrecisionLand::ArucoTag PrecisionLand::getTagWorld(const ArucoTag& tag) // Convert from optical to FRD. This depends on camera mounting! // Optical (camera frame): X right, Y down, Z towards focal axis // FRD (drone frame/baselink): X forward, Y right, Z down - Eigen::Matrix3d R; - R << 0, -1, 0, 1, 0, 0, 0, 0, 1; - Eigen::Quaterniond quat_NED(R); + Eigen::Matrix3d r; + r << 0, -1, 0, 1, 0, 0, 0, 0, 1; + Eigen::Quaterniond const quat_ned(r); auto vehicle_position = Eigen::Vector3d(_vehicle_local_position->positionNed().cast()); auto vehicle_orientation = Eigen::Quaterniond(_vehicle_attitude->attitude().cast()); - Eigen::Affine3d drone_transform = Eigen::Translation3d(vehicle_position) * vehicle_orientation; - Eigen::Affine3d camera_transform = Eigen::Translation3d(0, 0, 0) * quat_NED; - Eigen::Affine3d tag_transform = Eigen::Translation3d(tag.position) * tag.orientation; + Eigen::Affine3d const drone_transform = + Eigen::Translation3d(vehicle_position) * vehicle_orientation; + Eigen::Affine3d const camera_transform = + Eigen::Translation3d(0, 0, 0) * quat_ned; + Eigen::Affine3d const tag_transform = + Eigen::Translation3d(tag.position) * tag.orientation; Eigen::Affine3d tag_world_transform = drone_transform * camera_transform * tag_transform; ArucoTag world_tag = { @@ -113,7 +120,7 @@ void PrecisionLand::onDeactivate() void PrecisionLand::updateSetpoint(float dt_s) { - bool target_lost = checkTargetTimeout(); + bool const target_lost = checkTargetTimeout(); if (target_lost && !_target_lost_prev) { RCLCPP_INFO(_node.get_logger(), "Target lost: State %s", stateName(_state).c_str()); @@ -204,28 +211,30 @@ void PrecisionLand::updateSetpoint(float dt_s) Eigen::Vector2f PrecisionLand::calculateVelocitySetpointXY() { - float p_gain = _param_vel_p_gain; - float i_gain = _param_vel_i_gain; + float const p_gain = _param_vel_p_gain; + float const i_gain = _param_vel_i_gain; // P component - float delta_pos_x = _vehicle_local_position->positionNed().x() - _tag.position.x(); - float delta_pos_y = _vehicle_local_position->positionNed().y() - _tag.position.y(); + float const delta_pos_x = + _vehicle_local_position->positionNed().x() - _tag.position.x(); + float const delta_pos_y = + _vehicle_local_position->positionNed().y() - _tag.position.y(); // I component _vel_x_integral += delta_pos_x; _vel_y_integral += delta_pos_y; - float max_integral = _param_max_velocity; + float const max_integral = _param_max_velocity; _vel_x_integral = std::clamp(_vel_x_integral, -1.f * max_integral, max_integral); _vel_y_integral = std::clamp(_vel_y_integral, -1.f * max_integral, max_integral); - float Xp = delta_pos_x * p_gain; - float Xi = _vel_x_integral * i_gain; - float Yp = delta_pos_y * p_gain; - float Yi = _vel_y_integral * i_gain; + float const xp = delta_pos_x * p_gain; + float const xi = _vel_x_integral * i_gain; + float const yp = delta_pos_y * p_gain; + float const yi = _vel_y_integral * i_gain; // Sum P and I gains - float vx = -1.f * (Xp + Xi); - float vy = -1.f * (Yp + Yi); + float vx = -1.f * (xp + xi); + float vy = -1.f * (yp + yi); // 0.1m/s min vel and 3m/s max vel vx = std::clamp(vx, -1.f * _param_max_velocity, _param_max_velocity); @@ -252,21 +261,22 @@ void PrecisionLand::generateSearchWaypoints() // Generate spiral search waypoints // The search waypoints are generated in the NED frame // Parameters for the search pattern - double start_x = _vehicle_local_position->positionNed().x(); - double start_y = _vehicle_local_position->positionNed().y(); + double const start_x = _vehicle_local_position->positionNed().x(); + double const start_y = _vehicle_local_position->positionNed().y(); double current_z = _vehicle_local_position->positionNed().z(); auto min_z = -1.0; - double max_radius = 2.0; - double layer_spacing = 0.5; - int points_per_layer = 16; + double const max_radius = 2.0; + double const layer_spacing = 0.5; + int const points_per_layer = 16; std::vector waypoints; // Generate waypoints // Calculate the number of layers needed - int num_layers = (static_cast((min_z - current_z) / layer_spacing) / 2) < 1 - ? 1 - : (static_cast((min_z - current_z) / layer_spacing) / 2); + int const num_layers = + (static_cast((min_z - current_z) / layer_spacing) / 2) < 1 + ? 1 + : (static_cast((min_z - current_z) / layer_spacing) / 2); // Generate waypoints for (int layer = 0; layer < num_layers; ++layer) { @@ -276,12 +286,12 @@ void PrecisionLand::generateSearchWaypoints() double radius = 0.0; for (int point = 0; point < points_per_layer + 1; ++point) { - double angle = 2.0 * M_PI * point / points_per_layer; - double x = start_x + radius * cos(angle); - double y = start_y + radius * sin(angle); - double z = current_z; + double const angle = 2.0 * M_PI * point / points_per_layer; + double const x = start_x + radius * cos(angle); + double const y = start_y + radius * sin(angle); + double const z = current_z; - layer_waypoints.push_back(Eigen::Vector3f(x, y, z)); + layer_waypoints.emplace_back(x, y, z); radius += max_radius / points_per_layer; } diff --git a/px4_roscon_workshop/precision_land/PrecisionLand.hpp b/px4_roscon_workshop/precision_land/PrecisionLand.hpp index 0360af1..1e1463d 100644 --- a/px4_roscon_workshop/precision_land/PrecisionLand.hpp +++ b/px4_roscon_workshop/precision_land/PrecisionLand.hpp @@ -20,8 +20,10 @@ class PrecisionLand : public px4_ros2::ModeBase { public: explicit PrecisionLand(rclcpp::Node& node); - void targetPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg); - void vehicleLandDetectedCallback(const px4_msgs::msg::VehicleLandDetected::SharedPtr msg); + void targetPoseCallback( + const geometry_msgs::msg::PoseStamped::SharedPtr& msg); + void vehicleLandDetectedCallback( + const px4_msgs::msg::VehicleLandDetected::SharedPtr& msg); // See ModeBasep void onActivate() override; diff --git a/px4_roscon_workshop/precision_land_executor/CustomMode_v2.cpp b/px4_roscon_workshop/precision_land_executor/CustomMode_v2.cpp index 7109401..11590f4 100644 --- a/px4_roscon_workshop/precision_land_executor/CustomMode_v2.cpp +++ b/px4_roscon_workshop/precision_land_executor/CustomMode_v2.cpp @@ -27,10 +27,10 @@ void CustomWaypoints::loadParameters() void CustomWaypoints::onActivate() { // Initialize waypoints - _trajectory_waypoints.push_back(Eigen::Vector3f(1.0f, 2.5f, -3.0f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(14.0f, 2.5f, -3.0f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(14.0f, 8.0f, -3.0f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(-4.0f, 8.0f, -3.0f)); + _trajectory_waypoints.emplace_back(1.0f, 2.5f, -3.0f); + _trajectory_waypoints.emplace_back(14.0f, 2.5f, -3.0f); + _trajectory_waypoints.emplace_back(14.0f, 8.0f, -3.0f); + _trajectory_waypoints.emplace_back(-4.0f, 8.0f, -3.0f); _current_waypoint_index = 0; // Start at the first waypoint RCLCPP_INFO(_node.get_logger(), "CustomWaypoints mode activated"); @@ -71,11 +71,16 @@ PrecisionLand::PrecisionLand(rclcpp::Node& node) _target_pose_sub = _node.create_subscription( "/target_pose", rclcpp::QoS(1).best_effort(), - std::bind(&PrecisionLand::targetPoseCallback, this, std::placeholders::_1)); + [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { + targetPoseCallback(msg); + }); - _vehicle_land_detected_sub = _node.create_subscription( - "/fmu/out/vehicle_land_detected", rclcpp::QoS(1).best_effort(), - std::bind(&PrecisionLand::vehicleLandDetectedCallback, this, std::placeholders::_1)); + _vehicle_land_detected_sub = + _node.create_subscription( + "/fmu/out/vehicle_land_detected", rclcpp::QoS(1).best_effort(), + [this](const px4_msgs::msg::VehicleLandDetected::SharedPtr msg) { + vehicleLandDetectedCallback(msg); + }); loadParameters(); } @@ -103,13 +108,12 @@ void PrecisionLand::loadParameters() } void PrecisionLand::vehicleLandDetectedCallback( - const px4_msgs::msg::VehicleLandDetected::SharedPtr msg) -{ + const px4_msgs::msg::VehicleLandDetected::SharedPtr& msg) { _land_detected = msg->landed; } -void PrecisionLand::targetPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) -{ +void PrecisionLand::targetPoseCallback( + const geometry_msgs::msg::PoseStamped::SharedPtr& msg) { if (_search_started) { auto tag = ArucoTag{ .position = @@ -129,16 +133,19 @@ PrecisionLand::ArucoTag PrecisionLand::getTagWorld(const ArucoTag& tag) // Convert from optical to NED // Optical: X right, Y down, Z away from lens // NED: X forward, Y right, Z away from viewer - Eigen::Matrix3d R; - R << 0, -1, 0, 1, 0, 0, 0, 0, 1; - Eigen::Quaterniond quat_NED(R); + Eigen::Matrix3d r; + r << 0, -1, 0, 1, 0, 0, 0, 0, 1; + Eigen::Quaterniond const quat_ned(r); auto vehicle_position = Eigen::Vector3d(_vehicle_local_position->positionNed().cast()); auto vehicle_orientation = Eigen::Quaterniond(_vehicle_attitude->attitude().cast()); - Eigen::Affine3d drone_transform = Eigen::Translation3d(vehicle_position) * vehicle_orientation; - Eigen::Affine3d camera_transform = Eigen::Translation3d(0, 0, 0) * quat_NED; - Eigen::Affine3d tag_transform = Eigen::Translation3d(tag.position) * tag.orientation; + Eigen::Affine3d const drone_transform = + Eigen::Translation3d(vehicle_position) * vehicle_orientation; + Eigen::Affine3d const camera_transform = + Eigen::Translation3d(0, 0, 0) * quat_ned; + Eigen::Affine3d const tag_transform = + Eigen::Translation3d(tag.position) * tag.orientation; Eigen::Affine3d tag_world_transform = drone_transform * camera_transform * tag_transform; ArucoTag world_tag = { @@ -164,7 +171,7 @@ void PrecisionLand::onDeactivate() void PrecisionLand::updateSetpoint([[maybe_unused]] float dt_s) { - bool target_lost = checkTargetTimeout(); + bool const target_lost = checkTargetTimeout(); if (target_lost && !_target_lost_prev) { RCLCPP_INFO(_node.get_logger(), "Target lost: State %s", stateName(_state).c_str()); @@ -255,28 +262,30 @@ void PrecisionLand::updateSetpoint([[maybe_unused]] float dt_s) Eigen::Vector2f PrecisionLand::calculateVelocitySetpointXY() { - float p_gain = _param_vel_p_gain; - float i_gain = _param_vel_i_gain; + float const p_gain = _param_vel_p_gain; + float const i_gain = _param_vel_i_gain; // P component - float delta_pos_x = _vehicle_local_position->positionNed().x() - _tag.position.x(); - float delta_pos_y = _vehicle_local_position->positionNed().y() - _tag.position.y(); + float const delta_pos_x = + _vehicle_local_position->positionNed().x() - _tag.position.x(); + float const delta_pos_y = + _vehicle_local_position->positionNed().y() - _tag.position.y(); // I component _vel_x_integral += delta_pos_x; _vel_y_integral += delta_pos_y; - float max_integral = _param_max_velocity; + float const max_integral = _param_max_velocity; _vel_x_integral = std::clamp(_vel_x_integral, -1.f * max_integral, max_integral); _vel_y_integral = std::clamp(_vel_y_integral, -1.f * max_integral, max_integral); - float Xp = delta_pos_x * p_gain; - float Xi = _vel_x_integral * i_gain; - float Yp = delta_pos_y * p_gain; - float Yi = _vel_y_integral * i_gain; + float const xp = delta_pos_x * p_gain; + float const xi = _vel_x_integral * i_gain; + float const yp = delta_pos_y * p_gain; + float const yi = _vel_y_integral * i_gain; // Sum P and I gains - float vx = -1.f * (Xp + Xi); - float vy = -1.f * (Yp + Yi); + float vx = -1.f * (xp + xi); + float vy = -1.f * (yp + yi); // 0.1m/s min vel and 3m/s max vel vx = std::clamp(vx, -1.f * _param_max_velocity, _param_max_velocity); @@ -303,21 +312,22 @@ void PrecisionLand::generateSearchWaypoints() // Generate spiral search waypoints // The search waypoints are generated in the NED frame // Parameters for the search pattern - double start_x = _vehicle_local_position->positionNed().x(); - double start_y = _vehicle_local_position->positionNed().y(); + double const start_x = _vehicle_local_position->positionNed().x(); + double const start_y = _vehicle_local_position->positionNed().y(); double current_z = _vehicle_local_position->positionNed().z(); auto min_z = -1.0; - double max_radius = 1.0; - double layer_spacing = 0.5; - int points_per_layer = 16; + double const max_radius = 1.0; + double const layer_spacing = 0.5; + int const points_per_layer = 16; std::vector waypoints; // Generate waypoints // Calculate the number of layers needed - int num_layers = (static_cast((min_z - current_z) / layer_spacing) / 2) < 1 - ? 1 - : (static_cast((min_z - current_z) / layer_spacing) / 2); + int const num_layers = + (static_cast((min_z - current_z) / layer_spacing) / 2) < 1 + ? 1 + : (static_cast((min_z - current_z) / layer_spacing) / 2); // Generate waypoints for (int layer = 0; layer < num_layers; ++layer) { @@ -327,12 +337,12 @@ void PrecisionLand::generateSearchWaypoints() double radius = 0.0; for (int point = 0; point < points_per_layer + 1; ++point) { - double angle = 2.0 * M_PI * point / points_per_layer; - double x = start_x + radius * cos(angle); - double y = start_y + radius * sin(angle); - double z = current_z; + double const angle = 2.0 * M_PI * point / points_per_layer; + double const x = start_x + radius * cos(angle); + double const y = start_y + radius * sin(angle); + double const z = current_z; - layer_waypoints.push_back(Eigen::Vector3f(x, y, z)); + layer_waypoints.emplace_back(x, y, z); radius += max_radius / points_per_layer; } diff --git a/px4_roscon_workshop/precision_land_executor/CustomMode_v2.hpp b/px4_roscon_workshop/precision_land_executor/CustomMode_v2.hpp index d28682f..86ef02a 100644 --- a/px4_roscon_workshop/precision_land_executor/CustomMode_v2.hpp +++ b/px4_roscon_workshop/precision_land_executor/CustomMode_v2.hpp @@ -56,8 +56,10 @@ class PrecisionLand : public px4_ros2::ModeBase { public: explicit PrecisionLand(rclcpp::Node& node); - void targetPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg); - void vehicleLandDetectedCallback(const px4_msgs::msg::VehicleLandDetected::SharedPtr msg); + void targetPoseCallback( + const geometry_msgs::msg::PoseStamped::SharedPtr& msg); + void vehicleLandDetectedCallback( + const px4_msgs::msg::VehicleLandDetected::SharedPtr& msg); // See ModeBase void onActivate() override; diff --git a/px4_roscon_workshop/px4_tf/include/px4_tf/px4_tf_publisher_node.hpp b/px4_roscon_workshop/px4_tf/include/px4_tf/px4_tf_publisher_node.hpp index 4c4a269..cda8143 100644 --- a/px4_roscon_workshop/px4_tf/include/px4_tf/px4_tf_publisher_node.hpp +++ b/px4_roscon_workshop/px4_tf/include/px4_tf/px4_tf_publisher_node.hpp @@ -16,5 +16,5 @@ class Px4TfPublisherNode : public rclcpp::Node { std::unique_ptr tf_static_broadcaster_; rclcpp::Subscription::SharedPtr odom_sub_; void make_static_transforms(); - void handle_odometry(const px4_msgs::msg::VehicleOdometry::SharedPtr msg); + void handle_odometry(const px4_msgs::msg::VehicleOdometry::SharedPtr& msg); }; \ No newline at end of file diff --git a/px4_roscon_workshop/px4_tf/src/px4_tf_publisher_node.cpp b/px4_roscon_workshop/px4_tf/src/px4_tf_publisher_node.cpp index 2f0d240..7fcbf28 100644 --- a/px4_roscon_workshop/px4_tf/src/px4_tf_publisher_node.cpp +++ b/px4_roscon_workshop/px4_tf/src/px4_tf_publisher_node.cpp @@ -46,8 +46,8 @@ void Px4TfPublisherNode::make_static_transforms() tf_static_broadcaster_->sendTransform(static_transforms); } -void Px4TfPublisherNode::handle_odometry(const px4_msgs::msg::VehicleOdometry::SharedPtr msg) -{ +void Px4TfPublisherNode::handle_odometry( + const px4_msgs::msg::VehicleOdometry::SharedPtr& msg) { geometry_msgs::msg::TransformStamped t; t.header.stamp = this->get_clock()->now(); t.header.frame_id = px4_tf_prefix_ + "odom_ned"; diff --git a/px4_roscon_workshop/rover_teleop/Teleop.cpp b/px4_roscon_workshop/rover_teleop/Teleop.cpp index c02d614..f735977 100644 --- a/px4_roscon_workshop/rover_teleop/Teleop.cpp +++ b/px4_roscon_workshop/rover_teleop/Teleop.cpp @@ -56,7 +56,7 @@ void Teleop::updateSetpoint([[maybe_unused]] float dt_s) { const auto now = _clock->now(); - if (((now - _last_twist_time) > _teleop_duration) || (_teleop_active == false)) { + if (((now - _last_twist_time) > _teleop_duration) || (!_teleop_active)) { RCLCPP_WARN( _node.get_logger(), "Teleop keyboard was closed or no Twist commands for %.0f seconds, exiting Teleop mode.", diff --git a/px4_roscon_workshop/teleop/Teleop.cpp b/px4_roscon_workshop/teleop/Teleop.cpp index 51aa716..00b7d04 100644 --- a/px4_roscon_workshop/teleop/Teleop.cpp +++ b/px4_roscon_workshop/teleop/Teleop.cpp @@ -1,5 +1,6 @@ #include "Teleop.hpp" +#include #include static const std::string kModeName = "Teleoperation"; @@ -55,7 +56,7 @@ void Teleop::updateSetpoint([[maybe_unused]] float dt_s) { const auto now = _clock->now(); - if (((now - _last_twist_time) > _teleop_duration) || (_teleop_active == false)) { + if (((now - _last_twist_time) > _teleop_duration) || (!_teleop_active)) { RCLCPP_WARN( _node.get_logger(), "Teleop keyboard was closed or no Twist commands for %.0f seconds, exiting Teleop mode.", @@ -66,23 +67,26 @@ void Teleop::updateSetpoint([[maybe_unused]] float dt_s) // Default values: zero velocity, no acceleration, no yaw input Eigen::Vector3f velocity_ned{0.f, 0.f, 0.f}; - std::optional acceleration = std::nullopt; - std::optional yaw = std::nullopt; + std::optional const acceleration = std::nullopt; + std::optional const yaw = std::nullopt; std::optional yaw_rate = std::nullopt; if ((now - _last_twist_time).seconds() <= 0.2) { // Convert Twist (assumed ENU) to NED // ENU: x=forward, y=left, z=up → NED: x=forward, y=right, z=down const geometry_msgs::msg::Twist& twist = _last_twist; - float yaw = _vehicle_attitude->yaw(); // Get current yaw from attitude + float const yaw = + _vehicle_attitude->yaw(); // Get current yaw from attitude // Convert to NED frame Eigen::Vector3f velocity_body; velocity_body.x() = twist.linear.x; velocity_body.y() = twist.linear.y; velocity_body.z() = twist.linear.z; - velocity_ned.x() = velocity_body.x() * cos(yaw) - velocity_body.y() * sin(yaw); - velocity_ned.y() = velocity_body.x() * sin(yaw) + velocity_body.y() * cos(yaw); + velocity_ned.x() = + velocity_body.x() * std::cos(yaw) - velocity_body.y() * std::sin(yaw); + velocity_ned.y() = + velocity_body.x() * std::sin(yaw) + velocity_body.y() * std::cos(yaw); velocity_ned.z() = velocity_body.z(); yaw_rate = -twist.angular.z; // ENU and NED both define yaw CCW from north From 81bcf7930f3bfeb300c6636519ad5c24e0d0f465 Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Thu, 27 Aug 2026 00:04:46 +0100 Subject: [PATCH 4/4] chore: re-run clang-format Signed-off-by: Beniamino Pozzan --- .../aruco_tracker/ArucoTracker.cpp | 60 +++++++++---------- .../aruco_tracker/ArucoTracker.hpp | 3 +- .../src/formation_control.cpp | 6 +- .../precision_land/PrecisionLand.cpp | 41 ++++++------- .../precision_land/PrecisionLand.hpp | 6 +- .../precision_land_executor/CustomMode_v2.cpp | 41 ++++++------- .../precision_land_executor/CustomMode_v2.hpp | 6 +- .../px4_tf/src/px4_tf_publisher_node.cpp | 4 +- px4_roscon_workshop/teleop/Teleop.cpp | 9 +-- 9 files changed, 74 insertions(+), 102 deletions(-) diff --git a/px4_roscon_workshop/aruco_tracker/ArucoTracker.cpp b/px4_roscon_workshop/aruco_tracker/ArucoTracker.cpp index 9752785..beab5ed 100644 --- a/px4_roscon_workshop/aruco_tracker/ArucoTracker.cpp +++ b/px4_roscon_workshop/aruco_tracker/ArucoTracker.cpp @@ -27,9 +27,8 @@ ArucoTrackerNode::ArucoTrackerNode() : Node("aruco_tracker_node") [this](const sensor_msgs::msg::Image::SharedPtr msg) { image_callback(msg); }); _camera_info_sub = create_subscription( - "/camera_info", qos, [this](const sensor_msgs::msg::CameraInfo::SharedPtr msg) { - camera_info_callback(msg); - }); + "/camera_info", qos, + [this](const sensor_msgs::msg::CameraInfo::SharedPtr msg) { camera_info_callback(msg); }); // Publishers _image_pub = create_publisher("/image_proc", qos); @@ -47,8 +46,8 @@ void ArucoTrackerNode::loadParameters() get_parameter("marker_size", _param_marker_size); } -void ArucoTrackerNode::image_callback( - const sensor_msgs::msg::Image::SharedPtr& msg) { +void ArucoTrackerNode::image_callback(const sensor_msgs::msg::Image::SharedPtr& msg) +{ try { // Convert ROS image message to OpenCV image cv_bridge::CvImagePtr const cv_ptr = @@ -65,8 +64,8 @@ void ArucoTrackerNode::image_callback( for (const auto& corner : corners) { std::vector undistorted_corner; - cv::undistortPoints(corner, undistorted_corner, _camera_matrix, - _dist_coeffs, cv::noArray(), _camera_matrix); + cv::undistortPoints(corner, undistorted_corner, _camera_matrix, _dist_coeffs, cv::noArray(), + _camera_matrix); undistorted_corners.push_back(undistorted_corner); } @@ -87,8 +86,8 @@ void ArucoTrackerNode::image_callback( // Use PnP solver to estimate pose cv::Vec3d rvec; cv::Vec3d tvec; - cv::solvePnP(object_points, undistorted_corners[i], _camera_matrix, - cv::noArray(), rvec, tvec); + cv::solvePnP(object_points, undistorted_corners[i], _camera_matrix, cv::noArray(), rvec, + tvec); // Annotate the image cv::drawFrameAxes(cv_ptr->image, _camera_matrix, cv::noArray(), rvec, tvec, _param_marker_size); @@ -97,11 +96,9 @@ void ArucoTrackerNode::image_callback( cv::Mat rot_mat; cv::Rodrigues(rvec, rot_mat); tf2::Matrix3x3 const tf_rotation( - rot_mat.at(0, 0), rot_mat.at(0, 1), - rot_mat.at(0, 2), rot_mat.at(1, 0), - rot_mat.at(1, 1), rot_mat.at(1, 2), - rot_mat.at(2, 0), rot_mat.at(2, 1), - rot_mat.at(2, 2)); + rot_mat.at(0, 0), rot_mat.at(0, 1), rot_mat.at(0, 2), + rot_mat.at(1, 0), rot_mat.at(1, 1), rot_mat.at(1, 2), + rot_mat.at(2, 0), rot_mat.at(2, 1), rot_mat.at(2, 2)); tf2::Quaternion quat; tf_rotation.getRotation(quat); quat.normalize(); @@ -140,8 +137,8 @@ void ArucoTrackerNode::image_callback( } } -void ArucoTrackerNode::camera_info_callback( - const sensor_msgs::msg::CameraInfo::SharedPtr& msg) { +void ArucoTrackerNode::camera_info_callback(const sensor_msgs::msg::CameraInfo::SharedPtr& msg) +{ // Always update the camera matrix and distortion coefficients from the new message _camera_matrix = cv::Mat(3, 3, CV_64F, const_cast(msg->k.data())) .clone(); // Use clone to ensure a deep copy @@ -174,8 +171,8 @@ void ArucoTrackerNode::camera_info_callback( } } -void ArucoTrackerNode::annotate_image(const cv_bridge::CvImagePtr& image, - const cv::Vec3d& target) { +void ArucoTrackerNode::annotate_image(const cv_bridge::CvImagePtr& image, const cv::Vec3d& target) +{ // Annotate the image with the target position and marker size std::ostringstream stream; stream << std::fixed << std::setprecision(2); @@ -186,13 +183,11 @@ void ArucoTrackerNode::annotate_image(const cv_bridge::CvImagePtr& image, double const font_scale = 1; int const thickness = 2; int baseline = 0; - cv::Size const text_size = - cv::getTextSize(text_xyz, font_face, font_scale, thickness, &baseline); + cv::Size const text_size = cv::getTextSize(text_xyz, font_face, font_scale, thickness, &baseline); baseline += thickness; - cv::Point const text_org((image->image.cols - text_size.width - 10), - (image->image.rows - 10)); - cv::putText(image->image, text_xyz, text_org, font_face, font_scale, - cv::Scalar(0, 255, 255), thickness, 8); + cv::Point const text_org((image->image.cols - text_size.width - 10), (image->image.rows - 10)); + cv::putText(image->image, text_xyz, text_org, font_face, font_scale, cv::Scalar(0, 255, 255), + thickness, 8); } int main(int argc, char** argv) @@ -281,7 +276,8 @@ int main(int argc, char** argv) // for (const auto& corner : corners) { // std::vector undistortedCorner; // cv::undistortPoints(corner, undistortedCorner, _camera_matrix, -// _dist_coeffs, cv::noArray(), _camera_matrix); undistortedCorners.push_back(undistortedCorner); +// _dist_coeffs, cv::noArray(), _camera_matrix); +// undistortedCorners.push_back(undistortedCorner); // } // for (size_t i = 0; i < ids.size(); i++) { @@ -290,8 +286,8 @@ int main(int argc, char** argv) // } // // verify the marker size using the distance to ground // float pixel_width = cv::norm(undistortedCorners[i][0] - -// undistortedCorners[i][1]); float focal_length = _camera_matrix.at(0, 0); -// _calculated_marker_size = (pixel_width / focal_length) * +// undistortedCorners[i][1]); float focal_length = +// _camera_matrix.at(0, 0); _calculated_marker_size = (pixel_width / focal_length) * // _distance_to_ground; // // RCLCPP_INFO(get_logger(), "Calculated marker size: %f", // _calculated_marker_size); @@ -365,11 +361,11 @@ int main(int argc, char** argv) // // Log the first row of the camera matrix to verify correct values // RCLCPP_INFO(get_logger(), "Camera matrix updated:\n[%f, %f, %f]\n[%f, %f, %f]\n[%f, %f, // %f]", _camera_matrix.at(0, 0), _camera_matrix.at(0, 1), -// _camera_matrix.at(0, 2), _camera_matrix.at(1, 0), _camera_matrix.at(1, -// 1), _camera_matrix.at(1, 2), _camera_matrix.at(2, 0), -// _camera_matrix.at(2, 1), _camera_matrix.at(2, 2)); RCLCPP_INFO(get_logger(), -// "Camera Matrix: fx=%f, fy=%f, cx=%f, cy=%f", _camera_matrix.at(0, 0), // fx -// _camera_matrix.at(1, 1), // fy +// _camera_matrix.at(0, 2), _camera_matrix.at(1, 0), +// _camera_matrix.at(1, 1), _camera_matrix.at(1, 2), +// _camera_matrix.at(2, 0), _camera_matrix.at(2, 1), _camera_matrix.at(2, +// 2)); RCLCPP_INFO(get_logger(), "Camera Matrix: fx=%f, fy=%f, cx=%f, cy=%f", +// _camera_matrix.at(0, 0), // fx _camera_matrix.at(1, 1), // fy // _camera_matrix.at(0, 2), // cx // _camera_matrix.at(1, 2) // cy // ); diff --git a/px4_roscon_workshop/aruco_tracker/ArucoTracker.hpp b/px4_roscon_workshop/aruco_tracker/ArucoTracker.hpp index b411a2c..333718e 100644 --- a/px4_roscon_workshop/aruco_tracker/ArucoTracker.hpp +++ b/px4_roscon_workshop/aruco_tracker/ArucoTracker.hpp @@ -20,8 +20,7 @@ class ArucoTrackerNode : public rclcpp::Node { void image_callback(const sensor_msgs::msg::Image::SharedPtr& msg); void camera_info_callback(const sensor_msgs::msg::CameraInfo::SharedPtr& msg); - void annotate_image(const cv_bridge::CvImagePtr& image, - const cv::Vec3d& target); + void annotate_image(const cv_bridge::CvImagePtr& image, const cv::Vec3d& target); rclcpp::Subscription::SharedPtr _image_sub; rclcpp::Subscription::SharedPtr _camera_info_sub; diff --git a/px4_roscon_workshop/formation_control/src/formation_control.cpp b/px4_roscon_workshop/formation_control/src/formation_control.cpp index 35280d5..92c46f5 100644 --- a/px4_roscon_workshop/formation_control/src/formation_control.cpp +++ b/px4_roscon_workshop/formation_control/src/formation_control.cpp @@ -96,14 +96,12 @@ void FormationControlMode::updateSetpoint(float dt_s) for (const auto& to_frame_rel : _neighbor_base_link_frames) { geometry_msgs::msg::TransformStamped t; try { - t = _tf_buffer->lookupTransform(to_frame_rel, _tf_prefix + "base_link", - tf2::TimePointZero); + t = _tf_buffer->lookupTransform(to_frame_rel, _tf_prefix + "base_link", tf2::TimePointZero); const Eigen::Vector2f relative_en{t.transform.translation.x, t.transform.translation.y}; const float distance = relative_en.norm(); const Eigen::Vector2f direction = relative_en.normalized(); const float distance_error = - distance - _neighbor_distances[&to_frame_rel - - _neighbor_base_link_frames.data()]; + distance - _neighbor_distances[&to_frame_rel - _neighbor_base_link_frames.data()]; const Eigen::Vector2f individual_control = -distance_error * direction * _gain; velocity_en.x() += individual_control.x(); velocity_en.y() += individual_control.y(); diff --git a/px4_roscon_workshop/precision_land/PrecisionLand.cpp b/px4_roscon_workshop/precision_land/PrecisionLand.cpp index 8846bba..5ec2c3a 100644 --- a/px4_roscon_workshop/precision_land/PrecisionLand.cpp +++ b/px4_roscon_workshop/precision_land/PrecisionLand.cpp @@ -20,16 +20,13 @@ PrecisionLand::PrecisionLand(rclcpp::Node& node) : ModeBase(node, kModeName), _n _target_pose_sub = _node.create_subscription( "/target_pose", rclcpp::QoS(1).best_effort(), - [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { - targetPoseCallback(msg); - }); + [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { targetPoseCallback(msg); }); - _vehicle_land_detected_sub = - _node.create_subscription( - "/fmu/out/vehicle_land_detected", rclcpp::QoS(1).best_effort(), - [this](const px4_msgs::msg::VehicleLandDetected::SharedPtr msg) { - vehicleLandDetectedCallback(msg); - }); + _vehicle_land_detected_sub = _node.create_subscription( + "/fmu/out/vehicle_land_detected", rclcpp::QoS(1).best_effort(), + [this](const px4_msgs::msg::VehicleLandDetected::SharedPtr msg) { + vehicleLandDetectedCallback(msg); + }); loadParameters(); } @@ -57,12 +54,13 @@ void PrecisionLand::loadParameters() } void PrecisionLand::vehicleLandDetectedCallback( - const px4_msgs::msg::VehicleLandDetected::SharedPtr& msg) { + const px4_msgs::msg::VehicleLandDetected::SharedPtr& msg) +{ _land_detected = msg->landed; } -void PrecisionLand::targetPoseCallback( - const geometry_msgs::msg::PoseStamped::SharedPtr& msg) { +void PrecisionLand::targetPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr& msg) +{ if (_search_started) { auto tag = ArucoTag{ .position = @@ -91,10 +89,8 @@ PrecisionLand::ArucoTag PrecisionLand::getTagWorld(const ArucoTag& tag) Eigen::Affine3d const drone_transform = Eigen::Translation3d(vehicle_position) * vehicle_orientation; - Eigen::Affine3d const camera_transform = - Eigen::Translation3d(0, 0, 0) * quat_ned; - Eigen::Affine3d const tag_transform = - Eigen::Translation3d(tag.position) * tag.orientation; + Eigen::Affine3d const camera_transform = Eigen::Translation3d(0, 0, 0) * quat_ned; + Eigen::Affine3d const tag_transform = Eigen::Translation3d(tag.position) * tag.orientation; Eigen::Affine3d tag_world_transform = drone_transform * camera_transform * tag_transform; ArucoTag world_tag = { @@ -215,10 +211,8 @@ Eigen::Vector2f PrecisionLand::calculateVelocitySetpointXY() float const i_gain = _param_vel_i_gain; // P component - float const delta_pos_x = - _vehicle_local_position->positionNed().x() - _tag.position.x(); - float const delta_pos_y = - _vehicle_local_position->positionNed().y() - _tag.position.y(); + float const delta_pos_x = _vehicle_local_position->positionNed().x() - _tag.position.x(); + float const delta_pos_y = _vehicle_local_position->positionNed().y() - _tag.position.y(); // I component _vel_x_integral += delta_pos_x; @@ -273,10 +267,9 @@ void PrecisionLand::generateSearchWaypoints() // Generate waypoints // Calculate the number of layers needed - int const num_layers = - (static_cast((min_z - current_z) / layer_spacing) / 2) < 1 - ? 1 - : (static_cast((min_z - current_z) / layer_spacing) / 2); + int const num_layers = (static_cast((min_z - current_z) / layer_spacing) / 2) < 1 + ? 1 + : (static_cast((min_z - current_z) / layer_spacing) / 2); // Generate waypoints for (int layer = 0; layer < num_layers; ++layer) { diff --git a/px4_roscon_workshop/precision_land/PrecisionLand.hpp b/px4_roscon_workshop/precision_land/PrecisionLand.hpp index 1e1463d..c41cec3 100644 --- a/px4_roscon_workshop/precision_land/PrecisionLand.hpp +++ b/px4_roscon_workshop/precision_land/PrecisionLand.hpp @@ -20,10 +20,8 @@ class PrecisionLand : public px4_ros2::ModeBase { public: explicit PrecisionLand(rclcpp::Node& node); - void targetPoseCallback( - const geometry_msgs::msg::PoseStamped::SharedPtr& msg); - void vehicleLandDetectedCallback( - const px4_msgs::msg::VehicleLandDetected::SharedPtr& msg); + void targetPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr& msg); + void vehicleLandDetectedCallback(const px4_msgs::msg::VehicleLandDetected::SharedPtr& msg); // See ModeBasep void onActivate() override; diff --git a/px4_roscon_workshop/precision_land_executor/CustomMode_v2.cpp b/px4_roscon_workshop/precision_land_executor/CustomMode_v2.cpp index 11590f4..ab8f967 100644 --- a/px4_roscon_workshop/precision_land_executor/CustomMode_v2.cpp +++ b/px4_roscon_workshop/precision_land_executor/CustomMode_v2.cpp @@ -71,16 +71,13 @@ PrecisionLand::PrecisionLand(rclcpp::Node& node) _target_pose_sub = _node.create_subscription( "/target_pose", rclcpp::QoS(1).best_effort(), - [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { - targetPoseCallback(msg); - }); + [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { targetPoseCallback(msg); }); - _vehicle_land_detected_sub = - _node.create_subscription( - "/fmu/out/vehicle_land_detected", rclcpp::QoS(1).best_effort(), - [this](const px4_msgs::msg::VehicleLandDetected::SharedPtr msg) { - vehicleLandDetectedCallback(msg); - }); + _vehicle_land_detected_sub = _node.create_subscription( + "/fmu/out/vehicle_land_detected", rclcpp::QoS(1).best_effort(), + [this](const px4_msgs::msg::VehicleLandDetected::SharedPtr msg) { + vehicleLandDetectedCallback(msg); + }); loadParameters(); } @@ -108,12 +105,13 @@ void PrecisionLand::loadParameters() } void PrecisionLand::vehicleLandDetectedCallback( - const px4_msgs::msg::VehicleLandDetected::SharedPtr& msg) { + const px4_msgs::msg::VehicleLandDetected::SharedPtr& msg) +{ _land_detected = msg->landed; } -void PrecisionLand::targetPoseCallback( - const geometry_msgs::msg::PoseStamped::SharedPtr& msg) { +void PrecisionLand::targetPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr& msg) +{ if (_search_started) { auto tag = ArucoTag{ .position = @@ -142,10 +140,8 @@ PrecisionLand::ArucoTag PrecisionLand::getTagWorld(const ArucoTag& tag) Eigen::Affine3d const drone_transform = Eigen::Translation3d(vehicle_position) * vehicle_orientation; - Eigen::Affine3d const camera_transform = - Eigen::Translation3d(0, 0, 0) * quat_ned; - Eigen::Affine3d const tag_transform = - Eigen::Translation3d(tag.position) * tag.orientation; + Eigen::Affine3d const camera_transform = Eigen::Translation3d(0, 0, 0) * quat_ned; + Eigen::Affine3d const tag_transform = Eigen::Translation3d(tag.position) * tag.orientation; Eigen::Affine3d tag_world_transform = drone_transform * camera_transform * tag_transform; ArucoTag world_tag = { @@ -266,10 +262,8 @@ Eigen::Vector2f PrecisionLand::calculateVelocitySetpointXY() float const i_gain = _param_vel_i_gain; // P component - float const delta_pos_x = - _vehicle_local_position->positionNed().x() - _tag.position.x(); - float const delta_pos_y = - _vehicle_local_position->positionNed().y() - _tag.position.y(); + float const delta_pos_x = _vehicle_local_position->positionNed().x() - _tag.position.x(); + float const delta_pos_y = _vehicle_local_position->positionNed().y() - _tag.position.y(); // I component _vel_x_integral += delta_pos_x; @@ -324,10 +318,9 @@ void PrecisionLand::generateSearchWaypoints() // Generate waypoints // Calculate the number of layers needed - int const num_layers = - (static_cast((min_z - current_z) / layer_spacing) / 2) < 1 - ? 1 - : (static_cast((min_z - current_z) / layer_spacing) / 2); + int const num_layers = (static_cast((min_z - current_z) / layer_spacing) / 2) < 1 + ? 1 + : (static_cast((min_z - current_z) / layer_spacing) / 2); // Generate waypoints for (int layer = 0; layer < num_layers; ++layer) { diff --git a/px4_roscon_workshop/precision_land_executor/CustomMode_v2.hpp b/px4_roscon_workshop/precision_land_executor/CustomMode_v2.hpp index 86ef02a..0c325f1 100644 --- a/px4_roscon_workshop/precision_land_executor/CustomMode_v2.hpp +++ b/px4_roscon_workshop/precision_land_executor/CustomMode_v2.hpp @@ -56,10 +56,8 @@ class PrecisionLand : public px4_ros2::ModeBase { public: explicit PrecisionLand(rclcpp::Node& node); - void targetPoseCallback( - const geometry_msgs::msg::PoseStamped::SharedPtr& msg); - void vehicleLandDetectedCallback( - const px4_msgs::msg::VehicleLandDetected::SharedPtr& msg); + void targetPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr& msg); + void vehicleLandDetectedCallback(const px4_msgs::msg::VehicleLandDetected::SharedPtr& msg); // See ModeBase void onActivate() override; diff --git a/px4_roscon_workshop/px4_tf/src/px4_tf_publisher_node.cpp b/px4_roscon_workshop/px4_tf/src/px4_tf_publisher_node.cpp index 7fcbf28..a1411ce 100644 --- a/px4_roscon_workshop/px4_tf/src/px4_tf_publisher_node.cpp +++ b/px4_roscon_workshop/px4_tf/src/px4_tf_publisher_node.cpp @@ -46,8 +46,8 @@ void Px4TfPublisherNode::make_static_transforms() tf_static_broadcaster_->sendTransform(static_transforms); } -void Px4TfPublisherNode::handle_odometry( - const px4_msgs::msg::VehicleOdometry::SharedPtr& msg) { +void Px4TfPublisherNode::handle_odometry(const px4_msgs::msg::VehicleOdometry::SharedPtr& msg) +{ geometry_msgs::msg::TransformStamped t; t.header.stamp = this->get_clock()->now(); t.header.frame_id = px4_tf_prefix_ + "odom_ned"; diff --git a/px4_roscon_workshop/teleop/Teleop.cpp b/px4_roscon_workshop/teleop/Teleop.cpp index 00b7d04..f1e8163 100644 --- a/px4_roscon_workshop/teleop/Teleop.cpp +++ b/px4_roscon_workshop/teleop/Teleop.cpp @@ -75,18 +75,15 @@ void Teleop::updateSetpoint([[maybe_unused]] float dt_s) // Convert Twist (assumed ENU) to NED // ENU: x=forward, y=left, z=up → NED: x=forward, y=right, z=down const geometry_msgs::msg::Twist& twist = _last_twist; - float const yaw = - _vehicle_attitude->yaw(); // Get current yaw from attitude + float const yaw = _vehicle_attitude->yaw(); // Get current yaw from attitude // Convert to NED frame Eigen::Vector3f velocity_body; velocity_body.x() = twist.linear.x; velocity_body.y() = twist.linear.y; velocity_body.z() = twist.linear.z; - velocity_ned.x() = - velocity_body.x() * std::cos(yaw) - velocity_body.y() * std::sin(yaw); - velocity_ned.y() = - velocity_body.x() * std::sin(yaw) + velocity_body.y() * std::cos(yaw); + velocity_ned.x() = velocity_body.x() * std::cos(yaw) - velocity_body.y() * std::sin(yaw); + velocity_ned.y() = velocity_body.x() * std::sin(yaw) + velocity_body.y() * std::cos(yaw); velocity_ned.z() = velocity_body.z(); yaw_rate = -twist.angular.z; // ENU and NED both define yaw CCW from north