diff --git a/.github/workflows/sdf_parser.yml b/.github/workflows/sdf_parser.yml new file mode 100644 index 0000000..268fae2 --- /dev/null +++ b/.github/workflows/sdf_parser.yml @@ -0,0 +1,58 @@ +name: SDF Parsing + +on: + push: + branches: + - '*' + +jobs: + build: + runs-on: ubuntu-latest + + steps: + - name: Checkout code + uses: actions/checkout@v3 + + - name: Install packages + id: install-packages + run: | + sudo apt install -y build-essential cmake python3-pip wget lsb-release gnupg curl + sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' + wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add - + sudo apt-get update + sudo apt install libsdformat14-dev libsdformat14 + + - name: Build check_sdf + id: build-check-sdf + run: | + cd ./sdf_parsing + mkdir build + cd build/ + cmake .. -Wno-dev + make + + - name: Get changed files + id: changed-files + run: | + commit_sha=$(git rev-parse HEAD) + + # Get the list of changed files + echo "changed_files=$(git diff --name-only $commit_sha^ $commit_sha)" >> $GITHUB_OUTPUT + + - name: Iterate, find and check .sdf files + run: | + for file in ${{ steps.changed-files.outputs.changed_files }}; do + if [[ $file == *.sdf ]]; then + echo "Checking $file" + + # Run sdf parser + ./sdf_parsing/build/check_sdf $file + exit_code=$? + + # Exit if exit code not equal 0 + if [ $exit_code -ne 0 ]; then + echo "sdf parsing error in $file, exit code: $exit_code" + fi + + fi + done diff --git a/sdf_parsing/CMakeLists.txt b/sdf_parsing/CMakeLists.txt new file mode 100644 index 0000000..b5126c0 --- /dev/null +++ b/sdf_parsing/CMakeLists.txt @@ -0,0 +1,9 @@ +cmake_minimum_required(VERSION 2.8 FATAL_ERROR) + +find_package(sdformat14 REQUIRED) # sdformat7 or higher + +include_directories(${SDFormat_INCLUDE_DIRS}) +link_directories(${SDFormat_LIBRARY_DIRS}) + +add_executable(check_sdf check_sdf.cc) +target_link_libraries(check_sdf ${SDFormat_LIBRARIES}) \ No newline at end of file diff --git a/sdf_parsing/check_sdf.cc b/sdf_parsing/check_sdf.cc new file mode 100644 index 0000000..ae08fc6 --- /dev/null +++ b/sdf_parsing/check_sdf.cc @@ -0,0 +1,67 @@ +#include + +#include + +int main(int argc, const char* argv[]) +{ + // check arguments + if (argc < 2) + { + std::cerr << "Usage: " << argv[0] + << " " << std::endl; + return -1; + } + const std::string sdfPath(argv[1]); + + // load and check sdf file + sdf::SDFPtr sdfElement(new sdf::SDF()); + sdf::init(sdfElement); + if (!sdf::readFile(sdfPath, sdfElement)) + { + std::cerr << sdfPath << " is not a valid SDF file!" << std::endl; + return -2; + } + + // start parsing model + const sdf::ElementPtr rootElement = sdfElement->Root(); + if (!rootElement->HasElement("model")) + { + std::cerr << sdfPath << " is not a model SDF file!" << std::endl; + return -3; + } + const sdf::ElementPtr modelElement = rootElement->GetElement("model"); + const std::string modelName = modelElement->Get("name"); + std::cout << "Found " << modelName << " model!" << std::endl; + + // parse model links + sdf::ElementPtr linkElement = modelElement->GetElement("link"); + while (linkElement) + { + const std::string linkName = linkElement->Get("name"); + std::cout << "Found " << linkName << " link in " + << modelName << " model!" << std::endl; + linkElement = linkElement->GetNextElement("link"); + } + + // parse model joints + sdf::ElementPtr jointElement = modelElement->GetElement("joint"); + while (jointElement) + { + const std::string jointName = jointElement->Get("name"); + std::cout << "Found " << jointName << " joint in " + << modelName << " model!" << std::endl; + + const sdf::ElementPtr parentElement = jointElement->GetElement("parent"); + const std::string parentLinkName = parentElement->Get(); + + const sdf::ElementPtr childElement = jointElement->GetElement("child"); + const std::string childLinkName = childElement->Get(); + + std::cout << "Joint " << jointName << " connects " << parentLinkName + << " link to " << childLinkName << " link" << std::endl; + + jointElement = jointElement->GetNextElement("joint"); + } + + return 0; +} \ No newline at end of file