Final PR before ROS Release

potential_fields/CHANGELOG.rst

@@ -0,0 +1,135 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package potential_fields
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.0.2 (2026-04-04)
+------------------
+* Clarified Obstacle type's geometry fields
+* Moved HSVtoRGB to pfield_manager instead
+* Estimate Robot Geometry using Ellipsoids and Control Points for smooth WBV Repulsion (`#42 <https://github.com/argallab/potential_fields/issues/42>`_)
+  Co-authored-by: Claude Sonnet 4.6 <noreply@anthropic.com>
+* 40 test main branch ruleset (testing Issue `#40 <https://github.com/argallab/potential_fields/issues/40>`_) (`#41 <https://github.com/argallab/potential_fields/issues/41>`_)
+* README edits and CONTRIBUTING.md
+* Created visualization method to visualize both TS and WBV
+* Fixed CSV bugs and tested demo1 and demo2, TS planning has problems
+* 1.0.1
+* Rename Packages (`#35 <https://github.com/argallab/potential_fields/issues/35>`_)
+* Enabling Github Workflow for building and testing repo (`#33 <https://github.com/argallab/potential_fields/issues/33>`_)
+* Successfully migrated all unit tests to `pfield_library` (`#32 <https://github.com/argallab/potential_fields/issues/32>`_)
+* December Final Demos (`#26 <https://github.com/argallab/potential_fields/issues/26>`_)
+* Use Robot Dynamics to convert Joint Torques to Joint Velocities (`#24 <https://github.com/argallab/potential_fields/issues/24>`_)
+* Independent pfield C++ Library (`#22 <https://github.com/argallab/potential_fields/issues/22>`_)
+* Updating ComputeAutonomyVector
+* Collision Avoidance between robot links and environment obstacles (`#21 <https://github.com/argallab/potential_fields/issues/21>`_)
+* XArm 7-DoF Arm Demo (`#19 <https://github.com/argallab/potential_fields/issues/19>`_)
+* Dynamic D* Threshold (`#20 <https://github.com/argallab/potential_fields/issues/20>`_)
+* Aligned AttractiveMoment Doc with Implementation
+* Fixing Rotational Attraction (`#18 <https://github.com/argallab/potential_fields/issues/18>`_)
+* Editing gains
+* Fixing goal and query pose visualizations
+* Fixing plotting functions and organizing code (`#16 <https://github.com/argallab/potential_fields/issues/16>`_)
+  Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
+* Adding Reference to Principles of Robot Motion and Updating PF Equations (`#15 <https://github.com/argallab/potential_fields/issues/15>`_)
+* Updating repulsive force computation from Choset text
+* Repulsive force piecewise equation includes equals to QStar
+* Merged in extra fixes
+* Comments cleanup
+* Using RK4 for integrating position from PF Twist (`#13 <https://github.com/argallab/potential_fields/issues/13>`_)
+* Tested another demo with plots
+* Docstrings for new functions
+* First pass at RK4
+* Fixing up tests
+* Prototyping path planning (`#11 <https://github.com/argallab/potential_fields/issues/11>`_)
+* Refactored influence scale and mesh collision (`#12 <https://github.com/argallab/potential_fields/issues/12>`_)
+* Merge branch 'main' of github.com:argallab/pfields_2025
+* Merged RobotParser into PFieldManager (`#10 <https://github.com/argallab/potential_fields/issues/10>`_)
+* Current TODO
+* Pinocchio for Obstacles and Motion Constraints (`#9 <https://github.com/argallab/potential_fields/issues/9>`_)
+* PlanPath Service Demo (`#7 <https://github.com/argallab/potential_fields/issues/7>`_)
+* Planning "World" for path-planning with robot (`#6 <https://github.com/argallab/potential_fields/issues/6>`_)
+* Motion Interface (`#5 <https://github.com/argallab/potential_fields/issues/5>`_)
+* Update license to Apache 2.0
+* Updating email to northwestern email for packages
+* Updating functions to write to folders
+* Documentation
+* Updating robot parser targets
+* typo
+* Custom service to plan path in PF
+* Merge pull request `#4 <https://github.com/argallab/potential_fields/issues/4>`_ from argallab/demo-package
+  Example Package
+* Changing error to warn
+* Launch arguments and parsing raw URDF
+* Created robot parser node to listen for the transforms and parse the URDF collision objects
+* Typo
+* RViz sets fixed frame from user
+* Moving static transforms to demo repository
+* Updating build info
+* Removing extra forward slash
+* Another delta robot typo
+* Fixing typo with delta robot include dir
+* Created interpolate and integration functions for PField linear velocity and angular velocity
+* A bit of hacking to get obstacles where the URDF defines them
+* Created TF from world
+* Attempting to fix TF listener and collision objects
+* Fixed ID incrementing
+* Fixed parsing problem but collision objects aren't showing up
+* Created URDF testing file
+* Adding urdf parser
+* Updating the transforms of every obstacle
+* Added path trajectory and tf at world frame
+* Merge pull request `#2 <https://github.com/argallab/potential_fields/issues/2>`_ from argallab/more-obstacles
+  More Obstacle Support
+* Moved rotated object to new position
+* Fixed frequency in timer
+* Removed the Lerp
+* Updated Obstacle class, tests, and visualizer
+* Added default case to withinInfluenceZone
+* Converted SphereObstacle to PFObstacle class
+* Cleaned up file comment
+* documentation and normalizePosition
+* Deleted default constructor for sphereObstacle to force data entry
+* Followed NL.16 and removed extra public: statements
+* Adjusting units and adding notes
+* Deleted influence_scale_radius param since it's unused
+* Changing query point size
+* Visualize a query point
+* Visualize a query point
+* Visualizing the orientation of the goal and setting the goal pose with subscriber to /goal_pose
+* Created an interfaces package
+* Merge pull request `#1 <https://github.com/argallab/potential_fields/issues/1>`_ from argallab/eigenify
+  Implementing Vector operations with Eigen3
+* Few more tests for angular distance
+* Removed geodesic distance function and replaced with Eigen angularDistance function
+* euler functions are now YPR and comments accordingly
+* Added more tests and verified euler to quaternion method
+* Collapsed implementation into header
+* Fixed euler conversion
+* All tests pass but need to write more
+* Added normalizePosition function
+* Fixed manager with new syntax and Eigen vectors
+* Edited attraction and repulsion functions using Eigen and new SpatialVector formatting
+* Changed tests to match new syntax
+* Reimplemented position and orientation with eigen and removed operator overloads and changed floats to doubles
+* Changed position to an Eigen::Vector3d and changed floats to doubles
+* Adjusted language and changed floats to doubles
+* Built PKG with Eigen dependency and with include statement
+* Added rotational attractive gain and equation to README
+* Wrote geodesic formulation in README and function as commented code
+* Replaced geodesic distance with raw implementation inside Quaternion difference
+* Wrote tests but orientation methods are failing
+* Removing uncrustify tests
+* Reverted formatting of uncrustify
+* Reformated with uncrustify and created tests
+* Attraction now includes orientation
+* Vector3D -> SpatialVector
+* Meeting notes
+* Cleaning up docs in pfield vector markers
+* Drew arrows in RViz
+* Saved rviz config
+* Included RViz and added config and launch folders/files
+* Started writing node
+* Edited math to match the python version that created graphs
+* Updating force computation
+* Docstrings
+* Started working on implementation of C++ Potential Fields in a ROS2 pkg
+* Contributors: Demiana Barsoum, Sharwin Patil, Sharwin24

