From 6fbb8274bef7b761b33f8cc6782a15500314730b Mon Sep 17 00:00:00 2001 From: kengohozumi Date: Fri, 11 Oct 2024 02:09:44 +0000 Subject: [PATCH 1/6] modify ekf_myself --- orange_bringup/launch/orange_robot.launch.xml | 6 +- orange_bringup/orange_bringup/ekf_myself.py | 64 ++++++++++-------- .../orange_bringup/ekf_myself_noGPS.py | 66 ++++++++++--------- 3 files changed, 76 insertions(+), 60 deletions(-) diff --git a/orange_bringup/launch/orange_robot.launch.xml b/orange_bringup/launch/orange_robot.launch.xml index a46f029..cdbcd3b 100644 --- a/orange_bringup/launch/orange_robot.launch.xml +++ b/orange_bringup/launch/orange_robot.launch.xml @@ -42,7 +42,7 @@ --> - + @@ -56,8 +56,12 @@ + + + diff --git a/orange_bringup/orange_bringup/ekf_myself.py b/orange_bringup/orange_bringup/ekf_myself.py index 59f6faf..7995b1b 100644 --- a/orange_bringup/orange_bringup/ekf_myself.py +++ b/orange_bringup/orange_bringup/ekf_myself.py @@ -242,34 +242,40 @@ def combine_yaw(self, Dtheta, theta1, theta2, w1, w2): return theta_sum def calculate_offset(self, combyaw, GTheta, GPStheta): - deference = abs(GTheta) + abs(GPStheta) - - if GTheta > 0 and GPStheta < 0 and combyaw > 0: - self.GOffset = -(GTheta - combyaw) - elif GTheta > 0 and GPStheta < 0 and combyaw < 0: - self.GOffset = -(GTheta + abs(combyaw)) - elif GTheta > 0 and GPStheta > 0 and combyaw > 0 and GTheta > GPStheta: - self.GOffset = -(abs(combyaw) - abs(GTheta)) - elif GTheta < 0 and GPStheta < 0 and combyaw < 0 and GTheta > GPStheta: - self.GOffset = -(GTheta + abs(combyaw)) - elif GTheta < 0 and GPStheta > 0 and combyaw > 0: - self.GOffset = abs(GTheta) + combyaw - elif GTheta < 0 and GPStheta > 0 and combyaw < 0: - self.GOffset = abs(GTheta) - abs(combyaw) - elif GTheta > 0 and GPStheta > 0 and combyaw > 0 and GTheta < GPStheta: - self.GOffset = combyaw - GTheta - elif GTheta < 0 and GPStheta < 0 and combyaw < 0 and GTheta < GPStheta: - self.GOffset = abs(GTheta) - abs(combyaw) - elif GTheta > 0 and GPStheta < 0 and combyaw > 0 and deference > math.pi: - self.GOffset = combyaw - GTheta - elif GTheta > 0 and GPStheta < 0 and combyaw < 0 and deference > math.pi: - self.GOffset = math.pi - GTheta + math.pi - abs(combyaw) - elif GTheta < 0 and GPStheta > 0 and combyaw > 0 and deference > math.pi: - self.GOffset = -((math.pi - combyaw) + (math.pi - abs(GTheta))) - elif GTheta < 0 and GPStheta > 0 and combyaw < 0 and deference > math.pi: - self.GOffset = -(abs(combyaw) - abs(GTheta)) - - if abs(self.GOffset) > 5 * math.pi / 180: # not -0.0872 ~ 0.0872 + abs_GTheta = abs(GTheta) + abs_GPStheta = abs(GPStheta) + abs_combyaw = abs(combyaw) + pi = math.pi + + deference = abs_GTheta + abs_GPStheta + + match (GTheta > 0, GPStheta > 0, combyaw > 0, GTheta > GPStheta, deference > pi): + case (True, False, True, _, _): + self.GOffset = -(GTheta - combyaw) + case (True, False, False, _, _): + self.GOffset = -(GTheta + abs_combyaw) + case (True, True, True, True, _): + self.GOffset = -(abs_combyaw - abs_GTheta) + case (False, False, False, True, _): + self.GOffset = -(GTheta + abs_combyaw) + case (False, True, True, _, _): + self.GOffset = abs_GTheta + combyaw + case (False, True, False, _, _): + self.GOffset = abs_GTheta - abs_combyaw + case (True, True, True, False, _): + self.GOffset = combyaw - GTheta + case (False, False, False, False, _): + self.GOffset = abs_GTheta - abs_combyaw + case (True, False, True, _, True): + self.GOffset = combyaw - GTheta + case (True, False, False, _, True): + self.GOffset = pi - GTheta + pi - abs_combyaw + case (False, True, True, _, True): + self.GOffset = -((pi - combyaw) + (pi - abs_GTheta)) + case (False, True, False, _, True): + self.GOffset = -(abs_combyaw - abs_GTheta) + + if abs(self.GOffset) > 5 * pi / 180: # not -0.0872 ~ 0.0872 self.GOffset = 0 self.get_logger().warn("GOffset warning") @@ -287,7 +293,7 @@ def publish_fused_value(self): fused_value = self.KalfGPSXY( self.Speed, self.SmpTime, self.GTheta, self.GpsXY, self.R1, self.R2) self.GPS_conut += 1 - if self.GPS_conut % 10 == 0: + if self.GPS_conut % 20 == 0: self.combyaw = self.combine_yaw( self.DGPStheta, self.GTheta, self.GPStheta, self.R3, self.R4) self.offsetyaw = self.calculate_offset( diff --git a/orange_bringup/orange_bringup/ekf_myself_noGPS.py b/orange_bringup/orange_bringup/ekf_myself_noGPS.py index b8d32b5..1e123e1 100755 --- a/orange_bringup/orange_bringup/ekf_myself_noGPS.py +++ b/orange_bringup/orange_bringup/ekf_myself_noGPS.py @@ -216,35 +216,41 @@ def combine_yaw(self, Dtheta, theta1, theta2, w1, w2): return theta_sum - def calculate_offset(self, combyaw, GTheta, odom2theta): - deference = abs(GTheta) + abs(odom2theta) - - if GTheta > 0 and odom2theta < 0 and combyaw > 0: - self.GOffset = -(GTheta - combyaw) - elif GTheta > 0 and odom2theta < 0 and combyaw < 0: - self.GOffset = -(GTheta + abs(combyaw)) - elif GTheta > 0 and odom2theta > 0 and combyaw > 0 and GTheta > odom2theta: - self.GOffset = -(abs(combyaw) - abs(GTheta)) - elif GTheta < 0 and odom2theta < 0 and combyaw < 0 and GTheta > odom2theta: - self.GOffset = -(GTheta + abs(combyaw)) - elif GTheta < 0 and odom2theta > 0 and combyaw > 0: - self.GOffset = abs(GTheta) + combyaw - elif GTheta < 0 and odom2theta > 0 and combyaw < 0: - self.GOffset = abs(GTheta) - abs(combyaw) - elif GTheta > 0 and odom2theta > 0 and combyaw > 0 and GTheta < odom2theta: - self.GOffset = combyaw - GTheta - elif GTheta < 0 and odom2theta < 0 and combyaw < 0 and GTheta < odom2theta: - self.GOffset = abs(GTheta) - abs(combyaw) - elif GTheta > 0 and odom2theta < 0 and combyaw > 0 and deference > math.pi: - self.GOffset = combyaw - GTheta - elif GTheta > 0 and odom2theta < 0 and combyaw < 0 and deference > math.pi: - self.GOffset = math.pi - GTheta + math.pi - abs(combyaw) - elif GTheta < 0 and odom2theta > 0 and combyaw > 0 and deference > math.pi: - self.GOffset = -((math.pi - combyaw) + (math.pi - abs(GTheta))) - elif GTheta < 0 and odom2theta > 0 and combyaw < 0 and deference > math.pi: - self.GOffset = -(abs(combyaw) - abs(GTheta)) - - if abs(self.GOffset) > 5 * math.pi / 180: # not -0.0872 ~ 0.0872 + def calculate_offset(self, combyaw, GTheta, GPStheta): + abs_GTheta = abs(GTheta) + abs_GPStheta = abs(GPStheta) + abs_combyaw = abs(combyaw) + pi = math.pi + + deference = abs_GTheta + abs_GPStheta + + match (GTheta > 0, GPStheta > 0, combyaw > 0, GTheta > GPStheta, deference > pi): + case (True, False, True, _, _): + self.GOffset = -(GTheta - combyaw) + case (True, False, False, _, _): + self.GOffset = -(GTheta + abs_combyaw) + case (True, True, True, True, _): + self.GOffset = -(abs_combyaw - abs_GTheta) + case (False, False, False, True, _): + self.GOffset = -(GTheta + abs_combyaw) + case (False, True, True, _, _): + self.GOffset = abs_GTheta + combyaw + case (False, True, False, _, _): + self.GOffset = abs_GTheta - abs_combyaw + case (True, True, True, False, _): + self.GOffset = combyaw - GTheta + case (False, False, False, False, _): + self.GOffset = abs_GTheta - abs_combyaw + case (True, False, True, _, True): + self.GOffset = combyaw - GTheta + case (True, False, False, _, True): + self.GOffset = pi - GTheta + pi - abs_combyaw + case (False, True, True, _, True): + self.GOffset = -((pi - combyaw) + (pi - abs_GTheta)) + case (False, True, False, _, True): + self.GOffset = -(abs_combyaw - abs_GTheta) + + if abs(self.GOffset) > 5 * pi / 180: # not -0.0872 ~ 0.0872 self.GOffset = 0 self.get_logger().warn("GOffset warning") @@ -256,7 +262,7 @@ def publish_fused_value(self): fused_value = self.Kalfodom2XY( self.Speed, self.SmpTime, self.GTheta, self.odom2XY, self.R1, self.R2) self.conut += 1 - if self.conut % 10 == 0: + if self.conut % 20 == 0: self.combyaw = self.combine_yaw( self.DTheta, self.GTheta, self.odom2theta, self.R3, self.R4) self.offsetyaw = self.calculate_offset( From 43ab8d4bbcdb78a474c2b75c4fa6653c99c0b63e Mon Sep 17 00:00:00 2001 From: kengohozumi Date: Fri, 11 Oct 2024 02:24:06 +0000 Subject: [PATCH 2/6] fix orange_robot launch --- orange_bringup/launch/orange_robot.launch.xml | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/orange_bringup/launch/orange_robot.launch.xml b/orange_bringup/launch/orange_robot.launch.xml index cdbcd3b..61048fa 100644 --- a/orange_bringup/launch/orange_robot.launch.xml +++ b/orange_bringup/launch/orange_robot.launch.xml @@ -43,11 +43,13 @@ --> - + - + + + From 079ca354a7e5f8c0f3553c4a6a72703564c206a3 Mon Sep 17 00:00:00 2001 From: kengohozumi Date: Mon, 14 Oct 2024 07:55:38 +0000 Subject: [PATCH 3/6] fix stationary state in ekfmyselfpy --- orange_bringup/launch/orange_robot.launch.xml | 2 + orange_bringup/orange_bringup/ekf_myself.py | 105 ++++++++++++------ .../orange_bringup/ekf_myself_noGPS.py | 53 ++++----- 3 files changed, 99 insertions(+), 61 deletions(-) diff --git a/orange_bringup/launch/orange_robot.launch.xml b/orange_bringup/launch/orange_robot.launch.xml index 61048fa..64c3a76 100644 --- a/orange_bringup/launch/orange_robot.launch.xml +++ b/orange_bringup/launch/orange_robot.launch.xml @@ -70,6 +70,8 @@ + + diff --git a/orange_bringup/orange_bringup/ekf_myself.py b/orange_bringup/orange_bringup/ekf_myself.py index 7995b1b..b7ce6ac 100644 --- a/orange_bringup/orange_bringup/ekf_myself.py +++ b/orange_bringup/orange_bringup/ekf_myself.py @@ -17,6 +17,7 @@ def __init__(self): self.GTheta = None self.GTheta0 = None + self.GOffset = 0 self.GPSthetayaw0 = 0 self.DGPStheta = 0 self.w = None @@ -31,11 +32,11 @@ def __init__(self): self.XX = None self.prev_time = None self.prev_pos = None + self.prev_GPSpos = None self.Speed = 0 self.SmpTime = 0.1 self.GpsXY = None self.GPS_conut = 0 - self.GOffset = 0 self.offsetyaw = 0 self.combineyaw = 0 self.robot_yaw = 0 @@ -43,6 +44,7 @@ def __init__(self): self.robot_orientationz = 0 self.robot_orientationw = 0 self.Number_of_satellites = 0 + self.is_static = False self.sub_a = self.create_subscription( Odometry, '/odom_fast', self.sensor_a_callback, 10) @@ -52,7 +54,14 @@ def __init__(self): self.declare_parameter("publish_TF", False) self.ekf_publish_TF = self.get_parameter( "publish_TF").get_parameter_value().bool_value - + + self.declare_parameter("speed_limit", 3.5) + self.speed_limit = self.get_parameter( + "speed_limit").get_parameter_value().double_value + self.declare_parameter("speed_stop", 0.15) + self.speed_stop = self.get_parameter( + "speed_stop").get_parameter_value().double_value + self.t = TransformStamped() self.br = tf2_ros.TransformBroadcaster(self) @@ -80,34 +89,59 @@ def sensor_a_callback(self, data): self.SmpTime = current_time - self.prev_time else: self.SmpTime = 0.1 + self.prev_time = current_time current_pos = np.array([ data.pose.pose.position.x, data.pose.pose.position.y ]) + if self.prev_pos is not None: distance = np.linalg.norm(current_pos - self.prev_pos) self.Speed = distance / self.SmpTime else: self.Speed = 0 + + if self.Speed > self.speed_limit: + self.Speed = 0 + #self.get_logger().info("Speed correction") + + #self.get_logger().info(f"Speed: {self.Speed}") + + if self.Speed < self.speed_stop:#0.15 + self.is_static = True + else: + self.is_static = False + self.prev_pos = current_pos - + self.GTheta = self.orientation_to_yaw( data.pose.pose.orientation.z, data.pose.pose.orientation.w) def sensor_b_callback(self, data): - self.GpsXY = np.array( - [data.pose.pose.position.x, data.pose.pose.position.y]) - - self.GPStheta = self.orientation_to_yaw( - data.pose.pose.orientation.z, data.pose.pose.orientation.w) - + self.GpsXY = np.array([data.pose.pose.position.x, data.pose.pose.position.y]) + #self.get_logger().info(f"self.GpsXY : {self.GpsXY}") + + if self.is_static: # stationary state(true) + self.get_logger().info("stationary state") + if self.prev_GPSpos is None: + self.GpsXY = np.array([0, 0]) + #self.get_logger().info("non self.prev_GPSpos") + else: + self.GpsXY = self.prev_GPSpos + else: + self.prev_GPSpos = self.GpsXY + + self.GPStheta = self.orientation_to_yaw(data.pose.pose.orientation.z, data.pose.pose.orientation.w) + self.DGPStheta = self.GPStheta - self.GPSthetayaw0 self.GPSthetayaw0 = self.GPStheta self.Number_of_satellites = data.pose.covariance[0] # + + #self.get_logger().info(f"self.Number_of_satellites: {self.Number_of_satellites}") def determination_of_R(self): if 0 <= self.Number_of_satellites < 4: # Bad @@ -249,36 +283,37 @@ def calculate_offset(self, combyaw, GTheta, GPStheta): deference = abs_GTheta + abs_GPStheta - match (GTheta > 0, GPStheta > 0, combyaw > 0, GTheta > GPStheta, deference > pi): - case (True, False, True, _, _): - self.GOffset = -(GTheta - combyaw) - case (True, False, False, _, _): - self.GOffset = -(GTheta + abs_combyaw) - case (True, True, True, True, _): - self.GOffset = -(abs_combyaw - abs_GTheta) - case (False, False, False, True, _): - self.GOffset = -(GTheta + abs_combyaw) - case (False, True, True, _, _): - self.GOffset = abs_GTheta + combyaw - case (False, True, False, _, _): - self.GOffset = abs_GTheta - abs_combyaw - case (True, True, True, False, _): - self.GOffset = combyaw - GTheta - case (False, False, False, False, _): - self.GOffset = abs_GTheta - abs_combyaw - case (True, False, True, _, True): - self.GOffset = combyaw - GTheta - case (True, False, False, _, True): - self.GOffset = pi - GTheta + pi - abs_combyaw - case (False, True, True, _, True): - self.GOffset = -((pi - combyaw) + (pi - abs_GTheta)) - case (False, True, False, _, True): - self.GOffset = -(abs_combyaw - abs_GTheta) + if GTheta > 0 and GPStheta < 0 and combyaw > 0: + self.GOffset = -(GTheta - combyaw) + elif GTheta > 0 and GPStheta < 0 and combyaw < 0: + self.GOffset = -(GTheta + abs_combyaw) + elif GTheta > 0 and GPStheta > 0 and combyaw > 0 and GTheta > GPStheta: + self.GOffset = -(abs_combyaw - abs_GTheta) + elif GTheta < 0 and GPStheta < 0 and combyaw < 0 and GTheta > GPStheta: + self.GOffset = -(GTheta + abs_combyaw) + elif GTheta < 0 and GPStheta > 0 and combyaw > 0: + self.GOffset = abs_GTheta + combyaw + elif GTheta < 0 and GPStheta > 0 and combyaw < 0: + self.GOffset = abs_GTheta - abs_combyaw + elif GTheta > 0 and GPStheta > 0 and combyaw > 0 and GTheta < GPStheta: + self.GOffset = combyaw - GTheta + elif GTheta < 0 and GPStheta < 0 and combyaw < 0 and GTheta < GPStheta: + self.GOffset = abs_GTheta - abs_combyaw + elif GTheta > 0 and GPStheta < 0 and combyaw > 0 and deference > pi: + self.GOffset = combyaw - GTheta + elif GTheta > 0 and GPStheta < 0 and combyaw < 0 and deference > pi: + self.GOffset = pi - GTheta + pi - abs_combyaw + elif GTheta < 0 and GPStheta > 0 and combyaw > 0 and deference > pi: + self.GOffset = -((pi - combyaw) + (pi - abs_GTheta)) + elif GTheta < 0 and GPStheta > 0 and combyaw < 0 and deference > pi: + self.GOffset = -(abs_combyaw - abs_GTheta) if abs(self.GOffset) > 5 * pi / 180: # not -0.0872 ~ 0.0872 self.GOffset = 0 self.get_logger().warn("GOffset warning") - + + #self.get_logger().info(f"GOffset: {self.GOffset}") ok + return self.GOffset def publish_fused_value(self): diff --git a/orange_bringup/orange_bringup/ekf_myself_noGPS.py b/orange_bringup/orange_bringup/ekf_myself_noGPS.py index 1e123e1..7e1a22d 100755 --- a/orange_bringup/orange_bringup/ekf_myself_noGPS.py +++ b/orange_bringup/orange_bringup/ekf_myself_noGPS.py @@ -224,36 +224,37 @@ def calculate_offset(self, combyaw, GTheta, GPStheta): deference = abs_GTheta + abs_GPStheta - match (GTheta > 0, GPStheta > 0, combyaw > 0, GTheta > GPStheta, deference > pi): - case (True, False, True, _, _): - self.GOffset = -(GTheta - combyaw) - case (True, False, False, _, _): - self.GOffset = -(GTheta + abs_combyaw) - case (True, True, True, True, _): - self.GOffset = -(abs_combyaw - abs_GTheta) - case (False, False, False, True, _): - self.GOffset = -(GTheta + abs_combyaw) - case (False, True, True, _, _): - self.GOffset = abs_GTheta + combyaw - case (False, True, False, _, _): - self.GOffset = abs_GTheta - abs_combyaw - case (True, True, True, False, _): - self.GOffset = combyaw - GTheta - case (False, False, False, False, _): - self.GOffset = abs_GTheta - abs_combyaw - case (True, False, True, _, True): - self.GOffset = combyaw - GTheta - case (True, False, False, _, True): - self.GOffset = pi - GTheta + pi - abs_combyaw - case (False, True, True, _, True): - self.GOffset = -((pi - combyaw) + (pi - abs_GTheta)) - case (False, True, False, _, True): - self.GOffset = -(abs_combyaw - abs_GTheta) + if GTheta > 0 and GPStheta < 0 and combyaw > 0: + self.GOffset = -(GTheta - combyaw) + elif GTheta > 0 and GPStheta < 0 and combyaw < 0: + self.GOffset = -(GTheta + abs_combyaw) + elif GTheta > 0 and GPStheta > 0 and combyaw > 0 and GTheta > GPStheta: + self.GOffset = -(abs_combyaw - abs_GTheta) + elif GTheta < 0 and GPStheta < 0 and combyaw < 0 and GTheta > GPStheta: + self.GOffset = -(GTheta + abs_combyaw) + elif GTheta < 0 and GPStheta > 0 and combyaw > 0: + self.GOffset = abs_GTheta + combyaw + elif GTheta < 0 and GPStheta > 0 and combyaw < 0: + self.GOffset = abs_GTheta - abs_combyaw + elif GTheta > 0 and GPStheta > 0 and combyaw > 0 and GTheta < GPStheta: + self.GOffset = combyaw - GTheta + elif GTheta < 0 and GPStheta < 0 and combyaw < 0 and GTheta < GPStheta: + self.GOffset = abs_GTheta - abs_combyaw + elif GTheta > 0 and GPStheta < 0 and combyaw > 0 and deference > pi: + self.GOffset = combyaw - GTheta + elif GTheta > 0 and GPStheta < 0 and combyaw < 0 and deference > pi: + self.GOffset = pi - GTheta + pi - abs_combyaw + elif GTheta < 0 and GPStheta > 0 and combyaw > 0 and deference > pi: + self.GOffset = -((pi - combyaw) + (pi - abs_GTheta)) + elif GTheta < 0 and GPStheta > 0 and combyaw < 0 and deference > pi: + self.GOffset = -(abs_combyaw - abs_GTheta) if abs(self.GOffset) > 5 * pi / 180: # not -0.0872 ~ 0.0872 self.GOffset = 0 self.get_logger().warn("GOffset warning") - + + #self.get_logger().info(f"GOffset: {self.GOffset}") ok + return self.GOffset def publish_fused_value(self): From dbccb5270231d073cf34643b810767eb2aabadcd Mon Sep 17 00:00:00 2001 From: kengohozumi Date: Tue, 15 Oct 2024 03:37:23 +0000 Subject: [PATCH 4/6] fix GPStoodom --- orange_bringup/launch/orange_robot.launch.xml | 2 +- orange_bringup/orange_bringup/fix_to_GPSodom.py | 3 ++- orange_bringup/orange_bringup/lonlat_to_odom.py | 3 ++- 3 files changed, 5 insertions(+), 3 deletions(-) diff --git a/orange_bringup/launch/orange_robot.launch.xml b/orange_bringup/launch/orange_robot.launch.xml index 64c3a76..81e38fb 100644 --- a/orange_bringup/launch/orange_robot.launch.xml +++ b/orange_bringup/launch/orange_robot.launch.xml @@ -18,7 +18,7 @@ - + diff --git a/orange_bringup/orange_bringup/fix_to_GPSodom.py b/orange_bringup/orange_bringup/fix_to_GPSodom.py index e676e85..df211b0 100644 --- a/orange_bringup/orange_bringup/fix_to_GPSodom.py +++ b/orange_bringup/orange_bringup/fix_to_GPSodom.py @@ -153,7 +153,8 @@ def conversion(self, coordinate, origin, theta): r_theta = theta * degree_to_radian h_x = math.cos(r_theta) * gps_x - math.sin(r_theta) * gps_y h_y = math.sin(r_theta) * gps_x + math.cos(r_theta) * gps_y - point = (h_y, -h_x) + point = (-h_y, h_x) + #point = (h_y, -h_x) return point diff --git a/orange_bringup/orange_bringup/lonlat_to_odom.py b/orange_bringup/orange_bringup/lonlat_to_odom.py index 269c7e8..eea6034 100755 --- a/orange_bringup/orange_bringup/lonlat_to_odom.py +++ b/orange_bringup/orange_bringup/lonlat_to_odom.py @@ -108,7 +108,8 @@ def conversion(self, coordinate, origin, theta): r_theta = theta * degree_to_radian h_x = math.cos(r_theta) * gps_x - math.sin(r_theta) * gps_y h_y = math.sin(r_theta) * gps_x + math.cos(r_theta) * gps_y - point = (h_y, -h_x) + point = (-h_y, h_x) + #point = (h_y, -h_x) return point From a585e5843efbf2f27f3ef8f79c822ce511e08c2d Mon Sep 17 00:00:00 2001 From: kengohozumi Date: Tue, 15 Oct 2024 07:29:55 +0000 Subject: [PATCH 5/6] fix ekf_myself v1 --- orange_bringup/orange_bringup/ekf_myself.py | 50 ++++++++------------- 1 file changed, 19 insertions(+), 31 deletions(-) diff --git a/orange_bringup/orange_bringup/ekf_myself.py b/orange_bringup/orange_bringup/ekf_myself.py index b7ce6ac..686275e 100644 --- a/orange_bringup/orange_bringup/ekf_myself.py +++ b/orange_bringup/orange_bringup/ekf_myself.py @@ -144,13 +144,7 @@ def sensor_b_callback(self, data): #self.get_logger().info(f"self.Number_of_satellites: {self.Number_of_satellites}") def determination_of_R(self): - if 0 <= self.Number_of_satellites < 4: # Bad - self.R1 = 1e-2 # 0.01 FAST-LIO - self.R2 = 9e-2 # 0.09 CLAS-movingbase - self.R3 = 9 # GTheta - self.R4 = 1 # GPStheta - - elif 4 <= self.Number_of_satellites < 8: # So-so + if 4 <= self.Number_of_satellites < 8: # So-so... self.R1 = 6e-2 # 0.06 FAST-LIO self.R2 = 4e-2 # 0.04 CLAS-movingbase self.R3 = 4 # GTheta @@ -161,7 +155,7 @@ def determination_of_R(self): self.R2 = 1e-2 # 0.01 CLAS-movingbase self.R3 = 2 # GTheta self.R4 = 8 # GPStheta - + R = np.array([self.R1, self.R2, self.R3, self.R4]) return R @@ -318,21 +312,17 @@ def calculate_offset(self, combyaw, GTheta, GPStheta): def publish_fused_value(self): if self.Speed is not None and self.SmpTime is not None and self.GTheta is not None: - R = self.determination_of_R() - self.R1 = R[0] - self.R2 = R[1] - self.R3 = R[2] - self.R4 = R[3] - - if self.GpsXY is not None: - fused_value = self.KalfGPSXY( - self.Speed, self.SmpTime, self.GTheta, self.GpsXY, self.R1, self.R2) + if self.GpsXY is not None and self.Number_of_satellites >= 4: + R = self.determination_of_R() + self.R1 = R[0] + self.R2 = R[1] + self.R3 = R[2] + self.R4 = R[3] + fused_value = self.KalfGPSXY(self.Speed, self.SmpTime, self.GTheta, self.GpsXY, self.R1, self.R2) self.GPS_conut += 1 if self.GPS_conut % 20 == 0: - self.combyaw = self.combine_yaw( - self.DGPStheta, self.GTheta, self.GPStheta, self.R3, self.R4) - self.offsetyaw = self.calculate_offset( - self.combyaw, self.GTheta, self.GPStheta) + self.combyaw = self.combine_yaw(self.DGPStheta, self.GTheta, self.GPStheta, self.R3, self.R4) + self.offsetyaw = self.calculate_offset(self.combyaw, self.GTheta, self.GPStheta) self.robot_yaw = self.GTheta + self.offsetyaw if self.robot_yaw < -np.pi: @@ -346,13 +336,13 @@ def publish_fused_value(self): self.fused_msg.pose.pose.position.x = float(fused_value[0]) self.fused_msg.pose.pose.position.y = float(fused_value[1]) - self.fused_msg.pose.pose.orientation.z = float( - self.robot_orientationz) - self.fused_msg.pose.pose.orientation.w = float( - self.robot_orientationw) + self.fused_msg.pose.pose.orientation.z = float(self.robot_orientationz) + self.fused_msg.pose.pose.orientation.w = float(self.robot_orientationw) else: - fused_value = self.KalfXY( - self.Speed, self.SmpTime, self.GTheta, self.R1, self.R2) + self.R1 = 0.13**2#? + self.R2 = 0.13**2#? + + fused_value = self.KalfXY(self.Speed, self.SmpTime, self.GTheta, self.R1, self.R2) self.robot_yaw = self.GTheta + self.offsetyaw if self.robot_yaw < -np.pi: self.robot_yaw += 2 * np.pi @@ -364,10 +354,8 @@ def publish_fused_value(self): self.fused_msg.pose.pose.position.x = float(fused_value[0]) self.fused_msg.pose.pose.position.y = float(fused_value[1]) - self.fused_msg.pose.pose.orientation.z = float( - self.robot_orientationz) - self.fused_msg.pose.pose.orientation.w = float( - self.robot_orientationw) + self.fused_msg.pose.pose.orientation.z = float(self.robot_orientationz) + self.fused_msg.pose.pose.orientation.w = float(self.robot_orientationw) self.fused_msg.header.stamp = self.get_clock().now().to_msg() self.fused_msg.header.frame_id = "odom" From b7f29589e52525275b101fb2af0293ead6816e5f Mon Sep 17 00:00:00 2001 From: kengohozumi Date: Wed, 16 Oct 2024 07:44:42 +0000 Subject: [PATCH 6/6] apply format --- orange_bringup/launch/orange_robot.launch.xml | 2 - orange_bringup/orange_bringup/ekf_myself.py | 90 ++++++++++--------- .../orange_bringup/ekf_myself_noGPS.py | 6 +- .../orange_bringup/fix_to_GPSodom.py | 2 +- .../orange_bringup/lonlat_to_odom.py | 2 +- 5 files changed, 55 insertions(+), 47 deletions(-) diff --git a/orange_bringup/launch/orange_robot.launch.xml b/orange_bringup/launch/orange_robot.launch.xml index d12e48f..e884ac5 100644 --- a/orange_bringup/launch/orange_robot.launch.xml +++ b/orange_bringup/launch/orange_robot.launch.xml @@ -94,12 +94,10 @@ - - diff --git a/orange_bringup/orange_bringup/ekf_myself.py b/orange_bringup/orange_bringup/ekf_myself.py index beb703f..751d74e 100644 --- a/orange_bringup/orange_bringup/ekf_myself.py +++ b/orange_bringup/orange_bringup/ekf_myself.py @@ -54,14 +54,14 @@ def __init__(self): self.declare_parameter("ekf_publish_TF", False) self.ekf_publish_TF = self.get_parameter( "ekf_publish_TF").get_parameter_value().bool_value - + self.declare_parameter("speed_limit", 3.5) self.speed_limit = self.get_parameter( "speed_limit").get_parameter_value().double_value self.declare_parameter("speed_stop", 0.15) self.speed_stop = self.get_parameter( - "speed_stop").get_parameter_value().double_value - + "speed_stop").get_parameter_value().double_value + self.t = TransformStamped() self.br = tf2_ros.TransformBroadcaster(self) @@ -89,59 +89,61 @@ def sensor_a_callback(self, data): self.SmpTime = current_time - self.prev_time else: self.SmpTime = 0.1 - + self.prev_time = current_time current_pos = np.array([ data.pose.pose.position.x, data.pose.pose.position.y ]) - + if self.prev_pos is not None: distance = np.linalg.norm(current_pos - self.prev_pos) self.Speed = distance / self.SmpTime else: self.Speed = 0 - + if self.Speed > self.speed_limit: self.Speed = 0 - #self.get_logger().info("Speed correction") - - #self.get_logger().info(f"Speed: {self.Speed}") - - if self.Speed < self.speed_stop:#0.15 + # self.get_logger().info("Speed correction") + + # self.get_logger().info(f"Speed: {self.Speed}") + + if self.Speed < self.speed_stop: # 0.15 self.is_static = True else: - self.is_static = False - + self.is_static = False + self.prev_pos = current_pos - + self.GTheta = self.orientation_to_yaw( data.pose.pose.orientation.z, data.pose.pose.orientation.w) def sensor_b_callback(self, data): - self.GpsXY = np.array([data.pose.pose.position.x, data.pose.pose.position.y]) - #self.get_logger().info(f"self.GpsXY : {self.GpsXY}") - - if self.is_static: # stationary state(true) + self.GpsXY = np.array( + [data.pose.pose.position.x, data.pose.pose.position.y]) + # self.get_logger().info(f"self.GpsXY : {self.GpsXY}") + + if self.is_static: # stationary state(true) self.get_logger().info("stationary state") if self.prev_GPSpos is None: - self.GpsXY = np.array([0, 0]) - #self.get_logger().info("non self.prev_GPSpos") + self.GpsXY = np.array([0, 0]) + # self.get_logger().info("non self.prev_GPSpos") else: self.GpsXY = self.prev_GPSpos else: self.prev_GPSpos = self.GpsXY - - self.GPStheta = self.orientation_to_yaw(data.pose.pose.orientation.z, data.pose.pose.orientation.w) - + + self.GPStheta = self.orientation_to_yaw( + data.pose.pose.orientation.z, data.pose.pose.orientation.w) + self.DGPStheta = self.GPStheta - self.GPSthetayaw0 self.GPSthetayaw0 = self.GPStheta self.Number_of_satellites = data.pose.covariance[0] # - - #self.get_logger().info(f"self.Number_of_satellites: {self.Number_of_satellites}") + + # self.get_logger().info(f"self.Number_of_satellites: {self.Number_of_satellites}") def determination_of_R(self): if 4 <= self.Number_of_satellites < 8: # So-so... @@ -155,7 +157,7 @@ def determination_of_R(self): self.R2 = 1e-2 # 0.01 CLAS-movingbase self.R3 = 2 # GTheta self.R4 = 8 # GPStheta - + R = np.array([self.R1, self.R2, self.R3, self.R4]) return R @@ -305,9 +307,9 @@ def calculate_offset(self, combyaw, GTheta, GPStheta): if abs(self.GOffset) > 5 * pi / 180: # not -0.0872 ~ 0.0872 self.GOffset = 0 self.get_logger().warn("GOffset warning") - - #self.get_logger().info(f"GOffset: {self.GOffset}") ok - + + # self.get_logger().info(f"GOffset: {self.GOffset}") ok + return self.GOffset def publish_fused_value(self): @@ -318,11 +320,14 @@ def publish_fused_value(self): self.R2 = R[1] self.R3 = R[2] self.R4 = R[3] - fused_value = self.KalfGPSXY(self.Speed, self.SmpTime, self.GTheta, self.GpsXY, self.R1, self.R2) + fused_value = self.KalfGPSXY( + self.Speed, self.SmpTime, self.GTheta, self.GpsXY, self.R1, self.R2) self.GPS_conut += 1 if self.GPS_conut % 20 == 0: - self.combyaw = self.combine_yaw(self.DGPStheta, self.GTheta, self.GPStheta, self.R3, self.R4) - self.offsetyaw = self.calculate_offset(self.combyaw, self.GTheta, self.GPStheta) + self.combyaw = self.combine_yaw( + self.DGPStheta, self.GTheta, self.GPStheta, self.R3, self.R4) + self.offsetyaw = self.calculate_offset( + self.combyaw, self.GTheta, self.GPStheta) self.robot_yaw = self.GTheta + self.offsetyaw if self.robot_yaw < -np.pi: @@ -336,13 +341,16 @@ def publish_fused_value(self): self.fused_msg.pose.pose.position.x = float(fused_value[0]) self.fused_msg.pose.pose.position.y = float(fused_value[1]) - self.fused_msg.pose.pose.orientation.z = float(self.robot_orientationz) - self.fused_msg.pose.pose.orientation.w = float(self.robot_orientationw) + self.fused_msg.pose.pose.orientation.z = float( + self.robot_orientationz) + self.fused_msg.pose.pose.orientation.w = float( + self.robot_orientationw) else: - self.R1 = 0.13**2#? - self.R2 = 0.13**2#? - - fused_value = self.KalfXY(self.Speed, self.SmpTime, self.GTheta, self.R1, self.R2) + self.R1 = 0.13**2 # ? + self.R2 = 0.13**2 # ? + + fused_value = self.KalfXY( + self.Speed, self.SmpTime, self.GTheta, self.R1, self.R2) self.robot_yaw = self.GTheta + self.offsetyaw if self.robot_yaw < -np.pi: self.robot_yaw += 2 * np.pi @@ -354,8 +362,10 @@ def publish_fused_value(self): self.fused_msg.pose.pose.position.x = float(fused_value[0]) self.fused_msg.pose.pose.position.y = float(fused_value[1]) - self.fused_msg.pose.pose.orientation.z = float(self.robot_orientationz) - self.fused_msg.pose.pose.orientation.w = float(self.robot_orientationw) + self.fused_msg.pose.pose.orientation.z = float( + self.robot_orientationz) + self.fused_msg.pose.pose.orientation.w = float( + self.robot_orientationw) self.fused_msg.header.stamp = self.get_clock().now().to_msg() self.fused_msg.header.frame_id = "odom" diff --git a/orange_bringup/orange_bringup/ekf_myself_noGPS.py b/orange_bringup/orange_bringup/ekf_myself_noGPS.py index 7e1a22d..72d2c0e 100755 --- a/orange_bringup/orange_bringup/ekf_myself_noGPS.py +++ b/orange_bringup/orange_bringup/ekf_myself_noGPS.py @@ -252,9 +252,9 @@ def calculate_offset(self, combyaw, GTheta, GPStheta): if abs(self.GOffset) > 5 * pi / 180: # not -0.0872 ~ 0.0872 self.GOffset = 0 self.get_logger().warn("GOffset warning") - - #self.get_logger().info(f"GOffset: {self.GOffset}") ok - + + # self.get_logger().info(f"GOffset: {self.GOffset}") ok + return self.GOffset def publish_fused_value(self): diff --git a/orange_bringup/orange_bringup/fix_to_GPSodom.py b/orange_bringup/orange_bringup/fix_to_GPSodom.py index df211b0..3b37ece 100644 --- a/orange_bringup/orange_bringup/fix_to_GPSodom.py +++ b/orange_bringup/orange_bringup/fix_to_GPSodom.py @@ -154,7 +154,7 @@ def conversion(self, coordinate, origin, theta): h_x = math.cos(r_theta) * gps_x - math.sin(r_theta) * gps_y h_y = math.sin(r_theta) * gps_x + math.cos(r_theta) * gps_y point = (-h_y, h_x) - #point = (h_y, -h_x) + # point = (h_y, -h_x) return point diff --git a/orange_bringup/orange_bringup/lonlat_to_odom.py b/orange_bringup/orange_bringup/lonlat_to_odom.py index eea6034..39e70d6 100755 --- a/orange_bringup/orange_bringup/lonlat_to_odom.py +++ b/orange_bringup/orange_bringup/lonlat_to_odom.py @@ -109,7 +109,7 @@ def conversion(self, coordinate, origin, theta): h_x = math.cos(r_theta) * gps_x - math.sin(r_theta) * gps_y h_y = math.sin(r_theta) * gps_x + math.cos(r_theta) * gps_y point = (-h_y, h_x) - #point = (h_y, -h_x) + # point = (h_y, -h_x) return point