├── .github ├── .cspell │ ├── rust-dependencies.txt │ └── project-dictionary.txt ├── dependabot.yml └── workflows │ ├── release.yml │ └── ci.yml ├── src ├── lib.rs ├── errors.rs ├── utils.rs ├── funcs.rs └── deserialize.rs ├── .gitignore ├── README.md ├── .cspell.json ├── Cargo.toml ├── sample.urdf ├── tools └── spell-check.sh └── LICENSE /.github/.cspell/rust-dependencies.txt: -------------------------------------------------------------------------------- 1 | // This file is @generated by spell-check.sh. 2 | // It is not intended for manual editing. 3 | 4 | thiserror 5 | -------------------------------------------------------------------------------- /.github/.cspell/project-dictionary.txt: -------------------------------------------------------------------------------- 1 | funcs 2 | inorder 3 | repr 4 | rgba 5 | rospack 6 | rosrun 7 | rustdocflags 8 | rustflags 9 | urdf 10 | xacro 11 | -------------------------------------------------------------------------------- /src/lib.rs: -------------------------------------------------------------------------------- 1 | #![doc = include_str!("../README.md")] 2 | 3 | mod errors; 4 | pub use errors::*; 5 | 6 | mod deserialize; 7 | pub use deserialize::*; 8 | 9 | mod funcs; 10 | pub use funcs::*; 11 | 12 | pub mod utils; 13 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Generated by Cargo 2 | # will have compiled files and executables 3 | /target/ 4 | 5 | # Remove Cargo.lock from gitignore if creating an executable, leave it for libraries 6 | # More information here http://doc.crates.io/guide.html#cargotoml-vs-cargolock 7 | Cargo.lock 8 | -------------------------------------------------------------------------------- /.github/dependabot.yml: -------------------------------------------------------------------------------- 1 | version: 2 2 | updates: 3 | - package-ecosystem: cargo 4 | directory: / 5 | schedule: 6 | interval: daily 7 | commit-message: 8 | prefix: '' 9 | labels: [] 10 | - package-ecosystem: github-actions 11 | directory: / 12 | schedule: 13 | interval: daily 14 | commit-message: 15 | prefix: '' 16 | labels: [] 17 | -------------------------------------------------------------------------------- /.github/workflows/release.yml: -------------------------------------------------------------------------------- 1 | name: Release 2 | 3 | on: 4 | push: 5 | tags: ['v[0-9]+.*'] 6 | 7 | env: 8 | CARGO_INCREMENTAL: 0 9 | CARGO_NET_RETRY: 10 10 | CARGO_TERM_COLOR: always 11 | RUST_BACKTRACE: 1 12 | RUSTFLAGS: -D warnings 13 | RUSTUP_MAX_RETRIES: 10 14 | 15 | defaults: 16 | run: 17 | shell: bash 18 | 19 | jobs: 20 | create-release: 21 | if: github.repository_owner == 'openrr' 22 | runs-on: ubuntu-latest 23 | timeout-minutes: 60 24 | steps: 25 | - uses: actions/checkout@v6 26 | - uses: dtolnay/rust-toolchain@stable 27 | - run: cargo package 28 | - uses: taiki-e/create-gh-release-action@v1 29 | with: 30 | branch: main 31 | env: 32 | GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} 33 | - run: cargo publish 34 | env: 35 | CARGO_REGISTRY_TOKEN: ${{ secrets.CARGO_REGISTRY_TOKEN }} 36 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # urdf-rs 2 | 3 | [![Build Status](https://img.shields.io/github/actions/workflow/status/openrr/urdf-rs/ci.yml?branch=main&logo=github)](https://github.com/openrr/urdf-rs/actions) [![crates.io](https://img.shields.io/crates/v/urdf-rs.svg?logo=rust)](https://crates.io/crates/urdf-rs) [![docs](https://docs.rs/urdf-rs/badge.svg)](https://docs.rs/urdf-rs) [![discord](https://dcbadge.vercel.app/api/server/8DAFFKc88B?style=flat)](https://discord.gg/8DAFFKc88B) 4 | 5 | [URDF](http://wiki.ros.org/urdf) parser for Rust. 6 | 7 | Only [link](http://wiki.ros.org/urdf/XML/link) and [joint](http://wiki.ros.org/urdf/XML/joint) are supported. 8 | 9 | ## Example 10 | 11 | You can access urdf elements like below example. 12 | 13 | ```rust 14 | let urdf_robot = urdf_rs::read_file("sample.urdf").unwrap(); 15 | let links = urdf_robot.links; 16 | println!("{:?}", links[0].visual[0].origin.xyz); 17 | let joints = urdf_robot.joints; 18 | println!("{:?}", joints[0].origin.xyz); 19 | ``` 20 | 21 | ## `OpenRR` Community 22 | 23 | [Here](https://discord.gg/8DAFFKc88B) is a discord server for `OpenRR` users and developers. 24 | -------------------------------------------------------------------------------- /.cspell.json: -------------------------------------------------------------------------------- 1 | { 2 | "version": "0.2", 3 | "gitignoreRoot": ".", 4 | "useGitignore": true, 5 | "dictionaryDefinitions": [ 6 | { 7 | "name": "project-dictionary", 8 | "path": "./.github/.cspell/project-dictionary.txt", 9 | "addWords": true 10 | }, 11 | { 12 | "name": "rust-dependencies", 13 | "path": "./.github/.cspell/rust-dependencies.txt", 14 | "addWords": true 15 | } 16 | ], 17 | "dictionaries": ["project-dictionary", "rust-dependencies"], 18 | "ignoreRegExpList": [ 19 | // Copyright notice 20 | "Copyright .*", 21 | // GHA actions/workflows 22 | "uses: .+@", 23 | // GHA context (repo name, owner name, etc.) 24 | "github.\\w+ (=|!)= '.+'", 25 | // GH username 26 | "( |\\[)@[\\w_-]+", 27 | // Git config username 28 | "git config user.name .*", 29 | // Username in todo comment 30 | "(TODO|FIXME)\\([\\w_., -]+\\)", 31 | // Cargo.toml authors 32 | "authors *= *\\[.*\\]", 33 | "\".* <[\\w_.+-]+@[\\w.-]+>\"" 34 | ], 35 | "languageSettings": [ 36 | { 37 | "languageId": ["*"], 38 | "dictionaries": ["bash", "rust"] 39 | } 40 | ], 41 | "ignorePaths": [ 42 | // Licenses 43 | "**/LICENSE*" 44 | ] 45 | } 46 | -------------------------------------------------------------------------------- /Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | name = "urdf-rs" 3 | # When publishing a new version: 4 | # - Create "v0.x.y" git tag 5 | # - Push the above tag (run `git push origin --tags`) 6 | # Then, CI will publish to crates.io and create a GitHub release. 7 | version = "0.9.0" 8 | authors = ["Takashi Ogura "] 9 | edition = "2021" 10 | description = "URDF parser" 11 | license = "Apache-2.0" 12 | keywords = ["robotics", "robot", "ros", "urdf"] 13 | categories = ["data-structures", "parsing"] 14 | repository = "https://github.com/openrr/urdf-rs" 15 | 16 | # Note: serde is public dependency. 17 | [dependencies] 18 | quick-xml = { version = "0.38", features = ["overlapped-lists", "serialize"] } 19 | regex = "1.4.2" 20 | RustyXML = "0.3.0" 21 | serde = { version = "1.0.118", features = ["derive"] } 22 | serde-xml-rs = "0.6.0" 23 | thiserror = "2.0.0" 24 | 25 | [dev-dependencies] 26 | assert_approx_eq = "1" 27 | 28 | [lints] 29 | workspace = true 30 | 31 | [workspace.lints.rust] 32 | missing_debug_implementations = "warn" 33 | # missing_docs = "warn" # TODO 34 | rust_2018_idioms = "warn" 35 | single_use_lifetimes = "warn" 36 | unreachable_pub = "warn" 37 | [workspace.lints.clippy] 38 | lint_groups_priority = { level = "allow", priority = 1 } # https://github.com/rust-lang/rust-clippy/issues/12920 39 | -------------------------------------------------------------------------------- /src/errors.rs: -------------------------------------------------------------------------------- 1 | use thiserror::Error; 2 | 3 | /// Alias for a `Result` with the error type `UrdfError`. 4 | pub type Result = std::result::Result; 5 | 6 | #[derive(Debug, Error)] 7 | #[error(transparent)] 8 | pub struct UrdfError(#[from] ErrorKind); 9 | 10 | // Hiding error variants from a library's public error type to prevent 11 | // dependency updates from becoming breaking changes. 12 | // We can add `UrdfErrorKind` enum or `is_*` methods that indicate the kind of 13 | // error if needed, but don't expose dependencies' types directly in the 14 | // public API. 15 | #[derive(Debug, Error)] 16 | pub(crate) enum ErrorKind { 17 | #[error(transparent)] 18 | File(#[from] std::io::Error), 19 | #[error(transparent)] 20 | Xml(#[from] serde_xml_rs::Error), 21 | #[error(transparent)] 22 | RustyXml(#[from] xml::BuilderError), 23 | #[error(transparent)] 24 | QuickXmlSe(#[from] quick_xml::SeError), 25 | #[error(transparent)] 26 | QuickXmlDe(#[from] quick_xml::DeError), 27 | #[error("command error {}\n--- stdout\n{}\n--- stderr\n{}", .msg, .stdout, .stderr)] 28 | Command { 29 | msg: String, 30 | stdout: String, 31 | stderr: String, 32 | }, 33 | #[error("{}", .0)] 34 | Other(String), 35 | } 36 | 37 | impl UrdfError { 38 | pub(crate) fn new(err: impl Into) -> Self { 39 | Self(err.into()) 40 | } 41 | } 42 | 43 | impl From for UrdfError { 44 | fn from(err: std::io::Error) -> UrdfError { 45 | ErrorKind::File(err).into() 46 | } 47 | } 48 | 49 | impl From<&str> for UrdfError { 50 | fn from(err: &str) -> UrdfError { 51 | ErrorKind::Other(err.to_owned()).into() 52 | } 53 | } 54 | 55 | impl From for UrdfError { 56 | fn from(err: String) -> UrdfError { 57 | ErrorKind::Other(err).into() 58 | } 59 | } 60 | 61 | impl From for UrdfError { 62 | fn from(err: std::string::FromUtf8Error) -> UrdfError { 63 | ErrorKind::Other(err.to_string()).into() 64 | } 65 | } 66 | 67 | // Note: These implementations are intentionally not-exist to prevent dependency 68 | // updates from becoming breaking changes. 69 | // impl From for UrdfError 70 | // impl From for UrdfError 71 | -------------------------------------------------------------------------------- /.github/workflows/ci.yml: -------------------------------------------------------------------------------- 1 | name: CI 2 | 3 | permissions: 4 | contents: read 5 | 6 | on: 7 | push: 8 | branches: [main] 9 | pull_request: 10 | branches: [main] 11 | schedule: 12 | - cron: '0 15 * * 0,4' # Every Monday and Friday at 00:00 JST 13 | 14 | env: 15 | CARGO_INCREMENTAL: 0 16 | CARGO_NET_RETRY: 10 17 | CARGO_TERM_COLOR: always 18 | RUST_BACKTRACE: 1 19 | RUSTDOCFLAGS: -D warnings 20 | RUSTFLAGS: -D warnings 21 | RUSTUP_MAX_RETRIES: 10 22 | 23 | defaults: 24 | run: 25 | shell: bash 26 | 27 | concurrency: 28 | group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.sha }} 29 | cancel-in-progress: true 30 | 31 | jobs: 32 | test: 33 | runs-on: ubuntu-latest 34 | timeout-minutes: 60 35 | steps: 36 | - uses: actions/checkout@v6 37 | - uses: dtolnay/rust-toolchain@stable 38 | with: 39 | components: clippy,rustfmt 40 | - run: cargo fmt --all --check 41 | - run: cargo clippy --all-targets 42 | - run: cargo build 43 | - run: cargo test 44 | 45 | spell-check: 46 | runs-on: ubuntu-latest 47 | timeout-minutes: 60 48 | permissions: 49 | contents: write 50 | pull-requests: write 51 | steps: 52 | - uses: actions/checkout@v6 53 | - run: echo "REMOVE_UNUSED_WORDS=1" >>"${GITHUB_ENV}" 54 | if: github.repository_owner == 'openrr' && (github.event_name == 'schedule' || github.event_name == 'push' && github.ref == 'refs/heads/main') 55 | - run: tools/spell-check.sh 56 | - id: diff 57 | run: | 58 | set -euo pipefail 59 | git config user.name "Taiki Endo" 60 | git config user.email "taiki@smilerobotics.com" 61 | git add -N .github/.cspell 62 | if ! git diff --exit-code -- .github/.cspell; then 63 | git add .github/.cspell 64 | git commit -m "Update cspell dictionary" 65 | echo 'success=false' >>"${GITHUB_OUTPUT}" 66 | fi 67 | if: github.repository_owner == 'openrr' && (github.event_name == 'schedule' || github.event_name == 'push' && github.ref == 'refs/heads/main') 68 | - uses: peter-evans/create-pull-request@v8 69 | with: 70 | title: Update cspell dictionary 71 | body: | 72 | Auto-generated by [create-pull-request][1] 73 | [Please close and immediately reopen this pull request to run CI.][2] 74 | 75 | [1]: https://github.com/peter-evans/create-pull-request 76 | [2]: https://github.com/peter-evans/create-pull-request/blob/main/docs/concepts-guidelines.md#workarounds-to-trigger-further-workflow-runs 77 | branch: update-cspell-dictionary 78 | if: github.repository_owner == 'openrr' && (github.event_name == 'schedule' || github.event_name == 'push' && github.ref == 'refs/heads/main') && steps.diff.outputs.success == 'false' 79 | -------------------------------------------------------------------------------- /sample.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | -------------------------------------------------------------------------------- /tools/spell-check.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # shellcheck disable=SC2046 3 | set -euo pipefail 4 | IFS=$'\n\t' 5 | cd "$(dirname "$0")"/.. 6 | 7 | # Usage: 8 | # ./tools/spell-check.sh 9 | 10 | check_diff() { 11 | if [[ -n "${CI:-}" ]]; then 12 | if ! git --no-pager diff --exit-code "$@"; then 13 | should_fail=1 14 | fi 15 | else 16 | if ! git --no-pager diff --exit-code "$@" &>/dev/null; then 17 | should_fail=1 18 | fi 19 | fi 20 | } 21 | error() { 22 | if [[ -n "${GITHUB_ACTIONS:-}" ]]; then 23 | echo "::error::$*" 24 | else 25 | echo >&2 "error: $*" 26 | fi 27 | should_fail=1 28 | } 29 | warn() { 30 | if [[ -n "${GITHUB_ACTIONS:-}" ]]; then 31 | echo "::warning::$*" 32 | else 33 | echo >&2 "warning: $*" 34 | fi 35 | } 36 | 37 | project_dictionary=.github/.cspell/project-dictionary.txt 38 | has_rust='' 39 | if [[ -n "$(git ls-files '*Cargo.toml')" ]]; then 40 | has_rust='1' 41 | dependencies='' 42 | for manifest_path in $(git ls-files '*Cargo.toml'); do 43 | if [[ "${manifest_path}" != "Cargo.toml" ]] && ! grep -Eq '\[workspace\]' "${manifest_path}"; then 44 | continue 45 | fi 46 | metadata=$(cargo metadata --format-version=1 --all-features --no-deps --manifest-path "${manifest_path}") 47 | for id in $(jq <<<"${metadata}" '.workspace_members[]'); do 48 | dependencies+="$(jq <<<"${metadata}" ".packages[] | select(.id == ${id})" | jq -r '.dependencies[].name')"$'\n' 49 | done 50 | done 51 | # shellcheck disable=SC2001 52 | dependencies=$(sed <<<"${dependencies}" 's/[0-9_-]/\n/g' | LC_ALL=C sort -f -u) 53 | fi 54 | config_old=$(<.cspell.json) 55 | config_new=$(grep <<<"${config_old}" -v '^ *//' | jq 'del(.dictionaries[])' | jq 'del(.dictionaryDefinitions[])') 56 | trap -- 'echo "${config_old}" >.cspell.json; echo >&2 "$0: trapped SIGINT"; exit 1' SIGINT 57 | echo "${config_new}" >.cspell.json 58 | if [[ -n "${has_rust}" ]]; then 59 | dependencies_words=$(npx <<<"${dependencies}" -y cspell stdin --no-progress --no-summary --words-only --unique || true) 60 | fi 61 | all_words=$(npx -y cspell --no-progress --no-summary --words-only --unique $(git ls-files | (grep -v "${project_dictionary//\./\\.}" || true)) || true) 62 | echo "${config_old}" >.cspell.json 63 | trap - SIGINT 64 | cat >.github/.cspell/rust-dependencies.txt <>.github/.cspell/rust-dependencies.txt 70 | fi 71 | if [[ -z "${REMOVE_UNUSED_WORDS:-}" ]]; then 72 | check_diff .github/.cspell/rust-dependencies.txt 73 | fi 74 | 75 | echo "+ npx -y cspell --no-progress --no-summary \$(git ls-files)" 76 | if ! npx -y cspell --no-progress --no-summary $(git ls-files); then 77 | error "spellcheck failed: please fix uses of below words or add to ${project_dictionary} if correct" 78 | echo >&2 "=======================================" 79 | (npx -y cspell --no-progress --no-summary --words-only $(git ls-files) || true) | LC_ALL=C sort -f -u >&2 80 | echo >&2 "=======================================" 81 | echo >&2 82 | fi 83 | 84 | # Make sure the project-specific dictionary does not contain duplicated words. 85 | for dictionary in .github/.cspell/*.txt; do 86 | if [[ "${dictionary}" == "${project_dictionary}" ]]; then 87 | continue 88 | fi 89 | dup=$(sed '/^$/d' "${project_dictionary}" "${dictionary}" | LC_ALL=C sort -f | uniq -d -i | (grep -v '//.*' || true)) 90 | if [[ -n "${dup}" ]]; then 91 | error "duplicated words in dictionaries; please remove the following words from ${project_dictionary}" 92 | echo >&2 "=======================================" 93 | echo >&2 "${dup}" 94 | echo >&2 "=======================================" 95 | echo >&2 96 | fi 97 | done 98 | 99 | # Make sure the project-specific dictionary does not contain unused words. 100 | if [[ -n "${REMOVE_UNUSED_WORDS:-}" ]]; then 101 | grep_args=() 102 | for word in $(grep -v '//.*' "${project_dictionary}" || true); do 103 | if ! grep <<<"${all_words}" -Eq -i "^${word}$"; then 104 | # TODO: single pattern with ERE: ^(word1|word2..)$ 105 | grep_args+=(-e "^${word}$") 106 | fi 107 | done 108 | if [[ ${#grep_args[@]} -gt 0 ]]; then 109 | warn "removing unused words from ${project_dictionary}" 110 | res=$(grep -v "${grep_args[@]}" "${project_dictionary}") 111 | echo "${res}" >"${project_dictionary}" 112 | fi 113 | else 114 | unused='' 115 | for word in $(grep -v '//.*' "${project_dictionary}" || true); do 116 | if ! grep <<<"${all_words}" -Eq -i "^${word}$"; then 117 | unused+="${word}"$'\n' 118 | fi 119 | done 120 | if [[ -n "${unused}" ]]; then 121 | warn "unused words in dictionaries; please remove the following words from ${project_dictionary}" 122 | echo >&2 "=======================================" 123 | echo >&2 -n "${unused}" 124 | echo >&2 "=======================================" 125 | echo >&2 126 | fi 127 | fi 128 | 129 | if [[ -n "${should_fail:-}" ]]; then 130 | exit 1 131 | fi 132 | -------------------------------------------------------------------------------- /src/utils.rs: -------------------------------------------------------------------------------- 1 | use crate::deserialize::Robot; 2 | use crate::errors::*; 3 | use crate::funcs::*; 4 | 5 | use regex::Regex; 6 | use std::borrow::Cow; 7 | use std::path::Path; 8 | use std::process::Command; 9 | use std::sync::LazyLock; 10 | 11 | pub fn convert_xacro_to_urdf_with_args

