From aa937e2da26eb7325b5df1df4c54f0dd78fe11f9 Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Wed, 26 Aug 2026 23:37:20 +0100 Subject: [PATCH 1/5] 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/5] 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/5] 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/5] 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 From 29cd84a0c6a17571cfed9cdc9b48a218fe08cb69 Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Wed, 2 Sep 2026 23:28:46 +0100 Subject: [PATCH 5/5] chore: re-apply format Signed-off-by: Beniamino Pozzan --- README.md | 2 +- docker/scripts/build_px4.sh | 6 +- docker/scripts/install_deps.sh | 2 +- docs/setup.md | 8 +- .../aruco_tracker/ArucoTracker.cpp | 4 +- .../launch/aruco_tracker.launch.py | 127 +++++----- .../launch/custom_executor_demo.launch.py | 58 +++-- .../launch/custom_mode_demo.launch.py | 58 +++-- .../launch/formation.launch.py | 228 +++++++++--------- .../launch/precision_land_executor.launch.py | 66 ++--- .../launch/common.launch.py | 135 +++++------ .../launch/gz_world.launch.py | 138 ++++++----- .../px4_roscon_workshop/launch/px4.launch.py | 159 ++++++------ .../launch/px4_vehicle.launch.py | 208 ++++++++-------- px4_roscon_workshop/px4_tf/README.md | 2 +- .../rover_teleop/launch/rover_launch.py | 69 +++--- .../teleop/launch/teleop.launch.py | 163 +++++++------ .../teleop_twist_rpyt_keyboard/setup.py | 25 +- .../teleop_twist_rpyt_keyboard.py | 54 +++-- 19 files changed, 790 insertions(+), 722 deletions(-) diff --git a/README.md b/README.md index be48fff..f55df81 100644 --- a/README.md +++ b/README.md @@ -68,7 +68,7 @@ In this section, we explore **three practical examples** of perception and contr 1. **ArUco Marker Detection** – Detect markers using ROS 2 and PX4. No custom flight mode is required. 2. **Teleoperation** – Ever seen a TurtleBot flying? This demo shows how to manually control a drone using a keyboard and to use a LiDAR scan for environmental awareness. 3. **Precision Landing** – Combine ArUco detection with a **Custom Mode** to perform precision landing. - - **Precision Landing with Executor** – This is a follow up exercise to incorporate Precision Land in the former Custom Modes Demo, where an Executor schedules Waypoints and Precison Land to find and land on the ArUco Marker in the maze. + - **Precision Landing with Executor** – This is a follow up exercise to incorporate Precision Land in the former Custom Modes Demo, where an Executor schedules Waypoints and Precison Land to find and land on the ArUco Marker in the maze. For more detailed instructions and exercises, refer to the following demos: diff --git a/docker/scripts/build_px4.sh b/docker/scripts/build_px4.sh index b49351b..ccbc4b5 100755 --- a/docker/scripts/build_px4.sh +++ b/docker/scripts/build_px4.sh @@ -16,7 +16,7 @@ index fed292b9d1..1d34854077 100644 @@ -126,15 +126,6 @@ then set AUTOCNF yes fi - + -# Allow overriding parameters via env variables: export PX4_PARAM_{name}={value} -env | while IFS='=' read -r line; do - value=${line#*=} @@ -32,7 +32,7 @@ index fed292b9d1..1d34854077 100644 @@ -239,6 +230,15 @@ then exit 1 fi - + +# Allow overriding parameters via env variables: export PX4_PARAM_{name}={value} +env | while IFS='=' read -r line; do + value=${line#*=} @@ -43,7 +43,7 @@ index fed292b9d1..1d34854077 100644 +done + dataman start - + # only start the simulator if not in replay mode, as both control the lockstep time EOF diff --git a/docker/scripts/install_deps.sh b/docker/scripts/install_deps.sh index 66e631d..74d9770 100755 --- a/docker/scripts/install_deps.sh +++ b/docker/scripts/install_deps.sh @@ -35,7 +35,7 @@ apt-get install -y --no-install-recommends \ ros-humble-gps-msgs \ ros-humble-vision-msgs \ libgflags-dev \ - python3-rospkg + python3-rospkg rm -rf /var/lib/apt/lists/* apt-get clean \ No newline at end of file diff --git a/docs/setup.md b/docs/setup.md index e112ebf..06e1e53 100644 --- a/docs/setup.md +++ b/docs/setup.md @@ -35,7 +35,7 @@ Download QGC ### ROS 2 workspace setup 1. Create a ROS 2 workspace - + ```bash mkdir -p ~/workspaces/px4_roscon26_ws/src cd ~/workspaces/px4_roscon26_ws/src @@ -51,14 +51,14 @@ Pick `ros-jazzy-desktop`. ``` 1. Clone the dependencies - + ```bash cd ~/workspaces/px4_roscon26_ws vcs import src < ./src/roscon-25-workshop/jazzy.repos ``` 1. Install dependencies - + ```bash cd ~/workspaces/px4_roscon26_ws source /opt/ros/jazzy/setup.bash @@ -66,7 +66,7 @@ Pick `ros-jazzy-desktop`. ``` 1. Build the workspace - + ```bash cd ~/workspaces/px4_roscon26_ws source /opt/ros/jazzy/setup.bash diff --git a/px4_roscon_workshop/aruco_tracker/ArucoTracker.cpp b/px4_roscon_workshop/aruco_tracker/ArucoTracker.cpp index beab5ed..bf17782 100644 --- a/px4_roscon_workshop/aruco_tracker/ArucoTracker.cpp +++ b/px4_roscon_workshop/aruco_tracker/ArucoTracker.cpp @@ -287,8 +287,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) * -// _distance_to_ground; +// _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); diff --git a/px4_roscon_workshop/aruco_tracker/launch/aruco_tracker.launch.py b/px4_roscon_workshop/aruco_tracker/launch/aruco_tracker.launch.py index 23b5b74..4f9efd2 100644 --- a/px4_roscon_workshop/aruco_tracker/launch/aruco_tracker.launch.py +++ b/px4_roscon_workshop/aruco_tracker/launch/aruco_tracker.launch.py @@ -10,31 +10,33 @@ from launch_ros.substitutions import FindPackageShare from launch_ros.descriptions import ComposableNode + def launch_setup(context, *args, **kwargs): return_array = [] world_name = LaunchConfiguration("world_name").perform(context) model_name = LaunchConfiguration("model_name").perform(context) - camera_topic = f"/world/{world_name}/model/{model_name}/link/camera_link/sensor/camera/image" - camera_info_topic = f"/world/{world_name}/model/{model_name}/link/camera_link/sensor/camera/camera_info" + camera_topic = ( + f"/world/{world_name}/model/{model_name}/link/camera_link/sensor/camera/image" + ) pkg_share = FindPackageShare("aruco_tracker").find("aruco_tracker") - bridge_config_file = os.path.join(pkg_share,"cfg","bridge.yaml") - aruco_tracker_config_file = os.path.join(pkg_share, 'cfg', 'params.yaml') + bridge_config_file = os.path.join(pkg_share, "cfg", "bridge.yaml") + aruco_tracker_config_file = os.path.join(pkg_share, "cfg", "params.yaml") - with open(bridge_config_file, 'r') as file: + with open(bridge_config_file, "r") as file: bridge_config = yaml.safe_load(file) for item in bridge_config: - item['gz_topic_name'] = item['gz_topic_name'].format( + item["gz_topic_name"] = item["gz_topic_name"].format( world_name=world_name, model_name=model_name, ) tmp_file = tempfile.NamedTemporaryFile(suffix=".yaml", delete=False) - with open(tmp_file.name, 'w') as file: + with open(tmp_file.name, "w") as file: yaml.dump(bridge_config, file) return_array.append( @@ -42,9 +44,7 @@ def launch_setup(context, *args, **kwargs): package="ros_gz_bridge", executable="parameter_bridge", name="gz_bridge", - parameters=[ - {"config_file": tmp_file.name} - ], + parameters=[{"config_file": tmp_file.name}], ) ) return_array.append( @@ -53,10 +53,7 @@ def launch_setup(context, *args, **kwargs): executable="aruco_tracker", name="aruco_tracker", output="screen", - parameters=[ - {"use_sim_time": True}, - aruco_tracker_config_file - ] + parameters=[{"use_sim_time": True}, aruco_tracker_config_file], ) ) return_array.append( @@ -76,61 +73,67 @@ def launch_setup(context, *args, **kwargs): ) return_array.append( LoadComposableNodes( - target_container='static_tf_container', + target_container="static_tf_container", composable_node_descriptions=[ ComposableNode( - package='tf2_ros', - plugin='tf2_ros::StaticTransformBroadcasterNode', - name='base_link_to_camera_broadcaster', - parameters=[{ - 'use_sim_time': True, - 'translation.x': 0.0, - 'translation.y': 0.0, - 'translation.z': 0.0, - 'rotation.x': -0.7071068, - 'rotation.y': 0.7071068, - 'rotation.z': 0.0, - 'rotation.w': 0.0, - 'frame_id': 'base_link', - 'child_frame_id': 'x500_mono_cam_down_0/camera_link/imager' - }] + package="tf2_ros", + plugin="tf2_ros::StaticTransformBroadcasterNode", + name="base_link_to_camera_broadcaster", + parameters=[ + { + "use_sim_time": True, + "translation.x": 0.0, + "translation.y": 0.0, + "translation.z": 0.0, + "rotation.x": -0.7071068, + "rotation.y": 0.7071068, + "rotation.z": 0.0, + "rotation.w": 0.0, + "frame_id": "base_link", + "child_frame_id": "x500_mono_cam_down_0/camera_link/imager", + } + ], ), ComposableNode( - package='tf2_ros', - plugin='tf2_ros::StaticTransformBroadcasterNode', - name='camera_to_camera_link_broadcaster', - parameters=[{ - 'use_sim_time': True, - 'translation.x': 0.0, - 'translation.y': 0.0, - 'translation.z': 0.0, - 'rotation.x': 0.0, - 'rotation.y': 0.0, - 'rotation.z': 0.0, - 'rotation.w': 10.0, - 'frame_id': 'x500_mono_cam_down_0/camera_link/imager', - 'child_frame_id': 'camera_link' - }] + package="tf2_ros", + plugin="tf2_ros::StaticTransformBroadcasterNode", + name="camera_to_camera_link_broadcaster", + parameters=[ + { + "use_sim_time": True, + "translation.x": 0.0, + "translation.y": 0.0, + "translation.z": 0.0, + "rotation.x": 0.0, + "rotation.y": 0.0, + "rotation.z": 0.0, + "rotation.w": 10.0, + "frame_id": "x500_mono_cam_down_0/camera_link/imager", + "child_frame_id": "camera_link", + } + ], ), - ] + ], ), ) return return_array -def generate_launch_description(): - return LaunchDescription([ - DeclareLaunchArgument( - "world_name", - default_value="aruco", - description="name of the Gazebo world", - ), - DeclareLaunchArgument( - "model_name", - default_value="x500_mono_cam_down_0", - description="name of the model", - ), - OpaqueFunction( - function=launch_setup, - ), - ]) \ No newline at end of file +def generate_launch_description(): + return LaunchDescription( + [ + DeclareLaunchArgument( + "world_name", + default_value="aruco", + description="name of the Gazebo world", + ), + DeclareLaunchArgument( + "model_name", + default_value="x500_mono_cam_down_0", + description="name of the model", + ), + OpaqueFunction( + function=launch_setup, + ), + ] + ) diff --git a/px4_roscon_workshop/custom_executor_demo/launch/custom_executor_demo.launch.py b/px4_roscon_workshop/custom_executor_demo/launch/custom_executor_demo.launch.py index e7d3515..5295953 100644 --- a/px4_roscon_workshop/custom_executor_demo/launch/custom_executor_demo.launch.py +++ b/px4_roscon_workshop/custom_executor_demo/launch/custom_executor_demo.launch.py @@ -7,11 +7,11 @@ from launch_ros.actions import Node from ament_index_python.packages import get_package_share_directory -def generate_launch_description(): +def generate_launch_description(): pkg_share = get_package_share_directory("custom_executor_demo") - - clock_bridge_config_file = os.path.join(pkg_share,"cfg","clock_bridge.yaml") + + clock_bridge_config_file = os.path.join(pkg_share, "cfg", "clock_bridge.yaml") run_uxrcedds_agent_arg = DeclareLaunchArgument( "run_uxrcedds_agent", @@ -25,30 +25,28 @@ def generate_launch_description(): description="Whether to run the Gazebo clock bridge", ) - return LaunchDescription([ - run_uxrcedds_agent_arg, - run_gz_clock_bridge_arg, - Node( - package="custom_executor_demo", - executable="custom_executor_demo", - name="custom_executor_demo", - output="screen", - parameters=[ - {"use_sim_time": True} - ] - ), - Node( - package="ros_gz_bridge", - executable="parameter_bridge", - name="gz_clock_bridge", - parameters=[ - {"config_file": clock_bridge_config_file} - ], - condition=IfCondition(LaunchConfiguration("run_gz_clock_bridge")) - ), - ExecuteProcess( - cmd=["MicroXRCEAgent", "udp4", "-p", "8888", "-v", "3"], - output="screen", - condition=IfCondition(LaunchConfiguration("run_uxrcedds_agent")) - ) - ]) + return LaunchDescription( + [ + run_uxrcedds_agent_arg, + run_gz_clock_bridge_arg, + Node( + package="custom_executor_demo", + executable="custom_executor_demo", + name="custom_executor_demo", + output="screen", + parameters=[{"use_sim_time": True}], + ), + Node( + package="ros_gz_bridge", + executable="parameter_bridge", + name="gz_clock_bridge", + parameters=[{"config_file": clock_bridge_config_file}], + condition=IfCondition(LaunchConfiguration("run_gz_clock_bridge")), + ), + ExecuteProcess( + cmd=["MicroXRCEAgent", "udp4", "-p", "8888", "-v", "3"], + output="screen", + condition=IfCondition(LaunchConfiguration("run_uxrcedds_agent")), + ), + ] + ) diff --git a/px4_roscon_workshop/custom_mode_demo/launch/custom_mode_demo.launch.py b/px4_roscon_workshop/custom_mode_demo/launch/custom_mode_demo.launch.py index 30ed928..ea9816b 100644 --- a/px4_roscon_workshop/custom_mode_demo/launch/custom_mode_demo.launch.py +++ b/px4_roscon_workshop/custom_mode_demo/launch/custom_mode_demo.launch.py @@ -7,11 +7,11 @@ from launch_ros.actions import Node from ament_index_python.packages import get_package_share_directory -def generate_launch_description(): +def generate_launch_description(): pkg_share = get_package_share_directory("custom_mode_demo") - - clock_bridge_config_file = os.path.join(pkg_share,"cfg","clock_bridge.yaml") + + clock_bridge_config_file = os.path.join(pkg_share, "cfg", "clock_bridge.yaml") run_uxrcedds_agent_arg = DeclareLaunchArgument( "run_uxrcedds_agent", @@ -25,30 +25,28 @@ def generate_launch_description(): description="Whether to run the Gazebo clock bridge", ) - return LaunchDescription([ - run_uxrcedds_agent_arg, - run_gz_clock_bridge_arg, - Node( - package="custom_mode_demo", - executable="custom_mode_demo", - name="custom_mode_demo", - output="screen", - parameters=[ - {"use_sim_time": True} - ] - ), - Node( - package="ros_gz_bridge", - executable="parameter_bridge", - name="gz_clock_bridge", - parameters=[ - {"config_file": clock_bridge_config_file} - ], - condition=IfCondition(LaunchConfiguration("run_gz_clock_bridge")) - ), - ExecuteProcess( - cmd=["MicroXRCEAgent", "udp4", "-p", "8888", "-v", "3"], - output="screen", - condition=IfCondition(LaunchConfiguration("run_uxrcedds_agent")) - ) - ]) + return LaunchDescription( + [ + run_uxrcedds_agent_arg, + run_gz_clock_bridge_arg, + Node( + package="custom_mode_demo", + executable="custom_mode_demo", + name="custom_mode_demo", + output="screen", + parameters=[{"use_sim_time": True}], + ), + Node( + package="ros_gz_bridge", + executable="parameter_bridge", + name="gz_clock_bridge", + parameters=[{"config_file": clock_bridge_config_file}], + condition=IfCondition(LaunchConfiguration("run_gz_clock_bridge")), + ), + ExecuteProcess( + cmd=["MicroXRCEAgent", "udp4", "-p", "8888", "-v", "3"], + output="screen", + condition=IfCondition(LaunchConfiguration("run_uxrcedds_agent")), + ), + ] + ) diff --git a/px4_roscon_workshop/formation_control/launch/formation.launch.py b/px4_roscon_workshop/formation_control/launch/formation.launch.py index 8dca4ad..f6d934c 100644 --- a/px4_roscon_workshop/formation_control/launch/formation.launch.py +++ b/px4_roscon_workshop/formation_control/launch/formation.launch.py @@ -8,125 +8,135 @@ from launch_ros.substitutions import FindPackageShare FORMATION_CONTROL_EXECUTABLE = ( - 'px4_formation_control', - 'px4_formation_control_executor_exercise', - 'px4_formation_control_executor_solution', + "px4_formation_control", + "px4_formation_control_executor_exercise", + "px4_formation_control_executor_solution", ) ACTIVE_EXECUTABLE_IDX = 2 def generate_launch_description() -> LaunchDescription: - px4_roscon_workshop_share = FindPackageShare('px4_roscon_workshop').find( - 'px4_roscon_workshop' + px4_roscon_workshop_share = FindPackageShare("px4_roscon_workshop").find( + "px4_roscon_workshop" ) gz_world_launch = path.join( px4_roscon_workshop_share, - 'launch', - 'gz_world.launch.py', + "launch", + "gz_world.launch.py", ) px4_vehicle_launch = path.join( px4_roscon_workshop_share, - 'launch', - 'px4_vehicle.launch.py', + "launch", + "px4_vehicle.launch.py", ) - return LaunchDescription([ - DeclareLaunchArgument( - 'px4_autopilot_path', - default_value=EnvironmentVariable('PX4_PATH', default_value='~/PX4-Autopilot'), - description='Path to PX4-Autopilot repository root (supports ~)', - ), - DeclareLaunchArgument( - 'world', - default_value='default', - description='Name of the Gazebo world to launch', - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource(gz_world_launch), - launch_arguments={ - 'px4_autopilot_path': LaunchConfiguration('px4_autopilot_path'), - 'world': LaunchConfiguration('world'), - }.items(), - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource(px4_vehicle_launch), - launch_arguments={ - 'px4_autopilot_path': LaunchConfiguration('px4_autopilot_path'), - 'world': LaunchConfiguration('world'), - 'px4_instance': '0', - 'model': 'x500_0', - 'px4_ns': 'px4_0', - 'spawn_pos_x': '0.0', - 'spawn_pos_y': '0.0', - 'spawn_pos_z': '0.3', - 'px4_extra_env_vars': 'PX4_PARAM_COM_RCL_EXCEPT=9,PX4_PARAM_COM_RC_IN_MODE=1,PX4_GZ_NO_FOLLOW=1', - }.items(), - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource(px4_vehicle_launch), - launch_arguments={ - 'px4_autopilot_path': LaunchConfiguration('px4_autopilot_path'), - 'world': LaunchConfiguration('world'), - 'px4_instance': '1', - 'model': 'x500_1', - 'px4_ns': 'px4_1', - 'spawn_pos_x': '10.0', - 'spawn_pos_y': '0.0', - 'spawn_pos_z': '0.3', - 'px4_extra_env_vars': 'PX4_PARAM_COM_RCL_EXCEPT=9,PX4_PARAM_COM_RC_IN_MODE=4,PX4_GZ_NO_FOLLOW=1', - }.items(), - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource(px4_vehicle_launch), - launch_arguments={ - 'px4_autopilot_path': LaunchConfiguration('px4_autopilot_path'), - 'world': LaunchConfiguration('world'), - 'px4_instance': '2', - 'model': 'x500_2', - 'px4_ns': 'px4_2', - 'spawn_pos_x': '5.0', - 'spawn_pos_y': '8.660254', - 'spawn_pos_z': '0.3', - 'px4_extra_env_vars': 'PX4_PARAM_COM_RCL_EXCEPT=9,PX4_PARAM_COM_RC_IN_MODE=4,PX4_GZ_NO_FOLLOW=1', - }.items(), - ), - Node( - package='px4_formation_control', - executable=FORMATION_CONTROL_EXECUTABLE[ACTIVE_EXECUTABLE_IDX], - name='formation_controller_0', - output='screen', - parameters=[{ - 'px4_ns': '/px4_0/', - 'tf_prefix': 'px4_0', - 'neighbor_prefixes': ['px4_1', 'px4_2'], - 'neighbor_distances': [10.0, 10.0], - 'gain': 1.0, - }], - ), - Node( - package='px4_formation_control', - executable=FORMATION_CONTROL_EXECUTABLE[ACTIVE_EXECUTABLE_IDX], - name='formation_controller_1', - output='screen', - parameters=[{ - 'px4_ns': '/px4_1/', - 'tf_prefix': 'px4_1', - 'neighbor_prefixes': ['px4_0', 'px4_2'], - 'neighbor_distances': [10.0, 10.0], - 'gain': 1.0, - }], - ), - Node( - package='px4_formation_control', - executable=FORMATION_CONTROL_EXECUTABLE[ACTIVE_EXECUTABLE_IDX], - name='formation_controller_2', - output='screen', - parameters=[{ - 'px4_ns': '/px4_2/', - 'tf_prefix': 'px4_2', - 'neighbor_prefixes': ['px4_0', 'px4_1'], - 'neighbor_distances': [10.0, 10.0], - 'gain': 1.0, - }], - ), - ]) + return LaunchDescription( + [ + DeclareLaunchArgument( + "px4_autopilot_path", + default_value=EnvironmentVariable( + "PX4_PATH", default_value="~/PX4-Autopilot" + ), + description="Path to PX4-Autopilot repository root (supports ~)", + ), + DeclareLaunchArgument( + "world", + default_value="default", + description="Name of the Gazebo world to launch", + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(gz_world_launch), + launch_arguments={ + "px4_autopilot_path": LaunchConfiguration("px4_autopilot_path"), + "world": LaunchConfiguration("world"), + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(px4_vehicle_launch), + launch_arguments={ + "px4_autopilot_path": LaunchConfiguration("px4_autopilot_path"), + "world": LaunchConfiguration("world"), + "px4_instance": "0", + "model": "x500_0", + "px4_ns": "px4_0", + "spawn_pos_x": "0.0", + "spawn_pos_y": "0.0", + "spawn_pos_z": "0.3", + "px4_extra_env_vars": "PX4_PARAM_COM_RCL_EXCEPT=9,PX4_PARAM_COM_RC_IN_MODE=1,PX4_GZ_NO_FOLLOW=1", + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(px4_vehicle_launch), + launch_arguments={ + "px4_autopilot_path": LaunchConfiguration("px4_autopilot_path"), + "world": LaunchConfiguration("world"), + "px4_instance": "1", + "model": "x500_1", + "px4_ns": "px4_1", + "spawn_pos_x": "10.0", + "spawn_pos_y": "0.0", + "spawn_pos_z": "0.3", + "px4_extra_env_vars": "PX4_PARAM_COM_RCL_EXCEPT=9,PX4_PARAM_COM_RC_IN_MODE=4,PX4_GZ_NO_FOLLOW=1", + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(px4_vehicle_launch), + launch_arguments={ + "px4_autopilot_path": LaunchConfiguration("px4_autopilot_path"), + "world": LaunchConfiguration("world"), + "px4_instance": "2", + "model": "x500_2", + "px4_ns": "px4_2", + "spawn_pos_x": "5.0", + "spawn_pos_y": "8.660254", + "spawn_pos_z": "0.3", + "px4_extra_env_vars": "PX4_PARAM_COM_RCL_EXCEPT=9,PX4_PARAM_COM_RC_IN_MODE=4,PX4_GZ_NO_FOLLOW=1", + }.items(), + ), + Node( + package="px4_formation_control", + executable=FORMATION_CONTROL_EXECUTABLE[ACTIVE_EXECUTABLE_IDX], + name="formation_controller_0", + output="screen", + parameters=[ + { + "px4_ns": "/px4_0/", + "tf_prefix": "px4_0", + "neighbor_prefixes": ["px4_1", "px4_2"], + "neighbor_distances": [10.0, 10.0], + "gain": 1.0, + } + ], + ), + Node( + package="px4_formation_control", + executable=FORMATION_CONTROL_EXECUTABLE[ACTIVE_EXECUTABLE_IDX], + name="formation_controller_1", + output="screen", + parameters=[ + { + "px4_ns": "/px4_1/", + "tf_prefix": "px4_1", + "neighbor_prefixes": ["px4_0", "px4_2"], + "neighbor_distances": [10.0, 10.0], + "gain": 1.0, + } + ], + ), + Node( + package="px4_formation_control", + executable=FORMATION_CONTROL_EXECUTABLE[ACTIVE_EXECUTABLE_IDX], + name="formation_controller_2", + output="screen", + parameters=[ + { + "px4_ns": "/px4_2/", + "tf_prefix": "px4_2", + "neighbor_prefixes": ["px4_0", "px4_1"], + "neighbor_distances": [10.0, 10.0], + "gain": 1.0, + } + ], + ), + ] + ) diff --git a/px4_roscon_workshop/precision_land_executor/launch/precision_land_executor.launch.py b/px4_roscon_workshop/precision_land_executor/launch/precision_land_executor.launch.py index ae4b75a..2b6dfee 100644 --- a/px4_roscon_workshop/precision_land_executor/launch/precision_land_executor.launch.py +++ b/px4_roscon_workshop/precision_land_executor/launch/precision_land_executor.launch.py @@ -5,12 +5,13 @@ from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node + def _launch_setup(context): px4_autopilot_path = os.path.expanduser( - LaunchConfiguration('px4_autopilot_path').perform(context) + LaunchConfiguration("px4_autopilot_path").perform(context) ) - px4_gz_resource_path = os.path.join(px4_autopilot_path, 'Tools', 'simulation', 'gz') - arucotag_model_path = os.path.join(px4_gz_resource_path, 'models', 'arucotag') + px4_gz_resource_path = os.path.join(px4_autopilot_path, "Tools", "simulation", "gz") + arucotag_model_path = os.path.join(px4_gz_resource_path, "models", "arucotag") return [ # SetEnvironmentVariable( # 'GZ_SIM_RESOURCE_PATH', @@ -18,39 +19,44 @@ def _launch_setup(context): # ), ExecuteProcess( cmd=[ - "gz", "service", - "-s", "/world/walls/create", - "--reqtype", "gz.msgs.EntityFactory", - "--reptype", "gz.msgs.Boolean", - "--timeout", "1000", + "gz", + "service", + "-s", + "/world/walls/create", + "--reqtype", + "gz.msgs.EntityFactory", + "--reptype", + "gz.msgs.Boolean", + "--timeout", + "1000", "--req", ( f'sdf_filename: "{arucotag_model_path}/model.sdf", ' 'name: "arucotag", ' - 'pose: { position: { x: 8, y: -4.0, z: 0.001000 }, ' - 'orientation: { x: 0.0, y: 0.0, z: 0.0, w: 1.0 } }' - ) + "pose: { position: { x: 8, y: -4.0, z: 0.001000 }, " + "orientation: { x: 0.0, y: 0.0, z: 0.0, w: 1.0 } }" + ), ], - output="screen" + output="screen", ) ] -def generate_launch_description(): - return LaunchDescription([ - DeclareLaunchArgument( - 'px4_autopilot_path', - default_value='~/PX4-Autopilot', - description='Path to PX4-Autopilot repository root (supports ~)', - ), - Node( - package="precision_land_executor", - executable="precision_land_executor", - name="precision_land_executor", - output="screen", - parameters=[ - {"use_sim_time": True} - ] - ), - OpaqueFunction(function=_launch_setup), - ]) +def generate_launch_description(): + return LaunchDescription( + [ + DeclareLaunchArgument( + "px4_autopilot_path", + default_value="~/PX4-Autopilot", + description="Path to PX4-Autopilot repository root (supports ~)", + ), + Node( + package="precision_land_executor", + executable="precision_land_executor", + name="precision_land_executor", + output="screen", + parameters=[{"use_sim_time": True}], + ), + OpaqueFunction(function=_launch_setup), + ] + ) diff --git a/px4_roscon_workshop/px4_roscon_workshop/launch/common.launch.py b/px4_roscon_workshop/px4_roscon_workshop/launch/common.launch.py index ce3a2cb..99f23f3 100644 --- a/px4_roscon_workshop/px4_roscon_workshop/launch/common.launch.py +++ b/px4_roscon_workshop/px4_roscon_workshop/launch/common.launch.py @@ -6,80 +6,71 @@ from launch_ros.descriptions import ComposableNode from launch_ros.substitutions import FindPackageShare -def generate_launch_description(): - pkg_share = FindPackageShare('px4_roscon_workshop').find('px4_roscon_workshop') +def generate_launch_description(): + pkg_share = FindPackageShare("px4_roscon_workshop").find("px4_roscon_workshop") - clock_bridge_config_file = os.path.join(pkg_share,"cfg","clock_bridge.yaml") + clock_bridge_config_file = os.path.join(pkg_share, "cfg", "clock_bridge.yaml") - urdf_dir = os.path.join(pkg_share, 'urdf') - urdf_file = os.path.join(urdf_dir, 'x500.urdf') - with open(urdf_file, 'r') as infp: + urdf_dir = os.path.join(pkg_share, "urdf") + urdf_file = os.path.join(urdf_dir, "x500.urdf") + with open(urdf_file, "r") as infp: x500_desc = infp.read() - return LaunchDescription([ - Node( - package="ros_gz_bridge", - executable="parameter_bridge", - name="gz_clock_bridge", - parameters=[ - {"config_file": clock_bridge_config_file} - ] - ), - Node( - package='robot_state_publisher', - executable='robot_state_publisher', - output='both', - parameters=[ - { - 'robot_description': x500_desc, - 'use_sim_time': True - } - ] - ), - Node( - package='px4_tf', - executable='px4_tf_publisher', - parameters=[ - { - 'use_sim_time': True - } - ] - ), - Node( - package='foxglove_bridge', - executable='foxglove_bridge', - parameters=[ - {'use_sim_time': True} - ], - ), - ComposableNodeContainer( - name='static_tf_container', - package='rclcpp_components', - executable='component_container', - namespace='', - composable_node_descriptions=[ - ComposableNode( - package='tf2_ros', - plugin='tf2_ros::StaticTransformBroadcasterNode', - name='map_to_odom_broadcaster', - parameters=[{ - 'use_sim_time': True, - 'translation.x': 0.0, - 'translation.y': 0.0, - 'translation.z': 0.24, - 'rotation.x': 0.0, - 'rotation.y': 0.0, - 'rotation.z': 0.0, - 'frame_id': 'map', - 'child_frame_id': 'odom' - }] - ), - # other static transform nodes - ] - ), - ExecuteProcess( - cmd=["MicroXRCEAgent", "udp4", "-p", "8888", "-v", "3"], - output="screen", - ) - ]) \ No newline at end of file + return LaunchDescription( + [ + Node( + package="ros_gz_bridge", + executable="parameter_bridge", + name="gz_clock_bridge", + parameters=[{"config_file": clock_bridge_config_file}], + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[{"robot_description": x500_desc, "use_sim_time": True}], + ), + Node( + package="px4_tf", + executable="px4_tf_publisher", + parameters=[{"use_sim_time": True}], + ), + Node( + package="foxglove_bridge", + executable="foxglove_bridge", + parameters=[{"use_sim_time": True}], + ), + ComposableNodeContainer( + name="static_tf_container", + package="rclcpp_components", + executable="component_container", + namespace="", + composable_node_descriptions=[ + ComposableNode( + package="tf2_ros", + plugin="tf2_ros::StaticTransformBroadcasterNode", + name="map_to_odom_broadcaster", + parameters=[ + { + "use_sim_time": True, + "translation.x": 0.0, + "translation.y": 0.0, + "translation.z": 0.24, + "rotation.x": 0.0, + "rotation.y": 0.0, + "rotation.z": 0.0, + "frame_id": "map", + "child_frame_id": "odom", + } + ], + ), + # other static transform nodes + ], + ), + ExecuteProcess( + cmd=["MicroXRCEAgent", "udp4", "-p", "8888", "-v", "3"], + output="screen", + ), + ] + ) diff --git a/px4_roscon_workshop/px4_roscon_workshop/launch/gz_world.launch.py b/px4_roscon_workshop/px4_roscon_workshop/launch/gz_world.launch.py index 778c2dd..b42c9c2 100644 --- a/px4_roscon_workshop/px4_roscon_workshop/launch/gz_world.launch.py +++ b/px4_roscon_workshop/px4_roscon_workshop/launch/gz_world.launch.py @@ -10,94 +10,110 @@ SetEnvironmentVariable, ) from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import EnvironmentVariable, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import ( + EnvironmentVariable, + LaunchConfiguration, + PathJoinSubstitution, +) from launch_ros.substitutions import FindPackageShare def _launch_setup(context): px4_autopilot_path = path.expanduser( - LaunchConfiguration('px4_autopilot_path').perform(context) + LaunchConfiguration("px4_autopilot_path").perform(context) + ) + world_name = LaunchConfiguration("world").perform(context) + extra_gz_resource_path = LaunchConfiguration("extra_gz_resource_path").perform( + context ) - world_name = LaunchConfiguration('world').perform(context) - extra_gz_resource_path = LaunchConfiguration('extra_gz_resource_path').perform(context) - bridge_config_file = LaunchConfiguration('bridge_config_file').perform(context) + bridge_config_file = LaunchConfiguration("bridge_config_file").perform(context) - ros_gz_sim_pkg_path = get_package_share_directory('ros_gz_sim') - px4_gz_resource_path = path.join(px4_autopilot_path, 'Tools', 'simulation', 'gz') + ros_gz_sim_pkg_path = get_package_share_directory("ros_gz_sim") + px4_gz_resource_path = path.join(px4_autopilot_path, "Tools", "simulation", "gz") px4_gz_plugin_path = path.join( px4_autopilot_path, - 'build', - 'px4_sitl_default', - 'src', - 'modules', - 'simulation', - 'gz_plugins', + "build", + "px4_sitl_default", + "src", + "modules", + "simulation", + "gz_plugins", ) px4_gz_server_config_path = path.join( px4_autopilot_path, - 'src', - 'modules', - 'simulation', - 'gz_bridge', - 'server.config', + "src", + "modules", + "simulation", + "gz_bridge", + "server.config", ) - gz_launch_path = path.join(ros_gz_sim_pkg_path, 'launch', 'ros_gz_sim.launch.py') + gz_launch_path = path.join(ros_gz_sim_pkg_path, "launch", "ros_gz_sim.launch.py") return [ SetEnvironmentVariable( - 'GZ_SIM_RESOURCE_PATH', - ':'.join([ - path.join(px4_gz_resource_path, 'worlds'), - path.join(px4_gz_resource_path, 'models'), - extra_gz_resource_path, - ]), + "GZ_SIM_RESOURCE_PATH", + ":".join( + [ + path.join(px4_gz_resource_path, "worlds"), + path.join(px4_gz_resource_path, "models"), + extra_gz_resource_path, + ] + ), ), SetEnvironmentVariable( - 'GZ_SIM_SYSTEM_PLUGIN_PATH', - ':'.join([environ.get('GZ_SIM_SYSTEM_PLUGIN_PATH', ''), px4_gz_plugin_path]), + "GZ_SIM_SYSTEM_PLUGIN_PATH", + ":".join( + [environ.get("GZ_SIM_SYSTEM_PLUGIN_PATH", ""), px4_gz_plugin_path] + ), ), - SetEnvironmentVariable('GZ_SIM_SERVER_CONFIG_PATH', px4_gz_server_config_path), + SetEnvironmentVariable("GZ_SIM_SERVER_CONFIG_PATH", px4_gz_server_config_path), IncludeLaunchDescription( PythonLaunchDescriptionSource(gz_launch_path), launch_arguments={ - 'world_sdf_file': f'{world_name}.sdf', - 'bridge_name': 'gz_ros_bridge', - 'config_file': bridge_config_file, + "world_sdf_file": f"{world_name}.sdf", + "bridge_name": "gz_ros_bridge", + "config_file": bridge_config_file, }.items(), ), ExecuteProcess( - cmd=['MicroXRCEAgent', 'udp4', '--port', '8888', '-v', '1'], - name='microxrce_agent', - output='screen', + cmd=["MicroXRCEAgent", "udp4", "--port", "8888", "-v", "1"], + name="microxrce_agent", + output="screen", ), ] def generate_launch_description(): - return LaunchDescription([ - DeclareLaunchArgument( - 'px4_autopilot_path', - default_value=EnvironmentVariable('PX4_PATH', default_value='~/PX4-Autopilot'), - description='Path to PX4-Autopilot repository root (supports ~)', - ), - DeclareLaunchArgument( - 'world', - default_value='default', - description='Name of the world to launch', - ), - DeclareLaunchArgument( - 'extra_gz_resource_path', - default_value='', - description='Extra GZ resource path to add to GZ_SIM_RESOURCE_PATH', - ), - DeclareLaunchArgument( - 'bridge_config_file', - default_value=PathJoinSubstitution([ - FindPackageShare('px4_roscon_workshop'), - 'cfg', - 'clock_bridge.yaml', - ]), - description='Path to the ROS-GZ bridge configuration file', - ), - OpaqueFunction(function=_launch_setup), - ]) \ No newline at end of file + return LaunchDescription( + [ + DeclareLaunchArgument( + "px4_autopilot_path", + default_value=EnvironmentVariable( + "PX4_PATH", default_value="~/PX4-Autopilot" + ), + description="Path to PX4-Autopilot repository root (supports ~)", + ), + DeclareLaunchArgument( + "world", + default_value="default", + description="Name of the world to launch", + ), + DeclareLaunchArgument( + "extra_gz_resource_path", + default_value="", + description="Extra GZ resource path to add to GZ_SIM_RESOURCE_PATH", + ), + DeclareLaunchArgument( + "bridge_config_file", + default_value=PathJoinSubstitution( + [ + FindPackageShare("px4_roscon_workshop"), + "cfg", + "clock_bridge.yaml", + ] + ), + description="Path to the ROS-GZ bridge configuration file", + ), + OpaqueFunction(function=_launch_setup), + ] + ) diff --git a/px4_roscon_workshop/px4_roscon_workshop/launch/px4.launch.py b/px4_roscon_workshop/px4_roscon_workshop/launch/px4.launch.py index f41fbb3..e2b18ec 100644 --- a/px4_roscon_workshop/px4_roscon_workshop/launch/px4.launch.py +++ b/px4_roscon_workshop/px4_roscon_workshop/launch/px4.launch.py @@ -2,77 +2,96 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import EnvironmentVariable, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import ( + EnvironmentVariable, + LaunchConfiguration, + PathJoinSubstitution, +) from launch_ros.substitutions import FindPackageShare + def generate_launch_description(): - pkg_share = FindPackageShare('px4_roscon_workshop').find('px4_roscon_workshop') + pkg_share = FindPackageShare("px4_roscon_workshop").find("px4_roscon_workshop") - return LaunchDescription([ - DeclareLaunchArgument( - 'px4_autopilot_path', - default_value=EnvironmentVariable('PX4_PATH', default_value='~/PX4-Autopilot'), - description='Path to PX4-Autopilot repository root (supports ~)', - ), - DeclareLaunchArgument( - 'world', - default_value='default', - description='Name of the world to launch', - ), - DeclareLaunchArgument( - 'model', - default_value='x500', - description='Name of the model to spawn', - ), - DeclareLaunchArgument( - 'model_path', - default_value=PathJoinSubstitution([ - LaunchConfiguration('px4_autopilot_path'), - 'Tools', - 'simulation', - 'gz', - 'models', - 'x500', - 'model.sdf', - ]), - description='Path to the model SDF file', - ), - DeclareLaunchArgument( - 'px4_autostart_id', - default_value='4001', - description='PX4 autostart ID', - ), - DeclareLaunchArgument( - 'extra_gz_resource_path', - default_value='', - description='Extra GZ resource path to add to GZ_SIM_RESOURCE_PATH', - ), - DeclareLaunchArgument( - 'bridge_config_file', - default_value=PathJoinSubstitution([ - FindPackageShare('px4_roscon_workshop'), - 'cfg', - 'clock_bridge.yaml', - ]), - description='Path to the ROS-GZ bridge configuration file', - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource(path.join(pkg_share, 'launch', 'gz_world.launch.py')), - launch_arguments={ - 'px4_autopilot_path': LaunchConfiguration('px4_autopilot_path'), - 'world': LaunchConfiguration('world'), - 'extra_gz_resource_path': LaunchConfiguration('extra_gz_resource_path'), - 'bridge_config_file': LaunchConfiguration('bridge_config_file'), - }.items(), - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource(path.join(pkg_share, 'launch', 'px4_vehicle.launch.py')), - launch_arguments={ - 'px4_autopilot_path': LaunchConfiguration('px4_autopilot_path'), - 'world': LaunchConfiguration('world'), - 'model': LaunchConfiguration('model'), - 'model_path': LaunchConfiguration('model_path'), - 'px4_autostart_id': LaunchConfiguration('px4_autostart_id'), - }.items(), - ), - ]) \ No newline at end of file + return LaunchDescription( + [ + DeclareLaunchArgument( + "px4_autopilot_path", + default_value=EnvironmentVariable( + "PX4_PATH", default_value="~/PX4-Autopilot" + ), + description="Path to PX4-Autopilot repository root (supports ~)", + ), + DeclareLaunchArgument( + "world", + default_value="default", + description="Name of the world to launch", + ), + DeclareLaunchArgument( + "model", + default_value="x500", + description="Name of the model to spawn", + ), + DeclareLaunchArgument( + "model_path", + default_value=PathJoinSubstitution( + [ + LaunchConfiguration("px4_autopilot_path"), + "Tools", + "simulation", + "gz", + "models", + "x500", + "model.sdf", + ] + ), + description="Path to the model SDF file", + ), + DeclareLaunchArgument( + "px4_autostart_id", + default_value="4001", + description="PX4 autostart ID", + ), + DeclareLaunchArgument( + "extra_gz_resource_path", + default_value="", + description="Extra GZ resource path to add to GZ_SIM_RESOURCE_PATH", + ), + DeclareLaunchArgument( + "bridge_config_file", + default_value=PathJoinSubstitution( + [ + FindPackageShare("px4_roscon_workshop"), + "cfg", + "clock_bridge.yaml", + ] + ), + description="Path to the ROS-GZ bridge configuration file", + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + path.join(pkg_share, "launch", "gz_world.launch.py") + ), + launch_arguments={ + "px4_autopilot_path": LaunchConfiguration("px4_autopilot_path"), + "world": LaunchConfiguration("world"), + "extra_gz_resource_path": LaunchConfiguration( + "extra_gz_resource_path" + ), + "bridge_config_file": LaunchConfiguration("bridge_config_file"), + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + path.join(pkg_share, "launch", "px4_vehicle.launch.py") + ), + launch_arguments={ + "px4_autopilot_path": LaunchConfiguration("px4_autopilot_path"), + "world": LaunchConfiguration("world"), + "model": LaunchConfiguration("model"), + "model_path": LaunchConfiguration("model_path"), + "px4_autostart_id": LaunchConfiguration("px4_autostart_id"), + }.items(), + ), + ] + ) diff --git a/px4_roscon_workshop/px4_roscon_workshop/launch/px4_vehicle.launch.py b/px4_roscon_workshop/px4_roscon_workshop/launch/px4_vehicle.launch.py index 73e1440..c28ae1b 100644 --- a/px4_roscon_workshop/px4_roscon_workshop/launch/px4_vehicle.launch.py +++ b/px4_roscon_workshop/px4_roscon_workshop/launch/px4_vehicle.launch.py @@ -2,133 +2,149 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, OpaqueFunction +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, + OpaqueFunction, +) from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import EnvironmentVariable, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import ( + EnvironmentVariable, + LaunchConfiguration, + PathJoinSubstitution, +) def _launch_setup(context): px4_autopilot_path = path.expanduser( - LaunchConfiguration('px4_autopilot_path').perform(context) + LaunchConfiguration("px4_autopilot_path").perform(context) ) - world_name = LaunchConfiguration('world').perform(context) - model_name = LaunchConfiguration('model').perform(context) - model_path = path.expanduser( - LaunchConfiguration('model_path').perform(context) - ) - px4_instance = LaunchConfiguration('px4_instance').perform(context) - px4_namespace = LaunchConfiguration('px4_ns').perform(context) + world_name = LaunchConfiguration("world").perform(context) + model_name = LaunchConfiguration("model").perform(context) + model_path = path.expanduser(LaunchConfiguration("model_path").perform(context)) + px4_instance = LaunchConfiguration("px4_instance").perform(context) + px4_namespace = LaunchConfiguration("px4_ns").perform(context) - px4_extra_env_vars = LaunchConfiguration('px4_extra_env_vars').perform(context) + px4_extra_env_vars = LaunchConfiguration("px4_extra_env_vars").perform(context) - ros_gz_sim_pkg_path = get_package_share_directory('ros_gz_sim') - gz_spawn_path = path.join(ros_gz_sim_pkg_path, 'launch', 'gz_spawn_model.launch.py') + ros_gz_sim_pkg_path = get_package_share_directory("ros_gz_sim") + gz_spawn_path = path.join(ros_gz_sim_pkg_path, "launch", "gz_spawn_model.launch.py") px4_env = { - 'PX4_SYS_AUTOSTART': LaunchConfiguration('px4_autostart_id').perform(context), - 'PX4_GZ_MODEL_NAME': model_name, - 'PX4_GZ_STANDALONE': '1', - 'PX4_GZ_WORLD': world_name, + "PX4_SYS_AUTOSTART": LaunchConfiguration("px4_autostart_id").perform(context), + "PX4_GZ_MODEL_NAME": model_name, + "PX4_GZ_STANDALONE": "1", + "PX4_GZ_WORLD": world_name, } px4_env.update( dict( (key.strip(), value.strip()) - for env_var in px4_extra_env_vars.split(',') + for env_var in px4_extra_env_vars.split(",") if env_var.strip() - for key, value in [env_var.split('=', 1)] + for key, value in [env_var.split("=", 1)] ) ) if px4_namespace: - px4_env['PX4_UXRCE_DDS_NS'] = px4_namespace + px4_env["PX4_UXRCE_DDS_NS"] = px4_namespace return [ IncludeLaunchDescription( PythonLaunchDescriptionSource(gz_spawn_path), launch_arguments={ - 'world': world_name, - 'file': model_path, - 'entity_name': model_name, - 'x': LaunchConfiguration('spawn_pos_x').perform(context), - 'y': LaunchConfiguration('spawn_pos_y').perform(context), - 'z': LaunchConfiguration('spawn_pos_z').perform(context), + "world": world_name, + "file": model_path, + "entity_name": model_name, + "x": LaunchConfiguration("spawn_pos_x").perform(context), + "y": LaunchConfiguration("spawn_pos_y").perform(context), + "z": LaunchConfiguration("spawn_pos_z").perform(context), }.items(), ), ExecuteProcess( cmd=[ - path.join(px4_autopilot_path, 'build', 'px4_sitl_default', 'bin', 'px4'), - '-i', px4_instance, + path.join( + px4_autopilot_path, "build", "px4_sitl_default", "bin", "px4" + ), + "-i", + px4_instance, ], additional_env=px4_env, - name=f'px4_{px4_instance}', - output='screen', + name=f"px4_{px4_instance}", + output="screen", ), ] def generate_launch_description(): - return LaunchDescription([ - DeclareLaunchArgument( - 'px4_autopilot_path', - default_value=EnvironmentVariable('PX4_PATH', default_value='~/PX4-Autopilot'), - description='Path to PX4-Autopilot repository root (supports ~)', - ), - DeclareLaunchArgument( - 'px4_instance', - default_value='0', - description='PX4 instance ID', - ), - DeclareLaunchArgument( - 'world', - default_value='default', - description='Name of the world where the model is spawned', - ), - DeclareLaunchArgument( - 'model', - default_value='x500', - description='Name of the model to start PX4 for', - ), - DeclareLaunchArgument( - 'model_path', - default_value=PathJoinSubstitution([ - LaunchConfiguration('px4_autopilot_path'), - 'Tools', - 'simulation', - 'gz', - 'models', - 'x500', - 'model.sdf', - ]), - description='Path to the model SDF file', - ), - DeclareLaunchArgument( - 'px4_autostart_id', - default_value='4001', - description='PX4 autostart ID', - ), - DeclareLaunchArgument( - 'px4_ns', - default_value='', - description='PX4 namespace', - ), - DeclareLaunchArgument( - 'px4_extra_env_vars', - default_value='', - description='Extra environment variables to set for PX4', - ), - DeclareLaunchArgument( - 'spawn_pos_x', - default_value='0.0', - description='X position to spawn the model at', - ), - DeclareLaunchArgument( - 'spawn_pos_y', - default_value='0.0', - description='Y position to spawn the model at', - ), - DeclareLaunchArgument( - 'spawn_pos_z', - default_value='0.0', - description='Z position to spawn the model at', - ), - OpaqueFunction(function=_launch_setup), - ]) \ No newline at end of file + return LaunchDescription( + [ + DeclareLaunchArgument( + "px4_autopilot_path", + default_value=EnvironmentVariable( + "PX4_PATH", default_value="~/PX4-Autopilot" + ), + description="Path to PX4-Autopilot repository root (supports ~)", + ), + DeclareLaunchArgument( + "px4_instance", + default_value="0", + description="PX4 instance ID", + ), + DeclareLaunchArgument( + "world", + default_value="default", + description="Name of the world where the model is spawned", + ), + DeclareLaunchArgument( + "model", + default_value="x500", + description="Name of the model to start PX4 for", + ), + DeclareLaunchArgument( + "model_path", + default_value=PathJoinSubstitution( + [ + LaunchConfiguration("px4_autopilot_path"), + "Tools", + "simulation", + "gz", + "models", + "x500", + "model.sdf", + ] + ), + description="Path to the model SDF file", + ), + DeclareLaunchArgument( + "px4_autostart_id", + default_value="4001", + description="PX4 autostart ID", + ), + DeclareLaunchArgument( + "px4_ns", + default_value="", + description="PX4 namespace", + ), + DeclareLaunchArgument( + "px4_extra_env_vars", + default_value="", + description="Extra environment variables to set for PX4", + ), + DeclareLaunchArgument( + "spawn_pos_x", + default_value="0.0", + description="X position to spawn the model at", + ), + DeclareLaunchArgument( + "spawn_pos_y", + default_value="0.0", + description="Y position to spawn the model at", + ), + DeclareLaunchArgument( + "spawn_pos_z", + default_value="0.0", + description="Z position to spawn the model at", + ), + OpaqueFunction(function=_launch_setup), + ] + ) diff --git a/px4_roscon_workshop/px4_tf/README.md b/px4_roscon_workshop/px4_tf/README.md index 2a1df99..a9833b8 100644 --- a/px4_roscon_workshop/px4_tf/README.md +++ b/px4_roscon_workshop/px4_tf/README.md @@ -44,7 +44,7 @@ Please run the following commands in separate terminals. 1. Start Gazebo, spawn a drone and attach a PX4 instance to it. From the PX4 repo, run - + ```sh make px4_sitl gz_500 ``` diff --git a/px4_roscon_workshop/rover_teleop/launch/rover_launch.py b/px4_roscon_workshop/rover_teleop/launch/rover_launch.py index 521259d..b251f21 100644 --- a/px4_roscon_workshop/rover_teleop/launch/rover_launch.py +++ b/px4_roscon_workshop/rover_teleop/launch/rover_launch.py @@ -1,4 +1,3 @@ - from os import path from launch import LaunchDescription @@ -10,38 +9,42 @@ from launch.substitutions import LaunchConfiguration, EnvironmentVariable - def generate_launch_description(): + px4_roscon_workshop_pkg_path = FindPackageShare(package="px4_roscon_workshop").find( + "px4_roscon_workshop" + ) + pkg_share = FindPackageShare(package="rover_teleop").find("rover_teleop") - px4_roscon_workshop_pkg_path = FindPackageShare(package='px4_roscon_workshop').find('px4_roscon_workshop') - pkg_share = FindPackageShare(package='rover_teleop').find('rover_teleop') - - return LaunchDescription([ - DeclareLaunchArgument( - 'px4_autopilot_path', - default_value=EnvironmentVariable('PX4_PATH', default_value='~/PX4-Autopilot'), - description='Path to PX4-Autopilot repository root (supports ~)', - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - path.join(px4_roscon_workshop_pkg_path, 'launch', 'px4.launch.py') + return LaunchDescription( + [ + DeclareLaunchArgument( + "px4_autopilot_path", + default_value=EnvironmentVariable( + "PX4_PATH", default_value="~/PX4-Autopilot" + ), + description="Path to PX4-Autopilot repository root (supports ~)", + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + path.join(px4_roscon_workshop_pkg_path, "launch", "px4.launch.py") + ), + launch_arguments={ + "px4_autopilot_path": LaunchConfiguration("px4_autopilot_path"), + "world": "rover", + "model": "rover_differential_xl", + "model_path": path.join( + pkg_share, "models", "rover_differential_xl", "model.sdf" + ), + "px4_autostart_id": "50000", + "extra_gz_resource_path": path.join(pkg_share, "models"), + }.items(), + ), + Node( + package="rover_teleop", + executable="rover_teleop", + name="rover_teleop", + output="screen", + parameters=[{"use_sim_time": True}], ), - launch_arguments={ - 'px4_autopilot_path': LaunchConfiguration('px4_autopilot_path'), - 'world': 'rover', - 'model': 'rover_differential_xl', - 'model_path': path.join(pkg_share, 'models', 'rover_differential_xl', 'model.sdf'), - 'px4_autostart_id': '50000', - 'extra_gz_resource_path': path.join(pkg_share, 'models') - }.items(), - ), - Node( - package="rover_teleop", - executable="rover_teleop", - name="rover_teleop", - output="screen", - parameters=[ - {"use_sim_time": True} - ] - ) - ]) + ] + ) diff --git a/px4_roscon_workshop/teleop/launch/teleop.launch.py b/px4_roscon_workshop/teleop/launch/teleop.launch.py index 45a92ff..3521414 100644 --- a/px4_roscon_workshop/teleop/launch/teleop.launch.py +++ b/px4_roscon_workshop/teleop/launch/teleop.launch.py @@ -6,12 +6,13 @@ from launch.substitutions import LaunchConfiguration from launch_ros.descriptions import ComposableNode + def _launch_setup(context): px4_autopilot_path = os.path.expanduser( - LaunchConfiguration('px4_autopilot_path').perform(context) + LaunchConfiguration("px4_autopilot_path").perform(context) ) - px4_gz_resource_path = os.path.join(px4_autopilot_path, 'Tools', 'simulation', 'gz') - arucotag_model_path = os.path.join(px4_gz_resource_path, 'models', 'arucotag') + px4_gz_resource_path = os.path.join(px4_autopilot_path, "Tools", "simulation", "gz") + arucotag_model_path = os.path.join(px4_gz_resource_path, "models", "arucotag") return [ # SetEnvironmentVariable( # 'GZ_SIM_RESOURCE_PATH', @@ -19,91 +20,97 @@ def _launch_setup(context): # ), ExecuteProcess( cmd=[ - "gz", "service", - "-s", "/world/walls/create", - "--reqtype", "gz.msgs.EntityFactory", - "--reptype", "gz.msgs.Boolean", - "--timeout", "1000", + "gz", + "service", + "-s", + "/world/walls/create", + "--reqtype", + "gz.msgs.EntityFactory", + "--reptype", + "gz.msgs.Boolean", + "--timeout", + "1000", "--req", ( f'sdf_filename: "{arucotag_model_path}/model.sdf", ' 'name: "arucotag", ' - 'pose: { position: { x: 8, y: -4.0, z: 0.001000 }, ' - 'orientation: { x: 0.0, y: 0.0, z: 0.0, w: 1.0 } }' - ) + "pose: { position: { x: 8, y: -4.0, z: 0.001000 }, " + "orientation: { x: 0.0, y: 0.0, z: 0.0, w: 1.0 } }" + ), ], - output="screen" + output="screen", ) ] def generate_launch_description(): - pkg_share = FindPackageShare("teleop").find("teleop") - bridge_config_file = os.path.join(pkg_share, "cfg", 'bridge.yaml') + bridge_config_file = os.path.join(pkg_share, "cfg", "bridge.yaml") - return LaunchDescription([ - DeclareLaunchArgument( - 'px4_autopilot_path', - default_value='~/PX4-Autopilot', - description='Path to PX4-Autopilot repository root (supports ~)', - ), - Node( - package="ros_gz_bridge", - executable="parameter_bridge", - name="gz_pointcloud_bridge", - parameters=[ - {"config_file": bridge_config_file} - ] - ), - Node( - package="teleop", - executable="teleop", - name="teleop", - output="screen", - parameters=[ - {"use_sim_time": True} - ] - ), - LoadComposableNodes( - target_container='static_tf_container', - composable_node_descriptions=[ - ComposableNode( - package='tf2_ros', - plugin='tf2_ros::StaticTransformBroadcasterNode', - name='base_link_to_link_broadcaster', - parameters=[{ - 'use_sim_time': True, - 'translation.x': 0.0, - 'translation.y': 0.0, - 'translation.z': 0.0, - 'rotation.x': 0.0, - 'rotation.y': 0.0, - 'rotation.z': 0.0, - 'rotation.w': 1.0, - 'frame_id': 'base_link', - 'child_frame_id': 'link' - }] - ), - ComposableNode( - package='tf2_ros', - plugin='tf2_ros::StaticTransformBroadcasterNode', - name='map_to_target_aruco_broadcaster', - parameters=[{ - 'use_sim_time': True, - 'translation.x': 8.0, - 'translation.y': -4.0, - 'translation.z': 0.001, - 'rotation.x': 0.0, - 'rotation.y': 0.0, - 'rotation.z': 0.0, - 'rotation.w': 1.0, - 'frame_id': 'map', - 'child_frame_id': 'aruco_target_gt' - }] - ), - ] - ), - OpaqueFunction(function=_launch_setup) - ]) \ No newline at end of file + return LaunchDescription( + [ + DeclareLaunchArgument( + "px4_autopilot_path", + default_value="~/PX4-Autopilot", + description="Path to PX4-Autopilot repository root (supports ~)", + ), + Node( + package="ros_gz_bridge", + executable="parameter_bridge", + name="gz_pointcloud_bridge", + parameters=[{"config_file": bridge_config_file}], + ), + Node( + package="teleop", + executable="teleop", + name="teleop", + output="screen", + parameters=[{"use_sim_time": True}], + ), + LoadComposableNodes( + target_container="static_tf_container", + composable_node_descriptions=[ + ComposableNode( + package="tf2_ros", + plugin="tf2_ros::StaticTransformBroadcasterNode", + name="base_link_to_link_broadcaster", + parameters=[ + { + "use_sim_time": True, + "translation.x": 0.0, + "translation.y": 0.0, + "translation.z": 0.0, + "rotation.x": 0.0, + "rotation.y": 0.0, + "rotation.z": 0.0, + "rotation.w": 1.0, + "frame_id": "base_link", + "child_frame_id": "link", + } + ], + ), + ComposableNode( + package="tf2_ros", + plugin="tf2_ros::StaticTransformBroadcasterNode", + name="map_to_target_aruco_broadcaster", + parameters=[ + { + "use_sim_time": True, + "translation.x": 8.0, + "translation.y": -4.0, + "translation.z": 0.001, + "rotation.x": 0.0, + "rotation.y": 0.0, + "rotation.z": 0.0, + "rotation.w": 1.0, + "frame_id": "map", + "child_frame_id": "aruco_target_gt", + } + ], + ), + ], + ), + OpaqueFunction(function=_launch_setup), + ] + ) diff --git a/px4_roscon_workshop/teleop_twist_rpyt_keyboard/setup.py b/px4_roscon_workshop/teleop_twist_rpyt_keyboard/setup.py index e8a7501..353e68b 100644 --- a/px4_roscon_workshop/teleop_twist_rpyt_keyboard/setup.py +++ b/px4_roscon_workshop/teleop_twist_rpyt_keyboard/setup.py @@ -1,25 +1,24 @@ from setuptools import setup -package_name = 'teleop_twist_rpyt_keyboard' +package_name = "teleop_twist_rpyt_keyboard" setup( name=package_name, - version='0.0.0', - py_modules=[package_name.replace('-', '_')], # use module name - install_requires=['setuptools'], + version="0.0.0", + py_modules=[package_name.replace("-", "_")], # use module name + install_requires=["setuptools"], zip_safe=True, - maintainer='patrik_ark', - maintainer_email='pordipatrik@gmail.com', - description='TODO: Package description', - license='TODO: License declaration', + maintainer="patrik_ark", + maintainer_email="pordipatrik@gmail.com", + description="TODO: Package description", + license="TODO: License declaration", entry_points={ - 'console_scripts': [ - 'teleop_twist_rpyt_keyboard = teleop_twist_rpyt_keyboard:main', + "console_scripts": [ + "teleop_twist_rpyt_keyboard = teleop_twist_rpyt_keyboard:main", ], }, data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), ], ) diff --git a/px4_roscon_workshop/teleop_twist_rpyt_keyboard/teleop_twist_rpyt_keyboard.py b/px4_roscon_workshop/teleop_twist_rpyt_keyboard/teleop_twist_rpyt_keyboard.py index e9b99db..5fda3c1 100644 --- a/px4_roscon_workshop/teleop_twist_rpyt_keyboard/teleop_twist_rpyt_keyboard.py +++ b/px4_roscon_workshop/teleop_twist_rpyt_keyboard/teleop_twist_rpyt_keyboard.py @@ -1,12 +1,14 @@ #!/usr/bin/env python -from platform import node import rclpy from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy from geometry_msgs.msg import Twist from std_msgs.msg import Bool -import sys, select, termios, tty +import sys +import select +import termios +import tty settings = termios.tcgetattr(sys.stdin) @@ -29,24 +31,24 @@ # Format: (pitch, roll, throttle, yaw) moveBindings = { - 'w': (1, 0, 0, 0), # Pitch forward - 's': (-1, 0, 0, 0), # Pitch backward - 'a': (0, -1, 0, 0), # Roll left - 'd': (0, 1, 0, 0), # Roll right - 'r': (0, 0, 1, 0), # Throttle up - 'f': (0, 0, -1, 0), # Throttle down - 'q': (0, 0, 0, 1), # Yaw left - 'e': (0, 0, 0, -1), # Yaw right + "w": (1, 0, 0, 0), # Pitch forward + "s": (-1, 0, 0, 0), # Pitch backward + "a": (0, -1, 0, 0), # Roll left + "d": (0, 1, 0, 0), # Roll right + "r": (0, 0, 1, 0), # Throttle up + "f": (0, 0, -1, 0), # Throttle down + "q": (0, 0, 0, 1), # Yaw left + "e": (0, 0, 0, -1), # Yaw right } # Speed tuning bindings (linear, angular) speedBindings = { - 't': (1.1, 1.1), # Increase both - 'g': (0.9, 0.9), # Decrease both - 'y': (1.1, 1.0), # Increase linear only - 'h': (0.9, 1.0), # Decrease linear only - 'u': (1.0, 1.1), # Increase angular only - 'j': (1.0, 0.9), # Decrease angular only + "t": (1.1, 1.1), # Increase both + "g": (0.9, 0.9), # Decrease both + "y": (1.1, 1.0), # Increase linear only + "h": (0.9, 1.0), # Decrease linear only + "u": (1.0, 1.1), # Increase angular only + "j": (1.0, 0.9), # Decrease angular only } @@ -67,15 +69,15 @@ def main(args=None): args = sys.argv rclpy.init() - node = rclpy.create_node('teleop_twist_keyboard') + node = rclpy.create_node("teleop_twist_keyboard") qos = QoSProfile( - reliability=QoSReliabilityPolicy.RELIABLE, - history=QoSHistoryPolicy.KEEP_LAST, - depth=10 + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=10, ) - pub = node.create_publisher(Twist, 'cmd_vel', qos) - active_pub = node.create_publisher(Bool, '/teleop/active', qos) + pub = node.create_publisher(Twist, "cmd_vel", qos) + active_pub = node.create_publisher(Bool, "/teleop/active", qos) # Publish active=True at start active_msg = Bool() active_msg.data = True @@ -97,7 +99,7 @@ def main(args=None): speed *= speedBindings[key][0] turn *= speedBindings[key][1] print(vels(speed, turn)) - + pitch = roll = throttle = yaw = 0.0 if status == 14: @@ -106,7 +108,7 @@ def main(args=None): else: pitch = roll = throttle = yaw = 0.0 - if key == '\x03': + if key == "\x03": break twist = Twist() @@ -135,6 +137,6 @@ def main(args=None): termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) print("\nTeleop shutdown. Sent /teleop/active = False") -if __name__ == '__main__': + +if __name__ == "__main__": main() - \ No newline at end of file