From b4c436b8dd876a9eaad1cc7dc538dfbc22ad89c0 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 9 Aug 2024 12:26:48 -0700 Subject: [PATCH] lintin --- .../launch/tb3_loopback_simulation.launch.py | 15 ++-------- .../launch/tb4_loopback_simulation.launch.py | 16 ++-------- .../nav2_loopback_sim/loopback_simulator.py | 29 ++++++++++--------- nav2_loopback_sim/nav2_loopback_sim/utils.py | 11 ++----- 4 files changed, 22 insertions(+), 49 deletions(-) diff --git a/nav2_bringup/launch/tb3_loopback_simulation.launch.py b/nav2_bringup/launch/tb3_loopback_simulation.launch.py index 2d31edf0f3..9e598e854c 100644 --- a/nav2_bringup/launch/tb3_loopback_simulation.launch.py +++ b/nav2_bringup/launch/tb3_loopback_simulation.launch.py @@ -15,25 +15,20 @@ """This is all-in-one launch script intended for use by nav2 developers.""" import os -import tempfile from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import ( DeclareLaunchArgument, - ExecuteProcess, GroupAction, IncludeLaunchDescription, - OpaqueFunction, - RegisterEventHandler, ) from launch.conditions import IfCondition -from launch.event_handlers import OnShutdown from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PythonExpression -from launch_ros.descriptions import ParameterFile +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node, SetParameter +from launch_ros.descriptions import ParameterFile from nav2_common.launch import RewrittenYaml @@ -46,7 +41,6 @@ def generate_launch_description(): sim_dir = get_package_share_directory('nav2_minimal_tb3_sim') # Create the launch configuration variables - slam = LaunchConfiguration('slam') namespace = LaunchConfiguration('namespace') use_namespace = LaunchConfiguration('use_namespace') map_yaml_file = LaunchConfiguration('map') @@ -73,10 +67,6 @@ def generate_launch_description(): description='Whether to apply a namespace to the navigation stack', ) - declare_slam_cmd = DeclareLaunchArgument( - 'slam', default_value='False', description='Whether run a SLAM' - ) - declare_map_yaml_cmd = DeclareLaunchArgument( 'map', default_value=os.path.join(bringup_dir, 'maps', 'tb3_sandbox.yaml'), @@ -214,7 +204,6 @@ def generate_launch_description(): # Declare the launch options ld.add_action(declare_namespace_cmd) ld.add_action(declare_use_namespace_cmd) - ld.add_action(declare_slam_cmd) ld.add_action(declare_map_yaml_cmd) ld.add_action(declare_params_file_cmd) ld.add_action(declare_autostart_cmd) diff --git a/nav2_bringup/launch/tb4_loopback_simulation.launch.py b/nav2_bringup/launch/tb4_loopback_simulation.launch.py index 6002c635de..0ad911ddc7 100644 --- a/nav2_bringup/launch/tb4_loopback_simulation.launch.py +++ b/nav2_bringup/launch/tb4_loopback_simulation.launch.py @@ -15,25 +15,20 @@ """This is all-in-one launch script intended for use by nav2 developers.""" import os -import tempfile from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import ( DeclareLaunchArgument, - ExecuteProcess, GroupAction, IncludeLaunchDescription, - OpaqueFunction, - RegisterEventHandler, ) from launch.conditions import IfCondition -from launch.event_handlers import OnShutdown from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import Command, LaunchConfiguration, PythonExpression -from launch_ros.descriptions import ParameterFile +from launch.substitutions import Command, LaunchConfiguration from launch_ros.actions import Node, SetParameter +from launch_ros.descriptions import ParameterFile from nav2_common.launch import RewrittenYaml @@ -43,11 +38,9 @@ def generate_launch_description(): bringup_dir = get_package_share_directory('nav2_bringup') loopback_sim_dir = get_package_share_directory('nav2_loopback_sim') launch_dir = os.path.join(bringup_dir, 'launch') - sim_dir = get_package_share_directory('nav2_minimal_tb4_sim') desc_dir = get_package_share_directory('nav2_minimal_tb4_description') # Create the launch configuration variables - slam = LaunchConfiguration('slam') namespace = LaunchConfiguration('namespace') use_namespace = LaunchConfiguration('use_namespace') map_yaml_file = LaunchConfiguration('map') @@ -74,10 +67,6 @@ def generate_launch_description(): description='Whether to apply a namespace to the navigation stack', ) - declare_slam_cmd = DeclareLaunchArgument( - 'slam', default_value='False', description='Whether run a SLAM' - ) - declare_map_yaml_cmd = DeclareLaunchArgument( 'map', default_value=os.path.join(bringup_dir, 'maps', 'depot.yaml'), # Try warehouse.yaml! @@ -222,7 +211,6 @@ def generate_launch_description(): # Declare the launch options ld.add_action(declare_namespace_cmd) ld.add_action(declare_use_namespace_cmd) - ld.add_action(declare_slam_cmd) ld.add_action(declare_map_yaml_cmd) ld.add_action(declare_params_file_cmd) ld.add_action(declare_autostart_cmd) diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index 8e02d09849..3990797cbe 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -13,21 +13,20 @@ # limitations under the License. -import copy import math -from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped, Twist -from geometry_msgs.msg import TransformStamped, Vector3, Quaternion +from geometry_msgs.msg import PoseWithCovarianceStamped, Twist +from geometry_msgs.msg import Quaternion, TransformStamped, Vector3 from nav_msgs.msg import Odometry -import numpy as np import rclpy from rclpy.duration import Duration from rclpy.node import Node -from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy +from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy from sensor_msgs.msg import LaserScan +from tf2_ros import TransformBroadcaster import tf_transformations -from tf2_ros import Buffer, TransformBroadcaster, TransformListener -from .utils import transformStampedToMatrix, matrixToTransform, addYawToQuat + +from .utils import addYawToQuat, matrixToTransform, transformStampedToMatrix """ @@ -87,10 +86,10 @@ def __init__(self): durability=DurabilityPolicy.VOLATILE, depth=10) self.scan_pub = self.create_publisher(LaserScan, 'scan', sensor_qos) - + self.setupTimer = self.create_timer(0.1, self.setupTimerCallback) self.info('Loopback simulator initialized') - + def setupTimerCallback(self): # Publish initial identity odom transform & laser scan to warm up system self.tf_broadcaster.sendTransform(self.t_odom_to_base_link) @@ -123,7 +122,7 @@ def initialPoseCallback(self, msg): self.setupTimer = None self.timer = self.create_timer(self.update_dur, self.timerCallback) return - + self.initial_pose = msg.pose.pose # Adjust map->odom transform based on new initial pose, keeping odom->base_link the same @@ -137,14 +136,16 @@ def initialPoseCallback(self, msg): mat_map_to_base_link = transformStampedToMatrix(t_map_to_base_link) mat_odom_to_base_link = transformStampedToMatrix(self.t_odom_to_base_link) mat_base_link_to_odom = tf_transformations.inverse_matrix(mat_odom_to_base_link) - mat_map_to_odom = tf_transformations.concatenate_matrices(mat_map_to_base_link, mat_base_link_to_odom) + mat_map_to_odom = \ + tf_transformations.concatenate_matrices(mat_map_to_base_link, mat_base_link_to_odom) self.t_map_to_odom.transform = matrixToTransform(mat_map_to_odom) def timerCallback(self): # If no data, just republish existing transforms without change - if self.curr_cmd_vel is None or self.get_clock().now() - self.curr_cmd_vel_time > Duration(seconds=1): + one_sec = Duration(seconds=1) + if self.curr_cmd_vel is None or self.get_clock().now() - self.curr_cmd_vel_time > one_sec: self.publishTransforms(self.t_map_to_odom, self.t_odom_to_base_link) - self.curr_cmd_vel= None + self.curr_cmd_vel = None return # Update odom->base_link from cmd_vel @@ -187,7 +188,7 @@ def publishTransforms(self, map_to_odom, odom_to_base_link): odom_to_base_link.header.stamp = self.get_clock().now().to_msg() self.tf_broadcaster.sendTransform(map_to_odom) self.tf_broadcaster.sendTransform(odom_to_base_link) - + def publishOdometry(self, odom_to_base_link): odom = Odometry() odom.header.stamp = self.get_clock().now().to_msg() diff --git a/nav2_loopback_sim/nav2_loopback_sim/utils.py b/nav2_loopback_sim/nav2_loopback_sim/utils.py index ba4dc8a3f0..f6ae5f5e7d 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/utils.py +++ b/nav2_loopback_sim/nav2_loopback_sim/utils.py @@ -13,9 +13,9 @@ # limitations under the License. +from geometry_msgs.msg import Quaternion, Transform import numpy as np import tf_transformations -from geometry_msgs.msg import Transform, TransformStamped, Quaternion """ @@ -35,10 +35,8 @@ def addYawToQuat(quaternion, yaw_to_add): new_quaternion.w = new_quaternion_array[3] return new_quaternion + def transformStampedToMatrix(transform): - """ - Converts a geometry_msgs/TransformStamped to a 4x4 numpy matrix. - """ translation = transform.transform.translation rotation = transform.transform.rotation matrix = np.eye(4) @@ -54,15 +52,12 @@ def transformStampedToMatrix(transform): matrix[:3, :3] = rotation_matrix[:3, :3] return matrix + def matrixToTransform(matrix): - """ - Converts a 4x4 numpy matrix to a geometry_msgs/Transform. - """ transform = Transform() transform.translation.x = matrix[0, 3] transform.translation.y = matrix[1, 3] transform.translation.z = matrix[2, 3] - rotation_matrix = matrix[:3, :3] quaternion = tf_transformations.quaternion_from_matrix(matrix) transform.rotation.x = quaternion[0] transform.rotation.y = quaternion[1]