(filename: P, args: &[(String, String)]) -> Result 12 | where 13 | P: AsRef, 14 | { 15 | let filename = filename.as_ref(); 16 | let output = Command::new("rosrun") 17 | .args(["xacro", "xacro", "--inorder"]) 18 | .arg(filename) 19 | .args(args.iter().map(|(k, v)| format!("{k}:={v}"))) 20 | .output() 21 | .or_else(|_| { 22 | Command::new("xacro") 23 | .arg(filename) 24 | .args(args.iter().map(|(k, v)| format!("{k}:={v}"))) 25 | .output() 26 | }) 27 | .map_err(|e| { 28 | format!("failed to execute xacro; consider installing xacro by `apt-get install ros-*-xacro`: {e}") 29 | })?; 30 | if output.status.success() { 31 | Ok(String::from_utf8(output.stdout)?) 32 | } else { 33 | Err(ErrorKind::Command { 34 | msg: format!("failed to xacro for {}", filename.display()), 35 | stdout: String::from_utf8_lossy(&output.stdout).into_owned(), 36 | stderr: String::from_utf8_lossy(&output.stderr).into_owned(), 37 | } 38 | .into()) 39 | } 40 | } 41 | 42 | pub fn convert_xacro_to_urdf

(filename: P) -> Result 43 | where 44 | P: AsRef, 45 | { 46 | convert_xacro_to_urdf_with_args(filename, &[]) 47 | } 48 | 49 | pub fn rospack_find(package: &str) -> Result { 50 | let output = Command::new("rospack") 51 | .arg("find") 52 | .arg(package) 53 | .output() 54 | // support ROS2 55 | .or_else(|_| { 56 | Command::new("ros2") 57 | .args(["pkg", "prefix", "--share"]) 58 | .arg(package) 59 | .output() 60 | }) 61 | .map_err(|e| { 62 | format!("failed to execute neither `rospack` nor `ros2 pkg`; consider installing ROS or replacing 'package://' with path: {e}") 63 | })?; 64 | if output.status.success() { 65 | Ok(String::from_utf8(output.stdout)?.trim().to_string()) 66 | } else { 67 | Err(ErrorKind::Command { 68 | msg: format!("failed to find ros package {package}"), 69 | stdout: String::from_utf8_lossy(&output.stdout).into_owned(), 70 | stderr: String::from_utf8_lossy(&output.stderr).into_owned(), 71 | } 72 | .into()) 73 | } 74 | } 75 | 76 | // Note: We return Result here, although there is currently no branch that returns an error. 77 | // This is to avoid a breaking change when changing the error about package:// from panic to error. 78 | pub fn expand_package_path<'a>(filename: &'a str, base_dir: Option<&Path>) -> Result> { 79 | static RE: LazyLock = LazyLock::new(|| Regex::new("^package://(\\w+)/").unwrap()); 80 | 81 | Ok(if filename.starts_with("package://") { 82 | RE.replace(filename, |ma: ®ex::Captures<'_>| { 83 | // TODO: It's better to propagate the error to the caller, 84 | // but regex doesn't provide API like try_replace. 85 | let found_path = rospack_find(&ma[1]).unwrap(); 86 | found_path + "/" 87 | }) 88 | } else if filename.starts_with("https://") || filename.starts_with("http://") { 89 | filename.into() 90 | } else if let Some(abs_path) = filename.strip_prefix("file://") { 91 | abs_path.into() 92 | } else if let Some(base_dir) = base_dir { 93 | let mut relative_path_from_urdf = base_dir.to_owned(); 94 | relative_path_from_urdf.push(filename); 95 | relative_path_from_urdf 96 | .into_os_string() 97 | .into_string() 98 | .unwrap() 99 | .into() 100 | } else { 101 | filename.into() 102 | }) 103 | } 104 | 105 | pub fn read_urdf_or_xacro_with_args

(input_path: P, args: &[(String, String)]) -> Result 106 | where 107 | P: AsRef, 108 | { 109 | let input_path = input_path.as_ref(); 110 | if let Some(ext) = input_path.extension() { 111 | if ext == "xacro" { 112 | let urdf_utf = convert_xacro_to_urdf_with_args(input_path, args)?; 113 | read_from_string(&urdf_utf) 114 | } else { 115 | read_file(input_path) 116 | } 117 | } else { 118 | Err(ErrorKind::Other(format!( 119 | "failed to get extension from {}", 120 | input_path.display() 121 | )) 122 | .into()) 123 | } 124 | } 125 | 126 | pub fn read_urdf_or_xacro

(input_path: P) -> Result 127 | where 128 | P: AsRef, 129 | { 130 | read_urdf_or_xacro_with_args(input_path, &[]) 131 | } 132 | 133 | #[test] 134 | fn it_works() { 135 | // test only for not packages 136 | assert_eq!(expand_package_path("home/aaa", None).unwrap(), "home/aaa"); 137 | assert_eq!( 138 | expand_package_path("home/aaa.obj", Some(Path::new(""))).unwrap(), 139 | "home/aaa.obj" 140 | ); 141 | assert_eq!( 142 | expand_package_path("home/aaa.obj", Some(Path::new("/var"))).unwrap(), 143 | "/var/home/aaa.obj" 144 | ); 145 | assert_eq!( 146 | expand_package_path("/home/aaa.obj", Some(Path::new(""))).unwrap(), 147 | "/home/aaa.obj" 148 | ); 149 | assert_eq!( 150 | expand_package_path("file:///home/aaa.obj", Some(Path::new("/var"))).unwrap(), 151 | "/home/aaa.obj" 152 | ); 153 | assert_eq!( 154 | expand_package_path("http://aaa.obj", Some(Path::new("/var"))).unwrap(), 155 | "http://aaa.obj" 156 | ); 157 | assert_eq!( 158 | expand_package_path("https://aaa.obj", Some(Path::new("/var"))).unwrap(), 159 | "https://aaa.obj" 160 | ); 161 | assert!(read_urdf_or_xacro("sample.urdf").is_ok()); 162 | assert!(read_urdf_or_xacro("sample_urdf").is_err()); 163 | } 164 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /src/funcs.rs: -------------------------------------------------------------------------------- 1 | use crate::deserialize::*; 2 | use crate::errors::*; 3 | use serde::Serialize; 4 | 5 | use std::mem; 6 | use std::path::Path; 7 | 8 | /// sort and to avoid the [issue](https://github.com/RReverser/serde-xml-rs/issues/5) 9 | fn sort_link_joint(string: &str) -> Result { 10 | let mut e: xml::Element = string.parse().map_err(UrdfError::new)?; 11 | let mut links = Vec::new(); 12 | let mut joints = Vec::new(); 13 | let mut materials = Vec::new(); 14 | for c in mem::take(&mut e.children) { 15 | if let xml::Xml::ElementNode(xml_elm) = c { 16 | if xml_elm.name == "link" { 17 | links.push(sort_visual_collision(xml_elm)); 18 | } else if xml_elm.name == "joint" { 19 | joints.push(xml::Xml::ElementNode(xml_elm)); 20 | } else if xml_elm.name == "material" { 21 | materials.push(xml::Xml::ElementNode(xml_elm)); 22 | } 23 | } 24 | } 25 | let mut new_elm = e; 26 | links.extend(joints); 27 | links.extend(materials); 28 | new_elm.children = links; 29 | Ok(format!("{new_elm}")) 30 | } 31 | 32 | fn sort_visual_collision(mut elm: xml::Element) -> xml::Xml { 33 | let mut visuals = Vec::new(); 34 | let mut collisions = Vec::new(); 35 | for c in mem::take(&mut elm.children) { 36 | if let xml::Xml::ElementNode(xml_elm) = c { 37 | if xml_elm.name == "visual" || xml_elm.name == "inertial" { 38 | visuals.push(xml::Xml::ElementNode(xml_elm)); 39 | } else if xml_elm.name == "collision" { 40 | collisions.push(xml::Xml::ElementNode(xml_elm)); 41 | } 42 | } 43 | } 44 | let mut new_elm = elm; 45 | visuals.extend(collisions); 46 | new_elm.children = visuals; 47 | xml::Xml::ElementNode(new_elm) 48 | } 49 | 50 | /// Read urdf file and create Robot instance 51 | /// 52 | /// # Examples 53 | /// 54 | /// ``` 55 | /// let urdf_robot = urdf_rs::read_file("sample.urdf").unwrap(); 56 | /// let links = urdf_robot.links; 57 | /// println!("{:?}", links[0].visual[0].origin.xyz); 58 | /// ``` 59 | pub fn read_file>(path: P) -> Result { 60 | read_from_string(&std::fs::read_to_string(path)?) 61 | } 62 | 63 | /// Read from string instead of file. 64 | /// 65 | /// 66 | /// # Examples 67 | /// 68 | /// ``` 69 | /// let s = r#" 70 | /// 71 | /// 72 | /// 73 | /// 74 | /// 75 | /// 76 | /// 77 | /// 78 | /// 79 | /// 80 | /// 81 | /// 82 | /// 83 | /// 84 | /// 85 | /// 86 | /// 87 | /// 88 | /// 89 | /// 90 | /// 91 | /// 92 | /// 93 | /// 94 | /// 95 | /// 96 | /// 97 | /// 98 | /// 99 | /// 100 | /// 101 | /// 102 | /// 103 | /// 104 | /// 105 | /// 106 | /// 107 | /// 108 | /// 109 | /// 110 | /// "#; 111 | /// let urdf_robot = urdf_rs::read_from_string(s).unwrap(); 112 | /// println!("{:?}", urdf_robot.links[0].visual[0].origin.xyz); 113 | /// ``` 114 | pub fn read_from_string(string: &str) -> Result { 115 | let sorted_string = sort_link_joint(string)?; 116 | serde_xml_rs::from_str(&sorted_string).map_err(UrdfError::new) 117 | } 118 | 119 | pub fn write_to_string(robot: &Robot) -> Result { 120 | let mut buffer = String::new(); 121 | let mut s = quick_xml::se::Serializer::new(&mut buffer); 122 | s.indent(' ', 2); 123 | robot.serialize(s).map_err(UrdfError::new)?; 124 | Ok(buffer) 125 | } 126 | 127 | #[cfg(test)] 128 | mod tests { 129 | use crate::{read_from_string, write_to_string}; 130 | use crate::{Geometry, JointType, Robot}; 131 | use assert_approx_eq::assert_approx_eq; 132 | 133 | fn check_robot(robot: &Robot) { 134 | assert_eq!(robot.name, "robot"); 135 | 136 | // 137 | assert_eq!(robot.links.len(), 3); 138 | let link = &robot.links[0]; 139 | assert_eq!(link.name, "shoulder1"); 140 | let xyz = link.inertial.origin.xyz; 141 | assert_approx_eq!(xyz[0], 0.0); 142 | assert_approx_eq!(xyz[1], 0.0); 143 | assert_approx_eq!(xyz[2], 0.5); 144 | let rpy = link.inertial.origin.rpy; 145 | assert_approx_eq!(rpy[0], 0.0); 146 | assert_approx_eq!(rpy[1], 0.0); 147 | assert_approx_eq!(rpy[2], 0.0); 148 | assert_approx_eq!(link.inertial.mass.value, 1.0); 149 | assert_approx_eq!(link.inertial.inertia.ixx, 100.0); 150 | assert_approx_eq!(link.inertial.inertia.ixy, 0.0); 151 | assert_approx_eq!(link.inertial.inertia.ixz, 0.0); 152 | assert_approx_eq!(link.inertial.inertia.iyy, 100.0); 153 | assert_approx_eq!(link.inertial.inertia.ixz, 0.0); 154 | assert_approx_eq!(link.inertial.inertia.izz, 100.0); 155 | 156 | assert_eq!(link.visual.len(), 3); 157 | let xyz = &link.visual[0].origin.xyz; 158 | assert_approx_eq!(xyz[0], 0.1); 159 | assert_approx_eq!(xyz[1], 0.2); 160 | assert_approx_eq!(xyz[2], 0.3); 161 | let rpy = &link.visual[0].origin.rpy; 162 | assert_approx_eq!(rpy[0], -0.1); 163 | assert_approx_eq!(rpy[1], -0.2); 164 | assert_approx_eq!(rpy[2], -0.3); 165 | 166 | // https://github.com/openrr/urdf-rs/issues/94 167 | let xyz = &link.visual[1].origin.xyz; 168 | assert_approx_eq!(xyz[0], 0.1); 169 | assert_approx_eq!(xyz[1], 0.2); 170 | assert_approx_eq!(xyz[2], 0.3); 171 | let rpy = &link.visual[1].origin.rpy; 172 | assert_approx_eq!(rpy[0], -0.1); 173 | assert_approx_eq!(rpy[1], -0.2); 174 | assert_approx_eq!(rpy[2], -0.3); 175 | 176 | let xyz = &link.visual[2].origin.xyz; 177 | assert_approx_eq!(xyz[0], 0.1); 178 | assert_approx_eq!(xyz[1], 0.2); 179 | assert_approx_eq!(xyz[2], 0.3); 180 | let rpy = &link.visual[2].origin.rpy; 181 | assert_approx_eq!(rpy[0], -0.1); 182 | assert_approx_eq!(rpy[1], -0.2); 183 | assert_approx_eq!(rpy[2], -0.3); 184 | 185 | // https://github.com/openrr/urdf-rs/issues/95 186 | assert!(link.visual[0].material.is_some()); 187 | let mat = link.visual[0].material.as_ref().unwrap(); 188 | assert_eq!(mat.name, "Cyan"); 189 | let rgba = mat.color.clone().unwrap().rgba; 190 | assert_approx_eq!(rgba[0], 0.0); 191 | assert_approx_eq!(rgba[1], 1.0); 192 | assert_approx_eq!(rgba[2], 1.0); 193 | assert_approx_eq!(rgba[3], 1.0); 194 | 195 | match &link.visual[0].geometry { 196 | Geometry::Box { size } => { 197 | assert_approx_eq!(size[0], 1.0f64); 198 | assert_approx_eq!(size[1], 2.0f64); 199 | assert_approx_eq!(size[2], 3.0f64); 200 | } 201 | _ => panic!("geometry error"), 202 | } 203 | match &link.visual[1].geometry { 204 | Geometry::Mesh { 205 | ref filename, 206 | scale, 207 | } => { 208 | assert_eq!(filename, "aa.dae"); 209 | assert!(scale.is_none()); 210 | } 211 | _ => panic!("geometry error"), 212 | } 213 | match &link.visual[2].geometry { 214 | Geometry::Mesh { 215 | ref filename, 216 | scale, 217 | } => { 218 | assert_eq!(filename, "bbb.dae"); 219 | let scale = scale.as_ref().unwrap(); 220 | assert_approx_eq!(scale[0], 2.0); 221 | assert_approx_eq!(scale[1], 3.0); 222 | assert_approx_eq!(scale[2], 4.0); 223 | } 224 | _ => panic!("geometry error"), 225 | } 226 | 227 | assert_eq!(link.collision.len(), 1); 228 | let xyz = &link.collision[0].origin.xyz; 229 | assert_approx_eq!(xyz[0], 0.0); 230 | assert_approx_eq!(xyz[1], 0.0); 231 | assert_approx_eq!(xyz[2], 0.0); 232 | let rpy = &link.collision[0].origin.rpy; 233 | assert_approx_eq!(rpy[0], 0.0); 234 | assert_approx_eq!(rpy[1], 0.0); 235 | assert_approx_eq!(rpy[2], 0.0); 236 | match &link.collision[0].geometry { 237 | Geometry::Cylinder { radius, length } => { 238 | assert_approx_eq!(radius, 1.0); 239 | assert_approx_eq!(length, 0.5); 240 | } 241 | _ => panic!("geometry error"), 242 | } 243 | 244 | assert_eq!(robot.links[1].name, "elbow1"); 245 | assert_eq!(robot.links[2].name, "wrist1"); 246 | 247 | // 248 | assert_eq!(robot.materials.len(), 1); 249 | let mat = &robot.materials[0]; 250 | assert_eq!(mat.name, "blue"); 251 | assert!(mat.color.is_some()); 252 | let rgba = mat.color.clone().unwrap().rgba; 253 | assert_approx_eq!(rgba[0], 0.0); 254 | assert_approx_eq!(rgba[1], 0.0); 255 | assert_approx_eq!(rgba[2], 0.8); 256 | assert_approx_eq!(rgba[3], 1.0); 257 | 258 | // 259 | assert_eq!(robot.joints.len(), 2); 260 | let joint = &robot.joints[0]; 261 | assert_eq!(joint.name, "shoulder_pitch"); 262 | assert_eq!(joint.parent.link, "shoulder1"); 263 | assert_eq!(joint.child.link, "elbow1"); 264 | assert_eq!(joint.joint_type, JointType::Revolute); 265 | assert_approx_eq!(joint.limit.upper, 1.0); 266 | assert_approx_eq!(joint.limit.lower, -1.0); 267 | assert_approx_eq!(joint.limit.effort, 0.0); 268 | assert_approx_eq!(joint.limit.velocity, 1.0); 269 | assert_eq!(joint.calibration.as_ref().unwrap().rising, None); 270 | assert_eq!(joint.calibration.as_ref().unwrap().falling, None); 271 | assert_approx_eq!(joint.dynamics.as_ref().unwrap().damping, 0.0); 272 | assert_approx_eq!(joint.dynamics.as_ref().unwrap().friction, 0.0); 273 | assert_eq!(joint.mimic.as_ref().unwrap().joint, "elbow1"); 274 | assert_approx_eq!(joint.safety_controller.as_ref().unwrap().k_velocity, 10.0); 275 | assert!(joint.mimic.as_ref().unwrap().multiplier.is_none()); 276 | assert!(joint.mimic.as_ref().unwrap().offset.is_none()); 277 | let xyz = &joint.axis.xyz; 278 | assert_approx_eq!(xyz[0], 0.0); 279 | assert_approx_eq!(xyz[1], 1.0); 280 | assert_approx_eq!(xyz[2], -1.0); 281 | 282 | let joint = &robot.joints[1]; 283 | assert_eq!(joint.name, "shoulder_pitch"); 284 | assert_eq!(joint.parent.link, "elbow1"); 285 | assert_eq!(joint.child.link, "wrist1"); 286 | assert_eq!(joint.joint_type, JointType::Revolute); 287 | assert_approx_eq!(joint.limit.upper, 1.0); 288 | assert_approx_eq!(joint.limit.lower, -2.0); 289 | assert_approx_eq!(joint.limit.effort, 0.0); 290 | assert_approx_eq!(joint.limit.velocity, 1.0); 291 | assert_approx_eq!(joint.dynamics.as_ref().unwrap().damping, 10.0); 292 | assert_approx_eq!(joint.dynamics.as_ref().unwrap().friction, 1.0); 293 | assert_eq!(joint.mimic.as_ref().unwrap().joint, "shoulder1"); 294 | assert_approx_eq!(joint.mimic.as_ref().unwrap().multiplier.unwrap(), 5.0); 295 | assert_approx_eq!(joint.mimic.as_ref().unwrap().offset.unwrap(), 1.0); 296 | assert_approx_eq!(joint.safety_controller.as_ref().unwrap().k_position, 10.0); 297 | assert_approx_eq!(joint.safety_controller.as_ref().unwrap().k_velocity, 1.0); 298 | assert_approx_eq!( 299 | joint.safety_controller.as_ref().unwrap().soft_lower_limit, 300 | -0.5 301 | ); 302 | assert_approx_eq!( 303 | joint.safety_controller.as_ref().unwrap().soft_upper_limit, 304 | 1.0 305 | ); 306 | let xyz = &joint.axis.xyz; 307 | assert_approx_eq!(xyz[0], 0.0); 308 | assert_approx_eq!(xyz[1], 1.0); 309 | assert_approx_eq!(xyz[2], 0.0); 310 | } 311 | 312 | #[test] 313 | fn deserialization() { 314 | let s = r#" 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | 324 | 325 | 326 | 327 | 328 | 329 | 330 | 331 | 332 | 333 | 334 | 335 | 336 | 337 | 338 | 339 | 340 | 341 | 342 | 343 | 344 | 345 | 346 | 347 | 348 | 349 | 350 | 351 | 352 | 353 | 354 | 355 | 356 | 357 | 358 | 359 | 360 | 361 | 362 | 363 | 364 | 365 | 366 | 367 | 368 | 369 | 370 | 371 | 372 | 373 | 374 | 375 | 376 | 377 | 378 | 379 | "#; 380 | let robot = read_from_string(s).unwrap(); 381 | check_robot(&robot); 382 | 383 | // Loopback test 384 | let s = write_to_string(&robot).unwrap(); 385 | assert!(!s.contains("Robot"), "{s}"); // https://github.com/openrr/urdf-rs/issues/80 386 | let robot = read_from_string(&s).unwrap(); 387 | check_robot(&robot); 388 | } 389 | } 390 | -------------------------------------------------------------------------------- /src/deserialize.rs: -------------------------------------------------------------------------------- 1 | use serde::de::Visitor; 2 | use serde::{Deserialize, Deserializer, Serialize}; 3 | 4 | use std::ops::{Deref, DerefMut}; 5 | 6 | #[derive(Debug, Deserialize, Serialize, Default, Clone)] 7 | pub struct Mass { 8 | #[serde(rename(serialize = "@value"))] 9 | #[serde(deserialize_with = "de_f64")] 10 | pub value: f64, 11 | } 12 | 13 | #[derive(Debug, Deserialize, Serialize, Default, Clone)] 14 | pub struct Inertia { 15 | #[serde(rename(serialize = "@ixx"))] 16 | #[serde(deserialize_with = "de_f64")] 17 | pub ixx: f64, 18 | #[serde(rename(serialize = "@ixy"))] 19 | #[serde(deserialize_with = "de_f64")] 20 | pub ixy: f64, 21 | #[serde(rename(serialize = "@ixz"))] 22 | #[serde(deserialize_with = "de_f64")] 23 | pub ixz: f64, 24 | #[serde(rename(serialize = "@iyy"))] 25 | #[serde(deserialize_with = "de_f64")] 26 | pub iyy: f64, 27 | #[serde(rename(serialize = "@iyz"))] 28 | #[serde(deserialize_with = "de_f64")] 29 | pub iyz: f64, 30 | #[serde(rename(serialize = "@izz"))] 31 | #[serde(deserialize_with = "de_f64")] 32 | pub izz: f64, 33 | } 34 | 35 | #[derive(Debug, Deserialize, Serialize, Default, Clone)] 36 | pub struct Inertial { 37 | #[serde(default)] 38 | pub origin: Pose, 39 | pub mass: Mass, 40 | pub inertia: Inertia, 41 | } 42 | 43 | #[derive(Debug, Deserialize, Clone)] 44 | #[serde(rename_all = "snake_case")] 45 | pub enum Geometry { 46 | Box { 47 | size: Vec3, 48 | }, 49 | Cylinder { 50 | #[serde(deserialize_with = "de_f64")] 51 | radius: f64, 52 | #[serde(deserialize_with = "de_f64")] 53 | length: f64, 54 | }, 55 | Capsule { 56 | #[serde(deserialize_with = "de_f64")] 57 | radius: f64, 58 | #[serde(deserialize_with = "de_f64")] 59 | length: f64, 60 | }, 61 | Sphere { 62 | #[serde(deserialize_with = "de_f64")] 63 | radius: f64, 64 | }, 65 | Mesh { 66 | filename: String, 67 | scale: Option, 68 | }, 69 | } 70 | 71 | impl Default for Geometry { 72 | fn default() -> Self { 73 | Self::Box { 74 | size: Vec3::default(), 75 | } 76 | } 77 | } 78 | 79 | impl Serialize for Geometry { 80 | fn serialize(&self, serializer: S) -> Result 81 | where 82 | S: serde::Serializer, 83 | { 84 | // Workaround for https://github.com/openrr/urdf-rs/pull/107#discussion_r1741875356. 85 | #[derive(Serialize)] 86 | #[serde(rename_all = "snake_case")] 87 | enum GeometryRepr<'a> { 88 | Box { 89 | #[serde(rename(serialize = "@size"))] 90 | size: Vec3, 91 | }, 92 | Cylinder { 93 | #[serde(rename(serialize = "@radius"))] 94 | radius: f64, 95 | #[serde(rename(serialize = "@length"))] 96 | length: f64, 97 | }, 98 | Capsule { 99 | #[serde(rename(serialize = "@radius"))] 100 | radius: f64, 101 | #[serde(rename(serialize = "@length"))] 102 | length: f64, 103 | }, 104 | Sphere { 105 | #[serde(rename(serialize = "@radius"))] 106 | radius: f64, 107 | }, 108 | Mesh { 109 | #[serde(rename(serialize = "@filename"))] 110 | filename: &'a str, 111 | #[serde(rename(serialize = "@scale"), skip_serializing_if = "Option::is_none")] 112 | scale: Option, 113 | }, 114 | } 115 | #[derive(Serialize)] 116 | struct GeometryTag<'a> { 117 | #[serde(rename(serialize = "$value"))] 118 | value: GeometryRepr<'a>, 119 | } 120 | let value = match *self { 121 | Self::Box { size } => GeometryRepr::Box { size }, 122 | Self::Cylinder { radius, length } => GeometryRepr::Cylinder { radius, length }, 123 | Self::Capsule { radius, length } => GeometryRepr::Capsule { radius, length }, 124 | Self::Sphere { radius } => GeometryRepr::Sphere { radius }, 125 | Self::Mesh { 126 | ref filename, 127 | scale, 128 | } => GeometryRepr::Mesh { filename, scale }, 129 | }; 130 | GeometryTag::serialize(&GeometryTag { value }, serializer) 131 | } 132 | } 133 | 134 | #[derive(Debug, Deserialize, Serialize, Default, Clone)] 135 | pub struct Color { 136 | #[serde(rename(serialize = "@rgba"))] 137 | pub rgba: Vec4, 138 | } 139 | 140 | #[derive(Debug, Deserialize, Serialize, Default, Clone)] 141 | pub struct Texture { 142 | #[serde(rename(serialize = "@filename"))] 143 | pub filename: String, 144 | } 145 | 146 | #[derive(Debug, Deserialize, Serialize, Default, Clone)] 147 | pub struct Material { 148 | #[serde(rename(serialize = "@name"))] 149 | pub name: String, 150 | #[serde(skip_serializing_if = "Option::is_none")] 151 | pub color: Option, 152 | #[serde( 153 | rename(serialize = "@texture"), 154 | skip_serializing_if = "Option::is_none" 155 | )] 156 | pub texture: Option, 157 | } 158 | 159 | #[derive(Debug, Deserialize, Serialize, Default, Clone)] 160 | pub struct Visual { 161 | #[serde(rename(serialize = "@name"), skip_serializing_if = "Option::is_none")] 162 | pub name: Option, 163 | #[serde(default)] 164 | pub origin: Pose, 165 | pub geometry: Geometry, 166 | #[serde(skip_serializing_if = "Option::is_none")] 167 | pub material: Option, 168 | } 169 | 170 | #[derive(Debug, Deserialize, Serialize, Default, Clone)] 171 | pub struct Collision { 172 | #[serde(rename(serialize = "@name"), skip_serializing_if = "Option::is_none")] 173 | pub name: Option, 174 | #[serde(default)] 175 | pub origin: Pose, 176 | pub geometry: Geometry, 177 | } 178 | 179 | /// Urdf Link element 180 | /// See for more detail. 181 | #[derive(Debug, Deserialize, Serialize, Clone)] 182 | pub struct Link { 183 | #[serde(rename(serialize = "@name"))] 184 | pub name: String, 185 | #[serde(default)] 186 | pub inertial: Inertial, 187 | #[serde(default)] 188 | pub visual: Vec, 189 | #[serde(default)] 190 | pub collision: Vec, 191 | } 192 | 193 | #[derive(Debug, Serialize, Default, Clone, Copy, PartialEq)] 194 | pub struct Vec3(#[serde(rename(serialize = "$text"))] pub [f64; 3]); 195 | 196 | impl Deref for Vec3 { 197 | type Target = [f64; 3]; 198 | 199 | fn deref(&self) -> &Self::Target { 200 | &self.0 201 | } 202 | } 203 | 204 | impl DerefMut for Vec3 { 205 | fn deref_mut(&mut self) -> &mut Self::Target { 206 | &mut self.0 207 | } 208 | } 209 | 210 | impl<'de> Deserialize<'de> for Vec3 { 211 | fn deserialize(deserializer: D) -> Result 212 | where 213 | D: serde::Deserializer<'de>, 214 | { 215 | deserializer.deserialize_str(Vec3Visitor) 216 | } 217 | } 218 | 219 | struct Vec3Visitor; 220 | impl Visitor<'_> for Vec3Visitor { 221 | type Value = Vec3; 222 | 223 | fn expecting(&self, formatter: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { 224 | formatter.write_str("a string containing three floating point values separated by spaces") 225 | } 226 | 227 | fn visit_str(self, v: &str) -> Result 228 | where 229 | E: serde::de::Error, 230 | { 231 | let split_results: Vec<_> = v 232 | .split_whitespace() 233 | .filter_map(|s| s.parse::().ok()) 234 | .collect(); 235 | if split_results.len() != 3 { 236 | return Err(E::custom(format!( 237 | "Wrong vector element count, expected 3 found {} for [{}]", 238 | split_results.len(), 239 | v 240 | ))); 241 | } 242 | let mut res = [0.0f64; 3]; 243 | res.copy_from_slice(&split_results); 244 | Ok(Vec3(res)) 245 | } 246 | } 247 | 248 | #[derive(Debug, Serialize, Default, Clone, Copy, PartialEq)] 249 | pub struct Vec4(#[serde(rename(serialize = "$text"))] pub [f64; 4]); 250 | 251 | impl Deref for Vec4 { 252 | type Target = [f64; 4]; 253 | 254 | fn deref(&self) -> &Self::Target { 255 | &self.0 256 | } 257 | } 258 | 259 | impl DerefMut for Vec4 { 260 | fn deref_mut(&mut self) -> &mut Self::Target { 261 | &mut self.0 262 | } 263 | } 264 | 265 | impl<'de> Deserialize<'de> for Vec4 { 266 | fn deserialize(deserializer: D) -> Result 267 | where 268 | D: serde::Deserializer<'de>, 269 | { 270 | deserializer.deserialize_str(Vec4Visitor) 271 | } 272 | } 273 | 274 | struct Vec4Visitor; 275 | impl Visitor<'_> for Vec4Visitor { 276 | type Value = Vec4; 277 | 278 | fn expecting(&self, formatter: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { 279 | formatter.write_str("a string containing four floating point values separated by spaces") 280 | } 281 | 282 | fn visit_str(self, v: &str) -> Result 283 | where 284 | E: serde::de::Error, 285 | { 286 | let split_results: Vec<_> = v 287 | .split_whitespace() 288 | .filter_map(|s| s.parse::().ok()) 289 | .collect(); 290 | if split_results.len() != 4 { 291 | return Err(E::custom(format!( 292 | "Wrong vector element count, expected 4 found {} for [{}]", 293 | split_results.len(), 294 | v 295 | ))); 296 | } 297 | let mut res = [0.0f64; 4]; 298 | res.copy_from_slice(&split_results); 299 | Ok(Vec4(res)) 300 | } 301 | } 302 | 303 | #[derive(Debug, Deserialize, Serialize, Clone)] 304 | pub struct Axis { 305 | #[serde(rename(serialize = "@xyz"))] 306 | pub xyz: Vec3, 307 | } 308 | 309 | impl Default for Axis { 310 | fn default() -> Axis { 311 | Axis { 312 | xyz: Vec3([1.0, 0.0, 0.0]), 313 | } 314 | } 315 | } 316 | 317 | #[derive(Debug, Default, Deserialize, Serialize, Clone)] 318 | pub struct Pose { 319 | #[serde(rename(serialize = "@xyz"), default)] 320 | pub xyz: Vec3, 321 | #[serde(rename(serialize = "@rpy"), default)] 322 | pub rpy: Vec3, 323 | } 324 | 325 | #[derive(Debug, Default, Deserialize, Serialize, Clone)] 326 | pub struct LinkName { 327 | #[serde(rename(serialize = "@link"))] 328 | pub link: String, 329 | } 330 | 331 | #[derive(Debug, Default, Deserialize, Serialize, Clone, PartialEq, Eq)] 332 | #[serde(rename_all = "snake_case")] 333 | pub enum JointType { 334 | Revolute, 335 | Continuous, 336 | Prismatic, 337 | #[default] 338 | Fixed, 339 | Floating, 340 | Planar, 341 | Spherical, 342 | } 343 | 344 | #[derive(Debug, Deserialize, Serialize, Default, Clone)] 345 | pub struct JointLimit { 346 | #[serde(rename(serialize = "@lower"), default)] 347 | #[serde(deserialize_with = "de_f64")] 348 | pub lower: f64, 349 | #[serde(rename(serialize = "@upper"), default)] 350 | #[serde(deserialize_with = "de_f64")] 351 | pub upper: f64, 352 | #[serde(rename(serialize = "@effort"), default)] 353 | #[serde(deserialize_with = "de_f64")] 354 | pub effort: f64, 355 | #[serde(rename(serialize = "@velocity"))] 356 | #[serde(deserialize_with = "de_f64")] 357 | pub velocity: f64, 358 | } 359 | 360 | #[derive(Debug, Deserialize, Serialize, Default, Clone)] 361 | pub struct Mimic { 362 | #[serde(rename(serialize = "@joint"))] 363 | pub joint: String, 364 | // `default` is needed when using `deserialize_with`: https://github.com/serde-rs/serde/issues/723#issuecomment-368135287 365 | #[serde( 366 | rename(serialize = "@multiplier"), 367 | default, 368 | skip_serializing_if = "Option::is_none" 369 | )] 370 | #[serde(deserialize_with = "de_opt_f64")] 371 | pub multiplier: Option, 372 | // `default` is needed when using `deserialize_with`: https://github.com/serde-rs/serde/issues/723#issuecomment-368135287 373 | #[serde( 374 | rename(serialize = "@offset"), 375 | default, 376 | skip_serializing_if = "Option::is_none" 377 | )] 378 | #[serde(deserialize_with = "de_opt_f64")] 379 | pub offset: Option, 380 | } 381 | 382 | #[derive(Debug, Deserialize, Serialize, Default, Clone)] 383 | pub struct SafetyController { 384 | #[serde(rename(serialize = "@soft_lower_limit"), default)] 385 | #[serde(deserialize_with = "de_f64")] 386 | pub soft_lower_limit: f64, 387 | #[serde(rename(serialize = "@soft_upper_limit"), default)] 388 | #[serde(deserialize_with = "de_f64")] 389 | pub soft_upper_limit: f64, 390 | #[serde(rename(serialize = "@k_position"), default)] 391 | #[serde(deserialize_with = "de_f64")] 392 | pub k_position: f64, 393 | #[serde(rename(serialize = "@k_velocity"))] 394 | #[serde(deserialize_with = "de_f64")] 395 | pub k_velocity: f64, 396 | } 397 | 398 | /// Urdf Joint element 399 | /// See for more detail. 400 | #[derive(Debug, Deserialize, Serialize, Clone)] 401 | pub struct Joint { 402 | #[serde(rename(serialize = "@name"))] 403 | pub name: String, 404 | #[serde(rename(deserialize = "type", serialize = "@type"))] 405 | pub joint_type: JointType, 406 | #[serde(default)] 407 | pub origin: Pose, 408 | pub parent: LinkName, 409 | pub child: LinkName, 410 | #[serde(default)] 411 | pub axis: Axis, 412 | #[serde(default)] 413 | pub limit: JointLimit, 414 | #[serde(skip_serializing_if = "Option::is_none")] 415 | pub calibration: Option, 416 | #[serde(skip_serializing_if = "Option::is_none")] 417 | pub dynamics: Option, 418 | #[serde(skip_serializing_if = "Option::is_none")] 419 | pub mimic: Option, 420 | #[serde(skip_serializing_if = "Option::is_none")] 421 | pub safety_controller: Option, 422 | } 423 | 424 | #[derive(Debug, Deserialize, Serialize, Default, Clone)] 425 | pub struct Calibration { 426 | // `default` is needed when using `deserialize_with`: https://github.com/serde-rs/serde/issues/723#issuecomment-368135287 427 | #[serde( 428 | rename(serialize = "@rising"), 429 | default, 430 | skip_serializing_if = "Option::is_none" 431 | )] 432 | #[serde(deserialize_with = "de_opt_f64")] 433 | pub rising: Option, 434 | // `default` is needed when using `deserialize_with`: https://github.com/serde-rs/serde/issues/723#issuecomment-368135287 435 | #[serde( 436 | rename(serialize = "@falling"), 437 | default, 438 | skip_serializing_if = "Option::is_none" 439 | )] 440 | #[serde(deserialize_with = "de_opt_f64")] 441 | pub falling: Option, 442 | } 443 | 444 | #[derive(Debug, Deserialize, Serialize, Default, Clone)] 445 | pub struct Dynamics { 446 | #[serde(rename(serialize = "@damping"), default)] 447 | #[serde(deserialize_with = "de_f64")] 448 | pub damping: f64, 449 | #[serde(rename(serialize = "@friction"), default)] 450 | #[serde(deserialize_with = "de_f64")] 451 | pub friction: f64, 452 | } 453 | 454 | /// Top level struct to access urdf. 455 | /// 456 | /// # Compatibility Note 457 | /// 458 | /// This type and its descendant types implement `serde::Serialize` and 459 | /// `serde::Deserialize`. However, because XML serialization and deserialization 460 | /// using `serde` require embedding special markers to distinguish between 461 | /// attributes and elements, there is no stability guarantee as to exactly which 462 | /// `serde` representation these types will be serialized with. 463 | /// 464 | /// In other words, serialization and deserialization without going through 465 | /// functions provided by this crate such as [`read_from_string`](crate::read_from_string) 466 | /// or [`write_to_string`](crate::write_to_string) may not work as expected at all 467 | /// or in the future. 468 | /// 469 | /// This is intentional to remove the need to create semver-incompatible 470 | /// release of this crate every time an XML-related crate, which frequently 471 | /// creates semver-incompatible releases is updated. And it also allows 472 | /// improving our situation of depending on multiple XML-related crates without 473 | /// a change considered breaking. 474 | #[derive(Debug, Deserialize, Serialize, Clone)] 475 | #[serde(rename = "robot")] 476 | pub struct Robot { 477 | #[serde(rename(serialize = "@name"), default)] 478 | pub name: String, 479 | 480 | #[serde(rename = "link", default, skip_serializing_if = "Vec::is_empty")] 481 | pub links: Vec, 482 | 483 | #[serde(rename = "joint", default, skip_serializing_if = "Vec::is_empty")] 484 | pub joints: Vec, 485 | 486 | #[serde(rename = "material", default, skip_serializing_if = "Vec::is_empty")] 487 | pub materials: Vec, 488 | } 489 | 490 | fn de_f64<'de, D>(deserializer: D) -> Result 491 | where 492 | D: Deserializer<'de>, 493 | { 494 | deserializer.deserialize_str(F64Visitor) 495 | } 496 | fn de_opt_f64<'de, D>(deserializer: D) -> Result, D::Error> 497 | where 498 | D: Deserializer<'de>, 499 | { 500 | deserializer.deserialize_option(OptF64Visitor) 501 | } 502 | 503 | struct F64Visitor; 504 | impl Visitor<'_> for F64Visitor { 505 | type Value = f64; 506 | 507 | fn expecting(&self, formatter: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { 508 | formatter.write_str("a string containing one floating point value") 509 | } 510 | 511 | fn visit_str(self, v: &str) -> Result 512 | where 513 | E: serde::de::Error, 514 | { 515 | let res = v.trim().parse::().map_err(E::custom)?; 516 | Ok(res) 517 | } 518 | } 519 | struct OptF64Visitor; 520 | impl<'de> Visitor<'de> for OptF64Visitor { 521 | type Value = Option; 522 | 523 | fn expecting(&self, formatter: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { 524 | formatter.write_str("a string containing one floating point value") 525 | } 526 | 527 | fn visit_some(self, deserializer: D) -> Result 528 | where 529 | D: Deserializer<'de>, 530 | { 531 | deserializer.deserialize_str(F64Visitor).map(Some) 532 | } 533 | 534 | fn visit_none(self) -> Result 535 | where 536 | E: serde::de::Error, 537 | { 538 | Ok(None) 539 | } 540 | } 541 | --------------------------------------------------------------------------------