Skip to content

Commit

Permalink
Merge pull request #92 from KBKN-Autonomous-Robotics-Lab/fix/ekf_myse…
Browse files Browse the repository at this point in the history 
…lfpy

Fix/ekf myselfpy
  • Loading branch information
@kugurofu
kugurofu authored Oct 16, 2024
2 parents 499258e + b7f2958 commit 2766019
Show file tree
Hide file tree
Showing 5 changed files with 137 additions and 65 deletions.
34 changes: 29 additions & 5 deletions orange_bringup/launch/orange_robot.launch.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@
<arg name="cmd_dist_topic" default="/pos/cmd_dist"/>
<arg name="odom_topic" default="/odom"/>
<arg name="imu_topic" default="/livox/imu"/>
<arg name="fusion_odom_topic" default="/fusion/odom"/>
<arg name="fusion_odom_topic" default="/localization/odom"/>
<arg name="ekf_publish_TF" default="false"/>
<arg name="hokuyo_scan_topic" default="/hokuyo_scan"/>
<arg name="velodyne_scan_topic" default="/velodyne_scan"/>
Expand Down Expand Up @@ -78,12 +78,14 @@
<arg name="fusion_odom_out" value="$(var fusion_odom_topic)"/>
</include>-->
<!-- GPSposition_publisher -->
<!-- initial heading:housei nakaniwa=179.169287 tsukuba= 291.09504 -->
<node pkg="orange_bringup" exec="fix_to_GPSodom" output="screen">
<!-- initial heading:housei nakaniwa=179.169287 tsukuba= 276.5 -->
<node pkg="orange_bringup" exec="get_lonlat" output="screen">
<param name="port" value="/dev/sensors/GNSSbase"/>
<param name="baud" value="9600"/>
<param name="country_id" value="0"/>
<param name="heading" value="180"/>
<param name="type" value="1"/>
</node>
<node pkg="orange_bringup" exec="lonlat_to_odom" output="screen">
<param name="Position_magnification" value="1.675"/>
</node>
<!-- GPSheading_publisher -->
Expand All @@ -92,14 +94,36 @@
<param name="baud" value="9600"/>
<param name="time_out" value="1.0"/>
</node>
<!--
<node pkg="orange_bringup" exec="GPSodom_correction" output="screen">
</node>
-->
<!-- combination_GPSposition_GPSheading -->
<node pkg="orange_bringup" exec="combination" output="screen">
</node>
<!-- ekf_myself -->
<node pkg="orange_bringup" exec="ekf_myself" output="screen">
<param name="ekf_publish_TF" value="$(var ekf_publish_TF)"/>
<param name="ekf_publish_TF" value="True"/>
<param name="speed_limit" value="3.5"/>
<param name="speed_stop" value="0.15"/>
</node>
<!-- motor_driver_node -->
<node pkg="orange_bringup" exec="motor_driver_node" output="screen">
<param from="$(var motor_driver_config_file_path)"/>
<param name="port" value="/dev/ZLAC8015D"/>
<param name="control_mode" value="$(var control_mode)"/>
<param name="debug" value="$(var debug_motor)"/>
<param name="twist_cmd_vel_topic" value="$(var twist_cmd_vel_topic)"/>
<param name="cmd_vel_topic" value="$(var cmd_vel_topic)"/>
<param name="cmd_rpm_topic" value="$(var cmd_rpm_topic)"/>
<param name="cmd_deg_topic" value="$(var cmd_deg_topic)"/>
<param name="cmd_dist_topic" value="$(var cmd_dist_topic)"/>
<param name="publish_TF" value="$(var publish_TF)"/>
<param name="TF_header_frame" value="odom"/>
<param name="TF_child_frame" value="base_footprint"/>
<param name="publish_odom" value="$(var publish_odom)"/>
<param name="odom_header_frame" value="odom"/>
<param name="odom_child_frame" value="base_footprint"/>
</node>
<!-- hokuyo -->
<!--<node pkg="urg_node" exec="urg_node_driver">
Expand Down
103 changes: 71 additions & 32 deletions orange_bringup/orange_bringup/ekf_myself.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -31,18 +32,19 @@ 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
self.combyaw = 0
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)
Expand All @@ -53,6 +55,13 @@ def __init__(self):
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

self.t = TransformStamped()
self.br = tf2_ros.TransformBroadcaster(self)

Expand Down Expand Up @@ -80,17 +89,31 @@ 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(
Expand All @@ -99,6 +122,17 @@ def sensor_a_callback(self, data):
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.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)
Expand All @@ -109,14 +143,10 @@ def sensor_b_callback(self, data):

self.Number_of_satellites = data.pose.covariance[0] #

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
# self.get_logger().info(f"self.Number_of_satellites: {self.Number_of_satellites}")

elif 4 <= self.Number_of_satellites < 8: # So-so
def determination_of_R(self):
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
Expand Down Expand Up @@ -242,52 +272,58 @@ def combine_yaw(self, Dtheta, theta1, theta2, w1, w2):
return theta_sum

def calculate_offset(self, combyaw, GTheta, GPStheta):
deference = abs(GTheta) + abs(GPStheta)
abs_GTheta = abs(GTheta)
abs_GPStheta = abs(GPStheta)
abs_combyaw = abs(combyaw)
pi = math.pi

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))
self.GOffset = -(GTheta + abs_combyaw)
elif GTheta > 0 and GPStheta > 0 and combyaw > 0 and GTheta > GPStheta:
self.GOffset = -(abs(combyaw) - abs(GTheta))
self.GOffset = -(abs_combyaw - abs_GTheta)
elif GTheta < 0 and GPStheta < 0 and combyaw < 0 and GTheta > GPStheta:
self.GOffset = -(GTheta + abs(combyaw))
self.GOffset = -(GTheta + abs_combyaw)
elif GTheta < 0 and GPStheta > 0 and combyaw > 0:
self.GOffset = abs(GTheta) + combyaw
self.GOffset = abs_GTheta + combyaw
elif GTheta < 0 and GPStheta > 0 and combyaw < 0:
self.GOffset = abs(GTheta) - abs(combyaw)
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 = 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 > 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
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):
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:
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 % 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(
Expand All @@ -310,6 +346,9 @@ def publish_fused_value(self):
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.robot_yaw = self.GTheta + self.offsetyaw
Expand Down
59 changes: 33 additions & 26 deletions orange_bringup/orange_bringup/ekf_myself_noGPS.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -216,38 +216,45 @@ def combine_yaw(self, Dtheta, theta1, theta2, w1, w2):

return theta_sum

def calculate_offset(self, combyaw, GTheta, odom2theta):
deference = abs(GTheta) + abs(odom2theta)
def calculate_offset(self, combyaw, GTheta, GPStheta):
abs_GTheta = abs(GTheta)
abs_GPStheta = abs(GPStheta)
abs_combyaw = abs(combyaw)
pi = math.pi

if GTheta > 0 and odom2theta < 0 and combyaw > 0:
deference = abs_GTheta + abs_GPStheta

if GTheta > 0 and GPStheta < 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:
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 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:
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 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
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):
Expand All @@ -256,7 +263,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(
Expand Down
3 changes: 2 additions & 1 deletion orange_bringup/orange_bringup/fix_to_GPSodom.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
3 changes: 2 additions & 1 deletion orange_bringup/orange_bringup/lonlat_to_odom.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down

0 comments on commit 2766019

Please sign in to comment.