potential_fields/include/potential_fields/ros/pfield_manager.hpp

@@ -54,26 +54,38 @@
 
 #include "rclcpp/rclcpp.hpp"
 
-using Marker = visualization_msgs::msg::Marker;
-using MarkerArray = visualization_msgs::msg::MarkerArray;
-using Pose = geometry_msgs::msg::Pose;
-using Point = geometry_msgs::msg::Point;
-using Quaternion = geometry_msgs::msg::Quaternion;
-using Path = nav_msgs::msg::Path;
-using TransformStamped = geometry_msgs::msg::TransformStamped;
-using Obstacle = potential_fields_interfaces::msg::Obstacle;
-using ObstacleArray = potential_fields_interfaces::msg::ObstacleArray;
-using PlanPath = potential_fields_interfaces::srv::PlanPath;
-using ComputeAutonomyVector = potential_fields_interfaces::srv::ComputeAutonomyVector;
-using JointState = sensor_msgs::msg::JointState;
+// Short aliases for ROS message/service types used throughout the node.
+// Scoped to pfield_manager_types to avoid polluting headers that include this file.
+namespace pfield_manager_types {
+  using MarkerMsg = visualization_msgs::msg::Marker;
+  using MarkerArrayMsg = visualization_msgs::msg::MarkerArray;
+  using PoseMsg = geometry_msgs::msg::Pose;
+  using PoseStampedMsg = geometry_msgs::msg::PoseStamped;
+  using PointMsg = geometry_msgs::msg::Point;
+  using QuaternionMsg = geometry_msgs::msg::Quaternion;
+  using TwistMsg = geometry_msgs::msg::Twist;
+  using TwistStampedMsg = geometry_msgs::msg::TwistStamped;
+  using PathMsg = nav_msgs::msg::Path;
+  using TransformStampedMsg = geometry_msgs::msg::TransformStamped;
+  using ObstacleMsg = potential_fields_interfaces::msg::Obstacle;
+  using ObstacleArrayMsg = potential_fields_interfaces::msg::ObstacleArray;
+  using PlanPathSrv = potential_fields_interfaces::srv::PlanPath;
+  using ComputeAutonomyVectorSrv = potential_fields_interfaces::srv::ComputeAutonomyVector;
+  using JointStateMsg = sensor_msgs::msg::JointState;
+  using JointTrajectoryMsg = trajectory_msgs::msg::JointTrajectory;
+  using JointTrajectoryPointMsg = trajectory_msgs::msg::JointTrajectoryPoint;
+}  // namespace pfield_manager_types
+
+// Bring aliases into scope for this header and its includers.
+using namespace pfield_manager_types;  // NOLINT(build/namespaces)
 
 class PotentialFieldManager : public rclcpp::Node {
 public:
   PotentialFieldManager();
   ~PotentialFieldManager() = default;
 
-  static Quaternion getQuaternionFromYaw(double yaw) {
-    Quaternion q;
+  [[nodiscard]] static QuaternionMsg getQuaternionFromYaw(double yaw) {
+    QuaternionMsg q;
     q.x = 0.0;
     q.y = 0.0;
     q.z = sin(yaw / 2.0);
@@ -115,53 +127,54 @@ class PotentialFieldManager : public rclcpp::Node {
   const double TASK_SPACE_AXIS_HEAD_DIAMETER = 0.025; // Diameter of axis heads [m]
 
   rclcpp::TimerBase::SharedPtr timer; // Timer to periodically update the potential field
-  rclcpp::Publisher<MarkerArray>::SharedPtr pFieldMarkerPub; // Publisher for PF Markers
-  rclcpp::Publisher<JointState>::SharedPtr planningJointStatePub; // Publisher for planning joint states
-  rclcpp::Publisher<Path>::SharedPtr plannedEndEffectorPathPub; // Publisher for planned end-effector path
-  rclcpp::Subscription<Pose>::SharedPtr goalPoseSub; // Subscriber for the goal pose
-  rclcpp::Subscription<Pose>::SharedPtr queryPoseSub; // Subscriber for query poses
-  rclcpp::Subscription<ObstacleArray>::SharedPtr obstacleSub; // Subscriber for obstacles
-  rclcpp::Service<PlanPath>::SharedPtr pathPlanningService; // Now hosted here
-  rclcpp::Service<ComputeAutonomyVector>::SharedPtr autonomyVectorService; // Service to compute velocity vector at a given pose
+  rclcpp::Publisher<MarkerArrayMsg>::SharedPtr pFieldMarkerPub; // Publisher for PF Markers
+  rclcpp::Publisher<JointStateMsg>::SharedPtr planningJointStatePub; // Publisher for planning joint states
+  rclcpp::Publisher<PathMsg>::SharedPtr plannedEndEffectorPathPub; // Publisher for planned end-effector path
+  rclcpp::Subscription<PoseMsg>::SharedPtr goalPoseSub; // Subscriber for the goal pose
+  rclcpp::Subscription<PoseMsg>::SharedPtr queryPoseSub; // Subscriber for query poses
+  rclcpp::Subscription<ObstacleArrayMsg>::SharedPtr obstacleSub; // Subscriber for obstacles
+  rclcpp::Service<PlanPathSrv>::SharedPtr pathPlanningService; // Now hosted here
+  // Service to compute velocity vector at a given pose
+  rclcpp::Service<ComputeAutonomyVectorSrv>::SharedPtr autonomyVectorService;
 
   void timerCallback();
 
   // Service callbacks
-  void handlePlanPath(const PlanPath::Request::SharedPtr request, PlanPath::Response::SharedPtr response);
+  void handlePlanPath(const PlanPathSrv::Request::SharedPtr request, PlanPathSrv::Response::SharedPtr response);
   void handleComputeAutonomyVector(
-    const ComputeAutonomyVector::Request::SharedPtr request, ComputeAutonomyVector::Response::SharedPtr response);
+    const ComputeAutonomyVectorSrv::Request::SharedPtr request, ComputeAutonomyVectorSrv::Response::SharedPtr response);
 
   /**
    * @brief Given a potential field, generate all visualization markers
    *        (goal, obstacles, obstacle influence zones, velocity vectors)
    *
    * @param pf The potential field instance
-   * @return MarkerArray The marker array containing all visualization markers
+   * @return MarkerArrayMsg The marker array containing all visualization markers
    */
-  MarkerArray visualizePF(std::shared_ptr<pfield::PotentialField> pf);
+  MarkerArrayMsg visualizePF(std::shared_ptr<pfield::PotentialField> pf);
 
-  MarkerArray createQueryPoseMarker();
+  MarkerArrayMsg createQueryPoseMarker();
 
-  MarkerArray createThresholdMarkers(std::shared_ptr<pfield::PotentialField> pf);
+  MarkerArrayMsg createThresholdMarkers(std::shared_ptr<pfield::PotentialField> pf);
 
   /**
    * @brief Get Obstacle and Obstacle Influence Zone markers from a potential field
    *
    * @param pf The potential field instance
-   * @return MarkerArray The marker array containing all obstacle markers
+   * @return MarkerArrayMsg The marker array containing all obstacle markers
    */
-  MarkerArray createObstacleMarkers(std::shared_ptr<pfield::PotentialField> pf);
+  MarkerArrayMsg 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",
+   * @return MarkerArrayMsg Markers in namespaces "robot_obstacles", "environment_obstacles",
    *         and "environment_influence_zones".
    */
-  MarkerArray createObstaclesWithInfluenceZoneMarkerArray(
+  MarkerArrayMsg createObstaclesWithInfluenceZoneMarkerArray(
     const std::vector<pfield::PotentialFieldObstacle>& obstacles,
     double influenceDistance);
 
@@ -171,28 +184,26 @@ class PotentialFieldManager : public rclcpp::Node {
    *        approximation in RViz.
    *
    * @param robotObstacles Robot link obstacles.
-   * @return MarkerArray Markers in namespace "robot_mesh_ghost".
+   * @return MarkerArrayMsg Markers in namespace "robot_mesh_ghost".
    */
-  MarkerArray createGhostMeshOverlayMarkerArray(
-    const std::vector<pfield::PotentialFieldObstacle>& robotObstacles);
+  MarkerArrayMsg 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".
+   * @return MarkerArrayMsg Markers in namespace "robot_control_points".
    */
-  MarkerArray createRobotLinkControlPointsMarkerArray(
-    const std::vector<pfield::PotentialFieldObstacle>& robotObstacles);
+  MarkerArrayMsg createRobotLinkControlPointsMarkerArray(const std::vector<pfield::PotentialFieldObstacle>& robotObstacles);
 
   /**
    * @brief Create a marker for the goal position in the potential field
    *
    * @param pf The potential field instance
-   * @return MarkerArray The marker array containing the goal marker and orientation indicator
+   * @return MarkerArrayMsg The marker array containing the goal marker and orientation indicator
    */
-  MarkerArray createGoalMarker(std::shared_ptr<pfield::PotentialField> pf);
+  MarkerArrayMsg createGoalMarker(std::shared_ptr<pfield::PotentialField> pf);
 
   /**
    * @brief Create velocity vector markers for visualization in RViz
@@ -201,9 +212,9 @@ class PotentialFieldManager : public rclcpp::Node {
    *       and the vector sizes are normalized but the color intensity represents the magnitude
    *
    * @param pf The potential field instance
-   * @return MarkerArray The marker array containing all velocity vector markers
+   * @return MarkerArrayMsg The marker array containing all velocity vector markers
    */
-  MarkerArray createPotentialVectorMarkers(std::shared_ptr<pfield::PotentialField> pf);
+  MarkerArrayMsg createPotentialVectorMarkers(std::shared_ptr<pfield::PotentialField> pf);
 
   /**
    * @brief Fuses the two twists using a weighted alpha parameter
@@ -213,21 +224,18 @@ class PotentialFieldManager : public rclcpp::Node {
    * @param twist1 The first twist to fuse
    * @param twist2 The second twist to fuse
    * @param alpha The weight for the second twist [0.0, 1.0]
-   * @return geometry_msgs::msg::Twist The fused twist
+   * @return TwistMsg The fused twist
    */
-  geometry_msgs::msg::Twist fuseTwists(
-    const geometry_msgs::msg::Twist::SharedPtr twist1,
-    const geometry_msgs::msg::Twist::SharedPtr twist2,
-    const double alpha);
+  TwistMsg fuseTwists(const TwistMsg::SharedPtr twist1, const TwistMsg::SharedPtr twist2, const double alpha);
 
   /**
    * @brief Clamps the twist to the specified limits.
    *
    * @param twist The twist to clamp
    * @param limits A twist message where linear and angular components represent the maximum allowed values
-   * @return geometry_msgs::msg::Twist The clamped twist as a new message
+   * @return TwistMsg The clamped twist as a new message
    */
-  geometry_msgs::msg::Twist clampTwist(const geometry_msgs::msg::Twist& twist, const geometry_msgs::msg::Twist& limits);
+  TwistMsg clampTwist(const TwistMsg& twist, const TwistMsg& limits);
 
   /**
    * @brief Exports the potential field data to a CSV file for external analysis or visualization.

potential_fields/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>potential_fields</name>
-  <version>1.0.1</version>
+  <version>1.0.2</version>
   <description>Potential Fields Motion Planner</description>
   <maintainer email="sharwinpatil@u.northwestern.edu">Sharwin Patil</maintainer>
   <license>Apache 2.0</license>