From ddbc8f7ec86070f18ba29829bc6ff740505e3c83 Mon Sep 17 00:00:00 2001
From: YuxuanLiuTier4Desktop <619684051@qq.com>
Date: Mon, 23 Dec 2024 15:30:04 +0900
Subject: [PATCH] added read me documentation, improve the warning and
 debugging outputs

Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com>
---
 aip_urdf_compiler/readme.md                | 121 +++++++++++++++++++++
 aip_urdf_compiler/scripts/compile_xacro.py |  14 ++-
 2 files changed, 131 insertions(+), 4 deletions(-)
 create mode 100644 aip_urdf_compiler/readme.md

diff --git a/aip_urdf_compiler/readme.md b/aip_urdf_compiler/readme.md
new file mode 100644
index 00000000..011ca13b
--- /dev/null
+++ b/aip_urdf_compiler/readme.md
@@ -0,0 +1,121 @@
+# aip_urdf_compiler
+
+## Overview
+
+The aip_urdf_compiler package provides tools for dynamically generating URDF (Unified Robot Description Format) files from configuration files during the build process. It simplifies sensor model management by automatically URDF models from sensor configurations.
+
+## Key Features
+
+- Dynamic URDF generation during colcon build
+- Automated sensor transform processing
+- Support for multiple sensor types and configurations
+
+## Usage
+
+### Package Integration
+
+To use aip_urdf_compiler in your description package:
+
+- Add the dependency in `package.xml`:
+
+  ```xml
+  <depend>aip_urdf_compiler</depend>
+  ```
+
+- Add the following to `CMakeLists.txt`:
+
+  ```cmake
+  find_package(aip_urdf_compiler REQUIRED)
+  aip_cmake_urdf_compile()
+  ```
+
+- Configure your sensors in `config/sensors.yaml` with required meta values:
+
+  - `type`: Sensor type identifier
+  - `frame_id`: TF frame identifier
+
+- Clean up existing `.xacro` files and add to `.gitignore`:
+
+  ```gitignore
+  # In your URDF folder
+  *.xacro
+  ```
+
+## Architecture
+
+### Components
+
+1. **aip_urdf_compiler**
+
+   - Main package handling URDF generation
+   - Processes configuration files
+   - Manages build-time compilation
+
+2. **aip_cmake_urdf_compile**
+
+   - CMake macro implementation
+   - Creates build targets
+   - Ensures URDF regeneration on each build
+
+3. **compile_xacro.py**
+   - Configuration parser
+   - Transform processor
+   - URDF generator
+
+### Compilation Process
+
+1. **Configuration Reading**
+
+   - Parses `config/sensors.yaml`
+   - Extracts transformation data
+   - Validates configurations
+
+2. **Transform Processing**
+
+   - Processes each sensor transform
+   - Determines sensor types and frame IDs
+   - Generates appropriate macro strings
+   - Creates `sensors.xacro`
+
+3. **Joint Unit Processing**
+   - Handles joint unit transforms
+   - Processes related YAML files
+   - Generates separate URDF xacro files
+
+## Adding New Sensors
+
+1. Add sensor descriptions (xacro files) in either:
+
+   - Your target package
+   - `common_sensor_description` package
+
+2. Update the following in `compile_xacro.py`:
+   - `LinkType` enumeration
+   - `link_dict` mapping
+
+## Troubleshooting
+
+### Debug Logs
+
+Check build logs for debugging information:
+
+```bash
+cat $workspace/log/build_<timestamp>/aip_{project}_description/streams.log
+```
+
+### Common Issues
+
+1. Missing sensor definitions
+
+   - Ensure sensor type is defined in `LinkType`
+   - Verify xacro file exists in description package
+
+2. TF Trees errors
+   - Check frame_id values in sensors.yaml
+   - Verify transform chain completeness
+
+## Contributing
+
+1. Follow ROS coding standards
+2. Test URDF generation with various configurations
+3. Update documentation for new features
diff --git a/aip_urdf_compiler/scripts/compile_xacro.py b/aip_urdf_compiler/scripts/compile_xacro.py
index ba49c66d..c56c4eee 100644
--- a/aip_urdf_compiler/scripts/compile_xacro.py
+++ b/aip_urdf_compiler/scripts/compile_xacro.py
@@ -16,6 +16,7 @@
 from typing import Callable
 from typing import Dict
 from typing import Union
+import warnings
 
 from jinja2 import Template
 import yaml
@@ -82,7 +83,10 @@ def __init__(self, transformation: Dict, base_frame: str, child_frame: str):
             self.name = self.child_frame.replace("_base_link", "").replace("_link", "")
 
             if len(self.type) == 0:
-                self.type = determine_link_type(self.name)
+                self.type = determine_link_type(self.name).value
+                warnings.warn(
+                    f"Warning: Link type not explicitly defined for '{self.name}'. Determining type from link name and obtained {self.type}"
+                )
 
             self.frame_id: str = transformation.get("frame_id", "")
             if len(self.frame_id) == 0:
@@ -401,6 +405,7 @@ def main(
         base_template = Template(file.read())
 
     # Render the template
+    print("Processing the main sensors_calibration.yaml")
     calibration_path = os.path.join(calibration_directory, "sensors_calibration.yaml")
     calib_yaml = load_yaml(calibration_path)
     calib = Calibration(calib_yaml)
@@ -418,6 +423,7 @@ def main(
     for _, transform in calib.transforms.items():
         link_type: LinkType = obtain_link_type(transform)
         if link_type == LinkType.JOINT_UNITS:
+            print(f"Collected joint sensor unit {transform.name}, which will be further rendered.")
             render_meta_data["sensor_units_includes"].append(
                 link_dicts[link_type]["including_file"].format(filename=transform.name)
             )
@@ -430,6 +436,7 @@ def main(
                 }
             )
         else:
+            print(f"Collected {transform.name}.")
             include_text.add(link_dicts[link_type]["including_file"])
             sensor_items.append(link_dicts[link_type]["string_api"](transform))
 
@@ -437,7 +444,6 @@ def main(
     render_meta_data["isolated_sensors"] = sensor_items
 
     rendered = base_template.render(render_meta_data)
-    print(rendered)
 
     print("=====================================")
     # Save the rendered template
@@ -449,6 +455,7 @@ def main(
         sensor_units_template = Template(file.read())
 
     for i, sensor_unit in enumerate(render_meta_data["sensor_units"]):
+        print(f"Processing {sensor_unit['name']}")
         sensor_unit_calib_path = os.path.join(
             calibration_directory, f"{sensor_unit['name']}_calibration.yaml"
         )
@@ -465,13 +472,12 @@ def main(
         for _, transform in sensor_unit_calib.transforms.items():
             link_type: LinkType = obtain_link_type(transform)
             include_text.add(link_dicts[link_type]["including_file"])
-            print(transform.name)
+            print(f"collected {transform.name}")
             sensor_unit_isolated_sensors.append(link_dicts[link_type]["string_api"](transform))
         sensor_unit_render_meta_data["isolated_sensors_includes"] = list(include_text)
         sensor_unit_render_meta_data["isolated_sensors"] = sensor_unit_isolated_sensors
 
         rendered = sensor_units_template.render(sensor_unit_render_meta_data)
-        print(rendered)
         with open(os.path.join(output_directory, f'{sensor_unit["name"]}.xacro'), "w") as file:
             file.write(rendered)
         print("=====================================")