diff --git a/nav2_docking/opennav_docking/test/test_docking_server.py b/nav2_docking/opennav_docking/test/test_docking_server.py index 46d69b0e40c..2c166da4cd7 100644 --- a/nav2_docking/opennav_docking/test/test_docking_server.py +++ b/nav2_docking/opennav_docking/test/test_docking_server.py @@ -26,8 +26,6 @@ import launch_testing import launch_testing.actions import launch_testing.asserts -import launch_testing.markers -import launch_testing.util from lifecycle_msgs.srv import GetState from nav2_common.launch import RewrittenYaml from nav2_msgs.action import DockRobot, NavigateToPose, UndockRobot @@ -36,6 +34,7 @@ import rclpy from rclpy.action.client import ActionClient from rclpy.action.server import ActionServer, ServerGoalHandle +from rclpy.client import Client from sensor_msgs.msg import BatteryState import tf2_ros from tf2_ros import TransformBroadcaster @@ -133,9 +132,10 @@ def setUp(self) -> None: self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self.node) self.odom_pub = self.node.create_publisher(Odometry, 'odom', 10) - def wait_for_node_to_be_active(self, node_name, timeout_sec=10.0): + def wait_for_node_to_be_active(self, node_name: str, timeout_sec: float = 30.0) -> None: """Wait for a managed node to become active.""" - client = self.node.create_client(GetState, f'{node_name}/get_state') + client: Client[GetState.Request, GetState.Response] = \ + self.node.create_client(GetState, f'{node_name}/get_state') if not client.wait_for_service(timeout_sec=2.0): self.fail(f'Service get_state for {node_name} not available.') @@ -144,7 +144,8 @@ def wait_for_node_to_be_active(self, node_name, timeout_sec=10.0): req = GetState.Request() future = client.call_async(req) rclpy.spin_until_future_complete(self.node, future, timeout_sec=1.0) - if future.result() and future.result().current_state.id == 3: # 3 = ACTIVE + result = future.result() + if result is not None and result.current_state.id == 3: # 3 = ACTIVE self.node.get_logger().info(f'Node {node_name} is active.') return time.sleep(0.5) @@ -199,12 +200,12 @@ def publish_odometry(self, odom_to_base_link: TransformStamped) -> None: odom.twist.twist = self.command self.odom_pub.publish(odom) - def action_feedback_callback(self, msg: DockRobot.Feedback) -> None: + def action_feedback_callback(self, msg: DockRobot.Impl.FeedbackMessage) -> None: # Force the docking action to run a full recovery loop and then # make contact with the dock (based on pose of robot) before # we report that the robot is charging if msg.feedback.num_retries > 0 and \ - msg.feedback.state == msg.feedback.WAIT_FOR_CHARGE: + msg.feedback.state == DockRobot.Feedback.WAIT_FOR_CHARGE: self.is_charging = True def nav_execute_callback( diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index ea901a20290..3e9c2c05c45 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -14,14 +14,14 @@ import math -from typing import Optional +from typing import Optional, Union -from geometry_msgs.msg import (PoseWithCovarianceStamped, Quaternion, TransformStamped, Twist, - TwistStamped, Vector3) +from geometry_msgs.msg import (Pose, PoseWithCovarianceStamped, Quaternion, TransformStamped, + Twist, TwistStamped, Vector3) from nav2_loopback_sim.utils import (addYawToQuat, getMapOccupancy, matrixToTransform, transformStampedToMatrix, worldToMap) from nav2_simple_commander.line_iterator import LineIterator -from nav_msgs.msg import Odometry +from nav_msgs.msg import OccupancyGrid, Odometry from nav_msgs.srv import GetMap import numpy as np import rclpy @@ -29,6 +29,7 @@ from rclpy.duration import Duration from rclpy.node import Node from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy +from rclpy.subscription import Subscription from rclpy.timer import Timer from rosgraph_msgs.msg import Clock from sensor_msgs.msg import LaserScan @@ -49,12 +50,12 @@ class LoopbackSimulator(Node): def __init__(self) -> None: super().__init__(node_name='loopback_simulator') - self.curr_cmd_vel = None + self.curr_cmd_vel: Optional[Twist] = None self.curr_cmd_vel_time = self.get_clock().now() - self.initial_pose: PoseWithCovarianceStamped = None + self.initial_pose: Optional[Pose] = None self.timer: Optional[Timer] = None self.setupTimer = None - self.map = None + self.map: Optional[OccupancyGrid] = None self.mat_base_to_laser: Optional[np.ndarray[np.float64, np.dtype[np.float64]]] = None self.declare_parameter('update_duration', 0.01) @@ -122,6 +123,9 @@ def __init__(self) -> None: self.initial_pose_sub = self.create_subscription( PoseWithCovarianceStamped, 'initialpose', self.initialPoseCallback, 10) + + self.cmd_vel_sub: Union[Subscription[Twist], Subscription[TwistStamped]] + if not use_stamped: self.cmd_vel_sub = self.create_subscription( Twist, @@ -292,7 +296,8 @@ def publishOdometry(self, odom_to_base_link: TransformStamped) -> None: odom.pose.pose.position.x = odom_to_base_link.transform.translation.x odom.pose.pose.position.y = odom_to_base_link.transform.translation.y odom.pose.pose.orientation = odom_to_base_link.transform.rotation - odom.twist.twist = self.curr_cmd_vel + if self.curr_cmd_vel is not None: + odom.twist.twist = self.curr_cmd_vel self.odom_pub.publish(odom) def info(self, msg: str) -> None: diff --git a/nav2_loopback_sim/nav2_loopback_sim/utils.py b/nav2_loopback_sim/nav2_loopback_sim/utils.py index be3b4e73c29..7131b8424d9 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/utils.py +++ b/nav2_loopback_sim/nav2_loopback_sim/utils.py @@ -15,7 +15,7 @@ import math -from geometry_msgs.msg import Quaternion, Transform +from geometry_msgs.msg import Quaternion, Transform, TransformStamped from nav_msgs.msg import OccupancyGrid import numpy as np import tf_transformations @@ -37,7 +37,8 @@ def addYawToQuat(quaternion: Quaternion, yaw_to_add: float) -> Quaternion: return new_quaternion -def transformStampedToMatrix(transform: Transform) -> np.ndarray[np.float64, np.dtype[np.float64]]: +def transformStampedToMatrix( + transform: TransformStamped) -> np.ndarray[np.float64, np.dtype[np.float64]]: translation = transform.transform.translation rotation = transform.transform.rotation matrix = np.eye(4) diff --git a/nav2_simple_commander/nav2_simple_commander/example_route.py b/nav2_simple_commander/nav2_simple_commander/example_route.py index 4921ef08493..5910b590d54 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_route.py +++ b/nav2_simple_commander/nav2_simple_commander/example_route.py @@ -13,7 +13,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from geometry_msgs.msg import Pose, PoseStamped +from geometry_msgs.msg import Point, PoseStamped from nav2_simple_commander.robot_navigator import BasicNavigator, RunningTask, TaskResult from nav2_simple_commander.utils import euler_to_quaternion import rclpy @@ -24,7 +24,7 @@ """ -def toPoseStamped(pt: Pose, header: Header) -> PoseStamped: +def toPoseStamped(pt: Point, header: Header) -> PoseStamped: pose = PoseStamped() pose.pose.position.x = pt.x pose.pose.position.y = pt.y diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index b1a8d91d268..5b30fd6d7a8 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -28,11 +28,11 @@ ComputePathThroughPoses, ComputePathToPose, ComputeRoute, DockRobot, DriveOnHeading, FollowGPSWaypoints, FollowPath, FollowWaypoints, NavigateThroughPoses, NavigateToPose, SmoothPath, Spin, UndockRobot) -from nav2_msgs.msg import Route +from nav2_msgs.msg import Costmap, Route from nav2_msgs.srv import (ClearCostmapAroundPose, ClearCostmapAroundRobot, ClearCostmapExceptRegion, ClearEntireCostmap, GetCostmap, LoadMap, ManageLifecycleNodes, Toggle) -from nav_msgs.msg import Goals, OccupancyGrid, Path +from nav_msgs.msg import Goals, Path import rclpy from rclpy.action import ActionClient from rclpy.action.client import ClientGoalHandle @@ -296,7 +296,7 @@ def goThroughPoses(self, poses: Goals, behavior_tree: str = '') -> Optional[Runn goal_msg.poses = poses goal_msg.behavior_tree = behavior_tree - self.info(f'Navigating with {len(goal_msg.poses)} goals....') + self.info(f'Navigating with {len(poses.goals)} goals....') send_goal_future = self.nav_through_poses_client.send_goal_async( goal_msg, self._feedbackCallback ) @@ -304,8 +304,8 @@ def goThroughPoses(self, poses: Goals, behavior_tree: str = '') -> Optional[Runn self.goal_handle = send_goal_future.result() if not self.goal_handle or not self.goal_handle.accepted: - msg = f'NavigateThroughPoses request with {len(poses)} was rejected!' - self.setTaskError(NavigateThroughPoses.UNKNOWN, msg) + msg = f'NavigateThroughPoses request with {len(poses.goals)} was rejected!' + self.setTaskError(NavigateThroughPoses.Result.UNKNOWN, msg) self.error(msg) return None @@ -344,7 +344,7 @@ def goToPose(self, pose: PoseStamped, behavior_tree: str = '') -> Optional[Runni + str(pose.pose.position.y) + ' was rejected!' ) - self.setTaskError(NavigateToPose.UNKNOWN, msg) + self.setTaskError(NavigateToPose.Result.UNKNOWN, msg) self.error(msg) return None @@ -370,7 +370,7 @@ def followWaypoints(self, poses: list[PoseStamped]) -> Optional[RunningTask]: if not self.goal_handle or not self.goal_handle.accepted: msg = f'Following {len(poses)} waypoints request was rejected!' - self.setTaskError(FollowWaypoints.UNKNOWN, msg) + self.setTaskError(FollowWaypoints.Result.UNKNOWN, msg) self.error(msg) return None @@ -396,7 +396,7 @@ def followGpsWaypoints(self, gps_poses: list[GeoPose]) -> Optional[RunningTask]: if not self.goal_handle or not self.goal_handle.accepted: msg = f'Following {len(gps_poses)} gps waypoints request was rejected!' - self.setTaskError(FollowGPSWaypoints.UNKNOWN, msg) + self.setTaskError(FollowGPSWaypoints.Result.UNKNOWN, msg) self.error(msg) return None @@ -424,7 +424,7 @@ def spin( if not self.goal_handle or not self.goal_handle.accepted: msg = 'Spin request was rejected!' - self.setTaskError(Spin.UNKNOWN, msg) + self.setTaskError(Spin.Result.UNKNOWN, msg) self.error(msg) return None @@ -454,7 +454,7 @@ def backup( if not self.goal_handle or not self.goal_handle.accepted: msg = 'Backup request was rejected!' - self.setTaskError(BackUp.UNKNOWN, msg) + self.setTaskError(BackUp.Result.UNKNOWN, msg) self.error(msg) return None @@ -484,7 +484,7 @@ def driveOnHeading( if not self.goal_handle or not self.goal_handle.accepted: msg = 'Drive On Heading request was rejected!' - self.setTaskError(DriveOnHeading.UNKNOWN, msg) + self.setTaskError(DriveOnHeading.Result.UNKNOWN, msg) self.error(msg) return None @@ -510,7 +510,7 @@ def assistedTeleop(self, time_allowance: int = 30) -> Optional[RunningTask]: if not self.goal_handle or not self.goal_handle.accepted: msg = 'Assisted Teleop request was rejected!' - self.setTaskError(AssistedTeleop.UNKNOWN, msg) + self.setTaskError(AssistedTeleop.Result.UNKNOWN, msg) self.error(msg) return None @@ -539,7 +539,7 @@ def followPath(self, path: Path, controller_id: str = '', if not self.goal_handle or not self.goal_handle.accepted: msg = 'FollowPath goal was rejected!' - self.setTaskError(FollowPath.UNKNOWN, msg) + self.setTaskError(FollowPath.Result.UNKNOWN, msg) self.error(msg) return None @@ -568,7 +568,7 @@ def dockRobotByPose(self, dock_pose: PoseStamped, if not self.goal_handle or not self.goal_handle.accepted: msg = 'DockRobot request was rejected!' - self.setTaskError(DockRobot.UNKNOWN, msg) + self.setTaskError(DockRobot.Result.UNKNOWN, msg) self.error(msg) return None @@ -595,7 +595,7 @@ def dockRobotByID(self, dock_id: str, nav_to_dock: bool = True) -> Optional[Runn if not self.goal_handle or not self.goal_handle.accepted: msg = 'DockRobot request was rejected!' - self.setTaskError(DockRobot.UNKNOWN, msg) + self.setTaskError(DockRobot.Result.UNKNOWN, msg) self.error(msg) return None @@ -620,7 +620,7 @@ def undockRobot(self, dock_type: str = '') -> Optional[RunningTask]: if not self.goal_handle or not self.goal_handle.accepted: msg = 'UndockRobot request was rejected!' - self.setTaskError(UndockRobot.UNKNOWN, msg) + self.setTaskError(UndockRobot.Result.UNKNOWN, msg) self.error(msg) return None @@ -759,9 +759,9 @@ def _getPathImpl( if not self.goal_handle or not self.goal_handle.accepted: self.error('Get path was rejected!') - self.status = GoalStatus.UNKNOWN + self.status = GoalStatus.STATUS_UNKNOWN result = ComputePathToPose.Result() - result.error_code = ComputePathToPose.UNKNOWN + result.error_code = ComputePathToPose.Result.UNKNOWN result.error_msg = 'Get path was rejected' return result @@ -773,7 +773,7 @@ def _getPathImpl( def getPath( self, start: PoseStamped, goal: PoseStamped, - planner_id: str = '', use_start: bool = False) -> Path: + planner_id: str = '', use_start: bool = False) -> Optional[Path]: """Send a `ComputePathToPose` action request.""" self.clearTaskError() rtn = self._getPathImpl(start, goal, planner_id, use_start) @@ -789,7 +789,7 @@ def getPath( return None def _getPathThroughPosesImpl( - self, start: PoseStamped, goals: Goals, + self, start: PoseStamped, goals: list[PoseStamped], planner_id: str = '', use_start: bool = False ) -> ComputePathThroughPoses.Result: """ @@ -823,7 +823,7 @@ def _getPathThroughPosesImpl( if not self.goal_handle or not self.goal_handle.accepted: self.error('Get path was rejected!') result = ComputePathThroughPoses.Result() - result.error_code = ComputePathThroughPoses.UNKNOWN + result.error_code = ComputePathThroughPoses.Result.UNKNOWN result.error_msg = 'Get path was rejected!' return result @@ -834,8 +834,8 @@ def _getPathThroughPosesImpl( return self.result_future.result().result # type: ignore[union-attr] def getPathThroughPoses( - self, start: PoseStamped, goals: Goals, - planner_id: str = '', use_start: bool = False) -> Path: + self, start: PoseStamped, goals: list[PoseStamped], + planner_id: str = '', use_start: bool = False) -> Optional[Path]: """Send a `ComputePathThroughPoses` action request.""" self.clearTaskError() rtn = self._getPathThroughPosesImpl(start, goals, planner_id, use_start) @@ -878,7 +878,7 @@ def _getRouteImpl( else: self.error('Invalid start and goal types. Must be PoseStamped for pose or int for ID') result = ComputeRoute.Result() - result.error_code = ComputeRoute.UNKNOWN + result.error_code = ComputeRoute.Result.UNKNOWN result.error_msg = 'Request type fields were invalid!' return result @@ -890,7 +890,7 @@ def _getRouteImpl( if not self.goal_handle or not self.goal_handle.accepted: self.error('Get route was rejected!') result = ComputeRoute.Result() - result.error_code = ComputeRoute.UNKNOWN + result.error_code = ComputeRoute.Result.UNKNOWN result.error_msg = 'Get route was rejected!' return result @@ -942,7 +942,8 @@ def getAndTrackRoute( goal_msg.goal = goal goal_msg.use_poses = True else: - self.setTaskError(ComputeAndTrackRoute.UNKNOWN, 'Request type fields were invalid!') + self.setTaskError(ComputeAndTrackRoute.Result.UNKNOWN, + 'Request type fields were invalid!') self.error('Invalid start and goal types. Must be PoseStamped for pose or int for ID') return None @@ -954,7 +955,7 @@ def getAndTrackRoute( if not self.route_goal_handle or not self.route_goal_handle.accepted: msg = 'Compute and track route was rejected!' - self.setTaskError(ComputeAndTrackRoute.UNKNOWN, msg) + self.setTaskError(ComputeAndTrackRoute.Result.UNKNOWN, msg) self.error(msg) return None @@ -988,7 +989,7 @@ def _smoothPathImpl( if not self.goal_handle or not self.goal_handle.accepted: self.error('Smooth path was rejected!') result = SmoothPath.Result() - result.error_code = SmoothPath.UNKNOWN + result.error_code = SmoothPath.Result.UNKNOWN result.error_msg = 'Smooth path was rejected' return result @@ -1000,7 +1001,7 @@ def _smoothPathImpl( def smoothPath( self, path: Path, smoother_id: str = '', - max_duration: float = 2.0, check_for_collision: bool = False) -> Path: + max_duration: float = 2.0, check_for_collision: bool = False) -> Optional[Path]: """Send a `SmoothPath` action request.""" self.clearTaskError() rtn = self._smoothPathImpl(path, smoother_id, max_duration, check_for_collision) @@ -1030,14 +1031,14 @@ def changeMap(self, map_filepath: str) -> bool: return False result = future_result.result - if result != LoadMap.Response().RESULT_SUCCESS: - if result == LoadMap.RESULT_MAP_DOES_NOT_EXIST: + if result != LoadMap.Response.RESULT_SUCCESS: + if result == LoadMap.Response.RESULT_MAP_DOES_NOT_EXIST: reason = 'Map does not exist' - elif result == LoadMap.INVALID_MAP_DATA: + elif result == LoadMap.Response.RESULT_INVALID_MAP_DATA: reason = 'Invalid map data' - elif result == LoadMap.INVALID_MAP_METADATA: + elif result == LoadMap.Response.RESULT_INVALID_MAP_METADATA: reason = 'Invalid map metadata' - elif result == LoadMap.UNDEFINED_FAILURE: + elif result == LoadMap.Response.RESULT_UNDEFINED_FAILURE: reason = 'Undefined failure' else: reason = 'Unknown' @@ -1144,7 +1145,7 @@ def clearGlobalCostmapAroundPose(self, pose: PoseStamped, reset_distance: float) return - def getGlobalCostmap(self) -> OccupancyGrid: + def getGlobalCostmap(self) -> Optional[Costmap]: """Get the global costmap.""" while not self.get_costmap_global_srv.wait_for_service(timeout_sec=1.0): self.info('Get global costmaps service not available, waiting...') @@ -1159,7 +1160,7 @@ def getGlobalCostmap(self) -> OccupancyGrid: return result.map - def getLocalCostmap(self) -> OccupancyGrid: + def getLocalCostmap(self) -> Optional[Costmap]: """Get the local costmap.""" while not self.get_costmap_local_srv.wait_for_service(timeout_sec=1.0): self.info('Get local costmaps service not available, waiting...') @@ -1201,7 +1202,7 @@ def lifecycleStartup(self) -> None: while not mgr_client.wait_for_service(timeout_sec=1.0): self.info(f'{srv_name} service not available, waiting...') req = ManageLifecycleNodes.Request() - req.command = ManageLifecycleNodes.Request().STARTUP + req.command = ManageLifecycleNodes.Request.STARTUP future = mgr_client.call_async(req) # starting up requires a full map->odom->base_link TF tree @@ -1226,7 +1227,7 @@ def lifecycleShutdown(self) -> None: while not mgr_client.wait_for_service(timeout_sec=1.0): self.info(f'{srv_name} service not available, waiting...') req = ManageLifecycleNodes.Request() - req.command = ManageLifecycleNodes.Request().SHUTDOWN + req.command = ManageLifecycleNodes.Request.SHUTDOWN future = mgr_client.call_async(req) rclpy.spin_until_future_complete(self, future) future.result() @@ -1268,12 +1269,13 @@ def _amclPoseCallback(self, msg: PoseWithCovarianceStamped) -> None: self.initial_pose_received = True return - def _feedbackCallback(self, msg: NavigateToPose.Feedback) -> None: + def _feedbackCallback(self, msg: Any) -> None: self.debug('Received action feedback message') self.feedback = msg.feedback return - def _routeFeedbackCallback(self, msg: ComputeAndTrackRoute.Feedback) -> None: + def _routeFeedbackCallback( + self, msg: ComputeAndTrackRoute.Impl.FeedbackMessage) -> None: self.debug('Received route action feedback message') self.route_feedback.append(msg.feedback) return diff --git a/nav2_system_tests/src/behaviors/backup/backup_tester.py b/nav2_system_tests/src/behaviors/backup/backup_tester.py index 1d51f3d492a..1d8748286e9 100755 --- a/nav2_system_tests/src/behaviors/backup/backup_tester.py +++ b/nav2_system_tests/src/behaviors/backup/backup_tester.py @@ -299,7 +299,7 @@ def shutdown(self) -> None: self.info_msg(f'{transition_service} service not available, waiting...') req = ManageLifecycleNodes.Request() - req.command = ManageLifecycleNodes.Request().SHUTDOWN + req.command = ManageLifecycleNodes.Request.SHUTDOWN future = mgr_client.call_async(req) try: rclpy.spin_until_future_complete(self, future) diff --git a/nav2_system_tests/src/gps_navigation/tester.py b/nav2_system_tests/src/gps_navigation/tester.py index b522f8e69a2..5ba69c35f80 100755 --- a/nav2_system_tests/src/gps_navigation/tester.py +++ b/nav2_system_tests/src/gps_navigation/tester.py @@ -34,7 +34,7 @@ class GpsWaypointFollowerTest(Node): def __init__(self) -> None: super().__init__(node_name='nav2_gps_waypoint_tester', namespace='') - self.waypoints: list[float] = [] + self.waypoints: list[GeoPose] = [] self.action_client: ActionClient[ FollowGPSWaypoints.Goal, FollowGPSWaypoints.Result, diff --git a/nav2_system_tests/src/route/tester_node.py b/nav2_system_tests/src/route/tester_node.py index 018b8f2dd33..892f9369277 100755 --- a/nav2_system_tests/src/route/tester_node.py +++ b/nav2_system_tests/src/route/tester_node.py @@ -316,7 +316,8 @@ def runTrackRouteTest(self) -> bool: self.info_msg('Goal succeeded!') return True - def feedback_callback(self, feedback_msg: ComputeAndTrackRoute.Feedback) -> None: + def feedback_callback( + self, feedback_msg: ComputeAndTrackRoute.Impl.FeedbackMessage) -> None: self.feedback_msgs.append(feedback_msg.feedback) def distanceFromGoal(self) -> float: @@ -386,7 +387,7 @@ def shutdown(self) -> None: self.info_msg(f'{transition_service} service not available, waiting...') req = ManageLifecycleNodes.Request() - req.command = ManageLifecycleNodes.Request().SHUTDOWN + req.command = ManageLifecycleNodes.Request.SHUTDOWN future = mgr_client.call_async(req) try: self.info_msg('Shutting down navigation lifecycle manager...') @@ -401,7 +402,7 @@ def shutdown(self) -> None: self.info_msg(f'{transition_service} service not available, waiting...') req = ManageLifecycleNodes.Request() - req.command = ManageLifecycleNodes.Request().SHUTDOWN + req.command = ManageLifecycleNodes.Request.SHUTDOWN future = mgr_client.call_async(req) try: self.info_msg('Shutting down localization lifecycle manager...') diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index 4ea17924f81..bc2f11bc3f7 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/nav2_system_tests/src/waypoint_follower/tester.py @@ -35,7 +35,7 @@ class WaypointFollowerTest(Node): def __init__(self) -> None: super().__init__(node_name='nav2_waypoint_tester', namespace='') - self.waypoints: list[float] = [] + self.waypoints: list[PoseStamped] = [] self.action_client: ActionClient[ FollowWaypoints.Goal, FollowWaypoints.Result, diff --git a/tools/planner_benchmarking/metrics.py b/tools/planner_benchmarking/metrics.py index 86cb4379b5f..443a998c3f9 100644 --- a/tools/planner_benchmarking/metrics.py +++ b/tools/planner_benchmarking/metrics.py @@ -20,12 +20,12 @@ from random import randint, seed, uniform import time +from builtin_interfaces.msg import Time from geometry_msgs.msg import PoseStamped from nav2_simple_commander.robot_navigator import BasicNavigator import numpy as np from numpy.typing import NDArray import rclpy -from rclpy.time import Time from transforms3d.euler import euler2quat @@ -118,7 +118,7 @@ def main() -> None: max_cost = 210 side_buffer = 100 time_stamp = navigator.get_clock().now().to_msg() - results: list[PoseStamped] = [] + results: list[list[PoseStamped]] = [] seed(33) random_pairs = 100 diff --git a/tools/pyproject.toml b/tools/pyproject.toml index 971360845f6..2dca7d8e045 100644 --- a/tools/pyproject.toml +++ b/tools/pyproject.toml @@ -45,8 +45,6 @@ module = [ "std_msgs.*", "zmq.*", "std_srvs.*", - "graphviz.*", - "transforms3d.*", "geopandas.*", "pandas.*", ] diff --git a/tools/smoother_benchmarking/metrics.py b/tools/smoother_benchmarking/metrics.py index 6eea9c36c29..45b9f5750de 100644 --- a/tools/smoother_benchmarking/metrics.py +++ b/tools/smoother_benchmarking/metrics.py @@ -20,12 +20,13 @@ from random import randint, seed, uniform from typing import Optional +from builtin_interfaces.msg import Time from geometry_msgs.msg import PoseStamped +from nav2_msgs.action import ComputePathToPose, SmoothPath from nav2_simple_commander.robot_navigator import BasicNavigator import numpy as np from numpy.typing import NDArray import rclpy -from rclpy.time import Time from transforms3d.euler import euler2quat # Note: Map origin is assumed to be (0,0) @@ -33,7 +34,7 @@ def getPlannerResults( navigator: BasicNavigator, initial_pose: PoseStamped, - goal_pose: PoseStamped, planner: str) -> PoseStamped: + goal_pose: PoseStamped, planner: str) -> Optional[ComputePathToPose.Result]: result = navigator._getPathImpl(initial_pose, goal_pose, planner, use_start=True) if result is None or result.error_code != 0: print(planner, 'planner failed to produce the path') @@ -43,7 +44,7 @@ def getPlannerResults( def getSmootherResults( navigator: BasicNavigator, path: PoseStamped, - smoothers: list[str]) -> Optional[list[PoseStamped]]: + smoothers: list[str]) -> Optional[list[SmoothPath.Result]]: smoothed_results = [] for smoother in smoothers: smoothed_result = navigator._smoothPathImpl(path, smoother)