diff --git a/.github/workflows/workflows.yml b/.github/workflows/workflows.yml
index d96384b3..ac59dfc2 100644
--- a/.github/workflows/workflows.yml
+++ b/.github/workflows/workflows.yml
@@ -9,6 +9,13 @@ on:
branches:
- master
- develop
+ workflow_dispatch:
+ inputs:
+ debug_enabled:
+ type: boolean
+ description: 'Host tmate session on failure for debugging.'
+ required: false
+ default: false
jobs:
build-and-test:
@@ -54,7 +61,7 @@ jobs:
- name: Build Submodules
run: ./scripts/setup_submodules.sh
-
+
- name: Build
run: |
env > ~/temp_env_1
@@ -62,6 +69,13 @@ jobs:
source $VEXU_HOME/install/setup.bash
env > ~/temp_env_2
diff ~/temp_env_1 ~/temp_env_2 | sed '/^[^>]/d' | sed 's/> //'g | xargs -n1 echo >> $GITHUB_ENV
-
+
- name: Test
run: ./scripts/test_full.sh
+
+ - name: Open TMate Session
+ if: ${{ failure() }}
+ # && github.event_name == 'workflow_dispatch' && inputs.debug_enabled }}
+ uses: mxschmitt/action-tmate@v3
+ with:
+ detached: true
diff --git a/.gitmodules b/.gitmodules
index 2106ef51..823f1e8f 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -1,32 +1,49 @@
[submodule "01_Libraries/ghost_estimation/src/shared"]
path = 01_Libraries/ghost_estimation/src/shared
- url = git@github.com:MaxxWilson/amrl_shared_lib.git
+ url = git@github.com:VEXU-GHOST/amrl_shared_lib.git
+
[submodule "09_External/yaml-cpp"]
path = 09_External/yaml-cpp
- url = git@github.com:jbeder/yaml-cpp.git
+ url = git@github.com:VEXU-GHOST/yaml-cpp.git
+
[submodule "09_External/Casadi-Tutorial-CPP"]
path = 09_External/Casadi-Tutorial-CPP
- url = git@github.com:zehuilu/Tutorial-on-CasADi-with-CPP.git
+ url = git@github.com:VEXU-GHOST/Tutorial-on-CasADi-with-CPP.git
+
[submodule "09_External/casadi"]
path = 09_External/casadi
- url = https://github.com/casadi/casadi.git
+ url = git@github.com:VEXU-GHOST/casadi.git
+
[submodule "09_External/Ipopt"]
path = 09_External/Ipopt
- url = git@github.com:MaxxWilson/Ipopt.git
+ url = git@github.com:VEXU-GHOST/Ipopt.git
+
[submodule "09_External/rplidar_ros"]
path = 09_External/rplidar_ros
- url = git@github.com:Slamtec/rplidar_ros.git
+ url = git@github.com:VEXU-GHOST/rplidar_ros.git
branch = ros2
+
[submodule "09_External/matplotlib-cpp"]
path = 09_External/matplotlib-cpp
- url = git@github.com:MaxxWilson/matplotlib-cpp.git
+ url = git@github.com:VEXU-GHOST/matplotlib-cpp.git
+
[submodule "09_External/ThirdParty-Mumps"]
path = 09_External/ThirdParty-Mumps
- url = git@github.com:MaxxWilson/ThirdParty-Mumps.git
+ url = git@github.com:VEXU-GHOST/ThirdParty-Mumps.git
branch = with-src
+
[submodule "09_External/BehaviorTree.CPP"]
path = 09_External/BehaviorTree.CPP
- url = git@github.com:BehaviorTree/BehaviorTree.CPP.git
+ url = git@github.com:VEXU-GHOST/BehaviorTree.CPP.git
+
[submodule "09_External/BehaviorTree.ROS2"]
path = 09_External/BehaviorTree.ROS2
- url = https://github.com/JakeWendling/BehaviorTree.ROS2
+ url = git@github.com:VEXU-GHOST/BehaviorTree.ROS2.git
+
+[submodule "09_External/plotjuggler"]
+ path = 09_External/plotjuggler
+ url = git@github.com:VEXU-GHOST/PlotJuggler.git
+
+[submodule "09_External/plotjuggler-ros-plugins"]
+ path = 09_External/plotjuggler-ros-plugins
+ url = git@github.com:VEXU-GHOST/plotjuggler-ros-plugins.git
diff --git a/03_ROS/ghost_viz/CMakeLists.txt b/03_ROS/ghost_viz/CMakeLists.txt
new file mode 100644
index 00000000..a6a4a8dc
--- /dev/null
+++ b/03_ROS/ghost_viz/CMakeLists.txt
@@ -0,0 +1,27 @@
+cmake_minimum_required(VERSION 3.8)
+project(ghost_viz)
+
+# Add Global CMake Config for Ghost Packages
+set(Ghost_DIR "$ENV{VEXU_HOME}/cmake")
+find_package(Ghost CONFIG REQUIRED)
+
+# Adds all our package dependencies to one list
+set(DEPENDENCIES
+ ament_cmake
+ )
+
+# Iterates through the list and finds the packages!
+foreach(pkg ${DEPENDENCIES})
+ find_package(${pkg} REQUIRED)
+endforeach()
+
+# Tell any packages that depend on us what dependencies we need.
+ament_export_dependencies(${DEPENDENCIES})
+
+install(DIRECTORY
+ config
+ launch
+ DESTINATION share/${PROJECT_NAME})
+
+# This generates the ROS2 package and should always be the last line.
+ament_package()
diff --git a/03_ROS/ghost_viz/config/plotjuggler_config.xml b/03_ROS/ghost_viz/config/plotjuggler_config.xml
new file mode 100644
index 00000000..ce3cf6b1
--- /dev/null
+++ b/03_ROS/ghost_viz/config/plotjuggler_config.xml
@@ -0,0 +1,64 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/03_ROS/ghost_viz/launch/trajectory_plotter.launch.py b/03_ROS/ghost_viz/launch/trajectory_plotter.launch.py
new file mode 100644
index 00000000..700df426
--- /dev/null
+++ b/03_ROS/ghost_viz/launch/trajectory_plotter.launch.py
@@ -0,0 +1,23 @@
+import os
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+ home_dir = os.path.expanduser("~")
+ pkg_share_dir = os.path.join(
+ home_dir, "VEXU_GHOST", "03_ROS", "ghost_viz"
+ )
+
+ # This contains all the parameters for our ROS nodes
+ pt_config_path = os.path.join(pkg_share_dir, "config/plotjuggler_config.xml")
+
+
+ plotjuggler_node = Node(
+ package="plotjuggler",
+ executable="plotjuggler",
+ output="screen",
+ arguments=["-n", "-l", pt_config_path]
+ )
+
+ return LaunchDescription([plotjuggler_node])
diff --git a/03_ROS/ghost_viz/package.xml b/03_ROS/ghost_viz/package.xml
new file mode 100644
index 00000000..614efeeb
--- /dev/null
+++ b/03_ROS/ghost_viz/package.xml
@@ -0,0 +1,21 @@
+
+
+
+ ghost_viz
+ 1.0.0
+ Provides tools for vizualization and plotting for GHOST projects
+ maxx
+ MIT
+
+ ament_cmake
+
+ ament_lint_auto
+ ament_lint_common
+
+ plotjuggler
+ plotjuggler_ros
+
+
+ ament_cmake
+
+
diff --git a/09_External/plotjuggler b/09_External/plotjuggler
new file mode 160000
index 00000000..f3eabf32
--- /dev/null
+++ b/09_External/plotjuggler
@@ -0,0 +1 @@
+Subproject commit f3eabf320805ca4c7534ad279e7a5ca6026465af
diff --git a/09_External/plotjuggler-ros-plugins b/09_External/plotjuggler-ros-plugins
new file mode 160000
index 00000000..e8a4873d
--- /dev/null
+++ b/09_External/plotjuggler-ros-plugins
@@ -0,0 +1 @@
+Subproject commit e8a4873d3dd4fb956288fe00f910c84dc64833b5
diff --git a/11_Robots/ghost_swerve_mpc_planner/src/casadi_swerve_model_generation.cpp b/11_Robots/ghost_swerve_mpc_planner/src/casadi_swerve_model_generation.cpp
index 98ba99ea..86da245a 100644
--- a/11_Robots/ghost_swerve_mpc_planner/src/casadi_swerve_model_generation.cpp
+++ b/11_Robots/ghost_swerve_mpc_planner/src/casadi_swerve_model_generation.cpp
@@ -201,7 +201,7 @@ int main(int argc, char * argv[])
auto node_ptr = std::make_shared("swerve_mpc_node");
auto mpc_trajectory_publisher = node_ptr->create_publisher(
- "/swerve_mpc_trajectory", 10);
+ "/trajectory/swerve_mpc_trajectory", 10);
std::thread node_thread([&]() {
rclcpp::spin(node_ptr);
@@ -359,6 +359,14 @@ int main(int argc, char * argv[])
}
}
+ // Allocate timeseries vector for each state/input variable
+ solution_map["time"] = std::vector(NUM_KNOTS);
+
+ // Iterate through timeseries and add final state values to solution vector
+ for (int k = 0; k < NUM_KNOTS; k++) {
+ solution_map["time"][k] = DT * ((double) k);
+ }
+
return solution_map;
};
diff --git a/scripts/build.sh b/scripts/build.sh
index 0353148e..264b9329 100755
--- a/scripts/build.sh
+++ b/scripts/build.sh
@@ -24,15 +24,23 @@ arch=$(uname -p)
cd $VEXU_HOME
echo "---Building Ghost ROS Packages---"
+skip=(
+ behaviortree_cpp
+ behaviortree_ros2
+ btcpp_ros2_interfaces
+ btcpp_ros2_samples
+ rplidar_ros
+)
+
# Build ignores simulator packages on embedded devices
if [ "$arch" == 'x86_64' ];
then
- colcon build --symlink-install --packages-skip --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON || exit -1
+ colcon build --symlink-install --packages-skip ${skip[@]} --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON || exit -1
fi
if [ "$arch" == 'aarch64' ];
then
- colcon build --symlink-install --packages-skip ghost_sim --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON || exit -1
+ colcon build --symlink-install --packages-skip ${skip[@]} ghost_sim ghost_viz plotjuggler plotjuggler-ros-plugins --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON || exit -1
fi
if [ "$1" != "-r" ];
diff --git a/scripts/setup_submodules.sh b/scripts/setup_submodules.sh
index 494cbef4..54b6e4fa 100755
--- a/scripts/setup_submodules.sh
+++ b/scripts/setup_submodules.sh
@@ -1,5 +1,56 @@
#!/bin/bash
+exit_unsupported_pkg() {
+ pkg=$1
+
+ echo "Failure: Unsupported package: $pkg. Please notify maintainers of ghost_dependencies!"
+ echo "https://github.com/VEXU-GHOST/ghost_dependencies"
+ exit -1
+}
+
+exit_unsupported_arch() {
+ arch=$1
+
+ echo "Failure: Unsupported processure architecture: $arch. Please notify maintainers of ghost_dependencies!"
+ echo "https://github.com/VEXU-GHOST/ghost_dependencies"
+ exit -1
+}
+
+install_submodule() {
+ pkg=$1
+
+ supported_pkgs=(
+ 'casadi'
+ 'ipopt'
+ 'matplotlibcpp'
+ 'mumps'
+ 'rplidar'
+ 'btcpp'
+ 'btros2'
+ 'btros2-interfaces'
+ )
+
+ supported_archs=(
+ 'amd64'
+ 'arm64'
+ )
+
+ # Check if given pkg is supported
+ if [[ ! " ${supported_pkgs[*]} " =~ " $pkg " ]]; then
+ exit_unsupported_pkg $pkg
+ fi
+
+ # Check if given arch is supported
+ if [[ ! " ${supported_archs[*]} " =~ " $arch " ]]; then
+ exit_unsupported_arch $arch
+ fi
+
+ sudo wget https://github.com/VEXU-GHOST/ghost_dependencies/raw/main/deb/ghost-$pkg-$arch.deb || exit -1
+ sudo --preserve-env=VEXU_HOME dpkg -i ghost-$pkg-$arch.deb || exit -1
+ sudo rm ghost-$pkg-$arch.deb
+}
+
+
# Verify repo path is set
if [ -z "${VEXU_HOME}" ]
then
@@ -8,74 +59,40 @@ then
fi
cd $VEXU_HOME
+git submodule init
git submodule update --recursive
+# Get processor architecture to determine proper .deb source
+arch=$(dpkg --print-architecture)
+
+
# Build matplotlib-cpp
echo "--------------- MATPLOTLIB_CPP ---------------"
-if [ ! -d "${VEXU_HOME}/09_External/matplotlib-cpp/build" ];
-then
- cd $VEXU_HOME/09_External/matplotlib-cpp
- mkdir build && cd build
+install_submodule matplotlibcpp
+echo; echo
- cmake .. || exit -1
- make || exit -1
- sudo make install || exit -1
-
- cd ../..
-else
- echo "Build already exists"
-fi
-echo
-echo
# Build Mumps
echo "--------------- MUMPS ---------------"
-cd $VEXU_HOME/09_External/ThirdParty-Mumps
sudo apt install gfortran-10 liblapack-dev pkg-config --install-recommends -y || exit -1
sudo apt install swig -y || exit -1
export FC=$(which gfortran-10)
-./configure || exit -1
-make || exit -1
-sudo make install || exit -1
-
-cd ..
+install_submodule mumps
+echo; echo
-echo
-echo
# Build IPOPT
echo "--------------- IPOPT ---------------"
-cd Ipopt
-
-./configure || exit -1
-make || exit -1
-make test || exit -1
-sudo make install || exit -1
-
-echo
-echo
+install_submodule ipopt
+echo; echo
# Build Casadi
echo "--------------- CASADI ---------------"
-if [ ! -d "${VEXU_HOME}/09_External/casadi/build" ];
-then
- cd $VEXU_HOME/09_External/casadi
- mkdir build && cd build
-
- cmake -DWITH_PYTHON=ON -DWITH_IPOPT=ON -DWITH_OPENMP=ON -DWITH_THREAD=ON .. || exit -1
- make || exit -1
- sudo make install || exit -1
-
- cd ../..
-else
- echo "Build already exists"
-fi
-
-echo
-echo
+install_submodule casadi
+echo; echo
# Build Casadi Tutorial CPP
echo "--------------- CASADI_TUTORIAL_CPP ---------------"
@@ -93,5 +110,20 @@ else
echo "Build already exists"
fi
-echo
-echo
+echo; echo
+
+echo "--------------- RPLIDAR ---------------"
+install_submodule rplidar
+echo; echo
+
+echo "--------------- BTCPP ---------------"
+install_submodule btcpp
+echo; echo
+
+echo "--------------- BTROS2 ---------------"
+install_submodule btros2
+echo; echo
+
+echo "--------------- BTROS2-INTERFACES ---------------"
+install_submodule btros2-interfaces
+echo; echo
diff --git a/scripts/test_full.sh b/scripts/test_full.sh
index d31f6bc0..dc3a63c6 100755
--- a/scripts/test_full.sh
+++ b/scripts/test_full.sh
@@ -7,5 +7,13 @@ then
exit -1
fi
+skip=(
+ behaviortree_cpp
+ behaviortree_ros2
+ btcpp_ros2_interfaces
+ btcpp_ros2_samples
+ rplidar_ros
+)
+
cd $VEXU_HOME
-colcon test --return-code-on-test-failure --event-handlers console_direct+ || exit -1
+colcon test --packages-skip ${skip[@]} --return-code-on-test-failure --event-handlers console_direct+ || exit -1
diff --git a/scripts/update_dependencies.sh b/scripts/update_dependencies.sh
index 8fd1220d..1b06a914 100755
--- a/scripts/update_dependencies.sh
+++ b/scripts/update_dependencies.sh
@@ -12,7 +12,7 @@ cd $VEXU_HOME
echo
echo "--------------- Non-ROS Dependencies ---------------"
sudo apt-get install -y libgoogle-glog-dev cmake python3-colcon-common-extensions gfortran-10 || exit -1
-sudo apt-get install -y python3-pip libgtest-dev libgoogle-glog-dev python3-rosdep2 apt-rdepends ros-humble-xacro ros-humble-plotjuggler ros-humble-plotjuggler-ros || exit -1
+sudo apt-get install -y python3-pip libgtest-dev libgoogle-glog-dev python3-rosdep2 apt-rdepends ros-humble-xacro || exit -1
pip install colcon-lint || exit -1
echo