Include navigation path length in ROS message

pyrobosim/pyrobosim/utils/motion.py

@@ -76,7 +76,7 @@ def __eq__(self, other):
         if not (isinstance(other, Path)):
             raise TypeError("Expected a Path object.")
 
-        return self.poses == other.poses
+        return (self.poses == other.poses) and (self.length == other.length)
 
     def __repr__(self):
         """Return brief description of the path."""

pyrobosim/test/core/test_robot.py

@@ -213,7 +213,7 @@ def test_robot_nav_validation(self):
             pose=init_pose,
             path_planner=WorldGraphPlanner(world=self.test_world),
             path_executor=ConstantVelocityExecutor(
-                linear_velocity=3.0,
+                linear_velocity=1.0,  # Move slowly to give time to cancel.
                 dt=0.1,
                 validate_during_execution=True,
             ),

pyrobosim/test/utils/test_motion_utils.py

@@ -77,10 +77,13 @@ def test_path_equality():
     path1 = Path(poses=test_poses)
     path2 = Path(poses=test_poses)  # Same poses, but different object
     path3 = Path(poses=list(reversed(test_poses)))  # Different poses
+    path4 = Path(poses=test_poses)  # Path with out-of-sync length attribute
+    path4.length = -1.0
 
     assert path1 == path1
     assert path1 == path2
     assert not path1 == path3
+    assert not path1 == path4
 
     # Check datatype exception.
     with pytest.raises(TypeError) as exc_info:

pyrobosim_msgs/msg/Path.msg

@@ -1,3 +1,4 @@
 # Path ROS Message
 
 geometry_msgs/Pose[] poses
+float64 length

pyrobosim_ros/pyrobosim_ros/ros_conversions.py

@@ -77,6 +77,7 @@ def path_to_ros(path):
     """
     path_msg = ros_msgs.Path()
     path_msg.poses = [pose_to_ros(p) for p in path.poses]
+    path_msg.length = path.length
     return path_msg
 
 

pyrobosim_ros/test/test_ros_conversions.py

@@ -25,7 +25,7 @@
 def test_pose_conversion():
     """Tests round-trip conversion of pose objects."""
     # Create a pyrobosim pose
-    orig_pose = Pose(x=1.0, y=2.0, z=3.0, q=[0.707, 0.0, 0.707, 0])  # wxyz
+    orig_pose = Pose(x=1.0, y=2.0, z=3.0, q=[0.707, 0.0, 0.707, 0.0])  # wxyz
 
     # Convert to a ROS Message
     ros_pose = pose_to_ros(orig_pose)
@@ -56,6 +56,7 @@ def test_path_conversion():
     # Convert to a ROS Message
     ros_path = path_to_ros(orig_path)
     assert len(ros_path.poses) == orig_path.num_poses
+    assert orig_path.length == ros_path.length
     for orig_pose, ros_pose in zip(orig_path.poses, ros_path.poses):
         assert ros_pose.position.x == pytest.approx(orig_pose.x)
         assert ros_pose.position.y == pytest.approx(orig_pose.y)
@@ -68,6 +69,7 @@ def test_path_conversion():
     # Convert back to a pyrobosim path
     new_path = path_from_ros(ros_path)
     assert new_path.num_poses == orig_path.num_poses
+    assert new_path.length == orig_path.length
     for orig_pose, new_pose in zip(orig_path.poses, new_path.poses):
         assert orig_pose.is_approx(new_pose)