Estimate Robot Geometry using Ellipsoids and Control Points for smooth WBV Repulsion

.gitignore

@@ -7,3 +7,6 @@ install/
 .venv/
 __pycache__/
 data/
+
+CLAUDE.md
+NOTES.md

potential_fields/config/pfield_config.yaml

@@ -3,12 +3,12 @@ pfield_manager:
     visualize_pf_frequency: 10.0 # [Hz]
     attractive_gain: 3.0 # [Nm]
     rotational_attractive_gain: 0.15 # [Nm/rad]
-    repulsive_gain: 0.6 # [Nm]
+    repulsive_gain: 0.3 # [Nm]
     max_linear_velocity: 0.05 # [m/s]
     max_angular_velocity: 0.4 # [rad/s]
     max_linear_acceleration: 1.0 # [m/s^2]
     max_angular_acceleration: 0.2 # [rad/s^2]
-    influence_distance: 0.2 # Influence distance for obstacle repulsion [m]
+    influence_distance: 0.1 # Influence distance for obstacle repulsion [m]
     visualize_field_vectors: false # Whether to visualize the potential field vectors (Slows down computation if true)
     visualizer_buffer_area: 0.5 # Extra area around obstacles and goal to visualize the PF [m]
     field_resolution: 1.0 # Resolution to show PF velocity arrows in RViz [m]

potential_fields/include/potential_fields/ros/pfield_manager.hpp

@@ -152,6 +152,40 @@ class PotentialFieldManager : public rclcpp::Node {
    */
   MarkerArray createObstacleMarkers(std::shared_ptr<pfield::PotentialField> pf);
 
+  /**
+   * @brief Build obstacle shape markers and their semi-transparent influence-zone overlays
+   *        for all environment and robot obstacles.
+   *
+   * @param obstacles Combined list of environment and robot obstacles.
+   * @param influenceDistance Repulsive influence radius [m].
+   * @return MarkerArray Markers in namespaces "robot_obstacles", "environment_obstacles",
+   *         and "environment_influence_zones".
+   */
+  MarkerArray createObstaclesWithInfluenceZoneMarkerArray(
+    const std::vector<pfield::PotentialFieldObstacle>& obstacles,
+    double influenceDistance);
+
+  /**
+   * @brief Build semi-transparent ghost mesh markers for CAPSULE robot obstacles that
+   *        were fitted from a mesh, so the original mesh is visible alongside the capsule
+   *        approximation in RViz.
+   *
+   * @param robotObstacles Robot link obstacles.
+   * @return MarkerArray Markers in namespace "robot_mesh_ghost".
+   */
+  MarkerArray createGhostMeshOverlayMarkerArray(
+    const std::vector<pfield::PotentialFieldObstacle>& robotObstacles);
+
+  /**
+   * @brief Build small sphere markers at each robot link control point used by the
+   *        whole-body velocity repulsion, for visualization in RViz.
+   *
+   * @param robotObstacles Robot link obstacles.
+   * @return MarkerArray Markers in namespace "robot_control_points".
+   */
+  MarkerArray createRobotLinkControlPointsMarkerArray(
+    const std::vector<pfield::PotentialFieldObstacle>& robotObstacles);
+
   /**
    * @brief Create a marker for the goal position in the potential field
    *