Improve type annotations for ament_mypy (#5575)

* Enable tools directory to be mypy compliant.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Enable nav2_system_tests to be mypy compliant.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Enable nav2_docking to be mypy compliant.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Enable nav2_simple_commander to be mypy compliant.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Enable nav2_loopback_sim to be mypy compliant.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Removed unused ignores for packages for mypy compliance.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Added nav2_msgs path fixes to mypy compliance.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

---------

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

Author: leander-dsouza

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(

nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py

@@ -14,21 +14,22 @@
 
 
 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
 from rclpy.client import Client
 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:

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)

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