diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json
index 84688714..206c4398 100644
--- a/.devcontainer/devcontainer.json
+++ b/.devcontainer/devcontainer.json
@@ -16,7 +16,8 @@
// "ROS_DOMAIN_ID": "${localEnv:ROS_DOMAIN_ID}"
},
"initializeCommand": "git config --global --add safe.directory '*' && git reset --soft && git submodule update --init --recursive && git config --global submodule.recurse true && git pull || true",
- "postCreateCommand": "echo 'source /opt/ros/humble/setup.bash' >> ~/.bashrc && echo 'source /workspaces/Lunabotics/install/setup.bash' >> ~/.bashrc && sudo apt-get update && sudo apt install git-lfs",
+ "postCreateCommand": "sudo rm -f /etc/apt/sources.list.d/ros2.list && sudo rm -f /etc/apt/sources.list.d/ros2-latest.list && sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg && echo \"deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu jammy main\" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null && sudo apt-get update && sudo apt install -y git-lfs && echo 'source /opt/ros/humble/setup.bash' >> ~/.bashrc && echo 'source /workspaces/Lunabotics/install/setup.bash' >> ~/.bashrc && sudo chown -R admin:admin /workspaces/Lunabotics",
+
"postStartCommand": "rosdep update && rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y --skip-keys 'nvblox negotiated draco_point_cloud_transport point_cloud_transport_plugins ffmpeg_encoder_decoder ffmpeg_image_transport isaac_ros_peoplesemseg_models_install isaac_ros_h264_decoder isaac_ros_unet isaac_ros_triton isaac_ros_dnn_image_encoder nova_carter_navigation'",
// "mounts": [
// "type=bind,source=/tmp/.X11-unix,target=/tmp/.X11-unix",
@@ -48,35 +49,20 @@
"files.autoSave": "afterDelay",
"editor.formatOnSave": true,
"[python]": {
- "editor.formatOnPaste": false,
- "editor.formatOnSaveMode": "file"
- },
- "[workbench.view.extension]": {
- "ms-python.python": false
+ "editor.defaultFormatter": "ms-python.black-formatter",
+ "editor.formatOnSave": true,
+ "editor.codeActionsOnSave": {
+ "source.fixAll": "explicit"
+ }
},
"black-formatter.args": [
"--line-length",
"120"
],
- "[C_Cpp]": {
- "default.cppStandard": "c++17",
- "default.cStandard": "c99"
+ "[c++]": {
+ "editor.defaultFormatter": "ms-vscode.cpptools"
},
- "ruff.lineLength": 99,
- "ruff.lint.enable": true,
- "ruff.lint.select": [
- "A",
- "C",
- "CNL",
- "D",
- "E",
- "F",
- "I",
- "Q",
- "W"
- ],
- "ruff.nativeServer": true,
- "C_Cpp.clang_format_fallbackStyle": "Google",
+ "C_Cpp.clang_format_fallbackStyle": "{ ColumnLimit: 120 }",
"diffEditor.ignoreTrimWhitespace": false,
// Make bash the default shell in the container terminal
"terminal.integrated.defaultProfile.linux": "bash"
diff --git a/.gitmodules b/.gitmodules
index ccf4d9d4..1b025f7b 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -30,3 +30,9 @@
path = src/isaac_ros/isaac_ros_nvblox
url = https://github.com/GOFIRST-Robotics/isaac_ros_nvblox.git
branch = main
+[submodule "src/behaviortree_ros2"]
+ path = src/behaviortree_ros2
+ url = https://github.com/BehaviorTree/BehaviorTree.ROS2.git
+[submodule "src/behaviortree_cpp"]
+ path = src/behaviortree_cpp
+ url = https://github.com/BehaviorTree/BehaviorTree.CPP.git
diff --git a/.vscode/tasks.json b/.vscode/tasks.json
index d70e11dc..a0dc610c 100644
--- a/.vscode/tasks.json
+++ b/.vscode/tasks.json
@@ -3,7 +3,7 @@
"tasks": [
{
"label": "Build All Task",
- "type": "colcon",
+ "command": "colcon",
"args": [
"build",
"--symlink-install"
@@ -13,7 +13,7 @@
},
{
"label": "Build No GPU Task",
- "type": "colcon",
+ "command": "colcon",
"args": [
"build",
"--symlink-install",
@@ -24,15 +24,6 @@
],
"problemMatcher": [],
"group": "build"
- },
- {
- "label": "Test All",
- "type": "colcon",
- "args": [
- "test"
- ],
- "problemMatcher": [],
- "group": "test"
- },
+ }
]
}
\ No newline at end of file
diff --git a/Back + Sides.perspective b/Back + Sides.perspective
new file mode 100644
index 00000000..4b1ff887
--- /dev/null
+++ b/Back + Sides.perspective
@@ -0,0 +1,253 @@
+{
+ "keys": {},
+ "groups": {
+ "mainwindow": {
+ "keys": {
+ "geometry": {
+ "repr(QByteArray.hex)": "QtCore.QByteArray(b'01d9d0cb00030000000000460000001b0000077f000004af00000046000000400000077f000004af0000000000000000078000000046000000400000077f000004af')",
+ "type": "repr(QByteArray.hex)",
+ "pretty-print": " F F @ F @ "
+ },
+ "state": {
+ "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000000fd00000001000000030000073a00000446fc0100000003fb0000005a007200710074005f0069006d006100670065005f0076006900650077005f005f0049006d0061006700650056006900650077005f005f0033005f005f0049006d0061006700650056006900650077005700690064006700650074010000000000000268000000d300fffffffb0000005a007200710074005f0069006d006100670065005f0076006900650077005f005f0049006d0061006700650056006900650077005f005f0031005f005f0049006d0061006700650056006900650077005700690064006700650074010000026e00000262000000be00fffffffb0000005a007200710074005f0069006d006100670065005f0076006900650077005f005f0049006d0061006700650056006900650077005f005f0032005f005f0049006d006100670065005600690065007700570069006400670065007401000004d600000264000000d300ffffff0000073a0000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')",
+ "type": "repr(QByteArray.hex)",
+ "pretty-print": " Zrqt_image_view__ImageView__3__ImageViewWidget Zrqt_image_view__ImageView__1__ImageViewWidget Zrqt_image_view__ImageView__2__ImageViewWidget 6MinimizedDockWidgetsToolbar "
+ }
+ },
+ "groups": {
+ "toolbar_areas": {
+ "keys": {
+ "MinimizedDockWidgetsToolbar": {
+ "repr": "8",
+ "type": "repr"
+ }
+ },
+ "groups": {}
+ }
+ }
+ },
+ "pluginmanager": {
+ "keys": {
+ "running-plugins": {
+ "repr": "{'rqt_image_view/ImageView': [1, 2, 3]}",
+ "type": "repr"
+ }
+ },
+ "groups": {
+ "plugin__rqt_image_view__ImageView__1": {
+ "keys": {},
+ "groups": {
+ "dock_widget__ImageViewWidget": {
+ "keys": {
+ "dock_widget_title": {
+ "repr": "'Image View'",
+ "type": "repr"
+ },
+ "dockable": {
+ "repr": "True",
+ "type": "repr"
+ },
+ "parent": {
+ "repr": "None",
+ "type": "repr"
+ }
+ },
+ "groups": {}
+ },
+ "plugin": {
+ "keys": {
+ "color_scheme": {
+ "repr": "-1",
+ "type": "repr"
+ },
+ "dynamic_range": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "max_range": {
+ "repr": "10.0",
+ "type": "repr"
+ },
+ "mouse_pub_topic": {
+ "repr": "'/back/image_raw/compressed_mouse_left'",
+ "type": "repr"
+ },
+ "num_gridlines": {
+ "repr": "0",
+ "type": "repr"
+ },
+ "publish_click_location": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "rotate": {
+ "repr": "3",
+ "type": "repr"
+ },
+ "smooth_image": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "toolbar_hidden": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "topic": {
+ "repr": "'/back/image_raw/compressed'",
+ "type": "repr"
+ },
+ "zoom1": {
+ "repr": "False",
+ "type": "repr"
+ }
+ },
+ "groups": {}
+ }
+ }
+ },
+ "plugin__rqt_image_view__ImageView__2": {
+ "keys": {},
+ "groups": {
+ "dock_widget__ImageViewWidget": {
+ "keys": {
+ "dock_widget_title": {
+ "repr": "'Image View (2)'",
+ "type": "repr"
+ },
+ "dockable": {
+ "repr": "True",
+ "type": "repr"
+ },
+ "parent": {
+ "repr": "None",
+ "type": "repr"
+ }
+ },
+ "groups": {}
+ },
+ "plugin": {
+ "keys": {
+ "color_scheme": {
+ "repr": "-1",
+ "type": "repr"
+ },
+ "dynamic_range": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "max_range": {
+ "repr": "10.0",
+ "type": "repr"
+ },
+ "mouse_pub_topic": {
+ "repr": "'/left/image_raw/compressed_mouse_left'",
+ "type": "repr"
+ },
+ "num_gridlines": {
+ "repr": "0",
+ "type": "repr"
+ },
+ "publish_click_location": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "rotate": {
+ "repr": "0",
+ "type": "repr"
+ },
+ "smooth_image": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "toolbar_hidden": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "topic": {
+ "repr": "'/left/image_raw/compressed'",
+ "type": "repr"
+ },
+ "zoom1": {
+ "repr": "False",
+ "type": "repr"
+ }
+ },
+ "groups": {}
+ }
+ }
+ },
+ "plugin__rqt_image_view__ImageView__3": {
+ "keys": {},
+ "groups": {
+ "dock_widget__ImageViewWidget": {
+ "keys": {
+ "dock_widget_title": {
+ "repr": "'Image View (3)'",
+ "type": "repr"
+ },
+ "dockable": {
+ "repr": "True",
+ "type": "repr"
+ },
+ "parent": {
+ "repr": "None",
+ "type": "repr"
+ }
+ },
+ "groups": {}
+ },
+ "plugin": {
+ "keys": {
+ "color_scheme": {
+ "repr": "-1",
+ "type": "repr"
+ },
+ "dynamic_range": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "max_range": {
+ "repr": "10.0",
+ "type": "repr"
+ },
+ "mouse_pub_topic": {
+ "repr": "'/right/image_raw/compressed_mouse_left'",
+ "type": "repr"
+ },
+ "num_gridlines": {
+ "repr": "0",
+ "type": "repr"
+ },
+ "publish_click_location": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "rotate": {
+ "repr": "0",
+ "type": "repr"
+ },
+ "smooth_image": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "toolbar_hidden": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "topic": {
+ "repr": "'/right/image_raw/compressed'",
+ "type": "repr"
+ },
+ "zoom1": {
+ "repr": "False",
+ "type": "repr"
+ }
+ },
+ "groups": {}
+ }
+ }
+ }
+ }
+ }
+ }
+}
\ No newline at end of file
diff --git a/Back.perspective b/Back.perspective
new file mode 100644
index 00000000..7ea9a964
--- /dev/null
+++ b/Back.perspective
@@ -0,0 +1,182 @@
+{
+ "keys": {},
+ "groups": {
+ "mainwindow": {
+ "keys": {
+ "geometry": {
+ "repr(QByteArray.hex)": "QtCore.QByteArray(b'01d9d0cb00030000000000460000001b0000077f000004af00000046000000400000077f000004af0000000000000000078000000046000000400000077f000004af')",
+ "type": "repr(QByteArray.hex)",
+ "pretty-print": " F F @ F @ "
+ },
+ "state": {
+ "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000000fd00000001000000030000073a00000446fc0100000002fb0000005a007200710074005f0069006d006100670065005f0076006900650077005f005f0049006d0061006700650056006900650077005f005f0031005f005f0049006d006100670065005600690065007700570069006400670065007401000000000000073a000000be00fffffffb0000005a007200710074005f0069006d006100670065005f0076006900650077005f005f0049006d0061006700650056006900650077005f005f0032005f005f0049006d0061006700650056006900650077005700690064006700650074010000039f0000039b00000000000000000000073a0000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')",
+ "type": "repr(QByteArray.hex)",
+ "pretty-print": " Zrqt_image_view__ImageView__1__ImageViewWidget Zrqt_image_view__ImageView__2__ImageViewWidget 6MinimizedDockWidgetsToolbar "
+ }
+ },
+ "groups": {
+ "toolbar_areas": {
+ "keys": {
+ "MinimizedDockWidgetsToolbar": {
+ "repr": "8",
+ "type": "repr"
+ }
+ },
+ "groups": {}
+ }
+ }
+ },
+ "pluginmanager": {
+ "keys": {
+ "running-plugins": {
+ "repr": "{'rqt_image_view/ImageView': [1]}",
+ "type": "repr"
+ }
+ },
+ "groups": {
+ "plugin__rqt_image_view__ImageView__1": {
+ "keys": {},
+ "groups": {
+ "dock_widget__ImageViewWidget": {
+ "keys": {
+ "dock_widget_title": {
+ "repr": "'Image View'",
+ "type": "repr"
+ },
+ "dockable": {
+ "repr": "True",
+ "type": "repr"
+ },
+ "parent": {
+ "repr": "None",
+ "type": "repr"
+ }
+ },
+ "groups": {}
+ },
+ "plugin": {
+ "keys": {
+ "color_scheme": {
+ "repr": "-1",
+ "type": "repr"
+ },
+ "dynamic_range": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "max_range": {
+ "repr": "10.0",
+ "type": "repr"
+ },
+ "mouse_pub_topic": {
+ "repr": "'/back/image_raw/compressed_mouse_left'",
+ "type": "repr"
+ },
+ "num_gridlines": {
+ "repr": "0",
+ "type": "repr"
+ },
+ "publish_click_location": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "rotate": {
+ "repr": "3",
+ "type": "repr"
+ },
+ "smooth_image": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "toolbar_hidden": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "topic": {
+ "repr": "'/back/image_raw/compressed'",
+ "type": "repr"
+ },
+ "zoom1": {
+ "repr": "False",
+ "type": "repr"
+ }
+ },
+ "groups": {}
+ }
+ }
+ },
+ "plugin__rqt_image_view__ImageView__2": {
+ "keys": {},
+ "groups": {
+ "dock_widget__ImageViewWidget": {
+ "keys": {
+ "dock_widget_title": {
+ "repr": "'Image View (2)'",
+ "type": "repr"
+ },
+ "dockable": {
+ "repr": "True",
+ "type": "repr"
+ },
+ "parent": {
+ "repr": "None",
+ "type": "repr"
+ }
+ },
+ "groups": {}
+ },
+ "plugin": {
+ "keys": {
+ "color_scheme": {
+ "repr": "-1",
+ "type": "repr"
+ },
+ "dynamic_range": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "max_range": {
+ "repr": "10.0",
+ "type": "repr"
+ },
+ "mouse_pub_topic": {
+ "repr": "'/dumper/image_raw_mouse_left'",
+ "type": "repr"
+ },
+ "num_gridlines": {
+ "repr": "0",
+ "type": "repr"
+ },
+ "publish_click_location": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "rotate": {
+ "repr": "0",
+ "type": "repr"
+ },
+ "smooth_image": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "toolbar_hidden": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "topic": {
+ "repr": "'/dumper/image_raw'",
+ "type": "repr"
+ },
+ "zoom1": {
+ "repr": "False",
+ "type": "repr"
+ }
+ },
+ "groups": {}
+ }
+ }
+ }
+ }
+ }
+ }
+}
\ No newline at end of file
diff --git a/Front + Sides.perspective b/Front + Sides.perspective
new file mode 100644
index 00000000..cae90381
--- /dev/null
+++ b/Front + Sides.perspective
@@ -0,0 +1,253 @@
+{
+ "keys": {},
+ "groups": {
+ "mainwindow": {
+ "keys": {
+ "geometry": {
+ "repr(QByteArray.hex)": "QtCore.QByteArray(b'01d9d0cb00030000000000460000001b0000077f000004af00000046000000400000077f000004af0000000000000000078000000046000000400000077f000004af')",
+ "type": "repr(QByteArray.hex)",
+ "pretty-print": " F F @ F @ "
+ },
+ "state": {
+ "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000000fd00000001000000030000073a00000446fc0100000003fb0000005a007200710074005f0069006d006100670065005f0076006900650077005f005f0049006d0061006700650056006900650077005f005f0032005f005f0049006d00610067006500560069006500770057006900640067006500740100000000000001e1000000d300fffffffb0000005a007200710074005f0069006d006100670065005f0076006900650077005f005f0049006d0061006700650056006900650077005f005f0031005f005f0049006d006100670065005600690065007700570069006400670065007401000001e70000035d000000be00fffffffb0000005a007200710074005f0069006d006100670065005f0076006900650077005f005f0049006d0061006700650056006900650077005f005f0033005f005f0049006d0061006700650056006900650077005700690064006700650074010000054a000001f0000000d300ffffff0000073a0000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')",
+ "type": "repr(QByteArray.hex)",
+ "pretty-print": " Zrqt_image_view__ImageView__2__ImageViewWidget Zrqt_image_view__ImageView__1__ImageViewWidget Zrqt_image_view__ImageView__3__ImageViewWidget 6MinimizedDockWidgetsToolbar "
+ }
+ },
+ "groups": {
+ "toolbar_areas": {
+ "keys": {
+ "MinimizedDockWidgetsToolbar": {
+ "repr": "8",
+ "type": "repr"
+ }
+ },
+ "groups": {}
+ }
+ }
+ },
+ "pluginmanager": {
+ "keys": {
+ "running-plugins": {
+ "repr": "{'rqt_image_view/ImageView': [1, 2, 3]}",
+ "type": "repr"
+ }
+ },
+ "groups": {
+ "plugin__rqt_image_view__ImageView__1": {
+ "keys": {},
+ "groups": {
+ "dock_widget__ImageViewWidget": {
+ "keys": {
+ "dock_widget_title": {
+ "repr": "'Image View'",
+ "type": "repr"
+ },
+ "dockable": {
+ "repr": "True",
+ "type": "repr"
+ },
+ "parent": {
+ "repr": "None",
+ "type": "repr"
+ }
+ },
+ "groups": {}
+ },
+ "plugin": {
+ "keys": {
+ "color_scheme": {
+ "repr": "-1",
+ "type": "repr"
+ },
+ "dynamic_range": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "max_range": {
+ "repr": "10.0",
+ "type": "repr"
+ },
+ "mouse_pub_topic": {
+ "repr": "'/zed2i/zed_node_zed2i/rgb_raw/image_raw_color/compressed_mouse_left'",
+ "type": "repr"
+ },
+ "num_gridlines": {
+ "repr": "0",
+ "type": "repr"
+ },
+ "publish_click_location": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "rotate": {
+ "repr": "0",
+ "type": "repr"
+ },
+ "smooth_image": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "toolbar_hidden": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "topic": {
+ "repr": "'/zed2i/zed_node_zed2i/rgb_raw/image_raw_color/compressed'",
+ "type": "repr"
+ },
+ "zoom1": {
+ "repr": "False",
+ "type": "repr"
+ }
+ },
+ "groups": {}
+ }
+ }
+ },
+ "plugin__rqt_image_view__ImageView__2": {
+ "keys": {},
+ "groups": {
+ "dock_widget__ImageViewWidget": {
+ "keys": {
+ "dock_widget_title": {
+ "repr": "'Image View (2)'",
+ "type": "repr"
+ },
+ "dockable": {
+ "repr": "True",
+ "type": "repr"
+ },
+ "parent": {
+ "repr": "None",
+ "type": "repr"
+ }
+ },
+ "groups": {}
+ },
+ "plugin": {
+ "keys": {
+ "color_scheme": {
+ "repr": "-1",
+ "type": "repr"
+ },
+ "dynamic_range": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "max_range": {
+ "repr": "10.0",
+ "type": "repr"
+ },
+ "mouse_pub_topic": {
+ "repr": "'/left/image_raw/compressed_mouse_left'",
+ "type": "repr"
+ },
+ "num_gridlines": {
+ "repr": "0",
+ "type": "repr"
+ },
+ "publish_click_location": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "rotate": {
+ "repr": "0",
+ "type": "repr"
+ },
+ "smooth_image": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "toolbar_hidden": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "topic": {
+ "repr": "'/left/image_raw/compressed'",
+ "type": "repr"
+ },
+ "zoom1": {
+ "repr": "False",
+ "type": "repr"
+ }
+ },
+ "groups": {}
+ }
+ }
+ },
+ "plugin__rqt_image_view__ImageView__3": {
+ "keys": {},
+ "groups": {
+ "dock_widget__ImageViewWidget": {
+ "keys": {
+ "dock_widget_title": {
+ "repr": "'Image View (3)'",
+ "type": "repr"
+ },
+ "dockable": {
+ "repr": "True",
+ "type": "repr"
+ },
+ "parent": {
+ "repr": "None",
+ "type": "repr"
+ }
+ },
+ "groups": {}
+ },
+ "plugin": {
+ "keys": {
+ "color_scheme": {
+ "repr": "-1",
+ "type": "repr"
+ },
+ "dynamic_range": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "max_range": {
+ "repr": "10.0",
+ "type": "repr"
+ },
+ "mouse_pub_topic": {
+ "repr": "'/right/image_raw/compressed_mouse_left'",
+ "type": "repr"
+ },
+ "num_gridlines": {
+ "repr": "0",
+ "type": "repr"
+ },
+ "publish_click_location": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "rotate": {
+ "repr": "0",
+ "type": "repr"
+ },
+ "smooth_image": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "toolbar_hidden": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "topic": {
+ "repr": "'/right/image_raw/compressed'",
+ "type": "repr"
+ },
+ "zoom1": {
+ "repr": "False",
+ "type": "repr"
+ }
+ },
+ "groups": {}
+ }
+ }
+ }
+ }
+ }
+ }
+}
\ No newline at end of file
diff --git a/Front.perspective b/Front.perspective
new file mode 100644
index 00000000..9aeb250e
--- /dev/null
+++ b/Front.perspective
@@ -0,0 +1,182 @@
+{
+ "keys": {},
+ "groups": {
+ "mainwindow": {
+ "keys": {
+ "geometry": {
+ "repr(QByteArray.hex)": "QtCore.QByteArray(b'01d9d0cb00030000000000460000001b0000077f000004af00000046000000400000077f000004af0000000000000000078000000046000000400000077f000004af')",
+ "type": "repr(QByteArray.hex)",
+ "pretty-print": " F F @ F @ "
+ },
+ "state": {
+ "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000000fd00000001000000030000073a00000446fc0100000002fb0000005a007200710074005f0069006d006100670065005f0076006900650077005f005f0049006d0061006700650056006900650077005f005f0031005f005f0049006d006100670065005600690065007700570069006400670065007401000000000000073a000000be00fffffffb0000005a007200710074005f0069006d006100670065005f0076006900650077005f005f0049006d0061006700650056006900650077005f005f0032005f005f0049006d0061006700650056006900650077005700690064006700650074010000039f0000039b00000000000000000000073a0000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')",
+ "type": "repr(QByteArray.hex)",
+ "pretty-print": " Zrqt_image_view__ImageView__1__ImageViewWidget Zrqt_image_view__ImageView__2__ImageViewWidget 6MinimizedDockWidgetsToolbar "
+ }
+ },
+ "groups": {
+ "toolbar_areas": {
+ "keys": {
+ "MinimizedDockWidgetsToolbar": {
+ "repr": "8",
+ "type": "repr"
+ }
+ },
+ "groups": {}
+ }
+ }
+ },
+ "pluginmanager": {
+ "keys": {
+ "running-plugins": {
+ "repr": "{'rqt_image_view/ImageView': [1]}",
+ "type": "repr"
+ }
+ },
+ "groups": {
+ "plugin__rqt_image_view__ImageView__1": {
+ "keys": {},
+ "groups": {
+ "dock_widget__ImageViewWidget": {
+ "keys": {
+ "dock_widget_title": {
+ "repr": "'Image View'",
+ "type": "repr"
+ },
+ "dockable": {
+ "repr": "True",
+ "type": "repr"
+ },
+ "parent": {
+ "repr": "None",
+ "type": "repr"
+ }
+ },
+ "groups": {}
+ },
+ "plugin": {
+ "keys": {
+ "color_scheme": {
+ "repr": "-1",
+ "type": "repr"
+ },
+ "dynamic_range": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "max_range": {
+ "repr": "10.0",
+ "type": "repr"
+ },
+ "mouse_pub_topic": {
+ "repr": "'/zed2i/zed_node_zed2i/rgb_raw/image_raw_color/compressed_mouse_left'",
+ "type": "repr"
+ },
+ "num_gridlines": {
+ "repr": "0",
+ "type": "repr"
+ },
+ "publish_click_location": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "rotate": {
+ "repr": "0",
+ "type": "repr"
+ },
+ "smooth_image": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "toolbar_hidden": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "topic": {
+ "repr": "'/zed2i/zed_node_zed2i/rgb_raw/image_raw_color/compressed'",
+ "type": "repr"
+ },
+ "zoom1": {
+ "repr": "False",
+ "type": "repr"
+ }
+ },
+ "groups": {}
+ }
+ }
+ },
+ "plugin__rqt_image_view__ImageView__2": {
+ "keys": {},
+ "groups": {
+ "dock_widget__ImageViewWidget": {
+ "keys": {
+ "dock_widget_title": {
+ "repr": "'Image View (2)'",
+ "type": "repr"
+ },
+ "dockable": {
+ "repr": "'true'",
+ "type": "repr"
+ },
+ "parent": {
+ "repr": "None",
+ "type": "repr"
+ }
+ },
+ "groups": {}
+ },
+ "plugin": {
+ "keys": {
+ "color_scheme": {
+ "repr": "'-1'",
+ "type": "repr"
+ },
+ "dynamic_range": {
+ "repr": "'false'",
+ "type": "repr"
+ },
+ "max_range": {
+ "repr": "'10'",
+ "type": "repr"
+ },
+ "mouse_pub_topic": {
+ "repr": "'/dumper/image_raw_mouse_left'",
+ "type": "repr"
+ },
+ "num_gridlines": {
+ "repr": "'0'",
+ "type": "repr"
+ },
+ "publish_click_location": {
+ "repr": "'false'",
+ "type": "repr"
+ },
+ "rotate": {
+ "repr": "'0'",
+ "type": "repr"
+ },
+ "smooth_image": {
+ "repr": "'false'",
+ "type": "repr"
+ },
+ "toolbar_hidden": {
+ "repr": "'false'",
+ "type": "repr"
+ },
+ "topic": {
+ "repr": "'/dumper/image_raw'",
+ "type": "repr"
+ },
+ "zoom1": {
+ "repr": "'false'",
+ "type": "repr"
+ }
+ },
+ "groups": {}
+ }
+ }
+ }
+ }
+ }
+ }
+}
\ No newline at end of file
diff --git a/Groot2 b/Groot2
new file mode 160000
index 00000000..b65aff68
--- /dev/null
+++ b/Groot2
@@ -0,0 +1 @@
+Subproject commit b65aff68445e34e63516e01378d489261e76c4f1
diff --git a/Inside Cams.perspective b/Inside Cams.perspective
new file mode 100644
index 00000000..f5f40894
--- /dev/null
+++ b/Inside Cams.perspective
@@ -0,0 +1,182 @@
+{
+ "keys": {},
+ "groups": {
+ "mainwindow": {
+ "keys": {
+ "geometry": {
+ "repr(QByteArray.hex)": "QtCore.QByteArray(b'01d9d0cb00030000000000460000001b0000077f000004af00000046000000400000077f000004af0000000000000000078000000046000000400000077f000004af')",
+ "type": "repr(QByteArray.hex)",
+ "pretty-print": " F F @ F @ "
+ },
+ "state": {
+ "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000000fd00000001000000030000073a00000446fc0100000002fb0000005a007200710074005f0069006d006100670065005f0076006900650077005f005f0049006d0061006700650056006900650077005f005f0031005f005f0049006d0061006700650056006900650077005700690064006700650074010000000000000399000000be00fffffffb0000005a007200710074005f0069006d006100670065005f0076006900650077005f005f0049006d0061006700650056006900650077005f005f0032005f005f0049006d0061006700650056006900650077005700690064006700650074010000039f0000039b000000d300ffffff0000073a0000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')",
+ "type": "repr(QByteArray.hex)",
+ "pretty-print": " Zrqt_image_view__ImageView__1__ImageViewWidget Zrqt_image_view__ImageView__2__ImageViewWidget 6MinimizedDockWidgetsToolbar "
+ }
+ },
+ "groups": {
+ "toolbar_areas": {
+ "keys": {
+ "MinimizedDockWidgetsToolbar": {
+ "repr": "8",
+ "type": "repr"
+ }
+ },
+ "groups": {}
+ }
+ }
+ },
+ "pluginmanager": {
+ "keys": {
+ "running-plugins": {
+ "repr": "{'rqt_image_view/ImageView': [1, 2]}",
+ "type": "repr"
+ }
+ },
+ "groups": {
+ "plugin__rqt_image_view__ImageView__1": {
+ "keys": {},
+ "groups": {
+ "dock_widget__ImageViewWidget": {
+ "keys": {
+ "dock_widget_title": {
+ "repr": "'Image View'",
+ "type": "repr"
+ },
+ "dockable": {
+ "repr": "True",
+ "type": "repr"
+ },
+ "parent": {
+ "repr": "None",
+ "type": "repr"
+ }
+ },
+ "groups": {}
+ },
+ "plugin": {
+ "keys": {
+ "color_scheme": {
+ "repr": "-1",
+ "type": "repr"
+ },
+ "dynamic_range": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "max_range": {
+ "repr": "10.0",
+ "type": "repr"
+ },
+ "mouse_pub_topic": {
+ "repr": "'/digger/image_raw_mouse_left'",
+ "type": "repr"
+ },
+ "num_gridlines": {
+ "repr": "0",
+ "type": "repr"
+ },
+ "publish_click_location": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "rotate": {
+ "repr": "0",
+ "type": "repr"
+ },
+ "smooth_image": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "toolbar_hidden": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "topic": {
+ "repr": "'/digger/image_raw'",
+ "type": "repr"
+ },
+ "zoom1": {
+ "repr": "False",
+ "type": "repr"
+ }
+ },
+ "groups": {}
+ }
+ }
+ },
+ "plugin__rqt_image_view__ImageView__2": {
+ "keys": {},
+ "groups": {
+ "dock_widget__ImageViewWidget": {
+ "keys": {
+ "dock_widget_title": {
+ "repr": "'Image View (2)'",
+ "type": "repr"
+ },
+ "dockable": {
+ "repr": "True",
+ "type": "repr"
+ },
+ "parent": {
+ "repr": "None",
+ "type": "repr"
+ }
+ },
+ "groups": {}
+ },
+ "plugin": {
+ "keys": {
+ "color_scheme": {
+ "repr": "-1",
+ "type": "repr"
+ },
+ "dynamic_range": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "max_range": {
+ "repr": "10.0",
+ "type": "repr"
+ },
+ "mouse_pub_topic": {
+ "repr": "'/dumper/image_raw_mouse_left'",
+ "type": "repr"
+ },
+ "num_gridlines": {
+ "repr": "0",
+ "type": "repr"
+ },
+ "publish_click_location": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "rotate": {
+ "repr": "0",
+ "type": "repr"
+ },
+ "smooth_image": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "toolbar_hidden": {
+ "repr": "False",
+ "type": "repr"
+ },
+ "topic": {
+ "repr": "'/dumper/image_raw'",
+ "type": "repr"
+ },
+ "zoom1": {
+ "repr": "False",
+ "type": "repr"
+ }
+ },
+ "groups": {}
+ }
+ }
+ }
+ }
+ }
+ }
+}
\ No newline at end of file
diff --git a/arduino/sensors/sensors.ino b/arduino/sensors/sensors.ino
index fc3ef28b..dc9ac952 100644
--- a/arduino/sensors/sensors.ino
+++ b/arduino/sensors/sensors.ino
@@ -1,30 +1,21 @@
// Define a struct to hold the sensor data
struct SensorData {
- bool diggerTopLimitSwitch;
- bool diggerBottomLimitSwitch;
- bool dumperTopLimitSwitch;
- bool dumperBottomLimitSwitch;
int leftMotorPotentiometer;
int rightMotorPotentiometer;
+ bool bottomLimitSwitch;
};
// Define the sensor pins here
-#define DIGGER_TOP_LIMIT_SWITCH 2
-#define DIGGER_BOTTOM_LIMIT_SWITCH 3
-#define DUMPER_TOP_LIMIT_SWITCH 4
-#define DUMPER_BOTTOM_LIMIT_SWITCH 5
#define LEFT_MOTOR_POT_PIN A0
#define RIGHT_MOTOR_POT_PIN A1
+#define RELAY_PIN 7
void setup() {
// Initialize serial communication
Serial.begin(9600);
- // Initialize the digital inputs (limit switches)
- pinMode(DIGGER_TOP_LIMIT_SWITCH, INPUT_PULLUP);
- pinMode(DIGGER_BOTTOM_LIMIT_SWITCH, INPUT_PULLUP);
- pinMode(DUMPER_TOP_LIMIT_SWITCH, INPUT_PULLUP);
- pinMode(DUMPER_BOTTOM_LIMIT_SWITCH, INPUT_PULLUP);
+ pinMode(RELAY_PIN, OUTPUT); // Set relay pin as an output
+ digitalWrite(RELAY_PIN, LOW); // Ensure the motor is off at the start
// No need to configure analog pins explicitly for potentiometers
// as they are used as inputs by default.
@@ -34,19 +25,35 @@ void loop() {
// Create a SensorData struct
SensorData data;
- // Read from the digital inputs (limit switches)
- data.diggerTopLimitSwitch = (bool)digitalRead(DIGGER_TOP_LIMIT_SWITCH);
- data.diggerBottomLimitSwitch = (bool)digitalRead(DIGGER_BOTTOM_LIMIT_SWITCH);
- data.dumperTopLimitSwitch = (bool)digitalRead(DUMPER_TOP_LIMIT_SWITCH);
- data.dumperBottomLimitSwitch = (bool)digitalRead(DUMPER_BOTTOM_LIMIT_SWITCH);
-
// Read from the analog inputs (potentiometers)
data.leftMotorPotentiometer = analogRead(LEFT_MOTOR_POT_PIN); // Read left motor potentiometer value
data.rightMotorPotentiometer = analogRead(RIGHT_MOTOR_POT_PIN); // Read right motor potentiometer value
+ data.bottomLimitSwitch = analogRead(bottom_limit_switch); //bottom limit switch value
+
// Send the struct over the serial bus to the Nvidia Jetson
Serial.write((byte *)&data, sizeof(SensorData));
+ // Check for incoming relay commands from the Jetson
+ if (Serial.available()) {
+ byte cmd = Serial.read();
+ static bool relayState = LOW;
+ switch (cmd) {
+ case '0': // Off
+ relayState = LOW;
+ break;
+ case '1': // On
+ relayState = HIGH;
+ break;
+ case '2': // Toggle
+ relayState = !relayState;
+ break;
+ default: // Unknown command
+ break;
+ }
+ digitalWrite(RELAY_PIN, relayState);
+ }
+
// Wait 100ms before the next loop iteration
delay(100);
}
diff --git a/config/auger_config.yaml b/config/auger_config.yaml
new file mode 100644
index 00000000..5d2a7153
--- /dev/null
+++ b/config/auger_config.yaml
@@ -0,0 +1,21 @@
+/**:
+ ros__parameters:
+ #TODO: Figure out all of these values
+ SCREW_SPEED: 4000 # TODO: update
+ POWER_LIMIT: 1 #TODO: update
+ MAX_SCREW_SPEED: 4000 # in RPM
+ MIN_SCREW_SPEED: 2000 # in RPM TODO: This is a guess
+ MAX_RETRACT_PUSH_MOTOR_VELOCITY: -600 # in RPM, potentially could be faster
+ MAX_EXTEND_PUSH_MOTOR_VELOCITY: 600 # in RPM
+ push_motor_position: 0
+ tilt_actuator_position: 0
+ TILT_ACTUATOR_CURRENT_THRESHOLD: 0
+ MAX_PUSH_MOTOR_POSITION: 0
+ MIN_PUSH_MOTOR_POSITION: 0
+ PUSH_MOTOR_POS_TOLERANCE: 0
+ MAX_PUSH_MOTOR_VELOCITY: 0
+ TILT_ACTUATOR_SPEED: 0
+ TILT_ACTUATOR_MIN_EXTENSION: 0
+ TILT_ACTUATOR_ID: 0
+ PUSH_MOTOR_ID: 0
+ SPIN_MOTOR_ID: 0
\ No newline at end of file
diff --git a/config/costmap_config.yaml b/config/costmap_config.yaml
new file mode 100644
index 00000000..61dfb40b
--- /dev/null
+++ b/config/costmap_config.yaml
@@ -0,0 +1,11 @@
+/**:
+ ros__parameters:
+ unknown_cost: -1
+ lethal_cost: 100
+ input_topic: /zed2i/zed_node_zed2i/mapping/fused_cloud
+ output_topic: /map
+ costmap_resolution: 0.1
+ lethal_slope: 0.3 # the slope at which a point is considered lethal
+ # Units for slope are non existant. higher = steeper
+
+
diff --git a/config/motor_control.yaml b/config/motor_control.yaml
index 173914f8..a756c74c 100644
--- a/config/motor_control.yaml
+++ b/config/motor_control.yaml
@@ -15,9 +15,9 @@
DIGGER_ACTUATORS_kP: 0.05 # kP for digger actuator position control
DIGGER_ACTUATORS_kP_coupling: 0.004 # kP for digger actuator position syncing
DIGGER_PITCH_kP: 2.5 # kP for tipping while digging
- TIPPING_SPEED_ADJUSTMENT: true # determines if pitch speed adjustment is used
+ TIPPING_SPEED_ADJUSTMENT: false # determines if pitch speed adjustment is used
DIGGER_SAFETY_ZONE: 120 # Measured in potentiometer units (0 to 1023)
CURRENT_SPIKE_THRESHOLD: 1.8 # Threshold for current spike detection (measured in Amps)
CURRENT_SPIKE_TIME: 0.2 # Time in seconds to consider it a current spike (measured in seconds)
- BUCKETS_CURRENT_SPIKE_THRESHOLD: 8.0 # Threshold for current spike detection (measured in Amps)
+ BUCKETS_CURRENT_SPIKE_THRESHOLD: 12.0 # Threshold for current spike detection (measured in Amps)
BUCKETS_CURRENT_SPIKE_TIME: 0.2 # Time in seconds to consider it a current spike (measured in seconds)
diff --git a/config/nav2_isaac_sim.yaml b/config/nav2_isaac_sim.yaml
index 2d0bdc10..15a0ec1c 100644
--- a/config/nav2_isaac_sim.yaml
+++ b/config/nav2_isaac_sim.yaml
@@ -5,22 +5,16 @@
bt_navigator:
ros__parameters:
use_sim_time: False
- global_frame: odom
+ global_frame: map
robot_base_frame: base_link
odom_topic: /odom
bt_loop_duration: 10
default_server_timeout: 20
navigators: ["navigate_to_pose", "navigate_through_poses"]
- #default_nav_to_pose_bt_xml: $(find-pkg-share nova_carter_navigation)/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml
- #default_nav_through_poses_bt_xml: $(find-pkg-share nova_carter_navigation)/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml
navigate_to_pose:
plugin: "nav2_bt_navigator/NavigateToPoseNavigator"
navigate_through_poses:
plugin: "nav2_bt_navigator/NavigateThroughPosesNavigator"
- # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
- # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
- # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
- # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
plugin_lib_names:
- nav2_compute_path_to_pose_action_bt_node
- nav2_compute_path_through_poses_action_bt_node
@@ -28,7 +22,6 @@ bt_navigator:
- nav2_follow_path_action_bt_node
- nav2_spin_action_bt_node
- nav2_wait_action_bt_node
- - nav2_assisted_teleop_action_bt_node
- nav2_back_up_action_bt_node
- nav2_drive_on_heading_bt_node
- nav2_clear_costmap_service_bt_node
@@ -54,7 +47,6 @@ bt_navigator:
- nav2_distance_traveled_condition_bt_node
- nav2_single_trigger_bt_node
- nav2_goal_updated_controller_bt_node
- - nav2_is_battery_low_condition_bt_node
- nav2_navigate_through_poses_action_bt_node
- nav2_navigate_to_pose_action_bt_node
- nav2_remove_passed_goals_action_bt_node
@@ -66,9 +58,7 @@ bt_navigator:
- nav2_wait_cancel_bt_node
- nav2_spin_cancel_bt_node
- nav2_back_up_cancel_bt_node
- - nav2_assisted_teleop_cancel_bt_node
- nav2_drive_on_heading_cancel_bt_node
- - nav2_is_battery_charging_condition_bt_node
bt_navigator_navigate_through_poses_rclcpp_node:
ros__parameters:
use_sim_time: False
@@ -83,7 +73,6 @@ controller_server:
progress_checker_plugin: "progress_checker"
goal_checker_plugins: ["stopped_goal_checker"]
controller_plugins: ["FollowPath"]
- # Progress checker parameters
progress_checker:
plugin: "nav2_controller::PoseProgressChecker"
required_movement_radius: 0.1
@@ -96,7 +85,6 @@ controller_server:
xy_goal_tolerance: 0.1
yaw_goal_tolerance: 0.1
FollowPath:
- # RotationShimController parameters
plugin: "nav2_rotation_shim_controller::RotationShimController"
primary_controller: "dwb_core::DWBLocalPlanner"
angular_dist_threshold: 0.785
@@ -104,7 +92,6 @@ controller_server:
rotate_to_heading_angular_vel: 0.75
max_angular_accel: 0.75
simulate_ahead_time: 1.0
- # Primary controller params (DWBLocalPlanner)
debug_trajectory_details: True
min_vel_x: -0.75
min_vel_y: 0.0
@@ -152,8 +139,6 @@ controller_server:
RotateToGoal.slowing_factor: 1.0
RotateToGoal.lookahead_time: -1.0
-# Update velocity smoother default params to match our controller server config
-# https://navigation.ros.org/configuration/packages/configuring-velocity-smoother.html
velocity_smoother:
ros__parameters:
use_sim_time: False
@@ -171,74 +156,43 @@ velocity_smoother:
local_costmap:
local_costmap:
ros__parameters:
- update_frequency: 10.0
- publish_frequency: 10.0
- global_frame: odom
+ global_frame: map
robot_base_frame: base_link
- use_sim_time: False
- rolling_window: True
- width: 10
- height: 10
+ update_frequency: 5.0
+ publish_frequency: 2.0
+ rolling_window: true
+ width: 3
+ height: 3
resolution: 0.05
- footprint: "[ [0.595, 0.38], [-0.595, 0.38,], [-0.595, -0.38], [0.595, -0.38] ]"
- plugins: ["nvblox_base_layer", "inflation_layer"]
- nvblox_base_layer:
- plugin: "nvblox::nav2::NvbloxCostmapLayer"
- enabled: True
- max_obstacle_distance: 1.0
- gradient_multiplier: 50.0
- # Need to play with this. this is just linear multiplier to the values found by the gradient.
- # As of right now, the formula for the cost value is
- # cost = min(max_cost, (gradient * gradient_multiplier * max_cost))
- # max cost = 255
- # gradient = abs(height(x - 1) - height(x + 1)) + abs(height(y - 1) - height(y + 1))
- nvblox_map_slice_topic: "/nvblox_node/static_map_slice"
+ plugins: ["static_layer", "inflation_layer"]
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
- enabled: True
- cost_scaling_factor: 8.0
- inflation_radius: 5.0
- always_send_full_costmap: True
- local_costmap_client:
- ros__parameters:
- use_sim_time: False
- local_costmap_rclcpp_node:
- ros__parameters:
- use_sim_time: False
+ cost_scaling_factor: 3.0
+ inflation_radius: 0.55
+ static_layer:
+ plugin: "nav2_costmap_2d::StaticLayer"
+ map_subscribe_transient_local: True
+ map_topic: /map_server/map
global_costmap:
global_costmap:
ros__parameters:
- update_frequency: 10.0
- publish_frequency: 10.0
- global_frame: odom
+ global_frame: map
robot_base_frame: base_link
- use_sim_time: False
- rolling_window: True
- width: 200
- height: 200
- footprint: "[ [0.595, 0.38], [-0.595, 0.38,], [-0.595, -0.38], [0.595, -0.38] ]"
- # resolution: 0.1
- origin_x: -100.0
- origin_y: -100.0
- plugins: ["nvblox_base_layer", "inflation_layer"]
- nvblox_base_layer:
- plugin: "nvblox::nav2::NvbloxCostmapLayer"
- enabled: True
- max_obstacle_distance: 2.0
- nvblox_map_slice_topic: "/nvblox_node/static_map_slice"
+ update_frequency: 5.0
+ publish_frequency: 2.0
+ rolling_window: false
+ resolution: 0.1
+ plugins: ["static_layer", "inflation_layer"]
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
- enabled: True
- cost_scaling_factor: 4.0
- inflation_radius: 5.0
- always_send_full_costmap: True
- global_costmap_client:
- ros__parameters:
- use_sim_time: False
- global_costmap_rclcpp_node:
- ros__parameters:
- use_sim_time: False
+ cost_scaling_factor: 3.0
+ inflation_radius: 0.55
+ static_layer:
+ plugin: "nav2_costmap_2d::StaticLayer"
+ map_subscribe_transient_local: False
+ subscribe_to_updates: True
+ map_topic: /map
planner_server:
ros__parameters:
@@ -292,11 +246,9 @@ smoother_server:
behavior_server:
ros__parameters:
- costmap_topic: local_costmap/costmap_raw
footprint_topic: local_costmap/published_footprint
cycle_frequency: 10.0
- behavior_plugins:
- ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
+ behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"]
spin:
plugin: "nav2_behaviors/Spin"
backup:
@@ -305,28 +257,15 @@ behavior_server:
plugin: "nav2_behaviors/DriveOnHeading"
wait:
plugin: "nav2_behaviors/Wait"
- assisted_teleop:
- plugin: "nav2_behaviors/AssistedTeleop"
- global_frame: odom
+ global_frame: map
robot_base_frame: base_link
transform_tolerance: 0.1
use_sim_time: False
- simulate_ahead_time: 2.0
- max_rotational_vel: 1.0
- min_rotational_vel: 0.4
- rotational_acc_lim: 3.2
+ simulate_ahead_time: 1.0
+ max_rotational_vel: 0.75
+ min_rotational_vel: -0.75
+ rotational_acc_lim: 0.75
robot_state_publisher:
ros__parameters:
use_sim_time: False
-
-waypoint_follower:
- ros__parameters:
- use_sim_time: False
- loop_rate: 20
- stop_on_failure: false
- waypoint_task_executor_plugin: "wait_at_waypoint"
- wait_at_waypoint:
- plugin: "nav2_waypoint_follower::WaitAtWaypoint"
- enabled: True
- waypoint_pause_duration: 200
diff --git a/config/rovr_control.yaml b/config/rovr_control.yaml
index 021729f7..b78d7bef 100644
--- a/config/rovr_control.yaml
+++ b/config/rovr_control.yaml
@@ -2,12 +2,13 @@
ros__parameters:
max_drive_power: 1.0 # Measured in Duty Cycle (0.0-1.0)
max_turn_power: 1.0 # Measured in Duty Cycle (0.0-1.0)
- digger_chain_power: 0.2 # Measured in Duty Cycle (0.0-1.0)
+ digger_chain_power: 0.165 # Measured in Duty Cycle (0.0-1.0)
digger_lift_manual_power_down: 0.12 # Measured in Duty Cycle (0.0-1.0)
digger_lift_manual_power_up: 0.5 # Measured in Duty Cycle (0.0-1.0)
autonomous_field_type: "cosmic" # The type of field ("cosmic", "top", "bottom", "nasa")
- lift_digging_start_position: 125.0 # Measured in potentiometer units (0 to 1023)
-
+ tilt_digging_start_position: 125.0 # Measured in potentiometer units (0 to 1023)
+ fast_screw_speed: 4000
+ slow_screw_speed: 2000
# Auto Dig cost starts at max_dig_cost, and increases to absolute_max_dig_cost
absolute_max_dig_cost: 200 # Measured as a Costmap grid value (0-255) # TODO: tune this
max_dig_cost: 100 # Measured as a Costmap grid value (0-255) # TODO: Tune this
@@ -15,4 +16,5 @@
# Locations of all dig sites to check, as [x, y] pairs
# Ros param lists must be flattened, so this is a list of all x values followed by all y values
# https://discord.com/channels/893292981094858754/896192143570403340/1345192968629387294
- all_dig_locations: [.6, .37, .6, 1.1, .6, 1.83, 1.8, .37, 1.8, 1.1, 1.8, 1.83, 3.0, .37, 3.0, 1.1, 3.0, 1.83, .6, 2.5, 1.8, 2.5, 3.0, 2.5]
+ all_dig_locations: [2.0, 2.0, .7, .7, .7, 1.4, .7, 2.1, 1.9, .7, 1.9, 1.4, 1.9, 2.1, 3.0, .7, 3.0, 1.4, 3.0, 2.1, .7, 2.8, 1.9, 2.8, 3.0, 2.8]
+ # all_dig_locations: [.6, .37, .6, 1.1, .6, 1.83, 1.8, .37, 1.8, 1.1, 1.8, 1.83, 3.0, .37, 3.0, 1.1, 3.0, 1.83, .6, 2.5, 1.8, 2.5, 3.0, 2.5]
diff --git a/config/rviz/zed_example.rviz b/config/rviz/zed_example.rviz
index e90abcf7..262ffeed 100644
--- a/config/rviz/zed_example.rviz
+++ b/config/rviz/zed_example.rviz
@@ -4,19 +4,14 @@ Panels:
Name: Displays
Property Tree Widget:
Expanded:
- - /Global Options1
- - /Status1
- - /TF1
- /TF1/Frames1
- /TF1/Tree1
- /Nvblox1
- /Nvblox1/Map Slice Bounds1
- /Nvblox1/Esdf Slice1
- - /Sensors1
- - /Plan1
- - /RobotModel1
+ - /Map2
Splitter Ratio: 0.5896806120872498
- Tree Height: 723
+ Tree Height: 1001
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
@@ -86,21 +81,29 @@ Visualization Manager:
odom:
zed2i_camera_link:
base_link:
- back_left_wheel_Link: {}
- back_right_wheel_Link: {}
- front_left_wheel_Link: {}
- front_right_wheel_Link: {}
+ back_left_wheel_Link:
+ {}
+ back_right_wheel_Link:
+ {}
+ front_left_wheel_Link:
+ {}
+ front_right_wheel_Link:
+ {}
zed2i_rear_camera_link:
zed2i_rear_camera_center:
zed2i_rear_left_camera_frame:
- zed2i_rear_left_camera_optical_frame: {}
+ zed2i_rear_left_camera_optical_frame:
+ {}
zed2i_rear_right_camera_frame:
- zed2i_rear_right_camera_optical_frame: {}
+ zed2i_rear_right_camera_optical_frame:
+ {}
zed2i_camera_center:
zed2i_left_camera_frame:
- zed2i_left_camera_optical_frame: {}
+ zed2i_left_camera_optical_frame:
+ {}
zed2i_right_camera_frame:
- zed2i_right_camera_optical_frame: {}
+ zed2i_right_camera_optical_frame:
+ {}
Update Interval: 0
Value: true
- Alpha: 0.5
@@ -140,7 +143,8 @@ Visualization Manager:
- Class: rviz_default_plugins/MarkerArray
Enabled: false
Name: Mesh Marker
- Namespaces: {}
+ Namespaces:
+ {}
Topic:
Depth: 5
Durability Policy: Volatile
@@ -151,7 +155,8 @@ Visualization Manager:
- Class: rviz_default_plugins/Marker
Enabled: false
Name: Map Slice Bounds
- Namespaces: {}
+ Namespaces:
+ {}
Topic:
Depth: 5
Durability Policy: Volatile
@@ -228,7 +233,7 @@ Visualization Manager:
Use Fixed Frame: true
Use rainbow: true
Value: true
- Enabled: true
+ Enabled: false
Name: Nvblox
- Class: rviz_common/Group
Displays:
@@ -485,7 +490,7 @@ Visualization Manager:
Visual Enabled: true
- Alpha: 0.699999988079071
Class: rviz_default_plugins/Map
- Color Scheme: map
+ Color Scheme: costmap
Draw Behind: false
Enabled: false
Name: Map
@@ -517,10 +522,65 @@ Visualization Manager:
Reliability Policy: Reliable
Value: /local_costmap/published_footprint
Value: true
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz_default_plugins/PointCloud2
+ Color: 255; 255; 255
+ Color Transformer: RGB8
+ Decay Time: 0
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Max Intensity: 4096
+ Min Color: 0; 0; 0
+ Min Intensity: 0
+ Name: PointCloud2
+ Position Transformer: XYZ
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.009999999776482582
+ Style: Flat Squares
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ Filter size: 10
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /zed2i/zed_node_zed2i/mapping/fused_cloud
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ - Alpha: 0.699999988079071
+ Class: rviz_default_plugins/Map
+ Color Scheme: map
+ Draw Behind: false
+ Enabled: true
+ Name: Map
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ Filter size: 10
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /map
+ Update Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /map_updates
+ Use Timestamp: false
+ Value: true
Enabled: true
Global Options:
Background Color: 212; 212; 212
- Fixed Frame: map
+ Fixed Frame: odom
Frame Rate: 30
Name: root
Tools:
@@ -563,25 +623,25 @@ Visualization Manager:
Views:
Current:
Class: rviz_default_plugins/Orbit
- Distance: 18.716569900512695
+ Distance: 15.352240562438965
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
- X: 5.1264262199401855
- Y: -3.5869696140289307
- Z: -2.170865774154663
+ X: 1.3153513669967651
+ Y: 0.44086113572120667
+ Z: 0.31986361742019653
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
- Pitch: 1.1947964429855347
+ Pitch: 1.1347968578338623
Target Frame: camera_link
Value: Orbit (rviz)
- Yaw: 1.8003982305526733
+ Yaw: 4.697237491607666
Saved: ~
Window Geometry:
Color Image:
@@ -590,16 +650,16 @@ Window Geometry:
collapsed: false
Displays:
collapsed: false
- Height: 1006
+ Height: 1385
Hide Left Dock: false
Hide Right Dock: true
- QMainWindow State: 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
+ QMainWindow State: 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
Selection:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
- Width: 1920
- X: 0
- Y: 726
+ Width: 2301
+ X: 798
+ Y: 101
diff --git a/config/sensors/zed_common.yaml b/config/sensors/zed_common.yaml
index 5a4ed976..15456a8a 100644
--- a/config/sensors/zed_common.yaml
+++ b/config/sensors/zed_common.yaml
@@ -64,7 +64,7 @@
pos_tracking:
pos_tracking_enabled: true # True to enable positional tracking from start
- pos_tracking_mode: "GEN_2" # Matches the ZED SDK setting: 'GEN_1', 'GEN_2'
+ pos_tracking_mode: "GEN_1" # Matches the ZED SDK setting: 'GEN_1', 'GEN_2'
imu_fusion: true # enable/disable IMU fusion. When set to false, only the optical odometry will be used.
publish_tf: true # [usually overwritten by launch file] publish `odom -> camera_link` TF
publish_map_tf: false # [usually overwritten by launch file] publish `map -> odom` TF
@@ -100,10 +100,10 @@
target_yaw_uncertainty: 1e-2 # defines the target yaw uncertainty at which the calibration process between GNSS and VIO concludes. The unit of this parameter is in radian. By default, the threshold is set at 0.1 radians.
mapping:
- mapping_enabled: false # True to enable mapping and fused point cloud publication
+ mapping_enabled: true # True to enable mapping and fused point cloud publication
resolution: 0.05 # maps resolution in meters [min: 0.01f - max: 0.2f]
max_mapping_range: 5.0 # maximum depth range while mapping in meters (-1 for automatic calculation) [2.0, 20.0]
- fused_pointcloud_freq: 1.0 # frequency of the publishing of the fused colored point cloud
+ fused_pointcloud_freq: 3.0 # frequency of the publishing of the fused colored point cloud
clicked_point_topic: "/clicked_point" # Topic published by Rviz when a point of the cloud is clicked. Used for plane detection
pd_max_distance_threshold: 0.15 # Plane detection: controls the spread of plane by checking the position difference.
pd_normal_similarity_threshold: 15.0 # Plane detection: controls the spread of plane by checking the angle difference.
diff --git a/docker/Dockerfile.deepstream b/docker/Dockerfile.deepstream
index 392406d9..44ac7782 100644
--- a/docker/Dockerfile.deepstream
+++ b/docker/Dockerfile.deepstream
@@ -1,6 +1,8 @@
ARG BASE_IMAGE
FROM ${BASE_IMAGE}
+RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
+
COPY deepstream/deepstream.deb deepstream.deb
RUN --mount=type=cache,target=/var/cache/apt apt-get update && apt-get install -y \
diff --git a/docker/Dockerfile.gazebo b/docker/Dockerfile.gazebo
index 22455481..b125a51d 100644
--- a/docker/Dockerfile.gazebo
+++ b/docker/Dockerfile.gazebo
@@ -3,6 +3,8 @@ FROM ${BASE_IMAGE}
USER root
+RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
+
RUN --mount=type=cache,target=/var/cache/apt apt-get update && apt-get install -y \
ros-humble-ros-gz \
ros-humble-sdformat-urdf \
diff --git a/docker/Dockerfile.umn b/docker/Dockerfile.umn
index a098cbfe..ff532cef 100644
--- a/docker/Dockerfile.umn
+++ b/docker/Dockerfile.umn
@@ -3,6 +3,8 @@ FROM ${BASE_IMAGE}
USER root
+RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
+
# To get list of packages currently missing run: rosdep install --simulate --from-path src --ignore-src --skip-keys "ros-humble-isaac-ros-h264-decoder ros-humble-nova-carter-navigation ros-humble-isaac-ros-dnn-image-encoder ros-humble-isaac-ros-triton ros-humble-isaac-ros-unet ros-humble-isaac-ros-peoplesemseg-models-install" | awk '{print $5 }'
RUN --mount=type=cache,target=/var/cache/apt apt-get update && apt-get install -y \
ros-humble-rclc \
@@ -38,7 +40,16 @@ RUN --mount=type=cache,target=/var/cache/apt apt-get update && apt-get install
ros-humble-ffmpeg-image-transport \
ros-humble-ffmpeg-encoder-decoder \
ros-humble-zed-msgs \
+ libudev-dev \
+ libusb-1.0-0-dev \
+ libhidapi-libusb0 \
&& rm -rf /var/lib/apt/lists/*
+RUN python3 -m pip install -U \
+ pyvista \
+ streamdeck
+
+RUN echo 'SUBSYSTEM=="usb", ATTR{idVendor}=="0fd9", MODE="0666"' | sudo tee /etc/udev/rules.d/99-streamdeck.rules
+
ARG USERNAME=admin
RUN adduser ${USERNAME} dialout && adduser ${USERNAME} tty && adduser ${USERNAME} input
diff --git a/point_cloud_data.npz b/point_cloud_data.npz
new file mode 100644
index 00000000..40f2ac91
Binary files /dev/null and b/point_cloud_data.npz differ
diff --git a/scripts/laptop.sh b/scripts/laptop.sh
index 5e26e117..f6432e78 100755
--- a/scripts/laptop.sh
+++ b/scripts/laptop.sh
@@ -1,4 +1,4 @@
#!/bin/bash
source install/setup.bash && \
-ros2 launch gstreamer laptop_launch.py
+ros2 run rqt_image_view rqt_image_view
diff --git a/src/digger/digger/__init__.py b/src/alexblox/alexblox/__init__.py
similarity index 100%
rename from src/digger/digger/__init__.py
rename to src/alexblox/alexblox/__init__.py
diff --git a/src/alexblox/alexblox/costmapGenerator.py b/src/alexblox/alexblox/costmapGenerator.py
new file mode 100644
index 00000000..121f5fc5
--- /dev/null
+++ b/src/alexblox/alexblox/costmapGenerator.py
@@ -0,0 +1,109 @@
+import rclpy
+from rclpy.node import Node
+import numpy as np
+
+from sensor_msgs.msg import PointCloud2
+from nav_msgs.msg import OccupancyGrid
+
+
+class CostmapGenerator(Node):
+ def __init__(self):
+ super().__init__("costmap_generator")
+
+ self.declare_parameter("costmap_resolution", 0.1)
+ self.resolution = float(self.get_parameter("costmap_resolution").value)
+
+ self.declare_parameter("lethal_slope", 0.3)
+ self.lethal_slope = float(self.get_parameter("lethal_slope").value)
+ # who knows what units this is in
+
+ self.declare_parameter("lethal_cost", 254)
+ self.lethal_cost = int(self.get_parameter("lethal_cost").value)
+
+ self.declare_parameter("unknown_cost", 255)
+ self.unknown_cost = int(self.get_parameter("unknown_cost").value)
+
+ self.declare_parameter("input_topic", "/zed2i/zed_node_zed2i/mapping/fused_cloud")
+ input_topic = str(self.get_parameter("input_topic").value)
+
+ self.declare_parameter("output_topic", "/costmap/costmap")
+ output_topic = str(self.get_parameter("output_topic").value)
+
+ self.pointCloudSubscription = self.create_subscription(PointCloud2, input_topic, self.pointCloudCallback, 10)
+
+ self.costmapPublisher = self.create_publisher(OccupancyGrid, output_topic, 10)
+ # self.distanceMapSubscription = self.create_subscription(
+ # DistanceMapSlice,
+ # '/nvblox_node/static_map_slice',
+ # self.distanceMapCallback,
+ # 10
+ # )
+ self.get_logger().info(f"Costmap generating on {input_topic} -> {output_topic}")
+ print(input_topic, output_topic)
+
+ def pointCloudCallback(self, msg):
+ if len(msg.data) == 0:
+ return
+ print("Received PointCloud2 message")
+ resolution = self.resolution
+
+ # UNPACK THE POINT CLOUD DATA
+ data = np.frombuffer(msg.data, dtype=np.uint8).view(dtype=np.float32)
+ points = data.reshape(-1, 4)[:, :3]
+
+ world_min_y = np.min(points[:, 1])
+ world_max_y = np.max(points[:, 1])
+ world_min_x = np.min(points[:, 0])
+ world_max_x = np.max(points[:, 0])
+ grid_x_size = int((world_max_x - world_min_x) / resolution) + 1
+ grid_y_size = int((world_max_y - world_min_y) / resolution) + 1
+
+ # ESDF PROJECTION
+ projected = np.full((grid_y_size, grid_x_size), fill_value=np.nan, dtype=np.float32)
+ projected_x_indices = ((points[:, 0] - world_min_x) / resolution).astype(np.int32)
+ projected_y_indices = ((points[:, 1] - world_min_y) / resolution).astype(np.int32)
+ sums = np.zeros_like(projected)
+ counts = np.zeros_like(projected, dtype=int)
+
+ np.add.at(sums, (projected_y_indices, projected_x_indices), points[:, 2])
+ np.add.at(counts, (projected_y_indices, projected_x_indices), 1)
+
+ average_heightmap = np.divide(sums, counts, out=np.full_like(sums, np.nan), where=counts != 0)
+ projected = average_heightmap
+
+ origin = (world_min_x, world_min_y)
+
+ # EDGE DETECTION
+ gradient = np.gradient(projected, resolution, edge_order=1)
+ magnitude = np.sqrt(gradient[0] ** 2 + gradient[1] ** 2)
+ costmap = np.full_like(magnitude, fill_value=self.unknown_cost, dtype=np.uint8)
+ costmap[magnitude <= self.lethal_slope] = 0
+ costmap[magnitude > self.lethal_slope] = self.lethal_cost
+
+ # MESSAGE PACKAGING/PUBLISHING
+ occ_grid = OccupancyGrid()
+ occ_grid.header = msg.header
+ occ_grid.info.resolution = float(resolution)
+ occ_grid.info.width = costmap.shape[1]
+ occ_grid.info.height = costmap.shape[0]
+ occ_grid.info.origin.position.x = float(origin[0])
+ occ_grid.info.origin.position.y = float(origin[1])
+ occ_grid.info.origin.position.z = 0.0
+ # For a 2D map, the orientation is typically an identity quaternion (no rotation)
+ occ_grid.info.origin.orientation.w = 1.0
+ occ_grid.info.origin.orientation.x = 0.0
+ occ_grid.info.origin.orientation.y = 0.0
+ occ_grid.info.origin.orientation.z = 0.0
+ occ_grid.data = costmap.astype(np.int8).flatten().tolist()
+
+ self.costmapPublisher.publish(occ_grid)
+
+ return
+
+
+def main(args=None):
+ rclpy.init(args=args)
+ node = CostmapGenerator()
+ rclpy.spin(node)
+ node.destroy_node()
+ rclpy.shutdown()
diff --git a/src/alexblox/alexblox/visualizer.py b/src/alexblox/alexblox/visualizer.py
new file mode 100644
index 00000000..b2e44a7d
--- /dev/null
+++ b/src/alexblox/alexblox/visualizer.py
@@ -0,0 +1,108 @@
+import rclpy
+from rclpy.node import Node
+import numpy as np
+import pyvista as pv # this isnt in the container right now,
+# either pip install each time, or rebuild the container. i added it as a dependency.
+
+from sensor_msgs.msg import PointCloud2
+from nav_msgs.msg import OccupancyGrid
+from threading import Lock
+
+
+class CostmapVisualizer(Node):
+ def __init__(self):
+ super().__init__("costmap_visualizer")
+ self.lock = Lock()
+
+ self.declare_parameter("costmap_resolution", 0.1)
+ self.resolution = float(self.get_parameter("costmap_resolution").value)
+
+ self.declare_parameter("lethal_slope", 0.3)
+ self.lethal_slope = float(self.get_parameter("lethal_slope").value)
+ # who knows what units this is in
+
+ self.declare_parameter("lethal_cost", 254)
+ self.lethal_cost = int(self.get_parameter("lethal_cost").value)
+
+ self.declare_parameter("unknown_cost", 255)
+ self.unknown_cost = int(self.get_parameter("unknown_cost").value)
+
+ self.declare_parameter("input_topic", "/zed2i/zed_node_zed2i/mapping/fused_cloud")
+ input_topic = str(self.get_parameter("input_topic").value)
+
+ self.declare_parameter("output_topic", "/costmap/costmap")
+ output_topic = str(self.get_parameter("output_topic").value)
+
+ self.pointCloudSubscription = self.create_subscription(PointCloud2, input_topic, self.pointCloudCallback, 10)
+
+ self.costmapPublisher = self.create_publisher(OccupancyGrid, output_topic, 10)
+ # self.distanceMapSubscription = self.create_subscription(
+ # DistanceMapSlice,
+ # '/nvblox_node/static_map_slice',
+ # self.distanceMapCallback,
+ # 10
+ # )
+ self.get_logger().info(f"Costmap generating on {input_topic} -> {output_topic}")
+ print(input_topic, output_topic)
+
+ def pointCloudCallback(self, msg):
+ if len(msg.data) == 0:
+ return
+
+ with self.lock:
+ print("Received PointCloud2 message")
+ resolution = self.resolution
+
+ # UNPACK THE POINT CLOUD DATA
+ data = np.frombuffer(msg.data, dtype=np.uint8).view(dtype=np.float32)
+ points = data.reshape(-1, 4)[:, :3]
+
+ world_min_y = np.min(points[:, 1])
+ world_max_y = np.max(points[:, 1])
+ world_min_x = np.min(points[:, 0])
+ world_max_x = np.max(points[:, 0])
+ grid_x_size = int((world_max_x - world_min_x) / resolution) + 1
+ grid_y_size = int((world_max_y - world_min_y) / resolution) + 1
+
+ # ESDF PROJECTION
+ projected = np.full((grid_x_size, grid_y_size), fill_value=np.nan, dtype=np.float32)
+ projected_x_indices = ((points[:, 0] - world_min_x) / resolution).astype(np.int32)
+ projected_y_indices = ((points[:, 1] - world_min_y) / resolution).astype(np.int32)
+ sums = np.zeros_like(projected)
+ counts = np.zeros_like(projected, dtype=int)
+
+ np.add.at(sums, (projected_x_indices, projected_y_indices), points[:, 2])
+ np.add.at(counts, (projected_x_indices, projected_y_indices), 1)
+
+ average_heightmap = np.divide(sums, counts, out=np.full_like(sums, np.nan), where=counts != 0)
+ projected = average_heightmap
+
+ # origin = (world_min_x, world_min_y)
+
+ # EDGE DETECTION
+ gradient = np.gradient(projected, resolution, edge_order=1)
+ magnitude = np.sqrt(gradient[0] ** 2 + gradient[1] ** 2)
+ costmap = np.full_like(magnitude, fill_value=self.unknown_cost, dtype=np.uint8)
+ costmap[magnitude <= self.lethal_slope] = 0
+ costmap[magnitude > self.lethal_slope] = self.lethal_cost
+
+ point_cloud = pv.PolyData(points)
+ plotter = pv.Plotter()
+ scalars = costmap[
+ ((points[:, 0] - world_min_x) // resolution).astype(int),
+ ((points[:, 1] - world_min_y) // resolution).astype(int),
+ ]
+ plotter.add_mesh(point_cloud, scalars=scalars, point_size=15, render_points_as_spheres=True)
+ plotter.set_background("white")
+ plotter.show()
+
+
+def main(args=None):
+ rclpy.init(args=args)
+
+ costmap_visualizer = CostmapVisualizer()
+
+ rclpy.spin(costmap_visualizer)
+
+ costmap_visualizer.destroy_node()
+ rclpy.shutdown()
diff --git a/src/alexblox/launch/alexblox_launch.py b/src/alexblox/launch/alexblox_launch.py
new file mode 100644
index 00000000..10d6ebc8
--- /dev/null
+++ b/src/alexblox/launch/alexblox_launch.py
@@ -0,0 +1,13 @@
+import launch
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+ alexblox_node = Node(
+ package='alexblox',
+ executable='costmapGenerator',
+ name='costmap',
+ parameters=['config/costmap_config.yaml']
+ )
+
+ return launch.LaunchDescription([alexblox_node])
diff --git a/src/alexblox/package.xml b/src/alexblox/package.xml
new file mode 100644
index 00000000..9d538c86
--- /dev/null
+++ b/src/alexblox/package.xml
@@ -0,0 +1,20 @@
+
+
+
+ alexblox
+ 0.0.1
+ Package for processing apriltag detections received from ISAAC ROS Apriltag.
+ Alex Berg
+ MIT License
+
+ rovr_interfaces
+
+ ament_copyright
+ ament_flake8
+ ament_pep257
+ python3-pytest
+
+
+ ament_python
+
+
diff --git a/src/digger/resource/digger b/src/alexblox/resource/alexblox
similarity index 100%
rename from src/digger/resource/digger
rename to src/alexblox/resource/alexblox
diff --git a/src/alexblox/setup.cfg b/src/alexblox/setup.cfg
new file mode 100644
index 00000000..09df28a3
--- /dev/null
+++ b/src/alexblox/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/alexblox
+[install]
+install_scripts=$base/lib/alexblox
diff --git a/src/alexblox/setup.py b/src/alexblox/setup.py
new file mode 100644
index 00000000..12556c4c
--- /dev/null
+++ b/src/alexblox/setup.py
@@ -0,0 +1,33 @@
+from setuptools import find_packages, setup
+import os
+from glob import glob
+
+package_name = 'alexblox'
+
+setup(
+ name=package_name,
+ version='0.0.0',
+ packages=find_packages(exclude=['test']),
+ data_files=[
+ ('share/ament_index/resource_index/packages',
+ ['resource/' + package_name]),
+ ('share/' + package_name, ['package.xml']),
+ (
+ os.path.join("share", package_name),
+ glob("launch/*launch.[pxy][yma]*", recursive=True),
+ ),
+ ],
+ install_requires=['setuptools'],
+ zip_safe=True,
+ maintainer='Alex Berg',
+ maintainer_email='aaberg333@gmail.com',
+ description='TODO: Package description',
+ license='MIT License',
+ tests_require=['pytest'],
+ entry_points={
+ 'console_scripts': [
+ 'costmapGenerator = alexblox.costmapGenerator:main',
+ 'visualizer = alexblox.visualizer:main',
+ ],
+ },
+)
diff --git a/src/alexblox/test/test_copyright.py b/src/alexblox/test/test_copyright.py
new file mode 100644
index 00000000..97a39196
--- /dev/null
+++ b/src/alexblox/test/test_copyright.py
@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found errors'
diff --git a/src/alexblox/test/test_flake8.py b/src/alexblox/test/test_flake8.py
new file mode 100644
index 00000000..27ee1078
--- /dev/null
+++ b/src/alexblox/test/test_flake8.py
@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+ rc, errors = main_with_errors(argv=[])
+ assert rc == 0, \
+ 'Found %d code style errors / warnings:\n' % len(errors) + \
+ '\n'.join(errors)
diff --git a/src/alexblox/test/test_pep257.py b/src/alexblox/test/test_pep257.py
new file mode 100644
index 00000000..b234a384
--- /dev/null
+++ b/src/alexblox/test/test_pep257.py
@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found code style errors / warnings'
diff --git a/src/apriltag/apriltag/apriltag_node.py b/src/apriltag/apriltag/apriltag_node.py
index 553e7ae5..a857fd36 100644
--- a/src/apriltag/apriltag/apriltag_node.py
+++ b/src/apriltag/apriltag/apriltag_node.py
@@ -100,7 +100,7 @@ def reset_callback(self, request, response):
# Publish transform if the tag is detected
def postTransform(self, tag):
if tag and (self.get_clock().now().to_msg().sec == tag.header.stamp.sec):
- self.get_logger().info(str("Resetting the map -> odom TF"))
+ self.get_logger().info("Resetting the map -> odom TF", throttle_duration_sec=5)
self.map_transform = tag
return True
return False
diff --git a/src/apriltag/setup.py b/src/apriltag/setup.py
index 231a4cb6..41537c3e 100644
--- a/src/apriltag/setup.py
+++ b/src/apriltag/setup.py
@@ -24,6 +24,6 @@
license="MIT License",
tests_require=["pytest"],
entry_points={
- "console_scripts": ["apriltag_node = apriltag.apriltag_node:main", "testingESDF = apriltag.testingESDF:main"],
+ "console_scripts": ["apriltag_node = apriltag.apriltag_node:main"],
},
)
diff --git a/src/auger/auger/__init__.py b/src/auger/auger/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/src/auger/auger/auger_node.py b/src/auger/auger/auger_node.py
new file mode 100644
index 00000000..d91f98c2
--- /dev/null
+++ b/src/auger/auger/auger_node.py
@@ -0,0 +1,583 @@
+# This ROS 2 node contains the code for the auger subsystem of the robot
+import time
+import math
+
+# Import the ROS 2 Python module
+import rclpy
+from rclpy.node import Node
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
+
+# Import ROS 2 formatted message types
+
+# Import custom ROS 2 interfaces
+from rovr_interfaces.srv import (
+ MotorCommandSet,
+ MotorCommandGet,
+ AugerSetPushMotor,
+ SetScrewMotorSpeed,
+)
+from rovr_interfaces.srv import SetExtension
+from rovr_interfaces.msg import Potentiometers
+from std_srvs.srv import Trigger
+
+
+class Auger(Node):
+ def __init__(self):
+ "Initialize the ROS 2 Auger node"
+ super().__init__("auger")
+
+ # Callback group for anything that changes motor speeds
+ # There should not be a need for a separate one because all services
+ # should eventually terminate timely
+ self.service_cb_group = MutuallyExclusiveCallbackGroup()
+
+ # TODO Define service clients here
+ self.cli_motor_set = self.create_client(MotorCommandSet, "motor/set")
+ self.cli_motor_get = self.create_client(MotorCommandGet, "motor/get")
+
+ # Define parameters here
+ self.declare_parameter("POWER_LIMIT", 1)
+ self.declare_parameter("SCREW_SPEED", 4000)
+ self.declare_parameter(
+ "MAX_SCREW_SPEED", 4_000
+ ) # in RPM for both negative and positive direction
+ self.declare_parameter("MIN_SCREW_DIG_SPEED", 2_000)
+ self.declare_parameter("MAX_SPIN_MOTOR_CURRENT", 0)
+ self.declare_parameter("push_motor_position", 0)
+ self.declare_parameter("tilt_actuator_position", 0)
+ # The error range to consider the current 0
+ self.declare_parameter("TILT_ACTUATOR_CURRENT_THRESHOLD", 0)
+ # TODO: Find real value for this
+ self.declare_parameter("MAX_PUSH_MOTOR_POSITION", 0)
+ self.declare_parameter("MIN_PUSH_MOTOR_POSITION", 0)
+ self.declare_parameter("DEFAULT_PUSH_MOTOR_SPEED", 0)
+ self.declare_parameter("MAX_PUSH_MOTOR_CURRENT", 0)
+ self.declare_parameter("PUSH_MOTOR_POS_TOLERANCE", 0)
+ # Could potentially be faster
+ self.declare_parameter("MAX_RETRACT_PUSH_MOTOR_VELOCITY", -600)
+ self.declare_parameter("MAX_EXTEND_PUSH_MOTOR_VELOCITY", 600)
+ # Since we only care if the tilt actuator is fully extened or fully
+ # retracted then we should only need to care about the speed it moves
+ # make sure to verify direction of this velocity
+ self.declare_parameter("TILT_ACTUATOR_SPEED", 0)
+ # Minimum amount the tilt actuator needs to be extened to safely extend
+ # push motor
+ self.declare_parameter("TILT_ACTUATOR_MIN_EXTENSION", 0)
+ # Minimum amount the push motor needs to be retracted to safely retract
+ # tilt actuator
+ self.declare_parameter("PUSH_MOTOR_MIN_RETRACTION", 0)
+ # TODO Get real can ids
+ self.declare_parameter("TILT_ACTUATOR_ID", 0)
+ self.declare_parameter("PUSH_MOTOR_ID", 0)
+ self.declare_parameter("SPIN_MOTOR_ID", 0)
+
+ # Local variables here
+ self.POWER_LIMIT = self.get_parameter("POWER_LIMIT").value
+ self.SCREW_SPEED = self.get_parameter("SCREW_SPEED").value
+ self.MAX_SCREW_SPEED = self.get_parameter("MAX_SCREW_SPEED").value
+ self.MIN_SCREW_DIG_SPEED = self.get_parameter("MIN_SCREW_DIG_SPEED").value
+ self.MAX_SPIN_MOTOR_CURRENT = self.get_parameter("MAX_SPIN_MOTOR_CURRENT").value
+ self.push_motor_position = self.get_parameter("push_motor_position").value
+ self.tilt_actuator_position = self.get_parameter("tilt_actuator_position").value
+ self.TILT_ACTUATOR_CURRENT_THRESHOLD = self.get_parameter(
+ "TILT_ACTUATOR_CURRENT_THRESHOLD"
+ )
+ self.MAX_PUSH_MOTOR_POSITION = self.get_parameter(
+ "MAX_PUSH_MOTOR_POSITION"
+ ).value
+ self.MIN_PUSH_MOTOR_POSITION = self.get_parameter(
+ "MIN_PUSH_MOTOR_POSITION"
+ ).value
+ self.DEFAULT_PUSH_MOTOR_SPEED = self.get_parameter(
+ "DEFAULT_PUSH_MOTOR_SPEED"
+ ).value
+ self.MAX_PUSH_MOTOR_CURRENT = self.get_parameter("MAX_PUSH_MOTOR_CURRENT").value
+ self.PUSH_MOTOR_POS_TOLERANCE = self.get_parameter(
+ "PUSH_MOTOR_POS_TOLERANCE"
+ ).value
+ self.MAX_RETRACT_PUSH_MOTOR_VELOCITY = self.get_parameter(
+ "MAX_RETRACT_PUSH_MOTOR_VELOCITY"
+ ).value
+ self.MAX_EXTEND_PUSH_MOTOR_VELOCITY = self.get_parameter(
+ "MAX_EXTEND_PUSH_MOTOR_VELOCITY"
+ ).value
+ self.TILT_ACTUATOR_SPEED = self.get_parameter("TILT_ACTUATOR_SPEED").value
+ self.TILT_ACTUATOR_MIN_EXTENSION = self.get_parameter(
+ "TILT_ACTUATOR_MIN_EXTENSION"
+ ).value
+ self.PUSH_MOTOR_MIN_RETRACTION = self.get_parameter(
+ "PUSH_MOTOR_MIN_RETRACTION"
+ ).value
+ self.TILT_ACTUATOR_ID = self.get_parameter("TILT_ACTUATOR_ID").value
+ self.PUSH_MOTOR_ID = self.get_parameter("PUSH_MOTOR_ID").value
+ self.SPIN_MOTOR_ID = self.get_parameter("SPIN_MOTOR_ID").value
+
+ # TODO Define services (methods callable from the outside) here
+ self.srv_set_tilt_extension = self.create_service(
+ SetExtension,
+ "auger/tilt_actuator/setExtension",
+ self.set_tilt_extension_callback,
+ callback_group=self.service_cb_group,
+ )
+
+ self.srv_stop_tilt = self.create_service(
+ Trigger,
+ "auger/tilt_actuator/stop",
+ self.stop_tilt_callback,
+ callback_group=self.service_cb_group,
+ )
+
+ self.srv_set_push_position = self.create_service(
+ AugerSetPushMotor,
+ "auger/push_motor/setPosition",
+ self.set_push_position_callback,
+ callback_group=self.service_cb_group,
+ )
+
+ self.srv_stop_push = self.create_service(
+ Trigger,
+ "auger/push_motor/stop",
+ self.stop_push_callback,
+ callback_group=self.service_cb_group,
+ )
+
+ self.srv_extend_push = self.create_service(
+ Trigger,
+ "auger/push_motor/extend",
+ self.extend_push_callback,
+ callback_group=self.service_cb_group,
+ )
+
+ self.srv_retract_push = self.create_service(
+ Trigger,
+ "auger/push_motor/retract",
+ self.retract_push_callback,
+ callback_group=self.service_cb_group,
+ )
+
+ self.srv_run_auger_spin = self.create_service(
+ SetScrewMotorSpeed,
+ "auger/screw/run",
+ self.run_auger_spin_velocity_callback,
+ callback_group=self.service_cb_group,
+ )
+
+ self.srv_stop_spin = self.create_service(
+ Trigger,
+ "auger/screw/stop",
+ self.stop_spin_callback,
+ callback_group=self.service_cb_group,
+ )
+
+ self.srv_extend_digger = self.create_service(
+ Trigger,
+ "auger/control/extend_digger",
+ self.extend_digger_callback,
+ callback_group=self.service_cb_group,
+ )
+
+ self.srv_retract_digger = self.create_service(
+ Trigger,
+ "auger/control/retract_digger",
+ self.retract_digger_callback,
+ callback_group=self.service_cb_group,
+ )
+
+ self.srv_retract_digger = self.create_service(
+ Trigger,
+ "auger/control/stop_all",
+ self.stop_all,
+ callback_group=self.service_cb_group,
+ )
+
+ # TODO Define subscribers here - need to subscribe to potentiometer
+ # readings?
+ self.potentiometer_sub = self.create_subscription(
+ Potentiometers, "potentiometer", self.push_motor_position_callback, 10
+ )
+
+ # TODO Define publishers here
+
+ # Define subsystem methods here
+
+ def set_actuator_tilt_extension(self, tilt: bool) -> bool:
+ """
+ Sets the auger tilt position of the actuator. True for extend, False for retract.
+ This method will spin until the actuator has hit a limit switch.
+ This will return false and do nothing if the push motor is currently extended.
+ Caller is responsible for timeouts.
+ """
+ push_motor_pos_future = self.cli_motor_get.call_async(
+ MotorCommandGet.Request(type="position", can_id=self.PUSH_MOTOR_ID)
+ )
+ rclpy.spin_until_future_complete(self, push_motor_pos_future)
+ push_motor_pos = push_motor_pos_future.result()
+ if not push_motor_pos.success:
+ self.get_logger().info(
+ "WARNING: Failed to move the tilt actuator because the push motor position could not be determined"
+ )
+ return False
+ if push_motor_pos.data > self.PUSH_MOTOR_MIN_RETRACTION:
+ self.get_logger().info(
+ "WARNING: Failed to move the tilt actuator because the push motor is extended too far"
+ )
+ return False
+
+ if tilt:
+ self.get_logger().info("Extending tilt actuator")
+ else:
+ self.get_logger().info("Retracting tilt actuator")
+
+ speed = self.TILT_ACTUATOR_SPEED * (1 if tilt else -1)
+
+ motor_set_future = self.cli_motor_set.call_async(
+ MotorCommandSet.Request(
+ type="velocity", can_id=self.TILT_ACTUATOR_ID, value=speed
+ )
+ )
+ rclpy.spin_until_future_complete(self, motor_set_future)
+ if not motor_set_future.result().success:
+ self.get_logger().info("WARNING: Failed to set tilt motor velocity")
+ return False
+
+ # gets motor current until it is 0 which means it has hit an limit
+ # switch
+ while True:
+ motor_get_future = self.cli_motor_get.call_async(
+ MotorCommandGet.Request(
+ type="current",
+ can_id=self.TILT_ACTUATOR_ID,
+ )
+ )
+ rclpy.spin_until_future_complete(self, motor_get_future)
+
+ if motor_get_future.result().success:
+ if (
+ abs(motor_get_future.result().data)
+ < self.TILT_ACTUATOR_CURRENT_THRESHOLD
+ ):
+ break
+ else:
+ self.get_logger().info("WARNING: Failed to read tilt actuator position")
+
+ time.sleep(0.1)
+
+ return True
+
+ def stop_actuator_tilt(self) -> bool:
+ """Stop the auger angular position of the auger motor."""
+ self.get_logger().info("Stopping tilt actuator")
+ motor_set_future = self.cli_motor_set.call_async(
+ MotorCommandSet.Request(
+ type="duty_cycle",
+ can_id=self.TILT_ACTUATOR_ID,
+ value=0.0,
+ )
+ )
+ rclpy.spin_until_future_complete(self, motor_set_future)
+ return motor_set_future.result().success
+
+ def set_motor_push_position(
+ self, speed: float, desired_position: float, power_limit: float
+ ) -> bool:
+ """
+ Set the target position of the motor that pushes the auger into the ground.
+ This will spin until the motor reaches given setpoint.
+ This will fail if the screw is not spinning fast enough
+ Caller is responsible for timeouts.
+ """
+ if self.tilt_actuator_position < self.TILT_ACTUATOR_MIN_EXTENSION:
+ self.get_logger().warn(
+ "WARNING: Push motor will not move because the tilt actuator is not extended"
+ )
+ return False
+
+ get_screw_speed_future = self.cli_motor_get.call_async(
+ MotorCommandGet.Request(
+ type="velocity",
+ can_id=self.SPIN_MOTOR_ID,
+ )
+ )
+ rclpy.spin_until_future_complete(self, get_screw_speed_future)
+
+ if not get_screw_speed_future.result().success:
+ self.get_logger().warn(
+ "WARNING: Push motor will not move because the screw speed could not be determined"
+ )
+ return False
+ elif get_screw_speed_future.result().data < self.MIN_SCREW_DIG_SPEED:
+ self.get_logger().warn(
+ "WARNING: Push motor will not move because the screw is not spinning fast enough"
+ )
+ return False
+
+ if (
+ desired_position > self.MAX_PUSH_MOTOR_POSITION
+ or desired_position < self.MIN_PUSH_MOTOR_POSITION
+ ):
+ self.get_logger().warn(
+ f"WARNING: Requested push motor position is out of range, clamping value; requested: {desired_position}"
+ )
+ desired_position = max(
+ self.MIN_PUSH_MOTOR_POSITION,
+ min(desired_position, self.MAX_PUSH_MOTOR_POSITION),
+ ) # clamp the value to be within range
+ self.get_logger().info(
+ "Setting auger push motor position to: " + str(desired_position)
+ )
+
+ motor_set_future = self.cli_motor_set.call_async(
+ MotorCommandSet.Request(
+ type="velocity",
+ power_limit=power_limit,
+ can_id=self.PUSH_MOTOR_ID,
+ value=float(speed),
+ )
+ )
+ rclpy.spin_until_future_complete(self, motor_set_future)
+
+ if not motor_set_future.result().success:
+ self.get_logger().warn("WARNING: Failed to set push motor voltage")
+ return False
+
+ # wait till motor reaches desired position
+ while True:
+ motor_get_pos_future = self.cli_motor_get.call_async(
+ MotorCommandGet.Request(
+ type="position",
+ can_id=self.PUSH_MOTOR_ID,
+ )
+ )
+ rclpy.spin_until_future_complete(self, motor_get_pos_future)
+
+ if motor_get_pos_future.result().success:
+ current_pos = motor_get_pos_future.result().data
+ if (speed <= 0 and current_pos <= desired_position) or (
+ speed > 0 and current_pos >= desired_position
+ ):
+ break
+ else:
+ self.get_logger().warn("WARNING: Failed to read push motor position")
+
+ time.sleep(0.1)
+
+ stop_motor_future = self.cli_motor_set.call_async(
+ MotorCommandSet.Request(
+ type="velocity",
+ power_limit=power_limit,
+ can_id=self.PUSH_MOTOR_ID,
+ value=0,
+ )
+ )
+ stop_motor_response = rclpy.spin_until_future_complete(self, stop_motor_future)
+ if not stop_motor_response.result().success:
+ self.get_logger().warn("WARNING: Failed to stop the auger screw")
+ return False
+
+ return True
+
+ def extend_motor_push(self) -> bool:
+ """
+ Extends the push motor at max extend speed, returns false
+ if it is not able to due to the tilt actuator not being fully extended.
+ """
+ return self.set_motor_push_position(
+ self.DEFAULT_PUSH_MOTOR_SPEED, self.MAX_PUSH_MOTOR_POSITION, 0.5
+ )
+
+ def retract_motor_push(self) -> bool:
+ """
+ Run the push motor at max retract speed.
+ Returns false if it is not able to due to the tilt actuator not being fully extended.
+ """
+ return self.set_motor_push_position(
+ -self.DEFAULT_PUSH_MOTOR_SPEED, self.MIN_PUSH_MOTOR_POSITION, 0.5
+ )
+
+ def stop_motor_push(self) -> bool:
+ """Stop the motor that pushes the auger into the ground."""
+ motor_set_future = self.cli_motor_set.call_async(
+ MotorCommandSet.Request(
+ type="duty_cycle",
+ can_id=self.PUSH_MOTOR_ID,
+ value=0.0,
+ )
+ )
+ rclpy.spin_until_future_complete(self, motor_set_future)
+ return motor_set_future.result().success
+
+ def run_auger_spin_velocity(self, desired_speed: float, power_limit: float) -> bool:
+ """Set the auger spin velocity of the auger motor."""
+ if abs(desired_speed) > self.MAX_SCREW_SPEED:
+ self.get_logger().info(
+ f"WARNING: Requested auger screw speed is too fast: {desired_speed}"
+ )
+ # This is the same as signum(desired_speed) * self.MAX_SCREW_SPEED
+ desired_speed = math.copysign(self.MAX_SCREW_SPEED, desired_speed)
+
+ self.get_logger().info(f"Running auger spin at velocity: {desired_speed}")
+
+ motor_set_future = self.cli_motor_set.call_async(
+ MotorCommandSet.Request(
+ type="velocity",
+ can_id=self.SPIN_MOTOR_ID,
+ value=float(desired_speed),
+ power_limit=power_limit,
+ )
+ )
+ rclpy.spin_until_future_complete(self, motor_set_future)
+ return motor_set_future.result().success
+
+ def stop_auger_spin(self) -> bool:
+ """Stop the auger motor from spinning."""
+ self.get_logger().info("Stopping auger spin")
+
+ motor_set_future = self.cli_motor_set.call_async(
+ MotorCommandSet.Request(
+ type="duty_cycle",
+ can_id=self.SPIN_MOTOR_ID,
+ value=0.0,
+ )
+ )
+ rclpy.spin_until_future_complete(self, motor_set_future)
+ return motor_set_future.result().success
+
+ def extend_digger(self) -> bool:
+ """Tilt and extend"""
+
+ tilt_success = self.set_actuator_tilt_extension(True)
+ if not tilt_success:
+ return False
+
+ spin_success = self.run_auger_spin_velocity(self.SCREW_SPEED, self.POWER_LIMIT)
+
+ if not spin_success:
+ return False
+
+ extend_success = self.extend_motor_push()
+ if not extend_success:
+ return False
+
+ return True
+
+ def retract_digger(self) -> bool:
+ """Tilt and retract"""
+
+ retract_success = self.retract_motor_push()
+ if not retract_success:
+ return False
+
+ spin_success = self.stop_auger_spin()
+ if not spin_success:
+ return False
+
+ tilt_success = self.set_actuator_tilt_extension(False)
+ if not tilt_success:
+ return False
+
+ return True
+
+ def stop_all(self) -> bool:
+ # This does not short circit but still returns false if any do
+ return (
+ self.stop_actuator_tilt() & self.stop_auger_spin() & self.stop_motor_push()
+ )
+
+ # TODO Define service callback methods here
+ def set_tilt_extension_callback(self, request, response):
+ """This service request sets position of the angular motor."""
+ response.success = self.set_actuator_tilt_extension(request.extension)
+ return response
+
+ def stop_tilt_callback(self, request, response):
+ """This service request stops the angular motor."""
+ self.stop_actuator_tilt()
+ response.success = True
+ return response
+
+ def set_push_position_callback(self, request, response):
+ """
+ This service request sets position of the motor that pushes the auger into the ground.
+ It will fail if the tilt actuator is not fully extended
+ """
+ response.success = self.set_motor_push_position(
+ request.speed, request.position, request.power_limit
+ )
+ return response
+
+ def stop_push_callback(self, request, response):
+ """This service request stops the motor that pushes the auger into the ground."""
+ response.success = self.stop_motor_push()
+ return response
+
+ def run_auger_spin_velocity_callback(self, request, response):
+ """This service request sets the turn velocity of the auger"""
+ response.success = self.run_auger_spin_velocity(
+ request.speed, request.power_limit
+ )
+ return response
+
+ def stop_spin_callback(self, request, response):
+ """This service request stops the motor that spins the auger."""
+ response.success = self.stop_auger_spin()
+ return response
+
+ def push_motor_position_callback(self, msg):
+ self.tilt_actuator_position = msg.data
+
+ def extend_push_callback(self, request, response):
+ """
+ This service requests to extend the push motor at full speed.
+ It will fail if the tilt actuator is not fully extended
+ """
+ response.success = self.extend_motor_push()
+ return response
+
+ def retract_push_callback(self, request, response):
+ """
+ This service requests to retract the push motor at full speed.
+ It will fail if the tilt actuator is not fully extended
+ """
+ response.success = self.retract_motor_push()
+ return response
+
+ def extend_digger_callback(self, request, response):
+ """This service will both tilt and extend the auger."""
+ response.success = self.extend_digger()
+ return response
+
+ def retract_digger_callback(self, request, response):
+ """This service will both tilt and retract the auger."""
+ response.success = self.retract_digger()
+ return response
+
+ def stop_all_callback(self, request, response):
+ """This Service will stop all three motors"""
+ response.success = self.stop_all()
+ return response
+
+
+def main(args=None):
+ """The main function."""
+ rclpy.init(args=args)
+
+ node = Auger()
+ executor = MultiThreadedExecutor()
+ executor.add_node(node)
+
+ node.get_logger().info("Initializing the Auger subsystem!")
+ try:
+ executor.spin()
+ # TODO this exception needed? Could this ever happen?
+ except KeyboardInterrupt:
+ node.get_logger().info("Shutting down the Auger subsystem.")
+ finally:
+ node.destroy_node()
+
+ rclpy.shutdown()
+
+
+# This code does NOT run if this file is imported as a module
+if __name__ == "__main__":
+ main()
diff --git a/src/digger/package.xml b/src/auger/package.xml
similarity index 89%
rename from src/digger/package.xml
rename to src/auger/package.xml
index ae5a1dd1..95ef8760 100644
--- a/src/digger/package.xml
+++ b/src/auger/package.xml
@@ -1,9 +1,9 @@
- digger
+ auger
0.2.0
- This package is for the digger subsystem.
+ This package is for the auger subsystem.
Anthony Brogni
MIT License
diff --git a/src/auger/resource/auger b/src/auger/resource/auger
new file mode 100644
index 00000000..e69de29b
diff --git a/src/auger/setup.cfg b/src/auger/setup.cfg
new file mode 100644
index 00000000..a04f6278
--- /dev/null
+++ b/src/auger/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/auger
+[install]
+install_scripts=$base/lib/auger
diff --git a/src/digger/setup.py b/src/auger/setup.py
similarity index 82%
rename from src/digger/setup.py
rename to src/auger/setup.py
index 37752556..e26bdd3c 100644
--- a/src/digger/setup.py
+++ b/src/auger/setup.py
@@ -2,7 +2,7 @@
from glob import glob
from setuptools import setup
-package_name = "digger"
+package_name = "auger"
setup(
name=package_name,
@@ -17,12 +17,12 @@
zip_safe=True,
maintainer="Anthony",
maintainer_email="anthonybrog@gmail.com",
- description="This package is for the digger subsystem.",
+ description="This package is for the auger subsystem.",
license="MIT License",
tests_require=["pytest"],
entry_points={
"console_scripts": [
- "digger_node = digger.digger_node:main",
+ "auger_node = auger.auger_node:main",
],
},
)
diff --git a/src/digger/test/test_copyright.py b/src/auger/test/test_copyright.py
similarity index 100%
rename from src/digger/test/test_copyright.py
rename to src/auger/test/test_copyright.py
diff --git a/src/digger/test/test_flake8.py b/src/auger/test/test_flake8.py
similarity index 94%
rename from src/digger/test/test_flake8.py
rename to src/auger/test/test_flake8.py
index 49c1644f..ee79f31a 100644
--- a/src/digger/test/test_flake8.py
+++ b/src/auger/test/test_flake8.py
@@ -20,4 +20,6 @@
@pytest.mark.linter
def test_flake8():
rc, errors = main_with_errors(argv=[])
- assert rc == 0, "Found %d code style errors / warnings:\n" % len(errors) + "\n".join(errors)
+ assert rc == 0, "Found %d code style errors / warnings:\n" % len(
+ errors
+ ) + "\n".join(errors)
diff --git a/src/digger/test/test_pep257.py b/src/auger/test/test_pep257.py
similarity index 100%
rename from src/digger/test/test_pep257.py
rename to src/auger/test/test_pep257.py
diff --git a/src/behaviortree b/src/behaviortree
new file mode 160000
index 00000000..6c6aa078
--- /dev/null
+++ b/src/behaviortree
@@ -0,0 +1 @@
+Subproject commit 6c6aa078ee7bc52fec98984bed4964556abf5beb
diff --git a/src/behaviortree_cpp b/src/behaviortree_cpp
new file mode 160000
index 00000000..14589e58
--- /dev/null
+++ b/src/behaviortree_cpp
@@ -0,0 +1 @@
+Subproject commit 14589e589c5049fbb7f1fb30ccaa3efc66c84f18
diff --git a/src/behaviortree_executor/CMakeLists.txt b/src/behaviortree_executor/CMakeLists.txt
new file mode 100644
index 00000000..ca780f58
--- /dev/null
+++ b/src/behaviortree_executor/CMakeLists.txt
@@ -0,0 +1,48 @@
+cmake_minimum_required(VERSION 3.8)
+project(behaviortree_executor)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(behaviortree_cpp REQUIRED)
+find_package(behaviortree_ros2 REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rovr_interfaces REQUIRED)
+find_package(std_srvs REQUIRED)
+find_package(action_msgs REQUIRED)
+
+add_executable(behaviortree_executor src/behaviortree_executor.cpp)
+ament_target_dependencies(behaviortree_executor rclcpp std_srvs behaviortree_ros2 behaviortree_cpp rovr_interfaces)
+
+install(DIRECTORY
+ behavior_trees
+ config
+ launch
+ DESTINATION share/${PROJECT_NAME}/
+)
+
+install(TARGETS
+ behaviortree_executor
+ DESTINATION lib/${PROJECT_NAME}
+)
+
+target_include_directories(behaviortree_executor PUBLIC
+ ${CMAKE_CURRENT_SOURCE_DIR}/include
+ ${CMAKE_CURRENT_SOURCE_DIR}/src/action)
+
+if(BUILD_TESTING)
+ find_package(ament_lint_auto REQUIRED)
+ # the following line skips the linter which checks for copyrights
+ # comment the line when a copyright and license is added to all source files
+ set(ament_cmake_copyright_FOUND TRUE)
+ # the following line skips cpplint (only works in a git repo)
+ # comment the line when this package is in a git repo and when
+ # a copyright and license is added to all source files
+ set(ament_cmake_cpplint_FOUND TRUE)
+ ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
diff --git a/src/behaviortree_executor/behavior_trees/main_tree.btproj b/src/behaviortree_executor/behavior_trees/main_tree.btproj
new file mode 100644
index 00000000..8ce5793c
--- /dev/null
+++ b/src/behaviortree_executor/behavior_trees/main_tree.btproj
@@ -0,0 +1,311 @@
+
+
+
+
+
+
+ Error code
+ Error codes to check, user defined
+
+
+ Allowed time for spinning
+ If true recovery count will be incremented
+ Service name
+ Server timeout
+ Assisted teleop error code
+
+
+ Name of the auto dig action server
+ Distance to backup after digging
+
+
+ Name of the auto offload action server
+
+
+ Distance to backup
+ Speed at which to backup
+ Allowed time for reversing
+ Server name
+ Server timeout
+ Disable collision checking
+ "Back up error code"
+
+
+ Name of the calibrate field coordinates action server
+
+
+ Name of the return to coordinate action server
+ X coordinate to return to
+ Y coordinate to return to
+
+
+ Name of the ROS2 action server to cancel all goals on
+
+
+ Server name to cancel the assisted teleop behavior
+ Server timeout
+
+
+ Server name to cancel the backup behavior
+ Server timeout
+
+
+ Server name to cancel the controller server
+ Server timeout
+
+
+ Service name to cancel the drive on heading behavior
+ Server timeout
+
+
+ Server name to cancel the spin behavior
+ Server timeout
+
+
+ Server name to cancel the wait behavior
+ Server timeout
+
+
+ Service name
+ Server timeout
+ Distance from the robot under which obstacles are cleared
+
+
+ Service name
+ Server timeout
+ Distance from the robot above which obstacles are cleared
+
+
+ Service name
+ Server timeout
+
+
+ Destinations to plan through
+ Start pose of the path if overriding current robot pose
+ Server name
+ Server timeout
+ Mapped name to the planner plugin type to use
+ Path created by ComputePathToPose node
+ "Compute path through poses error code"
+
+
+ Destination to plan to
+ Start pose of the path if overriding current robot pose
+ Mapped name to the planner plugin type to use
+ Server name
+ Server timeout
+ Path created by ComputePathToPose node
+ "Compute path to pose error code"
+
+
+ Name of the topic to receive controller selection commands
+ Default controller of the controller selector
+ Name of the selected controller received from the topic subcription
+
+
+ Distance
+ Reference frame
+ Robot base frame
+
+
+ Distance to check if passed
+ reference frame to check in
+ Robot frame to check relative to global_frame
+
+
+ Distance to travel
+ Speed at which to travel
+ Allowed time for reversing
+ Server name
+ Server timeout
+ Disable collision checking
+ "Drive on heading error code"
+
+
+
+ Path to follow
+ Goal checker
+ Progress checker
+ Service name
+ Server timeout
+ Follow Path error code
+
+
+ Vector of navigation goals
+ Navigation goal
+
+
+ Name of the go to dig location action server
+ X coordinate of the dig location
+ Y coordinate of the dig location
+
+
+ Name of the topic to receive goal checker selection commands
+ Default goal checker of the controller selector
+ Name of the selected goal checker received from the topic subcription
+
+
+ Destination
+ Reference frame
+ Robot base frame
+
+
+ Vector of navigation goals
+ Navigation goal
+
+
+ Vector of navigation goals
+ Navigation goal
+
+
+ Original goal in
+ Output goal set by subscription
+
+
+ SUCCESS if initial_pose_received true
+
+
+ Topic for battery info
+
+
+ Min battery % or voltage before triggering
+ Topic for battery info
+ Bool if check based on voltage or total %
+
+
+ Path to validate
+ Server timeout
+
+
+ Velocity threshold below which robot is considered stopped
+ Duration (ms) the velocity must remain below the threshold
+
+
+
+ Goals
+ Server name
+ Server timeout
+ Behavior tree to run
+ Navigate through poses error code
+
+
+ Goal
+ Server name
+ Server timeout
+ Behavior tree to run
+ Navigate to pose error code
+
+
+ Time to check if expired
+ Check if path has been updated to enable timer reset
+
+
+ Planned Path
+ Proximity length (m) for the path to be longer on approach
+ Length multiplication factor to check if the path is significantly longer
+
+
+
+ Name of the topic to receive planner selection commands
+ Default planner of the planner selector
+ Name of the selected planner received from the topic subcription
+
+
+ Name of the topic to receive progress checker selection commands
+ Default progress checker of the controller selector
+ Name of the selected progress checker received from the topic subcription
+
+
+ Rate
+
+
+ Number of retries
+
+
+ Service name
+ Server timeout
+
+
+ Costmap service name responsible for getting the cost
+ A vector of goals to check if in collision
+ Whether to use the footprint cost or the point cost.
+ The cost threshold above which a waypoint is considered in collision and should be removed.
+ A vector of goals containing only those that are not in collision.
+
+
+ Input goals to remove if passed
+ Radius tolerance on a goal to consider it passed
+ Global frame
+ Robot base frame
+ Set of goals after removing any passed
+
+
+
+
+
+ Path to be smoothed
+ Maximum smoothing duration
+ Bool if collision check should be performed
+ Smoothed path
+ Smoothing duration
+ True if smoothing was not interrupted by time limit
+
+
+ Name of the topic to receive smoother selection commands
+ Default smoother of the smoother selector
+ Name of the selected smoother received from the topic subcription
+
+
+ Minimum rate
+ Maximum rate
+ Minimum speed
+ Maximum speed
+ Vector of navigation goals
+ Navigation goal
+
+
+ Spin distance
+ Allowed time for spinning
+ Server name
+ Server timeout
+ Disable collision checking
+ Spin error code
+
+
+ Time to check if expired
+
+
+ Child frame for transform
+ Parent frame for transform
+
+
+ Distance before goal to truncate
+ Path to truncate
+ Truncated path to utilize
+
+
+ Distance in forward direction
+ Distance in backward direction
+ Robot base frame id
+ Transform lookup tolerance
+ Manually specified pose to be used if overriding current robot pose
+ Weight of angular distance relative to positional distance when finding which path pose is closest to robot. Not applicable on paths without orientations assigned
+ Maximum forward integrated distance along the path (starting from the last detected pose) to bound the search for the closest pose to the robot. When set to infinity (default), whole path is searched every time
+ Truncated path to utilize
+
+
+ Wait time
+ Server name
+ Server timeout
+
+
+ Error code
+
+
+ Error code
+
+
+ Error code
+
+
+
diff --git a/src/behaviortree_executor/behavior_trees/main_tree.xml b/src/behaviortree_executor/behavior_trees/main_tree.xml
new file mode 100644
index 00000000..5b011ccf
--- /dev/null
+++ b/src/behaviortree_executor/behavior_trees/main_tree.xml
@@ -0,0 +1,64 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Name of the ROS2 action server to cancel all goals on
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/behaviortree_executor/config/bt_executor.yaml b/src/behaviortree_executor/config/bt_executor.yaml
new file mode 100644
index 00000000..c74bc327
--- /dev/null
+++ b/src/behaviortree_executor/config/bt_executor.yaml
@@ -0,0 +1,9 @@
+bt_action_server:
+ ros__parameters:
+ action_name: "bt_action_server" # Optional (defaults to `bt_action_server`)
+ tick_frequency: 100 # Optional (defaults to 100 Hz)
+ groot2_port: 1667 # Optional (defaults to 1667)
+ ros_plugins_timeout: 1000 # Optional (defaults 1000 ms)
+
+ behavior_trees:
+ - behaviortree_executor/behavior_trees
diff --git a/src/behaviortree_executor/launch/behavior_tree_launch.py b/src/behaviortree_executor/launch/behavior_tree_launch.py
new file mode 100644
index 00000000..3a9e01a6
--- /dev/null
+++ b/src/behaviortree_executor/launch/behavior_tree_launch.py
@@ -0,0 +1,68 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+ ld = LaunchDescription()
+
+ auto_dig_server = Node(
+ package="rovr_control",
+ executable="auto_dig_server",
+ name="auto_dig_server",
+ )
+ auto_offload_server = Node(
+ package="rovr_control",
+ executable="auto_offload_server",
+ name="auto_offload_server",
+ )
+
+ calibrate_field_coordinate_server = Node(
+ package="rovr_control",
+ executable="calibrate_field_coordinate_server",
+ name="calibrate_field_coordinate_server",
+ )
+
+ dig_location_server = Node(
+ package="rovr_control",
+ executable="dig_location_server",
+ name="dig_location_server",
+ )
+
+ coord_return_server = Node(
+ package="rovr_control",
+ executable="coord_return_server",
+ name="coord_return_server",
+ )
+
+ cancel_action_server = Node(
+ package="rovr_control",
+ executable="cancel_action_server",
+ name="cancel_action_server",
+ )
+
+ behaviortree_executor = Node(
+ package="behaviortree_executor",
+ executable="behaviortree_executor",
+ name="bt_action_server",
+ output="screen",
+ parameters=[
+ PathJoinSubstitution([
+ FindPackageShare('behaviortree_executor'),
+ 'config',
+ 'bt_executor.yaml',
+ ]),
+ ],
+ emulate_tty=True,
+ )
+
+ ld.add_action(auto_dig_server)
+ ld.add_action(auto_offload_server)
+ ld.add_action(calibrate_field_coordinate_server)
+ ld.add_action(dig_location_server)
+ ld.add_action(coord_return_server)
+ ld.add_action(cancel_action_server)
+ ld.add_action(behaviortree_executor)
+
+ return ld
diff --git a/src/behaviortree_executor/package.xml b/src/behaviortree_executor/package.xml
new file mode 100644
index 00000000..fc4dca65
--- /dev/null
+++ b/src/behaviortree_executor/package.xml
@@ -0,0 +1,38 @@
+
+
+
+ behaviortree_executor
+ 0.1.0
+ This is the behavior tree of the robot
+ Adam Yaj
+ MIT License
+
+ ament_cmake
+
+ ament_lint_auto
+ ament_lint_common
+
+ rclpy
+ std_msgs
+ geometry_msgs
+ sensor_msgs
+ tf2_msgs
+ rovr_interfaces
+ action_msgs
+
+ rclpy
+ std_msgs
+ geometry_msgs
+ sensor_msgs
+ tf2_msgs
+ ros2launch
+ nav2_simple_commander
+ std_srvs
+ behaviortree_cpp
+ behaviortree_ros2
+ action_msgs
+
+
+ ament_cmake
+
+
diff --git a/src/behaviortree_executor/src/action/auto_dig.hpp b/src/behaviortree_executor/src/action/auto_dig.hpp
new file mode 100644
index 00000000..03a61da4
--- /dev/null
+++ b/src/behaviortree_executor/src/action/auto_dig.hpp
@@ -0,0 +1,52 @@
+#include "rovr_interfaces/action/auto_dig.hpp"
+
+#include "behaviortree_cpp/tree_node.h"
+#include "behaviortree_ros2/bt_action_node.hpp"
+#include "behaviortree_ros2/ros_node_params.hpp"
+
+using AutoDig = rovr_interfaces::action::AutoDig;
+using namespace BT;
+
+class AutoDigAction : public RosActionNode
+{
+public:
+ static BT::PortsList providedPorts()
+{
+ return {
+ BT::InputPort("action_name"),
+ BT::InputPort("backup_distance")
+ };
+}
+ AutoDigAction(const std::string &name, const BT::NodeConfig &conf,
+ const BT::RosNodeParams ¶ms)
+ : RosActionNode(name, conf, params)
+ {
+ }
+
+ bool setGoal(Goal &goal) override
+ {
+ // get inputs from the Input port
+ bool backup_distance_success = getInput("backup_distance", goal.backup_distance);
+ // return true, if we were able to set the goal correctly.
+ return backup_distance_success;
+ }
+
+ NodeStatus onResultReceived(const WrappedResult &result) override
+ {
+ switch(result.code)
+ {
+ case rclcpp_action::ResultCode::SUCCEDED
+ // The action server completed the dig successfully
+ return NodeStatus::SUCCESS;
+ case rclcpp_action::ResultCode::ABORTED
+ // Something went wrong (eg the dig got stuck or a sensor failed)
+ return NodeStatus::Failure;
+ case rclcpp_action::ResultCode::CANCELED:
+ // The action was canceled
+ return NodeStatus::CANCELED;
+ default:
+ // Any other weirdness should generally be a failure
+ return NodeStatus::FAILURE;
+ }
+ }
+};
\ No newline at end of file
diff --git a/src/behaviortree_executor/src/action/auto_dig_nav_offload.hpp b/src/behaviortree_executor/src/action/auto_dig_nav_offload.hpp
new file mode 100644
index 00000000..3e4d4831
--- /dev/null
+++ b/src/behaviortree_executor/src/action/auto_dig_nav_offload.hpp
@@ -0,0 +1,53 @@
+#include"rovr_interfaces/action/auto_dig_nav_offload.hpp"
+
+#include "behaviortree_cpp/tree_node.h"
+#include "behaviortree_ros2/bt_action_node.hpp"
+#include "behaviortree_ros2/ros_node_params.hpp"
+// actual file is in behaviortree_ros2 folder
+using namespace BT;
+
+class AutoDigNavOffLoadAction : public RosAction
+{
+ public:
+ static BT::PortsList providedPorts()
+ {
+ return
+ {
+ BT::InputPort("action_name")
+ };
+ }
+
+ AutoDigNavOffLoadAction(const std::string &name, const BT::NodeConfiguration &config,
+ const BT::RosNodeParams ¶ms) : RosActionNode(name, config, params)
+ {
+ }
+
+ bool setGoal(Goal &goal) override
+ {
+ bool target_x_success = getInput("target_x", goal.target_x);
+ bool target_y_success = getInput("target_y", goal.target_y);
+ return target_x_success && target_y_success;
+ }
+
+ NodeStatus onResultReceived(const WrappedResult & result) override
+ {
+ switch(result.code)
+ {
+ case rclcpp_action::ResultCode::SUCCEEDED:
+ // The action server completed the dig successfully
+ return NodeStatus::SUCCESS;
+ case rclcpp_action::ResultCode::ABORTED:
+ // Something went wrong (eg the dig got stuck or a sensor failed)
+ RCLCPP_ERROR(node_->get_logger(), "AutoDigNavOffLoad aborted!");
+ return NodeStatus::FAILURE;
+ case rclcpp_action::ResultCode::CANCELED:
+ // The action was canceled
+ RCLCPP_WARN(node_->get_logger(), "AutoDigNavOffLoad canceled.");
+ return NodeStatus::FAILURE;
+ default:
+ // Any other weirdness should generally be a failure
+ return NodeStatus::FAILURE;
+ }
+ return NodeStatus::SUCCESS;
+ }
+}
\ No newline at end of file
diff --git a/src/behaviortree_executor/src/action/auto_offload.hpp b/src/behaviortree_executor/src/action/auto_offload.hpp
new file mode 100644
index 00000000..5d6f1f76
--- /dev/null
+++ b/src/behaviortree_executor/src/action/auto_offload.hpp
@@ -0,0 +1,57 @@
+// Someone please double check this is correct
+#include "rovr_interfaces/action/auto_offload.hpp"
+
+#include "behaviortree_cpp/tree_node.h"
+#include "behaviortree_ros2/bt_action_node.hpp"
+#include "behaviortree_ros2/ros_node_params.hpp"
+
+using AutoOffload = rovr_interfaces::action::AutoOffload;
+using namespace BT;
+
+class AutoOffloadAction : public RosActionNode
+{
+public:
+static BT::PortsList providedPorts()
+{
+ return {
+ BT::InputPort("action_name")
+ };
+}
+ AutoOffloadAction(const std::string &name, const BT::NodeConfig &conf,
+ const BT::RosNodeParams ¶ms)
+
+ : RosActionNode(name, conf, params)
+ {
+ }
+
+ bool setGoal(Goal &goal) override
+ {
+ bool lift_dumping_position_succss = getInput("lift_dumping_position",
+ goal.lift_dumping_position);
+
+ // return true, if we were able to set the goal correctly.
+ return lift_dumping_position_success;
+
+ // Added a switch statement to handle different result codes from the action server
+ NodeStatus onResultReceived(const WrappedResult &result) override
+ {
+ switch (result.code)
+ {
+ case rclcpp_action::ResultCode::SUCCEEDED:
+ // The action server completed the offload successfully
+ return NodeStatus::SUCCESS;
+
+ case rclcpp_action::ResultCode::ABORTED:
+ // Something went wrong (eg the lift jammed or a sensor failed)
+ return NodeStatus::FAILURE;
+
+ case rclcpp_action::ResultCode::CANCELED:
+ // The action was canceled
+ return NodeStatus::FAILURE;
+
+ default:
+ // Any other weirdness should generally be a failure
+ return NodeStatus::FAILURE;
+ }
+ }
+};
\ No newline at end of file
diff --git a/src/behaviortree_executor/src/action/calibrate_field_coordinates.hpp b/src/behaviortree_executor/src/action/calibrate_field_coordinates.hpp
new file mode 100644
index 00000000..57adcb0f
--- /dev/null
+++ b/src/behaviortree_executor/src/action/calibrate_field_coordinates.hpp
@@ -0,0 +1,52 @@
+#include "rovr_interfaces/action/calibrate_field_coordinates.hpp"
+
+#include "behaviortree_cpp/tree_node.h"
+#include "behaviortree_ros2/bt_action_node.hpp"
+#include "behaviortree_ros2/ros_node_params.hpp"
+
+using CalibrateFieldCoordinates = rovr_interfaces::action::CalibrateFieldCoordinates;
+using namespace BT;
+
+class CalibrateFieldCoordinateAction : public RosActionNode
+{
+public:
+ static BT::PortsList providedPorts()
+ {
+ return
+ {
+ BT::InputPort("action_name")
+ };
+ }
+ CalibrateFieldCoordinateAction(const std::string &name, const BT::NodeConfig &conf,
+ const BT::RosNodeParams ¶ms)
+ : RosActionNode(name, conf, params)
+ {
+ }
+
+ bool setGoal(dGoal &goal) override
+ {
+ return true;
+ }
+
+ NodeStatus onResultReceived(const WrappedResult &result) override
+ {
+ //result is gotten from goal_handler_->get_result() in bt_action_node.hpp.
+ //It contains the result code and the result message from the action server
+
+ switch(result.code)
+ {
+ case rclcpp_action::ResultCode::SUCCEEDED:
+ // The action server completed the calibrated field coordinates successfully
+ return NodeStatus::SUCCESS;
+ case rclcpp_action::ResultCode::ABORTED:
+ // Something went wrong (eg the calibration failed or a sensor failed)
+ return NodeStatus::FAILURE;
+ case rclcpp_action::ResultCode::CANCELED:
+ // The action was canceled
+ return NodeStatus::CANCELED;
+ default:
+ // Any other weirdness should generally be a failure
+ return NodeStatus::FAILURE;
+ }
+ }
+};
\ No newline at end of file
diff --git a/src/behaviortree_executor/src/action/cancel_action.hpp b/src/behaviortree_executor/src/action/cancel_action.hpp
new file mode 100644
index 00000000..563f6811
--- /dev/null
+++ b/src/behaviortree_executor/src/action/cancel_action.hpp
@@ -0,0 +1,81 @@
+#include "behaviortree_cpp/action_node.h"
+#include "rclcpp/rclcpp.hpp"
+#include "action_msgs/srv/cancel_goal.hpp"
+
+using namespace BT;
+
+class CancelActionNode : public BT::SyncActionNode
+{
+public:
+ static BT::PortsList providedPorts()
+ {
+ return {
+ BT::InputPort("action_name"),
+ };
+ }
+
+ CancelActionNode(const std::string &name, const BT::NodeConfig &conf,
+ rclcpp::Node::SharedPtr node)
+ : BT::SyncActionNode(name, conf), node_(node)
+ {
+ }
+
+ BT::NodeStatus tick() override
+ {
+ std::string action_name;
+ if (!getInput("action_name", action_name))
+ {
+ RCLCPP_ERROR(node_->get_logger(), "CancelActionNode: missing required input 'action_name'");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ // The ROS2 action cancel service is always at //_action/cancel_goal
+ std::string service_name = action_name + "/_action/cancel_goal";
+
+ auto client = node_->create_client(service_name);
+
+ if (!client->wait_for_service(std::chrono::seconds(3)))
+ {
+ RCLCPP_ERROR(node_->get_logger(),
+ "CancelActionNode: cancel service '%s' not available",
+ service_name.c_str());
+ return BT::NodeStatus::FAILURE;
+ }
+
+ auto request = std::make_shared();
+
+ auto future = client->async_send_request(request);
+ auto deadline = node_->now() + rclcpp::Duration::from_seconds(3.0);
+ while (future.wait_for(std::chrono::milliseconds(10)) != std::future_status::ready)
+ {
+ if (node_->now() >= deadline)
+ {
+ RCLCPP_ERROR(node_->get_logger(),
+ "CancelActionNode: timed out waiting for cancel response on '%s'",
+ service_name.c_str());
+ return BT::NodeStatus::FAILURE;
+ }
+ }
+
+ auto response = future.get();
+ if (response->return_code == action_msgs::srv::CancelGoal::Response::ERROR_NONE)
+ {
+ RCLCPP_INFO(node_->get_logger(),
+ "CancelActionNode: successfully canceled all goals on '%s'",
+ action_name.c_str());
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ {
+ RCLCPP_WARN(node_->get_logger(),
+ "CancelActionNode: cancel returned code %d on '%s'",
+ response->return_code, action_name.c_str());
+ return (response->return_code == action_msgs::srv::CancelGoal::Response::ERROR_REJECTED)
+ ? BT::NodeStatus::SUCCESS
+ : BT::NodeStatus::FAILURE;
+ }
+ }
+
+private:
+ rclcpp::Node::SharedPtr node_;
+};
\ No newline at end of file
diff --git a/src/behaviortree_executor/src/action/coord_return.hpp b/src/behaviortree_executor/src/action/coord_return.hpp
new file mode 100644
index 00000000..68584c53
--- /dev/null
+++ b/src/behaviortree_executor/src/action/coord_return.hpp
@@ -0,0 +1,57 @@
+#include "rovr_interfaces/action/coord_return.hpp"
+
+#include "behaviortree_cpp/tree_node.h"
+#include "behaviortree_ros2/bt_action_node.hpp"
+#include "behaviortree_ros2/ros_node_params.hpp"
+
+using CoordReturn = rovr_interfaces::action::ReturnToCoordinate;
+using namespace BT;
+
+class CoordReturnAction : public RosActionNode
+{
+ public:
+ static BT::PortsList providedPorts()
+ {
+ return
+ {
+ BT::InputPort("action_name"),
+ BT::InputPort("x_pos"),
+ BT::InputPort("y_pos")
+ };
+ }
+
+ CoordReturnAction(const std::string &name, const BT::NodeConfig &conf, const BT::RosNodeParams ¶ms)
+ : RosActionNode(name, conf, params)
+ {
+ // Initialize any member variables or state here
+ }
+
+ // attribute__((unused)) is used to suppress compiler warnings about unused parameters
+ bool setGoal(Goal &goal) override
+ {
+ bool x_pos_success = getInput("x_pos", goal.x_pos);
+ bool y_pos_success = getInput("y_pos", goal.y_pos);
+ return x_pos_success && y_pos_success;
+ }
+
+ NodeStatus onResultReceived(const WrappedResult &result) override
+ {
+ switch(result.code)
+ {
+ case rclcpp_action::ResultCode::SUCCEEDED:
+ // The action server completed the return to coordinate successfully
+ return NodeStatus::SUCCESS;
+ case rclcpp_action::ResultCode::ABORTED:
+ // Something went wrong (eg the robot got stuck or a sensor failed)
+ return NodeStatus::FAILURE;
+ case rclcpp_action::ResultCode::CANCELED:
+ // The action was canceled
+ return NodeStatus::CANCELED;
+ default:
+ // Any other weirdness should generally be a failure
+ return NodeStatus::FAILURE;
+ }
+ }
+
+};
+
diff --git a/src/behaviortree_executor/src/action/go_to_dig_location.hpp b/src/behaviortree_executor/src/action/go_to_dig_location.hpp
new file mode 100644
index 00000000..6b79d40a
--- /dev/null
+++ b/src/behaviortree_executor/src/action/go_to_dig_location.hpp
@@ -0,0 +1,53 @@
+#include "rovr_interfaces/action/go_to_dig_location.hpp"
+
+#include "behaviortree_cpp/tree_node.h"
+#include "behaviortree_ros2/bt_action_node.hpp"
+#include "behaviortree_ros2/ros_node_params.hpp"
+
+using GoToDigLocation = rovr_interfaces::action::GoToDigLocation;
+using namespace BT;
+
+class GoToDigLocationAction : public RosActionNode
+{
+public:
+ static BT::PortsList providedPorts()
+ {
+ return
+ {
+ BT::InputPort("action_name"),
+ BT::InputPort("x"),
+ BT::InputPort("y")
+ };
+ }
+ GoToDigLocationAction(const std::string& name, const BT::NodeConfig& conf,
+ const BT::RosNodeParams& params)
+ : RosActionNode(name, conf, params)
+ {
+ }
+
+ bool setGoal(Goal &goal) override
+ {
+ bool x_success = getInput("x", goal.x);
+ bool y_success = getInput("y", goal.y);
+ return x_success && y_success;
+ }
+
+ NodeStatus onResultReceived(const WrappedResult &result) override
+ {
+ switch(result.code)
+ {
+ case rclcpp_action::ResultCode::SUCCEEDED:
+ // The action server completed the go to dig location successfully
+ return NodeStatus::SUCCESS;
+ case rclcpp_action::ResultCode::ABORTED:
+ // Something went wrong (eg the robot got stuck or a sensor failed)
+ return NodeStatus::FAILURE;
+ case rclcpp_action::ResultCode::CANCELED:
+ // The action was canceled
+ return NodeStatus::CANCELED;
+ default:
+ // Any other weirdness should generally be a failure
+ return NodeStatus::FAILURE;
+ }
+ }
+};
diff --git a/src/behaviortree_executor/src/behaviortree_executor.cpp b/src/behaviortree_executor/src/behaviortree_executor.cpp
new file mode 100644
index 00000000..520216ed
--- /dev/null
+++ b/src/behaviortree_executor/src/behaviortree_executor.cpp
@@ -0,0 +1,52 @@
+#include "rclcpp/rclcpp.hpp"
+
+#include "action/calibrate_field_coordinates.hpp"
+#include "action/auto_dig.hpp"
+#include "action/auto_offload.hpp"
+#include "action/go_to_dig_location.hpp"
+#include "action/cancel_action.hpp"
+#include "action/coord_return.hpp"
+#include "behaviortree_cpp/bt_factory.h"
+#include "behaviortree_ros2/tree_execution_server.hpp"
+
+
+#include
+#include
+#include
+
+#include "std_srvs/srv/trigger.hpp"
+
+class MyActionServer : public TreeExecutionServer
+{
+public:
+ MyActionServer(const rclcpp::NodeOptions &options) : TreeExecutionServer(options)
+ {
+ }
+ void registerNodesIntoFactory(BT::BehaviorTreeFactory &factory)
+ {
+ factory.registerNodeType("AutoOffload", this->node());
+ factory.registerNodeType("GoToDigLocation", this->node());
+ factory.registerNodeType("AutoDig", this->node());
+ factory.registerNodeType("CalibrateFieldCoordinates", this->node());
+ factory.registerNodeType("CancelAction", this->node());
+ factory.registerNodeType("ReturnToCoordinate", this->node());
+ }
+};
+
+int main(int argc, char **argv)
+{
+ rclcpp::init(argc, argv);
+
+ rclcpp::NodeOptions options;
+ auto action_server = std::make_shared(options);
+
+ // TODO: This workaround is for a bug in MultiThreadedExecutor where it can deadlock when spinning without a timeout.
+ // Deadlock is caused when Publishers or Subscribers are dynamically removed as the node is spinning.
+ rclcpp::executors::MultiThreadedExecutor exec(rclcpp::ExecutorOptions(), 0, false,
+ std::chrono::milliseconds(250));
+ exec.add_node(action_server->node());
+ exec.spin();
+ exec.remove_node(action_server->node());
+ rclcpp::shutdown();
+ return 0;
+}
\ No newline at end of file
diff --git a/src/behaviortree_ros2 b/src/behaviortree_ros2
new file mode 160000
index 00000000..cc31ea7b
--- /dev/null
+++ b/src/behaviortree_ros2
@@ -0,0 +1 @@
+Subproject commit cc31ea7b97947f1aac6e8c37df6cec379c84a7d9
diff --git a/src/digger/digger/digger_node.py b/src/digger/digger/digger_node.py
deleted file mode 100644
index c1709d58..00000000
--- a/src/digger/digger/digger_node.py
+++ /dev/null
@@ -1,320 +0,0 @@
-# This ROS 2 node contains code for the digger subsystem of the robot.
-# Original Author: Anthony Brogni in Fall 2023
-# Maintainer: Anthony Brogni
-# Last Updated: November 2023
-
-import time
-
-# Import the ROS 2 Python module
-import rclpy
-from rclpy.node import Node
-from rclpy.executors import MultiThreadedExecutor
-from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
-
-# Import ROS 2 formatted message types
-from std_msgs.msg import Float32, Float32MultiArray
-
-# Import custom ROS 2 interfaces
-from rovr_interfaces.srv import MotorCommandSet, MotorCommandGet
-from rovr_interfaces.srv import SetPower, SetPosition
-from rovr_interfaces.msg import Potentiometers
-from std_srvs.srv import Trigger
-
-
-class DiggerNode(Node):
- def __init__(self):
- """Initialize the ROS 2 digger node."""
- super().__init__("digger")
-
- self.cancel_current_srv = False
- self.long_service_running = False
-
- # Calling the lift_stop service will cancel any long-running lift services!
- self.stop_lift_cb_group = MutuallyExclusiveCallbackGroup()
- self.service_cb_group = MutuallyExclusiveCallbackGroup()
-
- # Define service clients here
- self.cli_motor_set = self.create_client(MotorCommandSet, "motor/set")
- self.cli_motor_get = self.create_client(MotorCommandGet, "motor/get")
- self.cli_digger_lift_set = self.create_client(MotorCommandSet, "digger_lift/set")
-
- # Define services (methods callable from the outside) here
- self.srv_toggle = self.create_service(
- SetPower, "digger/toggle", self.toggle_callback, callback_group=self.service_cb_group
- )
- self.srv_stop = self.create_service(
- Trigger, "digger/stop", self.stop_callback, callback_group=self.service_cb_group
- )
- self.srv_setPower = self.create_service(
- SetPower, "digger/setPower", self.set_power_callback, callback_group=self.service_cb_group
- )
- self.srv_setPosition = self.create_service(
- SetPosition, "lift/setPosition", self.set_position_callback, callback_group=self.service_cb_group
- )
- self.srv_lift_stop = self.create_service(
- Trigger, "lift/stop", self.stop_lift_callback, callback_group=self.stop_lift_cb_group
- )
- self.srv_lift_set_power = self.create_service(
- SetPower, "lift/setPower", self.lift_set_power_callback, callback_group=self.service_cb_group
- )
- self.srv_zero_lift = self.create_service(
- Trigger, "lift/zero", self.zero_lift_callback, callback_group=self.service_cb_group
- )
- self.srv_bottom_lift = self.create_service(
- Trigger, "lift/bottom", self.bottom_lift_callback, callback_group=self.service_cb_group
- )
-
- # Define subscribers here
- self.linear_actuator_duty_cycle_sub = self.create_subscription(
- Float32MultiArray, "Digger_Current", self.linear_actuator_current_callback, 10
- )
- self.potentiometer_sub = self.create_subscription(Potentiometers, "potentiometers", self.pot_callback, 10)
-
- # Define publishers here
- self.lift_pose_publisher = self.create_publisher(Float32, "lift_pose", 10)
-
- # Define default values for our ROS parameters below #
- self.declare_parameter("digger_lift_manual_power_down", 0.12)
- self.declare_parameter("digger_lift_manual_power_up", 0.5)
- self.declare_parameter("DIGGER_MOTOR", 3)
- self.declare_parameter("DIGGER_ACTUATORS_OFFSET", 12)
- self.declare_parameter("DIGGER_SAFETY_ZONE", 120) # Measured in potentiometer units (0 to 1023)
- # Assign the ROS Parameters to member variables below #
- self.digger_lift_manual_power_down = self.get_parameter("digger_lift_manual_power_down").value
- self.digger_lift_manual_power_up = self.get_parameter("digger_lift_manual_power_up").value
- self.DIGGER_MOTOR = self.get_parameter("DIGGER_MOTOR").value
- self.DIGGER_ACTUATORS_OFFSET = self.get_parameter("DIGGER_ACTUATORS_OFFSET").value
- self.DIGGER_SAFETY_ZONE = self.get_parameter("DIGGER_SAFETY_ZONE").value
- # Print the ROS Parameters to the terminal below #
- self.get_logger().info(
- "digger_lift_manual_power_down has been set to: " + str(self.digger_lift_manual_power_down)
- )
- self.get_logger().info("digger_lift_manual_power_up has been set to: " + str(self.digger_lift_manual_power_up))
- self.get_logger().info("DIGGER_MOTOR has been set to: " + str(self.DIGGER_MOTOR))
- self.get_logger().info("DIGGER_ACTUATORS_OFFSET has been set to: " + str(self.DIGGER_ACTUATORS_OFFSET))
- self.get_logger().info("DIGGER_SAFETY_ZONE has been set to: " + str(self.DIGGER_SAFETY_ZONE))
- # Current state of the digger chain
- self.running = False
- # Current position of the lift motor in potentiometer units (0 to 1023)
- self.current_lift_position = None # We don't know the current position yet
- # Goal Threshold
- self.goal_threshold = 2 # in potentiometer units (0 to 1023)
- # Current state of the lift system
- self.lift_lowering = False
-
- # Linear Actuator Current Threshold
- self.current_threshold = 0.3
- self.left_linear_actuator_current = 0.0
- self.right_linear_actuator_current = 0.0
-
- # Define subsystem methods here
- def set_power(self, digger_power: float) -> None:
- """This method sets power to the digger chain."""
- if self.current_lift_position < self.DIGGER_SAFETY_ZONE:
- self.get_logger().warn("WARNING: The digger is not extended enough! Stopping the buckets.")
- self.stop() # Stop the digger chain
- else:
- self.running = True
- self.cli_motor_set.call_async(
- MotorCommandSet.Request(type="duty_cycle", can_id=self.DIGGER_MOTOR, value=digger_power)
- )
-
- def stop(self) -> None:
- """This method stops the digger chain."""
- self.running = False
- self.cli_motor_set.call_async(MotorCommandSet.Request(type="duty_cycle", can_id=self.DIGGER_MOTOR, value=0.0))
-
- def toggle(self, digger_chain_power: float) -> None:
- """This method toggles the digger chain."""
- if self.running:
- self.stop()
- else:
- self.set_power(digger_chain_power)
-
- def set_position(self, position: int) -> None:
- """This method sets the position of the digger lift and waits until the goal is reached."""
- self.lift_lowering = position > self.current_lift_position
- if self.lift_lowering and (not self.running) and (self.current_lift_position >= self.DIGGER_SAFETY_ZONE):
- self.get_logger().warn(
- "WARNING: The digger buckets are not running! Will not lower.", throttle_duration_sec=5
- )
- self.stop_lift() # Stop the lift system
- return
- self.get_logger().info("Setting the lift position to: " + str(position))
- self.long_service_running = True
- self.cli_digger_lift_set.call_async(
- MotorCommandSet.Request(
- type="position",
- value=float(position),
- )
- )
- # Wait until the goal position goal is reached to return
- while abs(position - self.current_lift_position) > self.goal_threshold:
- if self.cancel_current_srv:
- self.cancel_current_srv = False
- break
- time.sleep(0.1) # We don't want to spam loop iterations too fast
- self.long_service_running = False
- self.get_logger().info("Done setting the lift position to: " + str(position))
-
- def stop_lift(self) -> None:
- """This method stops the lift."""
- self.cli_digger_lift_set.call_async(
- MotorCommandSet.Request(
- type="duty_cycle",
- value=0.0,
- )
- )
-
- def lift_set_power(self, power: float) -> None:
- """This method sets power to the lift system."""
- self.lift_lowering = power < 0
- if self.lift_lowering and (not self.running) and (self.current_lift_position >= self.DIGGER_SAFETY_ZONE):
- self.get_logger().warn(
- "WARNING: The digger buckets are not running! Will not lower.", throttle_duration_sec=5
- )
- self.stop_lift() # Stop the lift system
- return
- self.cli_digger_lift_set.call_async(
- MotorCommandSet.Request(
- type="duty_cycle",
- value=power,
- )
- )
-
- def zero_lift(self) -> None:
- """This method zeros the lift system by slowly raising it until the duty cycle is 0."""
- self.get_logger().info("Zeroing the lift system")
- self.long_service_running = True
- self.lift_set_power(self.digger_lift_manual_power_up)
- lastPowerTime = time.time()
- # Wait 0.5 seconds after the current goes below the threshold before stopping the motor
- while time.time() - lastPowerTime < 0.5:
- if self.cancel_current_srv:
- self.cancel_current_srv = False
- break
- # If the current is not below the threshold, update the last power time
- if not (
- self.left_linear_actuator_current < self.current_threshold
- or self.right_linear_actuator_current < self.current_threshold
- ):
- lastPowerTime = time.time()
- time.sleep(0.1) # We don't want to spam loop iterations too fast
- # self.get_logger().info("time.time() - lastPowerTime is currently: " + str(time.time() - lastPowerTime))
- self.stop_lift()
- self.long_service_running = False
- self.get_logger().info("Done zeroing the lift system")
-
- def bottom_lift(self) -> None:
- """This method bottoms out the lift system by slowly lowering it until the duty cycle is 0."""
- self.get_logger().info("Bottoming out the lift system")
- self.long_service_running = True
- self.lift_set_power(-self.digger_lift_manual_power_down)
- lastPowerTime = time.time()
- # Wait 0.5 seconds after the current goes below the threshold before stopping the motor
- while time.time() - lastPowerTime < 0.5:
- if self.cancel_current_srv:
- self.cancel_current_srv = False
- break
- # If the current is not below the threshold, update the last power time
- if not (
- self.left_linear_actuator_current < self.current_threshold
- or self.right_linear_actuator_current < self.current_threshold
- ):
- lastPowerTime = time.time()
- time.sleep(0.1) # We don't want to spam loop iterations too fast
- # self.get_logger().info("time.time() - lastPowerTime is currently: " + str(time.time() - lastPowerTime))
- self.stop_lift()
- self.long_service_running = False
- self.get_logger().info("Done bottoming out the lift system")
-
- # Define service callback methods here
- def set_power_callback(self, request, response):
- """This service request sets power to the digger chain."""
- self.set_power(request.power)
- response.success = True
- return response
-
- def stop_callback(self, request, response):
- """This service request stops the digger chain."""
- self.stop()
- response.success = True
- return response
-
- def toggle_callback(self, request, response):
- """This service request toggles the digger chain."""
- self.toggle(request.power)
- response.success = True
- return response
-
- def set_position_callback(self, request, response):
- """This service request sets the position of the lift."""
- self.set_position(request.position)
- response.success = True
- return response
-
- def stop_lift_callback(self, request, response):
- """This service request stops the lift system."""
- if self.long_service_running:
- self.cancel_current_srv = True
- self.stop_lift()
- response.success = True
- return response
-
- def lift_set_power_callback(self, request, response):
- """This service request sets power to the digger chain."""
- self.lift_set_power(request.power)
- response.success = True
- return response
-
- def zero_lift_callback(self, request, response):
- """This service request zeros the lift system."""
- self.zero_lift()
- response.success = True
- return response
-
- def bottom_lift_callback(self, request, response):
- """This service request bottoms out the lift system."""
- self.bottom_lift()
- response.success = True
- return response
-
- # Define the subscriber callback for the potentiometers topic
- def pot_callback(self, msg: Potentiometers):
- """Helps us know whether or not the current goal position has been reached."""
- # Average the two potentiometer values
- self.current_lift_position = ((msg.left_motor_pot - self.DIGGER_ACTUATORS_OFFSET) + msg.right_motor_pot) / 2
- self.lift_pose_publisher.publish(Float32(data=self.current_lift_position))
- if self.current_lift_position < self.DIGGER_SAFETY_ZONE and self.running:
- self.get_logger().warn("WARNING: The digger is not extended enough! Stopping the buckets.")
- self.stop() # Stop the digger chain
- if self.lift_lowering and (not self.running) and (self.current_lift_position >= self.DIGGER_SAFETY_ZONE):
- self.get_logger().warn(
- "WARNING: The digger buckets are not running! Will not lower.", throttle_duration_sec=5
- )
- self.stop_lift() # Stop the lift system
-
- # Define subscriber callback methods here
- def linear_actuator_current_callback(self, linear_acutator_msg):
- self.left_linear_actuator_current = linear_acutator_msg.data[0]
- self.right_linear_actuator_current = linear_acutator_msg.data[1]
-
-
-def main(args=None):
- """The main function."""
- rclpy.init(args=args)
-
- node = DiggerNode()
- executor = MultiThreadedExecutor()
- executor.add_node(node)
-
- node.get_logger().info("Initializing the Digger subsystem!")
- executor.spin()
-
- node.destroy_node()
- rclpy.shutdown()
-
-
-# This code does NOT run if this file is imported as a module
-if __name__ == "__main__":
- main()
diff --git a/src/digger/setup.cfg b/src/digger/setup.cfg
deleted file mode 100644
index 2943c827..00000000
--- a/src/digger/setup.cfg
+++ /dev/null
@@ -1,4 +0,0 @@
-[develop]
-script_dir=$base/lib/digger
-[install]
-install_scripts=$base/lib/digger
diff --git a/src/drivetrain/drivetrain/drivetrain_node.py b/src/drivetrain/drivetrain/drivetrain_node.py
index 712a1e22..5910eb0c 100644
--- a/src/drivetrain/drivetrain/drivetrain_node.py
+++ b/src/drivetrain/drivetrain/drivetrain_node.py
@@ -75,7 +75,11 @@ def __init__(self):
def drive(self, forward_power: float, turning_power: float) -> None:
"""This method drives the robot with the desired forward and turning power."""
- if self.current_lift_position is None or self.current_lift_position > self.DIGGER_SAFETY_ZONE:
+ if (
+ (self.current_lift_position is None or self.current_lift_position > self.DIGGER_SAFETY_ZONE)
+ and not self.GAZEBO_SIMULATION
+ and (forward_power != 0 or turning_power != 0)
+ ):
self.get_logger().warn("Digger outside the safety zone, cannot drive!")
self.stop()
return
diff --git a/src/dumper/dumper/dumper_node.py b/src/dumper/dumper/dumper_node.py
index 2dad12ba..48aa0470 100644
--- a/src/dumper/dumper/dumper_node.py
+++ b/src/dumper/dumper/dumper_node.py
@@ -13,7 +13,7 @@
# Import custom ROS 2 interfaces
from rovr_interfaces.srv import MotorCommandSet, MotorCommandGet, SetPower
from std_srvs.srv import Trigger
-from std_msgs.msg import Float32
+from std_msgs.msg import Float32, Bool
class DumperNode(Node):
@@ -40,19 +40,22 @@ def __init__(self):
Trigger, "dumper/stop", self.stop_callback, callback_group=self.stop_service_cb_group
)
self.srv_setPower = self.create_service(
- SetPower, "dumper/setPower", self.set_power_callback, callback_group=self.service_cb_group
+ SetPower,
+ "dumper/setPower",
+ self.set_power_callback,
+ callback_group=self.service_cb_group,
)
- self.srv_extendDumper = self.create_service(
- Trigger, "dumper/extendDumper", self.extend_callback, callback_group=self.service_cb_group
+ self.srv_dumpDumper = self.create_service(
+ Trigger, "dumper/storeDumper", self.dump_callback, callback_group=self.service_cb_group
)
- self.srv_retractDumper = self.create_service(
- Trigger, "dumper/retractDumper", self.retract_callback, callback_group=self.service_cb_group
+ self.srv_storeDumper = self.create_service(
+ Trigger, "dumper/dumpDumper", self.store_callback, callback_group=self.service_cb_group
)
# Define default values for our ROS parameters below #
self.declare_parameter("DUMPER_MOTOR", 11)
- self.declare_parameter("DUMPER_POWER", 0.75)
+ self.declare_parameter("DUMPER_POWER", 0.3)
# Assign the ROS Parameters to member variables below #
self.DUMPER_MOTOR = self.get_parameter("DUMPER_MOTOR").value
self.DUMPER_POWER = self.get_parameter("DUMPER_POWER").value
@@ -61,31 +64,43 @@ def __init__(self):
self.get_logger().info("DUMPER_MOTOR has been set to: " + str(self.DUMPER_MOTOR))
# Current state of the dumper
- self.extended_state = False
+ self.dumped_state = False
# Dumper Current Threshold
self.current_threshold = 0.3
self.dumper_current = 0.0
- self.dumper_current_sub = self.create_subscription(Float32, "Dumper_Current", self.dumper_current_callback, 10)
+ self.dumper_current_sub = self.create_subscription(
+ Float32, "Dumper_Current", self.dumper_current_callback, 10
+ )
+
+ self.KillSwitch_sub = self.create_subscription(
+ Bool, "DumperLimitSwitch", self.killSwitch_callback, 10
+ )
+ self.limitSwitchBottom = False
+ # self.auger_stowed = True
# Define subsystem methods here
def set_power(self, dumper_power: float) -> None:
"""This method sets power to the dumper."""
self.cli_motor_set.call_async(
- MotorCommandSet.Request(type="duty_cycle", can_id=self.DUMPER_MOTOR, value=-1*dumper_power)
+ MotorCommandSet.Request(
+ type="duty_cycle", can_id=self.DUMPER_MOTOR, value=-1 * dumper_power
+ )
)
def stop(self) -> None:
"""This method stops the dumper."""
- self.cli_motor_set.call_async(MotorCommandSet.Request(type="duty_cycle", can_id=self.DUMPER_MOTOR, value=0.0))
+ self.cli_motor_set.call_async(
+ MotorCommandSet.Request(type="duty_cycle", can_id=self.DUMPER_MOTOR, value=0.0)
+ )
def toggle(self) -> None:
"""This method toggles the dumper."""
- if not self.extended_state:
- self.extend_dumper()
+ if not self.dumped_state:
+ self.dump_dumper()
else:
- self.retract_dumper()
+ self.store_dumper()
# Define service callback methods here
def set_power_callback(self, request, response):
@@ -108,61 +123,67 @@ def toggle_callback(self, request, response):
response.success = True
return response
- def extend_dumper(self) -> None:
+ def dump_dumper(self) -> None:
+ # if not self.auger_stowed:
+ # self.get_logger().info("The auger is already extended")
+ # return
+
self.get_logger().info("Extending the dumper")
- self.extended_state = True
+ self.dumped_state = True
self.long_service_running = True
- self.set_power(self.DUMPER_POWER)
- lastPowerTime = time.time()
- # Wait 0.5 seconds after the dumper current goes below the threshold before stopping the motor
- while time.time() - lastPowerTime < 0.5:
+
+ future = self.cli_motor_set.call_async(
+ MotorCommandSet.Request(type="position", can_id=self.DUMPER_MOTOR, value=90)
+ )
+
+ while not future.done(): # While loop makes the motor keep going till limit switch is hit
if self.cancel_current_srv:
self.cancel_current_srv = False
break
- # If the dumper current is not below the threshold, update the last power time
- if not self.dumper_current < self.current_threshold:
- lastPowerTime = time.time()
- time.sleep(0.1) # We don't want to spam loop iterations too fast
- # self.get_logger().info("time.time() - lastPowerTime is currently: " + str(time.time() - lastPowerTime))
+ time.sleep(0.1)
+
self.stop()
self.long_service_running = False
- self.get_logger().info("Done extending the dumper")
+ self.get_logger().info("Done dumping the dumper")
- def extend_callback(self, request, response):
- """This service request extends the dumper"""
- self.extend_dumper()
+ def dump_callback(self, request, response):
+ self.dump_dumper()
response.success = True
return response
- def retract_dumper(self) -> None:
+ def store_dumper(self) -> None: # get the variables
+ # if not self.auger_stowed:
+ # self.get_logger().info("The Auger is already extended")
+ # return
self.get_logger().info("Retracting the dumper")
- self.extended_state = False
+ self.dumped_state = False
self.long_service_running = True
- self.set_power(-self.DUMPER_POWER)
- lastPowerTime = time.time()
- # Wait 0.5 seconds after the dumper current goes below the threshold before stopping the motor
- while time.time() - lastPowerTime < 0.5:
+ self.cli_motor_set.call_async(
+ MotorCommandSet.Request(type="duty_cycle", can_id=self.DUMPER_MOTOR, value=self.DUMPER_POWER)
+ )
+ while not self.limitSwitchBottom:
if self.cancel_current_srv:
self.cancel_current_srv = False
break
- # If the dumper current is not below the threshold, update the last power time
- if not self.dumper_current < self.current_threshold:
- lastPowerTime = time.time()
- time.sleep(0.1) # We don't want to spam loop iterations too fast
- # self.get_logger().info("time.time() - lastPowerTime is currently: " + str(time.time() - lastPowerTime))
+ time.sleep(0.1)
+
self.stop()
self.long_service_running = False
- self.get_logger().info("Done retracting the dumper")
+ self.get_logger().info("Done storing the dumper")
- def retract_callback(self, request, response):
- """This service request retracts the dumper"""
- self.retract_dumper()
+ # the storage bin can only be dumped back
+ # when the auger is completely stowed (both actuators fully stored)
+ def store_callback(self, request, response):
+ self.store_dumper()
response.success = True
return response
def dumper_current_callback(self, msg):
self.dumper_current = msg.data
- # self.get_logger().info("Dumper current: " + str(self.dumper_current))
+
+ def killSwitch_callback(self, msg):
+ # position control...
+ self.LimitSwitchBottom = msg.data
def main(args=None):
diff --git a/src/gazebo/ros_gz_launch/launch/COSMIC_field.launch.py b/src/gazebo/ros_gz_launch/launch/COSMIC_field.launch.py
index 70cba876..b56f7534 100644
--- a/src/gazebo/ros_gz_launch/launch/COSMIC_field.launch.py
+++ b/src/gazebo/ros_gz_launch/launch/COSMIC_field.launch.py
@@ -32,7 +32,7 @@ def generate_launch_description():
[PathJoinSubstitution([FindPackageShare("ros_gz_sim"), "launch", "gz_sim.launch.py"])]
),
launch_arguments={
- "gz_args": PathJoinSubstitution([FindPackageShare("gazebo_files"), "worlds", "COSMIC_field.sdf"])
+ "gz_args": ["-r ", PathJoinSubstitution([FindPackageShare("gazebo_files"), "worlds", "COSMIC_field.sdf"])]
}.items(),
)
diff --git a/src/gstreamer/launch/laptop_launch.py b/src/gstreamer/launch/laptop_launch.py
index da3abb0d..ac52a0a7 100644
--- a/src/gstreamer/launch/laptop_launch.py
+++ b/src/gstreamer/launch/laptop_launch.py
@@ -1,9 +1,8 @@
from launch import LaunchDescription
-from launch_ros.actions import Node
-from launch.actions import ExecuteProcess
-from launch.conditions import IfCondition
from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
# Launches the joystick node and rviz and the gstreamer camera client on the operator laptop
@@ -17,16 +16,23 @@ def generate_launch_description():
run_rviz_arg = DeclareLaunchArgument("run_rviz_client", default_value="True", description="Whether to start RVIZ")
- joystick_node = Node(
- package="joy",
- executable="joy_node",
- parameters=["config/joy_node.yaml"],
- )
+ # joystick_node = Node(
+ # package="joy",
+ # executable="joy_node",
+ # parameters=["config/joy_node.yaml"],
+ # )
- start_gStreamer_client = ExecuteProcess(
- cmd=["rqt", "--force-discover", "--standalone", "CameraClient"], shell=True, output="screen"
+ stream_deck_node = Node(
+ package="rovr_control",
+ executable="stream_deck_node",
+ name="stream_deck_node",
+ output="screen",
)
+ # start_gStreamer_client = ExecuteProcess(
+ # cmd=["rqt", "--force-discover", "--standalone", "CameraClient"], shell=True, output="screen"
+ # )
+
# NOTE: RVIZ uses a lot of bandwidth, so it should be turned off during competition matches
rviz_node = Node(
package="rviz2",
@@ -38,8 +44,9 @@ def generate_launch_description():
)
ld.add_action(run_rviz_arg)
- ld.add_action(joystick_node) # TODO: The joystick node currently does not work inside the container!
- ld.add_action(start_gStreamer_client)
+ ld.add_action(stream_deck_node)
+ # ld.add_action(joystick_node) # TODO: The joystick node currently does not work inside the container!
+ # ld.add_action(start_gStreamer_client)
ld.add_action(rviz_node)
return ld
diff --git a/src/isaac_ros/isaac_ros_apriltag b/src/isaac_ros/isaac_ros_apriltag
index 6037bf7e..b01ddd97 160000
--- a/src/isaac_ros/isaac_ros_apriltag
+++ b/src/isaac_ros/isaac_ros_apriltag
@@ -1 +1 @@
-Subproject commit 6037bf7e114279e47ef1fef1ceb29dbd2cedd316
+Subproject commit b01ddd97e06ce02f38078041b750cb215623d0b4
diff --git a/src/isaac_ros/isaac_ros_launch/launch/EVERYTHING_launch.py b/src/isaac_ros/isaac_ros_launch/launch/EVERYTHING_launch.py
index e49d4868..00974e67 100644
--- a/src/isaac_ros/isaac_ros_launch/launch/EVERYTHING_launch.py
+++ b/src/isaac_ros/isaac_ros_launch/launch/EVERYTHING_launch.py
@@ -1,6 +1,5 @@
from launch_ros.substitutions import FindPackageShare
-from launch_ros.actions import Node
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
@@ -21,26 +20,23 @@ def generate_launch_description():
launch_arguments={
"setup_for_zed": "True",
"setup_for_gazebo": "False",
- "use_nvblox": "True",
+ "use_nvblox": "False",
"use_nav2": "True",
"run_rviz_robot": "False", # We don't need to run RViz during matches
- "zed_multicam": "True", # Use multiple ZED cameras
- "record_svo": "True", # Record match data to an SVO file
+ "zed_multicam": "False", # Use multiple ZED cameras
+ "record_svo": "False", # Record match data to an SVO file
+ "use_apriltags": "False",
}.items(),
)
- gstreamer_server = Node(
- package="gstreamer",
- executable="server_node",
- name="gstreamer_server_node",
- output="screen",
- emulate_tty=True,
+ camera_launch = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare("rovr_control"), "camera_launch.py"]))
)
# Add all of the actions to the launch description
ld.add_action(main_launch)
- ld.add_action(gstreamer_server)
ld.add_action(isaac_launch)
+ ld.add_action(camera_launch)
# Return the launch description
return ld
diff --git a/src/isaac_ros/isaac_ros_launch/launch/isaac_launch.py b/src/isaac_ros/isaac_ros_launch/launch/isaac_launch.py
index ee3c83bb..78025284 100644
--- a/src/isaac_ros/isaac_ros_launch/launch/isaac_launch.py
+++ b/src/isaac_ros/isaac_ros_launch/launch/isaac_launch.py
@@ -5,7 +5,7 @@
DeclareLaunchArgument,
IncludeLaunchDescription,
)
-from launch.conditions import IfCondition, UnlessCondition
+from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression
from nav2_common.launch import RewrittenYaml
@@ -15,8 +15,10 @@ def generate_launch_description():
# Launch Configurations
setup_for_zed = LaunchConfiguration("setup_for_zed", default="True")
- use_nvblox = LaunchConfiguration("use_nvblox", default="True")
+ # use_nvblox = LaunchConfiguration("use_nvblox", default="False")
zed_multicam = LaunchConfiguration("zed_multicam", default="False")
+ use_apriltags = LaunchConfiguration("use_apriltags", default="True")
+ setup_for_gazebo = LaunchConfiguration("setup_for_gazebo", default="False")
# Launch Arguments
run_rviz_arg = DeclareLaunchArgument("run_rviz_robot", default_value="True", description="Whether to start RVIZ")
@@ -45,6 +47,12 @@ def generate_launch_description():
executable="dig_location_server",
name="dig_location_server",
)
+ alexblox_server = Node(
+ package="alexblox",
+ executable="costmapGenerator",
+ name="costmapGenerator",
+ parameters=["config/costmap_config.yaml"],
+ )
record_svo_arg = DeclareLaunchArgument(
"record_svo",
default_value="False",
@@ -55,6 +63,11 @@ def generate_launch_description():
default_value="False",
description="Whether to use two ZED cameras",
)
+ use_apriltags_arg = DeclareLaunchArgument(
+ "use_apriltags",
+ default_value="True",
+ description="Whether to run isaac ros apriltags",
+ )
global_frame = LaunchConfiguration("global_frame", default="odom")
@@ -98,32 +111,32 @@ def generate_launch_description():
)
# Nvblox
- nvblox_launch = IncludeLaunchDescription(
- PythonLaunchDescriptionSource(
- [PathJoinSubstitution([FindPackageShare("isaac_ros_launch"), "nvblox.launch.py"])]
- ),
- launch_arguments={
- "global_frame": global_frame,
- "setup_for_zed": LaunchConfiguration("setup_for_zed"),
- "setup_for_gazebo": LaunchConfiguration("setup_for_gazebo"),
- "attach_to_shared_component_container": "True",
- "component_container_name": shared_container_name,
- }.items(),
- condition=IfCondition(PythonExpression([use_nvblox, " and not ", zed_multicam])),
- )
- nvblox_multicam_launch = IncludeLaunchDescription(
- PythonLaunchDescriptionSource(
- [PathJoinSubstitution([FindPackageShare("isaac_ros_launch"), "nvblox_multicam.launch.py"])]
- ),
- launch_arguments={
- "global_frame": global_frame,
- "setup_for_zed": LaunchConfiguration("setup_for_zed"),
- "setup_for_gazebo": LaunchConfiguration("setup_for_gazebo"),
- "attach_to_shared_component_container": "True",
- "component_container_name": shared_container_name,
- }.items(),
- condition=IfCondition(PythonExpression([use_nvblox, " and ", zed_multicam])),
- )
+ # nvblox_launch = IncludeLaunchDescription(
+ # PythonLaunchDescriptionSource(
+ # [PathJoinSubstitution([FindPackageShare("isaac_ros_launch"), "nvblox.launch.py"])]
+ # ),
+ # launch_arguments={
+ # "global_frame": global_frame,
+ # "setup_for_zed": LaunchConfiguration("setup_for_zed"),
+ # "setup_for_gazebo": LaunchConfiguration("setup_for_gazebo"),
+ # "attach_to_shared_component_container": "True",
+ # "component_container_name": shared_container_name,
+ # }.items(),
+ # condition=IfCondition(PythonExpression([use_nvblox, " and not ", zed_multicam])),
+ # )
+ # nvblox_multicam_launch = IncludeLaunchDescription(
+ # PythonLaunchDescriptionSource(
+ # [PathJoinSubstitution([FindPackageShare("isaac_ros_launch"), "nvblox_multicam.launch.py"])]
+ # ),
+ # launch_arguments={
+ # "global_frame": global_frame,
+ # "setup_for_zed": LaunchConfiguration("setup_for_zed"),
+ # "setup_for_gazebo": LaunchConfiguration("setup_for_gazebo"),
+ # "attach_to_shared_component_container": "True",
+ # "component_container_name": shared_container_name,
+ # }.items(),
+ # condition=IfCondition(PythonExpression([use_nvblox, " and ", zed_multicam])),
+ # )
# Rviz
rviz_launch = IncludeLaunchDescription(
@@ -161,14 +174,14 @@ def generate_launch_description():
# apriltag launch
apriltag_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource([PathJoinSubstitution([FindPackageShare("apriltag"), "apriltag_launch.py"])]),
- condition=UnlessCondition(LaunchConfiguration("setup_for_gazebo")),
+ condition=IfCondition(PythonExpression([use_apriltags, " and not ", setup_for_gazebo])),
)
# apriltag (gazebo) launch
apriltag_gazebo_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[PathJoinSubstitution([FindPackageShare("apriltag"), "apriltag_gazebo_launch.py"])]
),
- condition=IfCondition(LaunchConfiguration("setup_for_gazebo")),
+ condition=IfCondition(PythonExpression([use_apriltags, " and ", setup_for_gazebo])),
)
return LaunchDescription(
@@ -180,9 +193,10 @@ def generate_launch_description():
use_nvblox_arg,
use_nav2_arg,
zed_multicam_arg,
+ use_apriltags_arg,
shared_container,
- nvblox_launch,
- nvblox_multicam_launch,
+ # nvblox_launch,
+ # nvblox_multicam_launch,
nav2_launch,
zed_launch,
zed_multicam_launch,
@@ -191,5 +205,6 @@ def generate_launch_description():
apriltag_launch,
apriltag_gazebo_launch,
dig_location_server,
+ alexblox_server,
]
)
diff --git a/src/isaac_ros/isaac_ros_launch/package.xml b/src/isaac_ros/isaac_ros_launch/package.xml
index 00248ac9..bb73efed 100644
--- a/src/isaac_ros/isaac_ros_launch/package.xml
+++ b/src/isaac_ros/isaac_ros_launch/package.xml
@@ -12,12 +12,13 @@
ament_pep257
python3-pytest
- isaac_ros_nvblox
+ alexblox
+
isaac_ros_apriltag
zed_wrapper
- nvblox_rviz_plugin
+
robot_description
apriltag
ros_gz_launch
diff --git a/src/isaac_ros/isaac_ros_nitros b/src/isaac_ros/isaac_ros_nitros
index 33aeabbc..f864589e 160000
--- a/src/isaac_ros/isaac_ros_nitros
+++ b/src/isaac_ros/isaac_ros_nitros
@@ -1 +1 @@
-Subproject commit 33aeabbcda9f972594709334a496af29da60b0a5
+Subproject commit f864589e7dbcd179ac1fbb295b3b3b843e06c8ba
diff --git a/src/isaac_ros/isaac_ros_visual_slam b/src/isaac_ros/isaac_ros_visual_slam
index 9745130a..b348f673 160000
--- a/src/isaac_ros/isaac_ros_visual_slam
+++ b/src/isaac_ros/isaac_ros_visual_slam
@@ -1 +1 @@
-Subproject commit 9745130af04b10353a0aa1542840c5573cda268f
+Subproject commit b348f673367b421d1d6c634f54a27f7530ce078e
diff --git a/src/motor_control/src/motor_control_node.cpp b/src/motor_control/src/motor_control_node.cpp
index 0e7d8416..13321d18 100644
--- a/src/motor_control/src/motor_control_node.cpp
+++ b/src/motor_control/src/motor_control_node.cpp
@@ -9,9 +9,8 @@
// Import ROS 2 Formatted Message Types
#include "can_msgs/msg/frame.hpp"
#include "geometry_msgs/msg/twist.hpp"
-#include "geometry_msgs/msg/pose_stamped.hpp"
#include "std_msgs/msg/float32.hpp"
-#include
+// #include
#include "std_msgs/msg/string.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2/LinearMath/Matrix3x3.h"
@@ -61,11 +60,7 @@ struct MotorData {
std::chrono::time_point timestamp;
};
-// Define a struct to store the current digger lift goal
-struct DiggerLiftGoal {
- std::string type;
- float value;
-};
+
class PIDController {
private:
@@ -248,13 +243,6 @@ class MotorControlNode : public rclcpp::Node {
RCLCPP_DEBUG(this->get_logger(), "Setting the position of CAN ID: %u to %d degrees", id, position); // Print Statement
}
- void set_digger_lift_callback(const std::shared_ptr request,
- std::shared_ptr response) {
- // Update the current digger lift goal
- this->digger_lift_goal = { request->type, request->value };
- response->success = true;
- }
-
// Get the motor controller's current duty cycle command
std::optional vesc_get_duty_cycle(uint32_t id) {
if (std::chrono::steady_clock::now() - this->can_data[id].timestamp < this->threshold) {
@@ -293,53 +281,23 @@ class MotorControlNode : public rclcpp::Node {
// Define default values for our ROS parameters below #
this->declare_parameter("CAN_INTERFACE_TRANSMIT", "can0");
this->declare_parameter("CAN_INTERFACE_RECEIVE", "can0");
- this->declare_parameter("DIGGER_LEFT_LINEAR_ACTUATOR", 8);
- this->declare_parameter("DIGGER_RIGHT_LINEAR_ACTUATOR", 7);
- this->declare_parameter("MAX_POS_DIFF", 30);
this->declare_parameter("DUMPER_MOTOR", 2);
- this->declare_parameter("DIGGER_MOTOR", 10);
- this->declare_parameter("DIGGER_ACTUATORS_OFFSET", 12);
- this->declare_parameter("DIGGER_ACTUATORS_kP", 0.05);
- this->declare_parameter("DIGGER_ACTUATORS_kP_coupling", 0.10);
- this->declare_parameter("DIGGER_PITCH_kP", 2.5);
- this->declare_parameter("TIPPING_SPEED_ADJUSTMENT", true);
- this->declare_parameter("CURRENT_SPIKE_THRESHOLD", 1.8);
- this->declare_parameter("CURRENT_SPIKE_TIME", 0.2);
- this->declare_parameter("BUCKETS_CURRENT_SPIKE_THRESHOLD", 8.0);
- this->declare_parameter("BUCKETS_CURRENT_SPIKE_TIME", 0.2);
// Print the ROS Parameters to the terminal below #
RCLCPP_INFO(this->get_logger(), "CAN_INTERFACE_TRANSMIT parameter set to: %s", this->get_parameter("CAN_INTERFACE_TRANSMIT").as_string().c_str());
RCLCPP_INFO(this->get_logger(), "CAN_INTERFACE_RECEIVE parameter set to: %s", this->get_parameter("CAN_INTERFACE_RECEIVE").as_string().c_str());
- RCLCPP_INFO(this->get_logger(), "DIGGER_LEFT_LINEAR_ACTUATOR parameter set to: %ld", this->get_parameter("DIGGER_LEFT_LINEAR_ACTUATOR").as_int());
- RCLCPP_INFO(this->get_logger(), "DIGGER_RIGHT_LINEAR_ACTUATOR parameter set to: %ld", this->get_parameter("DIGGER_RIGHT_LINEAR_ACTUATOR").as_int());
- RCLCPP_INFO(this->get_logger(), "MAX_POS_DIFF parameter set to: %ld", this->get_parameter("MAX_POS_DIFF").as_int());
RCLCPP_INFO(this->get_logger(), "DUMPER_MOTOR parameter set to: %ld", this->get_parameter("DUMPER_MOTOR").as_int());
- RCLCPP_INFO(this->get_logger(), "DIGGER_MOTOR parameter set to: %ld", this->get_parameter("DIGGER_MOTOR").as_int());
- RCLCPP_INFO(this->get_logger(), "DIGGER_ACTUATORS_OFFSET parameter set to: %ld", this->get_parameter("DIGGER_ACTUATORS_OFFSET").as_int());
- RCLCPP_INFO(this->get_logger(), "DIGGER_ACTUATORS_kP parameter set to: %f", this->get_parameter("DIGGER_ACTUATORS_kP").as_double());
- RCLCPP_INFO(this->get_logger(), "DIGGER_ACTUATORS_kP_coupling parameter set to: %f", this->get_parameter("DIGGER_ACTUATORS_kP_coupling").as_double());
- RCLCPP_INFO(this->get_logger(), "DIGGER_PITCH_kP parameter set to: %f", this->get_parameter("DIGGER_PITCH_kP").as_double());
- RCLCPP_INFO(this->get_logger(), "TIPPING_SPEED_ADJUSTMENT parameter set to: %d", this->get_parameter("TIPPING_SPEED_ADJUSTMENT").as_bool());
- RCLCPP_INFO(this->get_logger(), "CURRENT_SPIKE_THRESHOLD parameter set to: %f", this->get_parameter("CURRENT_SPIKE_THRESHOLD").as_double());
- RCLCPP_INFO(this->get_logger(), "CURRENT_SPIKE_TIME parameter set to: %f", this->get_parameter("CURRENT_SPIKE_TIME").as_double());
- RCLCPP_INFO(this->get_logger(), "CURRENT_SPIKE_THRESHOLD parameter set to: %f", this->get_parameter("BUCKETS_CURRENT_SPIKE_THRESHOLD").as_double());
- RCLCPP_INFO(this->get_logger(), "CURRENT_SPIKE_TIME parameter set to: %f", this->get_parameter("BUCKETS_CURRENT_SPIKE_TIME").as_double());
-
+
// Initialize services below //
srv_motor_set = this->create_service(
"motor/set", std::bind(&MotorControlNode::set_callback, this, _1, _2));
srv_motor_get = this->create_service(
"motor/get", std::bind(&MotorControlNode::get_callback, this, _1, _2));
- srv_set_digger_lift = this->create_service(
- "digger_lift/set", std::bind(&MotorControlNode::set_digger_lift_callback, this, _1, _2));
+ // digger_lift service removed (digger subsystem deprecated)
// Initialize timers below //
timer = this->create_wall_timer(500ms, std::bind(&MotorControlNode::timer_callback, this));
- cli_digger_stop = this->create_client("digger/stop");
-
- digger_linear_actuator_pub = this->create_publisher("Digger_Current", 5);
dumper_linear_actuator_pub = this->create_publisher("Dumper_Current", 5);
// Initialize publishers and subscribers below //
// The name of this topic is determined by ros2socketcan_bridge
@@ -347,11 +305,7 @@ class MotorControlNode : public rclcpp::Node {
// The name of this topic is determined by ros2socketcan_bridge
can_sub = this->create_subscription("CAN/" + this->get_parameter("CAN_INTERFACE_RECEIVE").as_string() + "/receive", 10, std::bind(&MotorControlNode::CAN_callback, this, _1));
- potentiometer_sub = this->create_subscription("potentiometers", 10, std::bind(&MotorControlNode::Potentiometer_callback, this, _1));
- pose_sub = this->create_subscription("/zed2i/zed_node_zed2i/pose", 10, std::bind(&MotorControlNode::Pose_callback, this, _1));
-
- // Initialize the current digger lift goal
- this->digger_lift_goal = { "duty_cycle", 0.0 }; // Stopped by default
+
}
private:
@@ -388,8 +342,10 @@ class MotorControlNode : public rclcpp::Node {
RPM = static_cast((can_msg->data[0] << 24) + (can_msg->data[1] << 16) + (can_msg->data[2] << 8) + can_msg->data[3]);
current = static_cast(static_cast((can_msg->data[4] << 8) + can_msg->data[5])) / 10.0;
dutyCycleNow = static_cast(static_cast((can_msg->data[6] << 8) + can_msg->data[7])) / 10.0 / 100.0;
- dumper_linear_actuator_msg.data = this->can_data[this->get_parameter("DUMPER_MOTOR").as_int()].current;
- dumper_linear_actuator_pub->publish(dumper_linear_actuator_msg);
+ if (motorId == this->get_parameter("DUMPER_MOTOR").as_int()) {
+ dumper_linear_actuator_msg.data = this->can_data[this->get_parameter("DUMPER_MOTOR").as_int()].current;
+ dumper_linear_actuator_pub->publish(dumper_linear_actuator_msg);
+ }
break;
case 27: // Packet Status 27 (Tachometer)
tachometer = static_cast((can_msg->data[0] << 24) + (can_msg->data[1] << 16) + (can_msg->data[2] << 8) + can_msg->data[3]);
@@ -412,167 +368,15 @@ class MotorControlNode : public rclcpp::Node {
RCLCPP_DEBUG(this->get_logger(), "RPM: %.2f Duty Cycle: %.2f%% Current: %.2fAmps Tachometer: %d", RPM, dutyCycleNow, current, tachometer);
}
- void Potentiometer_callback(const rovr_interfaces::msg::Potentiometers msg) {
- std_msgs::msg::Float32MultiArray digger_linear_actuator_msg;
- digger_linear_actuator_msg.data = {this->can_data[this->get_parameter("DIGGER_LEFT_LINEAR_ACTUATOR").as_int()].current, this->can_data[this->get_parameter("DIGGER_RIGHT_LINEAR_ACTUATOR").as_int()].current};
- digger_linear_actuator_pub->publish(digger_linear_actuator_msg);
-
- // Linear actuators current spike detection
- double current_threshold = this->get_parameter("CURRENT_SPIKE_THRESHOLD").as_double(); // in amps
- double time_limit = this->get_parameter("CURRENT_SPIKE_TIME").as_double(); // in seconds
- double left_current = this->can_data[this->get_parameter("DIGGER_LEFT_LINEAR_ACTUATOR").as_int()].current;
- double right_current = this->can_data[this->get_parameter("DIGGER_RIGHT_LINEAR_ACTUATOR").as_int()].current;
- // RCLCPP_INFO(this->get_logger(), "Left Current: %fA Right Current: %fA", left_current, right_current);
- if ((this->can_data[this->get_parameter("DIGGER_LEFT_LINEAR_ACTUATOR").as_int()].dutyCycle < 0.0
- || this->can_data[this->get_parameter("DIGGER_RIGHT_LINEAR_ACTUATOR").as_int()].dutyCycle < 0.0)
- && (left_current > current_threshold || right_current > current_threshold)) {
- if (start.has_value() && std::chrono::duration(std::chrono::steady_clock::now() - *start).count() > time_limit) {
- // Stop both linear actuators and the dumper motor
- this->digger_lift_goal = { "duty_cycle", 0.0 };
- vesc_set_duty_cycle(this->get_parameter("DIGGER_LEFT_LINEAR_ACTUATOR").as_int(), 0.0);
- vesc_set_duty_cycle(this->get_parameter("DIGGER_RIGHT_LINEAR_ACTUATOR").as_int(), 0.0);
- this->cli_digger_stop->async_send_request(std::make_shared());
- RCLCPP_WARN(this->get_logger(), "WARNING: Linear actuator current draw is too high! (%fA, %fA) Stopping the digger.", left_current, right_current);
- return;
- } else if (!start.has_value()) {
- start = std::chrono::steady_clock::now();
- RCLCPP_DEBUG(this->get_logger(), "Starting the timer for current spike detection.");
- }
- } else if (start.has_value()) {
- // Clear the start time when the current falls below the threshold
- start.reset();
- RCLCPP_DEBUG(this->get_logger(), "Resetting the timer for current spike detection.");
- }
-
- // Digging buckets current spike detection
- double buckets_current_threshold = this->get_parameter("BUCKETS_CURRENT_SPIKE_THRESHOLD").as_double(); // in amps
- double buckets_time_limit = this->get_parameter("BUCKETS_CURRENT_SPIKE_TIME").as_double(); // in seconds
- double buckets_current = this->can_data[this->get_parameter("DIGGER_MOTOR").as_int()].current;
- // RCLCPP_INFO(this->get_logger(), "Digging Buckets Current: %fA", buckets_current);
- if (buckets_current > buckets_current_threshold) {
- if (buckets_start.has_value() && std::chrono::duration(std::chrono::steady_clock::now() - *buckets_start).count() > buckets_time_limit) {
- // Stop both linear actuators and the dumper motor
- this->digger_lift_goal = { "duty_cycle", 0.0 };
- vesc_set_duty_cycle(this->get_parameter("DIGGER_LEFT_LINEAR_ACTUATOR").as_int(), 0.0);
- vesc_set_duty_cycle(this->get_parameter("DIGGER_RIGHT_LINEAR_ACTUATOR").as_int(), 0.0);
- this->cli_digger_stop->async_send_request(std::make_shared());
- RCLCPP_WARN(this->get_logger(), "WARNING: Buckets current draw is too high! (%fA) Stopping the digger.", buckets_current);
- return;
- } else if (!buckets_start.has_value()) {
- buckets_start = std::chrono::steady_clock::now();
- RCLCPP_DEBUG(this->get_logger(), "Starting the timer for buckets current spike detection.");
- }
- } else if (buckets_start.has_value()) {
- // Clear the start time when the current falls below the threshold
- buckets_start.reset();
- RCLCPP_DEBUG(this->get_logger(), "Resetting the timer for buckets current spike detection.");
- }
-
- double kP = this->get_parameter("DIGGER_ACTUATORS_kP").as_double();
- int left_motor_pot = msg.left_motor_pot - this->get_parameter("DIGGER_ACTUATORS_OFFSET").as_int();
- int right_motor_pot = msg.right_motor_pot;
+ //TODO Can we just completely get rid of this callback? - earlier we had: potentiometer readings for digger position control
- double kP_coupling = this->get_parameter("DIGGER_ACTUATORS_kP_coupling").as_double();
- int error = left_motor_pot - right_motor_pot;
- float speed_adjustment_coupling = error * kP_coupling;
-
- float kP_pitch = this->get_parameter("DIGGER_PITCH_kP").as_double();
- float error_pitch = pitch - 0.0; // may need to adjust desired state from 0.0
- float speed_adjustment_pitch = error_pitch * kP_pitch;
-
- //RCLCPP_INFO(this->get_logger(), "Error: %d, Adjustment: %f", error, speed_adjustment);
-
- if (abs(error) > this->get_parameter("MAX_POS_DIFF").as_int() && strcmp(this->digger_lift_goal.type.c_str(), "position") == 0) {
- // Stop both motors!
- this->digger_lift_goal = { "duty_cycle", 0.0 };
- vesc_set_duty_cycle(this->get_parameter("DIGGER_LEFT_LINEAR_ACTUATOR").as_int(), 0.0);
- vesc_set_duty_cycle(this->get_parameter("DIGGER_RIGHT_LINEAR_ACTUATOR").as_int(), 0.0);
- // Log an error message
- RCLCPP_ERROR(this->get_logger(), "ERROR: Position difference between linear actuators is too high! Stopping both motors.");
- }
- else if ((msg.left_motor_pot == 1023) || (msg.right_motor_pot == 1023)) {
- // Stop both motors!
- this->digger_lift_goal = { "duty_cycle", 0.0 };
- vesc_set_duty_cycle(this->get_parameter("DIGGER_LEFT_LINEAR_ACTUATOR").as_int(), 0.0);
- vesc_set_duty_cycle(this->get_parameter("DIGGER_RIGHT_LINEAR_ACTUATOR").as_int(), 0.0);
- // Log an error message
- RCLCPP_ERROR(this->get_logger(), "ERROR: Potentiometer has reached max value! Stopping both motors, check if one is unplugged");
- }
- else if ((msg.left_motor_pot == 0) || (msg.right_motor_pot == 0)) {
- // Stop both motors!
- this->digger_lift_goal = { "duty_cycle", 0.0 };
- vesc_set_duty_cycle(this->get_parameter("DIGGER_LEFT_LINEAR_ACTUATOR").as_int(), 0.0);
- vesc_set_duty_cycle(this->get_parameter("DIGGER_RIGHT_LINEAR_ACTUATOR").as_int(), 0.0);
- // Log an error message
- RCLCPP_ERROR(this->get_logger(), "ERROR: Potentiometer has reached min value! Stopping both motors, check if one is unplugged");
- }
- else if (abs(error) > this->get_parameter("MAX_POS_DIFF").as_int() && strcmp(this->digger_lift_goal.type.c_str(), "duty_cycle") == 0 && this->digger_lift_goal.value != 0.0) {
- RCLCPP_ERROR(this->get_logger(), "ERROR: Position difference between linear actuators is too high!");
- if (error > 0.0 && this->digger_lift_goal.value > 0.0) {
- vesc_set_duty_cycle(this->get_parameter("DIGGER_LEFT_LINEAR_ACTUATOR").as_int(), this->digger_lift_goal.value);
- vesc_set_duty_cycle(this->get_parameter("DIGGER_RIGHT_LINEAR_ACTUATOR").as_int(), 0.0);
- } else if (error < 0.0 && this->digger_lift_goal.value > 0.0) {
- vesc_set_duty_cycle(this->get_parameter("DIGGER_LEFT_LINEAR_ACTUATOR").as_int(), 0.0);
- vesc_set_duty_cycle(this->get_parameter("DIGGER_RIGHT_LINEAR_ACTUATOR").as_int(), this->digger_lift_goal.value);
- } else if (error > 0.0 && this->digger_lift_goal.value < 0.0) {
- vesc_set_duty_cycle(this->get_parameter("DIGGER_LEFT_LINEAR_ACTUATOR").as_int(), 0.0);
- vesc_set_duty_cycle(this->get_parameter("DIGGER_RIGHT_LINEAR_ACTUATOR").as_int(), this->digger_lift_goal.value);
- } else if (error < 0.0 && this->digger_lift_goal.value < 0.0) {
- vesc_set_duty_cycle(this->get_parameter("DIGGER_LEFT_LINEAR_ACTUATOR").as_int(), this->digger_lift_goal.value);
- vesc_set_duty_cycle(this->get_parameter("DIGGER_RIGHT_LINEAR_ACTUATOR").as_int(), 0.0);
- }
- }
- else if (strcmp(this->digger_lift_goal.type.c_str(), "duty_cycle") == 0 && this->digger_lift_goal.value != 0.0) {
- if (this->digger_lift_goal.value < 0.0 && this->get_parameter("TIPPING_SPEED_ADJUSTMENT").as_bool()) {
- vesc_set_duty_cycle(this->get_parameter("DIGGER_LEFT_LINEAR_ACTUATOR").as_int(), this->digger_lift_goal.value + speed_adjustment_coupling - speed_adjustment_pitch);
- vesc_set_duty_cycle(this->get_parameter("DIGGER_RIGHT_LINEAR_ACTUATOR").as_int(), this->digger_lift_goal.value - speed_adjustment_coupling - speed_adjustment_pitch);
- } else {
- vesc_set_duty_cycle(this->get_parameter("DIGGER_LEFT_LINEAR_ACTUATOR").as_int(), this->digger_lift_goal.value + speed_adjustment_coupling);
- vesc_set_duty_cycle(this->get_parameter("DIGGER_RIGHT_LINEAR_ACTUATOR").as_int(), this->digger_lift_goal.value - speed_adjustment_coupling);
- }
- //RCLCPP_INFO(this->get_logger(), "Output: %f, Pitch: %f", speed_adjustment_pitch, pitch);
- }
- else if (strcmp(this->digger_lift_goal.type.c_str(), "duty_cycle") == 0) {
- vesc_set_duty_cycle(this->get_parameter("DIGGER_LEFT_LINEAR_ACTUATOR").as_int(), this->digger_lift_goal.value);
- vesc_set_duty_cycle(this->get_parameter("DIGGER_RIGHT_LINEAR_ACTUATOR").as_int(), this->digger_lift_goal.value);
- }
- else if (strcmp(this->digger_lift_goal.type.c_str(), "position") == 0) {
- int left_error = left_motor_pot - int(this->digger_lift_goal.value);
- int right_error = right_motor_pot - int(this->digger_lift_goal.value);
-
- double left_controller_output = std::clamp(kP * left_error, -0.5, 0.5);
- double right_controller_output = std::clamp(kP * right_error, -0.5, 0.5);
-
- if (left_error < 0 && right_error < 0 && this->get_parameter("TIPPING_SPEED_ADJUSTMENT").as_bool()) {
- vesc_set_duty_cycle(this->get_parameter("DIGGER_LEFT_LINEAR_ACTUATOR").as_int(), left_controller_output + speed_adjustment_coupling - speed_adjustment_pitch);
- vesc_set_duty_cycle(this->get_parameter("DIGGER_RIGHT_LINEAR_ACTUATOR").as_int(), right_controller_output - speed_adjustment_coupling - speed_adjustment_pitch);
- } else {
- vesc_set_duty_cycle(this->get_parameter("DIGGER_LEFT_LINEAR_ACTUATOR").as_int(), left_controller_output + speed_adjustment_coupling);
- vesc_set_duty_cycle(this->get_parameter("DIGGER_RIGHT_LINEAR_ACTUATOR").as_int(), right_controller_output - speed_adjustment_coupling);
- }
-
- //RCLCPP_INFO(this->get_logger(), "Output: %f, Pitch: %f", speed_adjustment_pitch, pitch);
- //RCLCPP_INFO(this->get_logger(), "Current Pos: %d, Goal: %f, Output: %f", right_motor_pot, this->digger_lift_goal.value, right_controller_output);
- } else{
- RCLCPP_ERROR(this->get_logger(), "Unknown Digger Lift State: '%s'", this->digger_lift_goal.type.c_str());
- }
- }
-
- void Pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
- tf2::Quaternion q(msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z, msg->pose.orientation.w);
- tf2::Matrix3x3 m(q);
- double roll, yaw;
- m.getRPY(roll, pitch, yaw);
- }
// Initialize a hashmap to store the most recent motor data for each CAN ID
std::map can_data;
std::map pid_controllers;
- double pitch = 0.0;
std::optional start;
- std::optional buckets_start;
- DiggerLiftGoal digger_lift_goal;
+
// Adjust this data retention threshold as needed
const std::chrono::seconds threshold = std::chrono::seconds(1);
@@ -629,16 +433,11 @@ class MotorControlNode : public rclcpp::Node {
}
rclcpp::TimerBase::SharedPtr timer;
- rclcpp::Client::SharedPtr cli_digger_stop;
- rclcpp::Publisher::SharedPtr digger_linear_actuator_pub;
rclcpp::Publisher::SharedPtr dumper_linear_actuator_pub;
rclcpp::Publisher::SharedPtr can_pub;
rclcpp::Subscription::SharedPtr can_sub;
- rclcpp::Subscription::SharedPtr potentiometer_sub;
- rclcpp::Subscription::SharedPtr pose_sub;
rclcpp::Service::SharedPtr srv_motor_set;
rclcpp::Service::SharedPtr srv_motor_get;
- rclcpp::Service::SharedPtr srv_set_digger_lift;
};
// Main method for the node
diff --git a/src/realsense-ros b/src/realsense-ros
new file mode 160000
index 00000000..53169f32
--- /dev/null
+++ b/src/realsense-ros
@@ -0,0 +1 @@
+Subproject commit 53169f3265266d07e88c025b950aa4ccaa03e671
diff --git a/src/ros2socketcan_bridge b/src/ros2socketcan_bridge
index c57c3165..2fed453a 160000
--- a/src/ros2socketcan_bridge
+++ b/src/ros2socketcan_bridge
@@ -1 +1 @@
-Subproject commit c57c3165c92dd417b714c928cb4bd5e8e69cfcfb
+Subproject commit 2fed453a4c5ed6a1577103a86c39c2292b5cd286
diff --git a/src/rovr_control/launch/camera_launch.py b/src/rovr_control/launch/camera_launch.py
new file mode 100644
index 00000000..bccedfdc
--- /dev/null
+++ b/src/rovr_control/launch/camera_launch.py
@@ -0,0 +1,44 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_camera_nodes(
+ camera_name, video_device, pixel_format="yuyv", resolution=(320, 240)
+):
+ return [
+ # Camera driver node
+ Node(
+ package="usb_cam",
+ executable="usb_cam_node_exe",
+ namespace=camera_name,
+ name="camera",
+ parameters=[
+ {"video_device": video_device},
+ {"image_width": resolution[0]},
+ {"image_height": resolution[1]},
+ {"pixel_format": pixel_format},
+ ],
+ remappings=[("/image_raw", "image_raw"), ("/camera_info", "camera_info")],
+ # scoped to namespace
+ ),
+ # Compressed republisher
+ # Node(
+ # package="image_transport",
+ # executable="republish",
+ # namespace=camera_name,
+ # name="republish_compressed",
+ # arguments=["raw", "compressed"],
+ # remappings=[("in", f"{video_device}/image_raw"), ("out", "image_raw/compressed")],
+ # ),
+ ]
+
+
+def generate_launch_description():
+ return LaunchDescription(
+ generate_camera_nodes("right", "/dev/video6", "mjpeg2rgb")
+ + generate_camera_nodes("left", "/dev/video8", "mjpeg2rgb")
+ + generate_camera_nodes("back", "/dev/video0", "mjpeg2rgb")
+ + generate_camera_nodes("digger", "/dev/video4")
+ + generate_camera_nodes("dumper", "/dev/video5")
+ # + generate_camera_nodes("front", "/dev/video0")
+ )
diff --git a/src/rovr_control/launch/main_launch.py b/src/rovr_control/launch/main_launch.py
index 68f5b822..061aeda4 100644
--- a/src/rovr_control/launch/main_launch.py
+++ b/src/rovr_control/launch/main_launch.py
@@ -38,11 +38,11 @@ def generate_launch_description():
emulate_tty=True,
)
- digger = Node(
- package="digger",
- executable="digger_node",
- name="digger_node",
- parameters=["config/motor_control.yaml"],
+ auger = Node(
+ package="auger",
+ executable="auger_node",
+ name="auger_node",
+ parameters=["config/auger_config.yaml"],
output="screen",
)
@@ -82,17 +82,30 @@ def generate_launch_description():
executable="auto_offload_server",
name="auto_offload_server",
)
+ dig_location_server = Node(
+ package="rovr_control",
+ executable="dig_location_server",
+ name="dig_location_server"
+ )
+
+ auto_dig_nav_offload_server = Node(
+ package="rovr_control",
+ executable="auto_dig_nav_offload_server",
+ name="auto_dig_nav_offload_server",
+ )
ld.add_action(rovr_control)
ld.add_action(motor_control)
ld.add_action(joystick_node)
ld.add_action(drivetrain)
- ld.add_action(digger)
+ ld.add_action(auger)
ld.add_action(dumper)
ld.add_action(read_serial)
ld.add_action(can_bus)
ld.add_action(calibrate_field_coordinate_server)
ld.add_action(auto_dig_server)
ld.add_action(auto_offload_server)
+ ld.add_action(dig_location_server)
+ ld.add_action(auto_dig_nav_offload_server)
return ld
diff --git a/src/rovr_control/launch/main_no_joysticks_launch.py b/src/rovr_control/launch/main_no_joysticks_launch.py
index b5fae349..abcff403 100644
--- a/src/rovr_control/launch/main_no_joysticks_launch.py
+++ b/src/rovr_control/launch/main_no_joysticks_launch.py
@@ -32,11 +32,11 @@ def generate_launch_description():
emulate_tty=True,
)
- digger = Node(
- package="digger",
- executable="digger_node",
- name="digger_node",
- parameters=["config/dumper_config.yaml", "config/motor_control.yaml"],
+ auger = Node(
+ package="auger",
+ executable="auger_node",
+ name="auger_node",
+ parameters=["config/auger_config.yaml"],
output="screen",
)
@@ -76,16 +76,28 @@ def generate_launch_description():
executable="auto_offload_server",
name="auto_offload_server",
)
+ dig_location_server = Node(
+ package="rovr_control",
+ executable="dig_location_server",
+ name="dig_location_server"
+ )
+ auto_dig_nav_offload_server = Node(
+ package="rovr_control",
+ executable="auto_dig_nav_offload_server",
+ name="auto_dig_nav_offload_server",
+ )
ld.add_action(rovr_control)
ld.add_action(motor_control)
ld.add_action(drivetrain)
- ld.add_action(digger)
+ ld.add_action(auger)
ld.add_action(dumper)
ld.add_action(read_serial)
ld.add_action(can_bus)
ld.add_action(calibrate_field_coordinate_server)
ld.add_action(auto_dig_server)
ld.add_action(auto_offload_server)
+ ld.add_action(dig_location_server)
+ ld.add_action(auto_dig_nav_offload_server)
return ld
diff --git a/src/rovr_control/resource/STREAMDECK_APRILTAG_DETECT.png b/src/rovr_control/resource/STREAMDECK_APRILTAG_DETECT.png
new file mode 100644
index 00000000..9d3b37b0
Binary files /dev/null and b/src/rovr_control/resource/STREAMDECK_APRILTAG_DETECT.png differ
diff --git a/src/rovr_control/resource/STREAMDECK_AUTO_DIG.png b/src/rovr_control/resource/STREAMDECK_AUTO_DIG.png
new file mode 100644
index 00000000..48ac94e2
Binary files /dev/null and b/src/rovr_control/resource/STREAMDECK_AUTO_DIG.png differ
diff --git a/src/rovr_control/resource/STREAMDECK_AUTO_DUMP.png b/src/rovr_control/resource/STREAMDECK_AUTO_DUMP.png
new file mode 100644
index 00000000..98806123
Binary files /dev/null and b/src/rovr_control/resource/STREAMDECK_AUTO_DUMP.png differ
diff --git a/src/rovr_control/resource/STREAMDECK_ESTOP.png b/src/rovr_control/resource/STREAMDECK_ESTOP.png
new file mode 100644
index 00000000..8bc83d46
Binary files /dev/null and b/src/rovr_control/resource/STREAMDECK_ESTOP.png differ
diff --git a/src/rovr_control/resource/STREAMDECK_GO_TO_DIG_SITE.png b/src/rovr_control/resource/STREAMDECK_GO_TO_DIG_SITE.png
new file mode 100644
index 00000000..15771676
Binary files /dev/null and b/src/rovr_control/resource/STREAMDECK_GO_TO_DIG_SITE.png differ
diff --git a/src/rovr_control/resource/STREAMDECK_START_AUTO.png b/src/rovr_control/resource/STREAMDECK_START_AUTO.png
new file mode 100644
index 00000000..dd53d1c9
Binary files /dev/null and b/src/rovr_control/resource/STREAMDECK_START_AUTO.png differ
diff --git a/src/rovr_control/resource/test.png b/src/rovr_control/resource/test.png
new file mode 100644
index 00000000..0179b695
Binary files /dev/null and b/src/rovr_control/resource/test.png differ
diff --git a/src/rovr_control/rovr_control/auto_dig_nav_offload_server.py b/src/rovr_control/rovr_control/auto_dig_nav_offload_server.py
new file mode 100644
index 00000000..ecdca7c0
--- /dev/null
+++ b/src/rovr_control/rovr_control/auto_dig_nav_offload_server.py
@@ -0,0 +1,201 @@
+import rclpy
+from rclpy.action import ActionServer, ActionClient, CancelResponse
+from rclpy.action.server import ServerGoalHandle
+from nav2_msgs.action import NavigateToPose
+from rovr_interfaces.action import AutoDig, AutoOffload, AutoDigNavOffload
+from rovr_control.node_util import AsyncNode
+from action_msgs.msg import GoalStatus
+from rclpy.action.client import ClientGoalHandle
+import math
+
+
+class AutoDigNavOffloadServer(AsyncNode):
+ def __init__(self):
+ super().__init__("auto_dig_nav_offload_server")
+
+ # Combined action server
+ self._action_server = ActionServer(
+ self,
+ AutoDigNavOffload,
+ "auto_dig_nav_offload",
+ self.execute_callback,
+ cancel_callback=self.cancel_callback,
+ )
+
+ # Sub‑actions: dig, backup, offload
+ self._auto_dig_client = ActionClient(self, AutoDig, "auto_dig")
+ self._backup_client = ActionClient(self, NavigateToPose, "navigate_to_pose")
+ self._auto_offload_client = ActionClient(self, AutoOffload, "auto_offload")
+
+ # Status tracking
+ self.dig_in_progress = False
+ self.backup_in_progress = False
+ self.offload_in_progress = False
+
+ # Handle tracking
+ self.dig_handle = ClientGoalHandle(None, None, None)
+ self.backup_handle = ClientGoalHandle(None, None, None)
+ self.offload_handle = ClientGoalHandle(None, None, None)
+
+ self.get_logger().info("Auto Dig-Backup-Offload server ready")
+
+ async def execute_callback(self, goal_handle: ServerGoalHandle):
+ result = AutoDigNavOffload.Result()
+
+ # 1. Dig
+ if not goal_handle.is_cancel_requested:
+ if not await self._do_auto_dig(goal_handle):
+ goal_handle.abort()
+ return result
+
+ # 2. Back up
+ if not goal_handle.is_cancel_requested:
+ # Don't fail the whole action if backup fails
+ await self._do_nav_backup(goal_handle)
+
+ # 3. Offload
+ if not goal_handle.is_cancel_requested:
+ if not await self._do_auto_offload(goal_handle):
+ goal_handle.abort()
+ return result
+
+ if not goal_handle.is_cancel_requested:
+ self.get_logger().info("Auto Dig Nav Offload Procedure Complete!")
+ goal_handle.succeed()
+ return result
+ else:
+ self.get_logger().info("Goal was cancelled")
+ goal_handle.canceled()
+ return result
+
+ async def _do_auto_dig(self, goal_handle):
+ self.get_logger().info("→ Starting AutoDig")
+ if not self._auto_dig_client.wait_for_server(timeout_sec=5.0):
+ self.get_logger().error("AutoDig server unavailable")
+ return False
+
+ self.get_logger().info("AutoDig server available")
+
+ if not goal_handle.is_cancel_requested:
+ self.get_logger().info("start dig")
+
+ self.dig_in_progress = True
+ dig_goal = AutoDig.Goal(
+ tilt_digging_start_position=goal_handle.request.tilt_digging_start_position,
+ digger_chain_power=goal_handle.request.digger_chain_power,
+ )
+ self.dig_handle = await self._auto_dig_client.send_goal_async(dig_goal)
+ if not self.dig_handle.accepted:
+ self.get_logger().error("AutoDig rejected")
+ self.dig_in_progress = False
+ return False
+ self.get_logger().info("AutoDig Goal Accepted")
+
+ await self.dig_handle.get_result_async()
+ self.dig_in_progress = False
+ self.get_logger().info("→ AutoDig complete")
+ return True
+
+ return False
+
+ def get_quat_from_euler(self, yaw):
+ return {
+ 'x': 0.0,
+ 'y': 0.0,
+ 'z': math.sin(yaw / 2.0),
+ 'w': math.cos(yaw / 2.0)
+ }
+
+ async def _do_nav_backup(self, goal_handle):
+ if not goal_handle.is_cancel_requested:
+ if not self._backup_client.wait_for_server(timeout_sec=1.0):
+ self.get_logger().error("Backup navigation service not available")
+ goal_handle.abort()
+ return False
+
+ self.backup_in_progress = True
+
+ goal_msg = NavigateToPose.Goal()
+ goal_msg.pose.header.frame_id = "map"
+ goal_msg.pose.header.stamp = self.get_clock().now().to_msg()
+ goal_msg.pose.pose.position.x = goal_handle.request.x_pos
+ goal_msg.pose.pose.position.y = goal_handle.request.y_pos
+
+ goal_quat = self.get_quat_from_euler(0.0) # Facing forward
+ goal_msg.pose.pose.orientation.x = goal_quat['x']
+ goal_msg.pose.pose.orientation.y = goal_quat['y']
+ goal_msg.pose.pose.orientation.z = goal_quat['z']
+ goal_msg.pose.pose.orientation.w = goal_quat['w']
+
+ self.get_logger().info("Sending backup goal")
+ send_goal_future = self._backup_client.send_goal_async(goal_msg)
+ send_goal_handle = await send_goal_future
+
+ if not send_goal_handle.accepted:
+ self.get_logger().error("BackUp rejected")
+ self.backup_in_progress = False
+ return False
+
+ self.get_logger().info("BackUp Goal Accepted")
+ get_result_future = send_goal_handle.get_result_async()
+ result = await get_result_future
+
+ status = result.status
+ if status == GoalStatus.STATUS_SUCCEEDED:
+ self.get_logger().info("→ BackUp succeeded")
+ self.backup_in_progress = False
+ return True
+ else:
+ self.get_logger().error(f"BackUp failed: {result}")
+ self.backup_in_progress = False
+ return False
+
+ return False
+
+ async def _do_auto_offload(self, goal_handle):
+ self.get_logger().info("→ Starting AutoOffload")
+ if not self._auto_offload_client.wait_for_server(timeout_sec=5.0):
+ self.get_logger().error("AutoOffload server unavailable")
+ return False
+
+ if not goal_handle.is_cancel_requested:
+ self.offload_in_progress = True
+ offload_goal = AutoOffload.Goal()
+ self.offload_handle = await self._auto_offload_client.send_goal_async(
+ offload_goal
+ )
+ if not self.offload_handle.accepted:
+ self.get_logger().error("AutoOffload rejected")
+ self.offload_in_progress = False
+ return False
+
+ await self.offload_handle.get_result_async()
+ self.offload_in_progress = False
+ self.get_logger().info("→ AutoOffload complete")
+ return True
+
+ def cancel_callback(self, cancel_request):
+ super().cancel_callback(cancel_request)
+ self.get_logger().info("→ Cancellation requested")
+ if self.dig_in_progress:
+ self.get_logger().info("Cancelling AutoDig")
+ self.dig_handle.cancel_goal_async() # cancel AutoDig
+ if self.backup_in_progress:
+ self.get_logger().info("Cancelling BackUp")
+ self._backup_client.cancel_all_goals() # cancel Nav2 backup
+ if self.offload_in_progress:
+ self.get_logger().info("Cancelling AutoOffload")
+ self.offload_handle.cancel_goal_async() # cancel AutoOffload
+ return CancelResponse.ACCEPT
+
+
+def main(args=None):
+ rclpy.init(args=args)
+ server = AutoDigNavOffloadServer()
+ rclpy.spin(server)
+ server.destroy_node()
+ rclpy.shutdown()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/src/rovr_control/rovr_control/auto_dig_server.py b/src/rovr_control/rovr_control/auto_dig_server.py
index 4fef0023..dc9c91c5 100644
--- a/src/rovr_control/rovr_control/auto_dig_server.py
+++ b/src/rovr_control/rovr_control/auto_dig_server.py
@@ -1,105 +1,143 @@
import rclpy
-from rclpy.action import ActionServer
-
-
+from rclpy.action import ActionServer, ActionClient
+from rovr_control.costmap_2d import PyCostmap2D
from rovr_interfaces.action import AutoDig
-from rovr_interfaces.srv import SetPosition, SetPower
+from rovr_interfaces.srv import SetExtension
+from rovr_interfaces.srv import AugerSetPushMotor, SetScrewMotorSpeed
from rclpy.action.server import ServerGoalHandle, CancelResponse
-from std_srvs.srv import Trigger
-
+from std_srvs.srv import Trigger, SetBool
+from action_msgs.msg import GoalStatus
from rovr_control.node_util import AsyncNode
+from nav2_msgs.action import BackUp
+from geometry_msgs.msg import Point
+from builtin_interfaces.msg import Duration
+from nav2_msgs.msg import OccupancyGrid
class AutoDigServer(AsyncNode):
def __init__(self):
super().__init__("auto_dig_server")
self._action_server = ActionServer(
- self, AutoDig, "auto_dig", self.execute_callback, cancel_callback=self.cancel_callback
+ self,
+ AutoDig,
+ "auto_dig",
+ self.execute_callback,
+ goal_callback=self.goal_callback,
+ handle_accepted_callback=self.handle_accepted_callback,
+ cancel_callback=self.cancel_callback,
+ )
+
+ self.latest_costmap = None
+
+ # Don't know the topic for the costmap yet or for alex blox
+ self.costmap_sub = self.create_subscription(
+ OccupancyGrid, "/global_costmap/costmap", self.costmap_callback, 10
)
- self.cli_lift_zero = self.create_client(Trigger, "lift/zero")
- self.cli_lift_bottom = self.create_client(Trigger, "lift/bottom")
- self.cli_lift_setPosition = self.create_client(SetPosition, "lift/setPosition")
- self.cli_lift_set_power = self.create_client(SetPower, "lift/setPower")
- self.cli_lift_stop = self.create_client(Trigger, "lift/stop")
+ # tilt
+ self.set_tilt = self.create_client(
+ SetExtension, "auger/tilt_actuator/setExtension"
+ ) # /actuator_tilt/setExtension
+ self.stop_tilt = self.create_client(
+ Trigger, "auger/tilt_actuator/stop"
+ ) # /actuator_tilt/stop
+
+ # extend
+ self.set_extension = self.create_client(AugerSetPushMotor, "auger/push_motor/setPosition")
+ self.stop_extension = self.create_client(Trigger, "auger/push_motor/stop")
+ self.retract_extender = self.create_client(Trigger, "auger/push_motor/retract")
+
+ # spin auger
+ self.screw_stop = self.create_client(Trigger, "auger/screw/stop") # /motor_spin/stop
+ self.screw_start = self.create_client(
+ SetScrewMotorSpeed, "auger/screw/run"
+ ) # /motor_spin/run
+
+ # agitator #TODO: uncomment if needed.
+ # self.agitator = self.create_client(SetBool, "motor_on_off")
+ # self.cli_motor_toggle = self.create_client(Trigger, "motor_toggle")
+
+ self._backup_client = ActionClient(self, BackUp, "backup")
+
+ self.declare_parameter("fast_screw_speed", 4000)
+ self.declare_parameter("slow_screw_speed", 2000)
- self.cli_digger_stop = self.create_client(Trigger, "digger/stop")
- self.cli_digger_setPower = self.create_client(SetPower, "digger/setPower")
+ self.spin_dig_speed = self.get_parameter("fast_screw_speed").value
+ self.spin_stow_speed = self.get_parameter("slow_screw_speed").value
+
+ def costmap_callback(self, msg):
+ self.latest_costmap = msg
+
+ def goal_callback(self, goal_request):
+ self.get_logger().info("Received goal request")
+ return GoalStatus.STATUS_ACCEPTED
+
+ def handle_accepted_callback(self, goal_handle):
+ self.get_logger().info("Starting new goal")
+ goal_handle.execute()
async def execute_callback(self, goal_handle: ServerGoalHandle):
self.get_logger().info("Starting Autonomous Digging Procedure!")
result = AutoDig.Result()
# Make sure the services are available
- if not self.cli_lift_setPosition.wait_for_service(timeout_sec=1.0):
- self.get_logger().error("Lift set position service not available")
+ if not self.set_tilt.wait_for_service(timeout_sec=1.0):
+ self.get_logger().error("Tilt set position service not available")
goal_handle.abort()
return result
- if not self.cli_lift_stop.wait_for_service(timeout_sec=1.0):
- self.get_logger().error("Lift stop service not available")
+ if not self.stop_tilt.wait_for_service(timeout_sec=1.0):
+ self.get_logger().error("Tilt stop service not available")
goal_handle.abort()
return result
- if not self.cli_lift_set_power.wait_for_service(timeout_sec=1.0):
- self.get_logger().error("Lift set power service not available")
+ if not self.set_extension.wait_for_service(timeout_sec=1.0):
+ self.get_logger().error("Extension set power service not available")
goal_handle.abort()
return result
- if not self.cli_lift_zero.wait_for_service(timeout_sec=1.0):
- self.get_logger().error("Lift zero service not available")
+ if not self.retract_extender.wait_for_service(timeout_sec=1.0):
+ self.get_logger().error("Extension retract service not available")
goal_handle.abort()
return result
- if not self.cli_lift_bottom.wait_for_service(timeout_sec=1.0):
- self.get_logger().error("Lift bottom service not available")
+ if not self.stop_extension.wait_for_service(timeout_sec=1.0):
+ self.get_logger().error("Extension stop service not available")
goal_handle.abort()
return result
- if not self.cli_digger_setPower.wait_for_service(timeout_sec=1.0):
- self.get_logger().error("Digger set power service not available")
+ if not self.screw_start.wait_for_service(timeout_sec=1.0):
+ self.get_logger().error("Auger set power service not available")
goal_handle.abort()
return result
- if not self.cli_digger_stop.wait_for_service(timeout_sec=1.0):
- self.get_logger().error("Digger stop service not available")
+ if not self.screw_stop.wait_for_service(timeout_sec=1.0):
+ self.get_logger().error("Auger stop service not available")
goal_handle.abort()
return result
+ # if not self.agitator.wait_for_service(timeout_sec=1.0):
+ # self.get_logger().error("Agitator motor on/off service not available")
+ # goal_handle.abort()
+ # return result
+ # if not self.cli_motor_toggle.wait_for_service(timeout_sec=1.0):
+ # self.get_logger().error("Agitator motor toggle service not available")
+ # goal_handle.abort()
+ # return result
- # Lower the digger so that it is just above the ground (get to this position fast)
+ # Tilt the auger into digging position
if not goal_handle.is_cancel_requested:
- self.get_logger().info("Lowering the digger to the starting position")
- await self.cli_lift_setPosition.call_async(
- SetPosition.Request(position=goal_handle.request.lift_digging_start_position)
- )
+ self.get_logger().info("Tilting the auger into digging position")
+ await self.set_tilt.call_async(SetExtension.Request(extension=True))
+ for i in range(4):
+ if not goal_handle.is_cancel_requested:
+ self.get_logger().info("Starting first dig")
+ await self.auto_dig(goal_handle)
- # Start the digger chain
- if not goal_handle.is_cancel_requested:
- self.get_logger().info("Starting the digger chain")
- await self.cli_digger_setPower.call_async(SetPower.Request(power=goal_handle.request.digger_chain_power))
+ if not goal_handle.is_cancel_requested:
+ self.get_logger().info("driving back")
+ await self._do_backup(goal_handle)
- # Lower the digger into the ground slowly
if not goal_handle.is_cancel_requested:
- self.get_logger().info("Lowering the digger into the ground")
- await self.cli_lift_bottom.call_async(Trigger.Request())
+ self.get_logger().info("stopping screw")
+ await self.screw_stop.call_async(Trigger.Request())
- # Stay at the lowest position for 5 seconds while digging
if not goal_handle.is_cancel_requested:
- self.get_logger().info("Start of Auto Digging in Place")
- await self.async_sleep(5)
- self.get_logger().info("Done Digging in Place")
-
- # Raise the digger so that it is just below the safety zone
- if not goal_handle.is_cancel_requested:
- self.get_logger().info("Raising the digger to the starting position")
- await self.cli_lift_setPosition.call_async(
- SetPosition.Request(position=goal_handle.request.lift_digging_start_position)
- )
-
- # Start the digger chain
- if not goal_handle.is_cancel_requested:
- self.get_logger().info("Starting the digger chain")
- await self.cli_digger_setPower.call_async(SetPower.Request(power=goal_handle.request.digger_chain_power))
- await self.async_sleep(10)
-
- # Raise the digger back up to the top using the lift
- if not goal_handle.is_cancel_requested:
- self.get_logger().info("Raising the digger up to the top")
- await self.cli_lift_zero.call_async(Trigger.Request())
+ self.get_logger().info("Resetting tilt")
+ await self.set_tilt.call_async(SetExtension.Request(extension=False))
if not goal_handle.is_cancel_requested:
self.get_logger().info("Autonomous Digging Procedure Complete!")
@@ -110,14 +148,221 @@ async def execute_callback(self, goal_handle: ServerGoalHandle):
goal_handle.canceled()
return result
- def cancel_callback(self, cancel_request: ServerGoalHandle):
+ async def cancel_callback(self, cancel_request: ServerGoalHandle):
"""This method is called when the action is canceled."""
super().cancel_callback(cancel_request)
self.get_logger().info("Goal is cancelling")
- self.cli_digger_stop.call_async(Trigger.Request())
+ self.screw_stop.call_async(Trigger.Request())
self.cli_lift_stop.call_async(Trigger.Request())
+ self.agitator.call_async(SetBool.Request(data=False))
return CancelResponse.ACCEPT
+ async def auto_dig(self, goal_handle: ServerGoalHandle):
+
+ self.spin_dig_speed = self.get_parameter("fast_screw_speed").value
+
+ if not goal_handle.is_cancel_requested:
+ self.get_logger().info("Starting screw")
+ await self.screw_start.call_async(
+ SetScrewMotorSpeed.Request(speed=self.spin_dig_speed)
+ )
+
+ fails = 0
+ max_fails = 4
+ self.goal_handle = goal_handle
+
+ # TODO: All of these numbers need to be tuned based on the actual robot
+ # and digging conditions.
+ fails += await self.set_position_retry(400.0, 0.12, max_fails - fails)
+
+ fails += await self.set_position_retry(475.0, 0.108, max_fails - fails)
+
+ fails += await self.set_position_retry(525.0, 0.098, max_fails - fails)
+
+ # if not goal_handle.is_cancel_requested and fails < max_fails:
+ # # Start the agitator motor
+ # self.get_logger().info("Starting Agitator Motor")
+ # await self.agitator.call_async(SetBool.Request(data=True))
+
+ fails += await self.set_position_retry(575.0, 0.098, max_fails - fails)
+
+ fails += await self.set_position_retry(650.0, 0.088, max_fails - fails)
+
+ fails += await self.set_position_retry(725.0, 0.098, max_fails - fails)
+
+ # fails += await self.set_position_retry(750.0, 0.105, max_fails-fails)
+
+ fails += await self.set_position_retry(800.0, 0.103, max_fails - fails)
+
+ # fails += await self.set_position_retry(850.0, 0.11, max_fails-fails)
+
+ fails += await self.set_position_retry(900.0, 0.108, max_fails - fails)
+
+ # Dig in place (no lift lowering) for 5 seconds
+ if not goal_handle.is_cancel_requested:
+ await self.screw_start.call_async(
+ SetScrewMotorSpeed.Request(speed=self.spin_dig_speed)
+ )
+ self.get_logger().info("Auto Digging in Place")
+ await self.async_sleep(5)
+ self.get_logger().info("Done Digging in Place")
+
+ # TODO: uncomment if using auger.
+ # if not goal_handle.is_cancel_requested:
+ # # Stop the agitator motor
+ # self.get_logger().info("Stopping Agitator Motor")
+ # await self.agitator.call_async(SetBool.Request(data=False))
+
+ # TODO: Uncomment if we decide to spin while raising.
+ # Raise the digger so that it is just below the safety zone
+ # if not goal_handle.is_cancel_requested:
+ # self.get_logger().info("Raising the digger to the starting position")
+ # await self.cli_lift_setPosition.call_async(
+ # SetPosition.Request(position=goal_handle.request.tilt_digging_start_position)
+ # )
+
+ # Start the digger chain
+ # if not goal_handle.is_cancel_requested:
+ # self.get_logger().info("Starting the digger chain")
+ # await self.screw_start.call_async(SetPower.Request(power=goal_handle.request.digger_chain_power))
+ # await self.async_sleep(5)
+
+ # Raise the digger back up to the top using the lift
+ if not goal_handle.is_cancel_requested:
+ self.get_logger().info("Raising the digger up to the top")
+ await self.retract_extender.call_async(Trigger.Request())
+
+ if not goal_handle.is_cancel_requested:
+ self.get_logger().info("Slowing the screw")
+ await self.screw_start.call_async(
+ SetScrewMotorSpeed.Request(speed=self.spin_stow_speed)
+ )
+
+ async def set_position_retry(self, position: float, power_limit: float, max_retries: int = 4):
+ self.get_logger().info("Starting the digger chain")
+ await self.screw_start.call_async(SetScrewMotorSpeed.Request(speed=self.spin_dig_speed))
+
+ for i in range(max_retries):
+ if not self.goal_handle.is_cancel_requested:
+ self.get_logger().info(
+ f"Attempting to set position to {position} with power limit {power_limit}"
+ )
+ if (
+ await self.cli_lift_setPosition.call_async(
+ AugerSetPushMotor.Request(position=position, speed=power_limit)
+ )
+ ).success:
+ self.get_logger().info(f"Successfully set position to {position}")
+ return i
+
+ if i == max_retries - 1:
+ break
+
+ await self.async_sleep(1)
+ await self.screw_start.call_async(
+ SetScrewMotorSpeed.Request(speed=self.spin_dig_speed)
+ )
+ await self.async_sleep(5)
+
+ else:
+ return -1
+
+ return max_retries
+
+ async def _do_backup(self, goal_handle):
+
+ dist = 0 # backup distance in meters, will be determined by costmap or behavior tree input
+
+ if self.latest_costmap is not None:
+ # --------------------------------------------------------------------------
+ costmap = PyCostmap2D(self.latest_costmap)
+ dist = 0.5
+ search_step = 0.1
+ max_search_distance = 1.0 # max distance to search for free space in costmap
+ found_free_space = False
+ while dist < max_search_distance:
+ # Check the cost at the current distance behind the robot
+ cost = costmap.getDigCost(
+ -dist, 0, 0.5, 0.5
+ ) # Checking the cost directly behind the robot
+ # parameters are placeholders and should be tuned
+ # based on robot size and costmap resolution
+ if (
+ cost == 0
+ ): # Assuming 0 means free space, may need to be adjusted based on costmap config
+ self.get_logger().info(
+ f"Found free space in costmap at distance {dist}m, backing up"
+ )
+ found_free_space = True
+ dist += search_step
+ break
+
+ if not found_free_space:
+ self.get_logger().warn(
+ f"No free space found in costmap within {max_search_distance}m, using default backup distance"
+ )
+ dist = 0.5 # Place holder value until decided
+ # -------------------------------------------------------------------------
+ else:
+ # --------------------------------------------------------------------------
+ self.get_logger().warn(
+ "No costmap received yet, using behavior tree input backup distance"
+ )
+
+ dist = goal_handle.request.backup_distance
+ if dist <= 0:
+ self.get_logger().info("No backup distance specified, going to default backup")
+ dist = 0.5 # Place holder value until decided
+
+ else:
+ self.get_logger().info(f"Backing up {dist} meters")
+ # --------------------------------------------------------------------------
+
+ if not goal_handle.is_cancel_requested:
+ speed = 0.5 # duty cycle
+ timeout = 9.0 # seconds
+ self.get_logger().info(f"→ Backing up {dist}m @ {speed} (duty cycle)")
+
+ if not self._backup_client.wait_for_server(timeout_sec=5.0):
+ self.get_logger().error("BackUp server unavailable")
+ return False
+
+ target_point = Point()
+ target_point.x = -abs(dist) # negative x = backward
+ target_point.y = 0.0
+ target_point.z = 0.0
+
+ self.backup_in_progress = True
+ backup_goal = BackUp.Goal(
+ speed=speed,
+ target=target_point,
+ time_allowance=Duration(sec=int(timeout)),
+ )
+ send = await self._backup_client.send_goal_async(backup_goal)
+ if not send.accepted:
+ self.get_logger().error("BackUp rejected")
+ self.backup_in_progress = False
+ return False
+
+ # Wait for completion or cancel
+ result_future = send.get_result_async()
+ while not result_future.done():
+ if goal_handle.is_cancel_requested:
+ self._backup_client.cancel_goal_async(send) # ask Nav2 to stop
+ self.get_logger().info("BackUp canceled")
+ self.backup_in_progress = False
+ return False
+ await self.async_sleep(0.1)
+
+ result = await result_future
+ self.backup_in_progress = False
+ if result.status == GoalStatus.STATUS_SUCCEEDED:
+ self.get_logger().info("→ BackUp succeeded")
+ return True
+ else:
+ self.get_logger().error(f"BackUp failed: {result}")
+ return False
+
def main(args=None) -> None:
rclpy.init(args=args)
diff --git a/src/rovr_control/rovr_control/auto_offload_server.py b/src/rovr_control/rovr_control/auto_offload_server.py
index 53145171..6f933bbc 100644
--- a/src/rovr_control/rovr_control/auto_offload_server.py
+++ b/src/rovr_control/rovr_control/auto_offload_server.py
@@ -4,7 +4,7 @@
from rovr_interfaces.action import AutoOffload
from std_srvs.srv import Trigger
-
+from action_msgs.msg import GoalStatus
from rovr_control.node_util import AsyncNode
@@ -19,47 +19,53 @@ def __init__(self):
cancel_callback=self.cancel_callback,
)
- self.cli_dumper_extend = self.create_client(Trigger, "dumper/extendDumper")
- self.cli_dumper_retract = self.create_client(Trigger, "dumper/retractDumper")
+ self.cli_dumper_dump = self.create_client(Trigger, "dumper/dumpDumper")
+ self.cli_dumper_store = self.create_client(Trigger, "dumper/storeDumper")
self.cli_dumper_stop = self.create_client(Trigger, "dumper/stop")
async def execute_callback(self, goal_handle: ServerGoalHandle):
"""This method lays out the procedure for autonomously offloading!"""
self.get_logger().info("Starting Autonomous Offload Procedure!")
result = AutoOffload.Result()
+ result.success = False
# Make sure the services are available
- if not self.cli_dumper_extend.wait_for_service(timeout_sec=1.0):
- self.get_logger().error("Dumper extend service not available")
+ if not self.cli_dumper_dump.wait_for_service(timeout_sec=1.0):
+ self.get_logger().error("Dumper dump service not available")
goal_handle.abort()
+ result.success = False
return result
- if not self.cli_dumper_retract.wait_for_service(timeout_sec=1.0):
- self.get_logger().error("Dumper retract service not available")
+ if not self.cli_dumper_store.wait_for_service(timeout_sec=1.0):
+ self.get_logger().error("Dumper store service not available")
goal_handle.abort()
+ result.success = False
return result
if not self.cli_dumper_stop.wait_for_service(timeout_sec=1.0):
self.get_logger().error("Dumper stop service not available")
goal_handle.abort()
+ result.success = False
return result
# Dump the material
if not goal_handle.is_cancel_requested:
self.get_logger().info("Auto Dumping")
- await self.cli_dumper_extend.call_async(Trigger.Request())
+ await self.cli_dumper_dump.call_async(Trigger.Request())
if not goal_handle.is_cancel_requested:
- # wait for 5 seconds before retracting the dumper
- await self.async_sleep(5) # Allows for task to be canceled
+ # wait for 2 seconds before storing the dumper
+ await self.async_sleep(2) # Allows for task to be canceled
if not goal_handle.is_cancel_requested:
- # retract the dumper
- await self.cli_dumper_retract.call_async(Trigger.Request())
+ # store the dumper
+ await self.cli_dumper_store.call_async(Trigger.Request())
if not goal_handle.is_cancel_requested:
self.get_logger().info("Autonomous Offload Procedure Complete!")
goal_handle.succeed()
+ result.success = True
return result
else:
self.get_logger().info("Goal was cancelled")
goal_handle.canceled()
+ result.success = False
return result
def cancel_callback(self, cancel_request: ServerGoalHandle):
@@ -69,6 +75,9 @@ def cancel_callback(self, cancel_request: ServerGoalHandle):
self.cli_dumper_stop.call_async(Trigger.Request())
return CancelResponse.ACCEPT
+ def goal_callback(self, goal_request):
+ self.get_logger().info("Received goal request to autonomously offload")
+ return GoalStatus.STATUS_ACCEPTED
def main(args=None) -> None:
rclpy.init(args=args)
diff --git a/src/rovr_control/rovr_control/calibrate_field_coordinate_server.py b/src/rovr_control/rovr_control/calibrate_field_coordinate_server.py
index f6881f5d..b1391053 100644
--- a/src/rovr_control/rovr_control/calibrate_field_coordinate_server.py
+++ b/src/rovr_control/rovr_control/calibrate_field_coordinate_server.py
@@ -46,30 +46,39 @@ async def execute_callback(self, goal_handle: ServerGoalHandle):
# Spin around to find the apriltag
spin_goal = Spin.Goal(target_yaw=2 * math.pi)
- self.spin_handle: ClientGoalHandle = await self.cli_spin.send_goal_async(spin_goal)
+ self.spin_handle: ClientGoalHandle = await self.cli_spin.send_goal_async(
+ spin_goal
+ )
future_spin: Future = self.spin_handle.get_result_async()
self.future_odom = Future()
while not future_spin.done():
- self.future_odom = self.cli_set_apriltag_odometry.call_async(Trigger.Request())
+ self.future_odom = self.cli_set_apriltag_odometry.call_async(
+ Trigger.Request()
+ )
if (await self.future_odom).success is True:
- self.get_logger().info("Found an apriltag!")
+ self.get_logger().info("Found an apriltag!", throttle_duration_sec=5)
await self.spin_handle.cancel_goal_async()
if self.spin_handle.status == GoalStatus.STATUS_SUCCEEDED:
self.get_logger().warn("Failed to find an apriltag")
goal_handle.abort()
+ result.success = False
return result
if not self.cli_spin.wait_for_server(timeout_sec=1.0):
self.get_logger().error("Apriltag odom service not available")
goal_handle.abort()
+ result.success = False
return result
spin_goal = Spin.Goal(target_yaw=math.pi)
- self.spin_handle: ClientGoalHandle = await self.cli_spin.send_goal_async(spin_goal)
+ self.spin_handle: ClientGoalHandle = await self.cli_spin.send_goal_async(
+ spin_goal
+ )
await self.spin_handle.get_result_async()
goal_handle.succeed()
+ result.success = True
return result
def cancel_callback(self, cancel_request: ServerGoalHandle):
diff --git a/src/rovr_control/rovr_control/controls.txt b/src/rovr_control/rovr_control/controls.txt
new file mode 100644
index 00000000..9b01c4f2
--- /dev/null
+++ b/src/rovr_control/rovr_control/controls.txt
@@ -0,0 +1,11 @@
+X Button: Toggle Digger Belt Power +
+B Button: Toggle dumper
+A Button: Calibrate Field Coordinates
+
+Right Bumper: dumper set power +
+Left Bumper: dumper set power -
+Right Trigger: digger lift set power +
+Left Trigger: digger lift set power -
+
+Start Button: Auto Dig
+Back Button: Auto Offload
diff --git a/src/rovr_control/rovr_control/coord_return_server.py b/src/rovr_control/rovr_control/coord_return_server.py
new file mode 100644
index 00000000..ab847ff3
--- /dev/null
+++ b/src/rovr_control/rovr_control/coord_return_server.py
@@ -0,0 +1,127 @@
+import rclpy
+from rclpy.action import ActionServer, ActionClient, GoalResponse
+from rovr_interfaces.action import ReturnToCoordinate
+
+# This is the server that will return the robot to a specified coordinate.
+# It will be used in the auto dig-nav-offload action to return the robot
+# to the dig location after offloading.
+from rclpy.action.server import ServerGoalHandle, CancelResponse
+from action_msgs.msg import GoalStatus
+from rovr_control.node_util import AsyncNode
+from nav2_msgs.action import NavigateToPose
+from rclpy.action.client import ClientGoalHandle
+from nav2_msgs.msg import OccupancyGrid
+import math
+
+
+class ReturnToCoordinateServer(AsyncNode):
+ def __init__(self):
+ super().__init__("return_to_coordinate_server")
+
+ self._action_server = ActionServer(
+ self,
+ ReturnToCoordinate,
+ "return_to_coordinate",
+ self.execute_callback,
+ goal_callback=self.goal_callback,
+ cancel_callback=self.cancel_callback,
+ handle_accepted_callback=self.handle_accepted_callback,
+ # handle_accepted_callback is a function that is called when a goal is accepted,
+ # it is used to start the execution of the goal in a new thread.
+ )
+
+ self.backup_in_progress = False
+
+ self.coord_return_client = ActionClient(self, NavigateToPose, "navigate_to_pose")
+ self.coord_return_handle = ClientGoalHandle(None, None, None)
+
+ def goal_callback(self, goal_request):
+ self.get_logger().info(
+ "Received goal request to return to coordinate ({}, {})".format(
+ goal_request.x, goal_request.y
+ )
+ )
+ return GoalResponse.ACCEPT
+
+ def handle_accepted_callback(self, goal_handle):
+ self.get_logger().info("Accepted goal to return to coordinate, starting execution")
+ goal_handle.execute()
+
+ def cancel_callback(self, goal_handle):
+
+ super().cancel_callback(goal_handle)
+ self.get_logger().info("Received request to cancel return to coordinate action")
+
+ if self.coord_return_handle:
+ if self.coord_return_handle.status == GoalStatus.STATUS_EXECUTING:
+ self.get_logger().info("Cancelling return to coordinate action")
+ self.coord_return_handle.cancel_goal_async()
+
+ if not self.coord_return_handle.is_done():
+ self.get_logger().info("Cancelling return to coordinate action")
+ self.coord_return_handle.cancel_goal_async()
+
+ if self.coord_return_handle.status == GoalStatus.STATUS_CANCELED:
+ self.get_logger().info("Return to coordinate action cancelled successfully")
+ return CancelResponse.ACCEPT
+
+ def get_quat_from_euler(self, yaw):
+ return {"x": 0.0, "y": 0.0, "z": math.sin(yaw / 2.0), "w": math.cos(yaw / 2.0)}
+
+ async def execute_callback(self, goal_handle: ServerGoalHandle):
+ # result = ReturnToCoordinate.Result() still need to define result
+ # using boolean for now, can be expanded later if needed
+ result = ReturnToCoordinate.Result()
+ target_x = goal_handle.request.x_pos
+ target_y = goal_handle.request.y_pos
+ self.backup_in_progress = True
+
+ if not self.coord_return_client.wait_for_server(timeout_sec=1.0):
+ self.get_logger().error("Navigate to pose action server not available")
+ goal_handle.abort()
+ result.success = False
+ return result
+
+ goal_msg = NavigateToPose.Goal()
+ goal_msg.pose.header.frame_id = "map"
+ goal_msg.pose.header.stamp = self.get_clock().now().to_msg()
+ goal_msg.pose.pose.position.x = target_x
+ goal_msg.pose.pose.position.y = target_y
+
+ goal_quat = self.get_quat_from_euler(0.0) # Facing forward
+ goal_msg.pose.pose.orientation.x = goal_quat["x"]
+ goal_msg.pose.pose.orientation.y = goal_quat["y"]
+ goal_msg.pose.pose.orientation.z = goal_quat["z"]
+ goal_msg.pose.pose.orientation.w = goal_quat["w"]
+
+ intended_location = self.coord_return_client.send_goal_async(goal_msg)
+ self.coord_return_handle = await intended_location
+
+ if not self.coord_return_handle.accepted:
+ self.get_logger().error("BackUp rejected")
+ self.backup_in_progress = False
+ goal_handle.abort()
+ result.success = False
+ return result
+
+ self.get_logger().info("BackUp Goal Accepted")
+ get_intended_location = self.coord_return_handle.get_result_async()
+ intended_location_result = await get_intended_location
+
+ intended_location_status = intended_location_result.status
+
+ if intended_location_status == GoalStatus.STATUS_SUCCEEDED:
+ self.get_logger().info("→ BackUp succeeded")
+ goal_handle.succeed()
+ self.backup_in_progress = False
+ result.success = True
+ return result
+
+ elif intended_location_status == GoalStatus.STATUS_CANCELED:
+ self.get_logger().error("BackUp Failed")
+ goal_handle.canceled()
+ self.backup_in_progress = False
+ result.success = False
+ return result
+
+ return result
diff --git a/src/rovr_control/rovr_control/costmap_2d.py b/src/rovr_control/rovr_control/costmap_2d.py
index a8f9612b..8ff55ca3 100644
--- a/src/rovr_control/rovr_control/costmap_2d.py
+++ b/src/rovr_control/rovr_control/costmap_2d.py
@@ -55,7 +55,9 @@ def __init__(self, costmap):
self.global_frame_id = costmap.header.frame_id
self.costmap_timestamp = costmap.header.stamp
# Extract costmap
- self.costmap = np.array(costmap.data, dtype=np.uint8).reshape(self.size_x, self.size_y)
+ self.costmap = np.array(costmap.data, dtype=np.uint8).reshape(
+ self.size_x, self.size_y
+ )
def getSizeInCellsX(self):
"""Get map width in cells."""
diff --git a/src/rovr_control/rovr_control/dig_location_server.py b/src/rovr_control/rovr_control/dig_location_server.py
index 023e9997..cabbc519 100644
--- a/src/rovr_control/rovr_control/dig_location_server.py
+++ b/src/rovr_control/rovr_control/dig_location_server.py
@@ -1,5 +1,5 @@
import rclpy
-from rclpy.action import ActionClient, ActionServer
+from rclpy.action import ActionClient, ActionServer, CancelResponse
from rclpy.node import Node
from rovr_control.costmap_2d import PyCostmap2D
from rovr_interfaces.srv import DigLocation
@@ -8,73 +8,139 @@
from nav2_msgs.action import NavigateToPose
from nav2_msgs.srv import GetCostmap
from nav_msgs.msg import OccupancyGrid
+from action_msgs.msg import GoalStatus
+from rclpy.action.client import ClientGoalHandle
+from rclpy.action.server import ServerGoalHandle
import math
from geometry_msgs.msg import PolygonStamped, PoseStamped
+from nav_msgs.msg import OccupancyGrid
class DigLocationFinder(Node):
def __init__(self):
super().__init__("dig_location_server")
+ self.latest_costmap = None
+
+ # Don't know the topic for the costmap yet or for alex blox
+ self.costmap_sub = self.create_subscription(
+ OccupancyGrid,
+ "/global_costmap/costmap",
+ self.costmap_callback,
+ 10,
+ )
self._action_server = ActionServer(
self,
GoToDigLocation, # Empty action message
"go_to_dig_location",
self.drive_to_dig_location,
# cancel_callback=self.drive_to_dig_location,
- # TODO: Make a cancel callback that actually cancels all running futures please
+ # TODO: Make a cancel callback that actually cancels all running
+ # futures please
)
self.nav2_client = ActionClient(self, NavigateToPose, "navigate_to_pose")
- self.get_costmap_global_srv = self.create_client(
- GetCostmap, 'global_costmap/get_costmap'
+ self.get_costmap_global_srv = self.create_client(GetCostmap, "global_costmap/get_costmap")
+ self.srv = self.create_service(
+ DigLocation, "find_dig_location", self.find_dig_location_callback
)
- self.srv = self.create_service(DigLocation, "find_dig_location", self.find_dig_location_callback)
self.footprint_sub = self.create_subscription(
- PolygonStamped, "/local_costmap/published_footprint", self.get_robot_footprint, 10
+ PolygonStamped,
+ "/local_costmap/published_footprint",
+ self.get_robot_footprint,
+ 10,
)
self.footprint = (1.2, 0.75)
self.absolute_max_dig_cost = self.declare_parameter("absolute_max_dig_cost", 200).value
self.max_dig_cost = self.declare_parameter("max_dig_cost", 100).value
self.all_dig_locations = self.declare_parameter(
- "all_dig_locations", [.6, .37, .6, 1.1, .6, 1.83, 1.8, .37, 1.8, 1.1, 1.8, 1.83, 3.0,
- .37, 3.0, 1.1, 3.0, 1.83, .6, 2.5, 1.8, 2.5, 3.0, 2.5]
+ "all_dig_locations",
+ [
+ 0.6,
+ 0.37,
+ 0.6,
+ 1.1,
+ 0.6,
+ 1.83,
+ 1.8,
+ 0.37,
+ 1.8,
+ 1.1,
+ 1.8,
+ 1.83,
+ 3.0,
+ 0.37,
+ 3.0,
+ 1.1,
+ 3.0,
+ 1.83,
+ 0.6,
+ 2.5,
+ 1.8,
+ 2.5,
+ 3.0,
+ 2.5,
+ ],
).value # If you default to an empty list things break (it thinks its a byte array)
- # ROS doesn't like nested lists, so the config file has to be flattened. This unflattens that list
+ # ROS doesn't like nested lists, so the config file has to be
+ # flattened. This unflattens that list
self.all_dig_locations = [
- (self.all_dig_locations[i], self.all_dig_locations[i + 1]) for i in range(0, len(self.all_dig_locations), 2)
+ (self.all_dig_locations[i], self.all_dig_locations[i + 1])
+ for i in range(0, len(self.all_dig_locations), 2)
]
+
self.potential_dig_locations = self.all_dig_locations.copy()
- async def drive_to_dig_location(self, goal_handle):
+ self.nav_handle = ClientGoalHandle(None, None, None)
+
+ async def drive_to_dig_location(self, goal_handle: ServerGoalHandle):
result = GoToDigLocation.Result()
- goal_pose_xy = self.getDigLocation()
+ # Behavior tree input will go here if it isn't automatic
+ target_x = goal_handle.request.x
+ target_y = goal_handle.request.y
+
+ goal_pose_xy = None
+ if target_x == 0.0 and target_y == 0.0:
+ self.get_logger().info("No target provided, finding dig location")
+ goal_pose_xy = self.getDigLocation()
+ else:
+ goal_pose_xy = (target_x, target_y)
+
+ self.get_logger().info(f"Dig location: {goal_pose_xy[0]}, {goal_pose_xy[1]}")
if goal_pose_xy is None:
- goal_handle.abort()
- self.get_logger().warn("goal_pose_xy is None")
- return result
+ self.get_logger().error("goal_pose_xy is None")
nav_goal = self.get_goal_pose(goal_pose_xy[0], goal_pose_xy[1], math.pi)
- send_goal_future = self.nav2_client.send_goal_async(nav_goal)
- goal_response = await send_goal_future
- if not goal_response.accepted:
- self.get_logger().error("Goal rejected")
+ # send_goal_future = self.nav2_client.send_goal_async(nav_goal)
+
+ self.nav_handle: ClientGoalHandle = await self.nav2_client.send_goal_async(nav_goal)
+ # await self.nav_handle.get_result_async()
+ await self.nav_handle.get_result_async()
+
+ if self.nav_handle.status == GoalStatus.STATUS_SUCCEEDED:
+ self.get_logger().info("Nav2 succeeded")
+ goal_handle.succeed()
+ elif self.nav_handle.status == GoalStatus.STATUS_CANCELED:
+ self.get_logger().error("Nav2 canceled")
+ goal_handle.canceled()
+ else:
+ self.get_logger().error(f"Nav2 completed in unknown state {self.nav_handle.status}")
goal_handle.abort()
- return result
- result_future = goal_response.get_result_async()
+ return result
- await result_future
+ def costmap_callback(self, msg):
+ self.latest_costmap = msg
- # result = result_future.result()
- if result and result.status == 4: # STATUS_SUCCEEDED (4)
- self.get_logger().info("Navigation succeeded!")
- return result
- else:
- self.get_logger().error("Navigation failed!")
- return result
+ def cancel_callback(self, cancel_request: ServerGoalHandle):
+ self.get_logger().info("Cancelling drive to dig location")
+ if self.nav_handle.status == GoalStatus.STATUS_EXECUTING:
+ self.nav_handle.cancel_goal_async()
+ if not self.nav_handle.is_done():
+ self.nav_handle.cancel_goal_async()
+ return CancelResponse.ACCEPT
# yaw in rads.
# yaw = 0 at x axis, positive is counter clockwise
@@ -90,9 +156,7 @@ def get_goal_pose(self, x, y, yaw):
goal_msg.pose.pose.position.x = x
goal_msg.pose.pose.position.y = y
- quat = R.from_euler(
- "xyz", [float(0), float(0), float(yaw)], degrees=False
- ).as_quat()
+ quat = R.from_euler("xyz", [float(0), float(0), float(yaw)], degrees=False).as_quat()
goal_msg.pose.pose.orientation.x = quat[0]
goal_msg.pose.pose.orientation.y = quat[1]
@@ -119,49 +183,68 @@ def find_dig_location_callback(self, request, response):
response.success = False
return response
- response.success = True
+ # response.success = True
response.x = coords[0]
response.y = coords[1]
+
+ self.get_logger().info(f"Dig location: {coords[0]}, {coords[1]}")
+
return response
def updatePotentialDigLocations(self):
+ safe_locations = []
+ if self.latest_costmap is None:
+ self.get_logger().warn("Latest costmap is None, cannot update potential dig locations")
+ return
+
try:
- costmap = PyCostmap2D(self.getGlobalCostmap())
- robot_width, robot_height = (0.5, 0.5)
+ costmap = PyCostmap2D(self.latest_costmap)
+ robot_width, robot_height = (0.5, 0.5) # Are dimensions still correct?
for location in self.potential_dig_locations:
# dig_cost = maximum cost of the cells that the robot will dig
dig_cost = costmap.getDigCost(location[0], location[1], robot_width, robot_height)
- if dig_cost >= self.max_dig_cost:
- self.potential_dig_locations.remove(location)
+ if dig_cost < self.max_dig_cost:
+ safe_locations.append(location)
+
+ self.potential_dig_locations = safe_locations
except Exception as e:
self.get_logger().error(f"Error in updatePotentialDigLocations {e}")
+ '''
+ Old Global Cost Map Function, may be useful for future reference but currently not being used.
+ Subscribe to costmap topic with Alex Blox.
+
def getGlobalCostmap(self) -> OccupancyGrid:
"""Get the global costmap."""
while not self.get_costmap_global_srv.wait_for_service(timeout_sec=1.0):
- self.info('Get global costmaps service not available, waiting...')
+ self.info("Get global costmaps service not available, waiting...")
req = GetCostmap.Request()
future = self.get_costmap_global_srv.call_async(req)
rclpy.spin_until_future_complete(self, future)
result = future.result()
if result is None:
- self.error('Get global costmap request failed!')
+ self.error("Get global costmap request failed!")
return None
return result.map
+ '''
def getDigLocation(self):
- # self.updatePotentialDigLocations()
+ self.updatePotentialDigLocations() # Uncommented for automation
+ self.get_logger().info(f"Checking potential dig locations: {self.potential_dig_locations}")
+
# If there are no potential dig locations, reset the potential dig locations
- # and increase the max dig cost if the max dig cost is > absolute max dig cost, return None
+ # and increase the max dig cost if the max dig cost is > absolute max
+ # dig cost, return None
if len(self.potential_dig_locations) == 0:
self.potential_dig_locations = self.all_dig_locations.copy()
self.max_dig_cost += 10
if self.max_dig_cost >= self.absolute_max_dig_cost:
return None
return self.getDigLocation()
+
return self.potential_dig_locations[0]
@@ -171,3 +254,7 @@ def main(args=None):
rclpy.spin(dig_location_finder)
dig_location_finder.destroy_node()
rclpy.shutdown()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/src/rovr_control/rovr_control/gamepad_constants.py b/src/rovr_control/rovr_control/gamepad_constants.py
index 481ff5c6..a12f130f 100644
--- a/src/rovr_control/rovr_control/gamepad_constants.py
+++ b/src/rovr_control/rovr_control/gamepad_constants.py
@@ -17,3 +17,11 @@
LEFT_JOYSTICK_VERTICAL_AXIS = 1
RIGHT_JOYSTICK_HORIZONTAL_AXIS = 2
RIGHT_JOYSTICK_VERTICAL_AXIS = 3
+
+# Define streamdeck buttons here
+STREAMDECK_START_AUTO = 0
+STREAMDECK_AUTO_DIG = 1
+STREAMDECK_AUTO_DUMP = 2
+STREAMDECK_APRILTAG_DETECT = 3
+STREAMDECK_GO_TO_DIG_SITE = 4
+STREAMDECK_ESTOP = 5
diff --git a/src/rovr_control/rovr_control/main_control_node.py b/src/rovr_control/rovr_control/main_control_node.py
index 3b04d4ba..a3b7f592 100644
--- a/src/rovr_control/rovr_control/main_control_node.py
+++ b/src/rovr_control/rovr_control/main_control_node.py
@@ -4,9 +4,12 @@
# Last Updated: November 2023
+import time
+
# Import the ROS 2 module
import rclpy
-import time
+
+# Import ROS 2 formatted message types
from rclpy.action import ActionClient
from rclpy.action.client import ClientGoalHandle
from rclpy.callback_groups import ReentrantCallbackGroup
@@ -20,21 +23,35 @@
from std_msgs.msg import Float32
# Import custom ROS 2 interfaces
-from rovr_interfaces.srv import SetPower, SetPosition
-from rovr_interfaces.action import CalibrateFieldCoordinates, AutoDig, AutoOffload
-from std_srvs.srv import Trigger
# Import Python Modules
from scipy.spatial.transform import Rotation as R
+from std_srvs.srv import SetBool, Trigger
# Import our logitech gamepad button mappings
from rovr_control import gamepad_constants as bindings
+from rovr_interfaces.action import (
+ AutoDig,
+ AutoDigNavOffload,
+ AutoOffload,
+ CalibrateFieldCoordinates,
+ BehaviorTreeExecutor,
+ ReturnToCoordinate,
+ CancelAction,
+)
+from rovr_interfaces.msg import StreamDeckState
+
+# Import custom ROS 2 interfaces
+from rovr_interfaces.srv import SetPower, SetScrewMotorSpeed
# Uncomment the line below to use the Xbox controller mappings instead
# from rovr_control import xbox_controller_constants as bindings
# GLOBAL VARIABLES #
buttons = [0] * 11 # This is to help with button press detection
+# To track previous states of streamdeck buttons
+old_streamdeck_buttons = [False] * 6
+
# Define the possible states of our robot
states = {"Teleop": 0, "Autonomous": 1}
@@ -67,14 +84,28 @@ def __init__(self) -> None:
super().__init__("rovr_control")
# Define default values for our ROS parameters below #
- self.declare_parameter("max_drive_power", 1.0) # Measured in Duty Cycle (0.0-1.0)
- self.declare_parameter("max_turn_power", 1.0) # Measured in Duty Cycle (0.0-1.0)
- self.declare_parameter("digger_chain_power", 0.2) # Measured in Duty Cycle (0.0-1.0)
- self.declare_parameter("digger_lift_manual_power_down", 0.12) # Measured in Duty Cycle (0.0-1.0)
- self.declare_parameter("digger_lift_manual_power_up", 0.5) # Measured in Duty Cycle (0.0-1.0)
- self.declare_parameter("lift_digging_start_position", 125.0) # Measured in encoder counts
- self.declare_parameter("DIGGER_SAFETY_ZONE", 120) # Measured in potentiometer units (0 to 1023)
- self.declare_parameter("dumper_power", 0.75) # The power the dumper needs to go
+ # Measured in Duty Cycle (0.0-1.0)
+ self.declare_parameter("max_drive_power", 1.0)
+ # Measured in Duty Cycle (0.0-1.0)
+ self.declare_parameter("max_turn_power", 1.0)
+ # Measured in Duty Cycle (0.0-1.0)
+ self.declare_parameter("digger_chain_power", 0.18)
+ self.declare_parameter(
+ "digger_lift_manual_power_down", 0.12
+ ) # Measured in Duty Cycle (0.0-1.0)
+ self.declare_parameter(
+ "digger_lift_manual_power_up", 0.5
+ ) # Measured in Duty Cycle (0.0-1.0)
+ self.declare_parameter(
+ "tilt_digging_start_position", 125.0
+ ) # Measured in encoder counts
+ self.declare_parameter(
+ "fast_screw_speed", 4000
+ )
+ # Measured in potentiometer units (0 to 1023)
+ self.declare_parameter("DIGGER_SAFETY_ZONE", 120)
+ # The power the dumper needs to go
+ self.declare_parameter("dumper_power", 0.75)
# The type of field ("cosmic", "top", "bottom", "nasa")
self.declare_parameter("autonomous_field_type", "cosmic")
@@ -82,25 +113,51 @@ def __init__(self) -> None:
self.max_drive_power = self.get_parameter("max_drive_power").value
self.max_turn_power = self.get_parameter("max_turn_power").value
self.digger_chain_power = self.get_parameter("digger_chain_power").value
- self.digger_lift_manual_power_down = self.get_parameter("digger_lift_manual_power_down").value
- self.digger_lift_manual_power_up = self.get_parameter("digger_lift_manual_power_up").value
+ self.digger_lift_manual_power_down = self.get_parameter(
+ "digger_lift_manual_power_down"
+ ).value
+ self.digger_lift_manual_power_up = self.get_parameter(
+ "digger_lift_manual_power_up"
+ ).value
self.autonomous_field_type = self.get_parameter("autonomous_field_type").value
- self.lift_digging_start_position = self.get_parameter("lift_digging_start_position").value
+ self.tilt_digging_start_position = self.get_parameter(
+ "tilt_digging_start_position"
+ ).value
+ self.screw_speed = self.get_parameter("fast_screw_speed").value
self.dumper_power = self.get_parameter("dumper_power").value
self.DIGGER_SAFETY_ZONE = self.get_parameter("DIGGER_SAFETY_ZONE").value
# Print the ROS Parameters to the terminal below #
- self.get_logger().info("max_drive_power has been set to: " + str(self.max_drive_power))
- self.get_logger().info("max_turn_power has been set to: " + str(self.max_turn_power))
- self.get_logger().info("digger_chain_power has been set to: " + str(self.digger_chain_power))
self.get_logger().info(
- "digger_lift_manual_power_down has been set to: " + str(self.digger_lift_manual_power_down)
+ "max_drive_power has been set to: " + str(self.max_drive_power)
+ )
+ self.get_logger().info(
+ "max_turn_power has been set to: " + str(self.max_turn_power)
+ )
+ self.get_logger().info(
+ "digger_chain_power has been set to: " + str(self.digger_chain_power)
+ )
+ self.get_logger().info(
+ "digger_lift_manual_power_down has been set to: "
+ + str(self.digger_lift_manual_power_down)
+ )
+ self.get_logger().info(
+ "digger_lift_manual_power_up has been set to: "
+ + str(self.digger_lift_manual_power_up)
+ )
+ self.get_logger().info(
+ "autonomous_field_type has been set to: " + str(self.autonomous_field_type)
+ )
+ self.get_logger().info(
+ "tilt_digging_start_position has been set to: "
+ + str(self.tilt_digging_start_position)
+ )
+ self.get_logger().info(
+ "dumper_power has been set to: " + str(self.dumper_power)
+ )
+ self.get_logger().info(
+ "DIGGER_SAFETY_ZONE has been set to: " + str(self.DIGGER_SAFETY_ZONE)
)
- self.get_logger().info("digger_lift_manual_power_up has been set to: " + str(self.digger_lift_manual_power_up))
- self.get_logger().info("autonomous_field_type has been set to: " + str(self.autonomous_field_type))
- self.get_logger().info("lift_digging_start_position has been set to: " + str(self.lift_digging_start_position))
- self.get_logger().info("dumper_power has been set to: " + str(self.dumper_power))
- self.get_logger().info("DIGGER_SAFETY_ZONE has been set to: " + str(self.DIGGER_SAFETY_ZONE))
# Define some initial states here
self.state = states["Teleop"]
@@ -109,17 +166,37 @@ def __init__(self) -> None:
self.cli_dumper_toggle = self.create_client(Trigger, "dumper/toggle")
self.cli_dumper_setPower = self.create_client(SetPower, "dumper/setPower")
self.cli_dumper_stop = self.create_client(Trigger, "dumper/stop")
- self.cli_digger_toggle = self.create_client(SetPower, "digger/toggle")
- self.cli_digger_stop = self.create_client(Trigger, "digger/stop")
- self.cli_digger_setPower = self.create_client(SetPower, "digger/setPower")
- self.cli_lift_setPosition = self.create_client(SetPosition, "lift/setPosition")
- self.cli_drivetrain_stop = self.create_client(Trigger, "drivetrain/stop")
- self.cli_lift_stop = self.create_client(Trigger, "lift/stop")
- self.cli_lift_set_power = self.create_client(SetPower, "lift/setPower")
+ # self.cli_digger_toggle = self.create_client(SetPower, "digger/toggle")
+ self.cli_auger_stop = self.create_client(Trigger, "auger/control/stop_all")
+ self.cli_auger_extend = self.create_client(
+ Trigger, "auger/control/extend_digger"
+ )
+ self.cli_auger_retract = self.create_client(
+ Trigger, "auger/control/retract_digger"
+ )
+ self.cli_screw_stop = self.create_client(
+ Trigger, "auger/screw/stop"
+ )
+ self.cli_screw_start = self.create_client(
+ SetScrewMotorSpeed, "auger/screw/run"
+ )
+ self.cli_big_agitator_on_off = self.create_client(
+ SetBool, "big_agitator_on_off"
+ )
+ self.cli_big_agitator_toggle = self.create_client(
+ Trigger, "big_agitator_toggle"
+ )
+ self.cli_small_agitator_on_off = self.create_client(
+ SetBool, "small_agitator_on_off"
+ )
+ self.cli_small_agitator_toggle = self.create_client(
+ Trigger, "small_agitator_toggle"
+ )
# Define publishers and subscribers here
self.drive_power_publisher = self.create_publisher(Twist, "cmd_vel", 10)
- # In order to have actions be cancellable they need to be called in a ReentrantCallbackGroup
+ # In order to have actions be cancellable they need to be called in a
+ # ReentrantCallbackGroup
self.joy_subscription = self.create_subscription(
Joy,
"joy",
@@ -127,17 +204,46 @@ def __init__(self) -> None:
10,
callback_group=ReentrantCallbackGroup(),
)
- self.lift_pose_subscription = self.create_subscription(Float32, "lift_pose", self.lift_pose_callback, 10)
+ self.stream_deck_subscription = self.create_subscription(
+ StreamDeckState,
+ "control/stream_deck",
+ self.stream_deck_callback,
+ 10,
+ callback_group=ReentrantCallbackGroup(),
+ )
+ self.lift_pose_subscription = self.create_subscription(
+ Float32, "lift_pose", self.lift_pose_callback, 10
+ )
self.act_calibrate_field_coordinates = ActionClient(
self, CalibrateFieldCoordinates, "calibrate_field_coordinates"
)
self.act_auto_dig = ActionClient(self, AutoDig, "auto_dig")
+ self.act_auto_dig_nav = ActionClient(self, AutoDigNavOffload, "auto_dig_nav")
self.act_auto_offload = ActionClient(self, AutoOffload, "auto_offload")
- self.field_calibrated_handle: ClientGoalHandle = ClientGoalHandle(None, None, None)
+ self.act_auto_dig_nav_offload = ActionClient(
+ self, AutoDigNavOffload, "auto_dig_nav_offload"
+ )
+ self.act_return_to_coordinate = ActionClient(
+ self, ReturnToCoordinate, "return_to_coordinate"
+ )
+
+ self.act_cancel_action = ActionClient(self, CancelAction, "cancel_action_server")
+ self.bt_action = ActionClient(self, BehaviorTreeExecutor, "bt_action_server")
+
+ self.field_calibrated_handle: ClientGoalHandle = ClientGoalHandle(
+ None, None, None
+ )
self.auto_dig_handle: ClientGoalHandle = ClientGoalHandle(None, None, None)
self.auto_offload_handle: ClientGoalHandle = ClientGoalHandle(None, None, None)
+ self.auto_dig_nav_offload_handle: ClientGoalHandle = ClientGoalHandle(
+ None, None, None
+ )
+
+ self.return_to_coordinate_handle: ClientGoalHandle = ClientGoalHandle(None, None, None)
+ self.bt_action_handle: ClientGoalHandle = ClientGoalHandle(None, None, None)
+ self.cancel_action_handle: ClientGoalHandle = ClientGoalHandle(None, None, None)
# Current position of the lift motor in potentiometer units (0 to 1023)
self.current_lift_position = None # We don't know the current position yet
@@ -147,16 +253,21 @@ def __init__(self) -> None:
self.watchdog_timeout = self.get_parameter("watchdog_timeout").value
self.last_joy_timestamp = time.time()
- # Create timer for watchdog
- self.watchdog_timer = self.create_timer(0.1, self.watchdog_callback) # Check every 0.1 seconds
- self.connection_active = True
+ # # Create timer for watchdog
+ # self.watchdog_timer = self.create_timer(0.1, self.watchdog_callback) # Check every 0.1 seconds
+ # self.connection_active = True
def stop_all_subsystems(self) -> None:
"""This method stops all subsystems on the robot."""
- self.cli_digger_stop.call_async(Trigger.Request()) # Stop the digger chain
+ self.cli_auger_stop.call_async(Trigger.Request()) # Stop the digger chain
self.cli_drivetrain_stop.call_async(Trigger.Request()) # Stop the drivetrain
- self.cli_lift_stop.call_async(Trigger.Request()) # Stop the digger lift
self.cli_dumper_stop.call_async(Trigger.Request()) # Stop the dumper
+ self.cli_big_agitator_on_off.call_async(
+ SetBool.Request(data=False)
+ ) # Stop the agitator motor
+ self.cli_small_agitator_on_off.call_async(
+ SetBool.Request(data=False)
+ ) # Stop the agitator motor
def end_autonomous(self) -> None:
"""This method returns to teleop control."""
@@ -171,6 +282,132 @@ def get_result_callback(self, future: Future):
else:
self.get_logger().info("Autonomous Goal failed (or terminated)!")
self.end_autonomous()
+
+ async def run_behavior_tree(self) -> None:
+ """This method runs a behavior tree using the BehaviorTreeExecutor action server."""
+ if self.bt_action_handle.status != GoalStatus.STATUS_EXECUTING:
+ if not self.bt_action.wait_for_server(timeout_sec = 1.0):
+ self.get_logger().error("BehaviorTreeExecutor action not available")
+ return
+
+ self.stop_all_subsystems()
+ goal_msg = BehaviorTreeExecutor.Goal()
+
+ self.get_logger().info("Sending goal to BehaviorTreeExecutor action server...")
+ self.bt_action_handle = await self.bt_action.send_goal_async(goal_msg)
+
+ if not self.bt_action_handle.accepted:
+ self.get_logger().info("BehaviorTreeExecutor Goal rejected")
+ return
+
+ self.bt_action_handle.get_result_async().add_done_callback(self.get_result_callback)
+ self.state = states["Autonomous"]
+
+ else:
+ self.get_logger().warn("Manually Terminating Behavior Tree Mission")
+ future = self.bt_action_handle.cancel_goal_async()
+ future.add_done_callback(self.cancel_done)
+
+ async def auto_dig_nav_offload_sequence(self) -> None:
+ # Check if the Auto Dig Nav Offload process is not running
+ if self.auto_dig_nav_offload_handle.status != GoalStatus.STATUS_EXECUTING:
+ if not self.act_auto_dig_nav_offload.wait_for_server(timeout_sec=1.0):
+ self.get_logger().error("Auto Dig Nav Offload action not available")
+ return
+ self.stop_all_subsystems()
+ self.auto_dig_nav_offload_handle = (
+ await self.act_auto_dig_nav_offload.send_goal_async(
+ AutoDigNavOffload.Goal(
+ lift_digging_start_position=self.lift_digging_start_position,
+ digger_chain_power=self.digger_chain_power,
+ backward_distance=1.8, # meters
+ )
+ )
+ )
+ if not self.auto_dig_nav_offload_handle.accepted:
+ self.get_logger().info("Auto Dig Nav Offload Goal rejected")
+ return
+ self.auto_dig_nav_offload_handle.get_result_async().add_done_callback(
+ self.get_result_callback
+ )
+ self.state = states["Autonomous"]
+ # Terminate the Auto Dig Nav Offload process
+ else:
+ self.get_logger().warn("Auto Dig Nav Offload Terminated")
+ # Cancel the goal
+ future = self.auto_dig_nav_offload_handle.cancel_goal_async()
+ future.add_done_callback(self.cancel_done)
+
+ async def auto_dig_sequence(self) -> None:
+ # Check if the auto digging process is not running
+ if self.auto_dig_handle.status != GoalStatus.STATUS_EXECUTING:
+ if not self.act_auto_dig.wait_for_server(timeout_sec=1.0):
+ self.get_logger().error("Auto dig action not available")
+ return
+ self.stop_all_subsystems()
+ goal = AutoDig.Goal(
+ lift_digging_start_position=self.lift_digging_start_position,
+ digger_chain_power=self.digger_chain_power,
+ )
+ self.auto_dig_handle = await self.act_auto_dig.send_goal_async(goal)
+ self.auto_dig_handle.get_result_async().add_done_callback(
+ self.get_result_callback
+ )
+ self.state = states["Autonomous"]
+ # Terminate the auto dig process
+ else:
+ self.get_logger().warn("Auto Dig Terminated")
+ # Cancel the goal
+ future = self.auto_dig_handle.cancel_goal_async()
+ future.add_done_callback(self.cancel_done)
+
+ async def auto_offload_sequence(self) -> None:
+ # Check if the auto offload process is not running
+ if self.auto_offload_handle.status != GoalStatus.STATUS_EXECUTING:
+ if not self.act_auto_offload.wait_for_server(timeout_sec=1.0):
+ self.get_logger().error("Auto offload action not available")
+ return
+ self.stop_all_subsystems()
+ goal = AutoOffload.Goal()
+ self.auto_offload_handle = await self.act_auto_offload.send_goal_async(goal)
+ self.auto_offload_handle.get_result_async().add_done_callback(
+ self.get_result_callback
+ )
+ self.state = states["Autonomous"]
+ # Terminate the auto offload process
+ else:
+ self.get_logger().warn("Auto Offload Terminated")
+ # Cancel the goal
+ future = self.auto_offload_handle.cancel_goal_async()
+ future.add_done_callback(self.cancel_done)
+
+ async def calibrate_field_coordinates(self) -> None:
+ # Check if the field calibration process is not running
+ if self.field_calibrated_handle.status != GoalStatus.STATUS_EXECUTING:
+ if not self.act_calibrate_field_coordinates.wait_for_server(
+ timeout_sec=1.0
+ ):
+ self.get_logger().error("Field calibration action not available")
+ return
+ self.stop_all_subsystems()
+ self.field_calibrated_handle = (
+ await self.act_calibrate_field_coordinates.send_goal_async(
+ CalibrateFieldCoordinates.Goal()
+ )
+ )
+ if not self.field_calibrated_handle.accepted:
+ self.get_logger().info("Field calibration Goal rejected")
+ return
+ self.field_calibrated_handle.get_result_async().add_done_callback(
+ self.get_result_callback
+ )
+ self.state = states["Autonomous"]
+ # Terminate the field calibration process
+ else:
+ self.get_logger().warn("Field Calibration Terminated")
+ # Cancel the goal
+ future = self.field_calibrated_handle.cancel_goal_async()
+ future.add_done_callback(self.cancel_done)
async def joystick_callback(self, msg: Joy) -> None:
"""This method is called whenever a joystick message is received."""
@@ -179,132 +416,195 @@ async def joystick_callback(self, msg: Joy) -> None:
self.last_joy_timestamp = time.time()
# PUT TELEOP CONTROLS BELOW #
-
if self.state == states["Teleop"]:
# Drive the robot using joystick input during Teleop (Arcade Drive)
- forward_power = msg.axes[bindings.RIGHT_JOYSTICK_VERTICAL_AXIS] * self.max_drive_power # Forward power
- turning_power = msg.axes[bindings.LEFT_JOYSTICK_HORIZONTAL_AXIS] * self.max_turn_power # Turning power
- self.drive_power_publisher.publish(Twist(linear=Vector3(x=forward_power), angular=Vector3(z=turning_power)))
+ # Forward power
+ forward_power = (
+ msg.axes[bindings.RIGHT_JOYSTICK_VERTICAL_AXIS] * self.max_drive_power
+ )
+ # Turning power
+ turning_power = (
+ msg.axes[bindings.LEFT_JOYSTICK_HORIZONTAL_AXIS] * self.max_turn_power
+ )
+ self.drive_power_publisher.publish(
+ Twist(linear=Vector3(x=forward_power), angular=Vector3(z=turning_power))
+ )
# Check if the digger button is pressed #
if msg.buttons[bindings.X_BUTTON] == 1 and buttons[bindings.X_BUTTON] == 0:
- self.cli_digger_toggle.call_async(SetPower.Request(power=self.digger_chain_power))
+ self.cli_screw_start.call_async(
+ SetScrewMotorSpeed.Request(speed=self.screw_speed)
+ )
+
+ if msg.buttons[bindings.X_BUTTON] == 0 and buttons[bindings.X_BUTTON] == 1:
+ self.cli_screw_stop.call_async(Trigger.Request())
# Check if the dumper button is pressed #
if msg.buttons[bindings.B_BUTTON] == 1 and buttons[bindings.B_BUTTON] == 0:
- self.cli_dumper_stop.call_async(Trigger.Request()) # Stop whatever the dumper is doing
- self.cli_dumper_toggle.call_async(Trigger.Request()) # Toggle the dumper (extended or retracted)
-
- # Manually adjust the dumper position with the left and right bumpers
- if msg.buttons[bindings.RIGHT_BUMPER] == 1 and buttons[bindings.RIGHT_BUMPER] == 0:
- self.cli_dumper_setPower.call_async(SetPower.Request(power=self.dumper_power))
- elif msg.buttons[bindings.RIGHT_BUMPER] == 0 and buttons[bindings.RIGHT_BUMPER] == 1:
+ self.cli_dumper_stop.call_async(
+ Trigger.Request()
+ ) # Stop whatever the dumper is doing
+ # Toggle the dumper (extended or retracted)
+ self.cli_dumper_toggle.call_async(Trigger.Request())
+
+ # Check if the agitator button is pressed #
+ if msg.buttons[bindings.Y_BUTTON] == 1 and buttons[bindings.Y_BUTTON] == 0:
+ self.cli_big_agitator_toggle.call_async(
+ Trigger.Request()
+ ) # Toggle the agitator motor
+ # self.cli_small_agitator_toggle.call_async(Trigger.Request())
+ # # Toggle the agitator motor
+
+ # Manually adjust the dumper position with the left and right
+ # bumpers
+ if (
+ msg.buttons[bindings.RIGHT_BUMPER] == 1
+ and buttons[bindings.RIGHT_BUMPER] == 0
+ ):
+ self.cli_dumper_setPower.call_async(
+ SetPower.Request(power=self.dumper_power)
+ )
+ elif (
+ msg.buttons[bindings.RIGHT_BUMPER] == 0
+ and buttons[bindings.RIGHT_BUMPER] == 1
+ ):
self.cli_dumper_stop.call_async(Trigger.Request())
- elif msg.buttons[bindings.LEFT_BUMPER] == 1 and buttons[bindings.LEFT_BUMPER] == 0:
- self.cli_dumper_setPower.call_async(SetPower.Request(power=-self.dumper_power))
- elif msg.buttons[bindings.LEFT_BUMPER] == 0 and buttons[bindings.LEFT_BUMPER] == 1:
+ elif (
+ msg.buttons[bindings.LEFT_BUMPER] == 1
+ and buttons[bindings.LEFT_BUMPER] == 0
+ ):
+ self.cli_dumper_setPower.call_async(
+ SetPower.Request(power=-self.dumper_power)
+ )
+ elif (
+ msg.buttons[bindings.LEFT_BUMPER] == 0
+ and buttons[bindings.LEFT_BUMPER] == 1
+ ):
self.cli_dumper_stop.call_async(Trigger.Request())
- # Manually adjust the height of the digger with the left and right triggers
- if msg.buttons[bindings.LEFT_TRIGGER] == 1 and buttons[bindings.LEFT_TRIGGER] == 0:
- self.cli_lift_set_power.call_async(SetPower.Request(power=self.digger_lift_manual_power_up))
- elif msg.buttons[bindings.LEFT_TRIGGER] == 0 and buttons[bindings.LEFT_TRIGGER] == 1:
- self.cli_lift_stop.call_async(Trigger.Request())
- elif msg.buttons[bindings.RIGHT_TRIGGER] == 1 and buttons[bindings.RIGHT_TRIGGER] == 0:
- if self.current_lift_position and self.current_lift_position < self.DIGGER_SAFETY_ZONE:
- self.cli_lift_set_power.call_async(SetPower.Request(power=-self.digger_lift_manual_power_up))
- else:
- self.cli_lift_set_power.call_async(SetPower.Request(power=-self.digger_lift_manual_power_down))
- elif msg.buttons[bindings.RIGHT_TRIGGER] == 0 and buttons[bindings.RIGHT_TRIGGER] == 1:
- self.cli_lift_stop.call_async(Trigger.Request())
+ # Manually adjust the height of the digger with the left and right
+ # triggers
+ if (
+ msg.buttons[bindings.LEFT_TRIGGER] == 1
+ and buttons[bindings.LEFT_TRIGGER] == 0
+ ):
+ self.cli_auger_extend.call_async(Trigger.Request())
+ elif (
+ msg.buttons[bindings.LEFT_TRIGGER] == 0
+ and buttons[bindings.LEFT_TRIGGER] == 1
+ ):
+ self.cli_auger_stop.call_async(Trigger.Request())
+ elif (
+ msg.buttons[bindings.RIGHT_TRIGGER] == 1
+ and buttons[bindings.RIGHT_TRIGGER] == 0
+ ):
+ self.cli_auger_retract.call_async(Trigger.Request())
+ elif (
+ msg.buttons[bindings.RIGHT_TRIGGER] == 0
+ and buttons[bindings.RIGHT_TRIGGER] == 1
+ ):
+ self.cli_auger_stop.call_async(Trigger.Request())
# THE CONTROLS BELOW ALWAYS WORK #
- # Check if the Apriltag calibration button is pressed
- # TODO: This autonomous action needs to be tested on the physical robot!
+ # # Check if the Apriltag calibration button is pressed
+ # # TODO: This autonomous action needs to be tested on the physical robot!
+ # if msg.buttons[bindings.A_BUTTON] == 1 and buttons[bindings.A_BUTTON] == 0:
+ # await self.calibrate_field_coordinates()
+
+ # Check if the Auto Dig Nav button is pressed
+ # Update, will be changed to behavior tree execution in the future,
+ # but keeping this code in case we want to use it as a standalone action
if msg.buttons[bindings.A_BUTTON] == 1 and buttons[bindings.A_BUTTON] == 0:
- # Check if the field calibration process is not running
- if self.field_calibrated_handle.status != GoalStatus.STATUS_EXECUTING:
- if not self.act_calibrate_field_coordinates.wait_for_server(timeout_sec=1.0):
- self.get_logger().error("Field calibration action not available")
- return
- self.stop_all_subsystems()
- self.field_calibrated_handle = await self.act_calibrate_field_coordinates.send_goal_async(
- CalibrateFieldCoordinates.Goal()
- )
- if not self.field_calibrated_handle.accepted:
- self.get_logger().info("Field calibration Goal rejected")
- return
- self.field_calibrated_handle.get_result_async().add_done_callback(self.get_result_callback)
- self.state = states["Autonomous"]
- # Terminate the field calibration process
- else:
- self.get_logger().warn("Field Calibration Terminated")
- # Cancel the goal
- future = self.field_calibrated_handle.cancel_goal_async()
- future.add_done_callback(self.cancel_done)
+ # await self.auto_dig_nav_offload_sequence()
+ await self.run_behavior_tree()
+
+
# Check if the autonomous digging button is pressed
- if msg.buttons[bindings.START_BUTTON] == 1 and buttons[bindings.START_BUTTON] == 0:
- # Check if the auto digging process is not running
- if self.auto_dig_handle.status != GoalStatus.STATUS_EXECUTING:
- if not self.act_auto_dig.wait_for_server(timeout_sec=1.0):
- self.get_logger().error("Auto dig action not available")
- return
- self.stop_all_subsystems()
- goal = AutoDig.Goal(
- lift_digging_start_position=self.lift_digging_start_position,
- digger_chain_power=self.digger_chain_power,
- )
- self.auto_dig_handle = await self.act_auto_dig.send_goal_async(goal)
- self.auto_dig_handle.get_result_async().add_done_callback(self.get_result_callback)
- self.state = states["Autonomous"]
- # Terminate the auto dig process
- else:
- self.get_logger().warn("Auto Dig Terminated")
- # Cancel the goal
- future = self.auto_dig_handle.cancel_goal_async()
- future.add_done_callback(self.cancel_done)
+ if (
+ msg.buttons[bindings.START_BUTTON] == 1
+ and buttons[bindings.START_BUTTON] == 0
+ ):
+ await self.auto_dig_sequence()
# Check if the autonomous offload button is pressed
- if msg.buttons[bindings.BACK_BUTTON] == 1 and buttons[bindings.BACK_BUTTON] == 0:
- # Check if the auto offload process is not running
- if self.auto_offload_handle.status != GoalStatus.STATUS_EXECUTING:
- if not self.act_auto_offload.wait_for_server(timeout_sec=1.0):
- self.get_logger().error("Auto offload action not available")
- return
- self.stop_all_subsystems()
- goal = AutoOffload.Goal()
- self.auto_offload_handle = await self.act_auto_offload.send_goal_async(goal)
- self.auto_offload_handle.get_result_async().add_done_callback(self.get_result_callback)
- self.state = states["Autonomous"]
- # Terminate the auto offload process
- else:
- self.get_logger().warn("Auto Offload Terminated")
- # Cancel the goal
- future = self.auto_offload_handle.cancel_goal_async()
- future.add_done_callback(self.cancel_done)
-
- # Update button states (this allows us to detect changing button states)
+ if (
+ msg.buttons[bindings.BACK_BUTTON] == 1
+ and buttons[bindings.BACK_BUTTON] == 0
+ ):
+ await self.auto_offload_sequence()
+
+ # Update button states (this allows us to detect changing button
+ # states)
for index in range(len(buttons)):
buttons[index] = msg.buttons[index]
- def watchdog_callback(self):
- """Check if we've received joystick messages recently"""
- current_time = time.time()
- time_since_last_joy = current_time - self.last_joy_timestamp
-
- # If we haven't received a message in watchdog_timeout seconds
- if time_since_last_joy > self.watchdog_timeout:
- if self.connection_active: # Only trigger once when connection is lost
- self.get_logger().warn(
- f"No joystick messages received for {time_since_last_joy:.2f} seconds! Stopping the robot."
- )
- self.stop_all_subsystems() # Stop all robot movement! (safety feature)
- self.connection_active = False
- elif not self.connection_active:
- self.get_logger().warn("Joystick messages received! Functionality of the robot has been restored.")
- self.connection_active = True
+ async def stream_deck_callback(self, msg: StreamDeckState) -> None:
+ button_states = msg.button_states
+
+ # emergency stop. not a toggle
+ if button_states[bindings.STREAMDECK_ESTOP]:
+ self.get_logger().warn("EMERGENCY STOP ACTIVATED FROM STREAM DECK!")
+ self.stop_all_subsystems()
+ self.state = states["Teleop"]
+ return
+
+ if (
+ button_states[bindings.STREAMDECK_AUTO_DIG]
+ and not old_streamdeck_buttons[bindings.STREAMDECK_AUTO_DIG]
+ ):
+ await self.auto_dig_sequence()
+
+ if (
+ button_states[bindings.STREAMDECK_AUTO_DUMP]
+ and not old_streamdeck_buttons[bindings.STREAMDECK_AUTO_DUMP]
+ ):
+ await self.auto_offload_sequence()
+
+ if (
+ button_states[bindings.STREAMDECK_GO_TO_DIG_SITE]
+ and not old_streamdeck_buttons[bindings.STREAMDECK_GO_TO_DIG_SITE]
+ ):
+ await self.auto_dig_nav_offload_sequence()
+
+ if (
+ button_states[bindings.STREAMDECK_START_AUTO]
+ and not old_streamdeck_buttons[bindings.STREAMDECK_START_AUTO]
+ ):
+ # Placeholder for future autonomous mode
+ self.get_logger().info(
+ "Streamdeck Start Auto button pressed - no action assigned yet."
+ )
+
+ if (
+ button_states[bindings.STREAMDECK_APRILTAG_DETECT]
+ and not old_streamdeck_buttons[bindings.STREAMDECK_APRILTAG_DETECT]
+ ):
+ # Placeholder for future AprilTag detection calibration
+ self.get_logger().info(
+ "Streamdeck AprilTag Detect button pressed - no action assigned yet."
+ )
+
+ for index in range(len(button_states)):
+ old_streamdeck_buttons[index] = button_states[index]
+
+ # def watchdog_callback(self):
+ # """Check if we've received joystick messages recently"""
+ # current_time = time.time()
+ # time_since_last_joy = current_time - self.last_joy_timestamp
+
+ # # If we haven't received a message in watchdog_timeout seconds
+ # if time_since_last_joy > self.watchdog_timeout:
+ # if self.connection_active: # Only trigger once when connection is lost
+ # self.get_logger().warn(
+ # f"No joystick messages received for {time_since_last_joy:.2f} seconds! Stopping the robot."
+ # )
+ # self.stop_all_subsystems() # Stop all robot movement! (safety feature)
+ # self.connection_active = False
+ # elif not self.connection_active:
+ # self.get_logger().warn("Joystick messages received! Functionality of the robot has been restored.")
+ # self.connection_active = True
# Define the subscriber callback for the lift pose topic
def lift_pose_callback(self, msg: Float32):
diff --git a/src/rovr_control/rovr_control/read_serial.py b/src/rovr_control/rovr_control/read_serial.py
index 4dfe2751..1d5e83fe 100644
--- a/src/rovr_control/rovr_control/read_serial.py
+++ b/src/rovr_control/rovr_control/read_serial.py
@@ -1,7 +1,7 @@
import rclpy
from rclpy.node import Node
-from rovr_interfaces.msg import LimitSwitches
-from rovr_interfaces.msg import Potentiometers
+from std_srvs.srv import SetBool, Trigger
+from std_msgs.msg import Bool, Int16
import serial
import struct
@@ -12,37 +12,99 @@ class read_serial(Node):
def __init__(self):
super().__init__("read_serial")
- self.limitSwitchesPub = self.create_publisher(LimitSwitches, "limitSwitches", 10)
- self.potentiometerPub = self.create_publisher(Potentiometers, "potentiometers", 10)
+ self.potentiometerPub = self.create_publisher(Int16, "potentiometer", 10)
+ self.LimitSwitchPub = self.create_publisher(Bool, "DumperLimitSwitch", 10)
+
+ # Services to control the relay-driven agitator motor
+ self.srv_bigonoff = self.create_service(
+ SetBool, "big_agitator_on_off", self.big_on_off_callback
+ )
+ self.srv_bigtoggle = self.create_service(
+ Trigger, "big_agitator_toggle", self.big_toggle_callback
+ )
+
+ self.srv_smallonoff = self.create_service(
+ SetBool, "small_agitator_on_off", self.small_on_off_callback
+ )
+ self.srv_smalltoggle = self.create_service(
+ Trigger, "small_agitator_toggle", self.small_toggle_callback
+ )
try:
self.arduino = serial.Serial("/dev/ttyACM0", 9600)
- time.sleep(1) # https://stackoverflow.com/questions/7266558/pyserial-buffer-wont-flush
+ # https://stackoverflow.com/questions/7266558/pyserial-buffer-wont-flush
+ time.sleep(1)
self.arduino.read_all()
except Exception as e:
self.get_logger().fatal(f"Error connecting to serial: {e}")
self.destroy_node()
return
- while True:
- if self.arduino is None:
- self.get_logger().fatal("Killing read_serial node")
- self.destroy_node()
- return
- data = self.arduino.read(8) # Pause until 8 bytes are read
- decoded = struct.unpack("????hh", data) # Use h for integers and ? for booleans
-
- msg = LimitSwitches()
- msg.digger_top_limit_switch = decoded[0]
- msg.digger_bottom_limit_switch = decoded[1]
- msg.dumper_top_limit_switch = decoded[2]
- msg.dumper_bottom_limit_switch = decoded[3]
- self.limitSwitchesPub.publish(msg)
-
- msg = Potentiometers()
- msg.left_motor_pot = decoded[4]
- msg.right_motor_pot = decoded[5]
- self.potentiometerPub.publish(msg)
+ self.timer = self.create_timer(0.1, self.timer_callback)
+
+ self.bigAgitatorOn = False
+ self.smallAgitatorOn = False
+
+ def timer_callback(self):
+ if self.arduino is None:
+ self.get_logger().fatal("Killing read_serial node")
+ self.destroy_node()
+ return
+ data = self.arduino.read(4) # Pause until 4 bytes are read
+ # Use h for integers and ? for booleans
+ decoded = struct.unpack("h?", data)
+
+ potentiometer_msg = Int16()
+ potentiometer_msg.data = decoded[0]
+ self.potentiometerPub.publish(potentiometer_msg)
+
+ bool_msg = Bool()
+ bool_msg.data = decoded[1]
+ self.LimitSwitchPub.publish(bool_msg)
+
+ def big_on_off_callback(self, request, response):
+ # request.data == True → ON, False → OFF
+ cmd = b"1" if request.data else b"0"
+ self.arduino.write(cmd)
+ response.success = True
+ response.message = "Big agitator motor turned " + (
+ "on" if request.data else "off"
+ )
+ self.bigAgitatorOn = request.data
+ return response
+
+ def big_toggle_callback(self, request, response):
+ if self.bigAgitatorOn:
+ response2 = SetBool.Response()
+ self.big_on_off_callback(SetBool.Request(data=False), response2)
+ else:
+ response2 = SetBool.Response()
+ self.big_on_off_callback(SetBool.Request(data=True), response2)
+ response.success = response2.success
+ response.message = response2.message
+ return response
+
+ def small_on_off_callback(self, request, response):
+ # request.data == True → ON, False → OFF
+ cmd = b"3" if request.data else b"2"
+ self.arduino.write(cmd)
+ response.success = True
+ response.message = "Small agitator motor turned " + (
+ "on" if request.data else "off"
+ )
+ self.smallAgitatorOn = request.data
+ return response
+
+ def small_toggle_callback(self, request, response):
+ if self.smallAgitatorOn:
+ response2 = SetBool.Response()
+ self.small_on_off_callback(SetBool.Request(data=False), response2)
+ else:
+ response2 = SetBool.Response()
+ self.small_on_off_callback(SetBool.Request(data=True), response2)
+ response.success = response2.success
+ response.message = response2.message
+ return response
def main(args=None):
diff --git a/src/rovr_control/rovr_control/stream_deck_node.py b/src/rovr_control/rovr_control/stream_deck_node.py
new file mode 100644
index 00000000..d57620a8
--- /dev/null
+++ b/src/rovr_control/rovr_control/stream_deck_node.py
@@ -0,0 +1,111 @@
+# Import the ROS 2 module
+import os
+import time
+from queue import Queue
+
+import rclpy
+from PIL import Image, ImageDraw, ImageFont
+from rclpy.node import Node
+from StreamDeck.DeviceManager import DeviceManager, StreamDeckMini
+from StreamDeck.ImageHelpers import PILHelper
+
+from rovr_interfaces.msg import StreamDeckState
+from rovr_control import gamepad_constants as bindings
+
+
+class StreamDeckNode(Node):
+ ASSETS_PATH = "/workspaces/isaac_ros-dev/src/rovr_control/resource/"
+ button_states: list[bool] = [False] * StreamDeckMini.KEY_COUNT
+
+ def __init__(self) -> None:
+ super().__init__("StreamDeckNode")
+ self.publisher = self.create_publisher(
+ StreamDeckState, "control/stream_deck", 10
+ )
+ msg = StreamDeckState()
+ msg.button_states = self.button_states
+ self.publisher.publish(msg)
+
+ self.all_buttons = [
+ name for name in dir(bindings) if name.startswith("STREAMDECK_")
+ ]
+ self.indexToBinding = {
+ getattr(bindings, name): name for name in self.all_buttons
+ }
+
+ self.queue = Queue()
+
+ streamdecks = DeviceManager().enumerate()
+ while len(streamdecks) == 0:
+ print("No Stream Decks found. Retrying...")
+ time.sleep(1)
+ streamdecks = DeviceManager().enumerate()
+ print("Found Stream Deck.")
+ self.deck: StreamDeckMini = streamdecks[0]
+ self.deck.open()
+ self.deck.reset()
+ for key in range(self.deck.key_count()):
+ self.set_key_image(key)
+ self.deck.set_key_callback(self.key_change_callback)
+
+ self.timer = self.create_timer(0.1, self.timer_callback)
+
+ def timer_callback(self) -> None:
+ while not self.queue.empty():
+ msg = self.queue.get()
+ self.publisher.publish(msg)
+
+ def key_change_callback(self, _, key: int, state: bool) -> None:
+ self.button_states[key] = state
+ msg = StreamDeckState()
+ msg.button_states = self.button_states
+ self.queue.put(msg)
+
+ def set_key_image(self, key: int) -> None:
+ button_name = self.indexToBinding.get(key, "test")
+ image_path = self.indexToBinding.get(key, "test")
+ image = self.render_image(
+ f"{image_path}.png",
+ str.lower(button_name.replace("STREAMDECK_", "").replace("_", " ")),
+ )
+ with self.deck:
+ self.deck.set_key_image(key, image)
+
+ def render_image(self, icon_filename: str, label_text: str) -> bytes:
+ # Resize the source image asset to best-fit the dimensions of a single key,
+ # leaving a margin at the bottom so that we can draw the key title
+ # afterwards.
+ if not os.path.exists(os.path.join(self.ASSETS_PATH, icon_filename)):
+ icon_filename = "test.png"
+ icon = Image.open(os.path.join(self.ASSETS_PATH, icon_filename))
+ image = PILHelper.create_scaled_key_image(
+ self.deck, icon, margins=[0, 0, 20, 0]
+ )
+ draw = ImageDraw.Draw(image)
+ font = ImageFont.load_default()
+ draw.text(
+ (image.width / 2, image.height - 5),
+ text=label_text,
+ font=font,
+ anchor="ms",
+ fill="white",
+ )
+
+ return PILHelper.to_native_key_format(self.deck, image)
+
+
+def main(args=None) -> None:
+ rclpy.init(args=args)
+
+ stream_deck = StreamDeckNode()
+
+ try:
+ rclpy.spin(stream_deck)
+ finally:
+ stream_deck.deck.close()
+ stream_deck.destroy_node()
+ rclpy.shutdown()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/src/rovr_control/rovr_control/xbox_controller_constants.py b/src/rovr_control/rovr_control/xbox_controller_constants.py
index 9d78bab3..30ed9517 100644
--- a/src/rovr_control/rovr_control/xbox_controller_constants.py
+++ b/src/rovr_control/rovr_control/xbox_controller_constants.py
@@ -1,22 +1,22 @@
# Define all the buttons here
-X_BUTTON = 2
-A_BUTTON = 0
-B_BUTTON = 1
-Y_BUTTON = 3
-LEFT_BUMPER = 4
-RIGHT_BUMPER = 5
-BACK_BUTTON = 6
-START_BUTTON = 7
-XBOX_BUTTON = 8
-LEFT_JOYSTICK_BUTTON = 9
-RIGHT_JOYSTICK_BUTTON = 10
+X_BUTTON = 2 # toggle screw
+A_BUTTON = 0 # auto_dig_nav_offload
+B_BUTTON = 1 # toggle dumper
+Y_BUTTON = 3 # toggle big agitator
+LEFT_BUMPER = 4 # dumper retract
+RIGHT_BUMPER = 5 # dumper extend
+BACK_BUTTON = 6 # auto_offload
+START_BUTTON = 7 # auto_dig
+XBOX_BUTTON = 8 # FREE
+LEFT_JOYSTICK_BUTTON = 9 # FREE
+RIGHT_JOYSTICK_BUTTON = 10 # FREE
# Define all the axes here
-LEFT_JOYSTICK_HORIZONTAL_AXIS = 0
-LEFT_JOYSTICK_VERTICAL_AXIS = 1
-RIGHT_JOYSTICK_HORIZONTAL_AXIS = 3
-RIGHT_JOYSTICK_VERTICAL_AXIS = 4
-LEFT_TRIGGER_AXIS = 2
-RIGHT_TRIGGER_AXIS = 5
-DPAD_HORIZONTAL_AXIS = 6
-DPAD_VERTICAL_AXIS = 7
+LEFT_JOYSTICK_HORIZONTAL_AXIS = 0 # turn
+LEFT_JOYSTICK_VERTICAL_AXIS = 1 # FREE
+RIGHT_JOYSTICK_HORIZONTAL_AXIS = 3 # FREE
+RIGHT_JOYSTICK_VERTICAL_AXIS = 4 # drive
+LEFT_TRIGGER_AXIS = 2 # extend auger
+RIGHT_TRIGGER_AXIS = 5 # retract auger
+DPAD_HORIZONTAL_AXIS = 6 # FREE
+DPAD_VERTICAL_AXIS = 7 # FREE
diff --git a/src/rovr_control/setup.py b/src/rovr_control/setup.py
index 6c154756..cdff1a7d 100644
--- a/src/rovr_control/setup.py
+++ b/src/rovr_control/setup.py
@@ -32,6 +32,8 @@
"auto_dig_server = rovr_control.auto_dig_server:main",
"auto_offload_server = rovr_control.auto_offload_server:main",
"dig_location_server = rovr_control.dig_location_server:main",
+ "auto_dig_nav_offload_server = rovr_control.auto_dig_nav_offload_server:main",
+ "stream_deck_node = rovr_control.stream_deck_node:main",
}
],
},
diff --git a/src/rovr_control/test/test_flake8.py b/src/rovr_control/test/test_flake8.py
index 49c1644f..ee79f31a 100644
--- a/src/rovr_control/test/test_flake8.py
+++ b/src/rovr_control/test/test_flake8.py
@@ -20,4 +20,6 @@
@pytest.mark.linter
def test_flake8():
rc, errors = main_with_errors(argv=[])
- assert rc == 0, "Found %d code style errors / warnings:\n" % len(errors) + "\n".join(errors)
+ assert rc == 0, "Found %d code style errors / warnings:\n" % len(
+ errors
+ ) + "\n".join(errors)
diff --git a/src/rovr_interfaces/CMakeLists.txt b/src/rovr_interfaces/CMakeLists.txt
index 5788a559..98dfbec9 100644
--- a/src/rovr_interfaces/CMakeLists.txt
+++ b/src/rovr_interfaces/CMakeLists.txt
@@ -23,8 +23,9 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"action/AutoOffload.action"
"action/CalibrateFieldCoordinates.action"
"action/GoToDigLocation.action"
- "msg/LimitSwitches.msg"
+ "action/AutoDigNavOffload.action"
"msg/Potentiometers.msg"
+ "msg/StreamDeckState.msg"
"srv/Drive.srv"
"srv/MotorCommandGet.srv"
"srv/MotorCommandSet.srv"
@@ -34,6 +35,9 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"srv/SetEncoding.srv"
"srv/SetPosition.srv"
"srv/SetPower.srv"
+ "srv/SetExtension.srv"
+ "srv/AugerSetPushMotor.srv"
+ "srv/SetScrewMotorSpeed.srv"
)
ament_export_dependencies(rosidl_default_runtime)
diff --git a/src/rovr_interfaces/action/AutoDig.action b/src/rovr_interfaces/action/AutoDig.action
index f9d52222..b61ba2ba 100644
--- a/src/rovr_interfaces/action/AutoDig.action
+++ b/src/rovr_interfaces/action/AutoDig.action
@@ -1,7 +1,7 @@
# Request
-float64 lift_digging_start_position
-float64 digger_chain_power
+float64 backup_distance # Distance to back up after digging
---
# Result
+bool success
---
# Feedback
\ No newline at end of file
diff --git a/src/rovr_interfaces/action/AutoDigNavOffload.action b/src/rovr_interfaces/action/AutoDigNavOffload.action
new file mode 100644
index 00000000..fd5deaae
--- /dev/null
+++ b/src/rovr_interfaces/action/AutoDigNavOffload.action
@@ -0,0 +1,8 @@
+# Request
+float64 x_pos
+float64 y_pos
+---
+# Result
+
+---
+# Feedback
diff --git a/src/rovr_interfaces/action/AutoOffload.action b/src/rovr_interfaces/action/AutoOffload.action
index b95661d6..985bbdbd 100644
--- a/src/rovr_interfaces/action/AutoOffload.action
+++ b/src/rovr_interfaces/action/AutoOffload.action
@@ -1,7 +1,8 @@
# Request
float64 lift_dumping_position
-float64 digger_chain_power
+
---
# Result
+bool success
---
# Feedback
\ No newline at end of file
diff --git a/src/rovr_interfaces/action/BehaviorTreeExecutor.action b/src/rovr_interfaces/action/BehaviorTreeExecutor.action
new file mode 100644
index 00000000..123aac98
--- /dev/null
+++ b/src/rovr_interfaces/action/BehaviorTreeExecutor.action
@@ -0,0 +1,7 @@
+# Request
+
+---
+# Result
+bool success
+---
+# Feedback
\ No newline at end of file
diff --git a/src/rovr_interfaces/action/CalibrateFieldCoordinates.action b/src/rovr_interfaces/action/CalibrateFieldCoordinates.action
index dc15b075..5707a77f 100644
--- a/src/rovr_interfaces/action/CalibrateFieldCoordinates.action
+++ b/src/rovr_interfaces/action/CalibrateFieldCoordinates.action
@@ -1,5 +1,6 @@
# Request
---
# Result
+
---
# Feedback
\ No newline at end of file
diff --git a/src/rovr_interfaces/action/CancelAction.action b/src/rovr_interfaces/action/CancelAction.action
new file mode 100644
index 00000000..b4f142c6
--- /dev/null
+++ b/src/rovr_interfaces/action/CancelAction.action
@@ -0,0 +1,6 @@
+# Request
+---
+# Result
+bool success
+---
+# Feedback
\ No newline at end of file
diff --git a/src/rovr_interfaces/action/GoToDigLocation.action b/src/rovr_interfaces/action/GoToDigLocation.action
index dc15b075..c688a3ea 100644
--- a/src/rovr_interfaces/action/GoToDigLocation.action
+++ b/src/rovr_interfaces/action/GoToDigLocation.action
@@ -1,5 +1,7 @@
# Request
+float64 x
+float64 y
---
# Result
---
-# Feedback
\ No newline at end of file
+# Feedback
diff --git a/src/rovr_interfaces/action/ReturnToCoordinate.action b/src/rovr_interfaces/action/ReturnToCoordinate.action
new file mode 100644
index 00000000..6a1e934d
--- /dev/null
+++ b/src/rovr_interfaces/action/ReturnToCoordinate.action
@@ -0,0 +1,9 @@
+# Request
+float64 x_pos
+float64 y_pos
+
+---
+# Result
+bool success
+---
+# Feedback
diff --git a/src/rovr_interfaces/msg/LimitSwitches.msg b/src/rovr_interfaces/msg/LimitSwitches.msg
index 7fb8bbae..c90c28ff 100644
--- a/src/rovr_interfaces/msg/LimitSwitches.msg
+++ b/src/rovr_interfaces/msg/LimitSwitches.msg
@@ -1,4 +1,2 @@
-bool digger_top_limit_switch # Whether the top digger limit switch is pressed or not
-bool digger_bottom_limit_switch # Whether the bottom digger limit switch is pressed or not
-bool dumper_top_limit_switch #Whether the top dumper limit switch is pressed or not
-bool dumper_bottom_limit_switch #Whether the bottom dumper limit switch is pressed or no
+bool bottom_limit_switch;
+bool top_limit_switch;
\ No newline at end of file
diff --git a/src/rovr_interfaces/msg/StreamDeckState.msg b/src/rovr_interfaces/msg/StreamDeckState.msg
new file mode 100644
index 00000000..91266dd1
--- /dev/null
+++ b/src/rovr_interfaces/msg/StreamDeckState.msg
@@ -0,0 +1 @@
+bool[6] button_states
\ No newline at end of file
diff --git a/src/rovr_interfaces/srv/AugerSetPushMotor.srv b/src/rovr_interfaces/srv/AugerSetPushMotor.srv
new file mode 100644
index 00000000..9b655c76
--- /dev/null
+++ b/src/rovr_interfaces/srv/AugerSetPushMotor.srv
@@ -0,0 +1,6 @@
+# This service sets the position of a subsystem with 1 motor
+float32 speed # rpm
+float32 position
+float32 power_limit 0.5 # optional power cap
+---
+bool success
\ No newline at end of file
diff --git a/src/rovr_interfaces/srv/MotorCommandSet.srv b/src/rovr_interfaces/srv/MotorCommandSet.srv
index 8f93a57d..0830012e 100644
--- a/src/rovr_interfaces/srv/MotorCommandSet.srv
+++ b/src/rovr_interfaces/srv/MotorCommandSet.srv
@@ -3,5 +3,6 @@ string type # The type of command to send (ramp_duty_cycle, duty_cycle, position
uint32 can_id # CAN ID of the VESC
float32 value # Value to pass to the motor_control method
float32 value2 # Second value to pass to the motor_control method
+float32 power_limit 0.5 # optional power cap
---
bool success
\ No newline at end of file
diff --git a/src/rovr_interfaces/srv/SetExtension.srv b/src/rovr_interfaces/srv/SetExtension.srv
new file mode 100644
index 00000000..c340be49
--- /dev/null
+++ b/src/rovr_interfaces/srv/SetExtension.srv
@@ -0,0 +1,4 @@
+# True to extend and false to retract the auger
+bool extension
+---
+bool success
\ No newline at end of file
diff --git a/src/rovr_interfaces/srv/SetPosition.srv b/src/rovr_interfaces/srv/SetPosition.srv
index 9e84e28e..6c23c8bb 100644
--- a/src/rovr_interfaces/srv/SetPosition.srv
+++ b/src/rovr_interfaces/srv/SetPosition.srv
@@ -1,4 +1,5 @@
# This service sets the position of a subsystem with 1 motor
float32 position # position
+float32 power_limit 0.5 # optional power cap
---
bool success
\ No newline at end of file
diff --git a/src/rovr_interfaces/srv/SetScrewMotorSpeed.srv b/src/rovr_interfaces/srv/SetScrewMotorSpeed.srv
new file mode 100644
index 00000000..9c5aceb8
--- /dev/null
+++ b/src/rovr_interfaces/srv/SetScrewMotorSpeed.srv
@@ -0,0 +1,5 @@
+# This service sets the speed of the screw
+float32 speed # rpm
+float32 power_limit 0.5 # optional power cap
+---
+bool success
\ No newline at end of file
diff --git a/src/zed-ros2-wrapper b/src/zed-ros2-wrapper
index 4d77b3bb..45408409 160000
--- a/src/zed-ros2-wrapper
+++ b/src/zed-ros2-wrapper
@@ -1 +1 @@
-Subproject commit 4d77b3bb10dcd32b4e164bcbf6a0f690a041c5f2
+Subproject commit 4540840984da2ddab93daf07fe37b23af55a69ca