From 7f4dc71a1d53d25c9e43f1db964665de5b5f8621 Mon Sep 17 00:00:00 2001 From: Stefan Caldararu Date: Fri, 19 Jan 2024 12:20:03 -0600 Subject: [PATCH] add ekf_vel_only argument to ekf launch --- .../launch/ekf_estimation.launch.py | 2 ++ .../ekf_estimation/ekf_estimation/ekf_estimation.py | 12 ++++++++++-- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/workspace/src/common/launch/art_localization_launch/launch/ekf_estimation.launch.py b/workspace/src/common/launch/art_localization_launch/launch/ekf_estimation.launch.py index e0e382ff..b847553f 100644 --- a/workspace/src/common/launch/art_localization_launch/launch/ekf_estimation.launch.py +++ b/workspace/src/common/launch/art_localization_launch/launch/ekf_estimation.launch.py @@ -59,6 +59,7 @@ def generate_launch_description(): AddLaunchArgument( ld, "art_localization/output/filtered_state", "/vehicle/filtered_state" ) + AddLaunchArgument(ld, "ekf_vel_only", "False") # ----- # Nodes @@ -97,6 +98,7 @@ def generate_launch_description(): ), GetPackageSharePath("art_localization_launch", "config", "EKF_param.yml"), {"use_sim_time": GetLaunchArgument("use_sim_time")}, + {"ekf_vel_only": GetLaunchArgument("ekf_vel_only")}, ], ) ld.add_action(node) diff --git a/workspace/src/localization/ekf_estimation/ekf_estimation/ekf_estimation.py b/workspace/src/localization/ekf_estimation/ekf_estimation/ekf_estimation.py index 7586d4f5..08a54002 100644 --- a/workspace/src/localization/ekf_estimation/ekf_estimation/ekf_estimation.py +++ b/workspace/src/localization/ekf_estimation/ekf_estimation/ekf_estimation.py @@ -45,6 +45,10 @@ def __init__(self): self.use_sim_msg = ( self.get_parameter("use_sim_time").get_parameter_value().bool_value ) + self.declare_parameter("ekf_vel_only", False) + self.ekf_vel_only = ( + self.get_parameter("ekf_vel_only").get_parameter_value().bool_value + ) # EKF parameters self.declare_parameter("Q1", 0.1) @@ -237,8 +241,12 @@ def pub_callback(self): msg = VehicleState() # pos and velocity are in meters, from the origin, [x, y, z] - msg.pose.position.x = float(self.state[0, 0]) - msg.pose.position.y = float(self.state[1, 0]) + if self.ekf_vel_only: + msg.pose.position.x = float(self.x) + msg.pose.position.y = float(self.y) + else: + msg.pose.position.x = float(self.state[0, 0]) + msg.pose.position.y = float(self.state[1, 0]) msg.pose.orientation.z = float(self.state[2, 0]) msg.twist.linear.x = float(self.state[3, 0] * math.cos(self.state[2, 0])) msg.twist.linear.y = float(self.state[3, 0] * math.sin(self.state[2, 0]))