README changes, Add roadmap

Signed-off-by: silanus23 <berkantali23@outlook.com>

Author: silanus23

ADDING_A_ROBOT.md

@@ -104,8 +104,9 @@ Node(
 )
 ```
 
-**This single parameter is the entire connection between an action server and a
-specific physical robot.** Change it and the server plans for a different robot.
+**This parameter defines robot name groups for action servers.** Change it and the server plans for a different robot.
+
+> **Note:** The welder action server currently has some hardcoded robot-specific values such as link names and tool frame references. This is a temporary compromise and will be parameterised in the same way as the gripper server. Adding a second welder before that work is done will require matching those hardcoded values to your robot.
 
 ### 6. Coordinator — connects to action servers by topic name
 

README.md

@@ -1,109 +1,67 @@
 # Hold and Weld
 
-ROS 2 framework for automated dual-arm robotic welding systems that perform holding and welding simultaneously.
+**A ROS 2 framework for automated dual-arm robotic welding systems that perform holding and welding simultaneously with integrated grasp sampler and weld finder. Currently at PoC stage. Targets ROS 2 Jazzy.**
 
-## Overview
+![System Launch](docs/system_launch.GIF)
 
-A modular system for coordinating gripper and welding robots to autonomously manipulate and weld
-workpieces. This repository currently targets **ROS 2 Jazzy**.
-
-## Features
+## Packages
 
-- Automated weld path generation from seam geometry and saving that as a json file
-- Dual-robot coordination with collision avoidance
-- Action-based architecture for flexible workflows
-- Support for multiple joint types (T-joint, butt-joint, corner joint, lap joint, edge joint)
+- [`hold_and_weld_description`](hold_and_weld_description) — robot URDFs, meshes, and models
+- [`hold_and_weld_planning`](hold_and_weld_planning) — weld path generation and seam extraction
+- [`hold_and_weld_gripper_sampler`](hold_and_weld_gripper_sampler) — constraint-aware grasp sampling
+- [`hold_and_weld_application`](hold_and_weld_application) — action servers and workflow coordination
+- [`hold_and_weld_bringup`](hold_and_weld_bringup) — launch files and system bringup
 
 ## Architecture
 
 ![System Architecture](docs/hold_and_weld.png)
 
-## Packages
-
-- **hold_and_weld_description** - Robot URDFs, meshes, and models
-- **hold_and_weld_planning** - Python tools for weld path generation and geometry processing
-- **hold_and_weld_application** - C++ action servers and workflow coordination
-
-## Design Philosophy
-
-This project is designed with the following principles in mind:
-
-- **Separation of concerns**
-  Planning, description, and application logic are split into independent packages to allow
-  iteration and testing without cross-coupling.
-
-- **Action-driven orchestration**
-  Long-running and stateful robot operations are modeled as ROS 2 actions to support
-  feedback, cancellation, and extensibility.
-
-- **Simulation-first development**
-  All core functionality is designed to be testable in simulation before deployment on real hardware.
-
-- **Robot-agnostic extensibility**
-  The system aims to support different robot models and configurations with minimal code changes,
-  relying on URDF and configuration files rather than hardcoded assumptions.
-
-- **Incremental autonomy**
-  Advanced components such as geometry reasoning and behavior trees are introduced progressively,
-  only after a stable execution pipeline is established.
-
-
-## Planned Milestones
-
-### Milestone 1 – Core Stability
-- [ ] Realistic models for external-axis torch and gripper
-- [ ] Action server for moving a robot to a target pose
-- [ ] Utilize execute action messages instead only triggering ones.
-- [ ] Making task yaml monolithic and main source for every package
-- [ ] Replacing time based delay
-
-### Milestone 2 – Configuration & Extensibility
-- [ ] Guidelines for adding robots and jobs
-- [ ] Test coverage for all supported weld joint types
-- [ ] Adding reachability test by adding jacobian calculations
-
-### Milestone 3 – Advanced Autonomy
-- [ ] Behavior Tree–based task orchestration
-- [ ] URDF-driven geometry engine
+## Capabilities
+- Weld seam extraction from CAD and mesh geometry
+- Constraint-aware grasp sampling on exact CAD surfaces
+- Dual-arm coordination connecting both into an executable workcell
 
-## Conceptual Design Notes (Future Work)
 
-This section documents design considerations and architectural directions explored during development and is not a commitment to specific implementations.
+## Results
 
+**Grasp candidates example generated by `hold_and_weld_gripper_sampler`:**
 
-### Geometry Engine
+![Prism Grasp](docs/prism_grasp.png)
 
-Currently the system can extract weld line coordinates and workpiece poses from URDFs alone.
-This iteration depends on defining weld geometry from URDFs and only calculates flat surfaces.
+**Weld seam extraction example via the mesh pipeline in `hold_and_weld_planning`:**
 
-Future work includes making this geometry independent from joint types, choosing non-colliding
-orientations from multiple possibilities, extending to curved surfaces while maintaining
-work/travel angles, and being able to work with mesh files. Eventually this system should be
-capable of creating contemporary art installations with arbitrary weld geometries.
+![Cylinder Weld Finding](docs/cylinder_side.png)
 
+## Quick Start
 
-### Behavior Tree
+## Dependencies
+- [OpenCASCADE 7.9.3](https://github.com/Open-Cascade-SAS/OCCT/tree/OCCT-793)
+- CGAL
+- FCL
+- MoveIt 2
 
-Currently there is a `dual_robot_coordinator` node that is responsible for orchestrating nodes.
-A behavior tree integration and making current action servers, node will help user to create
-multiple step, arbitrary number of robot and object scenes. On top of this a behavior tree can
-help user to create conditional check nodes to avoid failure situations of real life like a
-gripping failure mid mission.
+**2. Build the mesh intersection extension**
 
-## Known Issues
+```bash
+cd hold_and_weld_planning/hold_and_weld_planning/mesh
+mkdir build && cd build
+cmake ..
+make
+cp mesh_intersection*.so ../
+```
 
-- Currently gripper system is showing inconsistencies due to model problems.
-- JSON system can create problems that can be solved by building from cache after creating json. This problem
-will be solved by adding json parserer independent from colcon.
-- Current version is tested in in butt_joint and T-joint only more test cases from contributors are welcomed
-lap_joint's logic could be better in urdf generator.
+**3. Launch the system**
 
-## License
+```bash
+ros2 launch hold_and_weld_bringup system_bringup.launch.py
+```
 
-Apache-2.0
+For adding a new robot to the workcell see [ADDING_A_ROBOT.md](ADDING_A_ROBOT.md).
+For the development roadmap see [ROADMAP.md](ROADMAP.md).
 
-## Author
+## References
 
-Berkan Tali (berkantali23@outlook.com)
+Duff, T., Burgess, B., & Hery, C. (2017). Building an orthonormal basis, revisited.
+*Journal of Computer Graphics Techniques (JCGT)*, 6(1), 1–8.
 
-GitHub: @silanus23
+Robot meshes from Yaskawa Motoman's [motoman_ros2_support_packages](https://github.com/ros-industrial/motoman) (Apache 2.0 / BSD 3-Clause). Thanks Yaskawa.

ROADMAP.md

@@ -0,0 +1,177 @@
+# Roadmap
+
+Items are not ordered by priority. Priority will be determined by integration
+requirements as the system matures.
+
+---
+
+## System
+
+- **Package names and namespaces** — current naming is verbose. Shorter, consistent
+  naming across packages, namespaces, and topics is planned.
+- **OCCT 8 transition** — currently built against OCCT 7.9.3.
+  Transitioning to OCCT 8 is planned to take advantage of API improvements and
+  long-term support.
+- **Test coverage** — unit and integration test coverage needs to be increased
+  systematically across all packages.
+- **Behavior tree integration** — the coordinator is a temporary placeholder.
+  Full behavior tree based task orchestration is the system-level end goal,
+  replacing the coordinator and enabling conditional, multi-step, arbitrary-robot
+  scenes.
+- **Mesh geometry support** — both the gripper sampler and planning pipeline have
+  mesh support as independent goals. At the system level this is a unified capability
+  milestone enabling sensor-driven workflows where CAD is unavailable.
+- **Real hardware validation** — current validation is limited to simulation and
+  GP25 geometry. Systematic real hardware testing across supported configurations
+  is planned.
+- **Mesh-based effectors and extra axes** — the system currently assumes parallel
+  jaw grippers and fixed-base robots. Integrating more realistic mesh-based effectors
+  and extended extra axis support is planned as part of the robot-agnostic extensibility
+  goal.
+
+---
+
+## hold_and_weld_gripper_sampler
+
+### Plugin System
+
+- Pluginize constraint system (`ExclusionZoneConstraint`, `KissingSurfaceConstraint`)
+  via pluginlib. Base interfaces are already defined.
+- Pluginize filter system (`SurfaceFilter`, `RegionFilter`) via pluginlib.
+  Base interfaces are already defined.
+- Orientation grader — currently quality score is grippable arc fraction only.
+  Add policy choices: force closure metric, approach clearance, robot joint reachability.
+
+### Pipeline Improvements
+
+- Smart ground rejection. Current halfspace model is geometry agnostic. Replace with
+  topology aware detection using workpiece surface normals and z-position analysis.
+- Concurrency. Pipeline is deliberately single threaded for proof of concept clarity.
+  Contact pair processing and radial map construction are the primary parallelization
+  targets.
+- Dynamic sampling. Several sampling parameters are currently static. Adaptive density
+  based on surface geometry and gripper dimensions is planned.
+- OCCT pipeline can benefit from mesh support techniques to identify geometrically
+  fertile sampling areas more efficiently, reducing brute-force surface traversal.
+
+### Validation
+
+- Integration testing for filters and exclusion zone constraints beyond current unit tests.
+- Organic shape testing. Current validation covers box, prism, and small cylinder only.
+  Complex non-convex geometry is not yet validated.
+
+### Action Server Integration
+
+- Wire `hold_and_weld_gripper_sampler` into the `hold_and_weld_application` action server
+  system via ROS 2 lifecycle action server. Reference implementations exist in
+  `hold_and_weld_application`.
+
+### Mesh Support
+
+Mesh based geometry input as an alternative to CAD for sensor driven or CAD unavailable
+scenarios. Planned as a two-phase pipeline.
+
+Phase one performs structural analysis of the mesh — identifying graspable regions and
+natural surface boundaries through graph based traversal. A spanning tree based approach
+is used to guarantee complete mesh coverage and handle topologically circular regions
+where no natural boundary exists. CGAL is the targeted library given existing dependency
+availability in the system.
+
+Phase two generates ranked contact pairs through guided sampling, starting from
+geometrically promising regions and propagating outward via breadth-first search. Surface
+boundaries are classified as either geometric features or traversal constructs, kept
+strictly separate so ranking reflects only real geometry. Output feeds into the existing
+contact pair interface with no downstream changes required.
+
+This is a significant architectural addition planned after core CAD pipeline stabilization.
+
+### Dataset Generation
+
+An output pipeline that takes mesh inputs and produces labeled grasp datasets. The
+practical motivation is that weld areas, screw holes, and similar features tend to repeat
+across workpieces in the same workplace. A generated dataset allows downstream models to
+learn avoidance of these regions without rerunning the full sampling pipeline on every
+new workpiece. This would also enable gripping strategies informed by spatial proximity
+to weld seams.
+
+### Code Quality
+
+- Add `BRepCheck_Analyzer` validation at geometry entry points for robustness on external
+  STEP files.
+- Fix known error handling inconsistencies per error handling policy. Primarily single
+  layer catch blocks in `contact_point_sampler.cpp` and unguarded `UIso`/`VIso` handle
+  checks in `shape_refiner.cpp`.
+
+---
+
+## hold_and_weld_application
+
+### Action Servers
+
+- Worker thread watchdog timeout.
+- `detach_object` wiring in `run_job` once re-grasp workflow is defined.
+- ACM collision allowance per object instead of per link — required to handle complex
+  multi-primitive objects correctly where per-link granularity is insufficient.
+- `YAML` value validation in `load_job_from_yaml`.
+- Online parameter update for execution-time tunable parameters: velocity scaling,
+  acceleration scaling, controller type.
+- Replace `move_to_pose` with proper implementation.
+- Limit extra axis movement by parameters.
+- Planning scene coordinator to make scene management event driven.
+
+### Coordinator
+
+- Multi-planner pipeline: OMPL for collision-free approach planning, Pilz Industrial
+  Motion Planner for sharp deterministic path execution — cleaner separation of concerns
+  than the current single-planner approach.
+- Deprecate coordinator in favor of behavior tree integration.
+- Scene management via behavior tree nodes.
+
+### Kinematics
+
+- More test coverage on viable and non-viable seam paths.
+- Lifecycle harmony with MoveIt 2 — currently requires architectural workarounds due to
+  MoveIt 2's internal nodes not accepting lifecycle node interfaces.
+- Approach configuration finder — derive a guaranteed-valid joint goal from the
+  validator's IK walk, feed directly to OMPL as goal state, eliminating the current
+  retry loop entirely.
+
+---
+
+## hold_and_weld_planning
+
+The OCCT pipeline is largely stable. Deterministic seam extraction, normal evaluation,
+and pose generation work reliably for the tested joint types. Pipe joint detection
+remains incomplete and is not planned for the near term.
+
+The mesh pipeline has one fundamental open problem: CGAL corefinement produces
+intersection segments whose endpoints are pinned to triangle edges rather than the true
+geometric intersection curve. On simple geometry this approximation is acceptable. On
+complex or organic shapes the scatter becomes significant enough to corrupt path
+classification and normal extraction downstream.
+
+The candidate approaches to fix this without introducing learned components are:
+
+- **Newton refinement** — project each corefinement vertex onto the true intersection
+  by iteratively solving the two-tangent-plane system using per-vertex normals from both
+  meshes. Eliminates tessellation pinning at the cost of one closest-point query per
+  vertex per iteration.
+- **Ridge-valley detection on the edge-contributing mesh** — since all practical weld
+  joints involve at least one geometric edge, the intersection region always has a
+  curvature discontinuity on at least one mesh. Detecting that ridge directly may give
+  a cleaner curve than post-processing corefinement output.
+
+Neither approach is implemented. Both remain as research directions pending
+re-engagement with the mesh pipeline.
+
+A new core primitive is planned for complex curves that cannot be adequately represented
+as sequences of line and arc segments. This will support point-by-point pose generation
+directly from the raw seam geometry, allowing organic and irregular intersection shapes
+to be planned without requiring classification into geometric primitives.
+
+A separate planned capability is seam extraction from scanned mesh inputs where CAD
+geometry is unavailable. This requires a different pipeline: plane segmentation from the
+mesh, intersection computation between segmented planes, and convexity-based validation
+to distinguish weld joints from internal mesh edges. This would extend the system to
+sensor-driven workflows and real-world scanned geometry where the current CAD-dependent
+pipelines cannot be used.

docs/cube grasp.png docs/cube_grasp.pngdocs/cube grasp.png renamed to docs/cube_grasp.png

@@ -8,14 +8,6 @@ geometric and environmental requirements. Contact points are computed on actual
 surfaces giving geometrically exact results. Currently supports CAD-based geometry
 input only.
 
-## Dependencies
-
-- OpenCASCADE (OCCT) — CAD geometry and topology
-- FCL — collision checking
-- Embree 4 — watertight ray intersection
-- ROS 2 — messaging and lifecycle
-- Eigen — linear algebra
-
 ## Pipeline
 
 ### Geometry Mapper
@@ -49,8 +41,7 @@ clustered pairs.
 
 ### Angle Finder
 
-The most technically significant component of the pipeline and the primary location
-where approach direction validity is determined. Uses a four phase approach.
+Approach direction validity is determined here. Implemented in four phases.
 
 Phase one builds a radial surface map around each contact point by casting
 classification rays in decreasing rings starting from finger length radius down to
@@ -90,6 +81,14 @@ sampling regions and acting as collision objects during the Angle Finder phase.
 Constraints can represent real objects like ground planes and fixtures or mission
 specific forbidden zones like weld seams and screw holes.
 
+## Results
+
+**Grasp candidates example generated by `hold_and_weld_gripper_sampler`:**
+
+![Box Grasp](../docs/cube_grasp.png)
+
+![Cylinder Grasp](../docs/cylinder_grasp.png)
+
 ## Quick Start
 
 ```bash
@@ -99,8 +98,6 @@ ros2 run hold_and_weld_gripper_sampler grasp_finder_node --config <path_to_yaml>
 See `config/grasp_finder_example.yaml` for a fully documented configuration example
 covering all supported workpiece types and constraint configurations.
 
-For parameter reference see [PARAMS.md](PARAMS.md).
-
 ## Known Limitations
 
 - CAD geometry input only. Mesh and point cloud support is planned.

docs/prsim_grasp.png docs/prism_grasp.pngdocs/prsim_grasp.png renamed to docs/prism_grasp.png

@@ -1,7 +1,3 @@
-Got it. Here is the Parameters.md:
-
----
-
 # Parameters
 
 ## Workpiece