diff --git a/docs/vis.rviz b/docs/vis.rviz index 6293a3f3..d66a0bee 100644 --- a/docs/vis.rviz +++ b/docs/vis.rviz @@ -1,23 +1,15 @@ Panels: - Class: rviz_common/Displays - Help Height: 138 + Help Height: 114 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - - /Perception1/DepthCloud1/Auto Size1 - - /Perception1/DepthCloud1/Occlusion Compensation1 - - /Perception1/DepthCloud1/Autocompute Value Bounds1 - - /Planning1/3DGridMap1 + - /Perception1/Local Cloud1/Autocompute Value Bounds1 + - /Planning1 - /Planning1/3DGridMap1/Autocompute Value Bounds1 - - /Planning1/TrajScores1 - - /Planning1/PointCloud21 - - /Planning1/ObstacleMask1 - - /Planning1/Footprint1 - - /PointCloud21 - - /Path1 Splitter Ratio: 0.5 - Tree Height: 756 + Tree Height: 114 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -62,17 +54,71 @@ Visualization Manager: Frame Timeout: 10000 Frames: All Enabled: true - cam_depth: + base_footprint: Value: true - cam_left: + base_link: Value: true - cam_rgb: + camera: Value: true - cam_right: + camera_color_frame: Value: true - camera_imu_optical_frame: + camera_color_optical_frame: Value: true - imu: + camera_depth_frame: + Value: true + camera_depth_optical_frame: + Value: true + camera_imu_frame: + Value: true + camera_left_ir_frame: + Value: true + camera_left_ir_optical_frame: + Value: true + camera_link: + Value: true + camera_right_ir_frame: + Value: true + camera_right_ir_optical_frame: + Value: true + imu_link: + Value: true + lf_foot_link: + Value: true + lf_hip_link: + Value: true + lf_lower_leg_link: + Value: true + lf_upper_leg_link: + Value: true + lh_foot_link: + Value: true + lh_hip_link: + Value: true + lh_lower_leg_link: + Value: true + lh_upper_leg_link: + Value: true + map: + Value: true + odom: + Value: true + rf_foot_link: + Value: true + rf_hip_link: + Value: true + rf_lower_leg_link: + Value: true + rf_upper_leg_link: + Value: true + rh_foot_link: + Value: true + rh_hip_link: + Value: true + rh_lower_leg_link: + Value: true + rh_upper_leg_link: + Value: true + trunk: Value: true world: Value: true @@ -83,16 +129,7 @@ Visualization Manager: Show Names: false Tree: world: - imu: - cam_left: - cam_depth: - {} - cam_rgb: - {} - cam_right: - {} - camera_imu_optical_frame: - {} + {} Update Interval: 0 Value: true - Class: rviz_common/Group @@ -121,6 +158,7 @@ Visualization Manager: Min Value: -3 Value: false Axis: Z + CameraInfo Topic: /slam/camera_info Channel Name: intensity Class: rviz_default_plugins/DepthCloud Color: 255; 255; 255 @@ -190,7 +228,7 @@ Visualization Manager: Reliability Policy: Reliable Value: /slam/odometry Value: true - Enabled: true + Enabled: false Name: Perception - Class: rviz_common/Group Displays: @@ -209,9 +247,9 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 0; 255; 0 - Max Intensity: 0.30000001192092896 + Max Intensity: -0.30000001192092896 Min Color: 116; 0; 0 - Min Intensity: -0.5 + Min Intensity: -1 Name: 3DGridMap Position Transformer: XYZ Selectable: true @@ -232,7 +270,7 @@ Visualization Manager: Buffer Length: 1 Class: rviz_default_plugins/Path Color: 25; 255; 0 - Enabled: false + Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 @@ -255,7 +293,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /planning/trajectory_path - Value: false + Value: true - Angle Tolerance: 0.10000000149011612 Class: rviz_default_plugins/Odometry Covariance: @@ -273,7 +311,7 @@ Visualization Manager: Scale: 1 Value: true Value: true - Enabled: false + Enabled: true Keep: 1 Name: TargetPose Position Tolerance: 0.10000000149011612 @@ -294,7 +332,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /control/target_pose - Value: false + Value: true - Alpha: 0.699999988079071 Class: rviz_default_plugins/Map Color Scheme: map @@ -400,12 +438,23 @@ Visualization Manager: Use Timestamp: false Value: true - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z Channel Name: intensity Class: rviz_default_plugins/PointCloud Color: 255; 255; 0 Color Transformer: FlatColor Decay Time: 0 Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 Name: Footprint Position Transformer: XYZ Selectable: true @@ -420,6 +469,7 @@ Visualization Manager: Reliability Policy: Reliable Value: /planning/footprint Use Fixed Frame: true + Use rainbow: true Value: true Enabled: true Name: Planning @@ -573,7 +623,7 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: RGB8 Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 4096 @@ -594,7 +644,7 @@ Visualization Manager: Value: /slam/global_cloud Use Fixed Frame: true Use rainbow: true - Value: true + Value: false - Alpha: 1 Buffer Length: 1 Class: rviz_default_plugins/Path @@ -669,7 +719,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/ThirdPersonFollower - Distance: 10.385795593261719 + Distance: 4.001773357391357 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -684,22 +734,22 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.6347965598106384 + Pitch: 1.289795994758606 Target Frame: camera Value: ThirdPersonFollower (rviz_default_plugins) - Yaw: 0.05539698153734207 + Yaw: 4.973613262176514 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1994 + Height: 749 HeightMap: collapsed: false Hide Left Dock: false Hide Right Dock: false Image: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Time: @@ -710,9 +760,9 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 2331 - X: 1419 - Y: 64 + Width: 1272 + X: 1280 + Y: 782 color image: collapsed: false gray image: diff --git a/tinynav/core/imu_propagator_node.py b/tinynav/core/imu_propagator_node.py new file mode 100644 index 00000000..a36002f4 --- /dev/null +++ b/tinynav/core/imu_propagator_node.py @@ -0,0 +1,119 @@ +import logging + +import numpy as np +import rclpy +from message_filters import ApproximateTimeSynchronizer, Subscriber +from nav_msgs.msg import Odometry +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy +from scipy.spatial.transform import Rotation as R +from sensor_msgs.msg import Imu + +from tinynav.core.math_utils import msg2np, np2msg + + +def integrate(odom_prev, imu, gravity_world=np.array([0.0, 0.0, -9.80])): + t0, odom_msg = odom_prev + pose_prev, velocity_prev = msg2np(odom_msg) + rotation_prev = pose_prev[:3, :3] + translation_prev = pose_prev[:3, 3] + + t1, imu_msg = imu + gyro = np.array([ + imu_msg.angular_velocity.x, + imu_msg.angular_velocity.y, + imu_msg.angular_velocity.z, + ], dtype=float) + accel = np.array([ + imu_msg.linear_acceleration.x, + imu_msg.linear_acceleration.y, + imu_msg.linear_acceleration.z, + ], dtype=float) + + dt = t1 - t0 + delta_rotation = R.from_rotvec(gyro * dt).as_matrix() + rotation_new = rotation_prev @ delta_rotation + accel_world = rotation_prev @ accel + gravity_world + velocity_new = velocity_prev + accel_world * dt + translation_new = translation_prev + velocity_prev * dt + 0.5 * accel_world * dt * dt + + pose_new = np.eye(4) + pose_new[:3, :3] = rotation_new + pose_new[:3, 3] = translation_new + + odom_new = np2msg(pose_new, imu_msg.header.stamp, odom_msg.header.frame_id, odom_msg.child_frame_id, velocity_new) + odom_new.twist.twist.angular.x = gyro[0] + odom_new.twist.twist.angular.y = gyro[1] + odom_new.twist.twist.angular.z = gyro[2] + return t1, odom_new + + +class ImuPropagatorNode(Node): + def __init__(self): + super().__init__("imu_propagator_node") + + qos_profile = QoSProfile(reliability=ReliabilityPolicy.BEST_EFFORT, depth=1000) + self.imu_sub = self.create_subscription(Imu, "/camera/camera/imu", self.imu_callback, qos_profile) + self.odom_sub = self.create_subscription(Odometry, "/slam/odometry", self.odom_callback, qos_profile) + self.odom_pub = self.create_publisher(Odometry, "/slam/odometry_100hz", 50) + + self.imu_buffer = [] + self.odom_10hz_buffer = [] + self.odom_100hz_buffer = [] + + def imu_callback(self, imu_msg: Imu): + if len(self.odom_100hz_buffer) == 0: + return + + timestamp = self._stamp_to_sec(imu_msg.header.stamp) + print("imu: ", timestamp) + self.imu_buffer.append((timestamp, imu_msg)) + if len(self.imu_buffer) > 2000: + self.imu_buffer.pop(0) + + if self.imu_buffer[-1][0] <= self.odom_100hz_buffer[-1][0] + 0.050: + return + + start_idx = None + for i in range(len(self.imu_buffer)): + if self.imu_buffer[-(i + 1)][0] > self.odom_100hz_buffer[-1][0]: + start_idx = -(i + 1) + else: + break + + if start_idx is None: + return + + for i in range(start_idx, 0): + self.odom_100hz_buffer.append(integrate(self.odom_100hz_buffer[-1], self.imu_buffer[i])) + if len(self.odom_100hz_buffer) > 1000: + self.odom_100hz_buffer.pop(0) + + self.odom_pub.publish(self.odom_100hz_buffer[-1][1]) + + def odom_callback(self, msg: Odometry): + timestamp = self._stamp_to_sec(msg.header.stamp) + self.odom_10hz_buffer.append((timestamp, msg)) + if len(self.odom_10hz_buffer) > 100: + self.odom_10hz_buffer.pop(0) + + while self.odom_100hz_buffer and self.odom_100hz_buffer[-1][0] > timestamp: + self.odom_100hz_buffer.pop() + self.odom_100hz_buffer.append((timestamp, msg)) + + @staticmethod + def _stamp_to_sec(stamp) -> float: + return stamp.sec + stamp.nanosec * 1e-9 + + +def main(args=None): + rclpy.init(args=args) + node = ImuPropagatorNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + logging.basicConfig(level=logging.INFO) + main() diff --git a/tinynav/core/perception_node.py b/tinynav/core/perception_node.py index 3e1453b2..5af55ffb 100644 --- a/tinynav/core/perception_node.py +++ b/tinynav/core/perception_node.py @@ -26,6 +26,7 @@ from dataclasses import dataclass from gtsam.symbol_shorthand import X, B, V +from tinynav.core.imu_propagator_node import ImuPropagatorNode _N = 5 _M = 1000 @@ -144,8 +145,8 @@ def __init__(self, verbose_timer: bool = True): # Noise model (continuous-time) # for Realsense D435i - accel_noise_density = 0.25 # [m/s^2/√Hz] - gyro_noise_density = 0.00005 # [rad/s/√Hz] + accel_noise_density = 0.50 # [m/s^2/√Hz] + gyro_noise_density = 0.50 # [rad/s/√Hz] bias_acc_rw_sigma = 0.001 bias_gyro_rw_sigma = 0.0001 self.pre_integration_params = gtsam.PreintegrationCombinedParams.MakeSharedU() @@ -567,31 +568,22 @@ async def process(self, left_msg, right_msg): def main(args=None): - rclpy.init(args=args) - parser = argparse.ArgumentParser() - parser.set_defaults(verbose_timer=True) - parser.add_argument("--verbose_timer", action="store_true", help="Enable verbose timer output") - parser.add_argument("--no_verbose_timer", dest="verbose_timer", action="store_false", help="Disable verbose timer output") - parser.add_argument("--log_file", type=str, default="odom.log", help="Path to the log file") - parsed_args, unknown_args = parser.parse_known_args(sys.argv[1:]) - print(f"Verbose timer: {parsed_args.verbose_timer}") - - logging.basicConfig( - level=logging.INFO, - format="%(asctime)s - %(filename)s:%(lineno)s - %(message)s", - datefmt="%Y-%m-%d %H:%M:%S", - handlers=[logging.StreamHandler(sys.stdout), logging.FileHandler(parsed_args.log_file)], - ) + parser = argparse.ArgumentParser(description='Run TinyNav perception node.') + parser.add_argument('--verbose_timer', action='store_true', help='Print timing for key pipeline stages.') + parsed_args = parser.parse_args(args=sys.argv[1:] if args is None else args) perception_node = PerceptionNode(verbose_timer=parsed_args.verbose_timer) + imu_propagator_node = ImuPropagatorNode() - executor = rclpy.executors.SingleThreadedExecutor() + executor = rclpy.executors.MultiThreadedExecutor() executor.add_node(perception_node) + executor.add_node(imu_propagator_node) executor.spin() perception_node.destroy_node() - executor.shutdown() - + imu_propagator_node.destroy_node() + rclpy.shutdown() -if __name__ == "__main__": +if __name__ == '__main__': + logging.basicConfig(level=logging.INFO) main()