From 95c49b0d16224bb530945f6a17efd7c78c8466dc Mon Sep 17 00:00:00 2001 From: Edd Date: Fri, 3 Dec 2021 14:45:40 +0300 Subject: [PATCH 1/2] wip --- .../state_estimation_metalbot.launch.py | 23 +++- .../state_estimation_2d/filter.py | 63 +++++---- .../state_estimation_2d/model.py | 1 - .../state_estimation_node.py | 124 +++++++++++++++--- 4 files changed, 166 insertions(+), 45 deletions(-) diff --git a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/launch/state_estimation_metalbot.launch.py b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/launch/state_estimation_metalbot.launch.py index 34f26b3..687d77a 100644 --- a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/launch/state_estimation_metalbot.launch.py +++ b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/launch/state_estimation_metalbot.launch.py @@ -23,14 +23,33 @@ def generate_launch_description(): launch.actions.DeclareLaunchArgument( 'viz', default_value=enable_viz, - description='update rate' + description='enable_visualization' ), Node( package="state_estimation_2d", executable="state_estimation_2d", output='screen', emulate_tty=True, - parameters=[{"use_sim_time": False}], + parameters=[ + {"use_sim_time": False}, + {"odom_topic": "velocity"}, + {"imu_topic": "imu"}, + {"imu_accel_topic": "/camera/accel/sample"}, + {"imu_gyro_topic": "/camera/gyro/sample"}, + {"cmd_topic": "cmd_vel"}, + {"odom_sim_topic": "odom_noised"}, + {"publish_topic": "odom_filtered"}, + {"imu_frame": "camera_gyro_optical_frame"}, + {"robot_base_frame": "base_link"}, + {"path_to_nn_model": "http://192.168.194.51:8345/ml-control/gz-rosbot/new_model_dynamic_batch.onnx"}, + {"time_step": 0.1}, + {"odom_sim_covariance": [0.5, 0, + 0, 0.1]}, + {"imu_sim_covariance": [0.5, 0, + 0, 0.1]}, + {"gyro_robot_covariance": 200.0}, + {"accel_robot_covariance": 200.0}, + ], ), Node( package="path_visualizer", diff --git a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/filter.py b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/filter.py index 8f2c192..2b99ecf 100644 --- a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/filter.py +++ b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/filter.py @@ -3,6 +3,7 @@ from numpy.linalg import inv import math import scipy +from numba import njit from state_estimation_2d.model import * from state_estimation_2d.measurement import * @@ -133,39 +134,49 @@ def update_odom(self, z_odom, R_odom): """ Update state vector using odometry measurements""" H = get_jacobian_odom(self.x_opt) y = z_odom - H @ self.x_opt - G = H @ self.P_opt @ H.T + R_odom - K = self.P_opt @ H.T @ inv(G) - I = np.eye(5) - self.P_opt = (I - K @ H) @ self.P_opt - self.x_opt = self.x_opt + K @ y + self.x_opt, self.P_opt = kalman_update( + self.x_opt, + self.P_opt, + y, + R_odom, + H + ) def update_imu(self, z_imu, R_imu): """ Update state vector using imu measurements""" H = get_jacobian_imu(self.x_opt) y = z_imu - imu(self.x_opt) - G = H @ self.P_opt @ H.T + R_imu - K = self.P_opt @ H.T @ inv(G) - I = np.eye(5) - self.P_opt = (I - K @ H) @ self.P_opt - self.x_opt = self.x_opt + K @ y - + self.x_opt, self.P_opt = kalman_update( + self.x_opt, + self.P_opt, + y, + R_imu, + H + ) + def update_imu_accel(self, z_accel, R_accel): + """ Update state vector using imu measurements""" H = get_jacobian_accel(self.x_opt) y = z_accel - accel(self.x_opt) - G = H @ self.P_opt @ H.T + R_accel - K = self.P_opt @ H.T @ inv(G) - I = np.eye(5) - self.P_opt = (I - K @ H) @ self.P_opt - self.x_opt = self.x_opt + K @ y + self.x_opt, self.P_opt = kalman_update( + self.x_opt, + self.P_opt, + y, + R_accel, + H + ) def update_imu_gyro(self, z_gyro, R_gyro): + """ Update state vector using imu measurements""" H = get_jacobian_gyro(self.x_opt) - y = z_gyro - accel(self.x_opt) - G = H @ self.P_opt @ H.T + R_gyro - K = self.P_opt @ H.T @ inv(G) - I = np.eye(5) - self.P_opt = (I - K @ H) @ self.P_opt - self.x_opt = self.x_opt + K @ y + y = z_gyro - gyro(self.x_opt) + self.x_opt, self.P_opt = kalman_update( + self.x_opt, + self.P_opt, + y, + R_gyro, + H + ) @property def v(self): @@ -194,3 +205,11 @@ def yaw(self): @yaw.setter def yaw(self, value): self.x_opt[3] = value + +@njit +def kalman_update(x, P, y, R, H): + G = H @ P @ H.T + R + K = P @ H.T @ inv(G) + new_x = x + K @ y + new_P = (np.eye(x.shape[0]) - K @ H) @ P + return new_x, new_P \ No newline at end of file diff --git a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/model.py b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/model.py index 65a57c2..13fc98f 100644 --- a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/model.py +++ b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/model.py @@ -6,7 +6,6 @@ def transform_jacobian(x, dt): """Dynamic model function jacobian""" J_f = np.eye(5) J_f[0, 2] = np.cos(x[3]) * dt - # J_f[1, 2] = np.sin(x[4]) * dt J_f[0, 3] = -x[2] * np.sin(x[3]) * dt J_f[1, 2] = np.sin(x[3]) * dt J_f[1, 3] = x[2] * np.cos(x[3]) * dt diff --git a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/state_estimation_node.py b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/state_estimation_node.py index bcb84cf..d9c92d1 100644 --- a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/state_estimation_node.py +++ b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/state_estimation_node.py @@ -82,27 +82,39 @@ class StateEstimation2D(Node): def __init__(self): super().__init__('state_estimation_2d') # ROS Subscribers + self.init_parameters() + self.update_parameters() self.odom_sub = self.create_subscription( Twist, - 'velocity', + self.odom_topic, self.odometry_callback, 10) + self.odom_sim_sub = self.create_subscription( + Odometry, + self.odom_sim_topic, + self.odom_sim_callback, + 10) self.cmd_vel_sub = self.create_subscription( Twist, - 'cmd_vel', + self.cmd_topic, self.control_callback, 15) self.imu_accel_sub = self.create_subscription( Imu, - '/camera/accel/sample', + self.imu_accel_topic, self.imu_accel_callback, 15) self.imu_gyro_sub = self.create_subscription( Imu, - '/camera/gyro/sample', + self.imu_gyro_topic, self.imu_gyro_callback, 15) - self.pose_pub = self.create_publisher(Odometry, '/odom_filtered', 10) + self.imu_sub = self.create_subscription( + Imu, + self.imu_topic, + self.imu_callback, + 15) + self.pose_pub = self.create_publisher(Odometry, self.publish_topic, 10) self.tf2_broadcaster = tf2_ros.TransformBroadcaster(self) self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) @@ -113,26 +125,21 @@ def __init__(self): self.got_measurements = 0 self.control = np.zeros(2) self.z_odom = np.zeros(2) - self.z_accel = np.zeros(1) + self.z_imu = np.zeros(2) + self.z_accel = None self.z_gyro = None self.imu_extrinsic = None self.rot_extrinsic = None # Upload NN control model - self.model_path = 'http://192.168.194.51:8345/ml-control/gz-rosbot/new_model_dynamic_batch.onnx' self.model = nnio.ONNXModel(self.model_path) # Filter parameters - self.dt = 0.1 - self.R_odom = np.array([[0.5, 0], - [0, 0.1]]) - self.R_accel = np.array([[200]]) - self.R_gyro = np.array([[200]]) self.filter = Filter2D( x_init=np.zeros(5), P_init=np.eye(5) * 0.01, dt=self.dt, - v_var=0.25, - w_var=0.01, + v_var=self.R_odom[0][0]**2, + w_var=self.R_odom[1][1]**2, ) self.distance = 0 self.x_prev = 0 @@ -145,6 +152,51 @@ def __init__(self): self.step_filter ) + def init_parameters(self): + self.declare_parameters( + namespace="", + parameters=[ + ("odom_topic", "odom"), + ("imu_topic", "imu"), + ("imu_accel_topic", "/camera/accel/sample"), + ("imu_gyro_topic", "/camera/gyro/sample"), + ("cmd_topic", "cmd"), + ("odom_sim_topic", "odom_noised"), + ("publish_topic", "odom_filtered"), + ("imu_frame", "camera_gyro_optical_frame"), + ("robot_base_frame", "base_link"), + ("path_to_nn_model", "http://192.168.194.51:8345/ml-control/gz-rosbot/new_model_dynamic_batch.onnx"), + ("odom_noised_sim_topic", "odom_noised"), + ("time_step", 0.1), + ("odom_sim_covariance", [0.5, 0, + 0, 0.1]), + ("imu_sim_covariance", [0.5, 0, + 0, 0.1]), + ("gyro_robot_covariance", 200.0), + ("accel_robot_covariance", 200.0), + ] + ) + + def update_parameters(self): + self.odom_topic = self.get_parameter('odom_topic').get_parameter_value().string_value + self.imu_topic = self.get_parameter('imu_topic').get_parameter_value().string_value + self.imu_accel_topic = self.get_parameter('imu_accel_topic').get_parameter_value().string_value + self.imu_gyro_topic = self.get_parameter('imu_gyro_topic').get_parameter_value().string_value + self.cmd_topic = self.get_parameter('cmd_topic').get_parameter_value().string_value + self.odom_sim_topic = self.get_parameter('odom_sim_topic').get_parameter_value().string_value + self.publish_topic = self.get_parameter('publish_topic').get_parameter_value().string_value + self.imu_frame = self.get_parameter('imu_frame').get_parameter_value().string_value + self.robot_base_frame = self.get_parameter('robot_base_frame').get_parameter_value().string_value + self.model_path = self.get_parameter('path_to_nn_model').get_parameter_value().string_value + self.odom_noised_topic = self.get_parameter('odom_noised_sim_topic').get_parameter_value().string_value + self.dt = self.get_parameter('time_step').get_parameter_value().double_value + R_odom_vec = self.get_parameter('odom_sim_covariance').get_parameter_value().double_array_value + R_imu_vec = self.get_parameter('imu_sim_covariance').get_parameter_value().double_array_value + self.R_accel = np.array(self.get_parameter('accel_robot_covariance').get_parameter_value().double_value) + self.R_gyro = np.array(self.get_parameter('gyro_robot_covariance').get_parameter_value().double_value) + self.R_odom = np.array(R_odom_vec).reshape(len(R_odom_vec)//2, len(R_odom_vec)//2) + self.R_imu = np.array(R_imu_vec).reshape(len(R_imu_vec)//2, len(R_imu_vec)//2) + def control_callback(self, msg): """ Callback from /cmd_vel topic with control commands @@ -163,6 +215,14 @@ def odometry_callback(self, msg): self.z_odom = self.odometry_to_vector(msg) self.got_measurements = 1 + def odom_sim_callback(self, msg): + self.odom_sim = msg + self.z_odom = np.array([ + msg.twist.twist.linear.x, + msg.twist.twist.angular.z, + ]) + self.got_measurements = 1 + def odometry_to_vector(self, odom): """ Transfer odometry message to measurement vector for filter @@ -172,7 +232,7 @@ def odometry_to_vector(self, odom): """ z_odom = np.zeros(2) z_odom[0] = odom.linear.x - z_odom[1] = odom.angular.z + z_odom[1] = odom.angular.z * 2 return z_odom def imu_accel_callback(self, msg): @@ -187,7 +247,7 @@ def imu_gyro_callback(self, msg): # print(self.imu_extrinsic) if self.imu_extrinsic is None: print("Finding imu extrinsics") - self.imu_extrinsic = self.get_extrinsic("camera_gyro_optical_frame", "base_link") + self.imu_extrinsic = self.get_extrinsic(self.imu_frame, self.robot_base_frame) if self.imu_extrinsic is not None and self.rot_extrinsic is None: self.rot_extrinsic = np.ascontiguousarray(self.imu_extrinsic[:3,:3]) self.imu_gyro = msg @@ -196,6 +256,22 @@ def imu_gyro_callback(self, msg): self.imu_gyro.angular_velocity.y, self.imu_gyro.angular_velocity.z]) self.z_gyro = gyro[2] + + def imu_callback(self, msg): + """ + Callback from /imu topic + @ parameters + msg: Imu + """ + self.z_imu = self.imu_to_vector(msg) + self.got_measurements = 1 + + def imu_to_vector(self, imu): + """Transfer imu message to measurement vector for filter""" + z_imu = np.zeros(2) + z_imu[0] = imu.linear_acceleration.y + z_imu[1] = imu.angular_velocity.z + return z_imu def step_filter(self): """ @@ -208,6 +284,8 @@ def step_filter(self): self.filter.predict_by_naive_model(self.control) # Measurement update step self.filter.update_odom(self.z_odom, self.R_odom) + if self.z_imu is not None: + self.filter.update_imu(self.z_imu, self.R_imu) # if self.z_gyro is not None: if self.z_gyro is not None: self.filter.update_imu_gyro(self.z_gyro, self.R_gyro) @@ -278,10 +356,16 @@ def state_to_odometry(self, x, P): self.odom_filtered.pose.pose.orientation.z = q[2] self.odom_filtered.pose.pose.orientation.w = q[3] self.odom_filtered.twist.twist.linear.x = x[2] - self.odom_filtered.twist.twist.linear.y = self.odom_noised.linear.y - self.odom_filtered.twist.twist.linear.z = self.odom_noised.linear.z - self.odom_filtered.twist.twist.angular.x = self.odom_noised.angular.x - self.odom_filtered.twist.twist.angular.y = self.odom_noised.angular.y + if self.z_accel is not None: + self.odom_filtered.twist.twist.linear.y = self.odom_noised.linear.y + self.odom_filtered.twist.twist.linear.z = self.odom_noised.linear.z + self.odom_filtered.twist.twist.angular.x = self.odom_noised.angular.x + self.odom_filtered.twist.twist.angular.y = self.odom_noised.angular.y + else: + self.odom_filtered.twist.twist.linear.y = self.odom_noised.twist.twist.linear.y + self.odom_filtered.twist.twist.linear.z = self.odom_noised.twist.twist.linear.z + self.odom_filtered.twist.twist.angular.x = self.odom_noised.twist.twist.angular.x + self.odom_filtered.twist.twist.angular.y = self.odom_noised.twist.twist.angular.y self.odom_filtered.twist.twist.angular.z = x[4] # Fill the odometry message covariance matrix with computed KF covariance From 697cc55bdeb91ddff2e50fa1343e1f78c31bdf48 Mon Sep 17 00:00:00 2001 From: Edd Date: Fri, 3 Dec 2021 16:41:16 +0300 Subject: [PATCH 2/2] state estimation refactored --- .../state_estimation_2d/launch/rviz.launch.py | 34 ++++ .../state_estimation_metalbot.launch.py | 15 +- .../launch/state_estimation_sim.launch.py | 157 ------------------ .../state_estimation_2d/setup.py | 1 + .../state_estimation_2d/filter.py | 6 +- .../state_estimation_2d/measurement.py | 6 +- .../state_estimation_2d/model.py | 10 +- .../state_estimation_node.py | 29 +++- 8 files changed, 83 insertions(+), 175 deletions(-) create mode 100644 ros2_ws/src/algorithms/state_estimation/state_estimation_2d/launch/rviz.launch.py delete mode 100644 ros2_ws/src/algorithms/state_estimation/state_estimation_2d/launch/state_estimation_sim.launch.py diff --git a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/launch/rviz.launch.py b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/launch/rviz.launch.py new file mode 100644 index 0000000..3edff03 --- /dev/null +++ b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/launch/rviz.launch.py @@ -0,0 +1,34 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +import launch +import launch_ros +from ament_index_python.packages import get_package_share_directory + +import os +from ament_index_python.packages import get_package_share_directory +import launch +import launch_ros.actions +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +state_estimation_path = get_package_share_directory('state_estimation_2d') +rviz_path = os.path.join(state_estimation_path, 'rviz', 'default.rviz') + +def generate_launch_description(): + return LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='use_sim_time', + default_value='true' + ), + Node( + package="rviz2", + executable="rviz2", + output='screen', + arguments=['-d'+str(rviz_path)] + ), + ]) \ No newline at end of file diff --git a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/launch/state_estimation_metalbot.launch.py b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/launch/state_estimation_metalbot.launch.py index 687d77a..f0dd695 100644 --- a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/launch/state_estimation_metalbot.launch.py +++ b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/launch/state_estimation_metalbot.launch.py @@ -19,19 +19,26 @@ enable_viz = 'true' def generate_launch_description(): + use_sim_time = LaunchConfiguration('use_sim_time', default='true') + return LaunchDescription([ launch.actions.DeclareLaunchArgument( 'viz', default_value=enable_viz, description='enable_visualization' ), + launch.actions.DeclareLaunchArgument( + name='use_sim_time', + default_value='true' + ), Node( package="state_estimation_2d", executable="state_estimation_2d", output='screen', emulate_tty=True, parameters=[ - {"use_sim_time": False}, + {"use_sim_time": use_sim_time}, + {"use_nn_model": False}, {"odom_topic": "velocity"}, {"imu_topic": "imu"}, {"imu_accel_topic": "/camera/accel/sample"}, @@ -51,6 +58,12 @@ def generate_launch_description(): {"accel_robot_covariance": 200.0}, ], ), + Node( + package="odom_noiser", + executable="odom_noiser", + output='screen', + emulate_tty=True + ), Node( package="path_visualizer", executable="path_visualizer", diff --git a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/launch/state_estimation_sim.launch.py b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/launch/state_estimation_sim.launch.py deleted file mode 100644 index 4159b71..0000000 --- a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/launch/state_estimation_sim.launch.py +++ /dev/null @@ -1,157 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node -import launch -import launch_ros -from ament_index_python.packages import get_package_share_directory - -import os -from ament_index_python.packages import get_package_share_directory -import launch -import launch_ros.actions -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.actions import IncludeLaunchDescription -from launch.conditions import IfCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - -def generate_launch_description(): -# ROSBOT - world_file_name = 'willow_garage.world' - - urdf_file_name = 'urdf/rosbot.urdf' - urdf = os.path.join( - get_package_share_directory('rosbot_description'), - urdf_file_name) - with open(urdf, 'r') as infp: - robot_desc = infp.read() - - gazebo_ros = get_package_share_directory('gazebo_ros') - rosbot_description = get_package_share_directory('rosbot_description') - - gazebo_client = launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(gazebo_ros, 'launch', 'gzclient.launch.py')), - condition=launch.conditions.IfCondition(launch.substitutions.LaunchConfiguration('gui')) - ) - - gazebo_server = launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(gazebo_ros, 'launch', 'gzserver.launch.py')) - ) - - spawn_rosbot = launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - os.path.join(rosbot_description, 'launch', 'rosbot_spawn.launch.py')) - ) - - rviz_path = "/home/user/ros2_ws/src/state_estimation_2d/rviz/default.rviz" - -#Teleop - default_update_rate = '20' - update_rate = launch.substitutions.LaunchConfiguration( - 'update_rate', - default=default_update_rate - ) - - keyboard_listener_dir = get_package_share_directory('teleop') - keyboard_listener_launch = launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - keyboard_listener_dir + '/keyboard_listener.launch.py' - ), - launch_arguments = {'output_path': update_rate}.items() - ) - - return LaunchDescription([ - - DeclareLaunchArgument( - 'world', - default_value=[os.path.join(rosbot_description, 'worlds', world_file_name), ''], - description='SDF world file'), - DeclareLaunchArgument( - name='gui', - default_value='true' - ), - DeclareLaunchArgument( - name='use_sim_time', - default_value='true' - ), - DeclareLaunchArgument('verbose', default_value='true', - description='Set "true" to increase messages written to terminal.'), - DeclareLaunchArgument('gdb', default_value='false', - description='Set "true" to run gzserver with gdb'), - DeclareLaunchArgument('state', default_value='true', - description='Set "false" not to load "libgazebo_ros_state.so"'), - - launch.actions.DeclareLaunchArgument( - 'update_rate', - default_value=default_update_rate, - description='update rate' - ), - - Node( - package="rviz2", - executable="rviz2", - output='screen', - arguments=['-d'+str(rviz_path)] - ), - Node( - package="odom_noiser", - executable="odom_noiser", - output='screen', - emulate_tty=True - ), - Node( - package="state_estimation_2d", - executable="state_estimation_2d", - output='screen', - emulate_tty=True, - parameters=[{"use_sim_time": False}], - ), - Node( - package="path_visualizer", - executable="path_visualizer" - ), - Node( - package="metric_calculator", - executable="metric_calculator", - output='screen', - emulate_tty=True - ), - # launh Teleop node - Node( - package='rosbot_controller', - executable='rosbot_teleop', - name='rosbot_teleop', - output='screen', - emulate_tty=True, - parameters=[ - {"update_rate": update_rate}, - {'keyboard_topic': "/keyboard"}, - {'control_topic': "/cmd_vel"}, - {'joystick_topic': "/joy"}, - {'movable_camera': "False"}, - {'v_limit': "0.5"}, - {'w_limit': "2.5"}, - {'lin_a': "0.1"}, - {'ang_a': "0.25"}, - ] - ), - Node( - package="robot_localization", - executable='ekf_node', - name='ekf_filter_node', - output='screen', - emulate_tty=True, - parameters=[{os.path.join(get_package_share_directory("robot_localization"), 'params', 'ekf.yaml')}, - {"use_sim_time": True}], - ), - - # launh listener node - gazebo_server, - gazebo_client, - spawn_rosbot, - keyboard_listener_launch - - ]) \ No newline at end of file diff --git a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/setup.py b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/setup.py index 35436b7..efaf3e6 100644 --- a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/setup.py +++ b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/setup.py @@ -13,6 +13,7 @@ ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + (os.path.join('share', package_name, 'rviz'), glob('rviz/*.rviz')) ], install_requires=['setuptools'], zip_safe=True, diff --git a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/filter.py b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/filter.py index 2b99ecf..7f0c42d 100644 --- a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/filter.py +++ b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/filter.py @@ -145,7 +145,7 @@ def update_odom(self, z_odom, R_odom): def update_imu(self, z_imu, R_imu): """ Update state vector using imu measurements""" H = get_jacobian_imu(self.x_opt) - y = z_imu - imu(self.x_opt) + y = z_imu - h_imu(self.x_opt) self.x_opt, self.P_opt = kalman_update( self.x_opt, self.P_opt, @@ -157,7 +157,7 @@ def update_imu(self, z_imu, R_imu): def update_imu_accel(self, z_accel, R_accel): """ Update state vector using imu measurements""" H = get_jacobian_accel(self.x_opt) - y = z_accel - accel(self.x_opt) + y = z_accel - h_accel(self.x_opt) self.x_opt, self.P_opt = kalman_update( self.x_opt, self.P_opt, @@ -169,7 +169,7 @@ def update_imu_accel(self, z_accel, R_accel): def update_imu_gyro(self, z_gyro, R_gyro): """ Update state vector using imu measurements""" H = get_jacobian_gyro(self.x_opt) - y = z_gyro - gyro(self.x_opt) + y = z_gyro - h_gyro(self.x_opt) self.x_opt, self.P_opt = kalman_update( self.x_opt, self.P_opt, diff --git a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/measurement.py b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/measurement.py index 5515f88..d816b7f 100644 --- a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/measurement.py +++ b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/measurement.py @@ -32,20 +32,20 @@ def get_jacobian_gyro(x): H[0, 4] = 1 return H -def imu(x): +def h_imu(x): """Imu measurement function""" z = np.zeros(2) z[0] = x[2] * x[4] z[1] = x[4] return z -def accel(x): +def h_accel(x): """Imu measurement function""" z = np.zeros(1) z[0] = x[2] * x[4] return z -def gyro(x): +def h_gyro(x): """Imu measurement function""" z = np.zeros(1) z[0] = x[4] diff --git a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/model.py b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/model.py index 13fc98f..5403a9a 100644 --- a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/model.py +++ b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/model.py @@ -1,9 +1,15 @@ import numpy as np -"""Model dynamics functions""" def transform_jacobian(x, dt): - """Dynamic model function jacobian""" + """Dynamic model function jacobian + State vector x: + 0. x + 1. y + 2. V_parallel (velocity along the robot direction) + 3. yaw + 4. yaw_vel + """ J_f = np.eye(5) J_f[0, 2] = np.cos(x[3]) * dt J_f[0, 3] = -x[2] * np.sin(x[3]) * dt diff --git a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/state_estimation_node.py b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/state_estimation_node.py index d9c92d1..193caa3 100644 --- a/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/state_estimation_node.py +++ b/ros2_ws/src/algorithms/state_estimation/state_estimation_2d/state_estimation_2d/state_estimation_node.py @@ -84,6 +84,7 @@ def __init__(self): # ROS Subscribers self.init_parameters() self.update_parameters() + self.odom_sub = self.create_subscription( Twist, self.odom_topic, @@ -114,10 +115,12 @@ def __init__(self): self.imu_topic, self.imu_callback, 15) + self.pose_pub = self.create_publisher(Odometry, self.publish_topic, 10) self.tf2_broadcaster = tf2_ros.TransformBroadcaster(self) self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) + # ROS variables self.odom_noised = Odometry() self.odom_gt = Odometry() @@ -130,10 +133,12 @@ def __init__(self): self.z_gyro = None self.imu_extrinsic = None self.rot_extrinsic = None + # Upload NN control model - self.model = nnio.ONNXModel(self.model_path) + if self.use_nn_model: + self.model = nnio.ONNXModel(self.model_path) + # Filter parameters - self.filter = Filter2D( x_init=np.zeros(5), P_init=np.eye(5) * 0.01, @@ -141,11 +146,13 @@ def __init__(self): v_var=self.R_odom[0][0]**2, w_var=self.R_odom[1][1]**2, ) + self.distance = 0 self.x_prev = 0 self.y_prev = 0 self.odom_gt_prev = Odometry() self.ate = ErrorEstimator() + # Timer for update function self.predict_timer = self.create_timer( self.dt, @@ -156,6 +163,7 @@ def init_parameters(self): self.declare_parameters( namespace="", parameters=[ + ("use_nn_model", False), ("odom_topic", "odom"), ("imu_topic", "imu"), ("imu_accel_topic", "/camera/accel/sample"), @@ -178,6 +186,7 @@ def init_parameters(self): ) def update_parameters(self): + self.use_nn_model = self.get_parameter('use_nn_model').get_parameter_value().bool_value self.odom_topic = self.get_parameter('odom_topic').get_parameter_value().string_value self.imu_topic = self.get_parameter('imu_topic').get_parameter_value().string_value self.imu_accel_topic = self.get_parameter('imu_accel_topic').get_parameter_value().string_value @@ -194,6 +203,7 @@ def update_parameters(self): R_imu_vec = self.get_parameter('imu_sim_covariance').get_parameter_value().double_array_value self.R_accel = np.array(self.get_parameter('accel_robot_covariance').get_parameter_value().double_value) self.R_gyro = np.array(self.get_parameter('gyro_robot_covariance').get_parameter_value().double_value) + self.R_odom = np.array(R_odom_vec).reshape(len(R_odom_vec)//2, len(R_odom_vec)//2) self.R_imu = np.array(R_imu_vec).reshape(len(R_imu_vec)//2, len(R_imu_vec)//2) @@ -244,7 +254,6 @@ def imu_accel_callback(self, msg): self.z_accel = accel[1] def imu_gyro_callback(self, msg): - # print(self.imu_extrinsic) if self.imu_extrinsic is None: print("Finding imu extrinsics") self.imu_extrinsic = self.get_extrinsic(self.imu_frame, self.robot_base_frame) @@ -280,8 +289,10 @@ def step_filter(self): """ if self.got_measurements: # Predict step - # self.filter.predict_by_nn_model(self.model, self.control) - self.filter.predict_by_naive_model(self.control) + if self.use_nn_model: + self.filter.predict_by_nn_model(self.model, self.control) + else: + self.filter.predict_by_naive_model(self.control) # Measurement update step self.filter.update_odom(self.z_odom, self.R_odom) if self.z_imu is not None: @@ -387,8 +398,8 @@ def pose_covariance_to_vector(self, P): Transfer filter computed pose covariance matrix to vector http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseWithCovariance.html @ parameters - P: np.array - Covariance matrix + P: np.array of shape (dim(x), dim(x)) + Covariance matrix for corresponding variables of state vector """ cov_vector = self.odom_noised.pose.covariance cov_vector[14] = 0.1 @@ -410,8 +421,8 @@ def twist_covariance_to_vector(self, P): Transfer filter computed twist covariance matrix to vector http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/TwistWithCovariance.html @ parameters - P: np.array - Covariance matrix + P: np.array of shape (dim(x), dim(x)) + Covariance matrix for corresponding variables of state vector """ cov_vector = self.odom_noised.twist.covariance cov_vector[0] = P[2,2]